/*
Sample code for parsing an SOD file, which is basically a CMS file embedded in an [APPLICATION 23], e.g.

   0  119: [APPLICATION 23] {
   4   48:   SEQUENCE {                                                -> start of the CMS
   8    6:     OBJECT IDENTIFIER signedData (1 2 840 113549 1 7 2)
  19  160:     [0] {
  23   48:       SEQUENCE {
  27    2:         INTEGER 3
  30   49:         SET {
  32   48:           SEQUENCE {
  34    6:             OBJECT IDENTIFIER sha2-256 (2 16 840 1 101 3 4 2 1)
         :             }
         :           }
  45   48:         SEQUENCE {                                           -> encapContentInfo
  48    6:           OBJECT IDENTIFIER '2 23 136 1 1 1'                   -> eContentType
  56  160:           [0] {                                                  -> eContent (the encoded contents of ldsSecurityObject)
  59    4:             OCTET STRING, encapsulates {
  62   48:                 SEQUENCE {
  65    2:                   INTEGER 0                                          -> version
  68   48:                   SEQUENCE {                                         -> hashAlgorithm
  70    6:                     OBJECT IDENTIFIER
         :                       sha2-256 (2 16 840 1 101 3 4 2 1)
         :                     }
  81   48:                   SEQUENCE {                                         -> dataGroupHashValues
  83   48:                     SEQUENCE {
  85    2:                       INTEGER 1                                          -> DataGroupNumber (refers to DG1 = the MRZ file)
  88    4:                       OCTET STRING                                       -> DataGroupHashValue (hash over the entire, raw DG1 file)
         :                   8F 3F B5 B7 AA BD 36 0C A7 CD 05 2B 14 2D 04 24
         :                   A7 59 AD 82 1D FC 8A 9D B6 B8 27 5E A9 4F AF 3D
         :                       }
 122   48:                     SEQUENCE {
 124    2:                       INTEGER 13                                         -> DataGroupNumber (refers to DG2 = the photo file)
 127    4:                       OCTET STRING                                       -> DataGroupHashValue
         :                   75 74 E3 54 C2 3E 87 4D CD 50 A2 5D CD 01 00 D8
         :                   0B B3 39 63 DD 88 A1 69 58 B5 FD 0B F8 BD FE 5E
         :                       }
 ...

SOD files are present in RFID chips of passports (the DG15 file, see ICAO 9303)) and on some eID cards.

Feel free to use it, just don't blame if things go wrong..
*/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "openssl/evp.h"
#include "openssl/err.h"
#include "openssl/cms.h"

void ParseSOD(unsigned char *buf, int buflen);
void GetHashesAndAlgo(const unsigned char *data, int len);
void GetHashAlgo(const unsigned char *data, long len);
void GetHash(const unsigned char **buf, long *pbufLen);
int ASN1_next_obj(unsigned char expectedTag, const unsigned char *asn1, int asn1Len,
    const unsigned char **data, int *pdataLen,
    int *premainingLen, const unsigned char **pnextAsn1);
void Dumphex(const unsigned char *data, int len);

void main(int argc, const char **argv)
{
		unsigned char buf[5000]; // should be plenty
		int buflen = 0;
		FILE *f = NULL;
		const char *filename = NULL;

		if (argc != 2)
		{
			printf("Usage: %s <SOD file>\n", argv[0]);
			return;
		}

		filename = argv[1];
		f = fopen(filename, "rb");
		if (!f)
		{
			printf("ERR: couldn't open file \"%s\", exiting\n", filename);
			return;
		}
		buflen = (int) fread(buf, 1, sizeof(buf), f);
	
		fclose(f);

		ParseSOD(buf, buflen);
}

void ParseSOD(unsigned char *buf, int buflen)
{
	int offset = 0;
    BIO *in = NULL;
    CMS_ContentInfo *cms = NULL;
    const ASN1_OBJECT *contentType = NULL;
    ASN1_OCTET_STRING *eContent = NULL;

	// Skip the [APPLICATION 23], if present
	if (buflen > 4 && buf[0] == 0x77)
	{
		if (buf[1] == 0x81)
			offset = 3;
		else if (buf[1] == 0x82)
			offset = 4;
		else
			offset = 2;
	}
	buflen -= offset;

	// Parse the CMS
	in = BIO_new_mem_buf(buf + offset, buflen);
	cms = d2i_CMS_bio(in, NULL);
	if (cms == NULL)	
	{
		printf("ERR: couldn't parse the SOD file:\n");
		ERR_print_errors_fp(stdout);
		return;
	}

	// Get the encapsulated content
    contentType = CMS_get0_type(cms);
	if (OBJ_obj2nid(contentType) != NID_pkcs7_signed)
	{
		printf("ERR: no SignedData found in the SOD file\n");
		return;
	}
	eContent = *CMS_get0_content(cms);

	OpenSSL_add_all_algorithms();
	ERR_load_crypto_strings();

	// Look into the encapsulated content
	GetHashesAndAlgo(eContent->data, eContent->length);

	// Use CMS_get1_certs() to get the signer cert chain (if present)

	// Use CMS_verify() to verify the SOD file

	CMS_ContentInfo_free(cms);
}

