#include "crypto_module.h"
#include "crypto_core_init.h"
#include "cryptolib_init.h"
#include "debug.h"
#include "skpm_crypto_util.h"

#include <stdarg.h>
#include <skpm_util.h>


#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_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 gen_key_fault;
    }
    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:
    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) == NULL) {
        return CRYPTO_STATUS_FAILED;
    }

    (*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; 
 }

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) == NULL) {
        return CRYPTO_STATUS_FAILED;
    }

    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; 
}

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

CRYPTO_STATUS crypto_gen_ecc_key(EC_KEY **ecc_key, ecc_key_t *ecc_key_info) {
    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_info == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;    
    }

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

    if (!EC_KEY_generate_key(*ecc_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(*ecc_key),
                           EC_KEY_get0_public_key(*ecc_key),
                           POINT_CONVERSION_UNCOMPRESSED,
                           transform_bignum,
                           bn_ctx) != transform_bignum) {
        goto transform_fault;
    }

    if (ecc_key_info != NULL) {
        bn_bin_size = BN_bn2bin(transform_bignum, bn_bin);
        memcpy(ecc_key_info->pubkey_binary, bn_bin, bn_bin_size);
        ecc_key_info->pubkey_size = bn_bin_size;
        hex_print_tag_debug("pub_key", ecc_key_info->pubkey_binary, ecc_key_info->pubkey_size);

        bn_bin_size = BN_bn2bin(EC_KEY_get0_private_key(*ecc_key), bn_bin);
        memcpy(ecc_key_info->privkey_binary, bn_bin, bn_bin_size);
        ecc_key_info->privkey_size = bn_bin_size;
        hex_print_tag_debug("priv_key", ecc_key_info->privkey_binary, ecc_key_info->privkey_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(*ecc_key);
    *ecc_key = NULL;

new_key_fault:
    return CRYPTO_STATUS_FAILED;
}

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

    // Init crypto core library
    initCryptoCore();

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

    *ecc_key = EC_KEY_new_by_curve_name(NID_X9_62_prime256v1);
    if (*ecc_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(*ecc_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(*ecc_key), transform_bignum, peer_public, bn_ctx) != peer_public) {
        goto point_translate_fault;
    }

    EC_KEY_set_public_key(*ecc_key, peer_public);

    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(*ecc_key);
    *ecc_key = NULL;

    return CRYPTO_STATUS_FAILED;
}

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

    // Init crypto core library
    initCryptoCore();

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

    *ecc_key = EC_KEY_new_by_curve_name(NID_X9_62_prime256v1);
    if (*ecc_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(*ecc_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(*ecc_key, transform_bignum);
    EC_KEY_set_public_key(*ecc_key, peer_public);

    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(*ecc_key);
    *ecc_key = NULL;

    return CRYPTO_STATUS_FAILED;
}

CRYPTO_STATUS crypto_gen_ecc_secret( uint8_t *priv_key, uint32_t priv_key_size, const uint8_t *peer_key, uint32_t peer_key_size, uint8_t *secretkey) {
    EC_KEY   *key = NULL;
    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 (priv_key == NULL || secretkey == NULL || peer_key == NULL || peer_key_size != ECC_PUBKEY_SIZE) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;    
    }

    if (crypto_regen_ecc_privkey(&key, priv_key, priv_key_size) != CRYPTO_STATUS_SUCCESS) {
        LOGE("crypto_regen_ecc_privkey is failed");
        return CRYPTO_STATUS_FAILED;    
    }

    if (key == NULL) {
        LOGE("crypto_regen_ecc_privkey is failed");
        return CRYPTO_STATUS_FAILED;    
    }

    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(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);
    crypto_clear_ecc_key(&key);
    
    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:
    if (key != NULL) {
        crypto_clear_ecc_key(&key);
    }
    return CRYPTO_STATUS_FAILED;
}

void crypto_clear_ecc_key(EC_KEY **ecc_key) {
    // Init crypto core library
    initCryptoCore();
    EC_KEY_free(*ecc_key);
}

