#include <scp03/SCP03_transceive.h>
#include <scp03/SCP03.h>
#include "sec_apdu.h"
#include "sec_EseStatus.h"
#include "dk_log.h"
#include "TZ_platform_defs.h"
//#include "tees_secure_object.h"

#include "SCP03lib.h"

DK_Result process_scp03_open_session(tl_dk_ctx_t *ctx, tz_scp03_transceive_cmd_t sendmsg, tz_scp03_open_session_payload_t *respmsg)
{
    uint32_t ret;

    uint8_t securityLevel = 0x33;
    uint8_t kvn;
    size_t klen;
    uint8_t *kenc = NULL, *kmac = NULL, *kdek = NULL;

    uint8_t appletAid[] = {(byte)0xA0, 0x00, 0x00, 0x08, 0x09, 0x43, 0x43, 0x43, 0x44, 0x4b, 0x41, 0x76, 0x31};
    uint32_t appletAidLen = sizeof(appletAid);

    uint8_t associatedAid[] = {(byte)0xA0, 0x00, 0x00, 0x02, 0x20, 0x19, 0x03, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05};
    uint32_t associatedAidLen = sizeof(associatedAid);

    uint8_t defaultAID[] = {(byte)0x00};
    uint32_t defaultAIDLen = sizeof(defaultAID);

    uint8_t rData[256] = {0};
    secEse_7816_rpdu_t rpdu;

    uint8_t keyblob[200];
    size_t keyblob_len = 200;

    uint8_t unwrapped_key_blob[48] = {0};

    ret = spiOpen();
    if (ret)
    {
        TTY_LOG("error on spiOpen %d ", ret);
        goto catch_error;
    }

    ret = secEseOpen(&ctx->channel.id);
    if (ret)
    {
        TTY_LOG("error on secEseOpen");
        goto catch_error;
    }

    ctx->channel.is_open = 1;

    memset(&rpdu, 0, sizeof(secEse_7816_rpdu_t));
    rpdu.pdata = rData;

    TTY_LOG("buffer len %d", sendmsg.credential.buffer_len);
    TTY_LOG("select on channel id %d", ctx->channel.id);
    ret = secEseSelect(ctx->channel.id, appletAid, 0, appletAidLen, &rpdu);
    TTY_LOG("select finished %d with %x%x", ret, rpdu.sw1, rpdu.sw2);
    if (ctx->channel.id < 1 || ctx->channel.id > 3 || ret)
    {
        TTY_LOG("error on secEseSelect");
        goto catch_error;
    }

    kvn = sendmsg.credential.kvn;

    ret = scp03_credential_un_wrap(ctx, &sendmsg.credential, unwrapped_key_blob);
    if (ret)
    {
        TTY_LOG("error on scp03_credential_un_wrap");
        goto catch_error;
    }

    //print_array(unwrapped_key_blob, sendmsg.buffer_len);

    klen = SCP03_CREDENTIAL_LEN / 3;

    dk_memcpy(keyblob, unwrapped_key_blob, SCP03_CREDENTIAL_LEN);

    if (klen != 16 && klen != 24 && klen != 32)
    {
        TTY_LOG("bad key length: %d. Must be either 16, 24 or 32 bytes long.", klen);
        ret = DK_ERROR_BAD_PARAM;
        goto catch_error;
    }

    // DK_LOG_INFO("klen: %d", klen);

    kenc = dk_malloc(klen);
    kmac = dk_malloc(klen);
    kdek = dk_malloc(klen);

    dk_memcpy(kenc, keyblob, klen);
    dk_memcpy(kmac, keyblob + klen, klen);
    dk_memcpy(kdek, keyblob + klen + klen, klen);

    // TTY_LOG("keyblob[%d]: ", keyblob_len);
    // print_array(keyblob, keyblob_len);
    // TTY_LOG("kvn: 0x%x\n", kvn);
    // TTY_LOG("klen: %d\n", klen);
    // TTY_LOG("kenc: ");
    // print_array(kenc, klen);
    // TTY_LOG("kmac: ");
    // print_array(kmac, klen);
    // TTY_LOG("kdek: ");
    // print_array(kdek, klen);
    // TTY_LOG("appletId: ");
    // print_array(appletAid, sizeof(appletAid));
    // TTY_LOG("securityLevel: 0x%x\n", securityLevel);

    ret = openSession(ctx->channel.id, kvn, kenc, kmac, kdek, appletAid, appletAidLen, securityLevel);
    if (ret)
    {
        TTY_LOG("could not open session %x", ret);
        goto catch_error;
    }
    TTY_LOG("opensession finished %x", ret);

    ret = DK_SUCCESS;

catch_error:
    if (kenc)
    {
        dk_memset(kenc, 0, klen);
        dk_free(kenc);
    }

    if (kmac)
    {
        dk_memset(kmac, 0, klen);
        dk_free(kmac);
    }

    if (kdek)
    {
        dk_memset(kdek, 0, klen);
        dk_free(kdek);
    }

    dk_memset(keyblob, 0, 200);

    respmsg->payload.resp.return_code = ret;
    return ret;
}

