#include "skpm_crypto_util.h"

#include "skpm.h"
#include "skpm_util.h"
#include "debug.h"

#ifdef USE_SCRYPTO

#include <openssl/x509.h>
#include <openssl/x509v3.h>
#include <openssl/rsa.h>
#include <openssl/pem.h>
#include <openssl/mem.h>
#include <stdbool.h>
#include <assert.h>

#define MAX_KEYNAME_SIZE 50
#define CSR_MAX_SIZE 3000
#define UUID_SIZE 37
#define XY_SIZE 32
#define CURVE_NAME NID_X9_62_prime256v1

typedef struct {
    uint8_t* data;
    size_t size;
} buff_t;

typedef struct {
    uint16_t tag;
    buff_t buff;
} leaf_tlv_t;

typedef struct {
    X509* sub1; //!< optional sub CA certificate
    X509* sub2; //!< optional sub CA certificate
    X509* cert; //!< device certificate

    EVP_PKEY* key; //!< device certificate private key
} leaf_data_t;

typedef enum {
    LEAF_TYPE_CERT_SUB1 = 0x110,
    LEAF_TYPE_CERT_SUB2 = 0x120,
    LEAF_TYPE_CERT_LEAF = 0x200,

    LEAF_TYPE_KEY_PRIV = 0x300,
} leaf_type_e;

#define LEAF_TYPE_MASK      0xFF0
#define LEAF_TYPE_GET(x)    (LEAF_TYPE_MASK & (x))

#define LEAF_KEY_TYPE_MASK      0xF
#define LEAF_KEY_TYPE_GET(x)    (LEAF_KEY_TYPE_MASK & (x))

static size_t leaf_data_parse( const buff_t blob, leaf_data_t* out );
static bool leaf_data_verify( const leaf_data_t leaf );
static void leaf_data_clean( leaf_data_t* data );

static bool leaf_keypair_verify( EVP_PKEY* pub, EVP_PKEY* priv );
static bool leaf_keypair_verify_ec( EVP_PKEY* pub, EVP_PKEY* priv );
static bool leaf_keypair_verify_rsa( EVP_PKEY* pub, EVP_PKEY* priv );

static bool decode_leaf_item( leaf_tlv_t tlv, leaf_data_t* out );
static bool decode_certificate( buff_t buff, X509** out );
static bool decode_leaf_key( buff_t buff, uint8_t key_type, EVP_PKEY** out );
static bool decode_leaf_key_ec( buff_t buff, EVP_PKEY* out );
static bool decode_leaf_key_rsa( buff_t buff, EVP_PKEY* out );

static size_t leaf_blob_read_tlv( const buff_t blob, leaf_tlv_t* tlv );
static uint16_t buff_get_uint16( const uint8_t* data ); //TODO: move it to utilities

int leaf_blob_verify( const uint8_t* blob, const size_t blob_size ) {
    LOGD("Verify leaf blob BEGIN");
    buff_t leaf_blob = { .data = (uint8_t*)blob, .size = blob_size };
    leaf_data_t device_cert_data = {
        .sub1 = NULL, .sub2 = NULL,
        .cert = NULL, .key = NULL
    };

    size_t parsed_size = leaf_data_parse( leaf_blob, &device_cert_data );
    if ( 0 == parsed_size ) {
        LOGE("leaf data format ERROR");
        return RET_ERR_WRONG_INPUT_FORMAT;
    }

    const bool leaf_data_ok = leaf_data_verify(device_cert_data);
    if (!leaf_data_ok) {
        LOGE("leaf data error");
    }
    leaf_data_clean( &device_cert_data );

    LOGD("Verify leaf blob END");
    return (leaf_data_ok == true) ? RET_SUCCESS : RET_ERR_VERIFY_FAIL;
}

//TODO: ask HQ documentation & description. Looks like CASD is X509 certificate
// 0x011X 0x0000 sub1 cert... (DER encoded : 0x0111 - X.509, 0x0112 - CASD) - Optional
// 0x012X 0x0000 sub2 cert... (DER encoded : 0x0121 - X.509, 0x0122 - CASD) - Optional
// 0x020X 0x0000 leaf cert... (DER encoded : 0x0201 - X.509, 0x0202 - CASD)
// 0x0300 0x0000 leaf private key...(DER encoded : 0x0301 - EC Key (RFC 5915) , RSA Key - 0x0302)
static size_t leaf_data_parse( const buff_t blob, leaf_data_t* out ) {
    if ( (NULL == out) ||
        (NULL != out->sub1) || (NULL != out->sub2) || (NULL != out->cert) || (NULL != out->key) ) {
        return 0;
    }

    size_t offset = 0;
    static const size_t leaf_elements_count_max = 4;
    for ( size_t i = 0; i < leaf_elements_count_max && offset < blob.size; ++i ) {
        buff_t data_to_parse = { .data = blob.data + offset, .size = blob.size - offset };
        leaf_tlv_t tlv;
        const size_t parsed_size = leaf_blob_read_tlv( data_to_parse, &tlv );
        if ( 0 == parsed_size ) {
            LOGE("Incorrect data");
            leaf_data_clean(out);
            return 0;
        }
        offset += parsed_size;

        if ( !decode_leaf_item(tlv, out) ) {
            LOGE("Failed to parse leaf data");
            leaf_data_clean(out);
            return 0;
        }
    }

    return offset;
}

