#include <string.h>

#include "basedef.h"
#include "cmd_skpm.h"
#include "dbg.h"
#include "parser_tlv.h"
#include "cmd_registry.h"
#include "tal_operation.h"
#include "tlv_parser.h"
#include "util.h"
#include "parser_certificate.h"


#define SKPM_RSA_KEY_LENGTH 256

AuthnrResult processSkpmProvisioning(const BlobData* pBlobInput, BlobData* pBlobOutput)
{
    AuthnrResult ret = AUTHNR_SUCCESS;
    boolean retBool = TRUE;
    BlobData blobBuf = {0};
    BlobData blobUnwrappedData = {0};
    BlobData blobWrappedData = {0};
    BlobData blobKey = {0};
    BlobData blobKeyhandle = {0};
    TlvEncodeData tlvBuf = {0};
    BlobData blobTlvBuf = {0};
    TlvSkpmProvisioningCommand tlvSpCommand = {0};
    TlvSkpmUnwrappedData tlvSkpmUnwrappedData = {0};
    uint32 authType = 0;
    BlobData blobTempSkpmKeyhandle = {0};
    BlobData blobTempCertificate = {0};

    printI("pvS");

    CHECK_BLOB_POINTER(pBlobInput, AUTHNR_ERROR_PARAM);
    CHECK_BLOB_POINTER(pBlobOutput, AUTHNR_ERROR_PARAM);

    retBool = parseSkpmProvisioningCommand(pBlobInput, &tlvSpCommand);
    if (retBool != TRUE || tlvSpCommand.tag != TAG_TAD_SKPM_PROVISIONING_COMMAND) {
        printE("tag is not correct. TAG_TAD_SKPM_PROVISIONING_COMMAND");
        return AUTHNR_ERROR_PARSING;
    }

#ifdef USE_DUMP_DATA
    {
        printD("tlvProvisionAttestaionKeyCommand. tag:%x, length:%d, offset:%d", tlvSpCommand.tag, tlvSpCommand.length, tlvSpCommand.offset);
        printD("tlvProvisionAttestaionKeyCommand.skpmWrappedData. tag:%x, length:%d, offset:%d", tlvSpCommand.skpmWrappedData.tag, tlvSpCommand.skpmWrappedData.length, tlvSpCommand.skpmWrappedData.offset);
    }
#endif

    do {
        //init
        blobBuf.dataSize = MAX_SKPM_COMMAND_LENGTH;
        blobBuf.data = gTal.malloc(blobBuf.dataSize);
        if (blobBuf.data == NULL){
            printE("blobBuf.Data malloc fail");
            ret = AUTHNR_ERROR_MALLOC;
            break;
        }
        blobBuf.length = 0;

        blobWrappedData.dataSize = MAX_SKPM_COMMAND_LENGTH;
        blobWrappedData.data = gTal.malloc(blobWrappedData.dataSize);
        if (blobWrappedData.data == NULL){
            printE("blobWrappedData.Data malloc fail");
            ret = AUTHNR_ERROR_MALLOC;
            break;
        }
        blobWrappedData.length = 0;

        blobUnwrappedData.dataSize = MAX_SKPM_COMMAND_LENGTH;
        blobUnwrappedData.data = gTal.malloc(blobUnwrappedData.dataSize);
        if (blobUnwrappedData.data == NULL){
            printE("blobUnwrappedData.Data malloc fail");
            ret = AUTHNR_ERROR_MALLOC;
            break;
        }
        blobUnwrappedData.length = 0;

        blobKey.dataSize = MAX_SKPM_COMMAND_LENGTH;
        blobKey.data = gTal.malloc(blobKey.dataSize);
        if (blobKey.data == NULL){
            printE("blobKey.Data malloc fail");
            ret = AUTHNR_ERROR_MALLOC;
            break;
        }
        blobKey.length = 0;

        blobKeyhandle.dataSize = MAX_SKPM_COMMAND_LENGTH;
        blobKeyhandle.data = gTal.malloc(blobKeyhandle.dataSize);
        if (blobKeyhandle.data == NULL){
            printE("blobKeyhandle.Data malloc fail");
            ret = AUTHNR_ERROR_MALLOC;
            break;
        }
        blobKeyhandle.length = 0;

        blobTlvBuf.dataSize = MAX_SKPM_COMMAND_LENGTH;
        blobTlvBuf.data = gTal.malloc(blobTlvBuf.dataSize);
        if (blobTlvBuf.data == NULL){
            printE("blobTlvBuf.Data malloc fail");
            ret = AUTHNR_ERROR_MALLOC;
            break;
        }
        blobTlvBuf.length = 0;

        tlvBuf.data = blobTlvBuf.data;
        tlvBuf.tag = TAG_TAD_SKPM_PROVISIONING_RESPONSE;
        tlvBuf.length = 0;

        blobTempSkpmKeyhandle.data = pBlobInput->data + tlvSpCommand.skpmWrappedData.offset;
        blobTempSkpmKeyhandle.length = tlvSpCommand.skpmWrappedData.length;
        blobTempSkpmKeyhandle.dataSize = blobTempSkpmKeyhandle.length;

        retBool = gTal.decapsulateSkpm(&blobTempSkpmKeyhandle, &blobUnwrappedData);
        if(retBool != TRUE && authType == 0) {
            printE("decapsulate fail");
            ret = AUTHNR_ERROR_DECAPSULATE;
            break;
        }

        retBool = parseSkpmUnwrappedData(&blobUnwrappedData, &tlvSkpmUnwrappedData);
        if (retBool != TRUE) {
            printE("tag is not correct. unwrapped data");
            ret = AUTHNR_ERROR_PARSING;
            break;
        }
#ifdef USE_DUMP_DATA
        {
            printD("tlvSkpmUnwrappedData.sub1Certificate tag:%x, length:%d, offset:%d", tlvSkpmUnwrappedData.sub1Certificate.tag, tlvSkpmUnwrappedData.sub1Certificate.length, tlvSkpmUnwrappedData.sub1Certificate.offset);
            printD("tlvSkpmUnwrappedData.leafCertificate tag:%x, length:%d, offset:%d", tlvSkpmUnwrappedData.leafCertificate.tag, tlvSkpmUnwrappedData.leafCertificate.length, tlvSkpmUnwrappedData.leafCertificate.offset);
            printD("tlvSkpmUnwrappedData.rsaPrivateKey tag:%x, length:%d, offset:%d", tlvSkpmUnwrappedData.rsaPrivateKey.tag, tlvSkpmUnwrappedData.rsaPrivateKey.length, tlvSkpmUnwrappedData.rsaPrivateKey.offset);
        }
#endif

        //TAG_TAD_CERTIFICATE - sub1Certificate
        retBool = encodeRawTlv2Blob(TAG_TAD_CERTIFICATE, tlvSkpmUnwrappedData.sub1Certificate.length, blobUnwrappedData.data + tlvSkpmUnwrappedData.sub1Certificate.offset, &blobBuf);
        if (retBool != TRUE) {
            printE("encode error. TAG_FIDO_CERTIFICATE - sub1");
            ret = AUTHNR_ERROR_ENCODE;
            break;
        }
        memcpy(tlvBuf.data + tlvBuf.length, blobBuf.data, blobBuf.length);
        tlvBuf.length += blobBuf.length;


        //TAG_TAD_CERTIFICATE - leafCertificate
        retBool = encodeRawTlv2Blob(TAG_TAD_CERTIFICATE, tlvSkpmUnwrappedData.leafCertificate.length, blobUnwrappedData.data + tlvSkpmUnwrappedData.leafCertificate.offset, &blobBuf);
        if (retBool != TRUE) {
            printE("encode error. TAG_FIDO_CERTIFICATE - leaf");
            ret = AUTHNR_ERROR_ENCODE;
            break;
        }
        memcpy(tlvBuf.data + tlvBuf.length, blobBuf.data, blobBuf.length);
        tlvBuf.length += blobBuf.length;

        //KEY_HANDLE : | MAGIC_NUMBER | VERSION | ACCESS_TOKEN | PRIVATE_KEY |
        //ACCESS_TOKEN : hash(CERTIFICATE)
        //magic number
        retBool = uint16_to_array(SKPM_KEY_HANDLE_MAGIC_NUMBER, blobKeyhandle.data);
        if (retBool != TRUE) {
            printD("set value failed");
            ret = AUTHNR_ERROR_UNKNOWN;
            break;
        }
        blobKeyhandle.length = SKPM_KEY_HANDLE_MAGIC_NUMBER_LENGTH;

        //version
        retBool = uint16_to_array(SKPM_KEY_HANDLE_VERSION, blobKeyhandle.data + blobKeyhandle.length);
        if (retBool != TRUE) {
            printD("set value failed");
            ret = AUTHNR_ERROR_UNKNOWN;
            break;
        }
        blobKeyhandle.length += SKPM_KEY_HANDLE_VERSION_LENGTH;

        //access token
        blobTempCertificate.length = tlvSkpmUnwrappedData.leafCertificate.length;
        blobTempCertificate.data = blobUnwrappedData.data + tlvSkpmUnwrappedData.leafCertificate.offset;
        blobTempCertificate.dataSize = blobTempCertificate.length;
        retBool = gTal.hash(&blobTempCertificate, HASH_TYPE_SHA256, &blobBuf);
        if (retBool != TRUE) {
            printD("hash error");
            ret = AUTHNR_ERROR_HASH;
            break;
        }
        memcpy(blobKeyhandle.data + blobKeyhandle.length, blobBuf.data, blobBuf.length);
        blobKeyhandle.length += SKPM_KEY_HANDLE_ACCESS_TKOEN_LENGTH;

        //private key
        BlobData blobTempData = {0};
        blobTempData.data = blobUnwrappedData.data + tlvSkpmUnwrappedData.rsaPrivateKey.offset;
        blobTempData.length = tlvSkpmUnwrappedData.rsaPrivateKey.length;
        blobTempData.dataSize = blobTempData.length;
        ret = getPrivateKeyWithLength(&blobTempData, &blobKey, SKPM_RSA_KEY_LENGTH);
        if (ret != AUTHNR_SUCCESS) {
            printD("private parsing fail");
            break;
        }

        memcpy(blobKeyhandle.data + blobKeyhandle.length, blobKey.data, blobKey.length);
        blobKeyhandle.length += blobKey.length;

        retBool = gTal.wrap(&blobKeyhandle, &blobWrappedData);
        if (retBool != TRUE) {
            printE("wrap error. keyhandle");
            ret = AUTHNR_ERROR_WRAP;
            break;
        }

        //TAG_TAD_WRAPPED_DATA
        retBool = encodeRawTlv2Blob(TAG_TAD_WRAPPED_DATA, blobWrappedData.length, blobWrappedData.data, &blobBuf);
        if (retBool != TRUE) {
            printE("encode error. TAG_COMMON_WRAPPED_DATA");
            ret = AUTHNR_ERROR_ENCODE;
            break;
        }
        memcpy(tlvBuf.data + tlvBuf.length, blobBuf.data, blobBuf.length);
        tlvBuf.length += blobBuf.length;

        pBlobOutput->length = 0;
    } while(0);

    do {
        //SKPM_MAX_LENGTH_PROVISION_ATTESTATION_KEY_RESPONSE
        retBool = encodeTlv2Blob(tlvBuf, pBlobOutput);
        if (retBool != TRUE) {
            printE("encode error. TAG_TAD_SKPM_PROVISIONING_RESPONSE");
            ret = AUTHNR_ERROR_ENCODE;
            break;
        }
    } while (0);
#ifdef USE_DUMP_DATA
    printBlob(*pBlobOutput, "TAG_TAD_SKPM_PROVISIONING_RESPONSE");
#endif

    if (blobBuf.data != NULL) {
        gTal.free(blobBuf.data, blobBuf.dataSize);
        blobBuf.data = NULL;
    }
    if (blobWrappedData.data != NULL) {
        gTal.free(blobWrappedData.data, blobWrappedData.dataSize);
        blobWrappedData.data = NULL;
    }
    if (blobUnwrappedData.data != NULL) {
        gTal.free(blobUnwrappedData.data, blobUnwrappedData.dataSize);
        blobUnwrappedData.data = NULL;
    }
    if (blobKey.data != NULL) {
        gTal.free(blobKey.data, blobKey.dataSize);
        blobKey.data = NULL;
    }
    if (blobKeyhandle.data != NULL) {
        gTal.free(blobKeyhandle.data, blobKeyhandle.dataSize);
        blobKeyhandle.data = NULL;
    }
    if (blobTlvBuf.data != NULL) {
        gTal.free(blobTlvBuf.data, blobTlvBuf.dataSize);
        blobTlvBuf.data = NULL;
    }

    return ret;
}