DK_Result process_non_scp03_open_session(tl_dk_ctx_t *ctx, tz_scp03_open_session_payload_t *respmsg)
{
    uint32_t ret;

    uint8_t appletAid[] = {(byte)0xA0, 0x00, 0x00, 0x08, 0x09, 0x43, 0x43, 0x43, 0x44, 0x4b, 0x41, 0x76, 0x31};
    uint32_t appletAidLen = sizeof(appletAid);

    uint8_t associatedAid[] = {(byte)0xA0, 0x00, 0x00, 0x02, 0x20, 0x19, 0x03, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05};
    uint32_t associatedAidLen = sizeof(associatedAid);

    uint8_t defaultAID[] = {(byte)0x00};
    uint32_t defaultAIDLen = sizeof(defaultAID);

    uint8_t rData[256] = {0};
    secEse_7816_rpdu_t rpdu;

    ret = spiOpen();
    if (ret)
    {
        TTY_LOG("error on spiOpen %d", ret);
        goto catch_error;
    }
    TTY_LOG("DigitalKey : spiOpen success %d ", ret);

    /*
    ret = initProcess();
    if (ret) {
        TTY_LOG("error on initProcess %d", ret);
        }*/

    ret = secEseOpen(&ctx->channel.id);
    if (ret)
    {
        TTY_LOG("error on secEseOpen %d", ret);
        goto catch_error;
    }
    TTY_LOG("DigitalKey : secEseOpen success $d", ret);

    ctx->channel.is_open = 1;

    memset(&rpdu, 0, sizeof(secEse_7816_rpdu_t));
    rpdu.pdata = rData;

    TTY_LOG("select on channel id %d", ctx->channel.id);
    ret = secEseSelect(ctx->channel.id, appletAid, 0, appletAidLen, &rpdu);
    TTY_LOG("select finished %d with %x%x", ret, rpdu.sw1, rpdu.sw2);
    if (ctx->channel.id < 1 || ctx->channel.id > 3 || ret)
    {
        TTY_LOG("error on secEseSelect");
        goto catch_error;
    }

    ret = DK_SUCCESS;

catch_error:

    respmsg->payload.resp.channel_id = ctx->channel.id;
    respmsg->payload.resp.return_code = ret;
    return ret;
}

DK_Result send_apdu(uint8_t id, p_secEse_7816_cpdu_t pcpdu, p_secEse_7816_rpdu_t prpdu)
{
    pcpdu->le_type = 1;
    pcpdu->le = 0x00;

    SCPSTATUS rc = (DK_Result)apduTransceive(id, pcpdu, prpdu);
    TTY_LOG("apdutransceve result rc %d", rc);
    if (rc != DK_SUCCESS)
    {
        TTY_LOG("apduTransceive failed");
        return rc;
    }

    TTY_LOG("rpdu sw : %02x%02x", prpdu->sw1, prpdu->sw2);
    return rc;
}

