#include <string.h>

#include "basedef.h"
#include "dbg.h"
#include "tlv_parser.h"
#include "parser_certificate.h"
#include "util.h"
#include "tal_operation.h"


#define _CERT_TAG_SIZE                  (1)
#define _CERT_LENGTH_SIZE               (1)


// val : little endian, ar : little endian
static boolean _uint16_to_array(const uint16 val, uint8* ar)
{
    if (ar == NULL) {
        printD("param is invalid");
        return FALSE;
    }

    ar[0] = (uint8) (val);
    ar[1] = (uint8) (val >> 8);

    return TRUE;
}


#define _CERT_TAG_SIZE                  (1)
#define _CERT_LENGTH_SIZE               (1)


static uint8 _getTagCert(const BlobData* pBlobInput, const uint16 offset, uint16* tagOctets)
{
    // Assume that it only uses tags with a number ranging from zero to 30(inclusive)
    uint32 uint32Offset = offset;

    CHECK_BLOB_POINTER(pBlobInput, 0);

    if (tagOctets == NULL) {
        printD("tagOctets is NULL");
        return FALSE;
    }
    *tagOctets = 0;

    if (pBlobInput->length < _CERT_TAG_SIZE || pBlobInput->length > UINT16_MAX) {
        printD("input is invalid");
        return 0;
    }

    if (uint32Offset > UINT16_MAX - _CERT_TAG_SIZE) {
        printD("input is invalid");
        return 0;
    }

    if (pBlobInput->length < uint32Offset + _CERT_TAG_SIZE) {
        printD("input is invalid");
        return 0;
    }

    *tagOctets = 1;
    return *((uint8*)(pBlobInput->data + offset));
}

static uint16 _getLengthCert(const BlobData* pBlobInput, const uint16 offset, uint16* lengthOctets)
{
    uint32 uint32Offset = offset;
    uint16 length = 0;
    uint8* ar = NULL;
    uint32 t = 0;
    uint16 counts = 0;;

    CHECK_BLOB_POINTER(pBlobInput, 0);

    if (lengthOctets == NULL) {
        printD("lengthOctets is NULL");
        return FALSE;
    }
    *lengthOctets = 0;

    if (pBlobInput->length < (_CERT_TAG_SIZE + _CERT_LENGTH_SIZE) || pBlobInput->length > UINT16_MAX) {
        printD("input is invalid");
        return 0;
    }

    if (uint32Offset > UINT16_MAX - (_CERT_TAG_SIZE + _CERT_LENGTH_SIZE)) {
        printD("input is invalid");
        return 0;
    }

    if (pBlobInput->length < uint32Offset + (_CERT_TAG_SIZE + _CERT_LENGTH_SIZE)) {
        printD("input is invalid");
        return 0;
    }

    length = *((uint8*)(pBlobInput->data + offset + _CERT_TAG_SIZE));

    if (length & 0x80) {
        // the long form and assume that it only can use 2 octets more
        if (length == 0x81) {
            counts = 2;
        } else if (length == 0x82) {
            counts = 3;
        } else {
            printD("input is invalid");
            return 0;
        }

        if (pBlobInput->length < (uint32)(_CERT_TAG_SIZE + counts)) {
            printD("input is invalid");
            return 0;
        }

        if (uint32Offset > (uint32)(UINT16_MAX - (_CERT_TAG_SIZE + counts))) {
            printD("input is invalid");
            return 0;
        }

        if (pBlobInput->length < uint32Offset + (_CERT_TAG_SIZE + counts)) {
            printD("input is invalid");
            return 0;
        }

        ar = pBlobInput->data + offset + _CERT_TAG_SIZE + _CERT_LENGTH_SIZE;

        if (length == 0x81) {
            length = *ar;
        } else if (length == 0x82) {
            length = 0;
            t = ar[0]; length |=  (t << 8);
            t = ar[1]; length |=  (t);
        }

    } else {
        counts = 1;
    }

    if (pBlobInput->length < uint32Offset + _CERT_TAG_SIZE + _CERT_LENGTH_SIZE + length) {
        printD("input is invalid");
        return 0;
    }

    *lengthOctets = counts;
    return length;
}


