#include "target.h"
/*#include <comdef.h>*/
#include "x509v3.h"
#include "tl_utils.h"
#include "tl_fs.h"
#include "tl_heap.h"
#include "tl_log.h"
#include "tl_inter_app_msg.h"

#define PROV_BUF_SIZE 4096

#define RSA_CERT_TAG  (uint8_t)0x01
#define IV_TAG        (uint8_t)0x02
#define KEY_TAG       (uint8_t)0x03
#define TL_NAME_TAG   (uint8_t)0x04
#define ATTRS_TAG     (uint8_t)0x05

#define ERROR_INTERNAL_ERROR 0xffff0001

#define MAX_RSA_CERT_LEN  2048

#define MAX_MODULUS_SIZE 	(512 + 1)
#define MAX_EXPONENT_SIZE 	(512 + 1)

#define PARSE_DEBUG 1


#define GET_UINT16T_LE(off, b) \
    (*(b + off + 0) + (*(b + off + 1)<<8))

extern char *TZ_APP_NAME;

//include room for header and hamc
#define ENCAPSULATED_DATA_LENGTH 4096

static uint8_t encapsulated[4096];
char src_app[32] = {0};

int init_auth_data(uint8_t *input, uint32_t in_len, uint8_t *output, uint32_t *output_len)
{
    uint8_t prov_buf[PROV_BUF_SIZE];
    uint32_t pos = 0;
    uint32_t res;
    uint32_t out_len = PROV_BUF_SIZE;
    uint32_t blob_size = 0;

    int rc = 0;;

#ifndef _NDEBUG_
    dump_bytes("input encrypted", input, in_len);
#endif

    SOFTSIM_LOGD("First line on skm data parse (%d bytes)", in_len );

    //decapsulate key_blob from skm
    res = tl_decrypt_tz_app_message(src_app, input, in_len, prov_buf, &out_len);
    if ( res ) {
        SOFTSIM_LOGE("tl_decrypt_tz_app_msg() FAILED : 0x%x", res);
        return res;
    }
    blob_size = out_len;

#if 0
    if( 0 != strcmp(src_app, "skm")){
        SOFTSIM_LOGE("ERROR: src name %s is not skm", src_app);
        return -1;
    }
#endif

    /*out_len = *p_out_len;*/
    SOFTSIM_LOGD("decapsulate %d bytes !!", out_len);

#ifndef _NDEBUG_
    dump_bytes("decapsulated blob key", prov_buf, blob_size);
#endif

    pos = 0;

    // parse DeviceRoot certificate
    if ( (pos + 3) >= PROV_BUF_SIZE )
    {
        SOFTSIM_LOGE("Error in parsing Root sertificate, parse position: %d.", pos);
        return ERROR_INTERNAL_ERROR;
    }

    if ( RSA_CERT_TAG != prov_buf[ pos ] )
    {
        SOFTSIM_LOGE("Wrong tag field of RSA Root certificate" );
        return ERROR_INTERNAL_ERROR;
    }

    uint16_t root_cert_buf_len = GET_UINT16T_LE( 1, (prov_buf + pos) );
    if ( root_cert_buf_len == 0 || root_cert_buf_len  > MAX_RSA_CERT_LEN )
    {
        SOFTSIM_LOGE("Wrong size of RSA Root certificate: 0x%08x", root_cert_buf_len);
        return ERROR_INTERNAL_ERROR;
    }
    pos += 3;
    if ( (pos + root_cert_buf_len + 3) >= PROV_BUF_SIZE )
    {
        SOFTSIM_LOGE("Error in parsing Root sertificate, parse position: 0x%08x.", pos);
        return ERROR_INTERNAL_ERROR;
    }

    uint32_t off = 4;;
    uint8_t *root_cert_buf = prov_buf + pos;
    pos += root_cert_buf_len;

    *((uint32_t*)(output + off)) = CHANGE_UINT32T_ENDIAN(root_cert_buf_len);
    off += 4;
    memcpy(output + off, root_cert_buf, root_cert_buf_len);
    off += root_cert_buf_len;

    dump_bytes("device cert ", root_cert_buf, root_cert_buf_len);
#ifndef _NDEBUG_
    tl_fs_save_device_cert(root_cert_buf, root_cert_buf_len);
#endif

    // parse Service Key Certificate
    if ( RSA_CERT_TAG != prov_buf[ pos ] )
    {
        SOFTSIM_LOGE("Wrong tag field of RSA DA certificate" );
        return ERROR_INTERNAL_ERROR;
    }

    uint16_t da_cert_buf_len = GET_UINT16T_LE( 1, (prov_buf + pos) );
    if ( da_cert_buf_len == 0 || da_cert_buf_len  > MAX_RSA_CERT_LEN )
    {
        SOFTSIM_LOGE("Wrong size of RSA DA certificate: 0x%08x", da_cert_buf_len);
        return ERROR_INTERNAL_ERROR;
    }
    pos += 3;
    if ( (pos + da_cert_buf_len + 3) >= PROV_BUF_SIZE )
    {
        SOFTSIM_LOGE("Error in parsing DA sertificate, parse position: 0x%08x", pos);
        return ERROR_INTERNAL_ERROR;
    }

    uint8_t *da_cert_buf = prov_buf + pos;
    pos += da_cert_buf_len;
    rc = tl_fs_save_service_cert(da_cert_buf, da_cert_buf_len);
    if (rc < 0 ) {
        return ERROR_INTERNAL_ERROR;
    }

    dump_bytes("device activate cert", da_cert_buf, da_cert_buf_len);

    *((uint32_t*)(output + off)) = CHANGE_UINT32T_ENDIAN(da_cert_buf_len);
    off += 4;

    *((uint32_t*)(output + 0 )) = CHANGE_UINT32T_ENDIAN(root_cert_buf_len + da_cert_buf_len);

    memcpy(output + off, da_cert_buf, da_cert_buf_len);
    off += da_cert_buf_len;
    *output_len = off;

    rc = tl_fs_save_rsa_certs_chain(output, *output_len);
    if (rc != 0 ) {
        SOFTSIM_LOGD("save RSA certificates chain error");
    }

    dump_bytes("output rawdata:", output, off);
    // parse Service Key Private key
    if ( KEY_TAG != prov_buf[ pos ] )
    {
        SOFTSIM_LOGE("Wrong tag field of RSA DA key" );
        return ERROR_INTERNAL_ERROR;
    }

    uint16_t da_key_buf_len = GET_UINT16T_LE( 1, (prov_buf + pos) );
    if ( da_key_buf_len == 0 || da_key_buf_len  > MAX_RSA_CERT_LEN )
    {
        SOFTSIM_LOGE("Wrong size of RSA DA key: 0x%08x", da_key_buf_len);
        return ERROR_INTERNAL_ERROR;
    }
    pos += 3;
    if ( (pos + da_key_buf_len ) > PROV_BUF_SIZE )
        pos += 3;

    if ( (pos + da_key_buf_len ) > PROV_BUF_SIZE ) {
        SOFTSIM_LOGE("Error in parsing DA key, parse position: 0x%08x.", pos);

        return ERROR_INTERNAL_ERROR;
    }

    uint8_t *da_key_buf = prov_buf + pos;
    dump_bytes("device private key", da_key_buf, da_key_buf_len);
    rc = tl_fs_save_rsa_private_key(da_key_buf, da_key_buf_len);
    if (rc < 0 ) {
        return ERROR_INTERNAL_ERROR;
    }

    pos += da_key_buf_len;

    if( pos >= blob_size ){
        SOFTSIM_HI_LOGD("+++reach the end of blob+++");
        return 0;
    }

    // parse TID(service name)
    if ( TL_NAME_TAG != prov_buf[ pos ] )
    {
        SOFTSIM_LOGE("Wrong tag field of TID" );
        return ERROR_INTERNAL_ERROR;
    }

    uint16_t tid_key_buf_len = GET_UINT16T_LE( 1, (prov_buf + pos) );
    if ( tid_key_buf_len == 0 || tid_key_buf_len  > MAX_RSA_CERT_LEN )
    {
        SOFTSIM_LOGE("Wrong size of RSA DA key: 0x%08x", tid_key_buf_len);
        return ERROR_INTERNAL_ERROR;
    }
    pos += 3;
    if ( (pos + tid_key_buf_len ) > PROV_BUF_SIZE )
        pos += 3;

    if ( (pos + tid_key_buf_len ) > PROV_BUF_SIZE ) {
        SOFTSIM_LOGE("Error in parsing DA key, parse position: 0x%08x.", pos);

        return ERROR_INTERNAL_ERROR;
    }

#ifndef _NDEBUG_
    uint8_t *tid_service_name_buf = prov_buf + pos;
    dump_bytes("TID service name", tid_service_name_buf, tid_key_buf_len);
#endif

#if 0
    parseSoftsimCert(da_cert_buf, da_cert_buf_len);

    memcpy(rsaPrivKeyBuff, da_key_buf, da_key_buf_len);
    rsaPrivKeyLen = da_key_buf_len;

    parse_private_key(rsaPrivKeyBuff, SoftsimRsaKeyInfo.modulus, &SoftsimRsaKeyInfo.mod_len,
                      SoftsimRsaKeyInfo.priv_exp, &SoftsimRsaKeyInfo.priv_exp_len,
                      SoftsimRsaKeyInfo.pub_exp, &SoftsimRsaKeyInfo.pub_exp_len);

    dump_bytes("modulus", SoftsimRsaKeyInfo.modulus, SoftsimRsaKeyInfo.mod_len);
    dump_bytes("priv exp", SoftsimRsaKeyInfo.priv_exp, SoftsimRsaKeyInfo.priv_exp_len);
    dump_bytes("pub exp", SoftsimRsaKeyInfo.pub_exp, SoftsimRsaKeyInfo.pub_exp_len);

    SOFTSIM_LOGE("modulus len=%d", SoftsimRsaKeyInfo.mod_len);
    SOFTSIM_LOGE("pub exp len=%d", SoftsimRsaKeyInfo.pub_exp_len);
    SOFTSIM_LOGE("priv exp len=%d", SoftsimRsaKeyInfo.priv_exp_len);

    rawToString(SoftsimRsaKeyInfo.modulus, (char *)SoftsimRsaKeyInfo.modulus_str, SoftsimRsaKeyInfo.mod_len);
    rawToString(SoftsimRsaKeyInfo.priv_exp, (char *)SoftsimRsaKeyInfo.priv_exp_str, SoftsimRsaKeyInfo.priv_exp_len);
    rawToString(SoftsimRsaKeyInfo.pub_exp, (char *)SoftsimRsaKeyInfo.pub_exp_str, SoftsimRsaKeyInfo.pub_exp_len);

    /*dump_bytes("dump SoftsimRsaKeyInfo", (uint8_t*)&SoftsimRsaKeyInfo, sizeof(RsaKeyParam_t));*/

    rsa_enc_test();
    rsa_dec_test();
#endif
    return 0;
}