DK_Result process_scp03_create_ca(tl_dk_ctx_t *ctx, tz_scp03_create_ca_payload_t *sendmsg, tz_scp03_create_ca_payload_t *respmsg)
{
    TTY_LOG("create ca start");
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    uint8_t data[CPDU_MAX_SIZE];

    uint8_t rdata[MAX_RAPDU_DATA_SIZE]; // should be 256
    uint16_t *sw = (uint16_t *)&respmsg->payload.resp.sw;

    cpdu.cla = 0x84;
    cpdu.ins = 0x38;
    cpdu.p1 = 0x00;
    cpdu.p2 = 0x04;
    cpdu.lc = sendmsg->payload.cmd.data_len;
    dk_memcpy(data, sendmsg->payload.cmd.data, cpdu.lc);
    cpdu.pdata = data;

    rpdu.pdata = rdata;
    cpdu_dump(&cpdu);
    rc = send_apdu(ctx->channel.id, &cpdu, &rpdu);
    if (rc != DK_SUCCESS)
    {
        TTY_LOG("apduTransceive failed");
        return rc;
    }


    uint32_t resp_index = 0;
    *sw = (rpdu.sw1 << 8 | rpdu.sw2);
    while (*sw == ISO7816_SW_MORE_DATA)
    {
        TTY_LOG("keep asking ca cert");
        secEse_7816_cpdu_t kcpdu = {0};
        uint8_t data2[CPDU_MAX_SIZE];
        dk_memcpy(respmsg->payload.resp.data + resp_index, rpdu.pdata, rpdu.len);
        resp_index += rpdu.len;

        respmsg->payload.resp.data_len = resp_index;

        memset(&rpdu, 0x00, sizeof(secEse_7816_rpdu_t));
        memset(rdata, 0x00, MAX_RAPDU_DATA_SIZE);

        kcpdu.cla = 0x84;
        kcpdu.ins = 0x38;
        kcpdu.p1 = 0x01;
        kcpdu.p2 = 0x04;
        kcpdu.lc = sendmsg->payload.cmd.data_len;
        dk_memcpy(data2, sendmsg->payload.cmd.data, kcpdu.lc);
        kcpdu.pdata = data2;
        kcpdu.le_type = 1;
        kcpdu.le = 0x00;

        rpdu.pdata = rdata;
        rc = apduTransceive(ctx->channel.id, &kcpdu, &rpdu);
        *sw = (rpdu.sw1 << 8 | rpdu.sw2);
        if (rc != DK_SUCCESS)
        {
            TTY_LOG("apduTransceive failed");
            return rc;
        }
    }

    if (*sw == ISO7816_SW_NO_ERROR)
    {
        dk_memcpy(respmsg->payload.resp.data + resp_index, rpdu.pdata, rpdu.len);
        resp_index += rpdu.len;
        respmsg->payload.resp.data_len = resp_index;
        TTY_LOG("return data len %d", resp_index);
        respmsg->payload.resp.return_code = DK_SUCCESS;
        return DK_SUCCESS;
    }
    else
    {
        respmsg->payload.resp.return_code = DK_ERROR_GENERIC;
        return DK_ERROR_GENERIC;
    }
}


DK_Result process_scp03_invoke_read(tl_dk_ctx_t *ctx, tz_scp03_invoke_read_payload_t *sendmsg, tz_scp03_invoke_read_payload_t *respmsg)
{
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    uint8_t pdata[CPDU_MAX_SIZE];
    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256

    uint8_t *apdu = sendmsg->payload.cmd.apdu;

    cpdu.cla = apdu[APDU_OFFSET_CLA];
    cpdu.ins = apdu[APDU_OFFSET_INS];
    cpdu.p1 = apdu[APDU_OFFSET_P1];
    cpdu.p2 = apdu[APDU_OFFSET_P2];
    cpdu.lc = apdu[APDU_OFFSET_LC];

    TTY_LOG("process invoke and read with lc %", cpdu.lc);
    cpdu.pdata = pdata;
    if (cpdu.lc > 0) {
        dk_memcpy(cpdu.pdata, apdu+APDU_OFFSET_CDATA, cpdu.lc);
    }
    cpdu.le_type = 0x01;
    cpdu.le = 0x00;

    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;
    rc = send_apdu(ctx->channel.id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        return DK_ERROR_GENERIC;
    }

    uint16_t *sw = (uint16_t *)&respmsg->payload.resp.sw;
    *sw = (uint16_t)(rpdu.sw1 << 8 | rpdu.sw2);
    if (*sw != ISO7816_SW_NO_ERROR) {
        TTY_LOG("invoke command returned error %02x%02x", rpdu.sw1, rpdu.sw2);
        return DK_ERROR_GENERIC;
    }

    int32_t length = rpdu.pdata[0] << 8 | rpdu.pdata[1];
    TTY_LOG("response length is %d", length);
    rc = read_buffer_range(ctx->channel.id, (uint8_t *)(respmsg->payload.resp.data), 0, length, (uint16_t *)&respmsg->payload.resp.sw);
    if (rc != DK_SUCCESS) {
        TTY_LOG("error to read internal buffer");
        return DK_ERROR_GENERIC;
    }
    respmsg->payload.resp.data_len = length;

    return DK_SUCCESS;
}

int32_t invoke_command(uint8_t id, uint32_t command_length, uint8_t *command, uint16_t *sw)
{
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    uint8_t pdata[CPDU_MAX_DATA_SIZE];
    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256

    if (command_length < APDU_OFFSET_LC) {
        TTY_LOG("command length %d is not enough", command_length);
        return DK_ERROR_GENERIC;
    }

    cpdu.cla = command[APDU_OFFSET_CLA];
    cpdu.ins = command[APDU_OFFSET_INS];
    cpdu.p1 = command[APDU_OFFSET_P1];
    cpdu.p2 = command[APDU_OFFSET_P2];

    if (command_length > APDU_OFFSET_LC) {
        cpdu.lc = command[APDU_OFFSET_LC];
    } else {
        cpdu.lc = 0;
    }
    cpdu.pdata = pdata;
    if (cpdu.lc != 0) {
        dk_memcpy(pdata, command+APDU_OFFSET_CDATA, command_length-APDU_OFFSET_CDATA);
    }
    cpdu.le_type = 0x01;
    cpdu.le = 0x00;

    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;

    rc = send_apdu(id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        return DK_ERROR_GENERIC;
    }

    for (int i = 0; i < rpdu.len; i++)
    {
        TTY_LOG("output : %d", rpdu.pdata[i]);
    }

    *sw = (uint16_t)(rpdu.sw1 << 8 | rpdu.sw2);
    if (*sw == ISO7816_SW_NO_ERROR)
    {
        int32_t ret = rpdu.pdata[0] << 8 | rpdu.pdata[1];
        TTY_LOG("response length is %d", ret);
        return ret;
    }
    else
    {
        TTY_LOG("invoke command failed");
        return 0;
    }
}