static AuthnrResult _parseKeyWithLength(const BlobData* pBlobInput, uint32* offset, BlobData* pBlobKey, boolean isCopyNeeded, boolean isLengthCopyNeeded)
{

    AuthnrResult ret = AUTHNR_SUCCESS;
    boolean retBool = TRUE;
    uint32 i = 0;
    uint32 len_size = 0;
    uint32 len = 0;
    uint8* key = NULL;
    uint32 lenLength = 0;

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, AUTHNR_ERROR_PARAM);
    CHECK_BLOB_POINTER(pBlobKey, AUTHNR_ERROR_PARAM);

    do {

        // | Length | Key |

        key = pBlobInput->data;

        i = 0;
        if (key[i++] != 0x02) {
            // 0x02 means integer in ASN1
            printD("invalid format");
            ret = AUTHNR_ERROR_PARSING;
            break;
        }

        // Get the length of integer
        if (key[i] <= 127) {
            len = key[i++];
        } else {
            len_size = key[i++] - 128;
            if (len_size > 2) {
                printD("invalid format");
                ret = AUTHNR_ERROR_PARSING;
                break;
            }
            len = 0;
            while (len_size > 0) {
                len = (len << 8) + key[i++];
                len_size--;
            }
        }
        // Skip the leading 0 if any. Leading zero means positive
        if (key[i] == 0x0) {
            i++;
            len--;
        }

        if (len > pBlobKey->dataSize) {
            printD("overflow failed");
            ret = AUTHNR_ERROR_BUF_TOO_SMALL;
            break;
        }

        if (isCopyNeeded == TRUE) {
            if (isLengthCopyNeeded == TRUE) {
                // | Length | Key |
                retBool = _uint16_to_array((uint16) len, pBlobKey->data);
                if (retBool != TRUE) {
                    printD("_uint16_to_array failed");
                    ret = AUTHNR_ERROR_ENCODE;
                    break;
                }

                if (KEYHANDLE_KEY_LENGTH_LENGTH != sizeof(uint16)) {
                    printD("overflow failed");
                    ret = AUTHNR_ERROR_BUF_TOO_SMALL;
                    break;
                }

                lenLength = KEYHANDLE_KEY_LENGTH_LENGTH;
            }

            if (len > pBlobKey->dataSize - lenLength) {
                printD("overflow failed");
                ret = AUTHNR_ERROR_BUF_TOO_SMALL;
                break;
            }

            memcpy(pBlobKey->data + lenLength, key + i, len);
            pBlobKey->length = lenLength + len;
        }
        *offset = i + len;

    } while (0);

    printD("end");

    return ret;
}



static uint16 _decodeCert(const BlobData* pBlobInput, const uint16 offset, TlvCertDecodeData* pResult)
{
    uint32 uint32Offset = offset;
    uint16 tagOctets = 0;
    uint16 lengthOctets = 0;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length < (_CERT_TAG_SIZE + _CERT_LENGTH_SIZE) || pBlobInput->length > UINT16_MAX) {
        printD("input length is invalid");
        return FALSE;
    }

    if (uint32Offset > UINT16_MAX - (_CERT_TAG_SIZE + _CERT_LENGTH_SIZE)) {
        printD("input length is invalid");
        return FALSE;
    }

    if (pBlobInput->length < uint32Offset + (_CERT_TAG_SIZE + _CERT_LENGTH_SIZE)) {
        printD("input length is invalid");
        return FALSE;
    }

    if (pResult == NULL) {
        printE("decode(): pResult is NULL");
        return FALSE;
    }

    pResult->tag = _getTagCert(pBlobInput, offset, &tagOctets);
    pResult->length = _getLengthCert(pBlobInput, offset, &lengthOctets);
    pResult->offset = offset + _CERT_TAG_SIZE + lengthOctets;
    pResult->lengthOctets = lengthOctets;

    return pResult->offset + pResult->length;
}



#define SECP256R1   {0x2a, 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07}


