#include "ssp_service.h"
#include "ssp_apdu.h"
#include "ssp_util.h"
#include "tz_debug.h"


static const uint8_t  ssp_provisioning_alg[] = "id-aes256-CBC";

SSPSTATUS generate_otp_key( uint8_t otp_key[ SHA256_DIGEST_SIZE ], const uint8_t ecc_secret[ SSP_ECC_SECRET_SIZE ],
                            const uint8_t apId[ SSP_AP_ID_SIZE ], const uint8_t ese_id[ SSP_ESE_ID_SIZE ] ) {
    static const uint8_t  kdf_prefix[4] = { 0, 0, 0, 1 };

    LOGD("generate_otp_key is called");
    return crypto_sha256( otp_key,
                          kdf_prefix, sizeof(kdf_prefix),
                          ecc_secret, SSP_ECC_SECRET_SIZE,
                          ssp_provisioning_alg, sizeof(ssp_provisioning_alg) - 1,
                          apId, SSP_AP_ID_SIZE,
                          ese_id, SSP_ESE_ID_SIZE,
                          NULL);
}


SSPSTATUS ssp_service_doProvisioning(
    uint8_t *drk_cert, uint32_t drk_cert_size, uint8_t *service_cert, uint32_t service_cert_size,
    uint8_t service_key[SSP_ECC_PRIVKEY_SIZE], uint8_t *otp_key_blob_out, uint32_t *otp_key_blob_out_size ) {
    SSPSTATUS       status;
    uint8_t         channel = INVALID_CHANNEL;
    uint8_t         otp_key[AES_256_KEY_SIZE];
    uint8_t         otp_wrap_key[AES_256_KEY_SIZE];
    uint8_t         tmp_buf[256];
    uint8_t         apId[SSP_AP_ID_SIZE];
    uint8_t         eSEId[SSP_ESE_ID_SIZE];
    uint8_t         publicKey[SSP_ECC_PUBKEY_SIZE];
    uint8_t         signature[ECDSA_SIG_SIZE];
    uint8_t         rsp_data[MAX_SSP_RAPDU_DATA_SIZE];
    uint8_t         answer_to_select[3];
    uint8_t         sspVer[2];
    uint8_t         eSEPubKey[SSP_ECC_PUBKEY_SIZE];
    ecc_key_t       eccKey;
    apdu_header_t   hcmd;
    uint32_t        rsp_size;
    uint32_t        tmp_buf_len;

    uint8_t         put_cert_data[SSP_MAX_X509_CERT_SIZE + SSP_MAX_X509_CERT_SIZE + 32];
    uint32_t        put_cert_data_len;
    uint32_t        put_cert_block_cnt;
    uint8_t         i;

    LOGD("ssp_service_doProvisioning is called");

    if (drk_cert == NULL) {
        LOGE("tee_root_cert is NULL");
        return SSPSTATUS_INVALID_ARGUMENT;
    }

    if (service_cert == NULL) {
        LOGE("tee_service_cert is NULL");
        return SSPSTATUS_INVALID_ARGUMENT;
    }

    /* Generate ECC key */
    CHECK_OP_STATUS( crypto_gen_ecc_key( &eccKey ) );
    CHECK_OP_STATUS( crypto_get_apId( apId ) );
    CHECK_OP_STATUS( ssp_open_service_connection( &channel, answer_to_select ) );

    /* Compose REQUEST EXCHANGE command data */
    tmp_buf_len = 0;
    BUFFER_APPEND( tmp_buf, tmp_buf_len, apId, SSP_AP_ID_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, eccKey.pubkey_binary + 1, SSP_ECC_PUBKEY_SIZE - 1 );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, ssp_provisioning_alg, sizeof(ssp_provisioning_alg) - 1 );

    hex_print_tag_debug("AP id", apId, SSP_AP_ID_SIZE);
    hex_print_tag_debug("xG", eccKey.pubkey_binary + 1, SSP_ECC_PUBKEY_SIZE - 1);
    hex_print_tag_debug("Algorithm id", (uint8_t *)ssp_provisioning_alg, sizeof(ssp_provisioning_alg) - 1);
    hex_print_tag_debug("tee service_key", service_key, SSP_ECC_PRIVKEY_SIZE);

    /* If eSE cert is not exist, Send a GET CERT command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF8, 0x00 );
    rsp_size = MAX_SSP_RAPDU_DATA_SIZE;
    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, NULL, rsp_data, &rsp_size ) );

    // CASD cert verification
    sspVer[0] = answer_to_select[1];
    sspVer[1] = answer_to_select[2];
    if (rsp_size > sizeof(rsp_data)){
        LOGE( "Unexpected response size to GET CERT: %u", rsp_size );
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }
    CHECK_OP_STATUS( verify_casd_cert(rsp_data, rsp_size, eSEPubKey, sspVer) );

    /* Send a REQUEST EXCHANGE command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF4, tmp_buf_len );
    rsp_size = MAX_SSP_RAPDU_DATA_SIZE;
    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, tmp_buf, rsp_data, &rsp_size ) );

    /* TEE cert is not exist in the eSE */
    if ( SSPSTATUS_CERT_NOT_EXIST == status ) {
        // Generate PUT CERT DATA
        CHECK_OP_STATUS( generatePutCertData(drk_cert, drk_cert_size, service_cert, service_cert_size, put_cert_data, &put_cert_data_len) );
        put_cert_block_cnt = put_cert_data_len / MAX_SSP_CAPDU_DATA_SIZE + 1;

        LOGD("put_cert_block_cnt : %u", put_cert_block_cnt);
        hex_print_tag_debug("put_cert_data", put_cert_data, put_cert_data_len);

        if ( put_cert_block_cnt == 1 ) {
            /* Send a First / Last PUT CERT command */
            SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, put_cert_data_len );
            hcmd.p1 = 0x01;
            rsp_size = 0x01;
            CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data, rsp_data, &rsp_size ) );
        } else {
            /* Send a PUT CERT command */
            SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, MAX_SSP_CAPDU_DATA_SIZE );
            rsp_size = 0x00;
            CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data, rsp_data, &rsp_size ) );

            /* Send a Mid PUT CERT command */
            for ( i = 0 ; i < put_cert_block_cnt - 2 ; i++ ) {
                SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, MAX_SSP_CAPDU_DATA_SIZE );
                CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data + MAX_SSP_CAPDU_DATA_SIZE * (i + 1), rsp_data, &rsp_size ) );
            }
            /* Send a Last PUT CERT command */
            SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, put_cert_data_len -  MAX_SSP_CAPDU_DATA_SIZE * (put_cert_block_cnt - 1));
            hcmd.p1 = 0x01;
            rsp_size = 0x01;
            CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data + MAX_SSP_CAPDU_DATA_SIZE * (put_cert_block_cnt - 1), rsp_data, &rsp_size ) );
        }
        LOGI("Certificate state : 0x%x", rsp_data[0]);

        /* Resend a REQUEST EXCHANGE command*/
        SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF4, tmp_buf_len );
        rsp_size = MAX_SSP_RAPDU_DATA_SIZE;
        CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, tmp_buf, rsp_data, &rsp_size ) );
    }

    if ( rsp_size > SSP_ESE_ID_SIZE + (SSP_ECC_PUBKEY_SIZE - 1) + ECDSA_SIG_WITH_ASN1_SIZE ) {
        LOGE( "Unexpected response size to REQUEST EXCHANGE: %u", rsp_size );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    /* Parse REQUEST EXCHANGE response */
    memcpy( eSEId, rsp_data, SSP_ESE_ID_SIZE );
    memcpy( publicKey + 1, rsp_data + SSP_ESE_ID_SIZE, SSP_ECC_PUBKEY_SIZE - 1 );
    publicKey[0] = 0x04;
    memcpy( signature, rsp_data + SSP_ESE_ID_SIZE + SSP_ECC_PUBKEY_SIZE - 1, ECDSA_SIG_SIZE );

    hex_print_tag_debug("Request exchange rsp : signature", signature, ECDSA_SIG_SIZE);

    /* Generate OTP key */
    CHECK_OP_STATUS( crypto_gen_ecc_secret( &eccKey, publicKey, SSP_ECC_PUBKEY_SIZE ) );
    CHECK_OP_STATUS( generate_otp_key( otp_key, eccKey.secretkey, apId, eSEId ) );

    /* Verify taken Signature */
    tmp_buf_len = 0;
    BUFFER_APPEND( tmp_buf, tmp_buf_len, otp_key, SHA256_DIGEST_SIZE);
    BUFFER_APPEND( tmp_buf, tmp_buf_len, apId, SSP_AP_ID_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, eSEId, SSP_ESE_ID_SIZE );

    CHECK_OP_STATUS( verify_ecdsa_signature(tmp_buf, tmp_buf_len, signature, eSEPubKey) );

    hex_print_tag_debug("eSE id", eSEId, SSP_ESE_ID_SIZE);
    hex_print_tag_debug("yG", publicKey + 1, SSP_ECC_PUBKEY_SIZE - 1);
    hex_print_tag_debug("secretkey", eccKey.secretkey, SSP_ECC_SECRET_SIZE);
    hex_print_tag_debug("otp_key", otp_key, SHA256_DIGEST_SIZE);

    /* Generate signautre for KEY CONFIRMATION command */
    tmp_buf_len = 0;
    BUFFER_APPEND( tmp_buf, tmp_buf_len, otp_key, SHA256_DIGEST_SIZE);
    BUFFER_APPEND( tmp_buf, tmp_buf_len, eSEId, SSP_ESE_ID_SIZE );
    BUFFER_APPEND( tmp_buf, tmp_buf_len, apId, SSP_AP_ID_SIZE );
    CHECK_OP_STATUS( generate_ecdsa_signature(tmp_buf, tmp_buf_len, signature, service_key) );

    hex_print_tag_debug("Key confirmation cmd : generated signature", signature, ECDSA_SIG_SIZE);

    /* Send a KEY CONFIRMATION command */
    SET_APDU_SSP_COMMAND( hcmd, 0x84, 0xF5, ECDSA_SIG_SIZE);
    status = ssp_send_service_command( channel, hcmd, signature, NULL, NULL ) ;

    if ( SSPSTATUS_CERT_NOT_EXIST == status ) {
        // Generate PUT CERT DATA
        CHECK_OP_STATUS( generatePutCertData(drk_cert, drk_cert_size, service_cert, service_cert_size, put_cert_data, &put_cert_data_len) );
        put_cert_block_cnt = put_cert_data_len / MAX_SSP_CAPDU_DATA_SIZE + 1;

        if ( put_cert_block_cnt == 1) {
            /* Send a First / Last PUT CERT command */
            SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, put_cert_data_len );
            hcmd.p1 = 0x01;
            rsp_size = 0x01;
            CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data, rsp_data, &rsp_size ) );
        } else {
            /* Send a PUT CERT command */
            SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, MAX_SSP_CAPDU_DATA_SIZE );
            rsp_size = 0x00;
            CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data, rsp_data, &rsp_size ) );

            /* Send a Mid PUT CERT command */
            for ( i = 0 ; i < put_cert_block_cnt - 2 ; i++ ) {
                SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, MAX_SSP_CAPDU_DATA_SIZE );
                CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data + MAX_SSP_CAPDU_DATA_SIZE * (i + 1), rsp_data, &rsp_size ) );
            }
            /* Send a Last PUT CERT command */
            SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF7, put_cert_data_len -  MAX_SSP_CAPDU_DATA_SIZE * (put_cert_block_cnt - 1));
            hcmd.p1 = 0x01;
            rsp_size = 0x01;
            CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, put_cert_data + MAX_SSP_CAPDU_DATA_SIZE * (put_cert_block_cnt - 1), rsp_data, &rsp_size ) );
        }
        LOGI("Certificate state : 0x%x", rsp_data[0]);
        status = SSPSTATUS_OTP_KEY_CONFIRMATION_CORRUPTED;
        goto ret_point;
    }
    else if (SSPSTATUS_SUCCESS != status) {
        goto ret_point;
    }

    /* Wrap and return otp_key */
