#include <string.h>

#include "qsee_rsa.h"

#include "ta_logger.h"
#include "tzWrappers/TzwMemory.h"
#include "tzWrappers/TzwString.h"
#include "ifaa_fingerprint_id_table.h"
#include "ifaa_ta_biz.h"
/*#include "ifaa_log_utils.h"*/
#include "ifaa_tlv_parser.h"
#include "ifaa_ta_common.h"
#include "ifaa_mem_utils.h"
#include "ifaa_km.h"

#define SKPM_KEY_HANDLE_LENGTH         10
const char skpm_key_handle_sfs[10] = {'s', 'k', 'p', 'm', 'h', 'a', 'n', 'd', 'l', 'e'};


IFAA_Result ifaa_tz_get_device_id(vlb_t *res) {
    uint32_t len_deviceid = __LEN_DEVICE_ID;
    if (!res) return IFAA_ERR_BAD_PARAM;

    res->buf = (uint8_t *) tzwMalloc(len_deviceid);
    if (NULL == res->buf) {
        LOG_E("malloc failed.");
        return IFAA_ERR_OUT_OF_MEM;
    }

    res->len = len_deviceid;
    memset(res->buf, 1, len_deviceid);

    return IFAA_GetDeviceId(res->buf, &res->len);
}

IFAA_Result ifaa_tz_get_version(vlb_t *res) {
    uint32_t len_taversion = __LEN_TA_VERSION;

    if (!res) return IFAA_ERR_BAD_PARAM;

    res->buf = (uint8_t *) tzwMalloc(len_taversion);
    if (NULL == res->buf) {
        LOG_E("malloc failed.");
        return IFAA_ERR_OUT_OF_MEM;
    }
    res->len = len_taversion;

    return IFAA_GetProtocolVersion(res->buf, &res->len);
}

IFAA_Result ifaa_tz_get_optcertalg(vlb_t *res) {
    if (!res) return IFAA_ERR_BAD_PARAM;

    res->buf = (uint8_t *) tzwMalloc(4);

    if (NULL == res->buf) {
        LOG_E("malloc failed.");
        return IFAA_ERR_OUT_OF_MEM;
    }
    res->len = 4;

    return IFAA_GetOptionalCertEncodeAlg(res->buf, &res->len);
}

IFAA_Result ifaa_tz_clear_sfs_files() {
    int ret;
    ret = IFAA_ClearFiles();
    return ret;
}

static IFAA_Result ifaa_in_validate_req_sig(tlv_node_t *root_node,
                                            tag_t tag_plain, tag_t tag_sig) {
    CHECK_BOOL_RET_VAL(root_node, IFAA_ERR_BAD_PARAM);
    tlv_node_t *node_regdata = find_node_by_tag(root_node, tag_plain);
    CHECK_BOOL_RET_VAL(node_regdata, IFAA_ERR_BAD_PARAM);
    tlv_node_t *node_sig = find_node_by_tag(root_node, tag_sig);
    CHECK_BOOL_RET_VAL(node_sig, IFAA_ERR_BAD_PARAM);

    uint8_t buf_digest[32];
    uint32_t len_digest = sizeof(buf_digest);
    CHECK_FUNC_RET(IFAA_Sha256(node_regdata->value, node_regdata->length, buf_digest, &len_digest));

    tlv_node_t *node_certchain = find_node_by_tag(root_node, TAG_CERT_CHAIN);
    CHECK_BOOL_RET_VAL(node_certchain, IFAA_ERR_BAD_PARAM);

    uint8_t *ptr = node_certchain->value;
    uint32_t cert_count = read32(ptr);
    ptr += 4;

#ifdef __DEV_DEBUG__
    logByteArrayHex(node_sig->value, node_sig->length,  "signature");
    logByteArrayHex(buf_digest, len_digest,  "digest msg");
    LOG_D("cert_count: %d", cert_count);
#endif

    cert_count = cert_count > __MAX_CERT_LEVEL?__MAX_CERT_LEVEL:cert_count;

    IFAA_Certificate certs[__MAX_CERT_LEVEL];

    for (uint32_t i = 0; i < cert_count; ++i) {
        certs[i].cert_enc_alg = (IFAA_CertEncodeAlgorithm) read32(ptr);
        ptr += 4;

        certs[i].body.ifaa_cert.pub_key.n.len = read32(ptr);
        ptr += 4;
        certs[i].body.ifaa_cert.pub_key.n.buf = ptr;
        ptr += certs[i].body.ifaa_cert.pub_key.n.len;

        certs[i].body.ifaa_cert.pub_key.e.len = read32(ptr);
        ptr += 4;
        certs[i].body.ifaa_cert.pub_key.e.buf = ptr;
        ptr += certs[i].body.ifaa_cert.pub_key.e.len;

        certs[i].body.ifaa_cert.sig.len = read32(ptr);
        ptr += 4;
        certs[i].body.ifaa_cert.sig.buf = ptr;
        ptr += certs[i].body.ifaa_cert.sig.len;

#ifdef __DEV_DEBUG__
        logByteArrayHex(certs[i].body.ifaa_cert.pub_key.n.buf, certs[i].body.ifaa_cert.pub_key.n.len, "pub_key n");
        logByteArrayHex(certs[i].body.ifaa_cert.pub_key.e.buf, certs[i].body.ifaa_cert.pub_key.e.len, "pub_key e");
        logByteArrayHex(certs[i].body.ifaa_cert.sig.buf, certs[i].body.ifaa_cert.sig.len, "cert sig");
#endif
    }

    CHECK_FUNC_RET(IFAA_AuthenticatorVerifyDigest(buf_digest, len_digest,
                                                  node_sig->value, node_sig->length,
                                                  certs, cert_count));
    return IFAA_ERR_SUCCESS;
}

