#include <comdef.h>
#include <qsee_fs.h>
#include <stdio.h>
#include <ctype.h>
#include <stringl.h>
#include <string.h>
#include <limits.h>
#include <qsee_services.h>
#include <qsee_rsa.h>
#include <secrsa_err.h>
#include <secrsa_utils.h>
#include <qsee_sfs.h>
#include <qsee_log.h>
#include <qsee_hash.h>
#include <qsee_heap.h>
#include <qsee_cfg_prop.h>
#include <qsee_core.h>
#include <qsee_oem_buffer.h>
#include "dashboard.h"
#include "qc_tima_attestation.h"
#include "TZ_Vendor_tl.h"

#include "tz_iccc_comdef.h" // ICCC_FEATURE

// Private key
//QSEE_BigInt a_key_p, a_key_q, a_key_dP, a_key_dQ, a_key_qP;
QSEE_BigInt a_key_N, a_key_d, a_key_e;

// Knox certs
uint8* knox_certs[10];
unsigned int knox_certs_len[10];
uint8 knox_certs_num = 0;

//the buffer to read key file from efs
uint8 key_file_buffer[8192];
uint32 key_file_buffer_len;

void get_alt_rot_distname(char *i_appname, char *o_distname)
{
    const char *prop_name = "alt_rot_domain_name_dot";
    uint32_t ret = 0;
    uint32_t ret_size = 0;
    size_t len = 0;
    qsee_cfg_propvar_t *ptr = NULL;
    uint32_t prop[2 + (MAX_DISTNAME_PREFIX_SZ / sizeof(uint32_t))] = {0};
    char distname_prefix[MAX_DISTNAME_PREFIX_SZ] = {0};

    ret = qsee_cfg_getpropval(prop_name,
                              strlen(prop_name) + 1, 0,
                              (qsee_cfg_propvar_t *)&prop,
                              sizeof(prop), &ret_size);
    if (QSEE_CFG_SUCCESS != ret) {
        qsee_log(QSEE_LOG_MSG_ERROR, "'alt_rot_domain_name_dot' read failed %d, using legacy appname", ret);
        ret_size = strlcpy(o_distname, i_appname, MAX_TANAME_SZ);
        return;
    }

    ptr = (qsee_cfg_propvar_t *)prop;
    /* len = ret_size - sizeof(qsee_cfg_propvar_t) + padding */
    len = ret_size - sizeof(*ptr) + 2 * sizeof(ptr->val) + 1;
    if (len > sizeof(distname_prefix)) {
        qsee_log(QSEE_LOG_MSG_ERROR, "'alt_rot_domain_name_dot' len invalid, using legacy appname");
        strlcpy(o_distname, i_appname, MAX_TANAME_SZ);
        return;
    }

    /* remove the quotes only when read from devcfg */
    memcpy(distname_prefix, &ptr->val[1], len - 1);
    distname_prefix[len-1] = '\0';
    /* finalize fully qualified distname */
    strlcpy(o_distname, distname_prefix, MAX_DISTNAME_PREFIX_SZ);
    strlcat(o_distname, i_appname, MAX_FULLNAME_SZ);
}

static uint32_t switch_endianness(
    uint32_t val
)
{
    uint32_t result;
    result = ((val & 0xff) << 24) |
        (((val >> 8) & 0xff) << 16) |
        (((val >> 16) & 0xff) << 8) | ((val >> 24) & 0xff);
    return result;
}

/* Check if memory allocation for QSEE_BigInt is successfuly or not */
int mem_check(QSEE_S_BIGINT * p)
{
    if (!p) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: No memory to allocate for QSEE_BigInt");
        return E_SECMATH_FAILURE;
    } else {
        (void)memset(p, 0, sizeof(QSEE_S_BIGINT));
        return E_SECMATH_SUCCESS;
    }
}


/*
 * Parse private key and retrieve one integer
 * In:
 *  key: the key buffer
 *  index: point to the current index to be parsed.
 * Outt:
 *  index: the new index to the next integer
 *  output: the output buffer
 *  len: the length of output buffer
 *
 */
tima_error_code parse_key_int(
    uint8 *key, unsigned int *index,
    uint8 **output, unsigned int *len)
{

    int i, len_size;

    i = *index;
    if (key[i++] != 0x02) {
        // 0x02 means integer in ASN1
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Parse private key: Not start with 0x02: i=%d", i);
        return TIMA_ERROR_KEY_ERROR;
    }

    // Get the length of integer
    if (key[i] <= 127) {
        *len = key[i++];
    } else {
        len_size = key[i++] - 128;
        *len = 0;
        qsee_log(QSEE_LOG_MSG_ERROR, "len_size=%d", len_size);
        while(len_size > 0) {
            qsee_log(QSEE_LOG_MSG_ERROR, "i=%d, key[i]=%d", i, key[i]);
            *len = (*len << 8) + key[i++];
            len_size--;
        }
    }

    // Skip the leading 0 if any. Leading zero means positive
    if (key[i] == 0x0) {
        i++;
        (*len)--;
    }
    qsee_log(QSEE_LOG_MSG_ERROR, "parse int len=%d, i=%d", *len, i);
    *output = key + i;
    *index = i + *len;
    return TIMA_SUCCESS;
}

/*
 * Retrieve the private key modulus and exponent
 * NOTE: here we are not writing a complete DER parser. Since we know
 * the public key length, we simply find the private key (n, e, d) part and return.
 *
 * RSAPrivateKey ::= SEQUENCE {
 *  version           Version,
 *  modulus           INTEGER,  -- n
 *  publicExponent    INTEGER,  -- e
 *  privateExponent   INTEGER,  -- d
 *  prime1            INTEGER,  -- p
 *  prime2            INTEGER,  -- q
 *  exponent1         INTEGER,  -- d mod (p1)
 *  exponent2         INTEGER,  -- d mod (q-1)
 *  coefficient       INTEGER,  -- (inverse of q) mod p
 *  otherPrimeInfos   OtherPrimeInfos OPTIONAL
}
 */