DK_Result write_buffer(uint8_t id, uint8_t *pdata_source, int32_t start, int32_t end, uint16_t *sw)
{
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    TTY_LOG("writebuffer from %d to %d length %d", start, end, end - start);

    uint8_t pdata[APDU_DATA_SIZE_MAX];
    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256

    cpdu.cla = 0x84;
    cpdu.ins = 0xD0;
    cpdu.p1 = (uint8_t)((start & 0xFF00) >> 8);
    cpdu.p2 = (uint8_t)(start & 0x00FF);
    cpdu.lc = end - start;

    dk_memcpy(pdata, pdata_source, cpdu.lc);
    cpdu.pdata = pdata;
    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;

    rc = send_apdu(id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        return DK_ERROR_GENERIC;
    }

    *sw = (uint16_t)(rpdu.sw1 << 8 | rpdu.sw2);
    if (*sw == ISO7816_SW_NO_ERROR)
    {
        TTY_LOG("write buffer is success");
        return DK_SUCCESS;
    }
    else
    {
        return DK_ERROR_GENERIC;
    }
}

DK_Result read_buffer_range(uint8_t id, uint8_t *data, int32_t from, int32_t size, uint16_t *sw)
{
    int32_t max_rpdu_size = get_max_rpdu_size();
    for (int32_t i = from; i < from+size; i += max_rpdu_size)
    {
        int32_t payload_end = (i + max_rpdu_size) > from+size ? from+size : i + max_rpdu_size;
        if (read_buffer(id, (uint8_t *)(data + i), i, payload_end - i, sw))
        {
            return DK_ERROR_GENERIC;
        }
    }
    return DK_SUCCESS;
}

DK_Result read_buffer(uint8_t id, uint8_t *data, int32_t start, int32_t size, uint16_t *sw)
{
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    TTY_LOG("readbuffer from %d size %d", start, size);

    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256
    
    uint8_t pdata[C_MAC_SIZE+SIZE_TLV_SIZE];
    set_size_tlv((uint8_t *)pdata, 0, (uint8_t)size);

    cpdu.cla = 0x84;
    cpdu.ins = 0xB0;
    cpdu.p1 = (uint8_t)((start & 0xFF00) >> 8);
    cpdu.p2 = (uint8_t)(start & 0x00FF);
    cpdu.lc = SIZE_TLV_SIZE;
    cpdu.pdata = pdata;

    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;

    rc = send_apdu(id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        return DK_ERROR_GENERIC;
    }

    rpdu_dump(&rpdu);

    *sw = (uint16_t)(rpdu.sw1 << 8 | rpdu.sw2);
    if (*sw == ISO7816_SW_NO_ERROR)
    {
        dk_memcpy(data, rdata, size);
        TTY_LOG("read buffer success");
        return DK_SUCCESS;
    }
    else
    {
        return DK_ERROR_GENERIC;
    }
}

DK_Result process_scp03_write_invoke(tl_dk_ctx_t *ctx, tz_scp03_write_invoke_payload_t *sendmsg, tz_scp03_write_invoke_payload_t *respmsg)
{
    DK_Result rc;

    if (ctx->channel.id == 0)
    {
        TTY_LOG("channel closed");
        return DK_ERROR_BAD_PARAM;
    }

    int32_t creation_data_tlv_len = sendmsg->payload.cmd.inputbuffer_len;
    for (int32_t i = 0; i < creation_data_tlv_len; i += SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE)
    {
        int32_t payload_end = (i + SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE) > creation_data_tlv_len ? creation_data_tlv_len : i + SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE;
        TTY_LOG("writing buffer from %d to %d at %p %x", i, payload_end, (uint8_t *)(sendmsg->payload.cmd.inputbuffer + i), sendmsg->payload.cmd.inputbuffer[i]);
        if (write_buffer(ctx->channel.id, (uint8_t *)(sendmsg->payload.cmd.inputbuffer + i), i, payload_end, (uint16_t *)&respmsg->payload.resp.sw))
        {
            respmsg->payload.resp.return_code = DK_ERROR_GENERIC;
            return DK_ERROR_GENERIC;
        }
    }

    rc = invoke_command(ctx->channel.id, sendmsg->payload.cmd.commandapdu_len, sendmsg->payload.cmd.commandapdu, (uint16_t *)&respmsg->payload.resp.sw);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        goto catch_error;
    }


    respmsg->payload.resp.data_len = 0;
    TTY_LOG("resp data len %d", respmsg->payload.resp.data_len);

    rc = DK_SUCCESS;

 catch_error:
    return rc;
}