/*
ECPrivateKey ::= SEQUENCE {
    version     INTEGER { ecPrivkeyVer1(1) } (ecPrivkeyVer1),
    privateKey  OCTET STRING,
    parameters  [0] ECParameters {{ NamedCurve }} OPTIONAL,
    publicKey   [1] BIT STRING OPTIONAL
}
*/
//boolean parseECPrivateKey(const BlobData* pBlobInput, uint16 offset, TlvECPrivateKey* pTlvECPrivateKey)
//{
//    uint16 retUint16 = 0;
//    TlvCertDecodeData tlvTempData;
//
//    printD("start");
//
//    CHECK_BLOB_POINTER(pBlobInput, FALSE);
//
//    if (pBlobInput->length == 0 || pBlobInput->length < offset) {
//        printD("pBlobInput->length is invalid");
//        return FALSE;
//    }
//
//    if (pTlvECPrivateKey == NULL) {
//        printD("pBlobSkpmPrivateKey is NULL");
//        return FALSE;
//    }
//
//    // ECPrivateKey ::= SEQUENCE { : 0x30
//    retUint16 = _decodeCert(pBlobInput, offset, (TlvCertDecodeData*) pTlvECPrivateKey);
//    if (retUint16 == FALSE || pTlvECPrivateKey->tag != 0x30 ||
//            pTlvECPrivateKey->length == 0) {
//        printD("decode error. ECPrivateKey");
//        return FALSE;
//    }
//
//    retUint16 = pTlvECPrivateKey->offset;
//    // version         INTEGER { ecPrivkeyVer1(1) } : 0x02
//    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvECPrivateKey->version);
//    if (retUint16 == FALSE || pTlvECPrivateKey->version.tag != 0x02 ||
//            pTlvECPrivateKey->version.length == 0) {
//        printD("decode error. version");
//        return FALSE;
//    }
//
//    // privateKey   OCTET STRING : 0x04
//    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvECPrivateKey->privatekey);
//    if (retUint16 == FALSE || pTlvECPrivateKey->privatekey.tag != 0x04 ||
//            pTlvECPrivateKey->privatekey.length == 0) {
//        printD("decode error. privateKey");
//        return FALSE;
//    }
//
//    ///////
//    // parameters  [0] ECParameters {{ NamedCurve }} OPTIONAL : 0xa0
//    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempData);
//    if (retUint16 == FALSE || tlvTempData.tag != 0xa0 ||
//            tlvTempData.length == 0) {
//        printD("decode error. ECParameters");
//        return FALSE;
//    }
//
//    retUint16 = tlvTempData.offset;
//    // OID : 0x06
//    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvECPrivateKey->namedCurve);
//    if (retUint16 == FALSE || pTlvECPrivateKey->namedCurve.tag != 0x06 ||
//            pTlvECPrivateKey->namedCurve.length == 0) {
//        printD("decode error. namedCurve");
//        return FALSE;
//    }
//
//    ///////
//    printD("tlvTempData.lengthOctets:%d,  tlvTempData.length:%d", tlvTempData.lengthOctets, tlvTempData.length);
//    retUint16 = tlvTempData.offset + tlvTempData.length;
//    // publicKey     [1] BIT STRING OPTIONAL : 0xa1
//    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempData);
//    if (retUint16 == FALSE || tlvTempData.tag != 0xa1 ||
//            tlvTempData.length == 0) {
//        printD("decode error. publicKey");
//        return FALSE;
//    }
//
//    retUint16 = tlvTempData.offset;
//    // Bit string : 0x03
//    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvECPrivateKey->publicKey);
//    if (retUint16 == FALSE || pTlvECPrivateKey->publicKey.tag != 0x03 ||
//            pTlvECPrivateKey->publicKey.length == 0) {
//        printD("decode error. publicKey Bit string");
//        return FALSE;
//    }
//
//    printD("end");
//    return TRUE;
//}



/*
ECPrivateKey ::= SEQUENCE {
    version     INTEGER { ecPrivkeyVer1(1) } (ecPrivkeyVer1),
    privateKey  OCTET STRING,
    parameters  [0] ECParameters {{ NamedCurve }} OPTIONAL,
    publicKey   [1] BIT STRING OPTIONAL
}
*/
boolean getEcPrivateKeyWithLength(const BlobData* pBlobInput, BlobData* pBlobPrivateKey)
{
    uint16 retUint16 = 0;
//    TlvCertDecodeData tlvTempParent = {0};
    TlvCertDecodeData tlvTempLeaf = {0};

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length == 0) {
        printD("pBlobInput->length is invalid");
        return FALSE;
    }

    if (pBlobPrivateKey == NULL) {
        printD("pBlobSkpmPrivateKey is NULL");
        return FALSE;
    }

    // ECPrivateKey ::= SEQUENCE { : 0x30
    retUint16 = _decodeCert(pBlobInput, 0, &tlvTempLeaf);
    if (retUint16 == FALSE || tlvTempLeaf.tag != 0x30 ||
            tlvTempLeaf.length == 0) {
        printD("decode error. ECPrivateKey");
        return FALSE;
    }

    retUint16 = tlvTempLeaf.offset;
    // version         INTEGER { ecPrivkeyVer1(1) } : 0x02
    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempLeaf);
    if (retUint16 == FALSE || tlvTempLeaf.tag != 0x02 ||
            tlvTempLeaf.length == 0) {
        printD("decode error. version");
        return FALSE;
    }

    // privateKey   OCTET STRING : 0x04
    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempLeaf);
    if (retUint16 == FALSE || tlvTempLeaf.tag != 0x04 ||
            tlvTempLeaf.length == 0) {
        printD("decode error. privateKey");
        return FALSE;
    }

    memcpy(pBlobPrivateKey->data, pBlobInput->data + tlvTempLeaf.offset, tlvTempLeaf.length);
    pBlobPrivateKey->length = tlvTempLeaf.length;

    //don't needed
