#include "crypto_module.h"
#include "crypto_core_init.h"
#include "ssp.h"
#include "ssp_util.h"

#include "tz_debug.h"

#include <stdarg.h>

#ifdef USE_BLOWFISH
#ifndef NULL
#define NULL '\0'
#endif
#endif

#define AES_WRAP_BLOCK_SIZE     8

#if ENCRYPTED_OTP_KEY_SIZE != AES_256_KEY_SIZE + AES_WRAP_BLOCK_SIZE
#   error Wrong value of encrypted OTP key size
#endif


#ifndef USE_SCRYPTO
static uint8_t g_isCryptoCoreInitialized = 0;
#endif

void initCryptoCore(void) {
#ifndef USE_SCRYPTO
    /* Initialize crypto core library if it is not initialized */
    if ( g_isCryptoCoreInitialized == 0 ) {
        init_crypto_core_lib();
        g_isCryptoCoreInitialized = 1;
    }
#endif
}

CRYPTO_STATUS crypto_gen_ecc_key( ecc_key_t *ecc_key ) {
    EC_KEY  *key;
    BN_CTX  *bn_ctx;
    BIGNUM  *transform_bignum;
    int32_t  bn_bin_size;
    uint8_t bn_bin[ECC_PUBKEY_SIZE];

    // Init crypto core library
    initCryptoCore();

    if (ecc_key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    key = EC_KEY_new_by_curve_name( NID_X9_62_prime256v1 );
    if ( key == NULL) {
        goto new_key_fault;
    }

    if ( !EC_KEY_generate_key( key ) ) {
        goto gen_key_fault;
    }

    bn_ctx = BN_CTX_new();
    if ( bn_ctx == NULL ) {
        goto gen_key_fault;
    }

    BN_CTX_start( bn_ctx );
    transform_bignum = BN_CTX_get( bn_ctx );

    if ( EC_POINT_point2bn( EC_KEY_get0_group(key),
                           EC_KEY_get0_public_key(key),
                           POINT_CONVERSION_UNCOMPRESSED,
                           transform_bignum,
                           bn_ctx) != transform_bignum ) {
        goto transform_fault;
    }

    bn_bin_size = BN_bn2bin( transform_bignum, bn_bin );

    ecc_key->ecc_keypair = (void*)key;
    memcpy(ecc_key->pubkey_binary, bn_bin, bn_bin_size);
    ecc_key->pubkey_size = bn_bin_size;

    BN_CTX_end( bn_ctx );
    BN_CTX_free( bn_ctx );

    return CRYPTO_STATUS_SUCCESS;

transform_fault:
    BN_free(transform_bignum);
    BN_CTX_end(bn_ctx);
    BN_CTX_free(bn_ctx);

gen_key_fault:
    EC_KEY_free(key);

new_key_fault:

    return CRYPTO_STATUS_FAILED;
}


CRYPTO_STATUS crypto_gen_ecc_secret( ecc_key_t *ecc_key, const uint8_t *peer_key, uint32_t peer_key_size ) {
    EC_KEY   *key;
    EC_POINT *peer_public;
    BN_CTX   *bn_ctx;
    BIGNUM   *transform_bignum;
    uint32_t  field_size;
    uint32_t  secret_len;

    // Init crypto core library
    initCryptoCore();

    if (ecc_key == NULL || peer_key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    key = (EC_KEY*)ecc_key->ecc_keypair;
    field_size = EC_GROUP_get_degree( EC_KEY_get0_group(key) );
    secret_len = (field_size + 7) / 8;

    if ( secret_len != ECC_SECRET_SIZE ) {
        goto secret_len_fault;
    }

    bn_ctx = BN_CTX_new();
    if ( bn_ctx == NULL ) {
        goto secret_len_fault;
    }

    BN_CTX_start( bn_ctx );
    transform_bignum = BN_CTX_get( bn_ctx );

    peer_public = EC_POINT_new( EC_KEY_get0_group(key) );
    if ( peer_public == NULL ) {
        goto point_new_fault;
    }

    if ( BN_bin2bn( peer_key, peer_key_size, transform_bignum ) != transform_bignum ) {
        goto point_translate_fault;
    }

    if ( EC_POINT_bn2point(EC_KEY_get0_group(key), transform_bignum, peer_public, bn_ctx ) != peer_public ) {
        goto point_translate_fault;
    }

    secret_len = ECDH_compute_key( ecc_key->secretkey, secret_len, peer_public, key, NULL );
    if ( secret_len != ECC_SECRET_SIZE ) {
        goto point_translate_fault;
    }

    EC_POINT_free( peer_public );
    BN_CTX_end( bn_ctx );
    BN_CTX_free( bn_ctx );

    return CRYPTO_STATUS_SUCCESS;

point_translate_fault:
    EC_POINT_free(peer_public);
    BN_free(transform_bignum);

point_new_fault:
    BN_CTX_end(bn_ctx);
    BN_CTX_free(bn_ctx);

secret_len_fault:
    return CRYPTO_STATUS_FAILED;
}


void crypto_clear_ecc_context( ecc_key_t *ecc_key ) {
    EC_KEY *key;

    // Init crypto core library
    initCryptoCore();

    if (ecc_key == NULL) {
        LOGE("Input parameter is NULL");
        return ;
    }

    key = (EC_KEY*)ecc_key->ecc_keypair;
    if ( key != NULL ) {
        EC_KEY_free( key );
    }
}


CRYPTO_STATUS crypto_regen_ecc_pubkey(ecc_key_t *ecc_key, uint8_t *pub_key, uint32_t pub_key_size) {
    EC_KEY  *key;
    BN_CTX *bn_ctx = NULL;
    EC_POINT *peer_public;
    BIGNUM *transform_bignum = NULL;

    // Init crypto core library
    initCryptoCore();

    if (ecc_key == NULL || pub_key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    key = EC_KEY_new_by_curve_name( NID_X9_62_prime256v1 );
    if ( key == NULL) {
        return CRYPTO_STATUS_FAILED;
    }

    bn_ctx = BN_CTX_new();
    if ( bn_ctx == NULL ) {
        goto regen_key_fault;
    }

    BN_CTX_start( bn_ctx );
    transform_bignum = BN_CTX_get( bn_ctx );

    peer_public = EC_POINT_new( EC_KEY_get0_group(key) );
    if ( peer_public == NULL ) {
        goto point_new_fault;
    }

    if ( BN_bin2bn( pub_key, pub_key_size, transform_bignum ) != transform_bignum ) {
        goto point_translate_fault;
    }

    if ( EC_POINT_bn2point( EC_KEY_get0_group(key), transform_bignum, peer_public, bn_ctx ) != peer_public ) {
        goto point_translate_fault;
    }

    EC_KEY_set_public_key(key, peer_public);

    ecc_key->ecc_keypair = (void*)key;
    ecc_key->pubkey_size = 0;

    EC_POINT_free(peer_public);
    BN_CTX_end( bn_ctx );
    BN_CTX_free( bn_ctx );

    return CRYPTO_STATUS_SUCCESS;

point_translate_fault:
    EC_POINT_free(peer_public);
    BN_free(transform_bignum);

point_new_fault:
    BN_CTX_end(bn_ctx);
    BN_CTX_free(bn_ctx);

regen_key_fault:
    EC_KEY_free(key);

    return CRYPTO_STATUS_FAILED;
}


CRYPTO_STATUS crypto_regen_ecc_privkey(ecc_key_t *ecc_key, uint8_t *priv_key, uint32_t priv_key_size) {
    EC_KEY  *key;
    BN_CTX *bn_ctx = NULL;
    EC_POINT *peer_public;
    const EC_GROUP *group = NULL;
    BIGNUM *transform_bignum = NULL;

    // Init crypto core library
    initCryptoCore();

    if (ecc_key == NULL || priv_key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    key = EC_KEY_new_by_curve_name( NID_X9_62_prime256v1 );
    if ( key == NULL) {
        return CRYPTO_STATUS_FAILED;
    }

    bn_ctx = BN_CTX_new();
    if ( bn_ctx == NULL ) {
        goto regen_key_fault;
    }

    BN_CTX_start( bn_ctx );
    transform_bignum = BN_CTX_get( bn_ctx );

    if ( BN_bin2bn( priv_key, priv_key_size, transform_bignum ) != transform_bignum ) {
        goto private_translate_fault;
    }

    group = EC_KEY_get0_group(key);
    peer_public = EC_POINT_new(group);
    if (peer_public == NULL) {
        goto point_translate_fault;
    }

    if (!EC_POINT_mul(group, peer_public, transform_bignum, NULL, NULL, bn_ctx)) {
        goto point_translate_fault;
    }

    EC_KEY_set_private_key(key, transform_bignum);
    EC_KEY_set_public_key(key, peer_public);

    ecc_key->ecc_keypair = (void*)key;
    ecc_key->pubkey_size = 0;

    EC_POINT_free(peer_public);
    BN_CTX_end( bn_ctx );
    BN_CTX_free( bn_ctx );

    return CRYPTO_STATUS_SUCCESS;

point_translate_fault:
    EC_POINT_free(peer_public);

private_translate_fault:
    BN_free(transform_bignum);
    BN_CTX_end(bn_ctx);
    BN_CTX_free(bn_ctx);

regen_key_fault:
    EC_KEY_free(key);

    return CRYPTO_STATUS_FAILED;
}


CRYPTO_STATUS crypto_ecdsa_sign_with_sha256(void *eckey, uint8_t *in, uint32_t inLen,
    uint8_t *sig_r, int32_t *sig_r_Len, uint8_t *sig_s, int32_t *sig_s_Len) {
    ECDSA_SIG *signature = NULL;
    uint8_t digest[SHA256_DIGEST_SIZE] = {0, };

    // Init crypto core library
    initCryptoCore();

    if (eckey == NULL || in == NULL || sig_r == NULL || sig_r_Len == NULL || sig_s == NULL || sig_s_Len == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    crypto_sha256(digest, in, inLen, NULL);
    signature = ECDSA_do_sign(digest, sizeof(digest), (EC_KEY *)eckey);
    if (signature == NULL) {
        LOGE("Failed to generate EC Signature");
        return CRYPTO_STATUS_FAILED;
    }

    *sig_r_Len = BN_bn2bin( signature->r, sig_r );
    *sig_s_Len = BN_bn2bin( signature->s, sig_s );

    ECDSA_SIG_free(signature);

    return CRYPTO_STATUS_SUCCESS;
}


CRYPTO_STATUS crypto_ecdsa_verify_with_sha256(void *eckey, uint8_t *in, uint32_t inLen,
    uint8_t *sig_r, int32_t sig_r_Len, uint8_t *sig_s, int32_t sig_s_Len) {
    uint8_t digest[SHA256_DIGEST_SIZE] = {0, };
    uint8_t rawSig[ECDSA_SIG_SIZE] = {0, }, asnSig[ECDSA_SIG_WITH_ASN1_SIZE];
    uint32_t asnSig_len;

    // Init crypto core library
    initCryptoCore();

    if (eckey == NULL || in == NULL || sig_r == NULL || sig_s == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memcpy(rawSig + (32 - sig_r_Len), sig_r, sig_r_Len);
    memcpy(rawSig + 32 + (32 - sig_s_Len), sig_s, sig_s_Len);
    rebuildAsn1FromRawSigWithEcc(rawSig, asnSig, &asnSig_len);

    crypto_sha256(digest, in, inLen, NULL);
    if (ECDSA_verify(EVP_PKEY_EC, digest, sizeof(digest), asnSig, asnSig_len, (EC_KEY *)eckey) != 1) {
        LOGE("Failed to verify EC Signature");
        return CRYPTO_STATUS_FAILED;
    }

    LOGD("Success to verify EC Signature");

    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_gen_rsa_key(RSA **rsa_key, rsa_key_t *rsa_key_info) {
    int ret = 0;
    BIGNUM *bne = NULL;
    int bits = 2048;
    unsigned long e = RSA_F4;

    int32_t  bn_bin_size;
    uint8_t bn_bin[RSA_KEY_COMPONENT_SIZE];

    // Init crypto core library
    initCryptoCore();

    if (rsa_key_info == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    bne = BN_new();
    if ( bne == NULL ) {
        goto gen_key_fault;
    }

    ret = BN_set_word(bne, e);
    if ( ret != 1 ) {
        goto gen_key_fault;
    }

    *rsa_key = RSA_new();
    if ( !(*rsa_key) ) {
        goto free_all;
    }
    ret = RSA_generate_key_ex(*rsa_key, bits, bne, NULL);
    if(ret != 1){
        goto free_all;
    }

    BN_free(bne);

    if (rsa_key_info != NULL) {
        bn_bin_size = BN_bn2bin((*rsa_key)->n, bn_bin);
        memcpy(rsa_key_info->modulus, bn_bin, bn_bin_size);
        rsa_key_info->modulus_size = bn_bin_size;
        hex_print_tag_debug("modulus", rsa_key_info->modulus, rsa_key_info->modulus_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->e, bn_bin);
        memcpy(rsa_key_info->publicExponent, bn_bin, bn_bin_size);
        rsa_key_info->publicExponent_size = bn_bin_size;
        hex_print_tag_debug("publicExponent", rsa_key_info->publicExponent, rsa_key_info->publicExponent_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->d, bn_bin);
        memcpy(rsa_key_info->privateExponent, bn_bin, bn_bin_size);
        rsa_key_info->privateExponent_size = bn_bin_size;
        hex_print_tag_debug("privateExponent", rsa_key_info->privateExponent, rsa_key_info->privateExponent_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->p, bn_bin);
        memcpy(rsa_key_info->prime1, bn_bin, bn_bin_size);
        rsa_key_info->prime1_size = bn_bin_size;
        hex_print_tag_debug("prime1", rsa_key_info->prime1, rsa_key_info->prime1_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->q, bn_bin);
        memcpy(rsa_key_info->prime2, bn_bin, bn_bin_size);
        rsa_key_info->prime2_size = bn_bin_size;
        hex_print_tag_debug("prime2", rsa_key_info->prime2, rsa_key_info->prime2_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->dmp1, bn_bin);
        memcpy(rsa_key_info->exponent1, bn_bin, bn_bin_size);
        rsa_key_info->exponent1_size = bn_bin_size;
        hex_print_tag_debug("exponent1", rsa_key_info->exponent1, rsa_key_info->exponent1_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->dmq1, bn_bin);
        memcpy(rsa_key_info->exponent2, bn_bin, bn_bin_size);
        rsa_key_info->exponent2_size = bn_bin_size;
        hex_print_tag_debug("exponent2", rsa_key_info->exponent2, rsa_key_info->exponent2_size);

        bn_bin_size = BN_bn2bin((*rsa_key)->iqmp, bn_bin);
        memcpy(rsa_key_info->coefficient, bn_bin, bn_bin_size);
        rsa_key_info->coefficient_size = bn_bin_size;
        hex_print_tag_debug("coefficient", rsa_key_info->coefficient, rsa_key_info->coefficient_size);
    }

    return CRYPTO_STATUS_SUCCESS;

free_all:
    if ((*rsa_key) != NULL)
        RSA_free(*rsa_key);
    *rsa_key = NULL;

gen_key_fault:
    BN_free(bne);

    return CRYPTO_STATUS_FAILED;
}

CRYPTO_STATUS crypto_regen_rsa_pubkey(RSA **rsa_key, rsa_key_t *rsa_key_info) {
    // Init crypto core library
    initCryptoCore();

    if (rsa_key_info == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    *rsa_key = RSA_new();
    if ( !(*rsa_key) ) {
        goto free_all;
    }

    (*rsa_key)->n = BN_bin2bn(rsa_key_info->modulus, rsa_key_info->modulus_size, NULL);
    (*rsa_key)->e = BN_bin2bn(rsa_key_info->publicExponent, rsa_key_info->publicExponent_size, NULL);
//    (*rsa_key)->flags = RSA_FLAG_CACHE_PUBLIC;

    return CRYPTO_STATUS_SUCCESS;

free_all:
    RSA_free(*rsa_key);
     *rsa_key = NULL;

    return CRYPTO_STATUS_FAILED;
}

CRYPTO_STATUS crypto_regen_rsa_privkey(RSA **rsa_key, rsa_key_t *rsa_key_info) {
    // Init crypto core library
    initCryptoCore();

    if (rsa_key_info == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    *rsa_key = RSA_new();
    if ( !(*rsa_key) ) {
        goto free_all;
    }

    hex_print_tag_debug("modulus", rsa_key_info->modulus, rsa_key_info->modulus_size);
    hex_print_tag_debug("publicExponent", rsa_key_info->publicExponent, rsa_key_info->publicExponent_size);
    hex_print_tag_debug("privateExponent", rsa_key_info->privateExponent, rsa_key_info->privateExponent_size);
    hex_print_tag_debug("prime1", rsa_key_info->prime1, rsa_key_info->prime1_size);
    hex_print_tag_debug("prime2", rsa_key_info->prime2, rsa_key_info->prime2_size);
    hex_print_tag_debug("exponent1", rsa_key_info->exponent1, rsa_key_info->exponent1_size);
    hex_print_tag_debug("exponent2", rsa_key_info->exponent2, rsa_key_info->exponent2_size);
    hex_print_tag_debug("coefficient", rsa_key_info->coefficient, rsa_key_info->coefficient_size);

    (*rsa_key)->n = BN_bin2bn(rsa_key_info->modulus, rsa_key_info->modulus_size, NULL);
    (*rsa_key)->e = BN_bin2bn(rsa_key_info->publicExponent, rsa_key_info->publicExponent_size, NULL);
    (*rsa_key)->d = BN_bin2bn(rsa_key_info->privateExponent, rsa_key_info->privateExponent_size, NULL);
    (*rsa_key)->p = BN_bin2bn(rsa_key_info->prime1, rsa_key_info->prime1_size, NULL);
    (*rsa_key)->q = BN_bin2bn(rsa_key_info->prime2, rsa_key_info->prime2_size, NULL);
    (*rsa_key)->dmp1 = BN_bin2bn(rsa_key_info->exponent1, rsa_key_info->exponent1_size, NULL);
    (*rsa_key)->dmq1 = BN_bin2bn(rsa_key_info->exponent2, rsa_key_info->exponent2_size, NULL);
    (*rsa_key)->iqmp = BN_bin2bn(rsa_key_info->coefficient, rsa_key_info->coefficient_size, NULL);
    //(*rsa_key)->flags = RSA_FLAG_CACHE_PUBLIC | RSA_FLAG_CACHE_PRIVATE;

    return CRYPTO_STATUS_SUCCESS;

free_all:
    RSA_free(*rsa_key);
    *rsa_key = NULL;

    return CRYPTO_STATUS_FAILED;
}

void crypto_clear_rsa_key(RSA **rsa_key) {
    // Init crypto core library
    initCryptoCore();
    RSA_free(*rsa_key);
}

CRYPTO_STATUS crypto_rsa_oaep_encrypt(RSA *rsakey, const uint8_t *in, uint32_t in_size, uint8_t *out, uint32_t out_size) {
    // Init crypto core library
    initCryptoCore();

    if (rsakey == NULL || in == NULL || out == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (out_size != (uint32_t)RSA_size(rsakey) ||
        out_size != (uint32_t)RSA_public_encrypt(in_size, in, out, rsakey, RSA_PKCS1_OAEP_PADDING)) {
        goto encryption_fault;
    }

    return CRYPTO_STATUS_SUCCESS;

encryption_fault:
    return CRYPTO_STATUS_FAILED;
}

CRYPTO_STATUS crypto_rsa_oaep_decrypt(RSA *rsakey, const uint8_t *in, uint32_t in_size, uint8_t *out, uint32_t *out_size) {
    int32_t res;

    // Init crypto core library
    initCryptoCore();

    if (rsakey == NULL || in == NULL || out == NULL || out_size == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (*out_size < RSA_size(rsakey)) {
        goto decryption_fault;
    }

    res = RSA_private_decrypt(in_size, in, out, rsakey, RSA_PKCS1_OAEP_PADDING);
    if ( res <= 0 ) {
        goto decryption_fault;
    }

    *out_size = res;
    return CRYPTO_STATUS_SUCCESS;

decryption_fault:
    return CRYPTO_STATUS_FAILED;
}

#if 0
CRYPTO_STATUS crypto_rsa_sign_pkcs(uint8_t md_type, uint8_t isHashed, RSA *rsakey, uint8_t *data, uint32_t data_len, uint8_t *sig, uint32_t *sig_len) {
    CRYPTO_STATUS ret;
    EVP_MD_CTX md_ctx;
    unsigned char pDigest[SHA256_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;

    // Init crypto core library
    initCryptoCore();

    if (!rsakey) {
        LOGE("RSA key is NULL");
        ret = CRYPTO_STATUS_INVALID_ARGUMENT;
        goto prog_end;
    }

    if (data == NULL || sig == NULL || sig_len == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (md_type == MD_TYPE_SHA1) {
        uDigestLen = SHA1_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA256) {
        uDigestLen = SHA256_DIGEST_SIZE;
    } else {
        LOGE("Not supported MD TYPE");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (isHashed) {
        memcpy(pDigest, data, data_len);
        uDigestLen = data_len;
    } else {
        /* hash the message */
        EVP_MD_CTX_init(&md_ctx);
        if (md_type == MD_TYPE_SHA1) {
            EVP_DigestInit(&md_ctx, EVP_sha1());
        } else if (md_type == MD_TYPE_SHA256) {
            EVP_DigestInit(&md_ctx, EVP_sha256());
        }
        EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
        EVP_DigestFinal(&md_ctx, pDigest, &uDigestLen);
        EVP_MD_CTX_cleanup(&md_ctx);
    }

    hex_print_tag_debug("pDigest", pDigest, uDigestLen);

    /* sign the data */
    if (md_type == MD_TYPE_SHA1) {
        status = RSA_sign(NID_sha1, pDigest, uDigestLen, sig, sig_len, rsakey);
    } else if (md_type == MD_TYPE_SHA256) {
        status = RSA_sign(NID_sha256, pDigest, uDigestLen, sig, sig_len, rsakey);
    }
    if (status == 1) {
        LOGD("RSA_sign successfull!");
    } else {
        LOGE("RSA_sign failed with error");
        ret = CRYPTO_STATUS_FAILED;
        goto prog_end;
    }

    return CRYPTO_STATUS_SUCCESS;

prog_end:
    return ret;
}

CRYPTO_STATUS crypto_rsa_verify_pkcs(uint8_t md_type, uint8_t isHashed, RSA *rsakey, uint8_t *data, uint32_t data_len, uint8_t *sig, uint32_t sig_len) {
    CRYPTO_STATUS ret;
    EVP_MD_CTX md_ctx;
    unsigned char pDigest[SHA256_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;

    // Init crypto core library
    initCryptoCore();

    if (!rsakey) {
        LOGE("RSA key is NULL");
        ret = CRYPTO_STATUS_INVALID_ARGUMENT;
        goto prog_end;
    }

    if (data == NULL || sig == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (md_type == MD_TYPE_SHA1) {
        uDigestLen = SHA1_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA256) {
        uDigestLen = SHA256_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        return CRYPTO_STATUS_FAILED;
    }

    if (isHashed) {
        memcpy(pDigest, data, data_len);
        uDigestLen = data_len;
    } else {
        /* hash the message */
        EVP_MD_CTX_init(&md_ctx);
        if (md_type == MD_TYPE_SHA1) {
            EVP_DigestInit(&md_ctx, EVP_sha1());
        } else if (md_type == MD_TYPE_SHA256) {
            EVP_DigestInit(&md_ctx, EVP_sha256());
        }
        EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
        EVP_DigestFinal(&md_ctx, pDigest, &uDigestLen);
        EVP_MD_CTX_cleanup(&md_ctx);
    }

    /* verify the data */
    if (md_type == MD_TYPE_SHA1) {
        status = RSA_verify(NID_sha1, pDigest, uDigestLen, sig, sig_len, rsakey);
    } else if (md_type == MD_TYPE_SHA256) {
        status = RSA_verify(NID_sha256, pDigest, uDigestLen, sig, sig_len, rsakey);
    }

    if (status == 1) {
        LOGD("RSA_verify successfull!");
    } else {
        LOGE("RSA_verify failed with error");
        ret = CRYPTO_STATUS_FAILED;
        goto prog_end;
    }

    return CRYPTO_STATUS_SUCCESS;

prog_end:
    return ret;
}
#endif

CRYPTO_STATUS crypto_rsa_sign_pss(uint8_t md_type, uint8_t isHashed, RSA *rsakey, uint8_t *data, uint32_t data_len, uint8_t *sig, uint32_t *sig_len) {
    CRYPTO_STATUS ret;
    EVP_MD_CTX md_ctx;
    unsigned char pDigest[SHA256_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;
    unsigned char EM[256];

    // Init crypto core library
    initCryptoCore();

    if (!rsakey) {
        LOGE("RSA key is NULL");
        ret = CRYPTO_STATUS_INVALID_ARGUMENT;
        goto prog_end;
    }

    if (data == NULL || sig == NULL || sig_len == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (md_type == MD_TYPE_SHA1) {
        uDigestLen = SHA1_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA256) {
        uDigestLen = SHA256_DIGEST_SIZE;
    } else {
        LOGE("Not supported MD TYPE");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (isHashed) {
        memcpy(pDigest, data, data_len);
        uDigestLen = data_len;
    } else {
        /* hash the message */
        EVP_MD_CTX_init(&md_ctx);
        if (md_type == MD_TYPE_SHA1) {
            EVP_DigestInit(&md_ctx, EVP_sha1());
        } else if (md_type == MD_TYPE_SHA256) {
            EVP_DigestInit(&md_ctx, EVP_sha256());
        }
        EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
        EVP_DigestFinal(&md_ctx, pDigest, (unsigned int*) &uDigestLen);
        EVP_MD_CTX_cleanup(&md_ctx);
    }

    /* compute the PSS padded data */
#ifdef USE_SCRYPTO
    if (md_type == MD_TYPE_SHA1) {
        status = RSA_padding_add_PKCS1_PSS_mgf1(rsakey, EM, pDigest, EVP_sha1(), NULL, uDigestLen /* sLen == hLen*/);
    } else if (md_type == MD_TYPE_SHA256) {
        status = RSA_padding_add_PKCS1_PSS_mgf1(rsakey, EM, pDigest, EVP_sha256(), NULL, uDigestLen /* sLen == hLen*/);
    }

#else
    if (md_type == MD_TYPE_SHA1) {
        status = RSA_padding_add_PKCS1_PSS(rsakey, EM, pDigest, EVP_sha1(), uDigestLen /* sLen == hLen*/);
    } else if (md_type == MD_TYPE_SHA256) {
        status = RSA_padding_add_PKCS1_PSS(rsakey, EM, pDigest, EVP_sha256(), uDigestLen /* sLen == hLen*/);
    }
#endif

    if (!status) {
        LOGE("RSA_padding_add_PKCS1_PSS failed with error");
        ret = CRYPTO_STATUS_FAILED;
        goto prog_end;
    }

    /* perform digital signature */
    status = RSA_private_encrypt(256, EM, sig, rsakey, RSA_NO_PADDING);
    if (status == -1) {
        LOGE("RSA_private_encrypt failed with error");
        ret = CRYPTO_STATUS_FAILED;
        goto prog_end;
    } else {
        LOGD("RSA_sign successfull!");
        *sig_len = 256;
    }

    return CRYPTO_STATUS_SUCCESS;

prog_end:
    return ret;
}

CRYPTO_STATUS crypto_rsa_verify_pss(uint8_t md_type, uint8_t isHashed, RSA *rsakey, uint8_t *data, uint32_t data_len, uint8_t *sig, uint32_t sig_len) {
    CRYPTO_STATUS ret = CRYPTO_STATUS_SUCCESS;
    EVP_MD_CTX md_ctx;
    unsigned char pDigest[SHA256_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;
    unsigned char pDecrypted[256];

    // Init crypto core library
    initCryptoCore();

    if (!rsakey) {
        LOGE("RSA key is NULL");
        ret = CRYPTO_STATUS_INVALID_ARGUMENT;
        goto prog_end;
    }

    if (data == NULL || sig == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (sig_len != 256) {
        ret = CRYPTO_STATUS_INVALID_ARGUMENT;
        goto prog_end;
    }

    if (md_type == MD_TYPE_SHA1) {
        uDigestLen = SHA1_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA256) {
        uDigestLen = SHA256_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        return CRYPTO_STATUS_FAILED;
    }

    if (isHashed) {
        memcpy(pDigest, data, data_len);
        uDigestLen = data_len;
    } else {
        /* hash the message */
        EVP_MD_CTX_init(&md_ctx);
        if (md_type == MD_TYPE_SHA1) {
            EVP_DigestInit(&md_ctx, EVP_sha1());
        } else if (md_type == MD_TYPE_SHA256) {
            EVP_DigestInit(&md_ctx, EVP_sha256());
        }
        EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
        EVP_DigestFinal(&md_ctx, pDigest, (unsigned int*) &uDigestLen);
        EVP_MD_CTX_cleanup(&md_ctx);
    }

    /* now we will verify the signature : Start by a RAW decrypt of the signature  */
    status = RSA_public_decrypt(256, sig, pDecrypted, rsakey, RSA_NO_PADDING);
    if (status == -1) {
       LOGE("RSA_public_decrypt failed with error");
       goto prog_end;
    }

    /* verify the data */


#ifdef USE_SCRYPTO
    if (md_type == MD_TYPE_SHA1) {
        status = RSA_verify_PKCS1_PSS_mgf1(rsakey, pDigest, EVP_sha1(), NULL, pDecrypted, -2 /* salt length is autorecovered from signature */);
    } else if (md_type == MD_TYPE_SHA256) {
        status = RSA_verify_PKCS1_PSS_mgf1(rsakey, pDigest, EVP_sha256(), NULL, pDecrypted, -2 /* salt length is autorecovered from signature */);
    }
#else
    if (md_type == MD_TYPE_SHA1) {
        status = RSA_verify_PKCS1_PSS(rsakey, pDigest, EVP_sha1(), pDecrypted, -2 /* salt length is autorecovered from signature */);
    } else if (md_type == MD_TYPE_SHA256) {
        status = RSA_verify_PKCS1_PSS(rsakey, pDigest, EVP_sha256(), pDecrypted, -2 /* salt length is autorecovered from signature */);
    }
#endif

    if (status == 1) {
       LOGD("RSA_verify_PKCS1_PSS successfull!");
    } else {
       LOGE("RSA_verify_PKCS1_PSS failed with error");
       ret = CRYPTO_STATUS_FAILED;
       goto prog_end;
    }

    return CRYPTO_STATUS_SUCCESS;

prog_end:
    return ret;
}

#if 0
int crypto_rsa_verify_pkcs_rawkey(unsigned char *data, unsigned int data_len, unsigned char *sig, unsigned int sig_len,
                                    const uint8_t *rsa_key_N, uint32_t rsa_key_N_size,
                                    const uint8_t *rsa_key_e, uint32_t rsa_key_e_size) {
    RSA *rsa;

    EVP_MD_CTX md_ctx;
    unsigned char pDigest[32];
    size_t uDigestLen = 32;
    int status = 0;

    // Init crypto core library
    initCryptoCore();

    if (data == NULL || sig == NULL || rsa_key_N == NULL || rsa_key_e == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    rsa = RSA_new();
    if ( !rsa ) {
        goto key_alloc_fault;
    }

    rsa->n = BN_bin2bn( rsa_key_N, rsa_key_N_size, NULL );
    rsa->e = BN_bin2bn( rsa_key_e, rsa_key_e_size, NULL );
//    rsa->flags = RSA_FLAG_CACHE_PUBLIC | RSA_FLAG_CACHE_PRIVATE;

    /* hash the message */
	EVP_MD_CTX_init(&md_ctx);
    EVP_DigestInit(&md_ctx, EVP_sha256());
	EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
	EVP_DigestFinal(&md_ctx, pDigest, &uDigestLen);
    EVP_MD_CTX_cleanup(&md_ctx);

     /* verify the data */
    status = RSA_verify(NID_sha256, pDigest, uDigestLen, sig, sig_len, rsa);
    if (status == 1) {
        LOGD("Signature verification successfull!");
    } else {
        LOGE("RSA_verify failed with error");
       goto prog_end;
   }

key_alloc_fault:
    RSA_free(rsa);

prog_end:
    return status;
}
#endif

CRYPTO_STATUS crypto_aes_cbc_256_encrypt( const uint8_t *in, uint32_t size, uint8_t *out, uint32_t *outSize,
                                      const uint8_t iv[AES_BLOCK_SIZE], const uint8_t key[AES_256_KEY_SIZE] ) {
#ifdef USE_SCRYPTO
    int outLen1 = 0; int outLen2 = 0;
    uint8_t tmp_iv[AES_BLOCK_SIZE];

    if (in == NULL || out == NULL || outSize == NULL || iv == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memcpy(tmp_iv, iv, AES_BLOCK_SIZE);
    memset(out, 0, *outSize);

    EVP_CIPHER_CTX ctx;
    if (EVP_EncryptInit(&ctx, EVP_aes_256_cbc(), key, tmp_iv) != 1) {
        return CRYPTO_STATUS_FAILED;
    }

    EVP_EncryptUpdate(&ctx, out, &outLen1, in, size);
    EVP_EncryptFinal_ex(&ctx, out + outLen1, &outLen2);
    *outSize = outLen1 + outLen2;

    EVP_CIPHER_CTX_cleanup(&ctx);

#else
    AES_KEY akey = {0};
    uint8_t tmp_iv[AES_BLOCK_SIZE];
    uint8_t tmp_buf[SSP_MAX_SECURE_OBJECT_SIZE];
    uint32_t tmp_buf_len;

    // Init crypto core library
    initCryptoCore();

    if (in == NULL || out == NULL || outSize == NULL || iv == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    /* Add padding to plain data */
    tmp_buf_len = size - (size % AES_BLOCK_SIZE) + AES_BLOCK_SIZE;
    memcpy(tmp_buf, in, size);
    memset(tmp_buf + size, tmp_buf_len - size, tmp_buf_len - size); /* Padding */
    hex_print_tag_debug("padded data", tmp_buf, tmp_buf_len);

    if (tmp_buf_len % AES_BLOCK_SIZE != 0) {
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (AES_set_encrypt_key( key,  AES_256_KEY_SIZE * 8, &akey) != 0) {
        return CRYPTO_STATUS_FAILED;
    }

    memcpy(tmp_iv, iv, AES_BLOCK_SIZE);
    AES_cbc_encrypt(tmp_buf, out, tmp_buf_len, &akey, tmp_iv, AES_ENCRYPT);

    if ( SSP_MAX_SECURE_OBJECT_SIZE < tmp_buf_len ) {
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }
    
    *outSize = tmp_buf_len;

#endif

    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_aes_cbc_256_decrypt( const uint8_t *in, uint32_t size, uint8_t *out, uint32_t *outSize,
                                      const uint8_t iv[AES_BLOCK_SIZE], const uint8_t key[AES_256_KEY_SIZE] ) {
#ifdef USE_SCRYPTO
    int outLen1 = 0;
    int outLen2 = 0;
    uint8_t tmp_iv[AES_BLOCK_SIZE];

    if (in == NULL || out == NULL || outSize == NULL || iv == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memcpy(tmp_iv, iv, AES_BLOCK_SIZE);
    memset(out, 0, *outSize);

    EVP_CIPHER_CTX ctx;
    if (EVP_DecryptInit(&ctx, EVP_aes_256_cbc(), key, tmp_iv) != 1) {
        return CRYPTO_STATUS_FAILED;
    }

    EVP_DecryptUpdate(&ctx, out, &outLen1, in, size);
    EVP_DecryptFinal_ex(&ctx, out + outLen1, &outLen2);

    *outSize = outLen1 + outLen2;

    EVP_CIPHER_CTX_cleanup(&ctx);

#else
    AES_KEY akey = {0};
    uint8_t tmp_iv[AES_BLOCK_SIZE];

    // Init crypto core library
    initCryptoCore();

    if (in == NULL || out == NULL || outSize == NULL || iv == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (size % AES_BLOCK_SIZE != 0) {
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (AES_set_decrypt_key( key, AES_256_KEY_SIZE * 8, &akey ) != 0) {
        return CRYPTO_STATUS_FAILED;
    }

    memcpy(tmp_iv, iv, AES_BLOCK_SIZE);
    AES_cbc_encrypt( in, out, size, &akey, tmp_iv, AES_DECRYPT);
    *outSize = crypto_aes_256_unpadded_data_size(out, size);

#endif
    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_aes_256_block_encrypt( const uint8_t in[AES_BLOCK_SIZE],
                                        uint8_t out[AES_BLOCK_SIZE],
                                        const uint8_t key[AES_256_KEY_SIZE] ) {
    AES_KEY akey = {0};

    // Init crypto core library
    initCryptoCore();

    if (in == NULL || out == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if ( AES_set_encrypt_key( key, AES_256_KEY_SIZE * 8, &akey ) != 0 ) {
        return CRYPTO_STATUS_FAILED;
    }

    AES_encrypt( in, out, &akey );

    return CRYPTO_STATUS_SUCCESS;
}


CRYPTO_STATUS crypto_aes_256_wrap_otp( const uint8_t input[AES_256_KEY_SIZE],
                                   uint8_t output[ENCRYPTED_OTP_KEY_SIZE],
                                   const uint8_t key[AES_256_KEY_SIZE] ) {
    AES_KEY     akey = {0};
    uint8_t     op_buf[ AES_BLOCK_SIZE ];
    uint8_t     ctr;
    int32_t     pos;
    uint32_t    round;

    // Init crypto core library
    initCryptoCore();

    if (input == NULL || output == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if ( AES_set_encrypt_key( key, AES_256_KEY_SIZE * 8, &akey ) != 0 ) {
        return CRYPTO_STATUS_FAILED;
    }

    memset( op_buf, 0xA6, AES_WRAP_BLOCK_SIZE ); /* AES key wrap IV = A6A6A6... */
    memcpy( output + AES_WRAP_BLOCK_SIZE, input, AES_256_KEY_SIZE ); /* Will use output buffer as working memory */
    ctr = 1;

    for ( round = 0 ; round < 6 ; ++round ) {
        for( pos = AES_WRAP_BLOCK_SIZE; pos <= AES_256_KEY_SIZE; pos += AES_WRAP_BLOCK_SIZE ) {
            memcpy( op_buf+AES_WRAP_BLOCK_SIZE, output + pos, AES_WRAP_BLOCK_SIZE );
            AES_encrypt( op_buf, op_buf, &akey );
            memcpy( output + pos, op_buf + AES_WRAP_BLOCK_SIZE, AES_WRAP_BLOCK_SIZE );

            /* Since counter is in MSB, it's least significant byte is the last of first sub-block */
            op_buf[AES_WRAP_BLOCK_SIZE-1] ^= ctr;
            ctr++;
        }
    }

    memcpy( output, op_buf, AES_WRAP_BLOCK_SIZE );

    return CRYPTO_STATUS_SUCCESS;
}


CRYPTO_STATUS crypto_aes_256_unwrap_otp( const uint8_t input[ENCRYPTED_OTP_KEY_SIZE],
                                     uint8_t output[AES_256_KEY_SIZE],
                                     const uint8_t key[AES_256_KEY_SIZE] ) {
    AES_KEY     akey = {0};
    uint8_t     op_buf[ AES_BLOCK_SIZE ];
    uint8_t     ctr;
    int32_t     pos;
    uint32_t    round;
    uint32_t    dmac_fp;

    // Init crypto core library
    initCryptoCore();

    if (input == NULL || output == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if ( AES_set_decrypt_key( key, AES_256_KEY_SIZE * 8, &akey ) != 0 ) {
        return CRYPTO_STATUS_FAILED;
    }

    memcpy( op_buf, input, AES_WRAP_BLOCK_SIZE ); /* Load AES-wrap appendix from 0th sub-block */
    memcpy( output, input + AES_WRAP_BLOCK_SIZE, AES_256_KEY_SIZE ); /* Will use output buffer as working memory */
    ctr = (AES_256_KEY_SIZE / AES_WRAP_BLOCK_SIZE) * 6; /* last value of counter during encryption */

    for ( round = 0; round < 6; ++round ) {
        for( pos = AES_256_KEY_SIZE - AES_WRAP_BLOCK_SIZE; pos >= 0; pos -= AES_WRAP_BLOCK_SIZE ) {
            /* Since counter is in MSB, it's least significzant byte is the last of first sub-block */
            op_buf[AES_WRAP_BLOCK_SIZE-1] ^= ctr;
            ctr--;

            memcpy( op_buf + AES_WRAP_BLOCK_SIZE, output + pos, AES_WRAP_BLOCK_SIZE );
            AES_decrypt( op_buf, op_buf, &akey );
            memcpy( output + pos, op_buf + AES_WRAP_BLOCK_SIZE, AES_WRAP_BLOCK_SIZE );
        }
    }

    /* Verify IV is original one */
    dmac_fp = 0;
    for ( pos = 0; pos < AES_WRAP_BLOCK_SIZE; pos += sizeof(uint32_t) ) {
        dmac_fp |= *(uint32_t*)(op_buf + pos) ^ 0xA6A6A6A6;
    }

    return (dmac_fp == 0) ? CRYPTO_STATUS_SUCCESS : CRYPTO_STATUS_OTP_KEY_CORRUPTED;
}


uint32_t crypto_aes_256_unpadded_data_size( const uint8_t *data, uint32_t size ) {
    uint32_t padding_len = 0;

    if (data == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if ( size != 0 ) {
        padding_len = data[size - 1];
    }

    /* If we have wrong padding, just return entire data.
     * Integrity check is not padding mechanism's responsibility */
    if ( padding_len > size || padding_len > AES_BLOCK_SIZE ) {
        return size;
    }

    return size - padding_len;
}


CRYPTO_STATUS crypto_sha1( uint8_t out[SHA1_DIGEST_SIZE], ... ) {
    va_list    args;
    SHA_CTX    sha_ctx;

    uint8_t    *chunk;
    uint32_t    chunk_len;

    // Init crypto core library
    initCryptoCore();

    if (out == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    va_start( args, out );
    SHA1_Init( &sha_ctx );

    while( 1 ) {
        chunk = va_arg( args, uint8_t* );
        if ( chunk == NULL ) {
            break;
        }

        chunk_len = va_arg( args, uint32_t );

        if ( chunk_len != 0 ) {
            SHA1_Update( &sha_ctx, chunk, chunk_len );
        }
    }

    va_end( args );
    SHA1_Final( out, &sha_ctx );

    return CRYPTO_STATUS_SUCCESS;
}


CRYPTO_STATUS crypto_sha256( uint8_t out[SHA256_DIGEST_SIZE], ... ) {
    va_list     args;
    SHA256_CTX  sha_ctx;

    uint8_t    *chunk;
    uint32_t    chunk_len;

    // Init crypto core library
    initCryptoCore();

    if (out == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    va_start( args, out );
    SHA256_Init( &sha_ctx );

    while( 1 ) {
        chunk = va_arg( args, uint8_t* );
        if ( chunk == NULL ) {
            break;
        }

        chunk_len = va_arg( args, uint32_t );

        if ( chunk_len != 0 ) {
            SHA256_Update( &sha_ctx, chunk, chunk_len );
        }
    }

    va_end( args );
    SHA256_Final( out, &sha_ctx );

    return CRYPTO_STATUS_SUCCESS;
}


static void crypto_aes_cmac_lshift( uint8_t data[AES_BLOCK_SIZE] ) {
    uint8_t  carry = 0;
    uint8_t  tmp;
    int32_t  i;

    if (data == NULL) {
        LOGE("Input parameter is NULL");
        return ;
    }

    for( i = AES_BLOCK_SIZE - 1; i >= 0; --i ) {
        tmp = data[i];
        data[i] = (tmp << 1) | carry;
        carry = tmp >> 7;
    }

    data[AES_BLOCK_SIZE - 1] ^= 0x87 & ~(carry - 1);
}


CRYPTO_STATUS crypto_aes_256_cmac( uint8_t out[AES_BLOCK_SIZE], const uint8_t key[AES_256_KEY_SIZE], ... ) {
    va_list     args;
    AES_KEY     akey = {0};
    uint8_t     K[AES_BLOCK_SIZE];
    uint8_t     buf[AES_BLOCK_SIZE];
    uint32_t    pos;

    uint8_t    *chunk;
    uint32_t    chunk_len;

    // Init crypto core library
    initCryptoCore();

    if (out == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }
    
    if ( AES_set_encrypt_key( key, AES_256_KEY_SIZE * 8, &akey ) != 0 ) {
        return CRYPTO_STATUS_FAILED;
    }

    memset( K, 0, AES_BLOCK_SIZE );
    memset( buf, 0, AES_BLOCK_SIZE );

    AES_encrypt( K, K, &akey );
    crypto_aes_cmac_lshift( K );
    pos = 0;

    va_start( args, key );

    while( 1 ) {
        chunk = va_arg( args, uint8_t* );
        if ( chunk == NULL ) {
            break;
        }

        chunk_len = va_arg( args, uint32_t );

        while( chunk_len != 0 ) {
            if ( pos == AES_BLOCK_SIZE ) {
                AES_encrypt( buf, buf, &akey );
                pos = 0;
            }

            buf[pos] ^= *chunk;
            pos++;

            chunk++;
            chunk_len--;
        }
    }

    va_end( args );

    if ( pos != AES_BLOCK_SIZE ) {
        buf[pos] ^= 0x80;
        crypto_aes_cmac_lshift( K );
    }

    for ( pos = 0; pos < AES_BLOCK_SIZE; pos += sizeof(uint32_t) ) {
        *(uint32_t*)(buf + pos) ^= *(uint32_t*)(K + pos);
    }

    AES_encrypt( buf, out, &akey );

    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_hmac (uint8_t md_type, uint8_t *out, const uint8_t *key, uint32_t keyLen,  ...) {
    va_list args;
    uint32_t ubfLen;

    uint8_t *chunk;
    uint32_t chunk_len;

    HMAC_CTX ctx;

    // Init crypto core library
    initCryptoCore();

    if (out == NULL || key == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (md_type == MD_TYPE_SHA1) {
        ubfLen = SHA1_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA256) {
        ubfLen = SHA256_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        return CRYPTO_STATUS_FAILED;
    }

    HMAC_CTX_init(&ctx);

    if (md_type == MD_TYPE_SHA1) {
        HMAC_Init_ex(&ctx, key, keyLen, EVP_sha1(), NULL);
    } else if (md_type == MD_TYPE_SHA256) {
        HMAC_Init_ex(&ctx, key, keyLen, EVP_sha256(), NULL);
    } else {
        LOGE("Not supported md algorithm");
        HMAC_CTX_cleanup(&ctx);
        return CRYPTO_STATUS_FAILED;
    }

    va_start(args, keyLen);

    while (1) {
        chunk = va_arg(args, uint8_t*);
        if (chunk == NULL) {
            break;
        }

        chunk_len = va_arg(args, uint32_t);

        if (chunk_len != 0) {
            HMAC_Update(&ctx, chunk, chunk_len);
        }
    }

    va_end( args );

    HMAC_Final(&ctx, out, &ubfLen);
    HMAC_CTX_cleanup(&ctx);

    return CRYPTO_STATUS_SUCCESS;
}

uint32_t crypto_secure_mac_cmp( const uint8_t mac1[AES_BLOCK_SIZE], const uint8_t mac2[AES_BLOCK_SIZE] ) {
    uint32_t res = 0;
    uint32_t pos;

    if (mac1 == NULL || mac2 == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    for ( pos = 0; pos < AES_BLOCK_SIZE; pos += sizeof(uint32_t) ) {
        res |= *(uint32_t*)&mac1[pos] ^ *(uint32_t*)&mac2[pos];
    }

    return res;
}


void crypto_clear_mem( void *mem, uint8_t size_bytes ) {
    size_t   *arr;
    uint32_t  i;

    if (mem == NULL) {
        LOGE("Input parameter is NULL");
        return ;
    }

    for ( arr = (size_t*)mem; size_bytes >= sizeof(size_t); arr++, size_bytes -= sizeof(size_t) ) {
        *arr = 0;
    }

    for ( i = 0; i < size_bytes; ++i ) {
        ((uint8_t*)arr)[i] = 0;
    }
}