static bool leaf_data_verify( const leaf_data_t leaf ) {
    LOGD("leaf data verify BEGIN");
    if ( (NULL == leaf.cert) || (NULL == leaf.key) ) {
        return false;
    }

    if ( NULL != leaf.sub2 ) {
        if ( NULL == leaf.sub1 ) {
            LOGD("certificate sub2 exists but sub1 missed!");
            return false;
        }

        if ( X509_V_OK != X509_check_issued(leaf.sub1, leaf.sub2) ) {
            LOGD("sub2 issued by sub1 verification failed");
            return false;
        }

        if ( X509_V_OK != X509_check_issued(leaf.sub2, leaf.cert) ) {
            LOGD("leaf issued by sub2 verification failed");
            return false;
        }
    } else if (( NULL != leaf.sub1) && (X509_V_OK != X509_check_issued(leaf.sub1, leaf.cert)) ) {
        LOGD("leaf issued by sub1 verification failed");
        return false;
    } else {
        LOGD("No sub certificates found");
        //this certificates are optional
    }

    EVP_PKEY* pub_key = X509_get_pubkey(leaf.cert);
    if ( NULL == pub_key ) {
        LOGD("unable to get pubkey from leaf cert");
        return false;
    }

    const bool keypair_ok = leaf_keypair_verify(pub_key, leaf.key);
    EVP_PKEY_free(pub_key);
    pub_key = NULL;
    if ( !keypair_ok || (1 != X509_check_private_key(leaf.cert, leaf.key)) ) {
        LOGD("leaf key verification failed");
        return false;
    }

    LOGD("leaf data verify OK");
    return true;
}

static bool leaf_keypair_verify( EVP_PKEY* pub, EVP_PKEY* priv ) {
    assert( (NULL != pub) && (NULL != priv) );
    if ( EVP_PKEY_id(pub) != EVP_PKEY_id(priv) ) {
        LOGD("TYpe of pub & priv key not match");
        return false;
    }

    switch ( EVP_PKEY_id(pub) ) {
        case EVP_PKEY_EC:   return leaf_keypair_verify_ec(pub, priv);
        case EVP_PKEY_RSA:  return leaf_keypair_verify_rsa(pub, priv);

        default:
            LOGE("Data format error");
            break;
    }
    return false;
}

static bool leaf_keypair_verify_ec( EVP_PKEY* pub, EVP_PKEY* priv ) {
    assert(EVP_PKEY_EC == EVP_PKEY_id(pub));
    if ( 1 != EVP_PKEY_assign_EC_KEY(pub, EVP_PKEY_get1_EC_KEY(priv)) ) {
        LOGD("Unable to assign EC pub/priv key");
        return false;
    }

    if ( 1 != EC_KEY_check_key(EVP_PKEY_get0_EC_KEY(pub)) ) {
        LOGD("EC keypair verification failed");
        return false;
    }
    return true;
}

static bool leaf_keypair_verify_rsa( EVP_PKEY* pub, EVP_PKEY* priv ) {
    assert(EVP_PKEY_RSA == EVP_PKEY_id(pub));
    if ( 1 != EVP_PKEY_assign_RSA(pub, EVP_PKEY_get1_RSA(priv)) ) {
        LOGD("Unable to assign RSA pub/priv key");
        return false;
    }

    if ( 1 != RSA_check_key(EVP_PKEY_get0_RSA(pub)) ) {
        LOGD("RSA keypair verification failed");
        return false;
    }
    return true;
}

static bool decode_leaf_item( leaf_tlv_t tlv, leaf_data_t* out ) {
    assert(NULL != out);

    LOGD("Parse leaf TLV tag: 0x%04X", tlv.tag);
    switch ( LEAF_TYPE_GET(tlv.tag) ) {
        case LEAF_TYPE_CERT_SUB1:
            if ( !decode_certificate(tlv.buff, &(out->sub1)) ) {
                LOGE("Incorrect data");
                return false;
            }
            break;

        case LEAF_TYPE_CERT_SUB2:
            if ( !decode_certificate(tlv.buff, &(out->sub2)) ) {
                LOGE("Incorrect data");
                return false;
            }
            break;

        case LEAF_TYPE_CERT_LEAF:
            if ( !decode_certificate(tlv.buff, &(out->cert)) ) {
                LOGE("Incorrect data");
                return false;
            }
            break;

        case LEAF_TYPE_KEY_PRIV:
            if ( !decode_leaf_key(tlv.buff, LEAF_KEY_TYPE_GET(tlv.tag), &(out->key)) ) {
                LOGE("Incorrect data");
                return false;
            }
            break;

        default:
            LOGE("Incorrect data");
            return false;
    }
    return true;
}

static bool decode_certificate( buff_t buff, X509** out ) {
    assert(NULL != out);
    if ( NULL != *out ) {
        return false;
    }

    *out = d2i_X509( NULL, (const uint8_t**)&(buff.data), buff.size );
    if ( NULL == *out ) {
        LOGE("incorrect cert");
        return false;
    }
    return true;
}

static bool decode_leaf_key( buff_t buff, uint8_t key_type, EVP_PKEY** out ) {
    assert(NULL != out);
    if ( NULL != *out ) {
        return false;
    }

    *out = EVP_PKEY_new();
    if ( NULL == out ) {
        LOGD("Unable to create object");
        return false;
    }

    bool decoded = false;
    switch( key_type ) {
        case 1: decoded = decode_leaf_key_ec(buff, *out);    break;
        case 2: decoded = decode_leaf_key_rsa(buff, *out);   break;

        default: decoded = false; break;
    }

    if ( !decoded ) {
        LOGE("Data format error");
        EVP_PKEY_free( *out );
        *out = NULL;
    }
    return decoded;
}

static bool decode_leaf_key_ec( buff_t buff, EVP_PKEY* out ) {
    assert( NULL != out );
    EC_KEY* key = d2i_ECPrivateKey(NULL, (const uint8_t**)&(buff.data), buff.size);
    if ( NULL == key ) {
        return false;
    }

    if ( 1 != EVP_PKEY_assign_EC_KEY(out, key) ) {
        EC_KEY_free(key);
        return false;
    }
    return true;
}