DK_Result process_scp03_write_invoke_read(tl_dk_ctx_t *ctx, tz_scp03_write_invoke_read_payload_t *sendmsg, tz_scp03_write_invoke_read_payload_t *respmsg)
{
    DK_Result rc;

    if (ctx->channel.id == 0)
    {
        TTY_LOG("channel closed");
        return DK_ERROR_BAD_PARAM;
    }

    int32_t creation_data_tlv_len = sendmsg->payload.cmd.inputbuffer_len;
    for (int32_t i = 0; i < creation_data_tlv_len; i += SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE)
    {
        int32_t payload_end = (i + SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE) > creation_data_tlv_len ? creation_data_tlv_len : i + SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE;
        TTY_LOG("writing buffer from %d to %d at %p %x", i, payload_end, (uint8_t *)(sendmsg->payload.cmd.inputbuffer + i), sendmsg->payload.cmd.inputbuffer[i]);
        if (write_buffer(ctx->channel.id, (uint8_t *)(sendmsg->payload.cmd.inputbuffer + i), i, payload_end, (uint16_t *)&respmsg->payload.resp.sw))
        {
            respmsg->payload.resp.return_code = DK_ERROR_GENERIC;
            return DK_ERROR_GENERIC;
        }
    }

    int32_t response_length = invoke_command(ctx->channel.id, sendmsg->payload.cmd.commandapdu_len, sendmsg->payload.cmd.commandapdu, (uint16_t *)&respmsg->payload.resp.sw);

    if (!response_length)
    {
        TTY_LOG("error to call command %x", sendmsg->payload.cmd.commandapdu[APDU_OFFSET_INS]);
        respmsg->payload.resp.return_code = DK_ERROR_GENERIC;
        return DK_ERROR_GENERIC;
    }

    rc = read_buffer_range(ctx->channel.id, (uint8_t *)(respmsg->payload.resp.data), 0, response_length, (uint16_t *)&respmsg->payload.resp.sw);
    if (rc != DK_SUCCESS) {
        TTY_LOG("error to read internal buffer");
        return DK_ERROR_GENERIC;
    }
    respmsg->payload.resp.data_len = response_length;

    return DK_SUCCESS;
}

DK_Result process_apdu_transceive(tl_dk_ctx_t *ctx, tz_scp03_transceive_payload_t *sendmsg, tz_scp03_transceive_payload_t *respmsg)
{
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    uint8_t data[CPDU_MAX_SIZE];
    uint32_t data_len;

    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256

    if (ctx->channel.id == 0)
    {
        TTY_LOG("channel closed");
        rc = DK_ERROR_BAD_PARAM;
        goto catch_error;
    }

    data_len = sendmsg->payload.cmd.data_len;

    if (data_len > SCP03LIB_MAX_CPDU_SIZE)
    {
        TTY_LOG("data length %d too big, must be at most %d bytes", data_len, CPDU_MAX_SIZE);
        rc = DK_ERROR_BAD_PARAM;
        goto catch_error;
    }

    for (int i = 0; i < sendmsg->payload.cmd.data_len; i++)
    {
        TTY_LOG("input : %d", sendmsg->payload.cmd.data[i]);
    }

    TTY_LOG("lc : %x", sendmsg->payload.cmd.data[APDU_OFFSET_LC]);
    TTY_LOG("data len : %x", data_len);
    dk_memcpy(data, sendmsg->payload.cmd.data, data_len);

    cpdu.cla = data[APDU_OFFSET_CLA];
    cpdu.ins = data[APDU_OFFSET_INS];
    cpdu.p1 = data[APDU_OFFSET_P1];
    cpdu.p2 = data[APDU_OFFSET_P2];
    if (data_len > APDU_OFFSET_P2)
    {
        cpdu.lc = data[APDU_OFFSET_LC];
        cpdu.pdata = data + APDU_OFFSET_CDATA;
    }

    if (cpdu.lc + APDU_OFFSET_CDATA < data_len)
    {
        cpdu.le = data[APDU_OFFSET_CDATA + cpdu.lc];
        cpdu.le_type = 1;
    }
    else
    {
        cpdu.le_type = 1;
        cpdu.le = 0x00;
    }
    TTY_LOG("cpdu lc : %x", cpdu.lc);
    TTY_LOG("cpdu le_type : %x", cpdu.le_type);
    TTY_LOG("cpdu le : %x", cpdu.le);

    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;

    rc = apduTransceive(ctx->channel.id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        goto catch_error;
    }

    dk_memcpy(respmsg->payload.resp.data, rpdu.pdata, rpdu.len);
    respmsg->payload.resp.data_len = rpdu.len;
    respmsg->payload.resp.sw = rpdu.sw1 << 8 | rpdu.sw2;

    for (int i = 0; i < rpdu.len; i++)
    {
        TTY_LOG("output : %d", respmsg->payload.resp.data[i]);
    }

    TTY_LOG("resp data len %d", respmsg->payload.resp.data_len);
    TTY_LOG("sw1 %x sw2 %x", rpdu.sw1, rpdu.sw2);

    rc = DK_SUCCESS;

catch_error:
    return rc;
}