tima_error_code parse_private_key(
    uint8 *private_key, unsigned int index,
    uint8 **modulus, unsigned int *modulus_len,
    uint8 **priv_expo, unsigned int *priv_expo_len,
    uint8 **pub_expo, unsigned int *pub_expo_len)
{
    unsigned int i=0;

    if (private_key[i] != 0x30 || private_key[i+1] != 0x82 ||
        private_key[i+4] != 0x02 || private_key[i+5] != 0x01) {
        qsee_log(QSEE_LOG_MSG_ERROR, 
                "ATN: Private key must start with {0x30,0x82,xx,xx,0x02,0x01}, actual: {0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x}",
                private_key[i], private_key[i+1], private_key[i+2], private_key[i+3], private_key[i+4], private_key[i+5]);
        return TIMA_ERROR_KEY_ERROR;
    }

    i += 7; // point to modulus
    if (parse_key_int(private_key, &i, modulus, modulus_len)) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Modulus error");
        return TIMA_ERROR_KEY_ERROR;
    }

    // i now point to publicExponent, most likely 65535
    if (parse_key_int(private_key, &i, pub_expo, pub_expo_len)) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Public key exponent error, i=%d, len=%d", i, *pub_expo_len);
        return TIMA_ERROR_KEY_ERROR;
    }

    // i now point to private exponent
    if (parse_key_int(private_key, &i, priv_expo, priv_expo_len)) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Private key exponent error, i=%d, len=%d", i, *priv_expo_len);
        return TIMA_ERROR_KEY_ERROR;
    }

    return TIMA_SUCCESS;
}

/*
 * Parse the key file, get private key and public key certificate
 * key file format:
 *
 * item_type (1 byte) | length (2 bytes big endian) | data (length bytes)

 * item_type:
 *  0x01: RSA cert
 *  0x03: RSA private key
 */
tima_error_code parse_attestation_key_file(
    uint8 *key_file, unsigned int key_file_len,
    uint8 **modulus, unsigned int *modulus_len,
    uint8 **priv_expo, unsigned int *priv_expo_len,
    uint8 **pub_expo, unsigned int *pub_expo_len)
{
    unsigned int i = 0;
    unsigned int len;
    tima_error_code ret;
    uint8_t tl_name[32] = {0};

    knox_certs_num = 0;

    while (i < (key_file_len-6)) { //key_file should remain at least 6 elements
        if (key_file[i] == KNOX_KEY_TYPE_RSA_CERT) {
            // public key certs
            len = key_file[++i];
            len += key_file[++i] << 8;
            i++;

            if(knox_certs_num < (sizeof(knox_certs)/sizeof(knox_certs[0]))) {
                knox_certs[knox_certs_num] = key_file + i;
                knox_certs_len[knox_certs_num++] = len;
            } else {
                qsee_log(QSEE_LOG_MSG_ERROR, "Cert num exceed buffer size");
                return TIMA_ERROR_KEY_ERROR;
            }

            qsee_log(QSEE_LOG_MSG_ERROR, "Cert len = %d", len);
            i += len;
        } else if (key_file[i] == KNOX_KEY_TYPE_RSA_PRIVATE) {
            // private key
            len = key_file[++i];
            len += key_file[++i] << 8;
            i++;
            ret = parse_private_key(key_file + i, i,
                    modulus, modulus_len,
                    priv_expo, priv_expo_len,
                    pub_expo, pub_expo_len);
            if (ret) {
                return TIMA_ERROR_KEY_ERROR;
            } else {
                qsee_log(QSEE_LOG_MSG_ERROR, "Private key len = %d", len);
            }
            i += len;
        } else if (key_file[i] == KNOX_KEY_TYPE_TL_NAME) {
            len = key_file[++i];
            len += key_file[++i] << 8;
            i++;
            memcpy(tl_name, key_file + i, len);
            qsee_log(QSEE_LOG_MSG_ERROR, "TL NAME : %s, length : %d", (char*)tl_name, len);
            i += len;
        } else {
            len = key_file[i+1];
            len += key_file[i+2] << 8;
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Unknown type in key file: %d, i=%d, len=%d", key_file[i], i, len);
            i += 3 + len;
            //TODO: figure out what is type 4
            //return TIMA_ERROR_KEY_ERROR;
        }
    }
    return TIMA_SUCCESS;
}

void free_attestation_key(QSEE_RSA_KEY * attestation_key)
{
    if (attestation_key->p)
        qsee_free(attestation_key->p);
    if (attestation_key->q)
        qsee_free(attestation_key->q);
    if (attestation_key->dP)
        qsee_free(attestation_key->dP);
    if (attestation_key->dQ)
        qsee_free(attestation_key->dQ);
    if (attestation_key->qP)
        qsee_free(attestation_key->qP);
    if (attestation_key->N)
        qsee_free(attestation_key->N);
    if (attestation_key->d)
        qsee_free(attestation_key->d);
    if (attestation_key->e)
        qsee_free(attestation_key->e);
}

/* Read Attestation key (KNOX key) using SFS API, parse the file and
 * populate the RSA key data
 */