//    ///////
//    // parameters  [0] ECParameters {{ NamedCurve }} OPTIONAL : 0xa0
//    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempParent);
//    if (retUint16 == FALSE || tlvTempParent.tag != 0xa0 ||
//            tlvTempParent.length == 0) {
//        printD("decode error. ECParameters");
//        return FALSE;
//    }
//
//    retUint16 = tlvTempParent.offset;
//    // OID : 0x06
//    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempLeaf);
//    if (retUint16 == FALSE || tlvTempLeaf.tag != 0x06 ||
//            tlvTempLeaf.length == 0) {
//        printD("decode error. namedCurve");
//        return FALSE;
//    }
//
//    ///////
//    retUint16 = tlvTempParent.offset + tlvTempParent.length;
//    // publicKey     [1] BIT STRING OPTIONAL : 0xa1
//    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempParent);
//    if (retUint16 == FALSE || tlvTempParent.tag != 0xa1 ||
//            tlvTempParent.length == 0) {
//        printD("decode error. publicKey");
//        return FALSE;
//    }
//
//    retUint16 = tlvTempParent.offset;
//    // Bit string : 0x03
//    retUint16 = _decodeCert(pBlobInput, retUint16, &tlvTempLeaf);
//    if (retUint16 == FALSE || tlvTempLeaf.tag != 0x03 ||
//            tlvTempLeaf.length == 0) {
//        printD("decode error. publicKey Bit string");
//        return FALSE;
//    }

    return TRUE;
}