static IFAA_Result ifaa_in_validate_regrequest(tlv_node_t *root_node) {
    return ifaa_in_validate_req_sig(root_node, TAG_REG_DATA, TAG_SIGNATURE);
}

static IFAA_Result ifaa_in_sign_regresponse(const uint8_t *source, uint32_t src_len,
                                            uint8_t *sig, uint32_t *sig_len)
{
    uint8_t digest[__LEN_SHA256_DIGEST] = {0};
    uint32_t len_digest = sizeof(digest);

    IFAA_Result ret = IFAA_Sha256(source, src_len, digest, &len_digest);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("sha256 error.");
        return IFAA_ERR_HASH;
    }

    return IFAA_AuthenticatorSignDigest(digest, len_digest, sig, sig_len);
}

static IFAA_Result ifaa_in_save_regdata(const char *token, uint32_t token_len,
                                          IFAA_EccKey* key, int32_t bio_type)
{
    if (!token || !key)
        return IFAA_ERR_BAD_PARAM;

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

    LOG_D("inn save regdata: ecc key");
    /*total_len = magic + bio_type + ECC_key buffer*/
    uint32_t total_len = sizeof(uint32_t) + sizeof(bio_type) + 3 * (__LEN_ECC256 + sizeof(uint32_t));
    uint8_t *buf = (uint8_t*)tzwMalloc(total_len);
    if (buf == NULL) {
        LOG_E("malloc failed");
        return IFAA_ERR_OUT_OF_MEM;
    }

    memset(buf, 0, total_len);
    uint8_t* ptr = buf;

    write32(ptr, ECC_MAGIC);
    ptr += sizeof(uint32_t);

    write32(ptr, bio_type);
    ptr += sizeof(bio_type);

    write32(ptr, key->p.len);
    memcpy(ptr + 4, key->p.buf, key->p.len);
    ptr += (4 + key->p.len);

    write32(ptr, key->x.len);
    memcpy(ptr + 4, key->x.buf, key->x.len);
    ptr += (4 + key->x.len);

    write32(ptr, key->y.len);
    memcpy(ptr + 4, key->y.buf, key->y.len);
    ptr += (4 + key->y.len);

    ret = IFAA_WriteFile(token, token_len, buf, ptr - buf);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("save ecc failed");
    }

    if (buf) tzwFree(buf);

    return ret;
}

