#include "ssp_apdu.h"
#include "tz_debug.h"

SSPSTATUS ssp_open_service_connection( uint8_t *channel, uint8_t ats[3] ) {
    ESESTATUS               status;
    secEse_7816_rpdu_t      cur_rsp;
    uint8_t                 resp_data[MAX_SSP_RAPDU_DATA_SIZE];
    uint8_t                 tmp_channel;
    static const uint8_t    ssp_applet_id[] =
        { 0xA0, 0x00, 0x00, 0x02, 0x20, 0x53, 0x45, 0x43, 0x53, 0x45, 0x53, 0x50, 0x52, 0x4F, 0x54, 0x31 };

    LOGD( "ssp_open_service_connection is called");
    status = secEseOpen( &tmp_channel );
    if ( ESESTATUS_SUCCESS != status ) {
        LOGE( "Cannot open channel. ESESTATUS = 0x%08X", status );
        tmp_channel = INVALID_CHANNEL;
        goto ret_point;
    }

    cur_rsp.pdata = resp_data;
    status = secEseSelect( tmp_channel, (uint8_t*)ssp_applet_id, 0, sizeof(ssp_applet_id), &cur_rsp );
    if ( ESESTATUS_SUCCESS != status ) {
        LOGE( "Cannot select SSP applet. ESESTATUS = 0x%08X", status );
        goto ret_point;
    }

    if ( cur_rsp.sw1 != 0x90 || cur_rsp.sw2 != 0x00 ) {
        LOGE( "Service command SW = %02X %02x", (uint32_t)cur_rsp.sw1, (uint32_t)cur_rsp.sw2 );
        ssp_close_service_connection( tmp_channel );
        return SSPSTATUS_SSP_NOT_EXIST;
    }

    if ( cur_rsp.len != 3 ) {
        LOGE( "SSP applet returned %u bytes on SELECT instead of 3", cur_rsp.len );
        status = SSPSTATUS_FAILED;
        goto ret_point;
    }

    memcpy( ats, resp_data, 3 );

    *channel = tmp_channel;
    tmp_channel = INVALID_CHANNEL;

    return ese_status_2_ssp_status( status );

ret_point:
    ssp_close_service_connection( tmp_channel );
    return ese_status_2_ssp_status( status );
}

SSPSTATUS ssp_open_service_connection_with_appletID( const uint8_t *applet_id, uint8_t applet_id_size, uint8_t *channel) {
    ESESTATUS               status;
    secEse_7816_rpdu_t      cur_rsp;
    uint8_t                 resp_data[MAX_SSP_RAPDU_DATA_SIZE];
    uint8_t                 tmp_channel;

    LOGD( "ssp_open_service_connection_with_appletID is called");
    status = secEseOpen( &tmp_channel );
    if ( ESESTATUS_SUCCESS != status ) {
        LOGE( "Cannot open channel. ESESTATUS = 0x%08X", status );
        tmp_channel = INVALID_CHANNEL;
        goto ret_point;
    }

    cur_rsp.pdata = resp_data;
    status = secEseSelect( tmp_channel, (uint8_t*)applet_id, 0, applet_id_size, &cur_rsp );
    if ( ESESTATUS_SUCCESS != status ) {
        LOGE( "Cannot select applet. ESESTATUS = 0x%08X", status );
        goto ret_point;
    }

    if ( cur_rsp.sw1 != 0x90 || cur_rsp.sw2 != 0x00 ) {
        LOGE( "Service command SW = %02X %02x", (uint32_t)cur_rsp.sw1, (uint32_t)cur_rsp.sw2 );
        ssp_close_service_connection( tmp_channel );
        return SSPSTATUS_SSP_NOT_EXIST;
    }

    LOGD( "applet returned %u bytes on SELECT", cur_rsp.len );
    hex_print_tag_debug("RSP DATA", cur_rsp.pdata, cur_rsp.len);

    *channel = tmp_channel;
    tmp_channel = INVALID_CHANNEL;

    return ese_status_2_ssp_status( status );

ret_point:
    ssp_close_service_connection( tmp_channel );
    return ese_status_2_ssp_status( status );
}