/*
RSAPrivateKey ::= SEQUENCE {
    version           Version,
    modulus           INTEGER,  -- n
    publicExponent    INTEGER,  -- e
    privateExponent   INTEGER,  -- d
    prime1            INTEGER,  -- p
    prime2            INTEGER,  -- q
    exponent1         INTEGER,  -- d mod (p1)
    exponent2         INTEGER,  -- d mod (q-1)
    coefficient       INTEGER,  -- (inverse of q) mod p
    otherPrimeInfos   OtherPrimeInfos OPTIONAL }
*/
AuthnrResult getPrivateKeyWithLength(const BlobData* pBlobInput, BlobData* pBlobKey, uint32 keyLength) {

    AuthnrResult ret = AUTHNR_SUCCESS;
    boolean retBool = TRUE;

    BlobData blobTmpData = {0};
    BlobData blobTmpKey = {0};

    uint32 i = 0;
    uint8* privateKey = NULL;

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, AUTHNR_ERROR_PARAM);
    CHECK_BLOB_POINTER(pBlobKey, AUTHNR_ERROR_PARAM);

    do {

        // Input validation
        if (pBlobKey->dataSize < keyLength/*n*/ + KEYHANDLE_KEY_LENGTH_LENGTH + keyLength/*d*/) {
            printD("input validation failed");
            ret = AUTHNR_ERROR_PARAM;
            break;
        }

        // Init output
        secure_zero_memory(pBlobKey->data, pBlobKey->dataSize);
        pBlobKey->length = 0;

        privateKey = pBlobInput->data;

        if (privateKey[i] != 0x30 || privateKey[i+1] != 0x82 ||
            privateKey[i+4] != 0x02 || privateKey[i+5] != 0x01) {
            printD("invalid format");
            ret = AUTHNR_ERROR_PARSING;
            break;
        }

        i += 7;

        // modulus
        blobTmpData.data = pBlobInput->data + i;
        blobTmpData.dataSize = pBlobInput->length - i;
        blobTmpData.length = blobTmpData.dataSize;
        blobTmpKey.data = pBlobKey->data;
        blobTmpKey.dataSize = pBlobKey->dataSize;
        blobTmpKey.length = 0;
        ret = _parseKeyWithLength(&blobTmpData, &i, &blobTmpKey, TRUE /* isCopyNeeded */, FALSE /* isLengthCopyNeeded */);
        if (ret != AUTHNR_SUCCESS || blobTmpKey.length > blobTmpKey.dataSize ||
                blobTmpKey.length != keyLength) {
            printD("_parseKey failed");
            break;
        }
        pBlobKey->length = blobTmpKey.length;

        // publicExponent - skip
        blobTmpData.data = blobTmpData.data + i;
        blobTmpData.dataSize = blobTmpData.dataSize - i;
        blobTmpData.length = blobTmpData.dataSize;
        blobTmpKey.data = blobTmpKey.data + blobTmpKey.length;
        blobTmpKey.dataSize = pBlobKey->dataSize - blobTmpKey.length;
        blobTmpKey.length = 0;
        ret = _parseKeyWithLength(&blobTmpData, &i, &blobTmpKey, FALSE /* isCopyNeeded */, FALSE /* isLengthCopyNeeded */);
        if (ret != AUTHNR_SUCCESS || blobTmpKey.length > blobTmpKey.dataSize ||
                blobTmpKey.length != 0) {
            printD("_parseKey failed2");
            break;
        }

        // privateExponent
        blobTmpData.data = blobTmpData.data + i;
        blobTmpData.dataSize = blobTmpData.dataSize - i;
        blobTmpData.length = blobTmpData.dataSize;
        blobTmpKey.length = 0;
        ret = _parseKeyWithLength(&blobTmpData, &i, &blobTmpKey, TRUE /* isCopyNeeded */, TRUE /* isLengthCopyNeeded */);
        if (ret != AUTHNR_SUCCESS || blobTmpKey.length > blobTmpKey.dataSize ||
                blobTmpKey.length > KEYHANDLE_KEY_LENGTH_LENGTH + keyLength) {
            printD("_parseKey failed3");
            break;
        }

        if (blobTmpKey.length < KEYHANDLE_KEY_LENGTH_LENGTH + keyLength) {
            retBool = gTal.getRandomNumber(KEYHANDLE_KEY_LENGTH_LENGTH + keyLength - blobTmpKey.length, blobTmpKey.data + blobTmpKey.length);
            if (retBool != TRUE) {
                printD("getRandomNumber failed");
                ret = AUTHNR_ERROR_GET_RANDOM;
                break;
            }
        }
        pBlobKey->length += (KEYHANDLE_KEY_LENGTH_LENGTH + keyLength);

        if (pBlobKey->length > pBlobKey->dataSize) {
            printD("overflow failed");
            ret = AUTHNR_ERROR_BUF_TOO_SMALL;
            break;
        }

    } while (0);

    printD("end");

    return ret;
}


/*
AlgorithmIdentifier  ::=  SEQUENCE  {
    algorithm               OBJECT IDENTIFIER,
    parameters              ANY DEFINED BY algorithm OPTIONAL  }
*/
static boolean _parseSignatureAlgorithm(const BlobData* pBlobInput, uint16 offset, TlvSignatureAlgorithm* pTlvSignatureAlgorithm)
{
    uint16 retUint16 = 0;
    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length == 0 || pBlobInput->length < offset) {
        printD("pBlobInput->length is invalid");
        return FALSE;
    }

    if (pTlvSignatureAlgorithm == NULL) {
        printD("pTlvSignatureAlgorithm is NULL");
        return FALSE;
    }

    // algorithm : OBJECT IDENTIFIER : 0x06
    retUint16 = _decodeCert(pBlobInput, offset + retUint16, &pTlvSignatureAlgorithm->algorithm);
    if (retUint16 == FALSE || pTlvSignatureAlgorithm->algorithm.tag != 0x06 ||
            pTlvSignatureAlgorithm->algorithm.length == 0) {
        printD("decode error. algorithm");
        return FALSE;
    }

    // parameters : ANY DEFINED BY algorithm OPTIONAL
    _decodeCert(pBlobInput, offset + retUint16, &pTlvSignatureAlgorithm->parameters);

    printD("end");
    return TRUE;
}

