#include "debug.h"

#include "skpm.h"
#include "skpm_tls.h"
#include "skpm_util.h"
#include "crypto_module.h"

#define MBEDTLS_SSL_SIG_ANON                 0
#define MBEDTLS_SSL_SIG_RSA                  1
#define MBEDTLS_SSL_SIG_ECDSA                3

#define MBEDTLS_MD_SHA256                    6
#define MBEDTLS_MD_SHA384                    7

tls_session_info_t sOtaTlsSession;


void setTlsSessionServerPubkey(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t otaServerPubkey[100];
    uint16_t otaServerPubkeyLen = 0;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint32_t inputSize;

    LOGD("setTlsSessionServerPubkey started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize < ECC_PUBKEY_SIZE - 1) {
        LOGE("Input data is not correct");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    // Add uncompressed tag as 0x04
    otaServerPubkey[0] = 0x04;
    otaServerPubkeyLen = ECC_PUBKEY_SIZE - 1;
    memcpy(otaServerPubkey + 1, cmd->data, otaServerPubkeyLen);
    otaServerPubkeyLen++;

    wrappedKeyInfoSize = inputSize - 64;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(wrappedKeyInfo, cmd->data + 64, wrappedKeyInfoSize);

    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    memcpy(sOtaTlsSession.serverPubkey, otaServerPubkey, otaServerPubkeyLen);
    hex_print_tag_debug("tlsSession.serverPubkey", sOtaTlsSession.serverPubkey, ECC_PUBKEY_SIZE);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen = wrappedKeyInfoSize;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfoSize is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, wrappedKeyInfo, wrappedKeyInfoSize);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("setTlsSessionServerPubkey end : ret = %d", ret);
    rsp->ret = ret;
}