tima_error_code load_attestation_key(QSEE_RSA_KEY * attestation_key)
{
    //int fd; // file where public/private keys are saved.
    tima_error_code ret = TIMA_SUCCESS;
    //int len;

    SECMATH_ERRNO_ET secmath_ret = E_SECMATH_SUCCESS;

    uint8 *modulus;
    unsigned int modulus_len;
    uint8 *priv_expo;
    unsigned int priv_expo_len;
    uint8 *pub_expo;
    unsigned int pub_expo_len;

    /*********************************************
    // The following code is for JF (8064)
    // read key from efs
    //qsee_sfs_init(4096);
    fd = qsee_sfs_open(KNOX_KEY_FILE, O_RDONLY );
    if (fd == 0) {
        qsee_log(QSEE_LOG_MSG_ERROR,"Attestation: failed to open key file");
        return TIMA_ERROR_KEY_ERROR;
    } else {
        len = qsee_sfs_read(fd, (char *)key_file_buffer, 4096);
        qsee_log(QSEE_LOG_MSG_ERROR, "sfs read ret = %d", len);
        qsee_sfs_close(fd);
        if (len <= 0) {
            return TIMA_ERROR_KEY_ERROR;
        }
    }
    */

    // For HF (8974), the key buffer should already be populated
    // parse the key file and extract data
    ret = parse_attestation_key_file(
            key_file_buffer, key_file_buffer_len,
            &modulus, &modulus_len,
            &priv_expo, &priv_expo_len,
            &pub_expo, &pub_expo_len);

    if (ret) {
        return ret;
    }


    // Create the private key
    attestation_key->type = QSEE_RSA_KEY_PRIVATE;
    attestation_key->bitLength = 2048;

    //attestation_key.p = (QSEE_S_BIGINT*) qsee_malloc(sizeof(QSEE_S_BIGINT)) ;
    attestation_key->p = NULL;

    //attestation_key.q = (QSEE_S_BIGINT*) qsee_malloc(sizeof(QSEE_S_BIGINT)) ;
    attestation_key->q = NULL;

    //attestation_key.qP = (QSEE_S_BIGINT*) qsee_malloc(sizeof(QSEE_S_BIGINT)) ;
    attestation_key->qP = NULL;

    //attestation_key.dP = (QSEE_S_BIGINT*) qsee_malloc(sizeof(QSEE_S_BIGINT)) ;
    attestation_key->dP = NULL;

    // The d mod (q - 1) CRT param
    //attestation_key.dQ = (QSEE_S_BIGINT*) qsee_malloc(sizeof(QSEE_S_BIGINT)) ;
    attestation_key->dQ = NULL;

    attestation_key->N = (QSEE_S_BIGINT *) qsee_malloc(sizeof(QSEE_S_BIGINT));
    attestation_key->d = (QSEE_S_BIGINT *) qsee_malloc(sizeof(QSEE_S_BIGINT));
    attestation_key->e = (QSEE_S_BIGINT *) qsee_malloc(sizeof(QSEE_S_BIGINT));
/*
    if (  mem_check(attestation_key.p ) != E_SECMATH_SUCCESS ||
          mem_check(attestation_key.q ) != E_SECMATH_SUCCESS ||
          mem_check(attestation_key.dP) != E_SECMATH_SUCCESS ||
          mem_check(attestation_key.dQ) != E_SECMATH_SUCCESS ||
          mem_check(attestation_key.qP) != E_SECMATH_SUCCESS ||
*/
    if (mem_check(attestation_key->N) != E_SECMATH_SUCCESS ||
        mem_check(attestation_key->e) != E_SECMATH_SUCCESS ||
        mem_check(attestation_key->d) != E_SECMATH_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR,"Cannot alloc mem for private key\n");
        return TIMA_ERROR_KEY_ERROR;
    }

/*
    secmath_ret = secmath_BIGINT_read_unsigned_bin( (BigInt*)&a_key_p, key_p, sizeof(key_p) / sizeof(key_p[0]) );
    if (secmath_ret != E_SECMATH_SUCCESS)
    {
        qsee_log("Failed to load key_p: %d\n", secmath_ret);
        return E_SECMATH_FAILURE;
    }
    if (secmath_BIGINT_read_unsigned_bin( (BigInt*)&a_key_q, key_q, sizeof(key_q)/sizeof(key_q[0])) != E_SECMATH_SUCCESS)
    {
        qsee_log("Failed to load key_q: %d\n", secmath_ret);
        return E_SECMATH_FAILURE;
    }
    if (secmath_BIGINT_read_unsigned_bin( (BigInt*)&a_key_dP, key_dp, sizeof(key_dp)/sizeof(key_dp[0])) != E_SECMATH_SUCCESS)
    {
        qsee_log("Failed to load key_dp: %d\n", secmath_ret);
        return E_SECMATH_FAILURE;
    }
    if (secmath_BIGINT_read_unsigned_bin( (BigInt*)&a_key_dQ, key_dq, sizeof(key_dq)/sizeof(key_dq[0])) != E_SECMATH_SUCCESS)
    {
        qsee_log("Failed to load key_dq: %d\n", secmath_ret);
        return E_SECMATH_FAILURE;
    }
    if (secmath_BIGINT_read_unsigned_bin( (BigInt*)&a_key_qP, key_qinv, sizeof(key_qinv)/sizeof(key_qinv[0])) != E_SECMATH_SUCCESS)
    {
        qsee_log("Failed to load key_qp: %d\n", secmath_ret);
        return E_SECMATH_FAILURE;
    }
*/
    if (secmath_BIGINT_read_unsigned_bin
        ((BigInt*)&a_key_N, modulus,
         modulus_len) != E_SECMATH_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR,"Attestation: Failed to load modulus: %d\n",
            secmath_ret);
        return TIMA_ERROR_KEY_ERROR;
    }
    if (secmath_BIGINT_read_unsigned_bin
        ((BigInt*)&a_key_d, priv_expo,
         priv_expo_len) != E_SECMATH_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR,"Attestation: Failed to load private exponent: %d\n",
            secmath_ret);
        return TIMA_ERROR_KEY_ERROR;
    }
    if (secmath_BIGINT_read_unsigned_bin
        ((BigInt*)&a_key_e, pub_expo,
         pub_expo_len) != E_SECMATH_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR,"Attestation: Failed to load public exponent: %d\n",
            secmath_ret);
        return TIMA_ERROR_KEY_ERROR;
    }

/*
    (attestation_key.p)->bi = a_key_p;
    (attestation_key.p)->sign = S_BIGINT_POS;
    (attestation_key.q)->bi = a_key_q;
    (attestation_key.q)->sign = S_BIGINT_POS;
    (attestation_key.dP)->bi = a_key_dP;
    (attestation_key.dP)->sign = S_BIGINT_POS;
    (attestation_key.dQ)->bi = a_key_dQ;
    (attestation_key.dQ)->sign = S_BIGINT_POS;
    (attestation_key.qP)->bi = a_key_qP;
    (attestation_key.qP)->sign = S_BIGINT_POS;
*/
    (attestation_key->N)->bi = a_key_N;
    (attestation_key->N)->sign = S_BIGINT_POS;
    (attestation_key->d)->bi = a_key_d;
    (attestation_key->d)->sign = S_BIGINT_POS;
    (attestation_key->e)->bi = a_key_e;
    (attestation_key->e)->sign = S_BIGINT_POS;

    return TIMA_SUCCESS;
}