void GetHashesAndAlgo(const unsigned char *data, int len)
{
    if (!ASN1_next_obj(0x30, data, len, &data, &len, 0, 0))  // jump into the eContent (an LDSSecurityObject)
    {
        printf("ERR jumping into the eContent SEQUENCE\n");
        return;
    }
    else if (!ASN1_next_obj(0x02, data, len, 0, 0, &len, &data))             // jump over the version
    {
        printf("ERR jumping over eContent.version\n");
        return;
    }
    else
    {
        // Get the hash algo
        GetHashAlgo(data, len);

        if (!ASN1_next_obj(0x30, data, len, NULL, NULL, &len, &data))   // jump over the hashAlgorithm
        {
            printf("ERR jumping over eContent.hashAlgorithm\n");
            return;
        }
        else if (!ASN1_next_obj(0x30, data, len, &data, &len, 0, 0))    // jump into the dataGroupHashValues
        {
        	printf("ERR jumping into eContent.dataGroupHashValues\n");
        	return;
        }
        else
        {
            // Now we're at the sequence that contains the INTEGER and the OCTET STRING, e.g.
            //    30 25
            //      02 01 01
            //      04 20 20 a2 bb a2 fe d8 7d 89 f6 9d d6 16 b6 6a 1b 9b 76 42 0d 4b c6 0e 2a ec cd 07 13 00 f5 50 d7 b9  // the hash
            while (len > 10)
                GetHash(&data, &len);
        }
    }
}

// E.g. data = 30  0b  06 09 60 86 48 01 65 03 04 02 01 -> m_md = EVP_sha256()
void GetHashAlgo(const unsigned char *data, long len)
{
	// TODO: should use the openssl lib to parse the OID
	
    // jump over the tag and length
    data += 2;
    len -= 2;

    if (len >= 10 && memcmp("\x06\x08\x2a\x86\x48\x86\xf7\x0d\x02\x05", data, 10) == 0)
        printf("eContentType hash algo: MD5\n");
    else if (len >= 7 && memcmp("\x06\x05\x2b\x0e\x03\x02\x1a", data, 7) == 0)
        printf("eContentType hash algo: SHA-1\n");
    else if (len >= 11 && memcmp("\x06\x09\x60\x86\x48\x01\x65\x03\x04\x02\x01", data, 11) == 0)
        printf("eContentType hash algo: SHA-256\n");
    else if (len >= 11 && memcmp("\x06\x09\x60\x86\x48\x01\x65\x03\x04\x02\x02", data, 11) == 0)
        printf("eContentType hash algo: SHA-384\n");
    else if (len >= 11 && memcmp("\x06\x09\x60\x86\x48\x01\x65\x03\x04\x02\x03", data, 11) == 0)
        printf("eContentType hash algo: SHA-512\n");
    else
        printf("Unknow hash OID in the eContentType\n"); // ERR
}

void GetHash(const unsigned char **buf, long *pbufLen)
{
    const unsigned char *data = *buf;
    long dataLen = *pbufLen;

    if (!ASN1_next_obj(0x30, data, dataLen, &data, &dataLen, pbufLen, buf))
    {
        printf("ERR parsing a dataGroupHashValues.SEQUENCE\n");
        return;
    }
    else
    {
    	int dgNr = data[2];
        if (!ASN1_next_obj(0x02, data, dataLen, NULL, NULL, &dataLen, &data))
        {
            printf("ERR parsing a DataGroupNumber\n");
            return;
        }

    	if (!ASN1_next_obj(0x04, data, dataLen, &data, &dataLen, NULL, NULL))
        {
            printf("ERR parsing a DataGroupHashValue\n");
            return;
        }

		printf("- Hash value for DataGroupNumber = %d:\n");
		Dumphex(data, dataLen);
    }
}

// Jump into (if premainingLen == NULL and pnextAsn1 == NULL) or
// jump over (if data == NULL and pdataLen == NULL) an ASN.1 element
int ASN1_next_obj(unsigned char expectedTag, const unsigned char *asn1, int asn1Len,
    const unsigned char **data, int *pdataLen,
    int *premainingLen, const unsigned char **pnextAsn1)
{
    const unsigned char *tmp = asn1;
    int tag;
    int clazz;
    int len;
    int info;

    if (asn1Len < 1)
        return 0;
    if ((0 != expectedTag) && (asn1[0] != expectedTag))
        return 0;

    info = ASN1_get_object(&tmp, &len, &tag, &clazz, asn1Len);

    if (data)
        *data = tmp;
    if (pdataLen)
        *pdataLen = len;
    if (premainingLen)
        *premainingLen = asn1Len - len - (long) (tmp - asn1);
    if (pnextAsn1)
        *pnextAsn1 = tmp + len;

    return !(0x80 & info);
}

void Dumphex(const unsigned char *data, int len)
{
	int i = 0;
	printf("    ");
	for (i = 0; i < len; i++)
		printf(" %02x", data[i]);
	printf("\n");
}