#if defined USE_MOBICORE || defined USE_BLOWFISH || defined(USE_TRUSTY_UNISOC)
    CHECK_OP_STATUS( ssp_wrap_secure_object( SEM_TID, otp_key, AES_256_KEY_SIZE, otp_key_blob_out, otp_key_blob_out_size) );
#else
    CHECK_OP_STATUS( crypto_get_tz_encryption_key( otp_wrap_key ) );
    CHECK_OP_STATUS( crypto_aes_256_wrap_otp( otp_key, otp_key_blob_out, otp_wrap_key ) );
    *otp_key_blob_out_size = 40;
#endif

    hex_print_tag_debug("otp_wrap_key", otp_wrap_key, AES_256_KEY_SIZE);
    hex_print_tag_debug("otp_key_blob_out", otp_key_blob_out, *otp_key_blob_out_size);

ret_point:
    crypto_clear_mem( tmp_buf, 0xFF );
    crypto_clear_mem( otp_wrap_key, AES_256_KEY_SIZE );
    crypto_clear_mem( service_key, SSP_ECC_PRIVKEY_SIZE);
    crypto_clear_mem( otp_key, AES_256_KEY_SIZE );
    crypto_clear_ecc_context( &eccKey );

    ssp_close_service_connection( channel );
    return status;
}