/* Load the measurement from some physical address, and compare with good
 * measurement to give a verdict
   Arguments:
    measurement: to save the measurement result
    measurement_len: the maximum size of measurement as input; the actual measurement size as output

   Changes on Jan 2014: retreive aboot version and kernel version, which follows measurement
*/
tima_error_code load_measurement_verdict(uint8 * measurement, int *measurement_len, int tamper_fuse, int reason, int version)
{
    uint8 vbmeta_measure_buffer[MAX_SVB_MEASUREMENT_LENGTH] = { 0 };
    int svb_measurement_len = MAX_SVB_MEASUREMENT_LENGTH;
    int verdict = 0; // 0: yes, 1: no
    int measurement_status = 0;    // 0 : match, 1 : mismatch, 2: unknown
    int offset; // used to remember the location of the actual measurement value in the buffer
    uint32_t tb_flag = TEXT_TRUSTBOOT_FAILURE;

    if(reason == -1) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: reason code is invalid");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    /* SVB enabled case */
    measurement[(*measurement_len)++] = ATTESTATION_RESULT_TYPE_MEASUREMENT;
    measurement[(*measurement_len)++] = 0;
    measurement[(*measurement_len)++] = 0;
    offset = *measurement_len;

#ifdef CONFIG_READ_VBHASH_WITH_ICCC
        if (ICCC_SUCCESS != Iccc_ReadData_TA(VERIFIEDBOOT_HASH, (uint32_t *) vbmeta_measure_buffer)) {
            qsee_log(QSEE_LOG_MSG_ERROR, "%s: Failed to read vbmeta msr", __func__);
            return TIMA_ERROR_MEASUREMENT_ERROR;
        }
#else
        if(qsee_read_oem_buffer(TIMA_VPCR_OFFSET+32,vbmeta_measure_buffer,MAX_SVB_MEASUREMENT_LENGTH)) {
            qsee_log(QSEE_LOG_MSG_ERROR, "%s: Failed to read vbmeta msr", __func__);
            return TIMA_ERROR_QSEE_ERROR;
        }
#endif

    measurement[(*measurement_len)++] = ATTESTATION_RESULT_TYPE_SVB_MEASUREMENT;
    measurement[(*measurement_len)++] = 0;
    measurement[(*measurement_len)++] = svb_measurement_len;
    memcpy(measurement + *measurement_len, vbmeta_measure_buffer, svb_measurement_len);
    offset = *measurement_len;
    *measurement_len += svb_measurement_len;
    if (ICCC_SUCCESS == Iccc_ReadData_TA(TRUSTBOOT_FLAG, &tb_flag)) {
        if (tb_flag == TEXT_TRUSTBOOT_SUCCESS) {
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: TB measurement match");
            measurement_status = 0;
        }
        else {
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: TB measurement mismatch");
            measurement_status = 1;
        }
    } else {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: TB measurement read failed");
        measurement_status = 2;
    }

    measurement[(*measurement_len)++] = ATTESTATION_RESULT_TYPE_VERDICT;
    measurement[(*measurement_len)++] = 0;

    if (reason != 0 || tamper_fuse == 1 || measurement_status == 1) {
        verdict = 1;
        measurement[(*measurement_len)++] = 2;
        strlcpy((char*)(measurement + *measurement_len), "No", sizeof("No"));
        *measurement_len += 2;
    } else if (tamper_fuse == 0 && measurement_status == 0) {
        measurement[(*measurement_len)++] = 3;
        strlcpy((char*)(measurement + *measurement_len), "Yes", sizeof("Yes"));
        *measurement_len += 3;
    } else {
        measurement[(*measurement_len)++] = 7;
        strlcpy((char*)(measurement + *measurement_len), "Unknown", sizeof("Unknown"));
        *measurement_len += 7;
    }

     if (verdict == 1) {
        measurement[(*measurement_len)++] = ATTESTATION_RESULT_TYPE_REASON_VERDICT_NO;
        measurement[(*measurement_len)++] = 0;

        reason = reason << 1;
        reason += measurement_status;

        reason = reason << 1;
        reason += tamper_fuse;

        measurement[(*measurement_len)++] = 1;
        measurement[(*measurement_len)++] = reason;
        qsee_log(QSEE_LOG_MSG_ERROR, "verdict is no, reason is %d", reason);
    }
    return TIMA_SUCCESS;
}

/*
 * prepare the data blob to be signed.
 */