SSPSTATUS ssp_close_service_connection( uint8_t channel ) {
    ESESTATUS               status;

    LOGD( "ssp_close_service_connection is called");
    if ( channel == INVALID_CHANNEL ) {
        return SSPSTATUS_SUCCESS;
    }

    status = secEseClose( channel );
    if ( ESESTATUS_SUCCESS != status ) {
        LOGE( "Cannot close channel. ESESTATUS = 0x%08X", status );
        return ese_status_2_ssp_status( status );
    }

    return SSPSTATUS_SUCCESS;
}


SSPSTATUS ssp_send_service_command( uint8_t channel, apdu_header_t hcmd,
                                    const uint8_t *cmd_data, uint8_t *rsp_data, uint32_t *rsp_data_size ) {
    ESESTATUS           status;
    secEse_7816_cpdu_t  cur_cmd;
    secEse_7816_rpdu_t  cur_rsp;

    cur_cmd.cla = hcmd.cla;
    cur_cmd.ins = hcmd.ins;
    cur_cmd.p1 = hcmd.p1;
    cur_cmd.p2 = hcmd.p2;
    cur_cmd.lc = hcmd.lc;
    cur_cmd.cpdu_type = 0;
    cur_cmd.pdata = (uint8_t*)cmd_data;
    cur_cmd.le = (rsp_data_size == NULL) ? 0 : *rsp_data_size;
    cur_cmd.le_type = (rsp_data_size == NULL) ? 0 : 1;
    cur_rsp.pdata = rsp_data;
//    cur_rsp.len = cur_cmd.le;

    LOGD( "ssp_send_service_command is called");

    LOGD("cur_cmd.lc : %u", cur_cmd.lc);
    if ( rsp_data_size != NULL ) {
        LOGD("rsp_data_size : %u", *rsp_data_size);
    }
    LOGD("cur_cmd.le : %u", cur_cmd.le);

    status = secEseTransmit( channel, &cur_cmd, &cur_rsp );
    if ( ESESTATUS_SUCCESS != status ) {
        LOGE( "Cannot communicate eSE. ESESTATUS = 0x%08X", status );
        return ese_status_2_ssp_status( status );
    }

    if ( cur_cmd.ins == 0xF4 && cur_rsp.sw1 == 0x69 && cur_rsp.sw2 == 0x85 ) {
        LOGI( "PUT CERT and GET CERT have not been executed, execution is needed" );
        return SSPSTATUS_CERT_NOT_EXIST;
    } else if ( cur_cmd.ins == 0xF5 && cur_rsp.sw1 == 0x69 && cur_rsp.sw2 == 0x82 ) {
        LOGI( "CERT is not matched, PUT CERT is needed" );
        return SSPSTATUS_CERT_NOT_EXIST;
    } else if ( cur_cmd.ins == 0xD4 && cur_rsp.sw1 == 0x63 && cur_rsp.sw2 == 0x10 ) {
        LOGI( "More data available" );
    } else if ( cur_rsp.sw1 != 0x90 || cur_rsp.sw2 != 0x00 ) {
        LOGE( "Service command SW = %02X %02x", (uint32_t)cur_rsp.sw1, (uint32_t)cur_rsp.sw2 );
        return SSPSTATUS_FAILED;
    }

    if ( rsp_data_size != NULL ) {
        *rsp_data_size = cur_rsp.len;
    }

    return SSPSTATUS_SUCCESS;
}


SSPSTATUS ese_status_2_ssp_status( ESESTATUS code ) {
    switch( code ) {
        case ESESTATUS_SUCCESS :
            return SSPSTATUS_SUCCESS;
        case ESESTATUS_INVALID_CHANNLE_ID :
            return SSPSTATUS_INVALID_ARGUMENT;
        default:
            return SSPSTATUS_FAILED;
    }
}