/*
SubjectPublicKeyInfo  ::=  SEQUENCE  {
    algorithm            AlgorithmIdentifier,
    subjectPublicKey     BIT STRING  }
*/
static boolean _parseSubjectPublicKeyInfo(const BlobData* pBlobInput, uint16 offset, TlvSubjectPublicKeyInfo* pTlvSubjectPublicKeyInfo)
{
    uint16 retUint16 = 0;
    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length == 0 || pBlobInput->length < offset) {
        printD("pBlobInput->length is invalid");
        return FALSE;
    }

    if (pTlvSubjectPublicKeyInfo == NULL) {
        printD("pTlvSubjectPublicKeyInfo is NULL");
        return FALSE;
    }

    // algorithm ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, offset + retUint16, (TlvCertDecodeData*) &pTlvSubjectPublicKeyInfo->algorithm);
    if (retUint16 == FALSE || pTlvSubjectPublicKeyInfo->algorithm.tag != 0x30 ||
            pTlvSubjectPublicKeyInfo->algorithm.length == 0) {
        printD("decode error. algorithm");
        return FALSE;
    }

    // subjectPublicKey ::= BIT STRING : 0x03
    retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvSubjectPublicKeyInfo->subjectPublicKey);
    if (retUint16 == FALSE || pTlvSubjectPublicKeyInfo->subjectPublicKey.tag != 0x03 ||
            pTlvSubjectPublicKeyInfo->subjectPublicKey.length == 0) {
        printD("decode error. subjectPublicKey");
        return FALSE;
    }

    printD("end");
    return TRUE;
}

/*
TBSCertificate  ::=  SEQUENCE  {
    version         [0]  EXPLICIT Version DEFAULT v1,
    serialNumber         CertificateSerialNumber,
    signature            AlgorithmIdentifier,
    issuer               Name,
    validity             Validity,
    subject              Name,
    subjectPublicKeyInfo SubjectPublicKeyInfo,
    issuerUniqueID  [1]  IMPLICIT UniqueIdentifier OPTIONAL,
                       -- If present, version MUST be v2 or v3
    subjectUniqueID [2]  IMPLICIT UniqueIdentifier OPTIONAL,
                       -- If present, version MUST be v2 or v3
    extensions      [3]  EXPLICIT Extensions OPTIONAL
                       -- If present, version MUST be v3
    }
*/
static boolean _parseTbsCertificate(const BlobData* pBlobInput, uint16 offset, TlvTbsCertificate* pTlvTbsCertificate)
{
    uint16 retUint16 = 0;
    boolean retBool = TRUE;

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length == 0 || pBlobInput->length < offset) {
        printD("pBlobInput->length is invalid");
        return FALSE;
    }

    if (pTlvTbsCertificate == NULL) {
        printD("pTlvTbsCertificate is NULL");
        return FALSE;
    }

    // version ::= [0]  EXPLICIT : 0xA0
    retUint16 = _decodeCert(pBlobInput, offset + retUint16, &pTlvTbsCertificate->version);
    if (retUint16 == FALSE || pTlvTbsCertificate->version.tag != 0xA0 ||
            pTlvTbsCertificate->version.length == 0) {
        printD("decode error. version");
        return FALSE;
    }

    // serialNumber ::= INTEGER : 0x02
    retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->serialNumber);
    if (retUint16 == FALSE || pTlvTbsCertificate->serialNumber.tag != 0x02 ||
            pTlvTbsCertificate->serialNumber.length == 0) {
        printD("decode error. serialNumber");
        return FALSE;
    }

    // signature ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->signature);
    if (retUint16 == FALSE || pTlvTbsCertificate->signature.tag !=  0x30 ||
            pTlvTbsCertificate->signature.length == 0) {
        printD("decode error. signature");
        return FALSE;
    }

    // issuer ::= SEQUENCE OF(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->issuer);
    if (retUint16 == FALSE || pTlvTbsCertificate->issuer.tag != 0x30 ||
            pTlvTbsCertificate->issuer.length == 0) {
        printD("decode error. issuer");
        return FALSE;
    }

    // validity ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->validity);
    if (retUint16 == FALSE || pTlvTbsCertificate->validity.tag != 0x30 ||
            pTlvTbsCertificate->validity.length == 0) {
        printD("decode error. validity");
        return FALSE;
    }

    // subject ::= SEQUENCE OF(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->subject);
    if (retUint16 == FALSE || pTlvTbsCertificate->subject.tag != 0x30 ||
            pTlvTbsCertificate->subject.length == 0) {
        printD("decode error. subject");
        return FALSE;
    }

    // subjectPublicKeyInfo ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvTbsCertificate->subjectPublicKeyInfo);
    if (retUint16 == FALSE || pTlvTbsCertificate->subjectPublicKeyInfo.tag != 0x30 ||
            pTlvTbsCertificate->subjectPublicKeyInfo.length == 0) {
        printD("decode error. subjectPublicKeyInfo");
        return FALSE;
    }

    retBool = _parseSubjectPublicKeyInfo(pBlobInput, (uint16) pTlvTbsCertificate->subjectPublicKeyInfo.offset, &pTlvTbsCertificate->subjectPublicKeyInfo);
    if (retBool == FALSE) {
        printD("parse error. subjectPublicKeyInfo");
        return FALSE;
    }

    // issuerUniqueID ::= [1]  IMPLICIT : 0xA1
    if (getTag(pBlobInput, retUint16) == 0xA1) {
        retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->issuerUniqueID);
        if (retUint16 == FALSE || pTlvTbsCertificate->issuerUniqueID.tag != 0xA1 ||
                pTlvTbsCertificate->issuerUniqueID.length == 0) {
            printD("decode error. issuerUniqueID");
            return FALSE;
        }
    }

    // subjectUniqueID ::= [2]  IMPLICIT : 0xA2
    if (getTag(pBlobInput, retUint16) == 0xA2) {
        retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->subjectUniqueID);
        if (retUint16 == FALSE || pTlvTbsCertificate->subjectUniqueID.tag != 0xA2 ||
                pTlvTbsCertificate->subjectUniqueID.length == 0) {
            printD("decode error. subjectUniqueID");
            return FALSE;
        }
    }

    // extensions ::= [3]  IMPLICIT : 0xA3
    if (getTag(pBlobInput, retUint16) == 0xA3) {
        retUint16 = _decodeCert(pBlobInput, retUint16, &pTlvTbsCertificate->extensions);
        if (retUint16 == FALSE || pTlvTbsCertificate->extensions.tag != 0xA3 ||
                pTlvTbsCertificate->extensions.length == 0) {
            printD("decode error. extensions");
            return FALSE;
        }
    }

    printD("end");
    return TRUE;
}