tima_error_code load_data(
    uint8 *nonce, int nonce_len, char *IMEI, char *MAC,
    uint8 *message, int *message_len, uint8_t *cid , int cid_len, int reason,int blobversion)
{
    tima_error_code ret;
    int serial_num = 0;
    int8 tamper_fuse= -1;
    uint32_t warranty_bit = 1;
    int resp = -1;
    uint32_t iccc_ret;
    uint32_t sectimer_base,sectimer_flag,sectimer_status;
    char *tmp;

    dashboard_row_t* dashboard;
    uint16 dashboard_size = sizeof(dashboard_row_t) * DASHBOARD_MAX_ENTRIES;

    int length_offset;

    uint8_t imei_hash[QSEE_SHA256_HASH_SZ];
    uint8_t imei_buffer[32] = {0};

    uint8_t mac_hash[QSEE_SHA256_HASH_SZ];
    uint8_t mac_buffer[17] = {0};

    // 0. Add version number and leave 2 byte for message length
    qsee_log(QSEE_LOG_MSG_DEBUG,"Blobversion =%d ", blobversion);
    message[(*message_len)++] = blobversion;
    length_offset = *message_len;
    *message_len += 2;

    // 1. Add Tamper fuse
    if (*message_len + 4 > MAX_MESSAGE_LEN) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save Tamper fuse");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_TAMPER_FUSE;
    message[(*message_len)++] = 0;
    message[(*message_len)++] = 1;

    resp =  Iccc_ReadData_TA(WARRANTY_BIT, &warranty_bit);
    if (resp == ICCC_SUCCESS) {
        if(warranty_bit == 0)
            tamper_fuse = 0;
        else
            tamper_fuse = 1;
    }

    qsee_log(QSEE_LOG_MSG_ERROR, " ATN:tamper_fuse=%d %x", tamper_fuse,tamper_fuse);
    if (tamper_fuse == -1) {
        message[(*message_len)++] = 255;
        qsee_log(QSEE_LOG_MSG_ERROR, "Failed to get tamper fuse value!");
    } else {
        message[(*message_len)++] = tamper_fuse ;
    }

    // 2. Add Measurement
    ret = load_measurement_verdict(message, message_len, tamper_fuse, reason, blobversion);
    qsee_log(QSEE_LOG_MSG_ERROR, "load_measurement_verdict  ret=%d",ret);
    if (ret) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Failed to read trusted boot measurement!");
        return ret;
    }

    // 3. Add nonce
    // to avoid integer overflow we compare against INT_MAX before adding
    if ((*message_len < 0) || (nonce_len < 0) || (*message_len > INT_MAX - nonce_len)) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: len validation failed!");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    // to avoid integer overflow we compare against MAX_MESSAGE_LEN - 3
    if (*message_len + nonce_len > MAX_MESSAGE_LEN - 3) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save Nonce");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_NONCE;
    message[(*message_len)++] = nonce_len >> 8;
    message[(*message_len)++] = nonce_len % 256;

    memcpy(message + *message_len, nonce, nonce_len);
    *message_len += nonce_len;

    // Add CID
    if ((cid_len < 0) || (*message_len > INT_MAX - cid_len)) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: len validation failed!");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    if (*message_len + cid_len > MAX_MESSAGE_LEN) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save TIMA measurement");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    memcpy(message + *message_len, cid, cid_len);
    *message_len += cid_len;

    // Add SEAndroid status
    //message[(*message_len)++] = ATTESTATION_RESULT_TYPE_SEANDROID;
    //message[(*message_len)++] = 0;
    //message[(*message_len)++] = 1;
    //message[(*message_len)++] = 0;

    // 4. Add Dash board, which should include SEAndroid status
    if (*message_len + dashboard_size + 3 > MAX_MESSAGE_LEN) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save Dashboard");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_DASHBOARD;
    message[(*message_len)++] = dashboard_size >> 8;
    message[(*message_len)++] = dashboard_size % 256;
    dashboard = qsee_malloc(dashboard_size);
    dashboard_read_all(dashboard);
    memcpy(message + *message_len, (uint8*)dashboard, dashboard_size);
    qsee_free(dashboard);
    //dashboard_read_all((dashboard_row_t*)(message + *message_len));
    *message_len += dashboard_size;

    // 5. Add Unique Device ID
    if (*message_len + 7 > MAX_MESSAGE_LEN) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save Serial Num");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_SERIAL_NUM;
    message[(*message_len)++] = 0;
    message[(*message_len)++] = 4;
    serial_num = qsee_read_serial_num();
    message[(*message_len)++] = serial_num >> 24;
    message[(*message_len)++] = (serial_num >> 16) % 256;
    message[(*message_len)++] = (serial_num >> 8) % 256;
    message[(*message_len)++] = serial_num % 256;

    // 6. Add IMEI hash
    if (strlen(IMEI) > 0 && strlen(IMEI) < 32) {
        if (strcmp(IMEI, "INVALID IMEI") == 0) {
            if (*message_len + strlen(IMEI) + 3 > MAX_MESSAGE_LEN) {
                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save IMEI");
                return TIMA_ERROR_ARGUMENT_ERROR;
            }
            message[(*message_len)++] = ATTESTATION_RESULT_TYPE_IMEI;
            message[(*message_len)++] = 0;
            message[(*message_len)++] = strlen(IMEI);
            memcpy(message + *message_len, IMEI, strlen(IMEI));
            *message_len += strlen(IMEI);
        } else {
            memcpy(imei_buffer, IMEI, strlen(IMEI));
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: IMEI length is %d", strlen((char*)imei_buffer));
            if (qsee_hash(QSEE_HASH_SHA256, imei_buffer, strlen(IMEI), imei_hash,
                        QSEE_SHA256_HASH_SZ) != E_SECMATH_SUCCESS) {
                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Failed to compute IEMI hash!");
                return TIMA_ERROR_GENERIC_ERROR;
            }

            if (*message_len + QSEE_SHA256_HASH_SZ + 3 > MAX_MESSAGE_LEN) {
                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space to save IMEI");
                return TIMA_ERROR_ARGUMENT_ERROR;
            }

            message[(*message_len)++] = ATTESTATION_RESULT_TYPE_IMEI;
            message[(*message_len)++] = 0;
            message[(*message_len)++] = QSEE_SHA256_HASH_SZ;
            memcpy(message + *message_len, imei_hash, QSEE_SHA256_HASH_SZ);
            *message_len += QSEE_SHA256_HASH_SZ;
        }
    }

    // 8. Add MAC SHA256 hash
    if (strcmp(MAC, MAC_ERROR_STR) == 0) {
        if (*message_len + strlen(MAC) + 3 > MAX_MESSAGE_LEN) {
            qsee_log(QSEE_LOG_MSG_ERROR, "TIMA ATN: Not enough space to save MAC");
            return TIMA_ERROR_ARGUMENT_ERROR;
        }

        message[(*message_len)++] = ATTESTATION_RESULT_TYPE_MAC;
        message[(*message_len)++] = 0;
        message[(*message_len)++] = strlen(MAC_ERROR_STR);
        memcpy(message + *message_len, MAC_ERROR_STR, strlen(MAC_ERROR_STR));
        *message_len += strlen(MAC_ERROR_STR);
    } else {
        qsee_log(QSEE_LOG_MSG_ERROR, "MAC MAC");
        memcpy(mac_buffer, MAC, MAC_LENGTH);
        if (qsee_hash(QSEE_HASH_SHA256, mac_buffer, MAC_LENGTH, mac_hash,
                QSEE_SHA256_HASH_SZ) != E_SECMATH_SUCCESS) {
            qsee_log(QSEE_LOG_MSG_ERROR, "qsee_hash returns an error");
            return TIMA_ERROR_GENERIC_ERROR;
        }

        if (*message_len + QSEE_SHA256_HASH_SZ + 3 > MAX_MESSAGE_LEN) {
            qsee_log(QSEE_LOG_MSG_ERROR, "TIMA ATN: Not enough space to save MAC");
            return TIMA_ERROR_ARGUMENT_ERROR;
        }

        message[(*message_len)++] = ATTESTATION_RESULT_TYPE_MAC;
        message[(*message_len)++] = 0;
        message[(*message_len)++] = QSEE_SHA256_HASH_SZ;
        memcpy(message + *message_len, mac_hash, QSEE_SHA256_HASH_SZ);
        *message_len += QSEE_SHA256_HASH_SZ;
    }

    // 8. Add SECTIMER_BASE 
    iccc_ret = Iccc_ReadData_TA(SECTIMER_BASE, &sectimer_base);
    if (iccc_ret == ICCC_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR, "Iccc_RaveData_TA success for SECTIMER_BASE %d", sectimer_base);
    } else{
        qsee_log(QSEE_LOG_MSG_ERROR, "Iccc_RaveData_TA Failed for SECTIMER_BASE ");
        sectimer_base = -1;
    }
        
    if (*message_len + sizeof(sectimer_base) + 3 > MAX_MESSAGE_LEN) {
        qsee_log(QSEE_LOG_MSG_ERROR, "TIMA ATN: Not enough space to save SECTIMER_BASE");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }

    sectimer_base = switch_endianness(sectimer_base);
    tmp = (char *)&sectimer_base;
    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_SECTIMER_BASE;
    message[(*message_len)++] = 0;
    message[(*message_len)++] = sizeof(sectimer_base);
    memcpy(message + *message_len,tmp, sizeof(sectimer_base));
    *message_len += sizeof(sectimer_base);
        
    // ADD SECTIMER_FLAG 
    iccc_ret = Iccc_ReadData_TA(SECTIMER_FLAG, &sectimer_flag);
    if (iccc_ret == ICCC_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR, "Iccc_RaveData_TA success for SECTIMER_FLAG %d", sectimer_flag);
    } else{
        qsee_log(QSEE_LOG_MSG_ERROR, "Iccc_RaveData_TA Failed for SECTIMER_FLAG ");
        sectimer_flag = -1;
    }

    if (*message_len + sizeof(sectimer_flag) + 3 > MAX_MESSAGE_LEN) {    
        qsee_log(QSEE_LOG_MSG_ERROR, "TIMA ATN: Not enough space to save SECTIMER_FLAG");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }
    sectimer_flag = switch_endianness(sectimer_flag);
    tmp = (char *)&sectimer_flag;
    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_SECTIMER_FLAG;
    message[(*message_len)++] = 0;
    message[(*message_len)++] = sizeof(sectimer_flag);
    memcpy(message + *message_len,tmp, sizeof(sectimer_flag));
    *message_len += sizeof(sectimer_flag);

    // ADD SECTIMER_STATUS
    iccc_ret = Iccc_ReadData_TA(SECTIMER_STATUS, &sectimer_status);
    if (iccc_ret == ICCC_SUCCESS) {
        qsee_log(QSEE_LOG_MSG_ERROR, "Iccc_RaveData_TA success for SECTIMER_STATUS %d", sectimer_status);
    } else {
        qsee_log(QSEE_LOG_MSG_ERROR, "Iccc_RaveData_TA Failed for SECTIMER_STATUS ");
        sectimer_status = -1;
    }

    if (*message_len + sizeof(sectimer_status) + 3 > MAX_MESSAGE_LEN) {    
        qsee_log(QSEE_LOG_MSG_ERROR, "TIMA ATN: Not enough space to save SECTIMER_STATUS");
        return TIMA_ERROR_ARGUMENT_ERROR;
    }
    
    sectimer_status=switch_endianness(sectimer_status);
    tmp = (char *)&sectimer_status;
    message[(*message_len)++] = ATTESTATION_RESULT_TYPE_SECTIMER_STATUS;
    message[(*message_len)++] = 0;
    message[(*message_len)++] = sizeof(sectimer_status);
    memcpy(message + *message_len,tmp, sizeof(sectimer_status));
    *message_len += sizeof(sectimer_status);

    qsee_log(QSEE_LOG_MSG_ERROR, "load_data SUCCESS");

    // Add message length back to message.
    // The length does not include version number and message length itself.
    message[length_offset] = (*message_len - 3) >> 8;
    message[length_offset+1] = (*message_len - 3) % 256;

    return TIMA_SUCCESS;
}