static IFAA_Result ifaa_in_load_regdata(const char* path, uint32_t path_len, IFAA_AsymKeyAndType* key, IFAA_SignAlgorithm *type)
{
    if (!path || !key || !type) {
        return IFAA_ERR_BAD_PARAM;
    }

    uint8_t* ptr = NULL;
    int32_t bio_type = 0;
    uint8_t* buf_tmp = NULL;
    uint32_t id_len = 0;
    uint8_t buf[__MAX_READ_LEN];
    uint32_t total_len = __MAX_READ_LEN;

    IFAA_Result ret = IFAA_ReadFile(path, path_len, buf, &total_len);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("keyid file not exists");
        return IFAA_ERR_READ;
    }
    uint32_t magic = read32(buf);//try to read magic number
    if (magic != ECC_MAGIC) {//rsa
        *type = SIGN_ALG_RSA_SHA256_RAW;
        /*total_len = bio_type + id_len + last_id + RSA_key buffer*/
        do {
            ptr = buf;
            bio_type = read32(ptr);
            ptr += sizeof(bio_type);

            id_len = read32(ptr);
            ptr += sizeof(uint32_t);
            ptr += id_len;

            memset(key, 0, sizeof(IFAA_AsymKeyAndType));
            key->type = ASYM_KEY_RSA2048;
            key->asym_key.rsa.n.len = read32(ptr);
            buf_tmp = (uint8_t*)tzwMalloc(key->asym_key.rsa.n.len);
            if (buf_tmp == NULL) {
                LOG_E("malloc error");
                ret = IFAA_ERR_OUT_OF_MEM;
                break;
            }
            key->asym_key.rsa.n.buf = buf_tmp;
            memcpy(key->asym_key.rsa.n.buf, ptr + 4, key->asym_key.rsa.n.len);
            ptr += (4 + key->asym_key.rsa.n.len);

            key->asym_key.rsa.d.len = read32(ptr);
            buf_tmp = (uint8_t*)tzwMalloc(key->asym_key.rsa.d.len);
            if (buf_tmp == NULL) {
                LOG_E("malloc error");
                ret = IFAA_ERR_OUT_OF_MEM;
                break;
            }
            key->asym_key.rsa.d.buf = buf_tmp;
            memcpy(key->asym_key.rsa.d.buf, ptr + 4, key->asym_key.rsa.d.len);
            ptr += (4 + key->asym_key.rsa.d.len);

            key->asym_key.rsa.e.len = read32(ptr);
            buf_tmp = (uint8_t*)tzwMalloc(key->asym_key.rsa.e.len);
            if (buf_tmp == NULL) {
                LOG_E("malloc error");
                ret = IFAA_ERR_OUT_OF_MEM;
                break;
            }
            key->asym_key.rsa.e.buf = buf_tmp;
            memcpy(key->asym_key.rsa.e.buf, ptr + 4, key->asym_key.rsa.e.len);
        } while (false);

        if (ret == IFAA_ERR_OUT_OF_MEM) {
            if (key->asym_key.rsa.n.buf) tzwFree(key->asym_key.rsa.n.buf);
            if (key->asym_key.rsa.d.buf) tzwFree(key->asym_key.rsa.d.buf);
        }
    } else {//ecc
        *type = SIGN_ALG_ECDSA_SHA256_RAW;
        /*total_len = magic + bio_type + ECC_key buffer*/
        do {
            ptr = buf;
            ptr += sizeof(magic);//skip magic
            bio_type = read32(ptr);
            ptr += sizeof(bio_type);

            memset(key, 0, sizeof(IFAA_AsymKeyAndType));
            key->type = ASYM_KEY_ECDSA256;
            key->asym_key.ecc.p.len = read32(ptr);
            buf_tmp = (uint8_t*)tzwMalloc(key->asym_key.ecc.p.len);
            if (buf_tmp == NULL) {
                LOG_E("malloc error");
                ret = IFAA_ERR_OUT_OF_MEM;
                break;
            }
            key->asym_key.ecc.p.buf = buf_tmp;
            memcpy(key->asym_key.ecc.p.buf, ptr + 4, key->asym_key.ecc.p.len);
            ptr += (4 + key->asym_key.ecc.p.len);

            key->asym_key.ecc.x.len = read32(ptr);
            buf_tmp = (uint8_t*)tzwMalloc(key->asym_key.ecc.x.len);
            if (buf_tmp == NULL) {
                LOG_E("malloc error");
                ret = IFAA_ERR_OUT_OF_MEM;
                break;
            }
            key->asym_key.ecc.x.buf = buf_tmp;
            memcpy(key->asym_key.ecc.x.buf, ptr + 4, key->asym_key.ecc.x.len);
            ptr += (4 + key->asym_key.ecc.x.len);

            key->asym_key.ecc.y.len = read32(ptr);
            buf_tmp = (uint8_t*)tzwMalloc(key->asym_key.ecc.y.len);
            if (buf_tmp == NULL) {
                LOG_E("malloc error");
                ret = IFAA_ERR_OUT_OF_MEM;
                break;
            }
            key->asym_key.ecc.y.buf = buf_tmp;
            memcpy(key->asym_key.ecc.y.buf, ptr + 4, key->asym_key.ecc.y.len);
        } while (false);

        if (ret == IFAA_ERR_OUT_OF_MEM) {
            if (key->asym_key.ecc.p.buf) tzwFree(key->asym_key.ecc.p.buf);
            if (key->asym_key.ecc.x.buf) tzwFree(key->asym_key.ecc.x.buf);
        }
    }

    memset(buf, 0x00, sizeof(buf));

    return ret;
}