/*
Certificate  ::=  SEQUENCE  {
    tbsCertificate       TBSCertificate,
    signatureAlgorithm   AlgorithmIdentifier,
    signatureValue       BIT STRING  }
*/
boolean parseCertificate(const BlobData* pBlobInput, uint16 offset, TlvCertificate* pTlvCertificate)
{
    uint16 retUint16 = 0;
    boolean retBool = TRUE;

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length == 0 || pBlobInput->length < offset) {
        printD("pBlobInput->length is invalid");
        return FALSE;
    }

    if (pTlvCertificate == NULL) {
        printD("pTlvCertificate is NULL");
        return FALSE;
    }

    // Certificate ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, offset, (TlvCertDecodeData*) pTlvCertificate);
    if (retUint16 == FALSE || pTlvCertificate->tag != 0x30 ||
        pTlvCertificate->length == 0) {
        printD("decode error. TlvCertificate");
        return FALSE;
    }

    retUint16 = pTlvCertificate->offset;
    // tbsCertificate ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvCertificate->tbsCertificate);
    if (retUint16 == FALSE || pTlvCertificate->tbsCertificate.tag != 0x30 ||
            pTlvCertificate->tbsCertificate.length == 0) {
        printD("decode error. tbsCertificate");
        return FALSE;
    }

    retBool = _parseTbsCertificate(pBlobInput, (uint16) pTlvCertificate->tbsCertificate.offset, &pTlvCertificate->tbsCertificate);
    if (retBool == FALSE) {
        printD("parse error. tbsCertificate");
        return FALSE;
    }

    // signatureAlgorithm ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvCertificate->signatureAlgorithm);
    if (retUint16 == FALSE || pTlvCertificate->signatureAlgorithm.tag != 0x30 ||
            pTlvCertificate->signatureAlgorithm.length == 0) {
        printD("decode error. signatureAlgorithm");
        return FALSE;
    }

    retBool = _parseSignatureAlgorithm(pBlobInput, (uint16) pTlvCertificate->signatureAlgorithm.offset, &pTlvCertificate->signatureAlgorithm);
    if (retBool == FALSE) {
        printD("parse error. signatureAlgorithm");
        return FALSE;
    }

    // signatureValue : BIT STRING : 0x03
    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvCertificate->signatureValue);
    if (retUint16 == FALSE || pTlvCertificate->signatureValue.tag != 0x03 ||
            pTlvCertificate->signatureValue.length == 0) {
        printD("decode error. signatureValue");
        return FALSE;
    }

    printD("end");
    return TRUE;
}