void check_mac(char *mac) {
    int i;
    char c;
    if (strcmp(mac, "INVALID MAC") == 0) {
        return;
    }

    for(i=0; i<MAC_LENGTH; i++) {
        if ((i+1) % 3 == 0) {
            if (mac[i] != ':') {
                strlcpy(mac, "INVALID MAC", sizeof("INVALID MAC"));
            }
        } else {
            c = (mac[i] >= 'a' && mac[i] <= 'z') ? mac[i] + 'A' - 'a' : mac[i];
            if (!(c >='0' && c <= '9') && !(c >= 'A' && c <= 'F')) {
                strlcpy(mac, "INVALID MAC", sizeof("INVALID MAC"));
                return;
            } else {
                mac[i] = c; // upper case only
            }
        }
    }
}


/* Return the signature for attestation: sign the hash of message
   Arguments:
    nonce: nonce given by Knox or MDM server
    nonce_len: the length of nonce, in bytes
    message: the message result retrieved (out)
    message_len: the size of message+nonce in bytes (out)(as input, it's the maximum space available in message structure)
    signature: the signature (out)
    signature_len: the size of signature in bytes (out)(as input, it's the maximum space available in signature structure)
    attestation_key: the private key used to sign message.
*/
tima_error_code attestation_sign(
    uint8 * nonce, int nonce_len, char *IMEI, char *MAC,
    uint8 * message, int *message_len, uint8 * signature,
    int *signature_len, QSEE_RSA_KEY * attestation_key, uint8_t *cid, int cid_len, int reason, int blobversion)
{
    QSEE_RSA_PSS_PAD_INFO     pss_pad_info;
    tima_error_code ret;
    int retcode;
    uint8 a_hash[QSEE_SHA256_HASH_SZ];

    pss_pad_info.hashidx =  QSEE_HASH_IDX_SHA256;
    pss_pad_info.saltlen =  32;

    check_mac(MAC);

    ret = load_data(nonce, nonce_len, IMEI, MAC, message, message_len, cid , cid_len, reason, blobversion);
    if (ret) {
        return ret;
    }

    ret = load_attestation_key(attestation_key);
    if (ret) {
        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Failed to load private key!");
        free_attestation_key(attestation_key);
        return ret;
    }

    do {
        // Compute hash for message result
        if (qsee_hash(QSEE_HASH_SHA256, message, *message_len, a_hash, QSEE_SHA256_HASH_SZ) != E_SECMATH_SUCCESS) {
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Failed to compute hash!");
            ret = TIMA_ERROR_SIGNING_ERROR;
            break;
        }

        retcode = qsee_rsa_sign_hash(attestation_key,QSEE_RSA_PAD_PKCS1_PSS, &pss_pad_info, 
                QSEE_HASH_IDX_SHA256, (unsigned char *)a_hash,QSEE_SHA256_HASH_SZ, signature,signature_len);
        if (retcode) {
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Failed to sign: ret = %d\n", retcode);
            ret = TIMA_ERROR_SIGNING_ERROR;
            break;
        }

        qsee_log(QSEE_LOG_MSG_ERROR, "Sign len = %d", *signature_len);
        *signature_len = TIMA_SIGN_LEN;// it's weird that signature_len is set to 0
    } while (0);

    free_attestation_key(attestation_key);

    return ret;
}