DK_Result set_size_tlv(uint8_t *data, int32_t idx, uint8_t size) {
    data[idx] = SIZE_TLV_TAG;
    data[idx+1] = 0x01;
    data[idx+2] = (uint8_t)size;
    
    return DK_SUCCESS;
}

DK_Result get_private_data(uint8_t id, uint8_t *keyid, uint8_t *data, int32_t from, int32_t size, uint16_t *sw) {
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    TTY_LOG("get private data from %d size %d", from, size);

    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256
    uint8_t pdata[KEY_IDENTIFIER_TLV_LENGTH+C_MAC_SIZE+SIZE_TLV_SIZE];
    dk_memcpy(pdata, keyid, KEY_IDENTIFIER_TLV_LENGTH);
    set_size_tlv((uint8_t *)pdata, KEY_IDENTIFIER_TLV_LENGTH, (uint8_t)size);
    

    cpdu.cla = 0x84;
    cpdu.ins = 0x78;
    cpdu.p1 = (uint8_t)((from & 0xFF00) >> 8);
    cpdu.p2 = (uint8_t)(from & 0x00FF);
    cpdu.lc = KEY_IDENTIFIER_TLV_LENGTH+SIZE_TLV_SIZE;
    cpdu.pdata = pdata;

    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;

    rc = send_apdu(id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        return DK_ERROR_GENERIC;
    }

    TTY_LOG("output length : %d", rpdu.len);
    for (int i = 0; i < rpdu.len; i++)
    {
        TTY_LOG("output : %d", rpdu.pdata[i]);
    }

    *sw = (uint16_t)(rpdu.sw1 << 8 | rpdu.sw2);
    if (*sw == ISO7816_SW_NO_ERROR)
    {
        dk_memcpy(data, rdata, size);
        TTY_LOG("get private data success");
        return DK_SUCCESS;
    }
    else
    {
        return DK_ERROR_GENERIC;
    }
}

DK_Result get_private_data_range(uint8_t id, uint8_t *keyid, uint8_t *data, int32_t from, int32_t total_size, uint16_t *sw)
{
    TTY_LOG("get private data range from %d size %d", from, total_size);
    const int32_t PAYLOAD_MAX =  get_max_rpdu_size() - KEY_IDENTIFIER_TLV_LENGTH;
    for (int32_t i = 0; i < total_size; i += PAYLOAD_MAX)
    {
        int32_t payload_size= (i + PAYLOAD_MAX) > total_size ? total_size - i : PAYLOAD_MAX;
        if (get_private_data(id, (uint8_t *)keyid, (uint8_t *)(data + i), from+i, payload_size, sw))
        {
            return DK_ERROR_GENERIC;
        }
    }
    return DK_SUCCESS;
}

DK_Result set_private_data(uint8_t id, uint8_t *keyid, uint8_t *data, int32_t from, int32_t size, uint16_t *sw) {
    DK_Result rc;
    secEse_7816_cpdu_t cpdu = {0};
    secEse_7816_rpdu_t rpdu = {0};

    TTY_LOG("set private data from %d size %d", from, size);

    uint8_t rdata[APDU_DATA_SIZE_MAX]; // should be 256
    uint8_t pdata[APDU_DATA_SIZE_MAX];
    uint8_t pdata_offset = KEY_IDENTIFIER_TLV_LENGTH;
    dk_memcpy(pdata, keyid, KEY_IDENTIFIER_TLV_LENGTH);
    pdata[pdata_offset++] = 0x4b;
    if (size > 128) {
        pdata[pdata_offset++] = 0x81;
        pdata[pdata_offset++] = (uint8_t)size;
    } else {
        pdata[pdata_offset++] = (uint8_t)size;
    }
    dk_memcpy(pdata+pdata_offset, data, size);
    cpdu.cla = 0x84;
    cpdu.ins = 0x7a;
    cpdu.p1 = (uint8_t)((from & 0xFF00) >> 8);
    cpdu.p2 = (uint8_t)(from & 0x00FF);
    cpdu.lc = pdata_offset+size;
    cpdu.pdata = pdata;

    cpdu_dump(&cpdu);

    rpdu.pdata = rdata;

    rc = send_apdu(id, &cpdu, &rpdu);
    if (rc)
    {
        TTY_LOG("error on transceive: %x", rc);
        return DK_ERROR_GENERIC;
    }

    TTY_LOG("output length : %d", rpdu.len);
    for (int i = 0; i < rpdu.len; i++)
    {
        TTY_LOG("output : %d", rpdu.pdata[i]);
    }

    *sw = (uint16_t)(rpdu.sw1 << 8 | rpdu.sw2);
    if (*sw == ISO7816_SW_NO_ERROR)
    {
        TTY_LOG("set private data success");
        return DK_SUCCESS;
    }
    else
    {
        TTY_LOG("set private data failed");
        return DK_ERROR_GENERIC;
    }
}