/*
RSAPublicKey ::= SEQUENCE {
    modulus           INTEGER,  -- n
    publicExponent    INTEGER   -- e
}
*/
boolean parseRSAPublicKey(const BlobData* pBlobInput, uint16 offset, TlvRSAPublicKey* pTlvRSAPublicKey)
{
    uint16 retUint16 = 0;

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if (pBlobInput->length == 0 || pBlobInput->length < offset) {
        printD("pBlobInput->length is invalid");
        return FALSE;
    }

    if (pTlvRSAPublicKey == NULL) {
        printD("pTlvCertificate is NULL");
        return FALSE;
    }

    // RSAPublicKey ::= SEQUENCE(Constructed) : 0x30
    retUint16 = _decodeCert(pBlobInput, offset, (TlvCertDecodeData*) pTlvRSAPublicKey);
    if (retUint16 == FALSE || pTlvRSAPublicKey->tag != 0x30 ||
        pTlvRSAPublicKey->length == 0) {
        printD("decode error. TlvRSAPublicKey");
        return FALSE;
    }

    retUint16 = pTlvRSAPublicKey->offset;
    // modulus : INTEGER : 0x02
    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvRSAPublicKey->modulus);
    if (retUint16 == FALSE || pTlvRSAPublicKey->modulus.tag != 0x02 ||
            pTlvRSAPublicKey->modulus.length == 0) {
        printD("decode error. modulus");
        return FALSE;
    }

    // publicExponent : INTEGER : 0x02
    retUint16 = _decodeCert(pBlobInput, retUint16, (TlvCertDecodeData*) &pTlvRSAPublicKey->publicExponent);
    if (retUint16 == FALSE || pTlvRSAPublicKey->publicExponent.tag != 0x02 ||
            pTlvRSAPublicKey->publicExponent.length == 0) {
        printD("decode error. publicExponent");
        return FALSE;
    }

    printD("end");
    return TRUE;
}




boolean getCertificateSubjectDn(const BlobData* pBlobCertificate, BlobData* pBlobDn) {
    boolean retBool = TRUE;
    TlvCertificate tlvCertificate = {0, };

    printD("start");

    CHECK_BLOB_POINTER(pBlobCertificate, FALSE);
    CHECK_BLOB_POINTER(pBlobDn, FALSE);

    retBool = parseCertificate(pBlobCertificate, 0, &tlvCertificate);
    if (retBool != TRUE) {
        printD("cert parsering error");
        return FALSE;
    }

    pBlobDn->length = tlvCertificate.tbsCertificate.subject.length;
    memcpy(pBlobDn->data, pBlobCertificate->data + tlvCertificate.tbsCertificate.subject.offset, pBlobDn->length);

    printD("end");

    return TRUE;
}


AuthnrResult _parseKey(const BlobData* pBlobInput, uint32* offset, BlobData* pBlobKey, boolean isCopyNeeded)
{

    AuthnrResult ret = AUTHNR_SUCCESS;
    uint32 i = 0;
    uint32 len_size = 0;
    uint32 len = 0;
    uint8* key = NULL;

    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, AUTHNR_ERROR_PARAM);
    CHECK_BLOB_POINTER(pBlobKey, AUTHNR_ERROR_PARAM);

    do {

        key = pBlobInput->data;

        i = 0;
        if (key[i++] != 0x02) {
            // 0x02 means integer in ASN1
            printD("invalid format");
            ret = AUTHNR_ERROR_PARSING;
            break;
        }

        // Get the length of integer
        if (key[i] <= 127) {
            len = key[i++];
        } else {
            len_size = key[i++] - 128;
            if (len_size > 2) {
                printD("invalid format");
                ret = AUTHNR_ERROR_PARSING;
                break;
            }
            len = 0;
            while (len_size > 0) {
                len = (len << 8) + key[i++];
                len_size--;
            }
        }
        // Skip the leading 0 if any. Leading zero means positive
        if (key[i] == 0x0) {
            i++;
            len--;
        }

        if (len > pBlobKey->dataSize) {
            printD("overflow failed");
            ret = AUTHNR_ERROR_BUF_TOO_SMALL;
            break;
        }

        if (isCopyNeeded == TRUE) {
            memcpy(pBlobKey->data, key + i, len);
            pBlobKey->length = len;
        }
        *offset = i + len;

    } while (0);

    printD("end");

    return ret;
}