static bool decode_leaf_key_rsa( buff_t buff, EVP_PKEY* out ) {
    assert( NULL != out );
    RSA* key = d2i_RSAPrivateKey(NULL, (const uint8_t**)&(buff.data), buff.size);
    if ( NULL == key ) {
        return false;
    }

    if ( 1 != EVP_PKEY_assign_RSA(out, key) ) {
        RSA_free(key);
        return false;
    }
    return true;
}

static size_t leaf_blob_read_tlv( const buff_t blob, leaf_tlv_t* tlv ) {
    if ( (NULL == blob.data) || (0 == blob.size) ||
        (NULL == tlv) ) {
        return 0;
    }

    static const size_t leaf_tlv_size_min = 5; // 2 bytes tag + 2 bytes length + 1 byte data
    if ( blob.size < leaf_tlv_size_min ) {
        return 0;
    }

    tlv->tag = buff_get_uint16(blob.data);
    size_t offset = sizeof(uint16_t);

    tlv->buff.size = buff_get_uint16(blob.data + offset);
    offset += sizeof(uint16_t);

    if ( 0 == tlv->buff.size || tlv->buff.size > (blob.size - offset) ) {
        return 0;
    }
    tlv->buff.data = blob.data + offset;
    offset += tlv->buff.size;

    return offset;
}

static void leaf_data_clean( leaf_data_t* data ) {
    if ( NULL == data ) {
        return;
    }

    LOGD("leaf_data_clean");

    X509_free( data->sub1 );
    data->sub1 = NULL;
    X509_free( data->sub2 );
    data->sub2 = NULL;
    X509_free( data->cert );
    data->cert = NULL;

    EVP_PKEY_free( data->key );
    data->key = NULL;
    LOGD("leaf_data_clean Done");
}

static uint16_t buff_get_uint16( const uint8_t* data ) {
    uint16_t result = data[0];
    result <<= 8;
    result |= data[1];

    return result;
}

static EC_KEY* generate_ec_key(void);
static size_t wrap_keypair(EC_KEY* key, uint8_t wrapped_keypair[WRAPPED_MAX_LEAF_KEY_BLOB_SIZE]);
static bool ec_key_serialize( const EC_KEY* in, ecc_key_t* out );

static bool x509_request_name_configure(X509_REQ* x509_request, const uint8_t* key_name, const size_t key_name_size,
    const uint8_t uuid[UUID_SIZE]);
static bool x509_request_add_extensions(X509_REQ* x509_request);
static bool x509_request_sign_with_pubkey(X509_REQ* x509_request, EC_KEY* ec_key);
static size_t x509_request_to_blob(X509_REQ* x509_request, uint8_t csr[CSR_MAX_SIZE]);

static size_t generateCsrScrypto( uint8_t csr[CSR_MAX_SIZE], EC_KEY* csr_key,
    const uint8_t* key_name, const size_t key_name_size, const uint8_t uuid[UUID_SIZE] );
static void print_openssl_error(void);

static size_t get_uuid_length(const uint8_t uuid[UUID_SIZE]);
static unsigned char* build_cn_text(const uint8_t* key_name, const size_t key_name_size,
    const uint8_t uuid[UUID_SIZE]);
static unsigned char* build_ou_text(const uint8_t* key_name, const size_t key_name_size);

void generateCsr(p_cmd_t cmd, p_rsp_t rsp) {
    LOGD("generateCsr");
    if (rsp == NULL) {
        LOGE("rsp == NULL");
        return;
    }
    if (cmd == NULL) {
        LOGE("cmd == NULL");
        rsp->ret = RET_ERR_WRONG_INPUT_FORMAT;
        return;
    }

    const uint32_t input_size = cmd->dataLen;
    if ( input_size < (UUID_SIZE + 1 /*key name size*/ + 1 /*key_name*/) ||
        (input_size > (UUID_SIZE + 1 /*key name size*/ + MAX_KEYNAME_SIZE)) ) {
        LOGE("Invalid input length: %d", input_size);
        rsp->ret = RET_ERR_WRONG_INPUT_FORMAT;
        return;
    }

    LOGD("input_size: %u", input_size);
    if (input_size == 0) {
        LOGE("input_size == 0");
        rsp->ret = RET_ERR_WRONG_INPUT_FORMAT;
        return;
    }

    size_t offset = 0;
    size_t key_name_size = cmd->data[offset];
    if (key_name_size > MAX_KEY_NAME_SIZE) {
        LOGE("key_name_size > MAX_KEY_NAME_SIZE");
        rsp->ret = RET_ERR_WRONG_INPUT_FORMAT;
        return;
    }
    offset++;

    uint8_t key_name[MAX_KEYNAME_SIZE] = {0,};
    memcpy(key_name, cmd->data + offset, key_name_size);
    offset += key_name_size;

    uint8_t uuid[UUID_SIZE] = {0,};
    memcpy(uuid, cmd->data + offset, UUID_SIZE);

    uint8_t csr[CSR_MAX_SIZE] = {0,};
    EC_KEY* csr_key = generate_ec_key();
    if( NULL == csr_key ) {
        LOGE("Failed to generate EC key");
        rsp->ret = RET_ERR_TZ;
        return;
    }

    size_t csr_size = generateCsrScrypto(csr, csr_key, key_name, key_name_size, uuid);
    if (csr_size == 0) {
        LOGE("Failed to generate CSR");
        EC_KEY_free(csr_key);
        rsp->ret = RET_ERR_TZ;
        return;
    }

    uint8_t wrapped_keypair[WRAPPED_MAX_LEAF_KEY_BLOB_SIZE] = {0,};
    size_t wrapped_size = wrap_keypair(csr_key, wrapped_keypair);
    EC_KEY_free(csr_key);
    if (wrapped_size == 0) {
        LOGE("Failed to wrap keypair data");
        rsp->ret = RET_ERR_TZ;
        return;
    }

    const size_t response_size = 2 + csr_size + 2 + wrapped_size;
    if (response_size > MAX_DATA_SIZE) {
        LOGE("response_size > MAX_DATA_SIZE");
        rsp->ret = RET_ERR_TZ;
        return;
    }

    rsp->dataLen = response_size;
    size_t rsp_offset = 0;

    rsp->data[rsp_offset] = (csr_size >> 8) & 0xFF;
    rsp_offset++;
    rsp->data[rsp_offset] = csr_size & 0xFF;
    rsp_offset++;
    memcpy(rsp->data + rsp_offset, csr, csr_size);
    rsp_offset += csr_size;

    rsp->data[rsp_offset] = (wrapped_size >> 8) & 0xFF;
    rsp_offset++;
    rsp->data[rsp_offset] = wrapped_size & 0xFF;
    rsp_offset++;
    memcpy(rsp->data + rsp_offset, wrapped_keypair, wrapped_size);

    LOGD("CSR generation success");
    rsp->ret = RET_SUCCESS;
}