DK_Result set_private_data_range(uint8_t id, uint8_t *keyid, uint8_t *data, int32_t from, int32_t total_size, uint16_t *sw)
{
    const uint32_t PAYLOAD_MAX = SCP03LIB_MAX_ENCRYPTED_APDU_DATA_SIZE - KEY_IDENTIFIER_TLV_LENGTH - TAG_LENGTH_MAX;
    for (int32_t i = 0; i < total_size; i += PAYLOAD_MAX)
    {
        int32_t payload_size = (i + PAYLOAD_MAX) > total_size ? total_size - i : PAYLOAD_MAX;
        if (set_private_data(id, (uint8_t *)keyid, (uint8_t *)(data + i), from+i, payload_size, sw))
        {
            return DK_ERROR_GENERIC;
        }
    }
    return DK_SUCCESS;
}

DK_Result process_scp03_set_private_data(tl_dk_ctx_t *ctx, tz_scp03_set_private_data_payload_t *sendmsg, tz_scp03_set_private_data_payload_t *respmsg) {
    if(set_private_data_range(ctx->channel.id, sendmsg->payload.cmd.key_id, sendmsg->payload.cmd.payload, sendmsg->payload.cmd.data_offset, sendmsg->payload.cmd.payload_len, &respmsg->payload.resp.sw)) {
        TTY_LOG("set private data failed");
        return DK_ERROR_GENERIC;
    }

    uint8_t sigbmp;
    if(get_private_data(ctx->channel.id, sendmsg->payload.cmd.key_id, &sigbmp, sendmsg->payload.cmd.sig_bmp_offset, 1, &respmsg->payload.resp.sw)) {
        TTY_LOG("get signaling bitmap from private mailbox failed");
        return DK_ERROR_GENERIC;
    }

    TTY_LOG("current sigbmp %x", sigbmp);
    sigbmp |= sendmsg->payload.cmd.sig_bmp;
    TTY_LOG("sigbmp after oring %x", sigbmp);

    if(set_private_data(ctx->channel.id, sendmsg->payload.cmd.key_id, &sigbmp, sendmsg->payload.cmd.sig_bmp_offset, 1, &respmsg->payload.resp.sw)) {
        TTY_LOG("set signaling bitmap to private mailbox failed");
        return DK_ERROR_GENERIC;
    }

    return DK_SUCCESS;
}

DK_Result process_scp03_get_private_data(tl_dk_ctx_t *ctx, tz_scp03_get_private_data_payload_t *sendmsg, tz_scp03_get_private_data_payload_t *respmsg) {
    if(get_private_data_range(ctx->channel.id, sendmsg->payload.cmd.key_id, respmsg->payload.resp.data, sendmsg->payload.cmd.offset, sendmsg->payload.cmd.size, &respmsg->payload.resp.sw)) {
        TTY_LOG("get private data failed");
        return DK_ERROR_GENERIC;
    }
    respmsg->payload.resp.data_len = sendmsg->payload.cmd.size;
    return DK_SUCCESS;
}



DK_Result process_scp03_close_session(tl_dk_ctx_t *ctx)
{
    DK_Result rc;

    if (ctx->channel.is_open == FALSE)
    {
        TTY_LOG("channel already closed");
        rc = DK_SUCCESS;
        goto catch_error;
    }

    rc = secEseClose(ctx->channel.id);
    if (rc)
    {
        TTY_LOG("error closing channel");
        goto catch_error;
    }

    rc = spiClose();
    if (rc)
    {
        TTY_LOG("error closing spi");
        goto catch_error;
    }

    closeSession();

    ctx->channel.is_open = FALSE;

    rc = DK_SUCCESS;

catch_error:
    return rc;
}