void tima_attestation_cmd_handler(void* cmd, uint32 cmdlen, void* rsp, uint32 rsplen)
{
    tima_attestation_cmd_type cmd_req = TIMA_ATTESTATION_CMD_UNKNOWN;
    void * cmd_addr = NULL;
    void * rsp_addr = NULL;
    tima_error_code ret;
    int retval;
    char skm_app_name[32] = "skm";
    char full_distname[MAX_DISTNAME_PREFIX_SZ + MAX_TANAME_SZ + 1] = {0}; // skm + RoT
    char src_app[MAX_DISTNAME_PREFIX_SZ + MAX_TANAME_SZ + 1] = {0};
    uint32_t wrap_ret;
    uint32_t wrapped_keylen = 0;

    // The followings are used for attestation.
    char *result = NULL;
    int index = 0, j, i=0;
    // The message to be signed
    uint8 message[MAX_MESSAGE_LEN];
    int message_len = 0;

    uint8 attestation_sig[TIMA_SIGN_LEN];
    int attestation_sig_len = TIMA_SIGN_LEN;
    int version = 0;
    uint32_t tb_flag = TEXT_TRUSTBOOT_FAILURE;
    int32_t is_svb = 0;
    QSEE_RSA_KEY attestation_key;
    char *error;
    knox_certs_num = 0;

    qsee_log(QSEE_LOG_MSG_ERROR, "TIMA_ATTESTATION FROM TRUSTEDAPPS BRANCH");

    if(cmd && rsp) {
        cmd_addr = cmd;
        rsp_addr = rsp;

        /* First 4 bytes are always command id */
        cmd_req = (tima_attestation_cmd_type)(*(uint32*)cmd_addr);
        if (cmd_req == TIMA_ATTESTATION_CMD || cmd_req == TIMA_ATTESTATION_CMD_NEW) {
            if (cmd_req == TIMA_ATTESTATION_CMD_NEW) {
                qsee_log(QSEE_LOG_MSG_DEBUG, "TIMA_ATTESTATION_CMD_NEW case");
                version = ((tima_attestation_req_t *) cmd)->blobVersion;
            } else {
                qsee_log(QSEE_LOG_MSG_DEBUG, "TIMA_ATTESTATION_CMD case");
                version = 8;
            }
            qsee_log(QSEE_LOG_MSG_ERROR,"Start attestation key_len=%d", ((tima_attestation_req_t *) cmd)->key_len);

            key_file_buffer_len = ((tima_attestation_req_t *) cmd)->key_len;

            if(key_file_buffer_len > sizeof(key_file_buffer)) {
                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: key length exceed key buffer size");
                ret = TIMA_ERROR_KEY_ERROR;
            } else if(((tima_attestation_req_t *) cmd)->is_wrapped_key == 0) {
                get_alt_rot_distname(skm_app_name, full_distname);
                retval = qsee_decapsulate_inter_app_message(src_app,
                        ((tima_attestation_req_t *) cmd)->key_buffer,
                        ((tima_attestation_req_t *) cmd)->key_len,
                        key_file_buffer, &key_file_buffer_len);
                if (retval) {
                    qsee_log(QSEE_LOG_MSG_DEBUG, "ATN qsee_decapsulate_inter_app_message() FAILED : %x", retval);
                    ret = TIMA_ERROR_KEY_ERROR;
                } else if (strncmp(src_app, full_distname, strlen(full_distname) + 1)) {
                    qsee_log(QSEE_LOG_MSG_DEBUG, "ATN: source app name \"%s\"", src_app);
                    qsee_log(QSEE_LOG_MSG_ERROR, "ATN: full_distname name \"%s\"", full_distname);
                    ret = TIMA_ERROR_KEY_ERROR;
                } else {
                    qsee_log(QSEE_LOG_MSG_ERROR, "GOT key in plain format,now converting it into wrapped form");
                    wrapped_keylen = MAX_WRAPPED_KEY_LEN;
                    wrap_ret = TZ_wrap_data((uint8_t*)"ATN",strlen("ATN"),key_file_buffer,(uint32_t)key_file_buffer_len,((tima_attestation_rsp_t *) rsp_addr)->wrapped_key.wrapped_keybuf,&wrapped_keylen);

                    if (wrap_ret != TZ_API_OK) {
                        qsee_log(QSEE_LOG_MSG_ERROR, "TZ_wrap_data() returns an error");
                        wrapped_keylen = 0;
                    }
                    ret =
                        attestation_sign(((tima_attestation_req_t *) cmd)->nonce,
                            ATTESTATION_NONCE_SIZE,
                            ((tima_attestation_req_t *) cmd)->IMEI,
                            ((tima_attestation_req_t *) cmd)->MAC,
                            message, &message_len,
                            attestation_sig,
                            &attestation_sig_len,
                            &attestation_key,
                            ((tima_attestation_req_t *) cmd)->cid,
                            ((tima_attestation_req_t *) cmd)->cid_len,
                            ((tima_attestation_req_t *) cmd)->reason,
                            version);

                    ((tima_attestation_rsp_t *) rsp_addr)->wrapped_key.wrapped_keylen = wrapped_keylen;
                }
            } else if(((tima_attestation_req_t *) cmd)->is_wrapped_key == 1) {
                qsee_log(QSEE_LOG_MSG_ERROR, "GOT key in wrapped format from tlc,now converting it into plain form");
                key_file_buffer_len = MAX_WRAPPED_KEY_LEN;
                wrap_ret = TZ_unwrap_data((uint8_t*)"ATN",strlen("ATN"),((tima_attestation_req_t *) cmd)->key_buffer,((tima_attestation_req_t *) cmd)->key_len,key_file_buffer,(uint32_t *)&key_file_buffer_len);

                if (wrap_ret != TZ_API_OK) {
                    qsee_log(QSEE_LOG_MSG_ERROR, "TZ_unwrap_data() returns an error");
                    ret = TIMA_ERROR_KEY_ERROR;
                } else {
                    ret =
                        attestation_sign(((tima_attestation_req_t *) cmd)->nonce,
                            ATTESTATION_NONCE_SIZE,
                            ((tima_attestation_req_t *) cmd)->IMEI,
                            ((tima_attestation_req_t *) cmd)->MAC,
                            message, &message_len,
                            attestation_sig,
                            &attestation_sig_len,
                            &attestation_key,
                            ((tima_attestation_req_t *) cmd)->cid,
                            ((tima_attestation_req_t *) cmd)->cid_len,
                            ((tima_attestation_req_t *) cmd)->reason,
                            version);
                    qsee_log(QSEE_LOG_MSG_ERROR, "attestation_sign ret = " + ret);
                }
            } else {
                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: is_wrapped_key has strange values");
                ret = TIMA_ERROR_KEY_ERROR;
            }

            ((tima_attestation_rsp_t *) rsp_addr)->cmd_id = cmd_req;
            ((tima_attestation_rsp_t *) rsp_addr)->ret = ret;

            qsee_log(QSEE_LOG_MSG_ERROR,"message_len is =%d", message_len);

            result =
                ((tima_attestation_rsp_t *)
                 rsp_addr)->result.attestation.buffer;

            while (ret == 0) {
                // Prepare the result to return. If no enough memory, set ret = TIMA_ERROR_OUT_OF_MEMORY and break
                // 1. Success
                index = 0;
                result[index++] = 0;
                result[index++] = strlen("Success");
                strlcpy(result + index, "Success", sizeof("Success"));
                index += strlen("Success");

                // 2. message
                /* the following 3 lines are moved into the message, so that they will also be signed.
                result[index++] = 1; // version number is 1
                result[index++] = message_len >> 8;
                result[index++] = message_len % 256;
                */
                if (index + message_len > MAX_RESULT_BUFFER_LEN) {
                    qsee_log(QSEE_LOG_MSG_ERROR, "ATN: not enough buffer to save message blob!");
                    ret = TIMA_ERROR_OUT_OF_MEMORY;
                    break;
                }

                memcpy(result + index, message, message_len);
                index += message_len;

                // 3. signature
                if (index + attestation_sig_len > MAX_RESULT_BUFFER_LEN) {
                    qsee_log(QSEE_LOG_MSG_ERROR, "ATN: not enough buffer to save signature!");
                    ret = TIMA_ERROR_OUT_OF_MEMORY;
                    break;
                }
                memcpy(result + index, attestation_sig,
                    attestation_sig_len);
                index += attestation_sig_len;

                // 4. certs
                for(j=0; j<knox_certs_num; j++) {
                    if (index + knox_certs_len[j] + 2 > MAX_RESULT_BUFFER_LEN) {
                        qsee_log(QSEE_LOG_MSG_ERROR, "ATN: not enough buffer to save cert %d!", j);
                        ret = TIMA_ERROR_OUT_OF_MEMORY;
                        break;
                    }

                    result[index++] = knox_certs_len[j] >> 8;
                    result[index++] = knox_certs_len[j] % 256;
                    memcpy(result + index, knox_certs[j], knox_certs_len[j]);
                    index += knox_certs_len[j];
                }
                if (j < knox_certs_num) break;

                ((tima_attestation_rsp_t *) rsp_addr)->result.attestation.len = index;
                qsee_log(QSEE_LOG_MSG_ERROR,"Attestation result is successfully signed: result len=%d", index);
                break;
            }

            if (ret != 0) {
                switch (ret)
                {
                    case TIMA_ERROR_QSEE_ERROR:
                        error = "QSEE Error";
                        break;
                    case TIMA_ERROR_ARGUMENT_ERROR:
                        error = "Argument Error";
                        break;
                    case TIMA_ERROR_MEASUREMENT_ERROR:
                        error = "Measurement Error";
                        break;
                    case TIMA_ERROR_KEY_ERROR:
                        error = "Attestation Key Error";
                        break;
                    case TIMA_ERROR_SIGNING_ERROR:
                        error = "Signing Error";
                        break;
                    case TIMA_ERROR_LOAD_MEASUREMENT_ERROR:
                        error = "Cannot load trusted boot measurement";
                        break;
                    case TIMA_ERROR_OUT_OF_MEMORY:
                        error = "Not enough memory";
                        break;
                    default:
                        error = "Generic Error";
                }

                index = 0;
                result[index++] = ret;
                result[index++] = strlen(error);
                strlcpy(result + index, error, strlen(error)+1);
                index += strlen(error);
                ((tima_attestation_rsp_t *) rsp_addr)->result.attestation.len = index;

                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Attestation failed");
            }
            return;
        } else if (cmd_req == TIMA_ATTESTATION_DEVICEID_CMD) {
            // retrieve device ID
            ((tima_attestation_rsp_t *) rsp_addr)->cmd_id = cmd_req;
            ((tima_attestation_rsp_t *) rsp_addr)->ret = TIMA_SUCCESS;

            ((tima_attestation_rsp_t *) rsp_addr)->result.serial_num = qsee_read_serial_num();
            return;
        } else if (cmd_req == TIMA_ATTESTATION_INIT) {
            ((tima_attestation_rsp_t *)rsp_addr)->cmd_id = cmd_req;

            if (((tima_attestation_init_t *)cmd)->buffer_len > MAX_MEASUREMENT_BUFFER_LEN) {
                qsee_log(QSEE_LOG_MSG_ERROR, "ATN: Not enough space for good measurement");
                ret = TIMA_ERROR_ARGUMENT_ERROR;
            } else {
                ret = TIMA_SUCCESS;
            }
            ((tima_attestation_rsp_t *) rsp_addr)->ret = ret;
            return;
        } else  {
            // Unknown command
            qsee_log(QSEE_LOG_MSG_ERROR, "ATN: unknown command id:%d", cmd_req);
            ((tima_attestation_rsp_t *) rsp_addr)->cmd_id = cmd_req;
            ((tima_attestation_rsp_t *) rsp_addr)->ret = TIMA_ERROR_UNKNOWN_COMMAND;
        }
    }
}

void exit_tima_attestation()
{
    return;
}