#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include "ta_logger.h"

#include "tzWrappers/TzwMemory.h"
#include "tzWrappers/TzwString.h"
#include "tzWrappers/TzwAuth.h"

#include "ifaa_ta_biz.h"
#include "ifaa_ta_common.h"
#include "ifaa_ta_vendor.h"
#include "ifaa_mem_utils.h"
#include "ifaa_version_number.h"
#include "ifaa_fingerprint_id_table.h"

#define EXTRACT_DATA(data)                              \
    vlb_t data = {NULL,0};                              \
    if (in_len < sizeof(uint32_t)) {                    \
        LOG_E("**NO data available**");                 \
        ret = IFAA_ERR_BUF_TOO_SHORT;                   \
        break;                                          \
    }                                                   \
    {                                                   \
        uint32_t total_len;                             \
        data.len = read32(in_ptr);                      \
        total_len = sizeof(uint32_t) + data.len;        \
        if (in_len < total_len) {                       \
            LOG_E("*IFAA_ERR_BUF_TOO_SHORT*");          \
            ret = IFAA_ERR_BUF_TOO_SHORT;               \
            break;                                      \
        }                                               \
        data.buf = in_ptr + sizeof(uint32_t);           \
        in_ptr += total_len;                            \
        in_len -= total_len;                            \
        logByteArrayHex(data.buf, data.len, #data);     \
    }

IFAA_Result IFAA_TaInvokeCmd(uint8_t *in, uint32_t in_len, uint8_t *out, uint32_t *out_len) {
    IFAA_Result ret = IFAA_ERR_UNKNOWN;
    vlb_t res = {0, };
    uint8_t *in_ptr = in;
    uint32_t cmd_id = 0;

    if (in == NULL || out == NULL || in_len == 0 || out_len == NULL) {
        ret = IFAA_ERR_BAD_PARAM;
        LOG_E("Invalid parameters");
        goto exit;
    }

#ifdef __DEV_DEBUG__
    logByteArrayHex(in, in_len, "request data");
#endif

    in_ptr += sizeof(uint32_t);
    do {
        EXTRACT_DATA(signature);
        EXTRACT_DATA(package_name);
    } while (false);
	
    if (ret == IFAA_ERR_BUF_TOO_SHORT) {
        LOG_E("input parameters is too short, in_len is : 0x%08x", in_len);
        goto exit;
	} else {
        if (in_len >= sizeof(uint32_t)) {
            LOG_D("in_len is : 0x%08x", in_len);
            cmd_id = read32(in_ptr);
            in_ptr += sizeof(uint32_t);	
        } else {
            LOG_E("input parameters is less than uint32, in_len is : 0x%08x", in_len);
            goto exit;
        }
    }

    LOG_I("cmd_id: 0x%08x(%s)", cmd_id, IFAA_getCmdString(cmd_id));

#ifdef __IFAA_TEST_MODE
    // TODO: memory MIGHT leakage in some case(DO NOT CARE)
    do{
        uint32_t shrink_size = *out_len - 0x8;
        res.buf = (uint8_t *) tzwMalloc(shrink_size);
        if (NULL == res.buf) {
            LOG_E("malloc failed.");
            return IFAA_ERR_OUT_OF_MEM;
        }

        res.len = shrink_size;
        LOG_D("out_len is : 0x%08x, res.len: 0x%08x", *out_len, res.len);
    }while(0);
#endif

    switch (cmd_id) {
        case IFAA_TA_CMD_GET_DEVICE_ID: {
            ret = ifaa_tz_get_device_id(&res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("failed to get deviceid");
                res.len = 0;
            }
            break;
        }

        case IFAA_TA_CMD_REGISTER: {
            EXTRACT_DATA(req);
            ret = ifaa_tz_register(&req, &res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("failed to register");
                res.len = 0;
            }
            break;
        }

        case IFAA_TA_CMD_AUTHENTICATE: {
            EXTRACT_DATA(req);
            ret = ifaa_tz_authenticate(&req, &res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("failed to authenticate");
                res.len = 0;
            }
            break;
        }

        case IFAA_TA_CMD_DEREGISTER: {
            EXTRACT_DATA(req);
            ret = ifaa_tz_deregister(&req);
            res.len = 0;
            break;
        }

        case IFAA_TA_CMD_QUERY_STATUS: {
            EXTRACT_DATA(token);
            ret = ifaa_tz_query_status(&token, &res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("failed to get status");
                res.len = 0;
            }
            break;
        }

        case IFAA_TA_CMD_GET_PROTOCOL_VERSION: {
            ret = ifaa_tz_get_version(&res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("failed to get version");
                res.len = 0;
            }
            break;
        }

        case IFAA_TA_CMD_GET_SKPM_INJECT_STATE: {
            ret = ifaa_tz_get_skpm_inject_state();
            res.len = 0;
            res.buf = NULL;
        }
        break;

        case IFAA_TA_CMD_PROVISIONING_KEY_HANDLE: {
            LOG_D("cmd : IFAA_TA_CMD_PROVISIONING_KEY_HANDLE");
            uint32_t skpm_key_handle_length = read32(in_ptr);
            LOG_D("skpm_key_handle_length:%d", skpm_key_handle_length);
            skpm_key_handle_length += sizeof(uint32_t);
            LOG_D("skpm_key_handle_length:%d", skpm_key_handle_length);
            ret = ifaa_tz_get_skpm_inject_state();
            if (ret != IFAA_ERR_SUCCESS) {
                ret = ifaa_tz_skpm_provision_operation(skpm_key_handle_length, in_ptr);
            }
//            ret =-1;
            res.len = 0;
            res.buf = NULL;
            break;
        }
        case IFAA_TA_CMD_SET_ID_LIST: {
            LOG_D("cmd: set id list");
            EXTRACT_DATA(idlist);
            ret = ifaa_tz_update_fid(&idlist);
            if(ret != IFAA_ERR_SUCCESS) {
                LOG_E("IFAA_TA_CMD_SET_ID_LIST failed ret = 0x%08x", ret);
            } 
            res.len = 0;
            res.buf = NULL;
            break;
        }
        case IFAA_TA_CMD_GET_ID_LIST: {
            LOG_D("cmd : IFAA_TA_CMD_GET_ID_LIST, in_len = %d", in_len);
            EXTRACT_DATA(idlist);
            ret = ifaa_tz_update_fid(&idlist);
            if(ret != IFAA_ERR_SUCCESS) {
                LOG_E("IFAA_TA_CMD_GET_ID_LIST failed ret = 0x%08x", ret);
            }
            res.buf = (uint8_t *) tzwMalloc(8*FID_TABLE_MAX);
            if(res.buf == NULL) {
                res.len = 0;
                ret = IFAA_ERR_MALLOC_FAILED;
                break;
            }
            ret = IFAA_GetFpEnrolledIdList(res.buf, &res.len);
            if (ret != IFAA_ERR_SUCCESS || res.len % 8 != 0) {
                LOG_E("IFAA_TA_CMD_GET_ID_LIST  get idlist failed., ret = 0x%08x, len = %d", ret, res.len);
                ret = IFAA_ERR_GET_ID_LIST; 
                res.len = 0; 
            } else {
#ifdef __DEV_DEBUG__
                int cnt = res.len/8;
                LOG_D("enrolled id  cnt = %d", cnt);
                for(int i = 0; i<cnt;i++) {
                    int offset = 8*(i+1)-4;
                    LOG_I("id[%d] = %u", i, read32(res.buf+offset));
                }
#endif
            }
            break;
        }
        case IFAA_TA_CMD_GET_SEC_DEVICE_ID: {
            EXTRACT_DATA(req);
            ret = ifaa_tz_get_sec_device_id(&req, &res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("fail to get sec deviceid");
                res.len = 0;
            }
        }
        break;
		
#ifdef __IFAA_TEST_MODE
        case IFAA_TA_CMD_GET_CERT_ALG_ENCODE: {
            LOG_D("cmd: (test - pass) get cert alg");

            ret = ifaa_tz_get_optcertalg(&res);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("failed to get cert alg");
                res.len = 0;
            }
        }
        break;

        case IFAA_TA_CMD_READ_FILE: {
            LOG_D("cmd: (test - pass) read file");

            EXTRACT_DATA(path);
            ret = IFAA_ReadFile((const char *) path.buf, path.len, res.buf, &res.len);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_ReadFile failed: 0x%08x", ret);
                res.len = 0;
            }else{
                LOG_D("IFAA_ReadFile read len: 0x%08x", res.len);
            }
        }
        break;

        case IFAA_TA_CMD_WRITE_FILE: {
            LOG_D("cmd: (test - pass) write file");

            EXTRACT_DATA(path);
            EXTRACT_DATA(write_data);

            ret = IFAA_WriteFile((const char *)path.buf, path.len, write_data.buf, write_data.len);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_WriteFile failed: 0x%08x", ret);
            }
            res.len = 0;
        }
        break;

        case IFAA_TA_CMD_DELETE_FILE: {
            LOG_D("cmd: (test - pass) delete file");

            EXTRACT_DATA(path);
            ret = IFAA_DeleteFile((const char *) path.buf, path.len);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_DeleteFile failed: 0x%08x", ret);
            }
            res.len = 0;
        }
        break;

        case IFAA_INTERNAL_SHA256: {
            LOG_D("cmd: (test - pass) SHA256");
            EXTRACT_DATA(msg);
            ret = IFAA_Sha256(msg.buf, msg.len, res.buf, &(res.len));
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_Sha256 failed: 0x%08x", ret);
                res.len = 0;
            }
            LOG_D("ret is %x", ret);
        }
        break;

        case IFAA_INTERNAL_SIGN: {
            LOG_D("cmd: (test - pass) sign");
            IFAA_RsaKey key_tmp;
            key_tmp.n.buf = NULL;
            key_tmp.d.buf = NULL;
            key_tmp.e.buf = NULL;
            uint8_t *key_data = (uint8_t*)tzwMalloc(2048);
            if (NULL == key_data) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            uint8_t *key_data_tmp = key_data;
            uint32_t key_data_len = 2048;
            memset(key_data, 0, 2048);
            LOG_D("Enter IFAA_INTERNAL_SIGN");

            EXTRACT_DATA(key_file_path);
            EXTRACT_DATA(digest);
            IFAA_ReadFile((char*)key_file_path.buf, key_file_path.len, key_data, &key_data_len);

            key_tmp.n.len = read32(key_data);
            key_data += 4;
            key_tmp.n.buf = (uint8_t*)tzwMalloc(key_tmp.n.len);
            if (NULL == key_tmp.n.buf) {
                tzwFree(key_data_tmp);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.n.buf, 0, key_tmp.n.len);
            memcpy(key_tmp.n.buf, key_data, key_tmp.n.len);
            key_data += key_tmp.n.len;

            key_tmp.e.len = read32(key_data);
            key_data += 4;
            key_tmp.e.buf = (uint8_t*)tzwMalloc(key_tmp.e.len);
            if (NULL == key_tmp.e.buf) {
                tzwFree(key_data_tmp);
                tzwFree(key_tmp.n.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.e.buf, 0, key_tmp.e.len);
            memcpy(key_tmp.e.buf, key_data, key_tmp.e.len);
            key_data += key_tmp.e.len;

            key_tmp.d.len = read32(key_data);
            key_data += 4;
            key_tmp.d.buf = (uint8_t*)tzwMalloc(key_tmp.d.len);
            if (NULL == key_tmp.d.buf) {
                tzwFree(key_data_tmp);
                tzwFree(key_tmp.n.buf);
                tzwFree(key_tmp.e.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.d.buf, 0, key_tmp.d.len);
            memcpy(key_tmp.d.buf, key_data, key_tmp.d.len);
            key_data += key_tmp.d.len;

#ifdef __DEV_DEBUG__
            LOG_D("key_tmp.n.len: %u", key_tmp.n.len);
            LOG_D("key_tmp.e.len: %u", key_tmp.e.len);
            LOG_D("key_tmp.d.len: %u", key_tmp.d.len);
            logByteArrayHex(digest.buf, digest.len, "digest");
#endif
            ret = IFAA_RsaSignDigest(&key_tmp, digest.buf, digest.len, res.buf, &(res.len));

            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_RsaSignDigest failed: 0x%08x", ret);
                res.len = 0;
            }else{
#ifdef __DEV_DEBUG__
            logByteArrayHex(res.buf, res.len, "signed digest");
#endif
            }

            if (key_tmp.n.buf != NULL) {
                tzwFree(key_tmp.n.buf);
                key_tmp.n.buf = NULL;
            }

            if (key_tmp.d.buf != NULL) {
                tzwFree(key_tmp.d.buf);
                key_tmp.d.buf = NULL;
            }

            if (key_tmp.e.buf != NULL) {
                tzwFree(key_tmp.e.buf);
                key_tmp.e.buf = NULL;
            }

            if (key_data_tmp != NULL) {
                tzwFree(key_data_tmp);
                key_data_tmp = NULL;
            }
        }
        break;

        case IFAA_INTERNAL_VERIFY: {
            LOG_D("cmd: (test - pass) verify");
            IFAA_RsaKey key_tmp;
            vlb_t digest_tmp;
            key_tmp.n.buf = NULL;
            key_tmp.d.buf = NULL;
            key_tmp.e.buf = NULL;
            digest_tmp.buf = NULL;
            uint8_t *key_data = (uint8_t*)tzwMalloc(2048);
            if (NULL == key_data) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            uint8_t *key_data_tmp = key_data;
            uint32_t key_data_len = 2048;
            memset(key_data, 0, 2048);
            LOG_D("Enter IFAA_INTERNAL_VERIFY");
            EXTRACT_DATA(key_file_path);
            EXTRACT_DATA(digest);
            EXTRACT_DATA(sig);
            IFAA_ReadFile((char*)key_file_path.buf, key_file_path.len, key_data, &key_data_len);

            key_tmp.n.len = read32(key_data);
            key_data += 4;
            key_tmp.n.buf = (uint8_t*)tzwMalloc(key_tmp.n.len);
            if (NULL == key_tmp.n.buf) {
                tzwFree(key_data_tmp);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.n.buf, 0, key_tmp.n.len);
            memcpy(key_tmp.n.buf, key_data, key_tmp.n.len);
            key_data += key_tmp.n.len;
            key_tmp.e.len = read32(key_data);
            key_data += 4;
            key_tmp.e.buf = (uint8_t*)tzwMalloc(key_tmp.e.len);
            if (NULL == key_tmp.e.buf) {
                tzwFree(key_data_tmp);
                tzwFree(key_tmp.n.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.e.buf, 0, key_tmp.e.len);
            memcpy(key_tmp.e.buf, key_data, key_tmp.e.len);
            key_data += key_tmp.e.len;
            key_tmp.d.len = read32(key_data);
            key_data += 4;
            key_tmp.d.buf = (uint8_t*)tzwMalloc(key_tmp.d.len);
            if (NULL == key_tmp.d.buf) {
                tzwFree(key_data_tmp);
                tzwFree(key_tmp.n.buf);
                tzwFree(key_tmp.e.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.d.buf, 0, key_tmp.d.len);
            memcpy(key_tmp.d.buf, key_data, key_tmp.d.len);
            key_data += key_tmp.d.len;

            digest_tmp.len = digest.len;
            digest_tmp.buf = (uint8_t*)tzwMalloc(digest.len);
            if (NULL == digest_tmp.buf) {
                tzwFree(key_data_tmp);
                tzwFree(key_tmp.n.buf);
                tzwFree(key_tmp.e.buf);
                tzwFree(key_tmp.d.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(digest_tmp.buf, 0, digest.len);
            memcpy(digest_tmp.buf, digest.buf, digest.len);

            ret = IFAA_RsaVerifyDigest(&key_tmp, digest_tmp.buf, digest_tmp.len, sig.buf, sig.len);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_RsaVerifyDigest failed: 0x%08x", ret);
            }

            res.len = 0;
            if (key_tmp.n.buf != NULL) {
                tzwFree(key_tmp.n.buf);
                key_tmp.n.buf = NULL;
            }

            if (key_tmp.e.buf != NULL) {
                tzwFree(key_tmp.e.buf);
                key_tmp.e.buf = NULL;
            }

            if (key_tmp.d.buf != NULL) {
                tzwFree(key_tmp.d.buf);
                key_tmp.d.buf = NULL;
            }

            if (key_data_tmp != NULL) {
                tzwFree(key_data_tmp);
                key_data_tmp = NULL;
            }

            if (digest_tmp.buf != NULL) {
                tzwFree(digest_tmp.buf);
                digest_tmp.buf = NULL;
            }

            break;
        }

        case IFAA_INTERNAL_KEY_GENERATE: {
            LOG_D("cmd: (test - pass) generate key");
            IFAA_RsaKey key_tmp;
            uint8_t *write_buf = (uint8_t*)tzwMalloc(2048);
            if (NULL == write_buf) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(write_buf, 0, 2048);
            uint8_t *write_buf_tmp = write_buf;
            uint32_t write_len = 0;
            vlb_t key_store_file;
            key_store_file.buf = (uint8_t*) "keyfile";
            key_store_file.len = strlen((const char*)key_store_file.buf);
            LOG_D("key_store_file.len is %d", key_store_file.len);
            key_tmp.n.buf = NULL;
            key_tmp.d.buf = NULL;
            key_tmp.e.buf = NULL;
            LOG_D("Enter IFAA_INTERNAL_KEY_GENEMRATE");
            EXTRACT_DATA(bits_tmp);
            read32(bits_tmp.buf);
            LOG_D("bits_tmp len is %x", bits_tmp.len);
            uint32_t ii = 0;
            for (ii = 0; ii < bits_tmp.len; ii++) {
                LOG_D("bits buf is %x", bits_tmp.buf[ii]);
            }

            key_tmp.n.buf = (uint8_t*)tzwMalloc(1024);
            if (NULL == key_tmp.n.buf) {
                tzwFree(write_buf_tmp);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            key_tmp.d.buf = (uint8_t*)tzwMalloc(1024);
            if (NULL == key_tmp.d.buf) {
                tzwFree(write_buf_tmp);
                tzwFree(key_tmp.n.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            key_tmp.e.buf = (uint8_t*)tzwMalloc(1024);
            if (NULL == key_tmp.e.buf) {
                tzwFree(write_buf_tmp);
                tzwFree(key_tmp.n.buf);
                tzwFree(key_tmp.d.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(key_tmp.n.buf, 0, 1024);
            memset(key_tmp.d.buf, 0, 1024);
            memset(key_tmp.e.buf, 0, 1024);

            key_tmp.n.len = 1024;
            key_tmp.d.len = 1024;
            key_tmp.e.len = 1024;

            ret = IFAA_RsaKeyGenerate(RSA_BITS_2048, &key_tmp);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_RsaKeyGenerate failed: 0x%08x", ret);
                res.len = 0;
            }
#ifdef __DEV_DEBUG__
            logByteArrayHex(key_tmp.n.buf, key_tmp.n.len, "rsa n: ");
            logByteArrayHex(key_tmp.d.buf, key_tmp.d.len, "rsa d: ");
            logByteArrayHex(key_tmp.e.buf, key_tmp.e.len, "rsa e: ");
#endif

            if (IFAA_ERR_SUCCESS == ret) {
                uint32_t  temp_len = 2048;
                if(temp_len < key_tmp.n.len + sizeof(uint32_t)){
                    LOG_E("IFAA_INTERNAL_KEY_GENERATE key_tmp.n ,can't use memcpy operation because of overflow.");
                    return IFAA_ERR_BUF_TOO_SHORT;
                }
                write32(write_buf, key_tmp.n.len);
                write_buf += sizeof(uint32_t);
                memcpy(write_buf, key_tmp.n.buf, key_tmp.n.len);
                write_buf += key_tmp.n.len;
                temp_len = temp_len - key_tmp.n.len - sizeof(uint32_t);

                if(temp_len < key_tmp.e.len + sizeof(uint32_t)){
                    LOG_E("IFAA_INTERNAL_KEY_GENERATE key_tmp.e,can't use memcpy operation because of overflow.");
                    return IFAA_ERR_BUF_TOO_SHORT;
                }
                write32(write_buf, key_tmp.e.len);
                write_buf += sizeof(uint32_t);
                memcpy(write_buf, key_tmp.e.buf, key_tmp.e.len);
                write_buf += key_tmp.e.len;
                temp_len = temp_len - key_tmp.e.len - sizeof(uint32_t);

                if(temp_len < key_tmp.d.len + sizeof(uint32_t)){
                    LOG_E("IFAA_INTERNAL_KEY_GENERATE key_tmp.d,can't use memcpy operation because of overflow.");
                    return IFAA_ERR_BUF_TOO_SHORT;
                }
                write32(write_buf, key_tmp.d.len);
                write_buf += sizeof(uint32_t);
                memcpy(write_buf, key_tmp.d.buf, key_tmp.d.len);
                write_buf += key_tmp.d.len;
                temp_len = temp_len - key_tmp.d.len - sizeof(uint32_t);
            }

            write_len = write_buf - write_buf_tmp;
            uint8_t *send_buf = (uint8_t*)tzwMalloc(2048);
            if (NULL == send_buf) {
                tzwFree(write_buf_tmp);
                tzwFree(key_tmp.n.buf);
                tzwFree(key_tmp.d.buf);
                tzwFree(key_tmp.e.buf);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(send_buf, 0, 2048);
            uint8_t *send_buf_tmp = send_buf;
            uint32_t temp_len = 2048;

            if(temp_len < 4 + key_store_file.len){
                LOG_E("IFAA_INTERNAL_KEY_GENERATE key_store_file ,can't use memcpy operation because of overflow.");
                return IFAA_ERR_BUF_TOO_SHORT;
            }
            write32(send_buf, key_store_file.len);
            memcpy(send_buf + 4, key_store_file.buf, key_store_file.len);

            if(res.len < key_store_file.len + 4){
                LOG_E("IFAA_INTERNAL_KEY_GENERATE res.buf ,can't use memcpy operation because of overflow.");
                return IFAA_ERR_BUF_TOO_SHORT;
            }
            memcpy(res.buf, send_buf, key_store_file.len + 4);
            res.len = key_store_file.len + 4;
            ret = IFAA_WriteFile((const char *) key_store_file.buf, key_store_file.len,
                                 write_buf_tmp, write_len);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_WriteFile failed: 0x%08x", ret);
            }

            if (key_tmp.n.buf != NULL) {
                tzwFree(key_tmp.n.buf);
                key_tmp.n.buf = NULL;
            }

            if (key_tmp.d.buf != NULL) {
                tzwFree(key_tmp.d.buf);
                key_tmp.d.buf = NULL;
            }

            if (key_tmp.e.buf != NULL) {
                tzwFree(key_tmp.e.buf);
                key_tmp.e.buf = NULL;
            }

            if (write_buf != NULL) {
                tzwFree(write_buf_tmp);
                write_buf = NULL;
            }

            if (send_buf != NULL) {
                tzwFree(send_buf_tmp);
                send_buf = NULL;
            }
            break;
        }

        case IFAA_INTERNAL_ECC_KEY_GENERATE:
        {
            LOG_D("Enter IFAA_INTERNAL_ECC_KEY_GENEMRATE");
            uint8_t p[__LEN_ECC256] = { 0 };
            uint8_t x[__LEN_ECC256] = { 0 };
            uint8_t y[__LEN_ECC256] = { 0 };

            IFAA_EccKey key;
            key.p.buf = p;
            key.p.len = __LEN_ECC256;
            key.x.buf = x;
            key.x.len = __LEN_ECC256;
            key.y.buf = y;
            key.y.len = __LEN_ECC256;

            uint8_t *write_buf = tzwMalloc(2048);
            if (NULL == write_buf) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(write_buf, 0 , 2048);
            uint8_t *write_buf_tmp = write_buf;
            uint32_t write_len = 0;
            vlb_t key_store_file;
            key_store_file.buf = (uint8_t *)"ecckeyfile";
            key_store_file.len = strlen((const char *)key_store_file.buf);
            LOG_D("key_store_file.len is %d", key_store_file.len);

            ret = IFAA_EccKeyGenerate(&key);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("ecc_key_gen failed");
                return ret;
            }

            if (IFAA_ERR_SUCCESS == ret) {
                write32(write_buf, ECC_MAGIC);
                write_buf += sizeof(uint32_t);

                write32(write_buf , key.p.len);
                write_buf  += sizeof(uint32_t);
                memcpy(write_buf , key.p.buf, key.p.len);
                write_buf  += key.p.len;

                write32(write_buf , key.x.len);
                write_buf += sizeof(uint32_t);
                memcpy(write_buf , key.x.buf, key.x.len);
                write_buf  += key.x.len;

                write32(write_buf , key.y.len);
                write_buf  += sizeof(uint32_t);
                memcpy(write_buf , key.y.buf, key.y.len);
                write_buf += key.y.len;
            }

            //write to file
            write_len = write_buf - write_buf_tmp;
            uint8_t *send_buf = tzwMalloc(2048);
            if (NULL == send_buf) {
                tzwFree(write_buf_tmp);
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(send_buf, 0 ,2048);
            uint8_t *send_buf_tmp = send_buf;
            write32(send_buf, key_store_file.len);
            memcpy(send_buf+4, key_store_file.buf, key_store_file.len);
            memcpy(res.buf, send_buf, key_store_file.len+4);
            res.len = key_store_file.len+4;
            ret = IFAA_WriteFile((const char*)key_store_file.buf, key_store_file.len, write_buf_tmp, write_len);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_WriteFile failed: 0x%08x", ret);
            }

            if(write_buf != NULL) {
                tzwFree(write_buf_tmp);
                write_buf = NULL;
            }

            if(send_buf != NULL) {
                tzwFree(send_buf_tmp);
                send_buf = NULL;
            }
            break;
        }

        case IFAA_INTERNAL_ECC_SIGN:
        {
            LOG_D("Enter IFAA_INTERNAL_ECC_SIGN");
            uint8_t *key_data = tzwMalloc(2048);
            if (NULL == key_data) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }
            uint8_t *key_data_tmp = key_data;
            uint32_t key_data_len = 2048;
            memset(key_data, 0, 2048);
            LOG_D("Enter IFAA_INTERNAL_SIGN");

            EXTRACT_DATA(key_file_path);
            EXTRACT_DATA(digest);
            IFAA_ReadFile((const char *)key_file_path.buf , key_file_path.len , key_data, &key_data_len);
            uint32_t magic = read32(key_data);//try to read magic number

            if (magic == ECC_MAGIC) {//ecc
                uint8_t p[__LEN_ECC256] = { 0 };
                uint8_t x[__LEN_ECC256] = { 0 };
                uint8_t y[__LEN_ECC256] = { 0 };

                IFAA_EccKey key;
                key.p.buf = p;
                key.x.buf = x;
                key.y.buf = y;

                key_data += 4;//skip maggic number
                key.p.len = read32(key_data);
                key_data += 4;
                memset(key.p.buf, 0 , key.p.len);
                memcpy(key.p.buf, key_data, key.p.len);
                key_data += key.p.len;

                key.x.len = read32(key_data);
                key_data += 4;
                memset(key.x.buf, 0 , key.x.len);
                memcpy(key.x.buf, key_data, key.x.len);
                key_data += key.x.len;

                key.y.len = read32(key_data);
                key_data += 4;
                memset(key.y.buf, 0 , key.y.len);
                memcpy(key.y.buf, key_data, key.y.len);
                key_data += key.y.len;

                ret = IFAA_EccSignDigest(&key, digest.buf, digest.len, res.buf, &(res.len));
                if (IFAA_ERR_SUCCESS != ret) {
                    LOG_E("IFAA_EccSignDigest failed: 0x%08x", ret);
                    res.len = 0;
                }

                if(key_data_tmp != NULL) {
                    tzwFree(key_data_tmp);
                    key_data_tmp = NULL;
                }
            } else {
                LOG_E("IFAA_EccSignDigest failed, no magic number");
                ret = IFAA_ERR_UNKNOWN;
            }

            break;
        }

        case IFAA_INTERNAL_ECC_VERIFY:
        {
            LOG_D("Enter IFAA_INTERNAL_ECC_VERIFY");
            uint8_t *key_data = tzwMalloc(2048);
            if (NULL == key_data) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            uint8_t *key_data_tmp = key_data;
            uint32_t key_data_len = 2048;
            memset(key_data, 0, 2048);
            LOG_D("Enter IFAA_INTERNAL_VERIFY");

            EXTRACT_DATA(key_file_path);
            EXTRACT_DATA(digest);
            EXTRACT_DATA(sig);
            IFAA_ReadFile((const char *)key_file_path.buf , key_file_path.len , key_data, &key_data_len);
            uint32_t magic = read32(key_data);//try to read magic number

            if (magic == ECC_MAGIC) {//ecc
                uint8_t p[__LEN_ECC256] = { 0 };
                uint8_t x[__LEN_ECC256] = { 0 };
                uint8_t y[__LEN_ECC256] = { 0 };

                IFAA_EccKey key;
                key.p.buf = p;
                key.x.buf = x;
                key.y.buf = y;

                key_data += 4;//skip maggic number
                key.p.len = read32(key_data);
                key_data += 4;
                memset(key.p.buf, 0 , key.p.len);
                memcpy(key.p.buf, key_data, key.p.len);
                key_data += key.p.len;

                key.x.len = read32(key_data);
                key_data += 4;
                memset(key.x.buf, 0 , key.x.len);
                memcpy(key.x.buf, key_data, key.x.len);
                key_data += key.x.len;

                key.y.len = read32(key_data);
                key_data += 4;
                memset(key.y.buf, 0 , key.y.len);
                memcpy(key.y.buf, key_data, key.y.len);
                key_data += key.y.len;

                ret = IFAA_EccVerifyDigest(&key, digest.buf, digest.len, sig.buf, sig.len);
                if (IFAA_ERR_SUCCESS != ret) {
                    LOG_E("IFAA_EccVerifyDigest failed: 0x%08x", ret);
                }

                res.len = 0;
                if(key_data_tmp != NULL) {
                    tzwFree(key_data_tmp);
                    key_data_tmp = NULL;
                }
            } else {
                LOG_E("IFAA_EccVerifyDigest failed, no magic number");
                ret = IFAA_ERR_UNKNOWN;
            }

            break;
        }

        case IFAA_INTERNAL_HMAC_SHA1:
        {
            vlb_t msg_tmp;
            msg_tmp.buf = NULL;
            EXTRACT_DATA(msg);
            EXTRACT_DATA(key_tmp);

            msg_tmp.len = msg.len;
            msg_tmp.buf = (uint8_t*)tzwMalloc(msg.len);
            if (NULL == msg_tmp.buf) {
                LOG_E("malloc failed.");
                return IFAA_ERR_OUT_OF_MEM;
            }

            memset(msg_tmp.buf, 0, msg_tmp.len);
            memcpy(msg_tmp.buf, msg.buf, msg_tmp.len);

            ret = IFAA_HmacSha1(msg_tmp.buf, msg_tmp.len, key_tmp.buf, key_tmp.len, res.buf);
            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_HmacSha1 failed: 0x%08x", ret);
                res.len = 0;
            }

#define TEE_SHA1_HASH_SIZE 20
#ifdef __DEV_DEBUG__
            logByteArrayHex(res.buf, TEE_SHA1_HASH_SIZE, "HMAC SHA1");
#endif

            if (IFAA_ERR_SUCCESS == ret) {
                res.len = TEE_SHA1_HASH_SIZE;
            }

            if (msg_tmp.buf != NULL) {
                tzwFree(msg_tmp.buf);
                msg_tmp.buf = NULL;
            }

            break;
        }

        case IFAA_INTERNAL_GET_LAST_IDENTIFIED_RESULT: {
            LOG_D("cmd: (test - pass) get last identified FID");
//            uint32_t bio_type = read32(in_ptr);
//            IFAA_LastIdentifiedResultGetter func_id_getter = NULL;
//            if (IFAA_TaGetEntry(bio_type, IFAA_ENTRY_LAST_IDENTIFIED_RESULT_GETTER,
//                                (void **) &func_id_getter) != IFAA_ERR_SUCCESS
//                || !func_id_getter) {
//                LOG_E("func none inited.");
//                ret = IFAA_ERR_UN_INITIALIZED;
//                res.len = 0;
//            }
//
//            ret = func_id_getter(res.buf, &(res.len));
//            if (ret != IFAA_ERR_SUCCESS) {
//                LOG_E("fail to get id.");
//                ret = IFAA_ERR_GET_LAST_IDENTIFIED_RESULT;
//                res.len = 0;
//            }
//            uint8_t real_fid[4];
//            uint32_t real_fid_len;
            ret=IFAA_GetFpLastIdentifiedResult(res.buf, &res.len);

            /*uint8_t realFid[128]; */
            /*uint32_t realFidSize = 128;*/
            /*uint32_t fpIndex; */
            /*uint64_t challenge;*/

            /*ret = getLatestAuthResult(realFid, &realFidSize,*/
                            /*&fpIndex, &challenge);*/
//            uint8_t real_fid[4]={0x00,0x01,0x00,0x00};
//            uint32_t real_fid_len=4;
//
//            ret=IFAA_ERR_SUCCESS;
//            res.buf[0]=0x00;
//            res.buf[1]=0x01;
//            res.buf[2]=0x00;
//            res.buf[3]=0x00;
//            res.len=4;
            break;
        }

        case IFAA_INTERNAL_GET_AUTHENTICATOR_VERSION: {
            LOG_D("cmd: (test - pass) get authenticator version");
            uint32_t bio_type = read32(in_ptr);
            uint32_t authenticator_version = 0;
            uint8_t *buf_tmp = res.buf;
            IFAA_AuthenticatorVersionGetter func_version_getter = NULL;

            LOG_D("bio_type: 0x%08x", bio_type);

            if (IFAA_TaGetEntry(bio_type, IFAA_ENTRY_AUTHENTICATOR_VERSION_GETTER,
                                (void **) &func_version_getter) != IFAA_ERR_SUCCESS
                || !func_version_getter) {
                LOG_E("func none inited.");
                ret = IFAA_ERR_UN_INITIALIZED;
                res.len = 0;
				break;
            }

            ret = func_version_getter(&authenticator_version);
            if (ret != IFAA_ERR_SUCCESS) {
                LOG_E("fail to get authenticator version.");
                ret = IFAA_ERR_GET_AUTHENTICATOR_VERSION;
                res.len = 0;
            } else {
                write32(buf_tmp, sizeof(uint32_t));
                write32(buf_tmp, authenticator_version);
                res.len = 4;
            }
        }
		break;

        case IFAA_INTERNAL_GET_ID_LIST: {
            LOG_D("cmd :IFAA_INTERNAL_GET_ID_LIST ");
            ret = IFAA_GetFpEnrolledIdList(res.buf, &res.len);
            if (ret != IFAA_ERR_SUCCESS || res.len % 8 != 0) {
                LOG_E("IFAA_INTERNAL_CMD_GET_ID_LIST  get idlist failed., ret = 0x%08x, len = %d", ret, res.len);
                ret = IFAA_ERR_GET_ID_LIST; 
                res.len = 0; 
            }
            break;
        }

        case IFAA_INTERNAL_BIO_ID_COMPARE: {
            LOG_D("cmd: (test - pass) BIO id compare");
            IFAA_IdEquator func_equator = NULL;
            uint32_t bio_type = read32(in_ptr);
            in_ptr += 4;
            EXTRACT_DATA(pBuf_l);
            EXTRACT_DATA(pBuf_r);
            if (pBuf_l.len != pBuf_r.len) {
                ret = IFAA_ERR_BAD_PARAM;
                LOG_E("IFAA_INTERNAL_BIO_ID_COMPARE failed: len is not equal");
            }

            ret = IFAA_TaGetEntry(bio_type, IFAA_ENTRY_EQUATOR, (void **) &func_equator);
            if (ret != IFAA_ERR_SUCCESS ||
                !func_equator) {
                LOG_E("func none inited.");
                ret = IFAA_ERR_UN_INITIALIZED;
            }

            res.len = 0;
            break;
        }

        case IFAA_INTERNAL_AUTHENTICATOR_SIGN: {
            LOG_D("cmd: (test) authenticator sign");
            EXTRACT_DATA(vlb_digest);
            ret = IFAA_AuthenticatorSignDigest(vlb_digest.buf, vlb_digest.len, res.buf, &(res.len));

            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_INTERNAL_AUTHENTICATOR_SIGN failed: 0x%08x", ret);
                res.len = 0;
            }

            break;
        }

        case IFAA_INTERNAL_AUTHENTICATOR_VERIFY: {
            LOG_D("cmd: (test) authenticator verify");
            IFAA_Certificate cert;
            uint32_t certlen;
            EXTRACT_DATA(vlb_vrydigest);
            EXTRACT_DATA(sig);
            certlen = read32(in_ptr);
            in_ptr += sizeof(uint32_t);
            in_len -= sizeof(uint32_t);

            cert.cert_enc_alg = read32(in_ptr);
            in_ptr += sizeof(uint32_t);
            in_len -= sizeof(uint32_t);

            EXTRACT_DATA(modulus);
            EXTRACT_DATA(pubk);
            EXTRACT_DATA(vlb_sig);

#ifdef __DEV_DEBUG__
            LOG_D("certlen: %x", certlen);
            LOG_D("cert_enc_alg: %x", cert.cert_enc_alg);
            logByteArrayHex(modulus.buf, modulus.len, "modulus");
            logByteArrayHex(pubk.buf, pubk.len, "pub key");
            logByteArrayHex(vlb_sig.buf, vlb_sig.len, "cert signature");
#endif

            cert.body.ifaa_cert.pub_key.n = modulus;
            cert.body.ifaa_cert.pub_key.e = pubk;
            cert.body.ifaa_cert.sig = vlb_sig;

            ret = IFAA_AuthenticatorVerifyDigest(vlb_vrydigest.buf, vlb_vrydigest.len,
                                                 sig.buf, sig.len, &cert, certlen);

            if (IFAA_ERR_SUCCESS != ret) {
                LOG_E("IFAA_INTERNAL_AUTHENTICATOR_VERIFY failed: 0x%08x", ret);
            }

            res.len = 0;
        }
        break;
#endif //end of __IFAA_TEST_MODE

#ifndef IFBIO_RELEASE
        case IFAA_TA_CMD_CLEAR_SFS_FILE_LIST: {
            res.len = 0;
            res.buf = NULL;
        }
        break;
        case IFAA_TA_CMD_DELETE_SKPM_INJECT_STATE: {
            ret = ifaa_tz_get_skpm_inject_state();
            res.len = 0;
            res.buf = NULL;
        }
        break;
#endif  //end of ifdef IFBIO_RELEASE

        default: {
            ret = IFAA_ERR_UNKNOWN_CMD;
            LOG_E("Invalid command ID: %d", cmd_id);
            break;
        }
    }
    if (res.len + 8 > *out_len) {
        LOG_E("out_len is too short: %d, actual response len: %d", *out_len, res.len + 8);
        ret = IFAA_ERR_BUF_TOO_SHORT;
        res.len = *out_len - 8;
    }
    *out_len = res.len + 8;
    write32(out, ret);

    /*   FOR IFAA TA ERROR LOG
     * +-------------+---------------+-------------+----------------------+--------------------
     * |     ret     | taErrorLogLen |    cmd_id   |  IFAA_VERSION_NUMBER |   Device_id  |  ---
     * +---4bytes----+-----4bytes----+---4bytes----+--------12bytes-------+----48bytes---+----
     */
    if (IFAA_ERR_SUCCESS != ret) {
        uint32_t  taErrorLogLen = res.len + 64 ;
        write32(out + 4, taErrorLogLen);
        LOG_D("IFAA_PRINT_ERROR_LOG  ret is: 0x%08x",ret);

        write32(out + 8,cmd_id);

        for(int i = 0; i < IFAA_VERSION_NUMBER_MEM_SIZE; i++){
           *(out + 12 + i) = IFAA_VERSION_NUMBER [i];
        }
        //get DeviceID
        ifaa_tz_get_device_id(&res);
        uint8_t *tempResBuf = res.buf ;
        for(int j = 0;j < 8; j = j + 2){
            *(out + 24 + j) = tempResBuf [j + 1];
            *(out + 24 + j + 1) = tempResBuf [j];
        }
        memcpy(out + 24 + 8, res.buf + 8, 40);
        /*for(int i = 0; i < 48; i++){
           LOG_D("IFAA_PRINT_ERROR_LOG *(out +24 + %d ) is %08x",i,*(out + 24 + i));
        }*/
        *out_len += taErrorLogLen;
    }else{
         write32(out + 4, res.len);
    }

    if ((res.buf != NULL) && (IFAA_ERR_SUCCESS == ret)) {
        memcpy(out + 8, res.buf, res.len);
        LOG_D("res.len :%d", res.len);
        tzwFree(res.buf);
        res.buf = NULL;
    }

    LOG_D("Output buffer length: %d", *out_len);

exit:
    return ret;
}