#include "dbg.h"
#include "basedef.h"

#include "tlv_parser.h"
#include "cmd_registry.h"
#include "cmd_crypto.h"
#include "parser_tlv.h"

#define INVALID_OFFSET 0
#define TAG_LENGTH_SIZE_DEFAULT 4
#define TAG_LENGTH_SIZE_2BYTE   2

static boolean _checkBasicDecodeResult(uint32 offset, TlvDecodeData* tlvDecodeData, uint32 expected)
{
    if (offset == INVALID_OFFSET) {
        printE("ret is false");
        return FALSE;
    }
    if (tlvDecodeData == 0) {
        printE("tlvDecodeData is null");
        return FALSE;
    }
    if (tlvDecodeData->tag != expected) {
        printE("wrong tag : expected: %x, actual %x", expected, tlvDecodeData->tag);
        return FALSE;
    }

    return TRUE;
}

static boolean _checkDecodeResult(uint32 offset, TlvDecodeData* tlvDecodeData, uint32 expected)
{
    boolean ok = _checkBasicDecodeResult(offset, tlvDecodeData, expected);
    if (ok != TRUE) {
        printE("_checkBasicDecodeResult fails");
        return FALSE;
    }

    if (tlvDecodeData->length == 0) {
        printE("tlvDecodeData->length == 0");
        return FALSE;
    }

    return TRUE;
}

static uint32 _decodeTlvData(const BlobData* pBlobInput, uint32 offset, TlvDecodeData* tlvData, uint32 expectedTag, uint32 tagLengthSize)
{
    if (tagLengthSize == TAG_LENGTH_SIZE_2BYTE) {
        offset = decode2ByteBE(pBlobInput, offset, (TlvDecodeData*)tlvData);
    } else {
        offset = decode(pBlobInput, offset, (TlvDecodeData*)tlvData);
    }

    if (_checkDecodeResult(offset, (TlvDecodeData*)tlvData, expectedTag) == FALSE) {
        printE("failed to decode 0x%X", expectedTag);
        return INVALID_OFFSET;
    }
    return offset;
}

#define DECODE_TLV_DATA(_input, _offset, _target, _tag)  \
    DECODE_TLV_DATA_WITH_SIZE(_input, _offset, _target, _tag, TAG_LENGTH_SIZE_DEFAULT)
#define DECODE_TLV_DATA_WITH_SIZE(_input, _offset, _target, _tag, _tagLengthSize) do {          \
    _offset = _decodeTlvData(_input, _offset, (TlvDecodeData*)_target, _tag, _tagLengthSize);   \
    if (_offset == INVALID_OFFSET) {                                                            \
        printE("failed to decode " #_tag);                                                      \
        return FALSE;                                                                           \
    }                                                                                           \
} while (0)


boolean parseDecryptionCommand(const BlobData* pBlobInput, TlvDecryptionCommand* pTlvDecryptionCommand)
{
    uint32 offset = 0;
    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

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

    if (pTlvDecryptionCommand == NULL) {
        printE("pTlvDecryptionCommand is NULL");
        return FALSE;
    }

    // TAG_TAD_DECRYPTION_COMMAND
    DECODE_TLV_DATA(pBlobInput, offset, pTlvDecryptionCommand, TAG_TAD_DECRYPTION_COMMAND);

    // TAG_TAD_CERTIFICATE - Sub1 Certificate
    offset = pTlvDecryptionCommand->offset;
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvDecryptionCommand->sub1Certificate, TAG_TAD_CERTIFICATE);

    // TAG_TAD_CERTIFICATE - Leaf Certificate
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvDecryptionCommand->leafCertificate, TAG_TAD_CERTIFICATE);

    // TAG_TAD_RSA_PRIVATE_KEY_HANDLE
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvDecryptionCommand->rsaPrivateKeyHandle, TAG_TAD_RSA_PRIVATE_KEY_HANDLE);

    // TAG_TAD_RSA_ENCRYPTED_DATA
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvDecryptionCommand->rsaEncryptedData, TAG_TAD_RSA_ENCRYPTED_DATA);

    // TAG_TAD_AES_ENCRYPTED_DATA
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvDecryptionCommand->aesEncryptedData, TAG_TAD_AES_ENCRYPTED_DATA);

    printD("end");
    return TRUE;
}

boolean parseRsaDecryptedData(const BlobData* pBlobInput, TlvRsaDecryptedData* pTlvRsaDecryptedData)
{
    uint32 offset = 0;
    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

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

    if (pTlvRsaDecryptedData == NULL) {
        printE("pTlvRsaDecryptedData is NULL");
        return FALSE;
    }

    // TAG_TAD_MSG_VERSION
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvRsaDecryptedData->msgVersion, TAG_TAD_MSG_VERSION);

    // TAG_TAD_MSG_HM
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvRsaDecryptedData->msgHM, TAG_TAD_MSG_HM);

    // TAG_TAD_MSG_HMAC_KEY
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvRsaDecryptedData->msgHmacKey, TAG_TAD_MSG_HMAC_KEY);

    // TAG_TAD_MSG_AES_KEY
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvRsaDecryptedData->msgAesKey, TAG_TAD_MSG_AES_KEY);

    printD("end");
    return TRUE;
}

boolean parseSkpmProvisioningCommand(const BlobData* pBlobInput, TlvSkpmProvisioningCommand* pTlvSkpmProvisioningCommand)
{
    uint32 offset = 0;
    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

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

    if (pTlvSkpmProvisioningCommand == NULL) {
        printE("pTlvSkpmProvisioningCommand is NULL");
        return FALSE;
    }

    // TAG_TAD_SKPM_PROVISIONING_COMMAND
    DECODE_TLV_DATA(pBlobInput, offset, pTlvSkpmProvisioningCommand, TAG_TAD_SKPM_PROVISIONING_COMMAND);

    // TAG_TAD_WRAPPED_DATA
    offset = pTlvSkpmProvisioningCommand->offset;
    DECODE_TLV_DATA(pBlobInput, offset, &pTlvSkpmProvisioningCommand->skpmWrappedData, TAG_TAD_WRAPPED_DATA);

    printD("end");
    return TRUE;
}


boolean parseSkpmUnwrappedData(const BlobData* pBlobInput, TlvSkpmUnwrappedData* pTlvSkpmUnwrappedData)
{
    uint16 offset = 0;
    printD("start");

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

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

    if (pTlvSkpmUnwrappedData == NULL) {
        printE("pPakCommand is NULL");
        return FALSE;
    }

    // TAG_SKPM_SUB1_CERTIFICATE
    DECODE_TLV_DATA_WITH_SIZE(pBlobInput, offset, &pTlvSkpmUnwrappedData->sub1Certificate, TAG_SKPM_SUB1_CERTIFICATE, TAG_LENGTH_SIZE_2BYTE);

    // TAG_SKPM_LEAF_CERTIFICATE
    DECODE_TLV_DATA_WITH_SIZE(pBlobInput, offset, &pTlvSkpmUnwrappedData->leafCertificate, TAG_SKPM_LEAF_CERTIFICATE, TAG_LENGTH_SIZE_2BYTE);

    // TAG_SKPM_LEAF_PRIVATE_KEY_RSA
    DECODE_TLV_DATA_WITH_SIZE(pBlobInput, offset, &pTlvSkpmUnwrappedData->rsaPrivateKey, TAG_SKPM_LEAF_PRIVATE_KEY_RSA, TAG_LENGTH_SIZE_2BYTE);

    return TRUE;
}