#include "ssp.h"
#include "ssp_public.h"
#include "ssp_apdu.h"
#include "ssp_util.h"
#include "crypto_module.h"
#include "ssp_service.h"
#include "tz_platform.h"

#include "tz_debug.h"

static SSPSTATUS get_session_handle( ssp_session_t **out );
static SSPSTATUS get_session_by_id( uint32_t id, ssp_session_t **out );
static void free_session_handle( ssp_session_t *session );


SSPSTATUS  generateSessionKeys( uint8_t enc_key[AES_256_KEY_SIZE],
                                 uint8_t mac_key[AES_256_KEY_SIZE],
                                 const uint8_t otp_key[AES_256_KEY_SIZE],
                                 const uint8_t s_rand_1[SSP_SESSION_RANDOM_SIZE],
                                 const uint8_t s_rand_2[SSP_SESSION_RANDOM_SIZE],
                                 const uint8_t tid[SSP_TID_SIZE], const uint8_t *aid, uint32_t aid_size ) {
    static const uint8_t  kdf_prefix_1[4] = { 0, 0, 0, 1 };
    static const uint8_t  kdf_prefix_2[4] = { 0, 0, 0, 2 };
    static const uint8_t  kdf_suffix[2] = { 1, 0 };

    static const uint8_t  enc_key_salt[4] = "ENC";
    static const uint8_t  mac_key_salt[4] = "MAC";

    SSPSTATUS  status;

    LOGD("generateSessionKeys is called");
    CHECK_OP_STATUS( crypto_aes_256_cmac( enc_key, otp_key,
                                          kdf_prefix_1, sizeof(kdf_prefix_1),
                                          enc_key_salt, sizeof(enc_key_salt),
                                          s_rand_1, SSP_SESSION_RANDOM_SIZE,
                                          s_rand_2, SSP_SESSION_RANDOM_SIZE,
                                          tid, SSP_TID_SIZE,
                                          aid, aid_size,
                                          kdf_suffix, sizeof(kdf_suffix),
                                          NULL) );

    CHECK_OP_STATUS( crypto_aes_256_cmac( enc_key + AES_BLOCK_SIZE, otp_key,
                                          kdf_prefix_2, sizeof(kdf_prefix_2),
                                          enc_key_salt, sizeof(enc_key_salt),
                                          s_rand_1, SSP_SESSION_RANDOM_SIZE,
                                          s_rand_2, SSP_SESSION_RANDOM_SIZE,
                                          tid, SSP_TID_SIZE,
                                          aid, aid_size,
                                          kdf_suffix, sizeof(kdf_suffix),
                                          NULL) );

    CHECK_OP_STATUS( crypto_aes_256_cmac( mac_key, otp_key,
                                          kdf_prefix_1, sizeof(kdf_prefix_1),
                                          mac_key_salt, sizeof(mac_key_salt),
                                          s_rand_2, SSP_SESSION_RANDOM_SIZE,
                                          s_rand_1, SSP_SESSION_RANDOM_SIZE,
                                          aid, aid_size,
                                          tid, SSP_TID_SIZE,
                                          kdf_suffix, sizeof(kdf_suffix),
                                          NULL) );

    CHECK_OP_STATUS( crypto_aes_256_cmac( mac_key + AES_BLOCK_SIZE, otp_key,
                                          kdf_prefix_2, sizeof(kdf_prefix_2),
                                          mac_key_salt, sizeof(mac_key_salt),
                                          s_rand_2, SSP_SESSION_RANDOM_SIZE,
                                          s_rand_1, SSP_SESSION_RANDOM_SIZE,
                                          aid, aid_size,
                                          tid, SSP_TID_SIZE,
                                          kdf_suffix, sizeof(kdf_suffix),
                                          NULL) );

ret_point:
    return status;
}