CRYPTO_STATUS crypto_ecdsa_sign(uint8_t md_type, EC_KEY *eckey, uint8_t *in, uint32_t inLen,
    uint8_t *sig_r, uint32_t *sig_r_Len, uint8_t *sig_s, uint32_t *sig_s_Len) {
    ECDSA_SIG *signature = NULL;
    uint8_t digest[SHA512_DIGEST_SIZE] = {0, };
    size_t uDigestLen;

    // Init crypto core library
    initCryptoCore();

    if (NULL == eckey || 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;
    }

    if (md_type != MD_TYPE_NONE) {
        if (md_type == MD_TYPE_SHA1) {
            uDigestLen = SHA1_DIGEST_SIZE;
        } else if (md_type == MD_TYPE_SHA256) {
            uDigestLen = SHA256_DIGEST_SIZE;
        } else if (md_type == MD_TYPE_SHA384) {
            uDigestLen = SHA384_DIGEST_SIZE;
        } else if (md_type == MD_TYPE_SHA512) {
            uDigestLen = SHA512_DIGEST_SIZE;
        } else {
            LOGE("Not supported MD TYPE");
            return CRYPTO_STATUS_INVALID_ARGUMENT;
        }

        crypto_sha(md_type, digest, in, inLen, NULL);
        signature = ECDSA_do_sign(digest, uDigestLen, (EC_KEY *)eckey);
    } else {
        signature = ECDSA_do_sign(in, inLen, (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(uint8_t md_type, EC_KEY *eckey, uint8_t *in, uint32_t inLen,
    uint8_t *sig_r, uint32_t sig_r_Len, uint8_t *sig_s, uint32_t sig_s_Len) {
    uint8_t digest[SHA512_DIGEST_SIZE] = {0, };
    size_t uDigestLen;
    uint8_t asnSig[ECDSA_SIG_WITH_ASN1_SIZE];
    uint32_t asnSig_len = sizeof(asnSig);

    // Init crypto core library
    initCryptoCore();

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

    rebuildAsn1FromRawSigWithEcc(sig_r, sig_r_Len, sig_s, sig_s_Len, asnSig, &asnSig_len);

    if (md_type != MD_TYPE_NONE) {
        if (md_type == MD_TYPE_SHA1) {
            uDigestLen = SHA1_DIGEST_SIZE;
        } else if (md_type == MD_TYPE_SHA256) {
            uDigestLen = SHA256_DIGEST_SIZE;
        } else if (md_type == MD_TYPE_SHA384) {
            uDigestLen = SHA384_DIGEST_SIZE;
        } else if (md_type == MD_TYPE_SHA512) {
            uDigestLen = SHA512_DIGEST_SIZE;
        } else {
            LOGE("Not supported MD TYPE");
            return CRYPTO_STATUS_INVALID_ARGUMENT;
        }

        crypto_sha(md_type, digest, in, inLen, NULL);
        if (ECDSA_verify(EVP_PKEY_EC, digest, uDigestLen, asnSig, asnSig_len, (EC_KEY *)eckey) != 1) {
            LOGE("Failed to verify EC Signature");
            return CRYPTO_STATUS_FAILED;
        }
    } else {
        crypto_sha(md_type, digest, in, inLen, NULL);
        if (ECDSA_verify(EVP_PKEY_EC, in, inLen, 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_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;
}

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[SHA512_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;
    
    // Init crypto core library
    initCryptoCore();

    if (rsakey == NULL || 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 if (md_type == MD_TYPE_SHA384) {
        uDigestLen = SHA384_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA512) {
        uDigestLen = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported MD TYPE");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (isHashed) {
        if (sizeof(pDigest) < data_len) {
            LOGE("pDigest buffer is not enough");
            return CRYPTO_STATUS_INVALID_ARGUMENT;  
        }
        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());
        } else if (md_type == MD_TYPE_SHA384) {
            EVP_DigestInit(&md_ctx, EVP_sha384());
        } else if (md_type == MD_TYPE_SHA512) {
            EVP_DigestInit(&md_ctx, EVP_sha512());
        }
        EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
        EVP_DigestFinal(&md_ctx, pDigest, (unsigned int*) &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);
    } else if (md_type == MD_TYPE_SHA384) {
        status = RSA_sign(NID_sha384, pDigest, uDigestLen, sig, sig_len, rsakey);
    } else if (md_type == MD_TYPE_SHA512) {
        status = RSA_sign(NID_sha512, 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[SHA512_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;

    // Init crypto core library
    initCryptoCore();

    if (rsakey == NULL || 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 if (md_type == MD_TYPE_SHA384) {
        uDigestLen = SHA384_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA512) {
        uDigestLen = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        return CRYPTO_STATUS_FAILED;
    }

    if (isHashed) {
        if (sizeof(pDigest) < data_len) {
            LOGE("pDigest buffer is not enough");
            return CRYPTO_STATUS_INVALID_ARGUMENT;  
        }
        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());
        } else if (md_type == MD_TYPE_SHA384) {
            EVP_DigestInit(&md_ctx, EVP_sha384());
        } else if (md_type == MD_TYPE_SHA512) {
            EVP_DigestInit(&md_ctx, EVP_sha512());
        }
        EVP_DigestUpdate(&md_ctx, (const void*) data, data_len);
        EVP_DigestFinal(&md_ctx, pDigest, (unsigned int*) &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);
    } else if (md_type == MD_TYPE_SHA384) {
        status = RSA_verify(NID_sha384, pDigest, uDigestLen, sig, sig_len, rsakey);
    } else if (md_type == MD_TYPE_SHA512) {
        status = RSA_verify(NID_sha512, 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;
}

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[SHA512_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;
    unsigned char EM[256];

    // Init crypto core library
    initCryptoCore();

    if (rsakey == NULL || 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 if (md_type == MD_TYPE_SHA384) {
        uDigestLen = SHA384_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA512) {
        uDigestLen = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported MD TYPE");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (isHashed) {
        if (sizeof(pDigest) < data_len) {
            LOGE("pDigest buffer is not enough");
            return CRYPTO_STATUS_INVALID_ARGUMENT;  
        }
        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());
        } else if (md_type == MD_TYPE_SHA384) {
            EVP_DigestInit(&md_ctx, EVP_sha384());
        } else if (md_type == MD_TYPE_SHA512) {
            EVP_DigestInit(&md_ctx, EVP_sha512());
        }
        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_SHA384) {
        status = RSA_padding_add_PKCS1_PSS_mgf1(rsakey, EM, pDigest, EVP_sha384(), NULL, uDigestLen /* sLen == hLen*/);
    } else if (md_type == MD_TYPE_SHA512) {
        status = RSA_padding_add_PKCS1_PSS_mgf1(rsakey, EM, pDigest, EVP_sha512(), 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*/);
    } else if (md_type == MD_TYPE_SHA384) {
        status = RSA_padding_add_PKCS1_PSS(rsakey, EM, pDigest, EVP_sha384(), uDigestLen /* sLen == hLen*/);
    } else if (md_type == MD_TYPE_SHA512) {
        status = RSA_padding_add_PKCS1_PSS(rsakey, EM, pDigest, EVP_sha512(), 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[SHA512_DIGEST_SIZE];
    size_t uDigestLen;
    int status = 0;
    unsigned char pDecrypted[256];

    // Init crypto core library
    initCryptoCore();

    if (rsakey == NULL || 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 if (md_type == MD_TYPE_SHA384) {
        uDigestLen = SHA384_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA512) {
        uDigestLen = SHA512_DIGEST_SIZE;
    } else {
        LOGE("Not supported md_type");
        return CRYPTO_STATUS_FAILED;
    }

    if (isHashed) {
        if (sizeof(pDigest) < data_len) {
            LOGE("pDigest buffer is not enough");
            return CRYPTO_STATUS_INVALID_ARGUMENT;  
        }
        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());
        } else if (md_type == MD_TYPE_SHA384) {
            EVP_DigestInit(&md_ctx, EVP_sha384());
        } else if (md_type == MD_TYPE_SHA512) {
            EVP_DigestInit(&md_ctx, EVP_sha512());
        }
        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_SHA384) {
        status = RSA_verify_PKCS1_PSS_mgf1(rsakey, pDigest, EVP_sha384(), NULL, pDecrypted, -2 /* salt length is autorecovered from signature */);
    } else if (md_type == MD_TYPE_SHA512) {
        status = RSA_verify_PKCS1_PSS_mgf1(rsakey, pDigest, EVP_sha512(), 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 */);
    } else if (md_type == MD_TYPE_SHA384) {
        status = RSA_verify_PKCS1_PSS(rsakey, pDigest, EVP_sha384(), pDecrypted, -2 /* salt length is autorecovered from signature */);
    } else if (md_type == MD_TYPE_SHA512) {
        status = RSA_verify_PKCS1_PSS(rsakey, pDigest, EVP_sha512(), 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;
}

CRYPTO_STATUS crypto_aes_cbc_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, const uint32_t keyLen) {
#ifdef USE_SCRYPTO
    int outLen1 = 0; int outLen2 = 0;
    uint8_t tmp_iv[AES_BLOCK_SIZE];
    uint32_t out_buff_size = size - (size % AES_BLOCK_SIZE) + AES_BLOCK_SIZE;
    EVP_CIPHER_CTX ctx;

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

    if (*outSize < out_buff_size) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

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

    if (keyLen == AES_128_KEY_SIZE) {
        if (EVP_EncryptInit(&ctx, EVP_aes_128_cbc(), key, tmp_iv) != 1) {
            return CRYPTO_STATUS_FAILED;
        }
    } else if (keyLen == AES_256_KEY_SIZE) {
        if (EVP_EncryptInit(&ctx, EVP_aes_256_cbc(), key, tmp_iv) != 1) {
            return CRYPTO_STATUS_FAILED;
        }
    } else {
        LOGE("Not supported key size, %u", keyLen);
        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;
    uint8_t tmp_iv[AES_BLOCK_SIZE];
    uint8_t tmp_buf[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;
    if (*outSize < tmp_buf_len) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (sizeof(tmp_buf) < tmp_buf_len) {
        LOGE("tmp buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }
    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, keyLen * 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);
    *outSize = tmp_buf_len;

#endif

    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_aes_cbc_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, const uint32_t keyLen) {
#ifdef USE_SCRYPTO
    int outLen1 = 0;
    int outLen2 = 0;
    uint8_t tmp_iv[AES_BLOCK_SIZE];
    EVP_CIPHER_CTX ctx;

    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 (*outSize < size) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

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

    if (keyLen == AES_128_KEY_SIZE) {
        if (EVP_DecryptInit(&ctx, EVP_aes_128_cbc(), key, tmp_iv) != 1) {
            return CRYPTO_STATUS_FAILED;
        }
    } else if (keyLen == AES_256_KEY_SIZE) {
        if (EVP_DecryptInit(&ctx, EVP_aes_256_cbc(), key, tmp_iv) != 1) {
            return CRYPTO_STATUS_FAILED;
        }
    } else {
        LOGE("Not supported key size, %u", keyLen);
        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;
    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 (*outSize < size) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (AES_set_decrypt_key( key, keyLen * 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_unpadded_data_size(out, size);

#endif
    return CRYPTO_STATUS_SUCCESS;
}

uint32_t crypto_aes_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_aes_ccm_encrypt(unsigned char *iv,unsigned int ivlen,
                                              unsigned char *key,unsigned int keylen,
                                              unsigned char *aad, unsigned long aadlen,
                                              unsigned char *data,unsigned long datalen,
                                              unsigned char *out, unsigned long *outlen,
                                              unsigned int taglen) {
    int status = 0;

    // Init crypto core library
    initCryptoCore();

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

    if (*outlen < datalen) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    status = AES_CCM_Encrypt(iv, ivlen, key, keylen, aad, aadlen, data, datalen, out, outlen, taglen);
    if (status != 1) {
        return CRYPTO_STATUS_FAILED;
    } else {
        return CRYPTO_STATUS_SUCCESS;
    }

}

/* Decrypt buffer with AES-128 CCM. Does not remove the padding */
CRYPTO_STATUS crypto_aes_ccm_decrypt(unsigned char *iv,unsigned int ivlen,
                                              unsigned char *key,unsigned int keylen,
                                              unsigned char *aad, unsigned long aadlen,
                                              unsigned char *data,unsigned long datalen,
                                              unsigned char *out, unsigned long *outlen,
                                              unsigned int taglen) {
    int status = 0;

    // Init crypto core library
    initCryptoCore();

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

    if (*outlen < datalen) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    status = AES_CCM_Decrypt(iv, ivlen, key, keylen, aad, aadlen, data, datalen, out, outlen, taglen);
    if (status != 1) {
        return CRYPTO_STATUS_FAILED;
    } else {
        return CRYPTO_STATUS_SUCCESS;
    }
}

CRYPTO_STATUS crypto_aes_gcm_encrypt(unsigned char *iv,unsigned int ivlen,
                                              unsigned char *key,unsigned int keylen,
                                              unsigned char *aad, unsigned long aadlen,
                                              unsigned char *data,unsigned long datalen,
                                              unsigned char *out, unsigned long *outlen,
                                              unsigned char *tag, int taglen) {

#ifndef USE_SCRYPTO
    GCM128_CONTEXT ctx;
    AES_KEY akey;

    // Init crypto core library
    initCryptoCore();

    if (iv == NULL || key == NULL || aad == NULL || data == NULL || out == NULL || outlen == NULL || tag == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (*outlen < datalen) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memset(out, 0, *outlen);
    *outlen = datalen;

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

    CRYPTO_gcm128_init(&ctx, &akey, (block128_f)AES_encrypt);
    CRYPTO_gcm128_setiv(&ctx, iv, ivlen);
    CRYPTO_gcm128_aad(&ctx, aad, aadlen);
    CRYPTO_gcm128_encrypt(&ctx, data, out, *outlen);
    CRYPTO_gcm128_tag(&ctx, tag, taglen);
#else
    EVP_CIPHER_CTX ctx;

    if (iv == NULL || key == NULL || aad == NULL || data == NULL || out == NULL || outlen == NULL || tag == NULL || keylen == 0) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (*outlen < datalen) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memset(out, 0, *outlen);
    *outlen = datalen;

    EVP_CIPHER_CTX_init(&ctx);
    EVP_CipherInit(&ctx, EVP_aes_128_gcm(), NULL, NULL, 1);
    EVP_CIPHER_CTX_ctrl(&ctx, EVP_CTRL_GCM_SET_IVLEN, ivlen, 0);
    EVP_CipherInit(&ctx, NULL, key, iv, ivlen);
    EVP_Cipher(&ctx, NULL, aad, aadlen);
    EVP_Cipher(&ctx, out, data, datalen);
    EVP_Cipher(&ctx, NULL, NULL, 0);
    EVP_CIPHER_CTX_ctrl(&ctx, EVP_CTRL_GCM_GET_TAG, taglen, tag);
    EVP_CIPHER_CTX_cleanup(&ctx);
#endif
    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_aes_gcm_decrypt(unsigned char *iv,unsigned int ivlen,
                                              unsigned char *key,unsigned int keylen,
                                              unsigned char *aad, unsigned long aadlen,
                                              unsigned char *data,unsigned long datalen,
                                              unsigned char *out, unsigned long *outlen,
                                              unsigned char *tag, int taglen) {

#ifndef USE_SCRYPTO
    GCM128_CONTEXT ctx;
    AES_KEY akey;

    // Init crypto core library
    initCryptoCore();

    if (iv == NULL || key == NULL || aad == NULL || data == NULL || out == NULL || outlen == NULL || tag == NULL) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (*outlen < datalen) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memset(out, 0, *outlen);
    *outlen = datalen;

    if (AES_set_encrypt_key( key, keylen * 8, &akey ) != 0) {
        return CRYPTO_STATUS_FAILED;
    }
    CRYPTO_gcm128_init(&ctx, &akey, (block128_f)AES_encrypt);
    CRYPTO_gcm128_setiv(&ctx, iv, ivlen);
    CRYPTO_gcm128_aad(&ctx, aad, aadlen);
    CRYPTO_gcm128_decrypt(&ctx, data, out, *outlen);
    CRYPTO_gcm128_finish(&ctx, tag, taglen);
#else
    EVP_CIPHER_CTX ctx;

    if (iv == NULL || key == NULL || aad == NULL || data == NULL || out == NULL || outlen == NULL || tag == NULL || keylen == 0) {
        LOGE("Input parameter is NULL");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    if (*outlen < datalen) {
        LOGE("Output buffer is not enough");
        return CRYPTO_STATUS_INVALID_ARGUMENT;
    }

    memset(out, 0, *outlen);
    *outlen = datalen;

    EVP_CipherInit(&ctx, EVP_aes_128_gcm(), NULL, NULL, 0);
    EVP_CIPHER_CTX_ctrl(&ctx, EVP_CTRL_GCM_SET_IVLEN, ivlen, 0);
    EVP_CipherInit(&ctx, NULL, key, iv, ivlen);
    EVP_Cipher(&ctx, NULL, aad, aadlen);
    *outlen = EVP_Cipher(&ctx, out, data, datalen);
    EVP_CIPHER_CTX_ctrl(&ctx, EVP_CTRL_GCM_SET_TAG, taglen, tag);
    EVP_CIPHER_CTX_cleanup(&ctx);
#endif

    return CRYPTO_STATUS_SUCCESS;
}

CRYPTO_STATUS crypto_sha(uint8_t md_type, uint8_t *out, ...) {
    va_list args;

    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);
    if (md_type == MD_TYPE_SHA1) {
        SHA_CTX sha_ctx;
        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);
    } else if (md_type == MD_TYPE_SHA256) {
        SHA256_CTX sha_ctx;
        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);
    } else if (md_type == MD_TYPE_SHA384) {
        SHA512_CTX sha_ctx;
        SHA384_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) {
                SHA384_Update(&sha_ctx, chunk, chunk_len);
            }
        }
        va_end(args);
        SHA384_Final(out, &sha_ctx);
    } else if (md_type == MD_TYPE_SHA512) {
        SHA512_CTX sha_ctx;
        SHA512_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) {
                SHA512_Update(&sha_ctx, chunk, chunk_len);
            }
        }
        va_end(args);
        SHA512_Final(out, &sha_ctx);
    } else {
        va_end(args);
        LOGE("Not supported md_type");
        return CRYPTO_STATUS_FAILED;
    }

    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_cmac(uint8_t out[AES_BLOCK_SIZE], const uint8_t *key, const uint32_t keyLen, ...) {
    va_list args;
    AES_KEY akey;
    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, keyLen * 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, keyLen);

    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 if (md_type == MD_TYPE_SHA384) {
        ubfLen = SHA384_DIGEST_SIZE;
    } else if (md_type == MD_TYPE_SHA512) {
        ubfLen = SHA512_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 if (md_type == MD_TYPE_SHA384) {
        HMAC_Init_ex(&ctx, key, keyLen, EVP_sha384(), NULL);
    } else if (md_type == MD_TYPE_SHA512) {
        HMAC_Init_ex(&ctx, key, keyLen, EVP_sha512(), 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_cmp(const uint8_t *mac1, const uint8_t *mac2, const uint32_t macLen) {
    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 < macLen; 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;
    }
}