static size_t generateCsrScrypto( uint8_t csr[CSR_MAX_SIZE], EC_KEY* csr_key,
    const uint8_t* key_name, const size_t key_name_size, const uint8_t uuid[UUID_SIZE] ) {
	X509_REQ* x509_request = NULL;
    size_t csr_size = 0;

    if( (NULL == csr_key) || (NULL == key_name) || (0 == key_name_size) ) {
        LOGE("Incorrect params");
        return 0;
    }

    do {
        x509_request = X509_REQ_new();
        if (x509_request == NULL) {
            LOGE("X509_REQ_new() failed");
            break;
        }

        int result = X509_REQ_set_version(x509_request, 0);
        if (result != 1) {
            LOGE("X509_REQ_set_version() failed");
            break;
        }

        if (!x509_request_name_configure(x509_request, key_name, key_name_size, uuid)) {
            LOGE("x509_name_configure() failed");
            break;
        }

        if (!x509_request_add_extensions(x509_request)) {
            LOGE("x509_request_add_extensions() failed");
            break;
        }

        if (!x509_request_sign_with_pubkey(x509_request, csr_key)) {
            LOGE("x509_request_sign_with_pubkey() failed");
            break;
        }

        csr_size = x509_request_to_blob(x509_request, csr);
        LOGD("csr_size: %d", csr_size);
    }
    while (0);

    print_openssl_error();

	X509_REQ_free(x509_request);

    return csr_size;
}

static EC_KEY* generate_ec_key() {
    EC_KEY* ec_key = NULL;
    EC_GROUP* ec_group = NULL;

    do {
        ec_key = EC_KEY_new(); //TODO: use EC_KEY_new_by_curve_name instead
        if (ec_key == NULL) {
            LOGE("EC_KEY_new() failed");
            break;
        }

        ec_group = EC_GROUP_new_by_curve_name(CURVE_NAME);
        if (ec_group == NULL) {
            LOGE("EC_GROUP_new_by_curve_name() failed");
            break;
        }

        int result = EC_KEY_set_group(ec_key, (const EC_GROUP*) ec_group);
        if (result != 1) {
            LOGE("EC_KEY_set_group() failed");
            break;
        }

        result = EC_KEY_generate_key(ec_key);
        if (result != 1) {
            LOGE("EC_KEY_generate_key() failed");
            break;
        }
    }
    while (0);

    EC_GROUP_free(ec_group);
    return ec_key;
}


static bool ec_key_serialize( const EC_KEY* in, ecc_key_t* out ) {
    const size_t pubkey_size = EC_POINT_point2oct( EC_KEY_get0_group(in), EC_KEY_get0_public_key(in),
        POINT_CONVERSION_UNCOMPRESSED, out->pubkey_binary, ECC_PUBKEY_SIZE, NULL );
    LOGD( "Public key size: %d", (int)pubkey_size );
    if( (pubkey_size <= 0) || (pubkey_size > ECC_PUBKEY_SIZE) ) {
        LOGE("Failed to serialize public key");
        return false;
    }

    const size_t privkey_size = BN_bn2bin( EC_KEY_get0_private_key(in), out->privkey_binary );
    LOGD( "Private key size: %d", (int)privkey_size );
    if ((privkey_size <= 0) || (privkey_size > ECC_PRIVKEY_SIZE)) {
        LOGE("Failed to serialize private key");
        return false;
    }

    out->pubkey_size = pubkey_size;
    out->privkey_size = privkey_size;
    return true;
}

static size_t wrap_keypair(EC_KEY* key, uint8_t wrapped_keypair[WRAPPED_MAX_LEAF_KEY_BLOB_SIZE]) {
    ecc_key_t keypair;
    //dangerous serialization however there is 3 places now (JAN 21 2021) with strict size check: "sizeof(ecc_key_t)"
    if ( !ec_key_serialize(key, &keypair) ) {
        LOGE("Failed to serialize EC key");
        return 0;
    }

    uint32_t wrapped_size = WRAPPED_MAX_LEAF_KEY_BLOB_SIZE;
    int result = wrapInternalData(SECURE_OBJECT_TYPE_ECC_KEY_INFO, (uint8_t*)(&keypair), sizeof(ecc_key_t),
        wrapped_keypair, &wrapped_size);
    if (result != STATUS_SUCCESS) {
        LOGE("wrapInternalData() failed: %d", result);
        return 0;
    }

    return wrapped_size;
}