IFAA_Result ifaa_tz_register(vlb_t *req, vlb_t *res) {
    if (!req || !req->buf || req->len <= 0 || !res) return IFAA_ERR_BAD_PARAM;

    tlv_node_t *node_regrequest = NULL;
    IFAA_Result ret = parse_request(req, REG_REQUEST, &node_regrequest, ifaa_in_validate_regrequest);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("parse failed");
        return IFAA_ERR_BAD_PARAM;
    }

    vlb_t vlb_deviceid;
    IFAA_INITIALISE_VLB(vlb_deviceid);

    do {
        tlv_node_t *reg_type = find_node_by_tag(node_regrequest, TAG_REG_TYPE);
        if (NULL == reg_type) {
            LOG_E("reg_type unfound.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        IFAA_BioType bio_type = (IFAA_BioType)read32(reg_type->value);
        uint8_t ifaa_fid[4] = {0};
        uint32_t ifaa_fid_len;
        bool bGetLastId = true;
        bool bGetEnrollList = true;
        bool hasExtInfo = false;
        uint8_t buf_extinfo[__LEN_EXTINFO] = {0};
        IFAA_Result lastIdRet = IFAA_ERR_SUCCESS;
        if(0) {
            IFAA_LastIdentifiedResultGetter func_id_getter;
            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;
                break;
            }
            lastIdRet = func_id_getter(ifaa_fid, &ifaa_fid_len); 
            /* IFAA_IsRoot func_isroot = NULL;
            bool hasExtInfo = false;
            if (IFAA_TaGetEntry(bio_type, IFAA_ENTRY_IS_ROOT, (void**)&func_isroot) == IFAA_ERR_SUCCESS
                && func_isroot) {
                hasExtInfo = true;
                if (func_isroot() == IFAA_YES) {
                LOG_DBG("ifaa_tz_register rooted");
                write32(buf_extinfo, 1);
                }
            } */
        } else {

            lastIdRet = IFAA_GetFpLastIdentifiedResult(ifaa_fid, &ifaa_fid_len);
       }

        if (lastIdRet != IFAA_ERR_SUCCESS || read32(ifaa_fid) == 0) {
            LOG_E("fail to get ifaa fid.");
            bGetLastId = false; // get last id fail
        }

        vlb_t vlb_extinfo = { buf_extinfo, sizeof(buf_extinfo)};

        uint8_t buf_idlist[8 * __MAX_COUNT_ID_LIST];
        uint32_t len_idlist = 0;
        IFAA_Result idlistRet = IFAA_ERR_SUCCESS;
        if(0) {
            IFAA_IdListGetter func_idlist_getter = NULL;
            if (IFAA_TaGetEntry((IFAA_BioType)bio_type, IFAA_ENTRY_ID_LIST_GETTER, (void**)&func_idlist_getter) != IFAA_ERR_SUCCESS
                || !func_idlist_getter) {
                LOG_E("func none inited.");
                bGetEnrollList = false;
                if(!bGetLastId) {
                    ret = IFAA_ERR_UN_INITIALIZED;
                    break;
                }
            }

            idlistRet = func_idlist_getter(buf_idlist, &len_idlist);
            if (idlistRet != IFAA_ERR_SUCCESS || len_idlist % 8 != 0) {
                LOG_E("get idlist failed in register. ret = 0x%08x, len = %d", ret, len_idlist);
                bGetEnrollList = false;
                if(!bGetLastId) {
                    ret = IFAA_ERR_GET_ID_LIST;
                    break;
                }
            }
        } else {
            //tbd
            idlistRet = IFAA_GetFpEnrolledIdList(buf_idlist, &len_idlist);
            if (idlistRet != IFAA_ERR_SUCCESS || len_idlist % 8 != 0) {
                LOG_E("get idlist failed.");
                bGetEnrollList = false;
                if(!bGetLastId) {
                    ret = IFAA_ERR_GET_ID_LIST;
                    break;
                }
            }
        }
        vlb_t vlb_idlist = {buf_idlist, len_idlist};
        LOG_D("ifaa_tz_register len_idlist = %d", len_idlist);
        /*
        int i = 0;
        for(i=0; i<len_idlist; i++) {
            LOG_D("ifaa_tz_register buf_idlist[%d]=%x",i, buf_idlist[i]);
        }
        */

        tlv_node_t *node_token = find_node_by_tag(node_regrequest, TAG_TOKEN);
        if (NULL == node_token) {
            LOG_E("token notfound.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        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;

        ret = ifaa_in_save_regdata((char*)node_token->value, node_token->length, &key, bio_type);
        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("save regdata failed.");
            ret = IFAA_ERR_WRITE;
            break;
        }

        vlb_t vlb_token = {node_token->value, node_token->length};

        uint8_t buf_pub_alg_encode[__LEN_PUB_ALG_ENCODE];
        write32(buf_pub_alg_encode, KEY_ENCODE_ALG_ECC_NISTP256R1_X962_RAW);
        vlb_t vlb_pubalgencode = { buf_pub_alg_encode, sizeof(buf_pub_alg_encode) };
 
        uint8_t buf_pubkey[2 * (__LEN_ECC256 + sizeof(uint32_t))];
        uint8_t *ptr = buf_pubkey;
        write32(ptr, (uint32_t)key.x.len);
        ptr += 4;
        memcpy(ptr, key.x.buf, key.x.len);
        ptr += key.x.len;
        write32(ptr, (uint32_t)key.y.len);
        ptr += 4;
        memcpy(ptr, key.y.buf, key.y.len);
        ptr += key.y.len;
        vlb_t vlb_pubkey = { buf_pubkey, (uint32_t)(ptr - buf_pubkey) };

        memset(&key, 0x00, sizeof(IFAA_EccKey));
        uint8_t buf_keytype[__LEN_KEY_TYPE];
        write32(buf_keytype, ASYM_KEY_ECDSA256);
        vlb_t vlb_keytype = { buf_keytype, sizeof(buf_keytype) };

        tlv_node_t *node_challenge = find_node_by_tag(node_regrequest, TAG_CHALLENGE);
        if (NULL == node_challenge) {
            LOG_E("no challenge.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        vlb_t vlb_challenge = { node_challenge->value, node_challenge->length };
        if (ifaa_tz_get_device_id(&vlb_deviceid) != IFAA_ERR_SUCCESS) {
            LOG_E("get deviceid failed.");
            ret = IFAA_ERR_GET_DEVICEID;
            break;
        }

        tlv_node_t *node_regtype = find_node_by_tag(node_regrequest, TAG_REG_TYPE);
        if (NULL == node_regtype) {
            LOG_E("no regtype.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }
        vlb_t vlb_regtype = { node_regtype->value, node_regtype->length };
        vlb_t vlb_reginfo = { ifaa_fid, ifaa_fid_len };
        uint8_t buf_signalgorithm[__LEN_SIGN_ALGORITHM];
        write32(buf_signalgorithm, SIGN_ALG_RSA_SHA256_RAW);
        vlb_t vlb_sign_algorithm = { buf_signalgorithm, sizeof(buf_signalgorithm) };

        tlv_node_t *node_level = find_node_by_tag(node_regrequest, TAG_LEVEL);
        if (NULL == node_level) {
            LOG_E("no level.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        uint32_t level = read32(node_level->value);
        LOG_I("level: %d", level);

        /* if((SECURITY_LEVEL_RPMB_PER_DEVICE & level) == 0) { */
        /*     LOG_E("level not support."); */
        /*     ret = IFAA_ERR_NO_OPTIONAL_LEVEL; */
        /*     break; */
        /* } */

        uint8_t buf_securitylevel[__LEN_SECURITY_LEVEL];

        write32(buf_securitylevel, SECURITY_LEVEL_RPMB_PER_MODULE);
        vlb_t vlb_security_level = {buf_securitylevel, sizeof(buf_securitylevel)};
        LOG_D("ifaa_tz_register ready to generate_req! bGetLastId is %d, bGetEnrollList is %d",bGetLastId,bGetEnrollList);
        ret = generate_reg_response(&vlb_token,
                                    &vlb_pubalgencode, &vlb_pubkey,
                                    &vlb_keytype, &vlb_challenge,
                                    &vlb_deviceid, &vlb_regtype,
                                    bGetLastId ? &vlb_reginfo : NULL, bGetEnrollList ? &vlb_idlist : NULL, &vlb_security_level, hasExtInfo ? &vlb_extinfo : NULL,
                                    &vlb_sign_algorithm, ifaa_in_sign_regresponse, res);

        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("gen response failed.");
            ret = IFAA_ERR_GEN_RESPONSE;
            break;
        }
    } while (false);

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

    if (node_regrequest != NULL)
        free_tlv_tree(node_regrequest);

    return ret;
}


static IFAA_Result ifaa_in_validate_authrequest(tlv_node_t *root_node) {
    return ifaa_in_validate_req_sig(root_node, TAG_AUTH_DATA, TAG_SIGNATURE);
}

static IFAA_Result ifaa_in_sign_authresponse(const char *token, uint32_t token_len,
                                             const uint8_t *source, uint32_t src_len,
                                             uint8_t *sig, uint32_t *sig_len,
                                             IFAA_AsymKeyAndType *key)
{
    uint8_t digest[__LEN_SHA256_DIGEST] = {0};
    uint32_t len_digest = sizeof(digest);
    IFAA_Result ret = 0;

    ret = IFAA_Sha256(source, src_len, digest, &len_digest);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("sha256 error.");
        return IFAA_ERR_HASH;
    }
    if (key->type == ASYM_KEY_RSA2048) {
        ret = IFAA_RsaSignDigest(&(key->asym_key.rsa), digest, len_digest, sig, sig_len);
        LOG_D("rsa sign result = 0x%x", ret);
        if (key->asym_key.rsa.n.buf) tzwFree(key->asym_key.rsa.n.buf);
        if (key->asym_key.rsa.d.buf) tzwFree(key->asym_key.rsa.d.buf);
        if (key->asym_key.rsa.e.buf) tzwFree(key->asym_key.rsa.e.buf);
    } else if (key->type == ASYM_KEY_ECDSA256) {
        ret = IFAA_EccSignDigest(&key->asym_key.ecc, digest, len_digest, sig, sig_len);
        LOG_D("Ecc sign result = 0x%x", ret);
        if (key->asym_key.ecc.p.buf) tzwFree(key->asym_key.ecc.p.buf);
        if (key->asym_key.ecc.x.buf) tzwFree(key->asym_key.ecc.x.buf);
        if (key->asym_key.ecc.y.buf) tzwFree(key->asym_key.ecc.y.buf);
    }
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("sign error.");
        return IFAA_ERR_SIGN;
    }

    return IFAA_ERR_SUCCESS;
}

IFAA_Result ifaa_tz_authenticate(vlb_t *req, vlb_t *res) {
    if (!req || !req->buf || req->len <= 0 || !res) return IFAA_ERR_BAD_PARAM;

    tlv_node_t *node_authrequest = NULL;
    IFAA_Result ret = parse_request(req, AUTH_REQUEST, &node_authrequest,
                                    ifaa_in_validate_authrequest);
    if (ret != IFAA_ERR_SUCCESS || !node_authrequest) {
        LOG_E("parse failed");
        return IFAA_ERR_BAD_PARAM;
    }

    vlb_t vlb_deviceid;
    IFAA_INITIALISE_VLB(vlb_deviceid);

    do {
        tlv_node_t *node_authtype = find_node_by_tag(node_authrequest, TAG_AUTH_TYPE);
        if (!node_authtype) {
            LOG_E("no auth type");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        IFAA_BioType bio_type = (IFAA_BioType)read32(node_authtype->value);
        uint8_t ifaa_fid[4] = {0};
        uint32_t ifaa_fid_len;
        bool hasExtInfo = false;
        bool bGetLastId = true;
        bool bGetEnrollList = true;
        uint8_t buf_extinfo[__LEN_EXTINFO] = {0};
        if(0) {
            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;
                break;
            }

            ret = func_id_getter(ifaa_fid, &ifaa_fid_len);
            /*
            uint8_t buf_extinfo[__LEN_EXTINFO] = {0};
            IFAA_IsRoot func_isroot = NULL;
            bool hasExtInfo = false;
            if (IFAA_TaGetEntry(bio_type, IFAA_ENTRY_IS_ROOT, (void**)&func_isroot) == IFAA_ERR_SUCCESS
                && func_isroot) {
                hasExtInfo = true;
                if (func_isroot() == IFAA_YES) {
                    LOG_DBG("ifaa_tz_auth rooted");
                    write32(buf_extinfo, 1);
                }
            }
            */
        }else {
            ret = IFAA_GetFpLastIdentifiedResult(ifaa_fid, &ifaa_fid_len);
        }

        if (ret != IFAA_ERR_SUCCESS || read32(ifaa_fid) == 0) {
            LOG_E("fail to get ifaa fid, ret = 0x%08x", ret);
            bGetLastId = false; // get last id fail
            ret = IFAA_ERR_GET_LAST_IDENTIFIED_RESULT;
            break;
        }

        vlb_t vlb_authinfo = { ifaa_fid, ifaa_fid_len };
        vlb_t vlb_extinfo = { buf_extinfo, sizeof(buf_extinfo)};

        uint8_t buf_idlist[8 * __MAX_COUNT_ID_LIST];
        uint32_t len_idlist = 0;

        IFAA_Result idListRet = IFAA_ERR_SUCCESS;
        if(0) {
            IFAA_IdListGetter func_idlist_getter = NULL;
            if (IFAA_TaGetEntry((IFAA_BioType)bio_type, IFAA_ENTRY_ID_LIST_GETTER, (void**)&func_idlist_getter) != IFAA_ERR_SUCCESS
                || !func_idlist_getter) {
                LOG_E("func none inited.");
                idListRet = IFAA_ERR_UN_INITIALIZED;
                bGetEnrollList = false;
            }
            idListRet = func_idlist_getter(buf_idlist, &len_idlist);
        } else {
            idListRet = IFAA_GetFpEnrolledIdList(buf_idlist, &len_idlist);
        }

        if (idListRet != IFAA_ERR_SUCCESS || len_idlist % 8 != 0) {
            LOG_E("get idlist failed in authenticate. idListRet = 0x%08x, len = %d", idListRet, len_idlist);
            bGetEnrollList = false;
        }

        vlb_t vlb_idlist = {buf_idlist, len_idlist};
        LOG_D("ifaa_tz_auth len_idlist = %d, bGetEnrollList = %d", len_idlist, bGetEnrollList);
        //for(int i=0; i<len_idlist; i++) {
        //    LOG_D("ifaa_tz_auth buf_idlist[%d]=%x",i, buf_idlist[i]);
        //}

        tlv_node_t *node_token = find_node_by_tag(node_authrequest, TAG_TOKEN);
        if (NULL == node_token) {
            LOG_E("token notfound.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        vlb_t vlb_token = { node_token->value, node_token->length };
        tlv_node_t *node_challenge = find_node_by_tag(node_authrequest, TAG_CHALLENGE);
        if (NULL == node_challenge) {
            LOG_E("no challenge.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        vlb_t vlb_challenge = {node_challenge->value, node_challenge->length};

        if (ifaa_tz_get_device_id(&vlb_deviceid) != IFAA_ERR_SUCCESS) {
            LOG_E("get deviceid failed.");
            ret = IFAA_ERR_GET_DEVICEID;
            break;
        }

        vlb_t vlb_authtype = { node_authtype->value, node_authtype->length };

        IFAA_AsymKeyAndType key;
        IFAA_SignAlgorithm type;
        ret = ifaa_in_load_regdata((char *)vlb_token.buf, vlb_token.len, &key, &type);
        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("load error.");
            ret = IFAA_ERR_READ;
            break;
        }
        LOG_D("SIGN ALGORITHM = %x", type);
        uint8_t buf_signalgorithm[__LEN_SIGN_ALGORITHM];
        write32(buf_signalgorithm, type);
        vlb_t vlb_sign_algorithm = { buf_signalgorithm, sizeof(buf_signalgorithm) };

        tlv_node_t *node_level = find_node_by_tag(node_authrequest, TAG_LEVEL);
        if (NULL == node_level) {
            LOG_E("no level.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        /* uint32_t level = read32(node_level->value); */
        /* if((SECURITY_LEVEL_RPMB_PER_DEVICE & level) == 0) { */
        /*     LOG_E("level not support.");		 */
        /*     ret = IFAA_ERR_NO_OPTIONAL_LEVEL; */
        /*     break;	 */
        /* } */

        uint8_t buf_securitylevel[__LEN_SECURITY_LEVEL];
        write32(buf_securitylevel, SECURITY_LEVEL_RPMB_PER_MODULE);
        vlb_t vlb_security_level = {buf_securitylevel, sizeof(buf_securitylevel)};

        ret = generate_auth_response(&vlb_token,
                                     &vlb_challenge, &vlb_deviceid,
                                     &vlb_authtype,
                                     bGetLastId ? &vlb_authinfo : NULL, bGetEnrollList ? &vlb_idlist : NULL, &vlb_security_level, hasExtInfo ? &vlb_extinfo : NULL,
                                     &vlb_sign_algorithm, ifaa_in_sign_authresponse, &key, res);

        memset(&key, 0x00, sizeof(IFAA_AsymKeyAndType));
        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("gen response failed.");
            ret = IFAA_ERR_GEN_RESPONSE;
            break;
        }
    } while (false);

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

    if (node_authrequest != NULL) {
        free_tlv_tree(node_authrequest);
    }

    LOG_D("authenticate success ret = 0x%08x", ret);
    return ret;
}

static IFAA_Result ifaa_in_validate_deregrequest(tlv_node_t *root_node) {
    return ifaa_in_validate_req_sig(root_node, TAG_DEREG_DATA, TAG_SIGNATURE);
}

IFAA_Result ifaa_tz_deregister(vlb_t *req) {
    if (!req || !req->buf || req->len <= 0) return IFAA_ERR_BAD_PARAM;

    tlv_node_t *node_deregrequest = NULL;
    IFAA_Result ret = parse_request(req, DEREG_REQUEST, &node_deregrequest,
                                    ifaa_in_validate_deregrequest);
    if (ret != IFAA_ERR_SUCCESS || !node_deregrequest) {
        LOG_E("parse failed");
        return IFAA_ERR_BAD_PARAM;
    }

    do {
        tlv_node_t *node_token = find_node_by_tag(node_deregrequest, TAG_TOKEN);
        if (NULL == node_token) {
            LOG_E("no token.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        tlv_node_t *node_level = find_node_by_tag(node_deregrequest, TAG_LEVEL);
        if (NULL == node_level) {
            LOG_E("no level.");
            ret = IFAA_ERR_BAD_PARAM;
            break;
        }

        /* vlb_t vlb_level = { node_level->value, node_level->length }; */
        /* uint32_t level = read32(node_level->value); */
        /* if((SECURITY_LEVEL_RPMB_PER_DEVICE & level) == 0) { */
        /*     LOG_E("level not support.");		 */
        /*     ret = IFAA_ERR_NO_OPTIONAL_LEVEL; */
        /*     break;	 */
        /* } */

        ret = IFAA_DeleteFile((char *) node_token->value, node_token->length);
        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("delete file failed.");
            ret = IFAA_ERR_ERASE;
            break;
        }
    } while (false);

    if (node_deregrequest != NULL) {
        free_tlv_tree(node_deregrequest);
    }

    return ret;
}

IFAA_Result ifaa_tz_query_status(vlb_t *token, vlb_t *res) {
    if (!token || !token->buf) return IFAA_ERR_BAD_PARAM;

    res->buf = (uint8_t *) tzwMalloc(4);
    if (res->buf == NULL) {
        LOG_E("tzwMalloc failed");
        return IFAA_ERR_OUT_OF_MEM;
    }

    res->len = 4;
    IFAA_Result ret = IFAA_ERR_UNKNOWN;
    uint8_t *buf_index = NULL;

    do {
        uint8_t buf_tmp[sizeof(int32_t) + sizeof(uint32_t)];
        uint32_t buf_tmp_len = sizeof(buf_tmp);  //add chl
        ret = IFAA_ReadFile((char *) token->buf, token->len, buf_tmp, &buf_tmp_len);
        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("key file not exists");
            ret = IFAA_ERR_READ;
            break;
        }

        uint8_t *ptr = buf_tmp;
        uint32_t tmp_index_len = read32(ptr + sizeof(int32_t));

        uint32_t total_len = sizeof(int32_t) + sizeof(uint32_t) + tmp_index_len;
        buf_index = (uint8_t *) tzwMalloc(total_len);

        if(buf_index == NULL) {
            LOG_E("query command malloc buf_index failed.");
            ret = IFAA_ERR_MALLOC_FAILED;
            break;
        }

        uint32_t len_index = total_len;
        ret = IFAA_ReadFile((char *) token->buf, token->len, buf_index, &len_index);
        if (ret != IFAA_ERR_SUCCESS) {
            LOG_E("key file not exists");
            ret = IFAA_ERR_READ;
            break;
        }
    } while (false);

    int32_t status = (IFAA_ERR_SUCCESS == ret) ? 1 : 0;

    write32(res->buf, status);

    if (buf_index) tzwFree(buf_index);

    return ret;
}


/* +--------------------------------------------------------------------------------+ */
/* | VER    | len_challenge | challenge | len_did | did | sign_type | len_sig | sig | */
/* | 4bytes | 4bytes        |           | 4bytes  |     | 4bytes    | 4bytes  |     | */
/* +--------------------------------------------------------------------------------+ */

IFAA_Result ifaa_tz_get_sec_device_id(vlb_t *req, vlb_t *res) {
    vlb_t vlb_did;
    IFAA_Result ret = ifaa_tz_get_device_id(&vlb_did);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("fail to get deviceid");
        return ret;
    }

    res->len = 4 + 4 + req->len + 4 + vlb_did.len + 4 + 4 + 256;
    res->buf = tzwMalloc(res->len);
    if (res->buf == NULL) {
        LOG_E("res->buf is NULL");
        return IFAA_ERR_MALLOC_FAILED;
    }
    unsigned char *p_res = res->buf;
    write32(p_res, 1);
    p_res += 4;

    write32(p_res, req->len);
    p_res += 4;

    memcpy(p_res, req->buf, req->len);
    p_res += req->len;

    write32(p_res, vlb_did.len);
    p_res += 4;

    memcpy(p_res, vlb_did.buf, vlb_did.len);
    p_res += vlb_did.len;

    if (vlb_did.buf) {
        tzwFree(vlb_did.buf);
    }

    write32(p_res, SHA256_RSA2048_PER_MODLE);
    p_res += 4;

    uint8_t digest[32];
    uint32_t len_digest = sizeof(digest);
    ret = IFAA_Sha256(res->buf + 4, 4 + req->len + 4 + vlb_did.len, digest, &len_digest);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("sha256 error.");
        return IFAA_ERR_HASH;
    }

    uint8_t sig[256];
    uint32_t sig_len = sizeof(sig);
    ret = IFAA_AuthenticatorSignDigest(digest, len_digest, sig, &sig_len);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("authenticatorsigndigest error.");
        return IFAA_ERR_SIGN;
    }

    write32(p_res, sig_len);
    p_res += 4;
    memcpy(p_res, sig, sig_len);
    p_res += sig_len;

    res->len = p_res - res->buf;

    return IFAA_ERR_SUCCESS;
}

IFAA_Result ifaa_tz_get_skpm_inject_state() {
    uint8_t buf_id_len[SKPM_KEY_HANDLE_LENGTH];
    uint32_t buf_id_len_length = sizeof(buf_id_len);
    IFAA_Result ret = IFAA_ReadFile((const char*)skpm_key_handle_sfs, SKPM_KEY_HANDLE_LENGTH, NULL, 0);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("Check skpm inject state failed ,error code : %x", ret);
    }
    return ret;
}

IFAA_Result
ifaa_tz_skpm_provision_operation(uint32_t skpm_key_handle_length, uint8_t *skpm_key_handle) {
    //provision skpm key handle
    uint32_t skpm_key_handle_real_length = skpm_key_handle_length - sizeof(uint32_t);
    LOG_D("skpm_key_handle_real_length:%d", skpm_key_handle_real_length);
    uint8_t *real_skpm_key_handle = skpm_key_handle + sizeof(uint32_t);
    ifaa_km_result_t result = ifaa_km_provision_key(real_skpm_key_handle,
                                                    skpm_key_handle_real_length);
    if (result != IFAA_KM_SUCCESS) {
        LOG_E("rovision SKPM key handle failed : %d", result);
        return IFAA_ERR_PROVISION_FAILED;
    } else {
        LOG_D("Provision SKPM key handle success");
    }
    //To save the skpm key handle data,skpm_key_handle is  length(4byte)+real data.
    LOG_D("Write skpm handle to sfs");
    IFAA_Result ret = IFAA_WriteFile((const char *)skpm_key_handle_sfs, SKPM_KEY_HANDLE_LENGTH,
                                     (const uint8_t *)skpm_key_handle_sfs,
                                     SKPM_KEY_HANDLE_LENGTH);
    if (ret != IFAA_ERR_SUCCESS) {
        LOG_E("ifaa_tz_skpm_provision_operation, Save skpm blob data failed ,error code : %x",
                  ret);
        return ret;
    }
    return ret;
}

#define IFAA_MAGIC 0x41464649
#define _REAL_FID_LEN 32

IFAA_Result ifaa_tz_update_fid(vlb_t *idlist) {

    IFAA_Result ret = IFAA_ERR_BAD_PARAM;
    //parse info from the head part
    uint8_t *pBuf = idlist->buf;
    uint32_t remainLen = idlist->len;

    if(remainLen < 16) {
        LOG_E("update fid len not enough = %u", remainLen);
        return ret;
    }

    uint32_t magic = read32(pBuf);
    pBuf += 4;

    if(magic != IFAA_MAGIC) {
        LOG_E("update fid magic = %u", magic);
        return ret;
    }

    uint32_t cnt = read32(pBuf);
    pBuf += 4;

    uint32_t itemSize = read32(pBuf);
    pBuf += 4;

    uint32_t offset = read32(pBuf);

    LOG_I("UPDATE FID cnt = %u, size = %u, offset = %u", cnt, itemSize, offset);

    remainLen -=16;

    if(remainLen < cnt*itemSize) {
        LOG_E("remainLen is not enough, remainLen = %u", remainLen);
        return ret;
    }
   
    uint32_t realCnt = 0;

    if(cnt > 0) {
        pBuf = idlist->buf + offset;
        SystemFidStruct enrolledFps[FID_TABLE_MAX] = {0};

        for(uint32_t i = 0; i<cnt; i++) {
            SystemFidStruct *curFp = &enrolledFps[i];
            curFp->realFpIndex = read32(pBuf);
            pBuf+=8;

/*          fingerPrint improved, following code no need
            if((pBuf[0]|pBuf[1]) == 0x00) {
                pBuf+=_REAL_FID_LEN;
                continue;
            }
*/
            tzwMemMove(curFp->realFid, pBuf, _REAL_FID_LEN);
            pBuf+=_REAL_FID_LEN;
            LOG_I("fid[%u] = %s", curFp->realFpIndex, curFp->realFid);
            realCnt++;
        }

        ret = save_system_fid_table(enrolledFps, realCnt);
    }
    return ret;
}