int tl_cert_drk_request(cmd_req_t *req, cmd_rsp_t *rsp){
    SOFTSIM_LOGD("file: %s, func: %s", __FILE_NAME__, __func__);

    uint32_t in_len = req->in_len;
    uint32_t out_len = ENCAPSULATED_DATA_LENGTH ;
    SOFTSIM_LOGI(" input length: %d, input:  %p", in_len, req->input);

    dump_bytes("ecrypted blob",(uint8_t*)req->input, in_len);

    if (req->in_len > ENCAPSULATED_DATA_LENGTH || req->in_len > SOFTSIM_INPUT_LEN) {
        SOFTSIM_LOGE(" tl_cert_drk_request req->in_len too large");
        rsp->out_len = 0;
        rsp->status = -1;
        return -1;
    }

    memcpy(encapsulated, req->input, req->in_len);

    /*int retval = init_auth_data(encapsulated,  in_len, drk_certificate, &out_len); */
    int retval = init_auth_data(encapsulated, in_len, rsp->output, &out_len);

    if (retval) {
        SOFTSIM_LOGE(" init_auth_data return out_len: 0x%08x, Error: 0x%08x", out_len, retval);
        rsp->out_len = 0;
        rsp->status = -1;
        return -1;
    }

    SOFTSIM_LOGI(" output length: %d", out_len);

    rsp->out_len = out_len;

    return 0;
}

int tl_cert_get_cached_certs(cmd_req_t *req, cmd_rsp_t *rsp){
    SOFTSIM_LOGD("file: %s, func: %s", __FILE_NAME__, __func__);
    int rc = 0;

    uint32_t output_len = SOFTSIM_OUTPUT_LEN;
    rc = tl_fs_restore_rsa_certs_chain(rsp->output, &output_len);
    rsp->out_len = output_len;
    SOFTSIM_LOGD("tl_fs_restore_rsa_certs_chain  rc %d, len: %d", rc, rsp->out_len);
    /*dump_bytes("dump output cached chain", rsp->output, rsp->out_len);*/

    if(rc == 0 ) {
        rsp->status = 0;
    }else {
        rsp->status = -1;
        rsp->out_len = 0;
    }

    return 0;
}