DK_Result process_non_scp03_close_session(tl_dk_ctx_t *ctx, tz_scp03_close_session_payload_t *respmsg)
{
    DK_Result rc;

    if (ctx->channel.is_open == FALSE)
    {
        TTY_LOG("channel already closed");
        rc = DK_SUCCESS;
        goto catch_error;
    }

    rc = secEseClose(ctx->channel.id);
    if (rc)
    {
        TTY_LOG("error closing channel");
        goto catch_error;
    }

    rc = spiClose();
    if (rc)
    {
        TTY_LOG("error closing spi");
        goto catch_error;
    }

    ctx->channel.is_open = FALSE;

    rc = DK_SUCCESS;

catch_error:
    respmsg->payload.resp.return_code = rc;
    return rc;
}

DK_Result process_scp03_re_wrap(tl_dk_ctx_t *ctx, tz_scp03_setup_keys_payload_t *sendmsg, tz_scp03_setup_keys_payload_t *respmsg)
{
    DK_Result rc;

    uint8_t key_blob[168];
    uint32_t key_blob_size = 168;

    uint8_t unwrapped_key_blob[48];
    uint32_t unwrapped_key_blob_size = 48;

#if 0
    SO_AccessControlInfoType ac_info = {
        TA_ID_AC,
        {},
        {}
    };
#endif
    if (sendmsg->payload.cmd.key_blob == NULL ||
        sendmsg->payload.cmd.key_blob_size < 0 ||
        sendmsg->payload.cmd.key_blob_size > 168)
    {
        rc = DK_ERROR_BAD_PARAM;
        goto catch_error;
    }

    key_blob_size = sendmsg->payload.cmd.key_blob_size;
    dk_memcpy(key_blob, sendmsg->payload.cmd.key_blob, key_blob_size);
    TTY_LOG("send key_blob_size = %d", key_blob_size);

    rc = TZ_unwrap_session_data((uint8_t *)CREDENTIAL_SO_SRC_SEM_UUID, sizeof(CREDENTIAL_SO_SRC_SEM_UUID), key_blob, key_blob_size, unwrapped_key_blob, &unwrapped_key_blob_size);
    //rc = TZ_unwrap_data(key_blob, key_blob_size, unwrapped_key_blob, &unwrapped_key_blob_size);
    if (rc)
    {
        TTY_LOG("unwrap unsuccessful: %x", rc);
        goto catch_error;
    }
    TTY_LOG("unwrapped_key_blob_size = %d", unwrapped_key_blob_size);

    printf(TAG "unwraped keys[%d]: ", unwrapped_key_blob_size);
    print_array(unwrapped_key_blob, unwrapped_key_blob_size);

    key_blob_size = 168;

    //TODO - change to dk common interface
    rc = TZ_wrap_data(unwrapped_key_blob, unwrapped_key_blob_size, key_blob, &key_blob_size);
    if (rc)
    {
        TTY_LOG("rewrapped unsuccessful: %x", rc);
        goto catch_error;
    }

    TTY_LOG("rewrapped key_blob size : %d", key_blob_size);
    // TTY_LOG(key_blob, key_blob_size);

    dk_memcpy(respmsg->payload.resp.so.encrypted_keys, key_blob, key_blob_size);
    respmsg->payload.resp.so.encrypted_keys_len = key_blob_size;
    respmsg->payload.resp.return_code = DK_SUCCESS;

    //   dk_memcpy(respmsg->payload.resp.so.encrypted_keys, unwrapped_key_blob, unwrapped_key_blob_size);
    // respmsg->payload.resp.so.encrypted_keys_len = unwrapped_key_blob_size;
    // respmsg->payload.resp.return_code = DK_SUCCESS;

    dk_memset(key_blob, 0, sizeof(key_blob));
    dk_memset(unwrapped_key_blob, 0, sizeof(unwrapped_key_blob));

    rc = DK_SUCCESS;

catch_error:
    return rc;
}

DK_Result scp03_credential_un_wrap(tl_dk_ctx_t *ctx, tz_scp03_credential_t *credential, uint8_t* unwrapped_key_blob)
{
    DK_Result rc;

    uint8_t key_blob[168];
    uint32_t key_blob_size = 168;
    uint32_t unwrapped_key_blob_size = SCP03_CREDENTIAL_LEN;

    if (credential->buffer == NULL ||
        credential->buffer_len < 0 ||
        credential->buffer_len > 168)
    {
        rc = DK_ERROR_BAD_PARAM;
        goto catch_error;
    }

    key_blob_size = credential->buffer_len;
    dk_memcpy(key_blob, credential->buffer, key_blob_size);
    TTY_LOG("scp03_credential_un_wrap key_blob_size = %d", key_blob_size);

    rc = TZ_unwrap_data(key_blob, key_blob_size, unwrapped_key_blob, &unwrapped_key_blob_size);
    return rc;

catch_error:
    return rc;
}