static bool x509_request_name_configure(X509_REQ* x509_request, const uint8_t* key_name, const size_t key_name_size,
    const uint8_t uuid[UUID_SIZE]) {
    X509_NAME* x509_name = NULL;
    unsigned char* common_name = NULL;
    unsigned char* organizational_unit = NULL;
    bool configuration_successful = false;

    do {
        x509_name = X509_REQ_get_subject_name(x509_request);
        if (x509_name == NULL) {
            LOGE("X509_REQ_get_subject_name() failed");
            break;
        }

        common_name = build_cn_text(key_name, key_name_size, uuid);
        if (common_name == NULL) {
            LOGE("Failed to build CN field");
            break;
        }

        // https://www.openssl.org/docs/man1.1.0/man3/X509_NAME_add_entry_by_txt.html
        int len = -1; // If len is -1 then the field length is calculated internally using strlen(bytes).
        /*
        The loc and set parameters determine where a new entry should be added. For almost all applications loc can be set to -1 and set to 0. This adds a new entry to the end of name as a single valued RelativeDistinguishedName (RDN).
        loc actually determines the index where the new entry is inserted: if it is -1 it is appended.
        set determines how the new type is added. If it is zero a new RDN is created.
        */
        int loc = -1;
        int set = 0;

        int result = X509_NAME_add_entry_by_txt(x509_name, "CN", MBSTRING_ASC, common_name, len, loc, set);
        if (result != 1) {
            LOGE("X509_NAME_add_entry_by_txt() [CN] failed");
            break;
        }

        organizational_unit = build_ou_text(key_name, key_name_size);
        if (organizational_unit == NULL) {
            LOGE("Failed to build OU field");
            break;
        }
        result = X509_NAME_add_entry_by_txt(x509_name, "OU", MBSTRING_ASC, organizational_unit, len, loc, set);
        if (result != 1) {
            LOGE("X509_NAME_add_entry_by_txt() [OU] failed");
            break;
        }

        const unsigned char o_text[] = "Samsung Electronics";
        result = X509_NAME_add_entry_by_txt(x509_name, "O", MBSTRING_ASC, o_text, len, loc, set);
        if (result != 1) {
            LOGE("X509_NAME_add_entry_by_txt() [O] failed");
            break;
        }

        const unsigned char c_text[] = "KR";
        result = X509_NAME_add_entry_by_txt(x509_name, "C", MBSTRING_ASC, c_text, len, loc, set);
        if (result != 1) {
            LOGE("X509_NAME_add_entry_by_txt() [C] failed");
            break;
        }
        configuration_successful = true;
    }
    while (0);

    OPENSSL_free(organizational_unit);
    OPENSSL_free(common_name);

    return configuration_successful;
}

static bool x509_request_add_extensions(X509_REQ* x509_request) {
    STACK_OF(X509_EXTENSION)* extensions_stack = NULL;
    X509_EXTENSION* extension = NULL;
    bool addition_successful = false;

    do {
        extensions_stack = sk_X509_EXTENSION_new_null();
        if (extensions_stack == NULL) {
            LOGE("sk_X509_EXTENSION_new_null() failed");
            break;
        }

        extension = X509V3_EXT_nconf_nid(NULL, NULL, NID_key_usage, "Digital Signature, Non Repudiation, Key Agreement");
        if (extension == NULL) {
            LOGE("X509V3_EXT_nconf_nid() failed");
            break;
        }

        sk_X509_EXTENSION_push(extensions_stack, extension);

        int result = X509_REQ_add_extensions(x509_request, extensions_stack);
        if (result != 1) {
            LOGE("X509_REQ_add_extensions() failed");
            break;
        }
        addition_successful = true;
    }
    while (0);

    sk_X509_EXTENSION_pop_free(extensions_stack, X509_EXTENSION_free);
    return addition_successful;
}

static bool x509_request_sign_with_pubkey(X509_REQ* x509_request, EC_KEY* ec_key) {
    EVP_PKEY* key = NULL; // key will be freed with x509_request object
    bool sign_successful = false;

    do {
        key = EVP_PKEY_new();
        if (key == NULL) {
            LOGE("EVP_PKEY_new() failed");
            break;
        }

        int result = EVP_PKEY_assign_EC_KEY(key, ec_key);
        if (result != 1) {
            LOGE("EVP_PKEY_assign_EC_KEY() failed");
            break;
        }

        result = X509_REQ_set_pubkey(x509_request, key);
        if (result != 1) {
            LOGE("X509_REQ_set_pubkey() failed");
            break;
        }

        result = X509_REQ_sign(x509_request, key, EVP_sha256()); // return x509_req->signature->length
        if (result <= 0) {
            LOGE("X509_REQ_sign() failed");
            break;
        }

        sign_successful = true;
    } while (0);

    return sign_successful;
}

static size_t x509_request_to_blob(X509_REQ* x509_request, uint8_t csr[CSR_MAX_SIZE]) {
	BIO* out = NULL;
    size_t csr_size = 0;

    do {
        out = BIO_new(BIO_s_mem());
        if (out == NULL) {
            LOGE("BIO_new() failed");
            break;
        }

        int result = i2d_X509_REQ_bio(out, x509_request);
        if (result != 1) {
            LOGE("i2d_X509_REQ_bio() failed");
            break;
        }

        result = BIO_read(out, csr, out->num_write);
        if (result <= 0) {
            LOGE("BIO_read() failed");
            break;
        }

        csr_size = out->num_write;
    }
    while (0);

    BIO_free(out);
    return csr_size;
}

static void print_openssl_error() {
    BIO *bio = BIO_new(BIO_s_mem());
    if (bio == NULL) {
        return;
    }
#ifndef USE_MOBICORE
    ERR_print_errors(bio);
#endif
    char *buffer = NULL;
    size_t len = BIO_get_mem_data(bio, &buffer);
    if (len != 0) {
        LOGE("OpenSSL error: %s", buffer);
    }
    BIO_free(bio);
}