/*
 * SSP communication APIs
 */
 SSPSTATUS ssp_openSession( uint32_t *sessionId, uint8_t channel,
                           const uint8_t *otp_key_blob, uint32_t otp_key_blob_size,
                           const uint8_t tid[SSP_TID_SIZE], const uint8_t *aId, uint32_t aIdLen ) {
    const uint8_t   success_sw[2] = { 0x90, 0x00 };

    SSPSTATUS       status;
    apdu_header_t   hcmd;
    uint8_t         response[MAX_SSP_RAPDU_DATA_SIZE];
    uint32_t        rsp_size;
    ssp_session_t  *hsession = NULL;

    uint8_t         otp_key[AES_256_KEY_SIZE];
#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    uint32_t        otp_key_size = sizeof(otp_key);
#endif
    uint8_t         otp_wrap_key[AES_256_KEY_SIZE];
    uint8_t         sr1[SSP_SESSION_RANDOM_SIZE];
    uint8_t         sr2[SSP_SESSION_RANDOM_SIZE];
    uint8_t         tmp_cmac[AES_BLOCK_SIZE];
    uint8_t         tmp_buf[64];
    uint32_t        tmp_buf_len;

    LOGD("ssp_openSession is called");
    if ( sessionId == NULL || aId == NULL || aIdLen == 0 || aIdLen > MAX_AID_SIZE ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    CHECK_OP_STATUS( get_session_handle( &hsession ) );
    hsession->channel = channel;
#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    if (otp_key_blob_size == 40) {
        CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
        CHECK_OP_STATUS( crypto_aes_256_unwrap_otp( otp_key_blob, otp_key, otp_wrap_key ) );
    } else {
#if defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
        CHECK_OP_STATUS( ssp_unwrap_secure_object( SEM_TID, otp_key_blob, otp_key_blob_size, otp_key, &otp_key_size ) );
#else
        CHECK_OP_STATUS( ssp_unwrap_secure_object( otp_key_blob, otp_key_blob_size, otp_key, &otp_key_size ) );
#endif
    }
#endif
#if defined USE_QSEE
    CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
    CHECK_OP_STATUS( crypto_aes_256_unwrap_otp( otp_key_blob, otp_key, otp_wrap_key ) );
#endif

    hex_print_tag_debug("otp_key", otp_key, AES_256_KEY_SIZE);

    /* Send SESSION CHALLENGE Command */
    /* Generate Session Random of TEE */
    CHECK_OP_STATUS( crypto_gen_random( sr1, SSP_SESSION_RANDOM_SIZE ) );

    /* Compose command data */
    tmp_buf_len = 0;
    BUFFER_APPEND( tmp_buf, tmp_buf_len, sr1, SSP_SESSION_RANDOM_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, tid, SSP_TID_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, aId, aIdLen );

    /* Run command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xD3, tmp_buf_len );
    rsp_size = SSP_SESSION_RANDOM_SIZE + AES_BLOCK_SIZE;

    hex_print_tag_debug("tmp_buf", tmp_buf, tmp_buf_len);
    LOGD("Input channel ID : %u", channel);

    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, tmp_buf, response, &rsp_size ) );

    hex_print_tag_debug("response", response, rsp_size);

    /* Parse Session Random 2 and C-MAC */
    if ( rsp_size != SSP_SESSION_RANDOM_SIZE + AES_BLOCK_SIZE ) {
        LOGE( "Unexpected response size to SESSION CHALLENGE: %u", rsp_size );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    memcpy( sr2, response, SSP_SESSION_RANDOM_SIZE );
    memcpy( hsession->cmac, response + SSP_SESSION_RANDOM_SIZE, AES_BLOCK_SIZE );

    /* Generate Session Keys */
    CHECK_OP_STATUS( generateSessionKeys( hsession->enc_key, hsession->mac_key, otp_key, sr1, sr2, tid, aId, aIdLen ) );

    hex_print_tag_debug("OTP key", otp_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("hsession->enc_key", hsession->enc_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("hsession->mac_key", hsession->mac_key, AES_256_KEY_SIZE);

    /* C-MAC verification */
    CHECK_OP_STATUS( crypto_aes_256_cmac( tmp_cmac, hsession->mac_key,
                                  sr2, SSP_SESSION_RANDOM_SIZE,
                                  sr1, SSP_SESSION_RANDOM_SIZE,
                                  aId, aIdLen,
                                  tid, SSP_TID_SIZE,
                                  success_sw, sizeof(success_sw),
                                  NULL ) );

    if ( crypto_secure_mac_cmp( hsession->cmac, tmp_cmac ) != 0 ) {
        LOGE( "Invalid MAC in response to SESSION CHALLENGE" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

    /* Save session info. */
    *sessionId = hsession->id;
#ifdef OT
    CHECK_OP_STATUS( ssp_getAppletState( &(hsession->applet_state) ) );
    LOGD("Ver : %2X.%2X.%2X", hsession->applet_state.version_major, hsession->applet_state.version_minor, hsession->applet_state.version_release);
#endif
    hsession = NULL;

ret_point:
    crypto_clear_mem( otp_wrap_key, AES_256_KEY_SIZE );
    crypto_clear_mem( otp_key, AES_256_KEY_SIZE );

    free_session_handle( hsession );

    return status;
}


SSPSTATUS ssp_transaction( uint32_t sessionId, p_secEse_7816_cpdu_t cpdu, p_secEse_7816_rpdu_t rpdu ) {
    SSPSTATUS           status;
    ssp_session_t      *hsession;
    apdu_header_t       hcmd;
    apdu_header_t       cmac_hcmd;
    secEse_7816_cpdu_t  cur_cmd;
    secEse_7816_rpdu_t  cur_rsp;
    uint8_t             tmp_buf[MAX_SSP_CAPDU_DATA_SIZE];
    uint8_t             iv[AES_BLOCK_SIZE];
    uint8_t             tmp_mcv[AES_BLOCK_SIZE];
    uint8_t             tmp_cmac[AES_BLOCK_SIZE];
    uint8_t             response[MAX_SSP_RAPDU_DATA_SIZE];
    uint32_t            enc_data_len;
    uint32_t            tmp_buf_len = sizeof(tmp_buf);
    uint8_t             tmp_buf2[MAX_SSP_CAPDU_DATA_SIZE];
    uint32_t            tmp_buf_len2 = sizeof(tmp_buf2);

    LOGD("ssp_transaction is called");
    if ( cpdu == NULL || rpdu == NULL ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    cur_cmd = *cpdu;

    LOGD("Incoming CPDU : CLA=0x%02X, INS=0x%02X, P1=0x%02X, P2=0x%02X, LC=%u, LE=%u", cur_cmd.cla, cur_cmd.ins, cur_cmd.p1, cur_cmd.p2, cur_cmd.lc, cur_cmd.le);
    hex_print_tag_debug("Incoming CPDU", cur_cmd.pdata, cur_cmd.lc);

    CHECK_OP_STATUS( get_session_by_id( sessionId, &hsession ) );

    /* Generate IV */
    CHECK_OP_STATUS( crypto_aes_256_block_encrypt( hsession->cmac, iv, hsession->enc_key ) );

    if ( cur_cmd.lc != 0 ) {
        /* Encrypt plain data */
        CHECK_OP_STATUS( crypto_aes_cbc_256_encrypt( cpdu->pdata, cur_cmd.lc, tmp_buf, &tmp_buf_len, iv, hsession->enc_key ) );
    } else {
        tmp_buf_len = 0;
    }

    cur_cmd.cla |= 0x04;
    cur_cmd.lc = tmp_buf_len + AES_BLOCK_SIZE;
    cur_cmd.le_type = 1;
    cur_cmd.le = MAX_SSP_RAPDU_DATA_SIZE;

    /* Make C-MAC */
    *(uint32_t*)&hcmd = *(uint32_t*)&cur_cmd;
    hcmd.lc = cur_cmd.lc;
    memcpy(&cmac_hcmd, &cur_cmd, sizeof(cmac_hcmd));
    hcmd.cla |= hsession->channel;
#ifdef OT
    if (hsession->applet_state.version_major == 0x02 && hsession->applet_state.version_minor == 0x00 && hsession->applet_state.version_release == 0x08) {
        cmac_hcmd.cla |= hsession->channel;
    }
#endif

    /* Save mcv for C-MAC calculation */
    memcpy(tmp_mcv, hsession->cmac, AES_BLOCK_SIZE);

    CHECK_OP_STATUS( crypto_aes_256_cmac( hsession->cmac, hsession->mac_key,
                                  tmp_mcv, AES_BLOCK_SIZE,
                                  &cmac_hcmd, sizeof(hcmd),
                                  tmp_buf, tmp_buf_len,
                                  NULL ) );

    /* Compose and run command */
    BUFFER_APPEND( tmp_buf, tmp_buf_len, hsession->cmac, AES_BLOCK_SIZE );
    cur_cmd.pdata = tmp_buf;
    cur_rsp.pdata = response;

    LOGD("Transaction cmd : CLA=0x%02X, INS=0x%02X, P1=0x%02X, P2=0x%02X, LC=%u, LE=%u", cur_cmd.cla, cur_cmd.ins, cur_cmd.p1, cur_cmd.p2, cur_cmd.lc, cur_cmd.le);
    hex_print_tag_debug("Encrypted Cmd", tmp_buf, tmp_buf_len);

    CHECK_OP_STATUS( ese_status_2_ssp_status( secEseTransmit( hsession->channel, &cur_cmd, &cur_rsp ) ) );

    LOGD("Transaction rsp : SW1=0x%02X, SW2=0x%02X, Len=%u", cur_rsp.sw1, cur_rsp.sw2, cur_rsp.len);
    hex_print_tag_debug("Encrypted Rsp", cur_rsp.pdata, cur_rsp.len);

    if ( cur_rsp.sw1 == 0x69 && cur_rsp.sw2 == 0x82 ) {
        LOGE( "eSE returned error 69 82 during secure transaction" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

    if ( cur_rsp.len == 0 || cur_rsp.len % AES_BLOCK_SIZE != 0 ) {
        LOGE( "Unexpected response : SW1=0x%02X, SW2=0x%02X, Len=%u", cur_rsp.sw1, cur_rsp.sw2, cur_rsp.len );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    /* Generate IV_2 */
    CHECK_OP_STATUS( crypto_aes_256_block_encrypt( hsession->cmac, iv, hsession->enc_key ) );

    /* Save mcv for C-MAC calculation */
    memcpy(tmp_mcv, hsession->cmac, AES_BLOCK_SIZE);

    /* Save data from APDU response */
    enc_data_len = cur_rsp.len - AES_BLOCK_SIZE;
    memcpy( tmp_buf, response, enc_data_len );
    memcpy( hsession->cmac, response + enc_data_len, AES_BLOCK_SIZE );

    /* C-MAC verification */
    CHECK_OP_STATUS( crypto_aes_256_cmac( tmp_cmac, hsession->mac_key,
                                  tmp_mcv, AES_BLOCK_SIZE,
                                  tmp_buf, enc_data_len,
                                  &cur_rsp.sw1, 2,
                                  NULL ) );

    if ( crypto_secure_mac_cmp( hsession->cmac, tmp_cmac ) != 0 ) {
        LOGE( "Invalid MAC in response to secure command" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

    CHECK_OP_STATUS( crypto_aes_cbc_256_decrypt( tmp_buf, enc_data_len, tmp_buf2, &tmp_buf_len2, iv, hsession->enc_key ) );

    /* Output response data */
    memcpy( rpdu->pdata, tmp_buf2, tmp_buf_len2 );
    rpdu->len = tmp_buf_len2;
    rpdu->sw1 = cur_rsp.sw1;
    rpdu->sw2 = cur_rsp.sw2;

    LOGD("Outgoing RPDU : SW1=0x%02X, SW2=0x%02X, Len=%u", rpdu->sw1, rpdu->sw2, rpdu->len);
    hex_print_tag_debug("Outgoing RPDU", rpdu->pdata, rpdu->len);

ret_point:
    return status;
}


 SSPSTATUS ssp_openSession_with_command( uint32_t *sessionId, uint8_t channel,
                           const uint8_t *otp_key_blob, uint32_t otp_key_blob_size,
                           const uint8_t tid[SSP_TID_SIZE], const uint8_t *aId, uint32_t aIdLen,
                           uint8_t *apdu, uint32_t apduLen, p_secEse_7816_rpdu_t rpdu ) {

    SSPSTATUS       status;
    ssp_session_t  *hsession = NULL;

    secEse_7816_cpdu_t  cur_cmd;

    uint8_t     success_sw[2] = { 0x90, 0x00 };

    uint8_t     otp_key[AES_256_KEY_SIZE];
#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    uint32_t    otp_key_size = sizeof(otp_key);
#endif
    uint8_t     otp_wrap_key[AES_256_KEY_SIZE];
    uint8_t     sr1[SSP_SESSION_RANDOM_SIZE];
    uint8_t     sr2[SSP_SESSION_RANDOM_SIZE];
    uint8_t     encrypted_data[MAX_SSP_RAPDU_DATA_SIZE];
    uint32_t    encrypted_data_len;
    uint8_t     iv[AES_BLOCK_SIZE];
    uint8_t     tmp_cmac[AES_BLOCK_SIZE];
    uint8_t     tmp_buf[MAX_SSP_CAPDU_DATA_SIZE];
    uint32_t    tmp_buf_len;
    uint8_t     tmp_buf_for_append[MAX_SSP_CAPDU_DATA_SIZE];
    uint8_t     tmp_buf_for_ec[64], tmp_buf_for_ed[MAX_SSP_CAPDU_DATA_SIZE];
    uint32_t    tmp_buf_for_ec_len, tmp_buf_for_ed_len;
    uint8_t     tag_E1[1] = { 0xE1 };
    uint8_t     tag_EC[1] = { 0xEC };
    uint8_t     tag_ED[1] = { 0xED };
    uint8_t     temp_tlv_buf[3];
    uint8_t     temp_tlv_buf_len;
    uint8_t     i;

    LOGD("ssp_openSession_with_command is called");
    if ( sessionId == NULL || aId == NULL || aIdLen == 0 || aIdLen > MAX_AID_SIZE || apdu == NULL || rpdu == NULL) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    if (5 + SSP_SESSION_RANDOM_SIZE + SSP_TID_SIZE + MAX_AID_SIZE + 3 + apduLen > MAX_SSP_CAPDU_DATA_SIZE) {
        status = SSPSTATUS_OVER_MAX_DATA_SIZE;
        goto ret_point;
    }

    CHECK_OP_STATUS( get_session_handle( &hsession ) );
    hsession->channel = channel;
#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    if (otp_key_blob_size == 40) {
        CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
        CHECK_OP_STATUS( crypto_aes_256_unwrap_otp( otp_key_blob, otp_key, otp_wrap_key ) );
    } else {
#if defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
        CHECK_OP_STATUS( ssp_unwrap_secure_object( SEM_TID, otp_key_blob, otp_key_blob_size, otp_key, &otp_key_size ) );
#else
        CHECK_OP_STATUS( ssp_unwrap_secure_object( otp_key_blob, otp_key_blob_size, otp_key, &otp_key_size ) );
#endif
    }
#endif
#if defined USE_QSEE
    CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
    CHECK_OP_STATUS( crypto_aes_256_unwrap_otp( otp_key_blob, otp_key, otp_wrap_key ) );
#endif

    hex_print_tag_debug("otp_key", otp_key, AES_256_KEY_SIZE);

    /* Send SESSION CHALLENGE Command */
    /* Generate Session Random of TEE */
    CHECK_OP_STATUS( crypto_gen_random( sr1, SSP_SESSION_RANDOM_SIZE ) );

    /* Compose command data */
    tmp_buf_for_ec_len = 0;
    BUFFER_APPEND( tmp_buf_for_ec, tmp_buf_for_ec_len, sr1, SSP_SESSION_RANDOM_SIZE );
    BUFFER_APPEND( tmp_buf_for_ec, tmp_buf_for_ec_len, tid, SSP_TID_SIZE );
    BUFFER_APPEND( tmp_buf_for_ec, tmp_buf_for_ec_len, aId, aIdLen );
    BER_TLV_LENGTH(tmp_buf_for_ec_len, temp_tlv_buf, temp_tlv_buf_len);
    BUFFER_APPEND_FRONT( tmp_buf_for_append, tmp_buf_for_ec, tmp_buf_for_ec_len, temp_tlv_buf, temp_tlv_buf_len );
    BUFFER_APPEND_FRONT( tmp_buf_for_append, tmp_buf_for_ec, tmp_buf_for_ec_len, tag_EC, 1 );

    tmp_buf_for_ed_len = 0;
    BUFFER_APPEND( tmp_buf_for_ed, tmp_buf_for_ed_len, apdu, apduLen);
    BER_TLV_LENGTH(tmp_buf_for_ed_len, temp_tlv_buf, temp_tlv_buf_len);
    BUFFER_APPEND_FRONT( tmp_buf_for_append, tmp_buf_for_ed, tmp_buf_for_ed_len, temp_tlv_buf, temp_tlv_buf_len );
    BUFFER_APPEND_FRONT( tmp_buf_for_append, tmp_buf_for_ed, tmp_buf_for_ed_len, tag_ED, 1 );

    tmp_buf_len = 0;
    BUFFER_APPEND( tmp_buf, tmp_buf_len, tmp_buf_for_ec, tmp_buf_for_ec_len );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, tmp_buf_for_ed, tmp_buf_for_ed_len );
    BER_TLV_LENGTH(tmp_buf_len, temp_tlv_buf, temp_tlv_buf_len);
    BUFFER_APPEND_FRONT( tmp_buf_for_append, tmp_buf, tmp_buf_len, temp_tlv_buf, temp_tlv_buf_len );
    BUFFER_APPEND_FRONT( tmp_buf_for_append, tmp_buf, tmp_buf_len, tag_E1, 1 );

    hex_print_tag_debug("tmp_buf", tmp_buf, tmp_buf_len);

    cur_cmd.cla = 0x80;
    cur_cmd.ins = 0xD4;
    cur_cmd.p1= 0x00;
    cur_cmd.p2 = 0x00;
    cur_cmd.cpdu_type = 0x00;
    cur_cmd.lc = tmp_buf_len;
    cur_cmd.pdata = tmp_buf;
    cur_cmd.le_type = 0x01;
    cur_cmd.le = 0x00;

    CHECK_OP_STATUS( ese_status_2_ssp_status( secEseTransmit( channel, &cur_cmd, rpdu ) ) );

    LOGD("Transaction rsp : SW1=0x%02X, SW2=0x%02X, Len=%u", rpdu->sw1, rpdu->sw2, rpdu->len);
    hex_print_tag_debug("Encrypted Rsp", rpdu->pdata, rpdu->len);

    if ( !(rpdu->sw1 == 0x90 && rpdu->sw2 == 0x00) && !(rpdu->sw1 == 0x63 && rpdu->sw2 == 0x10) ) {
        LOGE( "Invalid response state : SW1=0x%02X, SW2=0x%02X, Len=%u", rpdu->sw1, rpdu->sw2, rpdu->len );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

#ifdef OT
    CHECK_OP_STATUS( ssp_getAppletState( &(hsession->applet_state) ) );
    LOGD("Ver : %2X.%2X.%2X", hsession->applet_state.version_major, hsession->applet_state.version_minor, hsession->applet_state.version_release);

    // Remove SW from data part
    if (hsession->applet_state.version_major == 0x02 && hsession->applet_state.version_minor == 0x00 && hsession->applet_state.version_release == 0x08) {
        rpdu->len = rpdu->len - 2;
    }
#endif

    if ( rpdu->len == 0 || rpdu->len % AES_BLOCK_SIZE != 0 ) {
        LOGE( "Invalid response data size : SW1=0x%02X, SW2=0x%02X, Len=%u", rpdu->sw1, rpdu->sw2, rpdu->len );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    success_sw[0] = rpdu->sw1;
    success_sw[1] = rpdu->sw2;

    encrypted_data_len = rpdu->len - SSP_SESSION_RANDOM_SIZE - AES_BLOCK_SIZE;
    memcpy( encrypted_data, rpdu->pdata, encrypted_data_len );
    memcpy( sr2, rpdu->pdata + encrypted_data_len, SSP_SESSION_RANDOM_SIZE );
    memcpy( hsession->cmac, rpdu->pdata + encrypted_data_len + SSP_SESSION_RANDOM_SIZE, AES_BLOCK_SIZE );

    /* Generate Session Keys */
    CHECK_OP_STATUS( generateSessionKeys( hsession->enc_key, hsession->mac_key,
                                  otp_key, sr1, sr2, tid, aId, aIdLen ) );

    hex_print_tag_debug("OTP key", otp_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("hsession->enc_key", hsession->enc_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("hsession->mac_key", hsession->mac_key, AES_256_KEY_SIZE);

    /* C-MAC verification */
    CHECK_OP_STATUS( crypto_aes_256_cmac( tmp_cmac, hsession->mac_key,
                                  encrypted_data, encrypted_data_len,
                                  sr2, SSP_SESSION_RANDOM_SIZE,
                                  sr1, SSP_SESSION_RANDOM_SIZE,
                                  aId, aIdLen,
                                  tid, SSP_TID_SIZE,
                                  success_sw, sizeof(success_sw),
                                  NULL ) );

    if ( crypto_secure_mac_cmp( hsession->cmac, tmp_cmac ) != 0 ) {
        LOGE( "Invalid MAC in response to SESSION CHALLENGE" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

    /* Generate IV */
    memcpy(tmp_cmac, sr1, AES_BLOCK_SIZE);
    for (i = 0 ; i< AES_BLOCK_SIZE ; i++) {
        tmp_cmac[i] ^= sr2[i];
    }
    CHECK_OP_STATUS( crypto_aes_256_block_encrypt( tmp_cmac, iv, hsession->enc_key ) );

    /* Decrypt cipher data */
    CHECK_OP_STATUS( crypto_aes_cbc_256_decrypt( encrypted_data, encrypted_data_len, tmp_buf, &tmp_buf_len, iv, hsession->enc_key ) );

    /* Output response data */
    memcpy( rpdu->pdata, tmp_buf, tmp_buf_len );
    rpdu->len = tmp_buf_len;

    LOGD("Outgoing RPDU : SW1=0x%02X, SW2=0x%02X, Len=%u", rpdu->sw1, rpdu->sw2, rpdu->len);
    hex_print_tag_debug("Outgoing RPDU", rpdu->pdata, rpdu->len);

    /* Save session info. */
    *sessionId = hsession->id;
    hsession = NULL;

ret_point:
    crypto_clear_mem( otp_wrap_key, AES_256_KEY_SIZE );
    crypto_clear_mem( otp_key, AES_256_KEY_SIZE );

    free_session_handle( hsession );

    return status;
}

#if 0
SSPSTATUS ssp_transaction_with_command( uint32_t sessionId, p_secEse_7816_cpdu_t cpdu, p_secEse_7816_rpdu_t rpdu ) {
    SSPSTATUS           status;
    ssp_session_t      *hsession;
    apdu_header_t       hcmd;
    apdu_header_t       cmac_hcmd;
    secEse_7816_cpdu_t  cur_cmd;
    secEse_7816_rpdu_t  cur_rsp;
    uint8_t             iv[AES_BLOCK_SIZE];
    uint8_t             tmp_buf[MAX_CAPDU_DATA_SIZE];
    uint8_t             tmp_mcv[AES_BLOCK_SIZE];
    uint8_t             tmp_cmac[AES_BLOCK_SIZE];
    uint8_t             response[MAX_RAPDU_DATA_SIZE];
    uint32_t            enc_data_len;
    uint32_t            tmp_buf_len = sizeof(tmp_buf);
#ifdef USE_SCRYPTO
    uint8_t             tmp_buf2[MAX_CAPDU_DATA_SIZE];
    uint32_t            tmp_buf_len2 = sizeof(tmp_buf2);
#endif

    LOGD("ssp_transaction_with_command is called");
    if ( cpdu == NULL || rpdu == NULL ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    cur_cmd = *cpdu;

    LOGD("Incoming CPDU : CLA=0x%02X, INS=0x%02X, P1=0x%02X, P2=0x%02X, LC=%u, LE=%u", cur_cmd.cla, cur_cmd.ins, cur_cmd.p1, cur_cmd.p2, cur_cmd.lc, cur_cmd.le);
    hex_print_tag_debug("Incoming CPDU", cur_cmd.pdata, cur_cmd.lc);

    CHECK_OP_STATUS( get_session_by_id( sessionId, &hsession ) );

    if ( cur_cmd.lc != 0 ) {
        /* Add padding to plain data */
        tmp_buf_len = cur_cmd.lc - (cur_cmd.lc % AES_BLOCK_SIZE) + AES_BLOCK_SIZE;
        if ( tmp_buf_len > sizeof(tmp_buf) - AES_BLOCK_SIZE ) {
            /* If ciphertext + MAC length is more than maximum command data length... */
            status = SSPSTATUS_OVER_MAX_DATA_SIZE;
            goto ret_point;
        }

        tmp_buf_len = cur_cmd.lc;
        memcpy(tmp_buf, cur_cmd.pdata, cur_cmd.lc);
    } else {
        tmp_buf_len = 0;
    }

    cur_cmd.cla |= 0x04;
    cur_cmd.lc = tmp_buf_len + AES_BLOCK_SIZE;
    cur_cmd.le_type = 1;
    cur_cmd.le = MAX_RAPDU_DATA_SIZE;

    /* Make C-MAC */
    *(uint32_t*)&hcmd = *(uint32_t*)&cur_cmd;
    hcmd.lc = cur_cmd.lc;
    memcpy(&cmac_hcmd, &cur_cmd, sizeof(cmac_hcmd));
    hcmd.cla |= hsession->channel;

#ifdef OT
    if (hsession->applet_state.version_major == 0x02 && hsession->applet_state.version_minor == 0x00 && hsession->applet_state.version_release == 0x08) {
        cmac_hcmd.cla |= hsession->channel;
    }
#endif

    /* Save mcv for C-MAC calculation */
    memcpy(tmp_mcv, hsession->cmac, AES_BLOCK_SIZE);

    CHECK_OP_STATUS( crypto_aes_256_cmac( hsession->cmac, hsession->mac_key,
                                  tmp_mcv, AES_BLOCK_SIZE,
                                  &cmac_hcmd, sizeof(hcmd),
                                  tmp_buf, tmp_buf_len,
                                  NULL ) );

    /* Compose and run command */
    BUFFER_APPEND( tmp_buf, tmp_buf_len, hsession->cmac, AES_BLOCK_SIZE );
    cur_cmd.pdata = tmp_buf;
    cur_rsp.pdata = response;

    LOGD("Transaction cmd : CLA=0x%02X, INS=0x%02X, P1=0x%02X, P2=0x%02X, LC=%u, LE=%u", cur_cmd.cla, cur_cmd.ins, cur_cmd.p1, cur_cmd.p2, cur_cmd.lc, cur_cmd.le);
    hex_print_tag_debug("Encrypted Cmd", tmp_buf, tmp_buf_len);

    CHECK_OP_STATUS( ese_status_2_ssp_status( secEseTransmit( hsession->channel, &cur_cmd, &cur_rsp ) ) );

    LOGD("Transaction rsp : SW1=0x%02X, SW2=0x%02X, Len=%u", cur_rsp.sw1, cur_rsp.sw2, cur_rsp.len);
    hex_print_tag_debug("Encrypted Rsp", cur_rsp.pdata, cur_rsp.len);

    if ( cur_rsp.sw1 == 0x69 && cur_rsp.sw2 == 0x82 ) {
        LOGE( "eSE returned error 69 82 during secure transaction" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

    if ( cur_rsp.len == 0 || cur_rsp.len % AES_BLOCK_SIZE != 0 ) {
        LOGE( "Unexpected response : SW1=0x%02X, SW2=0x%02X, Len=%u", cur_rsp.sw1, cur_rsp.sw2, cur_rsp.len );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    /* Generate IV_2 */
    CHECK_OP_STATUS( crypto_aes_256_block_encrypt( hsession->cmac, iv, hsession->enc_key ) );

    /* Save mcv for C-MAC calculation */
    memcpy(tmp_mcv, hsession->cmac, AES_BLOCK_SIZE);

    /* Save data from APDU response */
    enc_data_len = cur_rsp.len - AES_BLOCK_SIZE;
    memcpy( tmp_buf, response, enc_data_len );
    memcpy( hsession->cmac, response + enc_data_len, AES_BLOCK_SIZE );

    /* C-MAC verification */
    CHECK_OP_STATUS( crypto_aes_256_cmac( tmp_cmac, hsession->mac_key,
                                  tmp_mcv, AES_BLOCK_SIZE,
                                  tmp_buf, enc_data_len,
                                  &cur_rsp.sw1, 2,
                                  NULL ) );

    if ( crypto_secure_mac_cmp( hsession->cmac, tmp_cmac ) != 0 ) {
        LOGE( "Invalid MAC in response to secure command" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

    CHECK_OP_STATUS( crypto_aes_cbc_256_decrypt( tmp_buf, enc_data_len, tmp_buf2, &tmp_buf_len2, iv, hsession->enc_key ) );

    /* Output response data */
    memcpy( rpdu->pdata, tmp_buf2, tmp_buf_len2 );
    rpdu->len = tmp_buf_len2;

    LOGD("Outgoing RPDU : SW1=0x%02X, SW2=0x%02X, Len=%u", rpdu->sw1, rpdu->sw2, rpdu->len);
    hex_print_tag_debug("Outgoing RPDU", rpdu->pdata, rpdu->len);

ret_point:
    return status;
}
#endif

SSPSTATUS ssp_closeSession( uint32_t sessionId ) {
    SSPSTATUS       status;
    ssp_session_t  *hsession;
    apdu_header_t   hcmd;

    /* TODO: temporary workaround */
    uint8_t buff;
    uint32_t buflen = 0;

    LOGD("ssp_closeSession is called");
    CHECK_OP_STATUS( get_session_by_id( sessionId, &hsession ) );

    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xD5, 0 );
    CHECK_OP_STATUS( ssp_send_service_command( hsession->channel, hcmd, NULL, &buff, &buflen ) );

    free_session_handle( hsession );

ret_point:
    return status;
}


SSPSTATUS ssp_openSession_public( const uint8_t *otp_key_blob, uint32_t otp_key_blob_size,
                           uint8_t tId[SSP_TID_SIZE], uint32_t tIdLen, const uint8_t *aId, uint32_t aIdLen,
                           uint8_t *wrappedSessionInfo, uint32_t *wrappedSessionInfoSize ) {
    const uint8_t   success_sw[2] = { 0x90, 0x00 };

    SSPSTATUS       status;
    apdu_header_t   hcmd;
    uint8_t         response[MAX_SSP_RAPDU_DATA_SIZE];
    uint32_t        rsp_size;
    ssp_session_t   hsession;
    uint8_t         channel;

    uint8_t         sr1[SSP_SESSION_RANDOM_SIZE];
    uint8_t         sr2[SSP_SESSION_RANDOM_SIZE];
    uint8_t         tmp_cmac[AES_BLOCK_SIZE];
    uint8_t         tmp_buf[64];
    uint32_t        tmp_buf_len;

    uint8_t         otp_wrap_key[AES_256_KEY_SIZE];
    uint8_t         otp_key[AES_256_KEY_SIZE];
#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    uint32_t        otp_key_size  = sizeof(otp_key);
#endif
#ifdef USE_QSEE
    char            appName[256] = {0, };
#endif

    LOGD("ssp_openSession_public is called");
    if ( aId == NULL || aIdLen == 0 || aIdLen > MAX_AID_SIZE ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    CHECK_OP_STATUS( ssp_open_service_connection_with_appletID(aId, aIdLen, &channel) );
    hsession.channel = channel;

#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    if (otp_key_blob_size == 40) {
        CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
        CHECK_OP_STATUS( crypto_aes_256_unwrap_otp( otp_key_blob, otp_key, otp_wrap_key ) );
    } else {
#if defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
        CHECK_OP_STATUS( ssp_unwrap_secure_object( SEM_TID, otp_key_blob, otp_key_blob_size, otp_key, &otp_key_size ) );
#else
        CHECK_OP_STATUS( ssp_unwrap_secure_object( otp_key_blob, otp_key_blob_size, otp_key, &otp_key_size ) );
#endif
    }
#endif
#ifdef USE_QSEE
    CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
    CHECK_OP_STATUS( crypto_aes_256_unwrap_otp( otp_key_blob, otp_key, otp_wrap_key ) );

    memcpy(appName, tId, tIdLen);
    ssp_genTaID((const uint8_t *)tId, tIdLen, tId);
    hex_print_tag_debug("Generated TA ID", tId, SSP_TID_SIZE);
#else
    (void)tIdLen;
#endif

    hex_print_tag_debug("otp_key", otp_key, AES_256_KEY_SIZE);

    /* Send SESSION CHALLENGE Command */
    /* Generate Session Random of TEE */
    CHECK_OP_STATUS( crypto_gen_random( sr1, SSP_SESSION_RANDOM_SIZE ) );

    /* Compose command data */
    tmp_buf_len = 0;
    BUFFER_APPEND( tmp_buf, tmp_buf_len, sr1, SSP_SESSION_RANDOM_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, tId, SSP_TID_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, aId, aIdLen );

    /* Run command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xD3, tmp_buf_len );
    rsp_size = SSP_SESSION_RANDOM_SIZE + AES_BLOCK_SIZE;

    hex_print_tag_debug("tmp_buf", tmp_buf, tmp_buf_len);
    LOGD("Input channel ID : %u", channel);

    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, tmp_buf, response, &rsp_size ) );

    hex_print_tag_debug("response", response, rsp_size);

    /* Parse Session Random 2 and C-MAC */
    if ( rsp_size != SSP_SESSION_RANDOM_SIZE + AES_BLOCK_SIZE ) {
        LOGE( "Unexpected response size to SESSION CHALLENGE: %u", rsp_size );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    memcpy( sr2, response, SSP_SESSION_RANDOM_SIZE );
    memcpy( hsession.cmac, response + SSP_SESSION_RANDOM_SIZE, AES_BLOCK_SIZE );

    /* Generate Session Keys */
    CHECK_OP_STATUS( generateSessionKeys( hsession.enc_key, hsession.mac_key, otp_key, sr1, sr2, tId, aId, aIdLen ) );

    /* C-MAC verification */
    CHECK_OP_STATUS( crypto_aes_256_cmac( tmp_cmac, hsession.mac_key,
                                  sr2, SSP_SESSION_RANDOM_SIZE,
                                  sr1, SSP_SESSION_RANDOM_SIZE,
                                  aId, aIdLen,
                                  tId, SSP_TID_SIZE,
                                  success_sw, sizeof(success_sw),
                                  NULL ) );

    if ( crypto_secure_mac_cmp( hsession.cmac, tmp_cmac ) != 0 ) {
        LOGE( "Invalid MAC in response to SESSION CHALLENGE" );
        status = SSPSTATUS_INVALID_CMAC;
        goto ret_point;
    }

#ifdef OT
    CHECK_OP_STATUS( ssp_getAppletState( &(hsession.applet_state) ) );
    LOGD("Ver : %2X.%2X.%2X", hsession.applet_state.version_major, hsession.applet_state.version_minor, hsession.applet_state.version_release);
#endif

    // Save Trust ID in session Info.
    memcpy(hsession.tid, tId, SSP_TID_SIZE);

    hex_print_tag_debug("OTP key", otp_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("hsession->cmac", hsession.cmac, AES_BLOCK_SIZE);
    hex_print_tag_debug("hsession->enc_key", hsession.enc_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("hsession->mac_key", hsession.mac_key, AES_256_KEY_SIZE);

#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    CHECK_OP_STATUS( ssp_wrap_secure_object( hsession.tid, (const uint8_t *)&hsession, sizeof(ssp_session_t), wrappedSessionInfo, wrappedSessionInfoSize) );
#endif
#ifdef USE_QSEE
    CHECK_OP_STATUS( ssp_wrap_secure_object( (const uint8_t *)appName, (const uint8_t *)&hsession, sizeof(ssp_session_t), wrappedSessionInfo, wrappedSessionInfoSize) );
#endif

ret_point:
#ifdef USE_QSEE
    crypto_clear_mem( otp_wrap_key, AES_256_KEY_SIZE );
#endif
    crypto_clear_mem( otp_key, AES_256_KEY_SIZE );

#if 0
    memset(&hsession, 0x01, sizeof(ssp_session_t));
    CHECK_OP_STATUS( ssp_wrap_secure_object( (const uint8_t *)appName, (const uint8_t *)&hsession, sizeof(ssp_session_t), wrappedSessionInfo, wrappedSessionInfoSize) );
#endif

    return status;
}

/* ========================== Session handles manipulations ========================== */
static ssp_session_t  handles[4];
static uint32_t handles_ocuppied_mask = 0;


static SSPSTATUS get_session_handle( ssp_session_t **out ) {
    int i;
    for ( i = 0; i < 4; ++i ) {
        if ( (handles_ocuppied_mask & (1<<i)) == 0 ) {
            handles_ocuppied_mask |= (1<<i);
            handles[i].id = ((handles[i].id & (~3)) + 4) | i;

            *out = &handles[i];
            return SSPSTATUS_SUCCESS;
        }
    }

    return SSPSTATUS_SESSION_OPEN_FAIL;
}

static SSPSTATUS get_session_by_id( uint32_t id, ssp_session_t **out ) {
    int i;

    i = id & 3;
    if ( (handles_ocuppied_mask & (1<<i)) == 0 || handles[i].id != id ) {
        return SSPSTATUS_INVALID_ARGUMENT;
    }

    *out = &handles[i];
    return SSPSTATUS_SUCCESS;
}

static void free_session_handle( ssp_session_t *session ) {
    if ( session != NULL ) {
        handles_ocuppied_mask &= ~(1<<(session->id & 3));
    }
}