#if 0
SSPSTATUS ssp_service_requestResetRandom( uint8_t enc_token[SSP_RESET_CHALLENGE_SIZE] ) {
    SSPSTATUS       status;
    uint8_t         channel = INVALID_CHANNEL;
    uint8_t         rsp_data[SSP_RESET_CHALLENGE_SIZE];
    uint8_t         answer_to_select[3];
    apdu_header_t   hcmd;
    uint32_t        rsp_size;

    LOGD("ssp_service_requestResetRandom is called");
    if ( enc_token == NULL ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    CHECK_OP_STATUS( ssp_open_service_connection( &channel, answer_to_select ) );

    /* Send a REQUEST RANDOM command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF3, 0 );
    rsp_size = SSP_RESET_CHALLENGE_SIZE;

    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, NULL, rsp_data, &rsp_size ) );

    if ( rsp_size != SSP_RESET_CHALLENGE_SIZE ) {
        LOGE( "Unexpected response size to REQUEST RANDOM: %u", rsp_size );
        status = SSPSTATUS_UNEXPECTED_RESPONSE;
        goto ret_point;
    }

    /* Return the challenge value */
    memcpy( enc_token, rsp_data, SSP_RESET_CHALLENGE_SIZE );
    hex_print_tag_debug("rsp_data", enc_token, rsp_size);

ret_point:
    ssp_close_service_connection( channel );
    return status;
}


SSPSTATUS ssp_service_doReset( const uint8_t token[SSP_RESET_RANDOM_SIZE] ) {
    SSPSTATUS       status;
    uint8_t         channel = INVALID_CHANNEL;
    apdu_header_t   hcmd;
    uint8_t         answer_to_select[3];

    LOGD("ssp_service_doReset is called");
    if ( token == NULL ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    CHECK_OP_STATUS( ssp_open_service_connection( &channel, answer_to_select ) );

    /* Send a RESET OTP command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF6, SSP_RESET_RANDOM_SIZE );

    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, token, NULL, NULL ) );

ret_point:
    ssp_close_service_connection( channel );
    return status;
}
#endif

SSPSTATUS ssp_service_doResetForFactory( void ) {
    SSPSTATUS       status;
    uint8_t         channel = INVALID_CHANNEL;
    apdu_header_t   hcmd;
    uint8_t         answer_to_select[3];

    LOGD("ssp_service_doResetForFactory is called");

    CHECK_OP_STATUS( ssp_open_service_connection( &channel, answer_to_select ) );

    /* Send a RESET OTP for factory process command */
    SET_APDU_SSP_COMMAND( hcmd, 0x80, 0xF6, 0 );
    hcmd.p2 = 0x01;

    CHECK_OP_STATUS( ssp_send_service_command( channel, hcmd, NULL, NULL, NULL ) );

ret_point:
    ssp_close_service_connection( channel );
    return status;

}


SSPSTATUS ssp_getAppletState( ssp_applet_state_t *out ) {
    SSPSTATUS       status;
    uint8_t         channel = INVALID_CHANNEL;
    uint8_t         answer_to_select[3] = {0, };

    LOGD("ssp_getAppletState is called");
    if ( out == NULL ) {
        status = SSPSTATUS_INVALID_ARGUMENT;
        goto ret_point;
    }

    CHECK_OP_STATUS( ssp_open_service_connection( &channel, answer_to_select ) );

    out->state = answer_to_select[0];
    out->version_major = (answer_to_select[1] >> 4) & 0x0f;
    out->version_minor = ((answer_to_select[1] & 0x0f) << 4) | ((answer_to_select[2] >> 4) & 0x0f);
    out->version_release = answer_to_select[2] & 0x0f;

ret_point:
    ssp_close_service_connection( channel );
    return status;
}