static unsigned char* build_cn_text(const uint8_t* key_name, const size_t key_name_size,
    const uint8_t uuid[UUID_SIZE]) {
    const char* opening_bracket = " (";
    const char* closing_bracket = ") ";
    const size_t brackets_length = 2;
    const size_t uuid_length = get_uuid_length(uuid);
    const size_t cn_size = key_name_size + brackets_length + uuid_length + brackets_length;
    char* common_name = OPENSSL_malloc(cn_size * sizeof(char));
    if (common_name == NULL) {
        LOGE("Failed to allocate memory for common name");
        return NULL;
    }

    size_t offset = 0;

    memcpy(common_name + offset, (unsigned char*) key_name, key_name_size);
    offset += key_name_size;

    memcpy(common_name + offset, opening_bracket, brackets_length);
    offset += brackets_length;

    memcpy(common_name + offset, (unsigned char*) uuid, uuid_length);
    offset += uuid_length;

    memcpy(common_name + offset, closing_bracket, brackets_length);
    offset += brackets_length;

    common_name[offset - 1] = '\0';

    return (unsigned char*) common_name;
}

static size_t get_uuid_length(const uint8_t uuid[UUID_SIZE]){
    for (size_t i = 0; i < UUID_SIZE; i++) {
        if (uuid[i] == '\0') {
            return i;
        }
    }
    return UUID_SIZE;
}

static unsigned char* build_ou_text(const uint8_t* key_name, const size_t key_name_size) {
    const unsigned char suffix[] = " MC Device";
    const size_t suffix_size = sizeof(suffix) / sizeof(unsigned char);
    unsigned char* organizational_unit = OPENSSL_malloc(key_name_size + suffix_size);
    if (organizational_unit == NULL) {
        LOGE("Failed to allocate memory for organizational unit");
        return NULL;
    }

    size_t offset = 0;

    memcpy(organizational_unit + offset, (unsigned char*) key_name, key_name_size);
    offset += key_name_size;

    memcpy(organizational_unit + offset, suffix, suffix_size);
    offset += suffix_size;

    organizational_unit[offset - 1] = '\0';

    return organizational_unit;
}

#else

void generateCsr(p_cmd_t cmd, p_rsp_t rsp) {
    LOGD("generateCsr");
    if (rsp == NULL) {
        LOGE("rsp == NULL");
        return;
    }

    rsp->ret = RET_ERR_NOT_SUPPORT;
}

#endif /* #ifdef USE_SCRYPTO */

STATUS extractRawSigFromAsn1WithEcc(uint8_t *sig, uint32_t sig_len, uint8_t *raw_sig_r, uint32_t *raw_sig_r_size, uint8_t *raw_sig_s, uint32_t *raw_sig_s_size) {
    STATUS status = STATUS_SUCCESS;

    uint16_t current_pos = 0;
	uint8_t sig_r[33], sig_s[33];
    uint32_t sig_r_len, sig_s_len;

    if (sig_len > ECDSA_SIG_WITH_ASN1_SIZE) {
       return STATUS_FAILED; 
    }
    
    current_pos++; // SIG tag
    current_pos++; // Length
    
    current_pos++; // SIG_R Tag
    sig_r_len = sig[current_pos++]; // Length
    if (sizeof(sig_r) < sig_r_len) {
       return STATUS_FAILED; 
    }
    memcpy(sig_r, sig + current_pos, sig_r_len); // SIG R copy
    current_pos += sig_r_len;

    current_pos++; // SIG_S Tag
    sig_s_len = sig[current_pos++]; // Length
    if (sizeof(sig_s) < sig_s_len) {
       return STATUS_FAILED; 
    }
    memcpy(sig_s, sig + current_pos, sig_s_len); // SIG S copy
    current_pos += sig_s_len;

    hex_print_tag_debug("Extracted sig r ", sig_r, sig_r_len);
    hex_print_tag_debug("Extracted sig s", sig_s, sig_s_len);

    // Remove 0x00 padding
    if (sig_r[0] == 0x00) {
        if (*raw_sig_r_size < sig_r_len - 1) {
            LOGE("raw_sig_r buffer is not enough");
            return STATUS_FAILED; 
        }
        *raw_sig_r_size = sig_r_len - 1;
        memcpy(raw_sig_r, sig_r + 1, *raw_sig_r_size);
    } else {
        if (*raw_sig_r_size < sig_r_len) {
            LOGE("raw_sig_r buffer is not enough");
            return STATUS_FAILED; 
        }
        *raw_sig_r_size = sig_r_len;
        memcpy(raw_sig_r, sig_r, *raw_sig_r_size);
    }

    if (sig_s[0] == 0x00) {
        if (*raw_sig_s_size < sig_s_len - 1) {
            LOGE("raw_sig_s buffer is not enough");
            return STATUS_FAILED; 
        }
        *raw_sig_s_size = sig_s_len - 1;
        memcpy(raw_sig_s, sig_s + 1, *raw_sig_s_size);
    } else {
        if (*raw_sig_s_size < sig_s_len) {
            LOGE("raw_sig_s buffer is not enough");
            return STATUS_FAILED; 
        }
        *raw_sig_s_size = sig_s_len;
        memcpy(raw_sig_s, sig_s, *raw_sig_s_size);
    }

	return status;
}