void getTlsSessionClientPubkey(p_cmd_t cmd, p_rsp_t rsp) {
    EC_KEY  *ecc_key = NULL;
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint32_t inputSize;

    LOGD("getTlsSessionClientPubkey started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    wrappedKeyInfoSize = inputSize;
    memcpy(wrappedKeyInfo, cmd->data, wrappedKeyInfoSize);

    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    if (crypto_gen_ecc_key(&ecc_key, &(sOtaTlsSession.clientkey)) != CRYPTO_STATUS_SUCCESS) {
        LOGE("getTlsSessionClientPubkey : crypto_gen_ecc_key is failed");
        ret = RET_ERR_GEN_ECC_KEY_FAIL;
        goto error;
    }

    // Without uncompressed tag as 0x04
    rsp->dataLen = sOtaTlsSession.clientkey.pubkey_size - 1;
    if (sOtaTlsSession.clientkey.pubkey_size - 1 == 0) {
        LOGE("sOtaTlsSession.clientkey.pubkey_size - 1 is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, (sOtaTlsSession.clientkey.pubkey_binary) + 1, sOtaTlsSession.clientkey.pubkey_size - 1);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen += wrappedKeyInfoSize;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data + 64, wrappedKeyInfo, wrappedKeyInfoSize);

    //hex_print_tag_debug("getTlsSessionClientPubkey", rsp->data, rsp->dataLen);

error:
    if (ecc_key != NULL) {
        crypto_clear_ecc_key(&ecc_key);
    }

    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("getTlsSessionClientPubkey end : ret = %d", ret);
    rsp->ret = ret;
}

void generateTlsSessionPremasterKey(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint32_t inputSize;

    LOGD("generateTlsSessionPremasterKey started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    wrappedKeyInfoSize = inputSize;
    memcpy(wrappedKeyInfo, cmd->data, wrappedKeyInfoSize);

    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    hex_print_tag_debug("tlsSession.serverPubkey", sOtaTlsSession.serverPubkey, ECC_PUBKEY_SIZE);
    if (crypto_gen_ecc_secret( sOtaTlsSession.clientkey.privkey_binary, sOtaTlsSession.clientkey.privkey_size, sOtaTlsSession.serverPubkey, ECC_PUBKEY_SIZE,
        sOtaTlsSession.preMasterKey) != CRYPTO_STATUS_SUCCESS) {
        LOGE("generatePremasterKey : crypto_gen_ecc_secret is failed");
        ret = RET_ERR_SIGN_FAIL;
        goto error;
    }

    hex_print_tag_debug("Generated secretkey", sOtaTlsSession.preMasterKey, ECC_SECRET_SIZE);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen = wrappedKeyInfoSize;
    memcpy(rsp->data, wrappedKeyInfo, wrappedKeyInfoSize);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("generateTlsSessionPremasterKey end : ret = %d", ret);
    rsp->ret = ret;
}

void generateTlsSessionMasterSecretKey(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    const char label[] = "master secret";
    uint8_t random[64];
    uint32_t rlen;

    uint32_t nb, dlen = TLS_MASTER_SECRET_SIZE;
    uint32_t i, j, k, md_len;
    uint8_t tmp[128];
    uint8_t h_i[SHA512_DIGEST_SIZE];

    uint8_t md_type;
    uint32_t inputSize;

    LOGD("generateTlsSessionMasterSecretKey started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    LOGD("cmd->data[0] : %u", cmd->data[0]);
    if (cmd->data[0] == MD_TYPE_SHA1) {
        md_type = MD_TYPE_SHA1;
        md_len = SHA1_DIGEST_SIZE;
    } else if (cmd->data[0] == MD_TYPE_SHA256) {
        md_type = MD_TYPE_SHA256;
        md_len = SHA256_DIGEST_SIZE;
    } else if (cmd->data[0] == MD_TYPE_SHA384) {
        md_type = MD_TYPE_SHA384;
        md_len = SHA384_DIGEST_SIZE;
    } else if (cmd->data[0] == MD_TYPE_SHA512) {
        md_type = MD_TYPE_SHA512;
        md_len = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    rlen = 64;
    memcpy(random, cmd->data + 1, rlen);
    hex_print_tag_debug("random", random, rlen);

    wrappedKeyInfoSize = inputSize - 65;
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(wrappedKeyInfo, cmd->data + 65, wrappedKeyInfoSize);
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    if (sizeof( tmp ) < md_len + strlen( label ) + rlen) {
        ret = RET_ERR_WRONG_PARAMETER;
        goto error;
    }

    nb = strlen( label );
    memcpy( tmp + md_len, label, nb );
    memcpy( tmp + md_len + nb, random, rlen );
    nb += rlen;

    /*
     * Compute P_<hash>(secret, label + random)[0..dlen]
     */
    if ( crypto_hmac( md_type, tmp, sOtaTlsSession.preMasterKey, ECC_SECRET_SIZE, tmp + md_len, nb, NULL) != CRYPTO_STATUS_SUCCESS) {
        ret = RET_ERR_TZ;
        goto error;
    }

    for ( i = 0; i < dlen; i += md_len ) {
        if ( crypto_hmac(md_type, h_i, sOtaTlsSession.preMasterKey, ECC_SECRET_SIZE, tmp, md_len + nb, NULL) != CRYPTO_STATUS_SUCCESS) {
            ret = RET_ERR_TZ;
            goto error;
        }

        if ( crypto_hmac(md_type, tmp, sOtaTlsSession.preMasterKey, ECC_SECRET_SIZE, tmp, md_len, NULL) != CRYPTO_STATUS_SUCCESS) {
            ret = RET_ERR_TZ;
            goto error;
        }

        k = ( i + md_len > dlen ) ? dlen % md_len : md_len;

        for ( j = 0; j < k; j++ ) {
            sOtaTlsSession.masterSecretKey[i + j]  = h_i[j];
        }
    }

    hex_print_tag_debug("Generated mater secret key", sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen = wrappedKeyInfoSize;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfoSize is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, wrappedKeyInfo, wrappedKeyInfoSize);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("generateTlsSessionMasterSecretKey end : ret = %d", ret);
    rsp->ret = ret;
}

void generateTlsSessionKeyBlobs(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    const char label[] = "key expansion";
    uint8_t random[64];
    uint32_t rlen;

    uint32_t nb, dlen = TLS_KEY_BLOB_SIZE;
    uint32_t i, j, k, md_len;
    uint8_t tmp[128];
    uint8_t h_i[SHA512_DIGEST_SIZE];

    uint8_t md_type;
    uint32_t inputSize;

    LOGD("generateTlsSessionKeyBlobs started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    if (cmd->data[0] == MD_TYPE_SHA1) {
        md_type = MD_TYPE_SHA1;
        md_len = SHA1_DIGEST_SIZE;
    } else if (cmd->data[0] == MD_TYPE_SHA256) {
        md_type = MD_TYPE_SHA256;
        md_len = SHA256_DIGEST_SIZE;
    } else if (cmd->data[0] == MD_TYPE_SHA384) {
        md_type = MD_TYPE_SHA384;
        md_len = SHA384_DIGEST_SIZE;
    } else if (cmd->data[0] == MD_TYPE_SHA512) {
        md_type = MD_TYPE_SHA512;
        md_len = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    rlen = 64;
    memcpy(random, cmd->data + 1, rlen);
    hex_print_tag_debug("random", random, rlen);

    wrappedKeyInfoSize = inputSize - 65;
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(wrappedKeyInfo, cmd->data + 65, wrappedKeyInfoSize);
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    if (sizeof( tmp ) < md_len + strlen( label ) + rlen) {
        ret = RET_ERR_WRONG_PARAMETER;
        goto error;
    }

    nb = strlen( label );
    memcpy( tmp + md_len, label, nb );
    memcpy( tmp + md_len + nb, random, rlen );
    nb += rlen;

    /*
     * Compute P_<hash>(secret, label + random)[0..dlen]
     */
    if ( crypto_hmac(md_type, tmp, sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE, tmp + md_len, nb, NULL) != CRYPTO_STATUS_SUCCESS) {
        ret = RET_ERR_TZ;
        goto error;
    }

    for ( i = 0; i < dlen; i += md_len ) {
        if ( crypto_hmac(md_type, h_i, sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE, tmp, md_len + nb, NULL) != CRYPTO_STATUS_SUCCESS) {
            ret = RET_ERR_TZ;
            goto error;
        }

        if ( crypto_hmac(md_type, tmp, sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE, tmp, md_len, NULL) != CRYPTO_STATUS_SUCCESS) {
            ret = RET_ERR_TZ;
            goto error;
        }

        k = ( i + md_len > dlen ) ? dlen % md_len : md_len;

        for ( j = 0; j < k; j++ ) {
            sOtaTlsSession.keyBlobs[i + j]  = h_i[j];
        }
    }

    hex_print_tag_debug("Generated key blobs", sOtaTlsSession.keyBlobs, TLS_KEY_BLOB_SIZE);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen = wrappedKeyInfoSize;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfoSize is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, wrappedKeyInfo, wrappedKeyInfoSize);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("generateTlsSessionKeyBlobs end : ret = %d", ret);
    rsp->ret = ret;
}


void generateTlsSessionFinishHash(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    char label[20] = {0, };
    uint8_t random[48];
    uint32_t rlen;
    uint8_t type;
    uint8_t md_type;

    uint8_t hash[SHA512_DIGEST_SIZE];

    uint32_t nb, dlen = 12;
    uint32_t i, j, k, md_len;
    uint8_t tmp[128];
    uint8_t h_i[SHA512_DIGEST_SIZE];

    uint32_t offset = 0;
    uint32_t inputSize;

    LOGD("generateTlsSessionClientFinishHash started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;;
        goto error;
    }

    type = cmd->data[0];
    offset++;

    memcpy(&rlen, cmd->data + offset, 4);
    offset += 4;
    if (rlen > sizeof(random)) {
        LOGE("random is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (rlen == 0) {
        LOGE("random is zero");
        ret = RET_ERR_WRONG_INPUT_FORMAT;;
        goto error;
    }
    memcpy(random, cmd->data + offset, rlen);
    offset += rlen;
    hex_print_tag_debug("random", random, rlen);
    if (rlen == 20) {
        md_type = MD_TYPE_SHA1;
        md_len= SHA1_DIGEST_SIZE;
    } else if (rlen == 32) {
        md_type = MD_TYPE_SHA256;
        md_len= SHA256_DIGEST_SIZE;
    } else if (rlen == 48) {
        md_type = MD_TYPE_SHA384;
        md_len = SHA384_DIGEST_SIZE;
    } else if (rlen == 64) {
        md_type = MD_TYPE_SHA512;
        md_len = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported hash length");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    memcpy(&wrappedKeyInfoSize, cmd->data + offset, 4);
    offset += 4;
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(wrappedKeyInfo, cmd->data + offset, wrappedKeyInfoSize);
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    if (type == 0) {
        memcpy((void*)label, "client finished", strlen("client finished") + 1);
    } else if (type == 1) {
        memcpy((void*)label, "server finished", strlen("server finished") + 1);
    } else {
        LOGE("Not supported type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    LOGD("label : %s", label);

    if (sizeof( tmp ) < md_len + strlen( label ) + rlen) {
        ret = RET_ERR_WRONG_PARAMETER;
        goto error;
    }

    nb = strlen( label );
    memcpy( tmp + md_len, label, nb );
    memcpy( tmp + md_len + nb, random, rlen );
    nb += rlen;

    /*
     * Compute P_<hash>(secret, label + random)[0..dlen]
     */
    if ( crypto_hmac(md_type, tmp, sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE, tmp + md_len, nb, NULL) != CRYPTO_STATUS_SUCCESS) {
        ret = RET_ERR_TZ;
        goto error;
    }

    for ( i = 0; i < dlen; i += md_len ) {
        if ( crypto_hmac(md_type, h_i, sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE, tmp, md_len + nb, NULL) != CRYPTO_STATUS_SUCCESS) {
            ret = RET_ERR_TZ;
            goto error;
        }

        if ( crypto_hmac(md_type, tmp, sOtaTlsSession.masterSecretKey, TLS_MASTER_SECRET_SIZE, tmp, md_len, NULL) != CRYPTO_STATUS_SUCCESS) {
            ret = RET_ERR_TZ;
            goto error;
        }

        k = ( i + md_len > dlen ) ? dlen % md_len : md_len;

        for ( j = 0; j < k; j++ ) {
            hash[i + j]  = h_i[j];
        }
    }

    hex_print_tag_debug("generateTlsSessionClientFinishHash", hash, dlen);
    rsp->dataLen = dlen;
    if (dlen == 0) {
        LOGE("dlen is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, hash, dlen);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("generateTlsSessionClientFinishHash end : ret = %d", ret);
    rsp->ret = ret;
}

void setTlsSessionKeyInfos(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint32_t inputSize;

    LOGD("setTlsSessionKeyInfos started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize < 20) {
        LOGE("Input data is not correct");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    wrappedKeyInfoSize = inputSize - 20;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(wrappedKeyInfo, cmd->data + 20, wrappedKeyInfoSize);
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    memcpy(&(sOtaTlsSession.tlsKeyLen), cmd->data, 4);
    memcpy(&(sOtaTlsSession.tlsFixed_ivlen), cmd->data + 4, 4);
    memcpy(&(sOtaTlsSession.tlsMinLen), cmd->data + 8, 4);
    memcpy(&(sOtaTlsSession.tlsIvLen), cmd->data + 12, 4);
    memcpy(&(sOtaTlsSession.tlsMacLen), cmd->data + 16, 4);

    LOGD("tlsKeyLen = %u, tlsFixed_ivlen = %u, tlsMinLen = %u, tlsIvLen = %u, tlsMacLen = %u",
        sOtaTlsSession.tlsKeyLen, sOtaTlsSession.tlsFixed_ivlen, sOtaTlsSession.tlsMinLen, sOtaTlsSession.tlsIvLen, sOtaTlsSession.tlsMacLen);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen = wrappedKeyInfoSize;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfoSize is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, wrappedKeyInfo, wrappedKeyInfoSize);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("setTlsSessionKeyInfos end : ret = %d", ret);
    rsp->ret = ret;
}

void generateTlsSessionCipherAndMacKeys(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint8_t *key1;
    uint8_t *key2;
    uint8_t *mac_enc;
    uint8_t *mac_dec;

    uint32_t iv_copy_len;
    uint32_t inputSize;

    LOGD("generateTlsSessionCipherAndMacKeys started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    wrappedKeyInfoSize = inputSize;
    memcpy(wrappedKeyInfo, cmd->data, wrappedKeyInfoSize);
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);
    if ((sOtaTlsSession.tlsMacLen * 2 + sOtaTlsSession.tlsKeyLen) > sizeof(sOtaTlsSession.keyBlobs))
    {
        LOGE("(sOtaTlsSession.tlsMacLen * 2 + sOtaTlsSession.tlsKeyLen) > sizeof(sOtaTlsSession.keyBlobs)");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    key1 = sOtaTlsSession.keyBlobs + sOtaTlsSession.tlsMacLen * 2;
    key2 = sOtaTlsSession.keyBlobs + sOtaTlsSession.tlsMacLen * 2 + sOtaTlsSession.tlsKeyLen;
    mac_enc = sOtaTlsSession.keyBlobs;
    mac_dec = sOtaTlsSession.keyBlobs + sOtaTlsSession.tlsMacLen;

    iv_copy_len = ( sOtaTlsSession.tlsFixed_ivlen ) ? sOtaTlsSession.tlsFixed_ivlen : sOtaTlsSession.tlsIvLen;
    if (iv_copy_len > sizeof(sOtaTlsSession.iv_enc)) {
        LOGE("iv_enc is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (sOtaTlsSession.tlsKeyLen > AES_256_KEY_SIZE) {
        LOGE("sOtaTlsSession.tlsKeyLen > AES_256_KEY_SIZE");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy( sOtaTlsSession.iv_enc, key2 + sOtaTlsSession.tlsKeyLen,  iv_copy_len );
    if (iv_copy_len > sizeof(sOtaTlsSession.iv_dec)) {
        LOGE("iv_enc is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy( sOtaTlsSession.iv_dec, key2 + sOtaTlsSession.tlsKeyLen + iv_copy_len, iv_copy_len );
    hex_print_tag_debug("tlsSession.iv_enc", sOtaTlsSession.iv_enc, iv_copy_len);
    hex_print_tag_debug("tlsSession.iv_dec", sOtaTlsSession.iv_dec, iv_copy_len);

    if (sOtaTlsSession.tlsMacLen > sizeof(sOtaTlsSession.mac_enc_key)) {
        LOGE("iv_enc is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy( sOtaTlsSession.mac_enc_key, mac_enc, sOtaTlsSession.tlsMacLen );
    if (sOtaTlsSession.tlsMacLen > sizeof(sOtaTlsSession.mac_dec_key)) {
        LOGE("iv_enc is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy( sOtaTlsSession.mac_dec_key, mac_dec, sOtaTlsSession.tlsMacLen );
    hex_print_tag_debug("tlsSession.mac_enc_key", sOtaTlsSession.mac_enc_key, sOtaTlsSession.tlsMacLen);
    hex_print_tag_debug("tlsSession.mac_dec_key", sOtaTlsSession.mac_dec_key, sOtaTlsSession.tlsMacLen);

    if (sOtaTlsSession.tlsKeyLen > sizeof(sOtaTlsSession.cipher_enc_key)) {
        LOGE("iv_enc is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy( sOtaTlsSession.cipher_enc_key, key1, sOtaTlsSession.tlsKeyLen );
    if (sOtaTlsSession.tlsKeyLen > sizeof(sOtaTlsSession.cipher_dec_key)) {
        LOGE("iv_enc is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy( sOtaTlsSession.cipher_dec_key, key2, sOtaTlsSession.tlsKeyLen );
    hex_print_tag_debug("tlsSession.cipher_enc_key", sOtaTlsSession.cipher_enc_key, sOtaTlsSession.tlsKeyLen);
    hex_print_tag_debug("tlsSession.cipher_dec_key", sOtaTlsSession.cipher_dec_key, sOtaTlsSession.tlsKeyLen);

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }
    rsp->dataLen = wrappedKeyInfoSize;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfoSize is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, wrappedKeyInfo, wrappedKeyInfoSize);

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("generateTlsSessionCipherAndMacKeys end : ret = %d", ret);
    rsp->ret = ret;

}

void encryptTlsSessionBuf(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;
    uint32_t offset = 0, inputSize;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint8_t mode;

    uint8_t ad[30];
    uint32_t ad_size;

    uint8_t tag[20];
    uint32_t tag_size;

    uint8_t plain[MAX_DATA_SIZE];
    uint32_t plain_size;

    uint8_t cipher[MAX_DATA_SIZE];
    uint32_t cipher_size = sizeof(cipher);
    unsigned long temp_cipher_size = sizeof(cipher);;

    LOGD("encryptTlsSessionBuf started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    memset(ad, 0, sizeof(ad));
    memset(tag, 0, sizeof(tag));

    mode = cmd->data[offset];
    offset += 1;

    memcpy(&ad_size, cmd->data + offset, 4);
    offset += 4;
    if (ad_size > sizeof(ad)) {
        LOGE("ad is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + ad_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during ad parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (ad_size != 0) {
        memcpy(ad, cmd->data + offset, ad_size);
        offset += ad_size;
    }

    memcpy(&tag_size, cmd->data + offset, 4);
    offset += 4;
    if (tag_size > sizeof(tag)) {
        LOGE("tag is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + tag_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during tag parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (tag_size != 0) {
        memcpy(tag, cmd->data + offset, tag_size);
        offset += tag_size;
    }

    memcpy(&plain_size, cmd->data + offset, 4);
    offset += 4;
    if (plain_size > sizeof(plain)) {
        LOGE("plain over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + plain_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during plain parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (plain_size != 0) {
        memcpy(plain, cmd->data + offset, plain_size);
        offset += plain_size;
    }

    if ((int)(sizeof(cmd->data) - (offset + 4)) < 0) {
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }

    memcpy(&wrappedKeyInfoSize, cmd->data + offset, 4);
    offset += 4;
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + wrappedKeyInfoSize >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during wrappedKeyInfo parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (wrappedKeyInfoSize != 0) {
        memcpy(wrappedKeyInfo, cmd->data + offset, wrappedKeyInfoSize);
        offset += wrappedKeyInfoSize;
    }
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    hex_print_tag_debug("sOtaTlsSession.cipher_enc_key", sOtaTlsSession.cipher_enc_key, sOtaTlsSession.tlsKeyLen);
    hex_print_tag_debug("sOtaTlsSession.iv_enc", sOtaTlsSession.iv_enc, sOtaTlsSession.tlsIvLen);
    if (sOtaTlsSession.tlsFixed_ivlen > sizeof(sOtaTlsSession.iv_enc)
        || 8 > (sizeof(sOtaTlsSession.iv_enc) - sOtaTlsSession.tlsFixed_ivlen)) {
        LOGE("iv is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(sOtaTlsSession.iv_enc + sOtaTlsSession.tlsFixed_ivlen, ad, 8);
    hex_print_tag_debug("sOtaTlsSession.iv_enc + extra iv", sOtaTlsSession.iv_enc, sOtaTlsSession.tlsIvLen);
    hex_print_tag_debug("ad", ad, ad_size);
    hex_print_tag_debug("plain", plain, plain_size);

    temp_cipher_size = cipher_size;

    if (sOtaTlsSession.tlsIvLen > sizeof(sOtaTlsSession.iv_enc))
    {
        LOGE("sOtaTlsSession.tlsIvLen > sizeof(sOtaTlsSession.iv_enc)");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (sOtaTlsSession.tlsKeyLen > sizeof(sOtaTlsSession.cipher_enc_key))
    {
        LOGE("sOtaTlsSession.tlsKeyLen > sizeof(sOtaTlsSession.cipher_enc_key)");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }

    if (mode == MBEDTLS_MODE_GCM) {
        if (crypto_aes_gcm_encrypt(sOtaTlsSession.iv_enc, sOtaTlsSession.tlsIvLen, sOtaTlsSession.cipher_enc_key, sOtaTlsSession.tlsKeyLen,
                                    ad, ad_size, plain, plain_size, cipher, &temp_cipher_size, tag, tag_size) != CRYPTO_STATUS_SUCCESS) {
            LOGE("crypto_aes_128_gcm_encrypt is failed");
            ret = RET_ERR_ENCRYPT_FAIL;
        }
    } else if (mode == MBEDTLS_MODE_CCM) {
        if (crypto_aes_ccm_encrypt(sOtaTlsSession.iv_enc, sOtaTlsSession.tlsIvLen, sOtaTlsSession.cipher_enc_key, sOtaTlsSession.tlsKeyLen,
                                    ad, ad_size, plain, plain_size, cipher, &temp_cipher_size, tag_size) != CRYPTO_STATUS_SUCCESS) {
            LOGE("crypto_aes_ccm_encrypt is failed");
            ret = RET_ERR_ENCRYPT_FAIL;
        }
    } else {
        LOGE("Not supported type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    cipher_size = (uint32_t)temp_cipher_size;

    hex_print_tag_debug("cipher", cipher, cipher_size);
    hex_print_tag_debug("tag", tag, tag_size);

    offset = 0;
    memcpy(rsp->data + offset, &tag_size, 4);
    offset += 4;
    if (tag_size == 0) {
        LOGE("tag is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    if (tag_size > sizeof(tag)) {
        LOGE("data buffer overrun during tag encode.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (offset + tag_size >= sizeof(rsp->data)) {
        LOGE("data buffer overflow during tag encode.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(rsp->data + offset, tag, tag_size);
    offset += tag_size;
    memcpy(rsp->data + offset, &cipher_size, 4);
    offset += 4;
    if (cipher_size == 0) {
        LOGE("cipher is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    if (cipher_size > sizeof(cipher)) {
        LOGE("data buffer overrun during cipher encode.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (offset + cipher_size >= sizeof(rsp->data)) {
        LOGE("data buffer overflow during cipher encode.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(rsp->data + offset, cipher, cipher_size);
    offset += cipher_size;

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }

    memcpy(rsp->data + offset, &wrappedKeyInfoSize, 4);
    offset += 4;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("data buffer overrun during wrappedKeyInfo encode.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (offset + wrappedKeyInfoSize >= sizeof(rsp->data)) {
        LOGE("data buffer overflow during wrappedKeyInfo encode.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(rsp->data + offset, wrappedKeyInfo, wrappedKeyInfoSize);
    offset += wrappedKeyInfoSize;

    rsp->dataLen = offset;

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("encryptTlsSessionBuf end : ret = %d", ret);
    rsp->ret = ret;
}

void decryptTlsSessionBuf(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unwrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unwrappedKeyInfoSize = sizeof(unwrappedKeyInfo);

    uint8_t mode;

    uint8_t in_iv[30];
    uint32_t in_iv_size;

    uint8_t ad[30];
    uint32_t ad_size;

    uint8_t tag[20];
    uint32_t tag_size;

    uint8_t plain[MAX_DATA_SIZE];
    uint32_t plain_size;
    unsigned long temp_plain_size;

    uint8_t cipher[MAX_DATA_SIZE];
    uint32_t cipher_size;

    uint32_t offset = 0;
    uint32_t inputSize;

    LOGD("decryptTlsSessionBuf started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    mode = cmd->data[offset];
    offset += 1;

    memcpy(&in_iv_size, cmd->data + offset, 4);
    offset += 4;
    if (in_iv_size > sizeof(in_iv)) {
        LOGE("in_iv is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + in_iv_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during in_iv parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (in_iv_size != 0) {
        memcpy(in_iv, cmd->data + offset, in_iv_size);
        offset += in_iv_size;
    }

    memcpy(&ad_size, cmd->data + offset, 4);
    offset += 4;
    if (ad_size > sizeof(ad)) {
        LOGE("ad is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + ad_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during ad parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (ad_size != 0) {
        memcpy(ad, cmd->data + offset, ad_size);
        offset += ad_size;
    }

    memcpy(&tag_size, cmd->data + offset, 4);
    offset += 4;
    if (tag_size > sizeof(tag)) {
        LOGE("tag is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + tag_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during tag parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (tag_size != 0) {
        memcpy(tag, cmd->data + offset, tag_size);
        offset += tag_size;
    }

    memcpy(&cipher_size, cmd->data + offset, 4);
    offset += 4;
    if (cipher_size > sizeof(cipher)) {
        LOGE("cipher is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + cipher_size >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during cipher parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (cipher_size != 0) {
        memcpy(cipher, cmd->data + offset, cipher_size);
        offset += cipher_size;
    }

    memcpy(&wrappedKeyInfoSize, cmd->data + offset, 4);
    offset += 4;
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (offset + wrappedKeyInfoSize >= sizeof(cmd->data)) {
        LOGE("data buffer overrun during wrappedKeyInfo parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (wrappedKeyInfoSize != 0) {
        if ((int)(sizeof(cmd->data) - (offset + wrappedKeyInfoSize)) < 0) {
            ret = RET_ERR_BUFFER_NOT_ENOUGH;
            goto error;
        }

        memcpy(wrappedKeyInfo, cmd->data + offset, wrappedKeyInfoSize);
        offset += wrappedKeyInfoSize;
    }
    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unwrappedKeyInfo, &unwrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }
    if (unwrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(&sOtaTlsSession, unwrappedKeyInfo, unwrappedKeyInfoSize);

    hex_print_tag_debug("sOtaTlsSession.cipher_dec_key", sOtaTlsSession.cipher_dec_key, sOtaTlsSession.tlsKeyLen);
    hex_print_tag_debug("in_iv", in_iv, in_iv_size);
    hex_print_tag_debug("sOtaTlsSession.iv_dec", sOtaTlsSession.iv_dec, sOtaTlsSession.tlsIvLen);
    if (in_iv_size - sOtaTlsSession.tlsFixed_ivlen > (sizeof(sOtaTlsSession.iv_dec) - sOtaTlsSession.tlsFixed_ivlen)) {
        LOGE("iv is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(sOtaTlsSession.iv_dec + sOtaTlsSession.tlsFixed_ivlen, in_iv + sOtaTlsSession.tlsFixed_ivlen, in_iv_size - sOtaTlsSession.tlsFixed_ivlen);
    hex_print_tag_debug("sOtaTlsSession.iv_dec + in_iv", sOtaTlsSession.iv_dec, sOtaTlsSession.tlsIvLen);
    hex_print_tag_debug("ad", ad, ad_size);
    hex_print_tag_debug("tag", tag, tag_size);
    hex_print_tag_debug("cipher", cipher, cipher_size);

    plain_size = cipher_size;
    temp_plain_size = plain_size;

    if (sOtaTlsSession.tlsIvLen > sizeof(sOtaTlsSession.iv_dec))
    {
        LOGE("sOtaTlsSession.tlsIvLen > sizeof(sOtaTlsSession.iv_dec)");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (sOtaTlsSession.tlsKeyLen > sizeof(sOtaTlsSession.cipher_dec_key))
    {
        LOGE("sOtaTlsSession.tlsKeyLen > sizeof(sOtaTlsSession.cipher_dec_key)");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }

    if (mode == MBEDTLS_MODE_GCM) {
        if (crypto_aes_gcm_decrypt(sOtaTlsSession.iv_dec, sOtaTlsSession.tlsIvLen, sOtaTlsSession.cipher_dec_key, sOtaTlsSession.tlsKeyLen,
                                    ad, ad_size, cipher, cipher_size, plain, &temp_plain_size, tag, tag_size) != CRYPTO_STATUS_SUCCESS) {
            LOGE("crypto_aes_128_gcm_decrypt is failed");
            ret = RET_ERR_ENCRYPT_FAIL;
        }

    } else if (mode == MBEDTLS_MODE_CCM) {
        if (crypto_aes_ccm_decrypt(sOtaTlsSession.iv_dec, sOtaTlsSession.tlsIvLen, sOtaTlsSession.cipher_dec_key, sOtaTlsSession.tlsKeyLen,
                                    ad, ad_size, cipher, cipher_size, plain, &temp_plain_size, tag_size) != CRYPTO_STATUS_SUCCESS) {
            LOGE("crypto_aes_ccm_decrypt is failed");
            ret = RET_ERR_ENCRYPT_FAIL;
        }
    } else {
        LOGE("Not supported type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    plain_size = (uint32_t)temp_plain_size;
    hex_print_tag_debug("plain", plain, plain_size);

    offset = 0;
    memcpy(rsp->data + offset, &tag_size, 4);
    offset += 4;
    if (tag_size == 0) {
        LOGE("tag is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    if (tag_size > sizeof(tag)) {
        LOGE("tag buffer overrun during tag parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (offset + tag_size >= sizeof(rsp->data)) {
        LOGE("data buffer overrun during tag parsing.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(rsp->data + offset, tag, tag_size);
    offset += tag_size;
    memcpy(rsp->data + offset, &plain_size, 4);
    offset += 4;
    if (plain_size == 0) {
        LOGE("plain is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    if (plain_size > sizeof(plain)) {
        LOGE("plain buffer overrun during plain parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (offset + plain_size >= sizeof(rsp->data)) {
        LOGE("data buffer overrun during plain parsing.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(rsp->data + offset, plain, plain_size);
    offset += plain_size;

    wrappedKeyInfoSize = sizeof(wrappedKeyInfo);
    if (wrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, (uint8_t *)&sOtaTlsSession, sizeof(tls_session_info_t), wrappedKeyInfo, &wrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_WRAPPING_FAIL;
        goto error;
    }

    memcpy(rsp->data + offset, &wrappedKeyInfoSize, 4);
    offset += 4;
    if (wrappedKeyInfoSize == 0) {
        LOGE("wrappedKeyInfo is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo buffer overrun during wrappedKeyInfo parsing.");
        ret = RET_ERR_SRC_BUFFER_OVERFLOW;
        goto error;
    }
    if (offset + wrappedKeyInfoSize >= sizeof(rsp->data)) {
        LOGE("data buffer overrun during wrappedKeyInfo parsing.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(rsp->data + offset, wrappedKeyInfo, wrappedKeyInfoSize);
    offset += wrappedKeyInfoSize;

    rsp->dataLen = offset;

error:
    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("decryptTlsSessionBuf end : ret = %d", ret);
    rsp->ret = ret;
}

void signWithClientKey(p_cmd_t cmd, p_rsp_t rsp) {
    int32_t ret = RET_SUCCESS;

    uint8_t wrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t wrappedKeyInfoSize;

    uint8_t unWrappedKeyInfo[WRAPPED_KEY_INFO_SIZE];
    uint32_t unWrappedKeyInfoSize = WRAPPED_KEY_INFO_SIZE;

    RSA *rsa_key = NULL;

    uint8_t key_type;
    uint8_t md_type;

    uint8_t input[100];
    uint32_t inputLen = 0;

    uint8_t signature[MAX_SIGNATURE_SIZE];
    uint32_t signatureLen = MAX_SIGNATURE_SIZE;

    uint8_t raw_rsa_sig[MAX_SIGNATURE_SIZE];
    uint32_t raw_rsa_sig_Len = MAX_SIGNATURE_SIZE;

    uint32_t offset = 0;
    uint32_t inputSize;

    LOGD("signWithClientKey started");
    inputSize = cmd->dataLen;
    if (inputSize > WRAPPED_KEY_INFO_SIZE) {
        LOGE("Not correct input format");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (inputSize == 0) {
        LOGE("cmd->data is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    key_type = cmd->data[offset++];
    md_type = cmd->data[offset++];
    if (md_type == MBEDTLS_MD_SHA256) {
        md_type = MD_TYPE_SHA256;
    } else if (md_type == MBEDTLS_MD_SHA384) {
        md_type = MD_TYPE_SHA384;
    } else {
        LOGE("Not supported MD type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }
    memcpy(&inputLen, cmd->data + offset, 4);
    offset += 4;
    if (inputLen > sizeof(input)) {
        LOGE("input is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    memcpy(input, cmd->data + offset, inputLen);
    offset += inputLen;
    hex_print_tag_debug("input", input, inputLen);
    memcpy(&wrappedKeyInfoSize, cmd->data + offset, 4);
    offset += 4;
    if (wrappedKeyInfoSize > sizeof(wrappedKeyInfo)) {
        LOGE("wrappedKeyInfo is over the buffer.");
        ret = RET_ERR_BUFFER_NOT_ENOUGH;
        goto error;
    }
    if (wrappedKeyInfoSize != 0) {
        memcpy(wrappedKeyInfo, cmd->data + offset, wrappedKeyInfoSize);
        offset += wrappedKeyInfoSize;
    }

    if (unwrapInternalData(SECURE_OBJECT_TYPE_TLS_SESSION_INFO, wrappedKeyInfo, wrappedKeyInfoSize, unWrappedKeyInfo, &unWrappedKeyInfoSize) != STATUS_SUCCESS) {
        ret = RET_ERR_DATA_UNWRAPPING_FAIL;
        goto error;
    }

    if (unWrappedKeyInfoSize != sizeof(tls_session_info_t)) {
        LOGE("tls_session_info_t size is not fit");
        ret = RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }

    memcpy((uint8_t *)&sOtaTlsSession, unWrappedKeyInfo, unWrappedKeyInfoSize);

    if (key_type == MBEDTLS_SSL_SIG_RSA) {
        LOGD("Sign with RSA");
        if ( (sOtaTlsSession.serviceRsaKey.modulus_size > sizeof(sOtaTlsSession.serviceRsaKey.modulus)) ||
            (sOtaTlsSession.serviceRsaKey.publicExponent_size > sizeof(sOtaTlsSession.serviceRsaKey.publicExponent)) ||
            (sOtaTlsSession.serviceRsaKey.privateExponent_size > sizeof(sOtaTlsSession.serviceRsaKey.privateExponent)) ||
            (sOtaTlsSession.serviceRsaKey.prime1_size > sizeof(sOtaTlsSession.serviceRsaKey.prime1)) ||
            (sOtaTlsSession.serviceRsaKey.prime2_size > sizeof(sOtaTlsSession.serviceRsaKey.prime2)) ||
            (sOtaTlsSession.serviceRsaKey.exponent1_size > sizeof(sOtaTlsSession.serviceRsaKey.exponent1)) ||
            (sOtaTlsSession.serviceRsaKey.exponent2_size > sizeof(sOtaTlsSession.serviceRsaKey.exponent2)) ||
            (sOtaTlsSession.serviceRsaKey.coefficient_size > sizeof(sOtaTlsSession.serviceRsaKey.coefficient)) )
        {
            LOGE("Invalid sOtaTlsSession.serviceRsaKey structure");
            ret = RET_ERR_BUFFER_NOT_ENOUGH;
            goto error;
        }
        if (crypto_regen_rsa_privkey(&rsa_key, &sOtaTlsSession.serviceRsaKey) != CRYPTO_STATUS_SUCCESS) {
            LOGE("signWithClientKey : crypto_regen_rsa_privkey is failed");
            ret = RET_ERR_REGEN_RSA_KEY_FAIL;
            goto error;
        }
        if (crypto_rsa_sign_pkcs(md_type, 1, rsa_key, input, inputLen, raw_rsa_sig, &raw_rsa_sig_Len ) != CRYPTO_STATUS_SUCCESS) {
            LOGE("signWithClientKey : crypto_rsa_sign_pkcs is failed");
            ret = RET_ERR_SIGN_FAIL;
            goto error;
        }
        hex_print_tag_debug("raw_rsa_sig", raw_rsa_sig, raw_rsa_sig_Len);
        memcpy(signature, raw_rsa_sig, raw_rsa_sig_Len);
        signatureLen = raw_rsa_sig_Len;
#ifdef SUPPORT_GUARDIAN_M
    } else if (key_type == MBEDTLS_SSL_SIG_ECDSA) {
        LOGD("Sign with EC key");

        if (sOtaTlsSession.isGuardianM == 1) {
            LOGD("Guardian M mode");
            if (guardianmSignWithClientKey(MD_TYPE_NONE, input, inputLen, signature, &signatureLen) != CRYPTO_STATUS_SUCCESS) {
                LOGE("signWithClientKey : guardianmSignWithClientKey is failed");
                ret = RET_ERR_SIGN_FAIL;
                goto error;
            }
        } else {
            LOGE("signWithClientKey : Not supported type");
            ret = RET_ERR_NOT_SUPPORT;
            goto error;
        }
#endif
    } else {
        LOGE("signWithClientKey : Not supported type");
        ret = RET_ERR_NOT_SUPPORT;
        goto error;
    }

    hex_print_tag_debug("signature", signature, signatureLen);
    rsp->dataLen = signatureLen;
    if (signatureLen == 0) {
        LOGE("signatureLen is zero");
        ret =  RET_ERR_WRONG_INPUT_FORMAT;
        goto error;
    }
    memcpy(rsp->data, signature, signatureLen);

error:
    if (rsa_key != NULL) {
        crypto_clear_rsa_key(&rsa_key);
    }

    secure_memclear(&sOtaTlsSession, sizeof(tls_session_info_t));
    LOGD("signWithClientKey end : ret = %d", ret);
    rsp->ret = ret;
}