STATUS rebuildAsn1FromRawSigWithEcc(uint8_t *raw_sig_r, uint32_t raw_sig_r_size, uint8_t *raw_sig_s, uint32_t raw_sig_s_size, uint8_t *sig, uint32_t *sig_len) {
    STATUS status = STATUS_SUCCESS;
  	uint8_t sig_r[33];
    uint8_t sig_r_len;
	uint8_t sig_s[33];
    uint8_t sig_s_len;

	if ( raw_sig_r[0] > 127 ) {
        sig_r[0] = 0x00;
        if (sizeof(sig_r) - 1 < raw_sig_r_size) {
            return STATUS_FAILED; 
        }
        memcpy(sig_r + 1, raw_sig_r, raw_sig_r_size);
        sig_r_len = 1 + raw_sig_r_size;
	} else {
        if (sizeof(sig_r) < raw_sig_r_size) {
            return STATUS_FAILED; 
        }
	    memcpy(sig_r, raw_sig_r, raw_sig_r_size);
        sig_r_len = raw_sig_r_size;
    }

	if ( raw_sig_s[0] > 127 ) {
        sig_s[0] = 0x00;
        if (sizeof(sig_s) - 1 < raw_sig_s_size) {
            return STATUS_FAILED; 
        }
        memcpy(sig_s + 1, raw_sig_s, raw_sig_s_size);
        sig_s_len = 1 + raw_sig_s_size;
    } else {
        if (sizeof(sig_s) < raw_sig_s_size) {
            return STATUS_FAILED; 
        }
        memcpy(sig_s, raw_sig_s, raw_sig_s_size);
        sig_s_len = raw_sig_s_size;
    }

    if (*sig_len < (uint32_t)(2 + 2 + sig_r_len + 2 + sig_s_len)) {
        return STATUS_FAILED; 
    }

    *sig_len = 0;
    sig[(*sig_len)++] = 0x30;
    sig[(*sig_len)++] = sig_r_len + sig_s_len + 4;
    sig[(*sig_len)++] = 0x02;
    sig[(*sig_len)++] = sig_r_len;
    memcpy(sig + (*sig_len), sig_r, sig_r_len);
    (*sig_len) += sig_r_len;
    sig[(*sig_len)++] = 0x02;
    sig[(*sig_len)++] = sig_s_len;
    memcpy(sig + (*sig_len), sig_s, sig_s_len);
    (*sig_len) += sig_s_len;

    return status;
}

void encodeAsn1EccPrivateKey(ecc_key_t key_info, uint8_t *key_block, uint16_t *key_blockSize) {
    uint32_t offset = 0;
    uint8_t prefix[] = {
        0x30, 0x77,                                     // SEQUENCE (119 bytes)
        0x02, 0x01,                                     // INTEGER (1 bytes) 
        0x01,                                           // Version 1 
        0x04, 0x20};                                    // OCTET STRING (32 bytes)

    uint8_t mid[] = {
        0xa0, 0x0a,                                     // EXPLICIT (10 bytes)
        0x06, 0x08,                                     // OID (8 bytes) 
        0x2a, 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07, // secp256r1 curve  
        0xa1, 0x44,                                     // EXPLICIT (68 bytes) 
        0x03, 0x42,                                     // BIT STRING (66 bytes)
        0x00};                                          // Bit Padding

    memcpy(key_block + offset, prefix, sizeof(prefix));
    offset += sizeof(prefix);
    if (key_info.privkey_size == 31) {
        key_block[offset++] = 0x00;
    }
    memcpy(key_block + offset, key_info.privkey_binary, key_info.privkey_size);
    offset += key_info.privkey_size;
    memcpy(key_block + offset, mid, sizeof(mid));
    offset += sizeof(mid);
    memcpy(key_block + offset, key_info.pubkey_binary, key_info.pubkey_size);
    offset += key_info.pubkey_size;
    *key_blockSize = offset;
}

static int32_t makeTlvLen(uint32_t length, uint8_t tlvLen[3]) {
    // DER encoding rule

    if (length > 255) {
        // 0x82 0100 - 0x82 FFFF
        tlvLen[0] = 0x82;
        tlvLen[1] = (0x00FF & (length >> 8));
        tlvLen[2] = (0x00FF & (length));
        return 3;
    } else if (length > 127) {
        // 0x81 80 - 0x81 FF
        tlvLen[0] = 0x81;
        tlvLen[1] = (0x00FF & (length));
        return 2;
    } else {
        // 0x00 - 0x7F
        tlvLen[0] = (0x00FF & (length));
        return 1;
    }
   
}

void encodeAsn1RsaPrivateKey(rsa_key_t key_info, uint8_t *key_block, uint16_t *key_blockSize) {
    uint32_t offset = 0;

    uint8_t modulus[300];
    uint32_t modulus_size;
    uint8_t publicExponent[5];
    uint32_t publicExponent_size;
    uint8_t privateExponent[300];
    uint32_t privateExponent_size;
    uint8_t prime1[150];
    uint32_t prime1_size;
    uint8_t prime2[150];
    uint32_t prime2_size;
    uint8_t exponent1[150];
    uint32_t exponent1_size;
    uint8_t exponent2[150];
    uint32_t exponent2_size;
    uint8_t coefficient[150];
    uint32_t coefficient_size;

    uint16_t temp_size;

    uint8_t temp_tlvLen[3];
    uint32_t temp_tlvLenSize;
    
    modulus_size = 0;
    modulus[modulus_size++] = 0x02;
    if (key_info.modulus[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.modulus_size + 1, temp_tlvLen);
        memcpy(modulus + modulus_size, temp_tlvLen, temp_tlvLenSize);
        modulus_size += temp_tlvLenSize;
        modulus[modulus_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.modulus_size, temp_tlvLen);
        memcpy(modulus + modulus_size, temp_tlvLen, temp_tlvLenSize);
        modulus_size += temp_tlvLenSize;
    }
    memcpy(modulus + modulus_size, key_info.modulus, key_info.modulus_size);
    modulus_size += key_info.modulus_size;
    hex_print_tag_debug("modulus_with_TLV", modulus, modulus_size);

    publicExponent_size = 0;
    publicExponent[publicExponent_size++] = 0x02;
    publicExponent[publicExponent_size++] = 0x03;
    publicExponent[publicExponent_size++] = 0x01;
    publicExponent[publicExponent_size++] = 0x00;
    publicExponent[publicExponent_size++] = 0x01;    
    hex_print_tag_debug("publicExponent_with_TLV", publicExponent, publicExponent_size);

    privateExponent_size = 0;
    privateExponent[privateExponent_size++] = 0x02;
    if (key_info.privateExponent[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.privateExponent_size + 1, temp_tlvLen);
        memcpy(privateExponent + privateExponent_size, temp_tlvLen, temp_tlvLenSize);
        privateExponent_size += temp_tlvLenSize;
        privateExponent[privateExponent_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.privateExponent_size, temp_tlvLen);
        memcpy(privateExponent + privateExponent_size, temp_tlvLen, temp_tlvLenSize);
        privateExponent_size += temp_tlvLenSize;
    }
    memcpy(privateExponent + privateExponent_size, key_info.privateExponent, key_info.privateExponent_size);
    privateExponent_size += key_info.privateExponent_size;
    hex_print_tag_debug("privateExponent_with_TLV", privateExponent, privateExponent_size);

    prime1_size = 0;
    prime1[prime1_size++] = 0x02;
    if (key_info.prime1[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.prime1_size + 1, temp_tlvLen);
        memcpy(prime1 + prime1_size, temp_tlvLen, temp_tlvLenSize);
        prime1_size += temp_tlvLenSize;
        prime1[prime1_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.prime1_size, temp_tlvLen);
        memcpy(prime1 + prime1_size, temp_tlvLen, temp_tlvLenSize);
        prime1_size += temp_tlvLenSize;
    }
    memcpy(prime1 + prime1_size, key_info.prime1, key_info.prime1_size);
    prime1_size += key_info.prime1_size;
    hex_print_tag_debug("prime1_with_TLV", prime1, prime1_size);

    prime2_size = 0;
    prime2[prime2_size++] = 0x02;
    if (key_info.prime2[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.prime2_size + 1, temp_tlvLen);
        memcpy(prime2 + prime2_size, temp_tlvLen, temp_tlvLenSize);
        prime2_size += temp_tlvLenSize;
        prime2[prime2_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.prime2_size, temp_tlvLen);
        memcpy(prime2 + prime2_size, temp_tlvLen, temp_tlvLenSize);
        prime2_size += temp_tlvLenSize;
    }
    memcpy(prime2 + prime2_size, key_info.prime2, key_info.prime2_size);
    prime2_size += key_info.prime2_size;
    hex_print_tag_debug("prime2_with_TLV", prime2, prime2_size);

    exponent1_size = 0;
    exponent1[exponent1_size++] = 0x02;
    if (key_info.exponent1[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.exponent1_size + 1, temp_tlvLen);
        memcpy(exponent1 + exponent1_size, temp_tlvLen, temp_tlvLenSize);
        exponent1_size += temp_tlvLenSize;
        exponent1[exponent1_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.exponent1_size, temp_tlvLen);
        memcpy(exponent1 + exponent1_size, temp_tlvLen, temp_tlvLenSize);
        exponent1_size += temp_tlvLenSize;
    }
    memcpy(exponent1 + exponent1_size, key_info.exponent1, key_info.exponent1_size);
    exponent1_size += key_info.exponent1_size;
    hex_print_tag_debug("exponent1_with_TLV", exponent1, exponent1_size);

    exponent2_size = 0;
    exponent2[exponent2_size++] = 0x02;
    if (key_info.exponent2[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.exponent2_size + 1, temp_tlvLen);
        memcpy(exponent2 + exponent2_size, temp_tlvLen, temp_tlvLenSize);
        exponent2_size += temp_tlvLenSize;
        exponent2[exponent2_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.exponent2_size, temp_tlvLen);
        memcpy(exponent2 + exponent2_size, temp_tlvLen, temp_tlvLenSize);
        exponent2_size += temp_tlvLenSize;
    }
    memcpy(exponent2 + exponent2_size, key_info.exponent2, key_info.exponent2_size);
    exponent2_size += key_info.exponent2_size;
    hex_print_tag_debug("exponent2_with_TLV", exponent2, exponent2_size);

    coefficient_size = 0;
    coefficient[coefficient_size++] = 0x02;
    if (key_info.coefficient[0] > 127) {
        temp_tlvLenSize = makeTlvLen(key_info.coefficient_size + 1, temp_tlvLen);
        memcpy(coefficient + coefficient_size, temp_tlvLen, temp_tlvLenSize);
        coefficient_size += temp_tlvLenSize;
        coefficient[coefficient_size++] = 0x00;
    } else {
        temp_tlvLenSize = makeTlvLen(key_info.coefficient_size, temp_tlvLen);
        memcpy(coefficient + coefficient_size, temp_tlvLen, temp_tlvLenSize);
        coefficient_size += temp_tlvLenSize;
    }
    memcpy(coefficient + coefficient_size, key_info.coefficient, key_info.coefficient_size);
    coefficient_size += key_info.coefficient_size;
    hex_print_tag_debug("coefficient_with_TLV", coefficient, coefficient_size);

    temp_size = modulus_size + publicExponent_size + privateExponent_size + prime1_size + prime2_size
        + exponent1_size + exponent2_size + coefficient_size;

    temp_size += 3; // 0x020100

    offset = 0;
    key_block[offset++] = 0x30;
    key_block[offset++] = 0x82;
    key_block[offset++] = (0x00FF & (temp_size >> 8));
    key_block[offset++] = (0x00FF & temp_size);
    key_block[offset++] = 0x02;
    key_block[offset++] = 0x01;
    key_block[offset++] = 0x00;

    memcpy(key_block + offset, modulus, modulus_size);
    offset += modulus_size;
    memcpy(key_block + offset, publicExponent, publicExponent_size);
    offset += publicExponent_size;
    memcpy(key_block + offset, privateExponent, privateExponent_size);
    offset += privateExponent_size;
    memcpy(key_block + offset, prime1, prime1_size);
    offset += prime1_size;
    memcpy(key_block + offset, prime2, prime2_size);
    offset += prime2_size;
    memcpy(key_block + offset, exponent1, exponent1_size);
    offset += exponent1_size;
    memcpy(key_block + offset, exponent2, exponent2_size);
    offset += exponent2_size;
    memcpy(key_block + offset, coefficient, coefficient_size);
    offset += coefficient_size;
    *key_blockSize = offset;
}