drivers: crypto: generic resources for crypto device driver - RSA

TEE Crypto generic APIs to HW driver interface

Signed-off-by: Cedric Neveux <cedric.neveux@nxp.com>
Acked-by: Jens Wiklander <jens.wiklander@linaro.org>

16 files changed, 1971 insertions, 6 deletions

diff --git a/core/arch/arm/plat-imx/conf.mk b/core/arch/arm/plat-imx/conf.mk
index 5b19c82d..c2278ffc 100644
--- a/core/arch/arm/plat-imx/conf.mk
+++ b/core/arch/arm/plat-imx/conf.mk
@@ -407,7 +407,9 @@ $(call force,CFG_IMX_CAAM,n)
 # it with generic crypto API can be enabled.
 CFG_CRYPTO_DRIVER ?= y
 # Crypto Driver Debug
-CFG_CRYPTO_DRIVER_DEBUG ?= n
+# DRV_DBG_TRACE BIT32(0) // Driver trace
+# DRV_DBG_BUF   BIT32(1) // Driver dump Buffer
+CFG_CRYPTO_DRIVER_DEBUG ?= 0
 else
 $(call force,CFG_CRYPTO_DRIVER,n)
 $(call force,CFG_WITH_SOFTWARE_PRNG,y)
diff --git a/core/drivers/crypto/crypto_api/acipher/local.h b/core/drivers/crypto/crypto_api/acipher/local.h
new file mode 100644
index 00000000..b7e22a81
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/acipher/local.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Definition of the functions shared locally.
+ */
+#ifndef __LOCAL_H__
+#define __LOCAL_H__
+
+#include <drvcrypt_acipher.h>
+
+/*
+ * Mask Generation function. Use a Hash operation
+ * to generate an output @mask from a input @seed
+ *
+ * @mgf_data  [in/out] MGF data
+ */
+TEE_Result drvcrypt_rsa_mgf1(struct drvcrypt_rsa_mgf *mgf_data);
+
+/*
+ * PKCS#1 - Signature of RSA message and encodes the signature.
+ *
+ * @ssa_data   [in/out] RSA data to sign / Signature
+ */
+TEE_Result drvcrypt_rsassa_sign(struct drvcrypt_rsa_ssa *ssa_data);
+
+/*
+ * PKCS#1 - Verification the encoded signature of RSA message.
+ *
+ * @ssa_data   RSA Encoded signature data
+ */
+TEE_Result drvcrypt_rsassa_verify(struct drvcrypt_rsa_ssa *ssa_data);
+
+#endif /* __LOCAL_H__ */
diff --git a/core/drivers/crypto/crypto_api/acipher/rsa.c b/core/drivers/crypto/crypto_api/acipher/rsa.c
new file mode 100644
index 00000000..ecf9d2f1
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/acipher/rsa.c
@@ -0,0 +1,458 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Crypto RSA interface implementation to enable HW driver.
+ */
+#include <drvcrypt.h>
+#include <crypto/crypto.h>
+#include <tee_api_defines_extensions.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+
+#include "local.h"
+
+TEE_Result crypto_acipher_alloc_rsa_keypair(struct rsa_keypair *key,
+					    size_t size_bits)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+
+	struct drvcrypt_rsa *rsa = NULL;
+
+	if (!key || !size_bits) {
+		CRYPTO_TRACE("Parameters error (key @%p) (size %zu bits)", key,
+			     size_bits);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa)
+		ret = rsa->alloc_keypair(key, size_bits);
+
+	CRYPTO_TRACE("RSA Keypair (%zu bits) alloc ret = 0x%" PRIx32, size_bits,
+		     ret);
+	return ret;
+}
+
+TEE_Result crypto_acipher_alloc_rsa_public_key(struct rsa_public_key *key,
+					       size_t size_bits)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+	struct drvcrypt_rsa *rsa = NULL;
+
+	if (!key || !size_bits) {
+		CRYPTO_TRACE("Parameters error (key @%p) (size %zu bits)", key,
+			     size_bits);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa)
+		ret = rsa->alloc_publickey(key, size_bits);
+
+	CRYPTO_TRACE("RSA Public Key (%zu bits) alloc ret = 0x%" PRIx32,
+		     size_bits, ret);
+	return ret;
+}
+
+void crypto_acipher_free_rsa_public_key(struct rsa_public_key *key)
+{
+	struct drvcrypt_rsa *rsa = NULL;
+
+	if (key) {
+		rsa = drvcrypt_get_ops(CRYPTO_RSA);
+		if (rsa) {
+			CRYPTO_TRACE("RSA Public Key free");
+			rsa->free_publickey(key);
+		}
+	}
+}
+
+TEE_Result crypto_acipher_gen_rsa_key(struct rsa_keypair *key, size_t size_bits)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+	struct drvcrypt_rsa *rsa = NULL;
+
+	if (!key || !size_bits) {
+		CRYPTO_TRACE("Parameters error (key @%p) (size %zu bits) ",
+			     key, size_bits);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa)
+		ret = rsa->gen_keypair(key, size_bits);
+
+	CRYPTO_TRACE("RSA Keypair (%zu bits) generate ret = 0x%" PRIx32,
+		     size_bits, ret);
+
+	return ret;
+}
+
+TEE_Result crypto_acipher_rsanopad_decrypt(struct rsa_keypair *key,
+					   const uint8_t *cipher,
+					   size_t cipher_len, uint8_t *msg,
+					   size_t *msg_len)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+	struct drvcrypt_rsa *rsa = NULL;
+	struct drvcrypt_rsa_ed rsa_data = { };
+
+	if (!key || !msg || !cipher || !msg_len) {
+		CRYPTO_TRACE("Parameters error (key @%p)\n"
+			     "(msg @%p size %zu bytes)\n"
+			     "(cipher @0%p size %zu bytes)",
+			     key, msg, *msg_len, cipher, cipher_len);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa_data.key.key = key;
+	rsa_data.key.isprivate = true;
+	rsa_data.key.n_size = crypto_bignum_num_bytes(key->n);
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		rsa_data.rsa_id = DRVCRYPT_RSA_NOPAD;
+		rsa_data.message.data = msg;
+		rsa_data.message.length = *msg_len;
+		rsa_data.cipher.data = (uint8_t *)cipher;
+		rsa_data.cipher.length = cipher_len;
+
+		ret = rsa->decrypt(&rsa_data);
+
+		*msg_len = rsa_data.message.length;
+	}
+
+	CRYPTO_TRACE("RSA Decrypt NO PAD ret = 0x%" PRIx32, ret);
+
+	return ret;
+}
+
+TEE_Result crypto_acipher_rsanopad_encrypt(struct rsa_public_key *key,
+					   const uint8_t *msg, size_t msg_len,
+					   uint8_t *cipher, size_t *cipher_len)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+	struct drvcrypt_rsa *rsa = NULL;
+	struct drvcrypt_rsa_ed rsa_data = { };
+
+	if (!key || !msg || !cipher || !cipher_len) {
+		CRYPTO_TRACE("Parameters error (key @%p)\n"
+			     "(msg @%p size %zu bytes)\n"
+			     "(cipher @%p size %zu bytes)",
+			     key, msg, msg_len, cipher, *cipher_len);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa_data.key.key = key;
+	rsa_data.key.isprivate = false;
+	rsa_data.key.n_size = crypto_bignum_num_bytes(key->n);
+
+	if (rsa_data.key.n_size > *cipher_len) {
+		CRYPTO_TRACE("Cipher length (%zu) too short expected %zu bytes",
+			     *cipher_len, rsa_data.key.n_size);
+		*cipher_len = rsa_data.key.n_size;
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the encryption data parameters */
+		rsa_data.rsa_id = DRVCRYPT_RSA_NOPAD;
+		rsa_data.message.data = (uint8_t *)msg;
+		rsa_data.message.length = msg_len;
+		rsa_data.cipher.data = cipher;
+		rsa_data.cipher.length = *cipher_len;
+
+		ret = rsa->encrypt(&rsa_data);
+
+		/* Set the cipher size */
+		*cipher_len = rsa_data.cipher.length;
+	}
+
+	CRYPTO_TRACE("RSA Encrypt NO PAD ret = 0x%" PRIx32, ret);
+
+	return ret;
+}
+
+TEE_Result crypto_acipher_rsaes_decrypt(uint32_t algo, struct rsa_keypair *key,
+					const uint8_t *label, size_t label_len,
+					const uint8_t *cipher,
+					size_t cipher_len, uint8_t *msg,
+					size_t *msg_len)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+	struct drvcrypt_rsa *rsa = NULL;
+	struct drvcrypt_rsa_ed rsa_data = { };
+
+	if (!key || !msg || !cipher || !msg_len || (!label && label_len)) {
+		CRYPTO_TRACE("Parameters error (key @%p)\n"
+			     "(msg @%p size %zu bytes)\n"
+			     "(cipher @%p size %zu bytes)\n"
+			     "(label @%p size %zu bytes)",
+			     key, msg, *msg_len, cipher, cipher_len, label,
+			     label_len);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the encryption data parameters */
+		if (algo == TEE_ALG_RSAES_PKCS1_V1_5) {
+			rsa_data.rsa_id = DRVCRYPT_RSA_PKCS_V1_5;
+		} else {
+			rsa_data.rsa_id = DRVCRYPT_RSA_OAEP;
+			rsa_data.hash_algo = TEE_INTERNAL_HASH_TO_ALGO(algo);
+
+			ret = tee_hash_get_digest_size(rsa_data.hash_algo,
+						       &rsa_data.digest_size);
+			if (ret != TEE_SUCCESS)
+				return ret;
+
+			rsa_data.mgf = &drvcrypt_rsa_mgf1;
+		}
+
+		rsa_data.key.key = key;
+		rsa_data.key.isprivate = true;
+		rsa_data.key.n_size = crypto_bignum_num_bytes(key->n);
+
+		rsa_data.message.data = msg;
+		rsa_data.message.length = *msg_len;
+		rsa_data.cipher.data = (uint8_t *)cipher;
+		rsa_data.cipher.length = cipher_len;
+		rsa_data.label.data =
+			((label_len > 0) ? (uint8_t *)label : NULL);
+		rsa_data.label.length = label_len;
+
+		ret = rsa->decrypt(&rsa_data);
+
+		/* Set the message size */
+		*msg_len = rsa_data.message.length;
+	}
+
+	CRYPTO_TRACE("RSAES Decrypt ret = 0x%" PRIx32, ret);
+
+	return ret;
+}
+
+TEE_Result crypto_acipher_rsaes_encrypt(uint32_t algo,
+					struct rsa_public_key *key,
+					const uint8_t *label, size_t label_len,
+					const uint8_t *msg, size_t msg_len,
+					uint8_t *cipher, size_t *cipher_len)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+	struct drvcrypt_rsa *rsa = NULL;
+	struct drvcrypt_rsa_ed rsa_data = { };
+
+	if (!key || !msg || !cipher || !cipher_len || (!label && label_len)) {
+		CRYPTO_TRACE("Parameters error (key @%p\n"
+			     "(msg @%p size %zu bytes)\n"
+			     "(cipher @%p size %zu bytes)\n"
+			     "(label @%p size %zu bytes)",
+			     key, msg, msg_len, cipher, *cipher_len, label,
+			     label_len);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	rsa_data.key.key = key;
+	rsa_data.key.isprivate = false;
+	rsa_data.key.n_size = crypto_bignum_num_bytes(key->n);
+
+	if (rsa_data.key.n_size > *cipher_len) {
+		CRYPTO_TRACE("Cipher length (%zu) too short expected %zu bytes",
+			     *cipher_len, rsa_data.key.n_size);
+		*cipher_len = rsa_data.key.n_size;
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the encryption data parameters */
+		if (algo == TEE_ALG_RSAES_PKCS1_V1_5) {
+			rsa_data.rsa_id = DRVCRYPT_RSA_PKCS_V1_5;
+
+			/* Message length <= (modulus_size - 11) */
+			if (msg_len > rsa_data.key.n_size - 11)
+				return TEE_ERROR_BAD_PARAMETERS;
+
+		} else {
+			rsa_data.rsa_id = DRVCRYPT_RSA_OAEP;
+			rsa_data.hash_algo = TEE_INTERNAL_HASH_TO_ALGO(algo);
+
+			/* Message length <= (modulus_size - 2 * hLength - 2) */
+			ret = tee_hash_get_digest_size(rsa_data.hash_algo,
+						       &rsa_data.digest_size);
+			if (ret != TEE_SUCCESS)
+				return ret;
+
+			if (msg_len >
+			    rsa_data.key.n_size - 2 * rsa_data.digest_size - 2)
+				return TEE_ERROR_BAD_PARAMETERS;
+
+			rsa_data.mgf = &drvcrypt_rsa_mgf1;
+		}
+
+		rsa_data.message.data = (uint8_t *)msg;
+		rsa_data.message.length = msg_len;
+		rsa_data.cipher.data = cipher;
+		rsa_data.cipher.length = rsa_data.key.n_size;
+		rsa_data.label.data = (label_len > 0) ? (uint8_t *)label : NULL;
+		rsa_data.label.length = label_len;
+
+		ret = rsa->encrypt(&rsa_data);
+
+		/* Set the cipher size */
+		*cipher_len = rsa_data.cipher.length;
+	}
+
+	CRYPTO_TRACE("RSAES Encrypt ret = 0x%" PRIx32, ret);
+
+	return ret;
+}
+
+TEE_Result crypto_acipher_rsassa_sign(uint32_t algo, struct rsa_keypair *key,
+				      int salt_len, const uint8_t *msg,
+				      size_t msg_len, uint8_t *sig,
+				      size_t *sig_len)
+{
+	TEE_Result ret = TEE_ERROR_BAD_PARAMETERS;
+	struct drvcrypt_rsa *rsa = NULL;
+	struct drvcrypt_rsa_ssa rsa_ssa = { };
+
+	if (!key || !msg || !sig || !sig_len) {
+		CRYPTO_TRACE("Input parameters reference error");
+		return ret;
+	}
+
+	if (algo != TEE_ALG_RSASSA_PKCS1_V1_5) {
+		/* Prepare the Digest */
+		rsa_ssa.hash_algo = TEE_DIGEST_HASH_TO_ALGO(algo);
+
+		/* Check if the message length is digest hash size */
+		ret = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+					       &rsa_ssa.digest_size);
+		if (ret != TEE_SUCCESS)
+			return ret;
+
+		if (msg_len != rsa_ssa.digest_size) {
+			CRYPTO_TRACE("Msg length (%zu expected %zu)", msg_len,
+				     rsa_ssa.digest_size);
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	} else {
+		rsa_ssa.hash_algo = 0;
+		rsa_ssa.digest_size = 0;
+	}
+
+	/* Prepare the Key */
+	rsa_ssa.key.key = key;
+	rsa_ssa.key.isprivate = true;
+	rsa_ssa.key.n_size = crypto_bignum_num_bytes(key->n);
+
+	if (rsa_ssa.key.n_size > *sig_len) {
+		CRYPTO_TRACE("Sign length (%zu) too short must be %zu bytes",
+			     *sig_len, rsa_ssa.key.n_size);
+		*sig_len = rsa_ssa.key.n_size;
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the Encoded Signature structure data */
+		rsa_ssa.algo = algo;
+		rsa_ssa.message.data = (uint8_t *)msg;
+		rsa_ssa.message.length = msg_len;
+		rsa_ssa.signature.data = (uint8_t *)sig;
+		rsa_ssa.signature.length = rsa_ssa.key.n_size;
+		rsa_ssa.salt_len = salt_len;
+		rsa_ssa.mgf = &drvcrypt_rsa_mgf1;
+
+		if (rsa->optional.ssa_sign)
+			ret = rsa->optional.ssa_sign(&rsa_ssa);
+		else
+			ret = drvcrypt_rsassa_sign(&rsa_ssa);
+
+		/* Set the signature length */
+		*sig_len = rsa_ssa.signature.length;
+	} else {
+		ret = TEE_ERROR_NOT_IMPLEMENTED;
+	}
+
+	CRYPTO_TRACE("Encode signature algo (0x%" PRIx32
+		     ") returned 0x%" PRIx32,
+		     algo, ret);
+	return ret;
+}
+
+TEE_Result crypto_acipher_rsassa_verify(uint32_t algo,
+					struct rsa_public_key *key,
+					int salt_len, const uint8_t *msg,
+					size_t msg_len, const uint8_t *sig,
+					size_t sig_len)
+{
+	TEE_Result ret = TEE_ERROR_BAD_PARAMETERS;
+	struct drvcrypt_rsa *rsa = NULL;
+	struct drvcrypt_rsa_ssa rsa_ssa = { };
+
+	if (!key || !msg || !sig) {
+		CRYPTO_TRACE("Input parameters reference error");
+		return ret;
+	}
+
+	if (algo != TEE_ALG_RSASSA_PKCS1_V1_5) {
+		/* Prepare the Digest */
+		rsa_ssa.hash_algo = TEE_DIGEST_HASH_TO_ALGO(algo);
+
+		/* Check if the message length is digest hash size */
+		ret = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+					       &rsa_ssa.digest_size);
+		if (ret != TEE_SUCCESS)
+			return ret;
+
+		if (msg_len != rsa_ssa.digest_size) {
+			CRYPTO_TRACE("Input msg length (%zu expected %zu)",
+				     msg_len, rsa_ssa.digest_size);
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	} else {
+		rsa_ssa.hash_algo = 0;
+		rsa_ssa.digest_size = 0;
+	}
+
+	/* Prepare the Key */
+	rsa_ssa.key.key = key;
+	rsa_ssa.key.isprivate = false;
+	rsa_ssa.key.n_size = crypto_bignum_num_bytes(key->n);
+
+	if (rsa_ssa.key.n_size > sig_len) {
+		CRYPTO_TRACE("Signature length expected %zu",
+			     rsa_ssa.key.n_size);
+		return TEE_ERROR_SIGNATURE_INVALID;
+	}
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the Encoded Signature structure data */
+		rsa_ssa.algo = algo;
+		rsa_ssa.message.data = (uint8_t *)msg;
+		rsa_ssa.message.length = msg_len;
+		rsa_ssa.signature.data = (uint8_t *)sig;
+		rsa_ssa.signature.length = sig_len;
+		rsa_ssa.salt_len = salt_len;
+		rsa_ssa.mgf = &drvcrypt_rsa_mgf1;
+
+		if (rsa->optional.ssa_verify)
+			ret = rsa->optional.ssa_verify(&rsa_ssa);
+		else
+			ret = drvcrypt_rsassa_verify(&rsa_ssa);
+
+	} else {
+		ret = TEE_ERROR_NOT_IMPLEMENTED;
+	}
+
+	CRYPTO_TRACE("Signature verif algo (0x%" PRIx32 ") returned 0x%" PRIx32,
+		     algo, ret);
+
+	return ret;
+}
diff --git a/core/drivers/crypto/crypto_api/acipher/rsamgf.c b/core/drivers/crypto/crypto_api/acipher/rsamgf.c
new file mode 100644
index 00000000..1663e51d
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/acipher/rsamgf.c
@@ -0,0 +1,105 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * RSA Mask Generation function implementation.
+ */
+#include <drvcrypt.h>
+#include <malloc.h>
+#include <string.h>
+#include <utee_defines.h>
+
+#include "local.h"
+
+TEE_Result drvcrypt_rsa_mgf1(struct drvcrypt_rsa_mgf *mgf_data)
+{
+	TEE_Result ret = TEE_ERROR_GENERIC;
+	void *ctx = NULL;
+	size_t lastBlock_size = 0;
+	size_t nbBlock = 0;
+	uint32_t counter = 0;
+	uint32_t swapcount = 0;
+	uint8_t *cur_mask = mgf_data->mask.data;
+	uint8_t *tmpdigest = NULL;
+
+	CRYPTO_TRACE("Generate Mask (%zu bytes) with seed of %zu bytes",
+		     mgf_data->mask.length, mgf_data->seed.length);
+
+	/* Calculate the number of complet hash digest */
+	lastBlock_size = mgf_data->mask.length % mgf_data->digest_size;
+	if (lastBlock_size) {
+		/* Allocate a digest buffer for the last block */
+		tmpdigest = malloc(mgf_data->digest_size);
+		if (!tmpdigest)
+			return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	/* Allocate the Hash Context */
+	ret = crypto_hash_alloc_ctx(&ctx, mgf_data->hash_algo);
+	if (ret != TEE_SUCCESS)
+		goto exit_mgf;
+
+	nbBlock = (mgf_data->mask.length - lastBlock_size) /
+		  mgf_data->digest_size;
+
+	CRYPTO_TRACE("Nb Loop (%zu bytes) = %zu, last Block = %zu bytes",
+		     mgf_data->digest_size, nbBlock, lastBlock_size);
+
+	for (; counter < nbBlock;
+	     counter++, cur_mask += mgf_data->digest_size) {
+		swapcount = TEE_U32_TO_BIG_ENDIAN(counter);
+
+		ret = crypto_hash_init(ctx);
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		ret = crypto_hash_update(ctx, mgf_data->seed.data,
+					 mgf_data->seed.length);
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		ret = crypto_hash_update(ctx, (uint8_t *)&swapcount,
+					 sizeof(swapcount));
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		ret = crypto_hash_final(ctx, cur_mask, mgf_data->digest_size);
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+	}
+
+	if (lastBlock_size) {
+		CRYPTO_TRACE("Last Block = %zu bytes", lastBlock_size);
+
+		swapcount = TEE_U32_TO_BIG_ENDIAN(counter);
+
+		ret = crypto_hash_init(ctx);
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		ret = crypto_hash_update(ctx, mgf_data->seed.data,
+					 mgf_data->seed.length);
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		ret = crypto_hash_update(ctx, (uint8_t *)&swapcount,
+					 sizeof(swapcount));
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		ret = crypto_hash_final(ctx, tmpdigest, mgf_data->digest_size);
+		if (ret != TEE_SUCCESS)
+			goto exit_mgf;
+
+		memcpy(cur_mask, tmpdigest, lastBlock_size);
+	}
+
+	ret = TEE_SUCCESS;
+
+exit_mgf:
+	crypto_hash_free_ctx(ctx);
+	free(tmpdigest);
+
+	CRYPTO_TRACE("return 0x%08" PRIx32, ret);
+	return ret;
+}
diff --git a/core/drivers/crypto/crypto_api/acipher/rsassa.c b/core/drivers/crypto/crypto_api/acipher/rsassa.c
new file mode 100644
index 00000000..8d589a2f
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/acipher/rsassa.c
@@ -0,0 +1,916 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * RSA Signature Software common implementation.
+ * Functions preparing and/or verifying the signature
+ * encoded string.
+ *
+ * PKCS #1 v2.1: RSA Cryptography Standard
+ * https://www.ietf.org/rfc/rfc3447.txt
+ */
+#include <crypto/crypto.h>
+#include <drvcrypt.h>
+#include <drvcrypt_asn1_oid.h>
+#include <drvcrypt_math.h>
+#include <malloc.h>
+#include <string.h>
+#include <tee_api_defines_extensions.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+#include <util.h>
+
+#include "local.h"
+
+/*
+ * PKCS#1 V1.5 - Encode the message in Distinguished Encoding Rules
+ * (DER) format.
+ * Refer to EMSA-PKCS1-v1_5 chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data  RSA data to encode
+ * @EM        [out] Encoded Message
+ */
+static TEE_Result emsa_pkcs1_v1_5_encode(struct drvcrypt_rsa_ssa *ssa_data,
+					 struct drvcrypt_buf *EM)
+{
+	const struct drvcrypt_oid *hash_oid = NULL;
+	size_t ps_size = 0;
+	uint8_t *buf = NULL;
+
+	hash_oid = drvcrypt_get_alg_hash_oid(ssa_data->hash_algo);
+	if (!hash_oid)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	/*
+	 * Calculate the PS size
+	 *  EM Size (modulus size) - 3 bytes - DigestInfo DER format size
+	 */
+	ps_size = ssa_data->key.n_size - 3;
+	ps_size -= ssa_data->digest_size;
+	ps_size -= 10 + hash_oid->asn1_length;
+
+	CRYPTO_TRACE("PS size = %zu (n %zu)", ps_size, ssa_data->key.n_size);
+
+	/*
+	 * EM = 0x00 || 0x01 || PS || 0x00 || T
+	 *
+	 * where T represent the message DigestInfo in DER:
+	 *    DigestInfo ::= SEQUENCE {
+	 *                digestAlgorithm AlgorithmIdentifier,
+	 *                digest OCTET STRING
+	 *                }
+	 *
+	 * T  Length = digest length + oid length
+	 * EM Length = T Length + 11 + PS Length
+	 */
+	buf = EM->data;
+
+	/* Set the EM first byte to 0x00 */
+	*buf++ = 0x00;
+
+	/* Set the EM second byte to 0x01 */
+	*buf++ = 0x01;
+
+	/* Fill PS with 0xFF */
+	memset(buf, UINT8_MAX, ps_size);
+	buf += ps_size;
+
+	/* Set the Byte after PS to 0x00 */
+	*buf++ = 0x00;
+
+	/*
+	 * Create the DigestInfo DER Sequence
+	 *
+	 *  DigestInfo ::= SEQUENCE {
+	 *                digestAlgorithm AlgorithmIdentifier,
+	 *                digest OCTET STRING
+	 *                }
+	 *
+	 */
+	/* SEQUENCE { */
+	*buf++ = DRVCRYPT_ASN1_SEQUENCE | DRVCRYPT_ASN1_CONSTRUCTED;
+	*buf++ = 0x08 + hash_oid->asn1_length + ssa_data->digest_size;
+
+	/* digestAlgorithm AlgorithmIdentifier */
+	*buf++ = DRVCRYPT_ASN1_SEQUENCE | DRVCRYPT_ASN1_CONSTRUCTED;
+	*buf++ = 0x04 + hash_oid->asn1_length;
+	*buf++ = DRVCRYPT_ASN1_OID;
+	*buf++ = hash_oid->asn1_length;
+
+	/* digest OCTET STRING */
+	memcpy(buf, hash_oid->asn1, hash_oid->asn1_length);
+	buf += hash_oid->asn1_length;
+	*buf++ = DRVCRYPT_ASN1_NULL;
+	*buf++ = 0x00;
+	*buf++ = DRVCRYPT_ASN1_OCTET_STRING;
+	*buf++ = ssa_data->digest_size;
+	/* } */
+
+	memcpy(buf, ssa_data->message.data, ssa_data->digest_size);
+
+	CRYPTO_DUMPBUF("Encoded Message", EM->data, (size_t)EM->length);
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * PKCS#1 V1.5 - Encode the message in Distinguished Encoding Rules
+ * (DER) format.
+ * Refer to EMSA-PKCS1-v1_5 chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data  RSA data to encode
+ * @EM        [out] Encoded Message
+ */
+static TEE_Result
+emsa_pkcs1_v1_5_encode_noasn1(struct drvcrypt_rsa_ssa *ssa_data,
+			      struct drvcrypt_buf *EM)
+{
+	size_t ps_size = 0;
+	uint8_t *buf = NULL;
+
+	/*
+	 * Calculate the PS size
+	 *  EM Size (modulus size) - 3 bytes - Message Length
+	 */
+	ps_size = ssa_data->key.n_size - 3;
+
+	if (ps_size < ssa_data->message.length)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ps_size -= ssa_data->message.length;
+
+	CRYPTO_TRACE("PS size = %zu (n %zu)", ps_size, ssa_data->key.n_size);
+
+	/*
+	 * EM = 0x00 || 0x01 || PS || 0x00 || T
+	 *
+	 * T  Length = message length
+	 * EM Length = T Length + PS Length
+	 */
+	buf = EM->data;
+
+	/* Set the EM first byte to 0x00 */
+	*buf++ = 0x00;
+
+	/* Set the EM second byte to 0x01 */
+	*buf++ = 0x01;
+
+	/* Fill PS with 0xFF */
+	memset(buf, UINT8_MAX, ps_size);
+	buf += ps_size;
+
+	/* Set the Byte after PS to 0x00 */
+	*buf++ = 0x00;
+
+	memcpy(buf, ssa_data->message.data, ssa_data->message.length);
+
+	CRYPTO_DUMPBUF("Encoded Message", EM->data, EM->length);
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * PKCS#1 V1.5 - Signature of RSA message and encodes the signature.
+ * Refer to RSASSA-PKCS1-v1_5 chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data   [in/out] RSA data to sign / Signature
+ */
+static TEE_Result rsassa_pkcs1_v1_5_sign(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	TEE_Result ret = TEE_ERROR_BAD_PARAMETERS;
+	struct drvcrypt_buf EM = { };
+	struct drvcrypt_rsa_ed rsa_data = { };
+	struct drvcrypt_rsa *rsa = NULL;
+
+	EM.length = ssa_data->key.n_size;
+	EM.data = malloc(EM.length);
+	if (!EM.data)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	/* Encode the Message */
+	if (ssa_data->algo != TEE_ALG_RSASSA_PKCS1_V1_5)
+		ret = emsa_pkcs1_v1_5_encode(ssa_data, &EM);
+	else
+		ret = emsa_pkcs1_v1_5_encode_noasn1(ssa_data, &EM);
+
+	if (ret != TEE_SUCCESS)
+		goto out;
+
+	/*
+	 * RSA Encrypt/Decrypt are doing the same operation except
+	 * that decrypt takes a RSA Private key in parameter
+	 */
+	rsa_data.key.key = ssa_data->key.key;
+	rsa_data.key.isprivate = true;
+	rsa_data.key.n_size = ssa_data->key.n_size;
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (!rsa) {
+		ret = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	/* Prepare the decryption data parameters */
+	rsa_data.rsa_id = DRVCRYPT_RSASSA_PKCS_V1_5;
+	rsa_data.message.data = ssa_data->signature.data;
+	rsa_data.message.length = ssa_data->signature.length;
+	rsa_data.cipher.data = EM.data;
+	rsa_data.cipher.length = EM.length;
+
+	ret = rsa->decrypt(&rsa_data);
+
+	/* Set the message decrypted size */
+	ssa_data->signature.length = rsa_data.message.length;
+
+out:
+	free(EM.data);
+
+	return ret;
+}
+
+/*
+ * PKCS#1 V1.5 - Verification of the RSA message's signature.
+ * Refer to RSASSA-PKCS1-v1_5 chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data   [int/out] RSA data signed and encoded signature
+ */
+static TEE_Result rsassa_pkcs1_v1_5_verify(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	TEE_Result ret = TEE_ERROR_BAD_PARAMETERS;
+	struct drvcrypt_buf EM = { };
+	struct drvcrypt_buf EM_gen = { };
+	struct drvcrypt_rsa_ed rsa_data = { };
+	struct drvcrypt_rsa *rsa = NULL;
+
+	EM.length = ssa_data->key.n_size;
+	EM.data = malloc(EM.length);
+
+	EM_gen.length = ssa_data->key.n_size;
+	EM_gen.data = malloc(EM.length);
+
+	if (!EM.data || !EM_gen.data) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto end_verify;
+	}
+
+	/*
+	 * RSA Encrypt/Decrypt are doing the same operation except
+	 * that the encrypt takes a RSA Public key in parameter
+	 */
+	rsa_data.key.key = ssa_data->key.key;
+	rsa_data.key.isprivate = false;
+	rsa_data.key.n_size = ssa_data->key.n_size;
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the encryption data parameters */
+		rsa_data.rsa_id = DRVCRYPT_RSASSA_PKCS_V1_5;
+		rsa_data.message.data = ssa_data->signature.data;
+		rsa_data.message.length = ssa_data->signature.length;
+		rsa_data.cipher.data = EM.data;
+		rsa_data.cipher.length = EM.length;
+
+		ret = rsa->encrypt(&rsa_data);
+
+		/* Set the cipher size */
+		EM.length = rsa_data.cipher.length;
+	} else {
+		ret = TEE_ERROR_NOT_IMPLEMENTED;
+	}
+
+	if (ret != TEE_SUCCESS)
+		goto end_verify;
+
+	/* Encode the Message */
+	if (ssa_data->algo != TEE_ALG_RSASSA_PKCS1_V1_5)
+		ret = emsa_pkcs1_v1_5_encode(ssa_data, &EM_gen);
+	else
+		ret = emsa_pkcs1_v1_5_encode_noasn1(ssa_data, &EM_gen);
+
+	if (ret != TEE_SUCCESS)
+		goto end_verify;
+
+	/* Check if EM decrypted and EM re-generated are identical */
+	ret = TEE_ERROR_SIGNATURE_INVALID;
+	if (EM.length == EM_gen.length) {
+		if (!memcmp(EM.data, EM_gen.data, EM.length))
+			ret = TEE_SUCCESS;
+	}
+
+end_verify:
+	free(EM.data);
+	free(EM_gen.data);
+
+	return ret;
+}
+
+/*
+ * PSS - Encode the message using a Probabilistic Signature Scheme (PSS)
+ * Refer to EMSA-PSS (encoding) chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data  RSA data to encode
+ * @emBits    EM size in bits
+ * @EM        [out] Encoded Message
+ */
+static TEE_Result emsa_pss_encode(struct drvcrypt_rsa_ssa *ssa_data,
+				  size_t emBits, struct drvcrypt_buf *EM)
+{
+	TEE_Result ret = TEE_ERROR_GENERIC;
+	struct drvcrypt_rsa_mgf mgf_data = { };
+	struct drvcrypt_buf hash = { };
+	struct drvcrypt_buf dbMask = { };
+	struct drvcrypt_buf DB = { };
+	size_t db_size = 0;
+	size_t ps_size = 0;
+	size_t msg_size = 0;
+	uint8_t *buf = NULL;
+	uint8_t *msg_db = NULL;
+	uint8_t *salt = NULL;
+	struct drvcrypt_mod_op mod_op = { };
+
+	/*
+	 * Build EM = maskedDB || H || 0xbc
+	 *
+	 * where
+	 *    maskedDB = DB xor dbMask
+	 *       DB     = PS || 0x01 || salt
+	 *       dbMask = MGF(H, emLen - hLen - 1)
+	 *
+	 *    H  = Hash(M')
+	 *       M' = (0x)00 00 00 00 00 00 00 00 || mHash || salt
+	 *
+	 * PS size = emLen - sLen - hLen - 2 (may be = 0)
+	 */
+
+	/*
+	 * Calculate the M' and DB size to allocate a temporary buffer
+	 * used for both object
+	 */
+	ps_size = EM->length - ssa_data->digest_size - ssa_data->salt_len - 2;
+	db_size = EM->length - ssa_data->digest_size - 1;
+	msg_size = 8 + ssa_data->digest_size + ssa_data->salt_len;
+
+	CRYPTO_TRACE("PS Len = %zu, DB Len = %zu, M' Len = %zu", ps_size,
+		     db_size, msg_size);
+
+	msg_db = malloc(MAX(db_size, msg_size));
+	if (!msg_db)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	if (ssa_data->salt_len) {
+		salt = malloc(ssa_data->salt_len);
+
+		if (!salt) {
+			ret = TEE_ERROR_OUT_OF_MEMORY;
+			goto end_pss_encode;
+		}
+	}
+
+	/*
+	 * Step 4 and 5
+	 * Generate the M' = (0x)00 00 00 00 00 00 00 00 || mHash || salt
+	 *
+	 * where
+	 *   mHash is the input message (already hash)
+	 *   salt is a random number of salt_len (input data) can be empty
+	 */
+	buf = msg_db;
+
+	memset(buf, 0, 8);
+	buf += 8;
+
+	memcpy(buf, ssa_data->message.data, ssa_data->message.length);
+	buf += ssa_data->message.length;
+
+	/* Get salt random number if salt length not 0 */
+	if (ssa_data->salt_len) {
+		ret = crypto_rng_read(salt, ssa_data->salt_len);
+		CRYPTO_TRACE("Get salt of %zu bytes (ret = 0x%08" PRIx32 ")",
+			     ssa_data->salt_len, ret);
+		if (ret != TEE_SUCCESS)
+			goto end_pss_encode;
+
+		memcpy(buf, salt, ssa_data->salt_len);
+	}
+
+	/*
+	 * Step 6
+	 * Hash the M' generated new message
+	 * H = hash(M')
+	 */
+	hash.data = &EM->data[db_size];
+	hash.length = ssa_data->digest_size;
+
+	ret = tee_hash_createdigest(ssa_data->hash_algo, msg_db, msg_size,
+				    hash.data, hash.length);
+
+	CRYPTO_TRACE("H = hash(M') returned 0x%08" PRIx32, ret);
+	if (ret != TEE_SUCCESS)
+		goto end_pss_encode;
+
+	CRYPTO_DUMPBUF("H = hash(M')", hash.data, hash.length);
+
+	/*
+	 * Step 7 and 8
+	 *   DB = PS || 0x01 || salt
+	 */
+	buf = msg_db;
+	if (ps_size)
+		memset(buf, 0, ps_size);
+	buf += ps_size;
+	*buf++ = 0x01;
+
+	if (ssa_data->salt_len)
+		memcpy(buf, salt, ssa_data->salt_len);
+
+	DB.data = msg_db;
+	DB.length = db_size;
+
+	CRYPTO_DUMPBUF("DB", DB.data, DB.length);
+
+	/*
+	 * Step 9
+	 * Generate a Mask of the seed value
+	 * dbMask = MGF(H, emLen - hLen - 1)
+	 *
+	 * Note: Will use the same buffer for the dbMask and maskedDB
+	 *       maskedDB is in the EM output
+	 */
+	dbMask.data = EM->data;
+	dbMask.length = db_size;
+
+	mgf_data.hash_algo = ssa_data->hash_algo;
+	mgf_data.digest_size = ssa_data->digest_size;
+	mgf_data.seed.data = hash.data;
+	mgf_data.seed.length = hash.length;
+	mgf_data.mask.data = dbMask.data;
+	mgf_data.mask.length = dbMask.length;
+	ret = ssa_data->mgf(&mgf_data);
+
+	CRYPTO_TRACE("dbMask = MGF(H, emLen - hLen - 1) returned 0x%08" PRIx32,
+		     ret);
+	if (ret != TEE_SUCCESS)
+		goto end_pss_encode;
+
+	CRYPTO_DUMPBUF("dbMask", dbMask.data, dbMask.length);
+
+	/*
+	 * Step 10
+	 * maskedDB = DB xor dbMask
+	 */
+	mod_op.n.length = dbMask.length;
+	mod_op.a.data = DB.data;
+	mod_op.a.length = DB.length;
+	mod_op.b.data = dbMask.data;
+	mod_op.b.length = dbMask.length;
+	mod_op.result.data = dbMask.data;
+	mod_op.result.length = dbMask.length;
+
+	ret = drvcrypt_xor_mod_n(&mod_op);
+	CRYPTO_TRACE("maskedDB = DB xor dbMask returned 0x%08" PRIx32, ret);
+	if (ret != TEE_SUCCESS)
+		goto end_pss_encode;
+
+	CRYPTO_DUMPBUF("maskedDB", dbMask.data, dbMask.length);
+
+	/*
+	 * Step 11
+	 * Set the leftmost 8emLen - emBits of the leftmost octet
+	 * in maskedDB to 0'
+	 */
+	EM->data[0] &= (UINT8_MAX >> ((EM->length * 8) - emBits));
+
+	/*
+	 * Step 12
+	 * EM = maskedDB || H || 0xbc
+	 */
+	EM->data[EM->length - 1] = 0xbc;
+
+	CRYPTO_DUMPBUF("EM", EM->data, EM->length);
+
+	ret = TEE_SUCCESS;
+end_pss_encode:
+	free(msg_db);
+	free(salt);
+
+	return ret;
+}
+
+/*
+ * PSS - Verify the message using a Probabilistic Signature Scheme (PSS)
+ * Refer to EMSA-PSS (verification) chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data  RSA data to encode
+ * @emBits    EM size in bits
+ * @EM        [out] Encoded Message
+ */
+static TEE_Result emsa_pss_verify(struct drvcrypt_rsa_ssa *ssa_data,
+				  size_t emBits, struct drvcrypt_buf *EM)
+{
+	TEE_Result ret = TEE_ERROR_GENERIC;
+	struct drvcrypt_rsa_mgf mgf_data = { };
+	struct drvcrypt_buf hash = { };
+	struct drvcrypt_buf hash_gen = { };
+	size_t db_size = 0;
+	size_t ps_size = 0;
+	size_t msg_size = 0;
+	uint8_t *msg_db = NULL;
+	uint8_t *salt = NULL;
+	uint8_t *buf = NULL;
+	struct drvcrypt_mod_op mod_op = { };
+
+	/*
+	 * EM = maskedDB || H || 0xbc
+	 *
+	 * where
+	 *    maskedDB = DB xor dbMask
+	 *       DB     = PS || 0x01 || salt
+	 *       dbMask = MGF(H, emLen - hLen - 1)
+	 *
+	 *    H  = Hash(M')
+	 *       M' = (0x)00 00 00 00 00 00 00 00 || mHash || salt
+	 *
+	 * PS size = emLen - sLen - hLen - 2 (may be = 0)
+	 */
+
+	/*
+	 * Calculate the M' and DB size to allocate a temporary buffer
+	 * used for both object
+	 */
+	ps_size = EM->length - ssa_data->digest_size - ssa_data->salt_len - 2;
+	db_size = EM->length - ssa_data->digest_size - 1;
+	msg_size = 8 + ssa_data->digest_size + ssa_data->salt_len;
+
+	CRYPTO_TRACE("PS Len = %zu, DB Len = %zu, M' Len = %zu", ps_size,
+		     db_size, msg_size);
+
+	msg_db = malloc(MAX(db_size, msg_size));
+	if (!msg_db)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	/*
+	 * Step 4
+	 * Check if rightmost octet of EM is 0xbc
+	 */
+	if (EM->data[EM->length - 1] != 0xbc) {
+		CRYPTO_TRACE("rigthmost octet != 0xbc (0x%" PRIX8 ")",
+			     EM->data[EM->length - 1]);
+		ret = TEE_ERROR_SIGNATURE_INVALID;
+		goto end_pss_verify;
+	}
+
+	/*
+	 * Step 6
+	 * Check if the leftmost 8emLen - emBits of the leftmost octet
+	 * in maskedDB are 0's
+	 */
+	if (EM->data[0] & ~(UINT8_MAX >> (EM->length * 8 - emBits))) {
+		CRYPTO_TRACE("Error leftmost octet maskedDB not 0's");
+		CRYPTO_TRACE("EM[0] = 0x%" PRIX8
+			     " - EM Len = %zu, emBits = %zu",
+			     EM->data[0], EM->length, emBits);
+		ret = TEE_ERROR_SIGNATURE_INVALID;
+		goto end_pss_verify;
+	}
+
+	hash.data = &EM->data[db_size];
+	hash.length = ssa_data->digest_size;
+
+	/*
+	 * Step 7
+	 * dbMask = MGF(H, emLen - hLen - 1)
+	 *
+	 * Note: Will use the same buffer for the dbMask and DB
+	 */
+	mgf_data.hash_algo = ssa_data->hash_algo;
+	mgf_data.digest_size = ssa_data->digest_size;
+	mgf_data.seed.data = hash.data;
+	mgf_data.seed.length = hash.length;
+	mgf_data.mask.data = msg_db;
+	mgf_data.mask.length = db_size;
+	ret = ssa_data->mgf(&mgf_data);
+
+	CRYPTO_TRACE("dbMask = MGF(H, emLen - hLen - 1) returned 0x%08" PRIx32,
+		     ret);
+	if (ret != TEE_SUCCESS)
+		goto end_pss_verify;
+
+	CRYPTO_DUMPBUF("dbMask", msg_db, db_size);
+
+	/*
+	 * Step 8
+	 * DB = maskedDB xor dbMask
+	 *
+	 *
+	 * Note: maskedDB is in the EM input
+	 */
+	mod_op.n.length = db_size;
+	mod_op.a.data = EM->data;
+	mod_op.a.length = db_size;
+	mod_op.b.data = msg_db;
+	mod_op.b.length = db_size;
+	mod_op.result.data = msg_db;
+	mod_op.result.length = db_size;
+
+	ret = drvcrypt_xor_mod_n(&mod_op);
+	CRYPTO_TRACE("DB = maskedDB xor dbMask returned 0x%08" PRIx32, ret);
+	if (ret != TEE_SUCCESS)
+		goto end_pss_verify;
+
+	/*
+	 * Step 9
+	 * Set the leftmost 8emLen - emBits of the leftmost octet in
+	 * DB to zero
+	 */
+	*msg_db &= UINT8_MAX >> (EM->length * 8 - emBits);
+
+	CRYPTO_DUMPBUF("DB", msg_db, db_size);
+
+	/*
+	 * Step 10
+	 * Expected to have
+	 *       DB     = PS || 0x01 || salt
+	 *
+	 * PS must be 0
+	 * PS size = emLen - sLen - hLen - 2 (may be = 0)
+	 */
+	buf = msg_db;
+	while (buf < msg_db + ps_size) {
+		if (*buf++ != 0) {
+			ret = TEE_ERROR_SIGNATURE_INVALID;
+			goto end_pss_verify;
+		}
+	}
+
+	if (*buf++ != 0x01) {
+		ret = TEE_ERROR_SIGNATURE_INVALID;
+		goto end_pss_verify;
+	}
+
+	/*
+	 * Step 11
+	 * Get the salt value
+	 */
+	if (ssa_data->salt_len) {
+		salt = malloc(ssa_data->salt_len);
+		if (!salt) {
+			ret = TEE_ERROR_OUT_OF_MEMORY;
+			goto end_pss_verify;
+		}
+
+		memcpy(salt, buf, ssa_data->salt_len);
+	}
+
+	/*
+	 * Step 12
+	 * Generate the M' = (0x)00 00 00 00 00 00 00 00 || mHash || salt
+	 *
+	 * where
+	 *   mHash is the input message (already hash)
+	 *   salt is a random number of salt_len (input data) can be empty
+	 */
+	buf = msg_db;
+
+	memset(buf, 0, 8);
+	buf += 8;
+
+	memcpy(buf, ssa_data->message.data, ssa_data->message.length);
+	buf += ssa_data->message.length;
+
+	if (ssa_data->salt_len)
+		memcpy(buf, salt, ssa_data->salt_len);
+
+	/*
+	 * Step 13
+	 * Hash the M' generated new message
+	 * H' = hash(M')
+	 *
+	 * Note: reuse the msg_db buffer as Hash result
+	 */
+	hash_gen.data = msg_db;
+	hash_gen.length = ssa_data->digest_size;
+
+	ret = tee_hash_createdigest(ssa_data->hash_algo, msg_db, msg_size,
+				    hash_gen.data, hash_gen.length);
+
+	CRYPTO_TRACE("H' = hash(M') returned 0x%08" PRIx32, ret);
+	if (ret != TEE_SUCCESS)
+		goto end_pss_verify;
+
+	CRYPTO_DUMPBUF("H' = hash(M')", hash_gen.data, hash_gen.length);
+
+	/*
+	 * Step 14
+	 * Compare H and H'
+	 */
+	ret = TEE_ERROR_SIGNATURE_INVALID;
+	if (!memcmp(hash_gen.data, hash.data, hash_gen.length))
+		ret = TEE_SUCCESS;
+
+end_pss_verify:
+	free(msg_db);
+	free(salt);
+
+	return ret;
+}
+
+/*
+ * PSS - Signature of RSA message and encodes the signature.
+ * Refer to RSASSA-PSS chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data   [in/out] RSA data to sign / Signature
+ */
+static TEE_Result rsassa_pss_sign(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	TEE_Result ret = TEE_ERROR_GENERIC;
+	struct rsa_keypair *key = NULL;
+	struct drvcrypt_buf EM = { };
+	size_t modBits = 0;
+	struct drvcrypt_rsa_ed rsa_data = { };
+	struct drvcrypt_rsa *rsa = NULL;
+
+	key = ssa_data->key.key;
+
+	/* Get modulus length in bits */
+	modBits = crypto_bignum_num_bits(key->n);
+	if (modBits <= 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/*
+	 * EM Length = (modBits - 1) / 8
+	 * if (modBits - 1) is not divisible by 8, one more byte is needed
+	 */
+	EM.length = ROUNDUP(modBits, 8) / 8;
+
+	EM.data = malloc(EM.length);
+	if (!EM.data)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	CRYPTO_TRACE("modBits = %zu, hence EM Length = %zu", modBits,
+		     EM.length);
+
+	/* Encode the Message */
+	ret = emsa_pss_encode(ssa_data, modBits, &EM);
+	CRYPTO_TRACE("EMSA PSS Encode returned 0x%08" PRIx32, ret);
+
+	/*
+	 * RSA Encrypt/Decrypt are doing the same operation
+	 * expect that the decrypt takes a RSA Private key in parameter
+	 */
+	if (ret == TEE_SUCCESS) {
+		rsa_data.key.key = ssa_data->key.key;
+		rsa_data.key.isprivate = true;
+		rsa_data.key.n_size = ssa_data->key.n_size;
+
+		rsa = drvcrypt_get_ops(CRYPTO_RSA);
+		if (rsa) {
+			/* Prepare the decryption data parameters */
+			rsa_data.rsa_id = DRVCRYPT_RSASSA_PSS;
+			rsa_data.message.data = ssa_data->signature.data;
+			rsa_data.message.length = ssa_data->signature.length;
+			rsa_data.cipher.data = EM.data;
+			rsa_data.cipher.length = EM.length;
+
+			ret = rsa->decrypt(&rsa_data);
+
+			/* Set the message decrypted size */
+			ssa_data->signature.length = rsa_data.message.length;
+		} else {
+			ret = TEE_ERROR_NOT_IMPLEMENTED;
+		}
+	}
+	free(EM.data);
+
+	return ret;
+}
+
+/*
+ * PSS - Signature verification of RSA message.
+ * Refer to RSASSA-PSS chapter of the PKCS#1 v2.1
+ *
+ * @ssa_data   [in/out] RSA Signature vs. message to verify
+ */
+static TEE_Result rsassa_pss_verify(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	TEE_Result ret = TEE_ERROR_GENERIC;
+	struct rsa_public_key *key = NULL;
+	struct drvcrypt_buf EM = { };
+	size_t modBits = 0;
+	struct drvcrypt_rsa_ed rsa_data = { };
+	struct drvcrypt_rsa *rsa = NULL;
+
+	key = ssa_data->key.key;
+
+	/* Get modulus length in bits */
+	modBits = crypto_bignum_num_bits(key->n);
+	if (modBits <= 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/*
+	 * EM Length = (modBits - 1) / 8
+	 * if (modBits - 1) is not divisible by 8, one more byte is needed
+	 */
+	modBits--;
+	EM.length = modBits / 8;
+	if (modBits % 8)
+		EM.length++;
+
+	EM.data = malloc(EM.length);
+	if (!EM.data)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	CRYPTO_TRACE("modBits = %zu, hence EM Length = %zu", modBits + 1,
+		     EM.length);
+
+	/*
+	 * RSA Encrypt/Decrypt are doing the same operation
+	 * expect that the encrypt takes a RSA Public key in parameter
+	 */
+	rsa_data.key.key = ssa_data->key.key;
+	rsa_data.key.isprivate = false;
+	rsa_data.key.n_size = ssa_data->key.n_size;
+
+	rsa = drvcrypt_get_ops(CRYPTO_RSA);
+	if (rsa) {
+		/* Prepare the encryption data parameters */
+		rsa_data.rsa_id = DRVCRYPT_RSASSA_PSS;
+		rsa_data.message.data = ssa_data->signature.data;
+		rsa_data.message.length = ssa_data->signature.length;
+		rsa_data.cipher.data = EM.data;
+		rsa_data.cipher.length = EM.length;
+
+		ret = rsa->encrypt(&rsa_data);
+
+		/* Set the cipher size */
+		EM.length = rsa_data.cipher.length;
+	} else {
+		ret = TEE_ERROR_NOT_IMPLEMENTED;
+		goto end_pss_verify;
+	}
+
+	if (ret == TEE_SUCCESS) {
+		/* Verify the Message */
+		ret = emsa_pss_verify(ssa_data, modBits, &EM);
+		CRYPTO_TRACE("EMSA PSS Verify returned 0x%08" PRIx32, ret);
+	} else {
+		CRYPTO_TRACE("RSA NO PAD returned 0x%08" PRIx32, ret);
+		ret = TEE_ERROR_SIGNATURE_INVALID;
+	}
+
+end_pss_verify:
+	free(EM.data);
+
+	return ret;
+}
+
+TEE_Result drvcrypt_rsassa_sign(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	switch (ssa_data->algo) {
+	case TEE_ALG_RSASSA_PKCS1_V1_5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+		return rsassa_pkcs1_v1_5_sign(ssa_data);
+
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		return rsassa_pss_sign(ssa_data);
+
+	default:
+		break;
+	}
+
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+TEE_Result drvcrypt_rsassa_verify(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	switch (ssa_data->algo) {
+	case TEE_ALG_RSASSA_PKCS1_V1_5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+		return rsassa_pkcs1_v1_5_verify(ssa_data);
+
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		return rsassa_pss_verify(ssa_data);
+
+	default:
+		break;
+	}
+
+	return TEE_ERROR_BAD_PARAMETERS;
+}
diff --git a/core/drivers/crypto/crypto_api/acipher/sub.mk b/core/drivers/crypto/crypto_api/acipher/sub.mk
new file mode 100644
index 00000000..e44da301
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/acipher/sub.mk
@@ -0,0 +1 @@
+srcs-$(CFG_CRYPTO_DRV_RSA) += rsa.c rsamgf.c rsassa.c
diff --git a/core/drivers/crypto/crypto_api/include/drvcrypt.h b/core/drivers/crypto/crypto_api/include/drvcrypt.h
index 875475f2..d2d32860 100644
--- a/core/drivers/crypto/crypto_api/include/drvcrypt.h
+++ b/core/drivers/crypto/crypto_api/include/drvcrypt.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright 2018-2019 NXP
+ * Copyright 2018-2020 NXP
  *
  * Brief   Crypto Driver exported constants and interfaces.
  */
@@ -9,18 +9,48 @@
 
 #include <tee_api_types.h>
 #include <trace.h>
+#include <util.h>
 
-#ifdef CFG_CRYPTO_DRIVER_DEBUG
-#define CRYPTO_TRACE	DMSG
+/*
+ * Debug Macros function of Crypto Driver Debug Level setting
+ * The CFG_CRYPTO_DRV_DBG is a bit mask 32 bits value defined
+ * as followed:
+ */
+#define DRV_DBG_TRACE BIT32(0) /* Driver trace */
+#define DRV_DBG_BUF   BIT32(1) /* Driver dump Buffer */
+
+#if (CFG_CRYPTO_DRIVER_DEBUG & DRV_DBG_TRACE)
+#define CRYPTO_TRACE DMSG
 #else
 #define CRYPTO_TRACE(...)
 #endif
+#if (CFG_CRYPTO_DRIVER_DEBUG & DRV_DBG_BUF)
+#define CRYPTO_DUMPBUF(title, buf, len)                                        \
+	do {                                                                   \
+		__typeof__(buf) _buf = (buf);                                  \
+		__typeof__(len) _len = (len);                                  \
+		CRYPTO_TRACE("%s @%p: %zu", title, _buf, _len);                \
+		dhex_dump(NULL, 0, 0, _buf, _len);                             \
+	} while (0)
+#else
+#define CRYPTO_DUMPBUF(...)
+#endif
+
+/*
+ * Definition of a crypto buffer type
+ */
+struct drvcrypt_buf {
+	uint8_t *data;
+	size_t length;
+};
 
 /*
  * Crypto Library Algorithm enumeration
  */
 enum drvcrypt_algo_id {
 	CRYPTO_HASH = 0, /* Hash driver */
+	CRYPTO_RSA,      /* Asymmetric RSA driver */
+	CRYPTO_MATH,	 /* Mathematical driver */
 	CRYPTO_MAX_ALGO  /* Maximum number of algo supported */
 };
 
diff --git a/core/drivers/crypto/crypto_api/include/drvcrypt_acipher.h b/core/drivers/crypto/crypto_api/include/drvcrypt_acipher.h
new file mode 100644
index 00000000..60029434
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/include/drvcrypt_acipher.h
@@ -0,0 +1,112 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Brief   Asymmetric Cipher interface calling the HW crypto driver.
+ */
+#ifndef __DRVCRYPT_ACIPHER_H__
+#define __DRVCRYPT_ACIPHER_H__
+
+#include <crypto/crypto.h>
+#include <tee_api_types.h>
+
+/*
+ * Assymetric Cipher RSA Algorithm enumerate
+ */
+enum drvcrypt_rsa_id {
+	DRVCRYPT_RSA_NOPAD = 0,	   /* RSA Algo mode NO PAD */
+	DRVCRYPT_RSA_OAEP,	   /* RSA Algo mode OAEP */
+	DRVCRYPT_RSA_PKCS_V1_5,	   /* RSA Algo mode PKCSv1.5 */
+	DRVCRYPT_RSASSA_PKCS_V1_5, /* RSA Signature Algo mode PKCSv1.5 */
+	DRVCRYPT_RSASSA_PSS,	   /* RSA Signature Algo mode PSS */
+};
+
+/*
+ * RSA Key object
+ */
+struct drvcrypt_rsakey {
+	void *key;	/* Public or Private key */
+	size_t n_size;	/* Size in bytes of the Modulus N */
+	bool isprivate; /* True if private key */
+};
+
+/*
+ * RSA Mask Generation data
+ */
+struct drvcrypt_rsa_mgf {
+	uint32_t hash_algo;	  /* HASH Algorithm */
+	size_t digest_size;	  /* Hash Digest Size */
+	struct drvcrypt_buf seed; /* Seed to generate mask */
+	struct drvcrypt_buf mask; /* Mask generated */
+};
+
+/*
+ * RSA Encoded Signature data
+ */
+struct drvcrypt_rsa_ssa {
+	uint32_t algo;		       /* Operation algorithm */
+	uint32_t hash_algo;	       /* HASH Algorithm */
+	size_t digest_size;	       /* Hash Digest Size */
+	struct drvcrypt_rsakey key;    /* Public or Private Key */
+	struct drvcrypt_buf message;   /* Message to sign or signed */
+	struct drvcrypt_buf signature; /* Signature of the message */
+	size_t salt_len;	       /* Signature Salt length */
+
+	/* RSA Mask Generation function */
+	TEE_Result (*mgf)(struct drvcrypt_rsa_mgf *mgf_data);
+};
+
+/*
+ * RSA Encrypt/Decript data
+ */
+struct drvcrypt_rsa_ed {
+	enum drvcrypt_rsa_id rsa_id; /* RSA Algorithm Id */
+	uint32_t hash_algo;	     /* HASH Algorithm */
+	size_t digest_size;	     /* Hash Digest Size */
+	struct drvcrypt_rsakey key;  /* Public or Private key */
+	struct drvcrypt_buf message; /* Message to encrypt or decrypted */
+	struct drvcrypt_buf cipher;  /* Cipher encrypted or to decrypt */
+	struct drvcrypt_buf label;   /* Additional Label (RSAES) */
+
+	/* RSA Mask Generation function */
+	TEE_Result (*mgf)(struct drvcrypt_rsa_mgf *mgf_data);
+};
+
+/*
+ * Crypto Library RSA driver operations
+ */
+struct drvcrypt_rsa {
+	/* Allocates the RSA keypair */
+	TEE_Result (*alloc_keypair)(struct rsa_keypair *key, size_t size_bits);
+	/* Allocates the RSA public key */
+	TEE_Result (*alloc_publickey)(struct rsa_public_key *key,
+				      size_t size_bits);
+	/* Free RSA public key */
+	void (*free_publickey)(struct rsa_public_key *key);
+	/* Generates the RSA keypair */
+	TEE_Result (*gen_keypair)(struct rsa_keypair *key, size_t size_bits);
+
+	/* RSA Encryption */
+	TEE_Result (*encrypt)(struct drvcrypt_rsa_ed *rsa_data);
+	/* RSA Decryption */
+	TEE_Result (*decrypt)(struct drvcrypt_rsa_ed *rsa_data);
+
+	struct {
+		/* RSA Sign a message and encode the signature */
+		TEE_Result (*ssa_sign)(struct drvcrypt_rsa_ssa *ssa_data);
+		/* RSA Encoded Signature Verification */
+		TEE_Result (*ssa_verify)(struct drvcrypt_rsa_ssa *ssa_data);
+	} optional;
+};
+
+/*
+ * Register a RSA processing driver in the crypto API
+ *
+ * @ops - Driver operations in the HW layer
+ */
+static inline TEE_Result drvcrypt_register_rsa(const struct drvcrypt_rsa *ops)
+{
+	return drvcrypt_register(CRYPTO_RSA, (void *)ops);
+}
+
+#endif /* __DRVCRYPT_ACIPHER_H__ */
diff --git a/core/drivers/crypto/crypto_api/include/drvcrypt_asn1_oid.h b/core/drivers/crypto/crypto_api/include/drvcrypt_asn1_oid.h
new file mode 100644
index 00000000..5be264cc
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/include/drvcrypt_asn1_oid.h
@@ -0,0 +1,151 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Brief   Definition of the cryptographic algorthim's OID in the
+ *         ASN1 String format.
+ *         Definition of the ASN1 DER tags.
+ *
+ * Computer Security Objects Register
+ * http://csrc.nist.gov/groups/ST/crypto_apps_infra/csor/algorithms.html
+ */
+#ifndef __ASN1_OID_H__
+#define __ASN1_OID_H__
+
+#include <drvcrypt_hash.h>
+#include <stdint.h>
+
+/*
+ * ASN1 Tags
+ */
+#define DRVCRYPT_ASN1_CONSTRUCTED  0x20
+#define DRVCRYPT_ASN1_SEQUENCE	   0x10
+#define DRVCRYPT_ASN1_OID	   0x06
+#define DRVCRYPT_ASN1_NULL	   0x05
+#define DRVCRYPT_ASN1_OCTET_STRING 0x04
+
+/*
+ * OID Top Level = first two Node (Standard and Registration-authority)
+ *
+ * iso(1) member-body(2)
+ * iso(1) identified-organization(3)
+ * joint-iso-itu-t(2) country(16)
+ */
+#define DRVCRYPT_OID_ISO_MEMBER_BODY "\x2a"
+#define DRVCRYPT_OID_ISO_ID_ORG	     "\x2b"
+#define DRVCRYPT_OID_ISO_ITU_COUNTRY "\x60"
+
+/*
+ * ISO Member body
+ *
+ * us(840)
+ * us(840) rsadsi(113549)
+ */
+#define DRVCRYPT_OID_MB_US	  "\x86\x48"
+#define DRVCRYPT_OID_MB_US_RSADSI DRVCRYPT_OID_MB_US "\x86\x48"
+
+/*
+ * ISO Identified organization
+ *
+ * oiw(14)
+ * oiw(14) secsig(3)
+ */
+#define DRVCRYPT_OID_IO_OIW	   "\x0e"
+#define DRVCRYPT_OID_IO_OIW_SECSIG DRVCRYPT_OID_IO_OIW "\x03"
+
+/*
+ * ISO ITU OID
+ *
+ * organization(1)
+ * organization(1) gov(101)
+ */
+#define DRVCRYPT_OID_ITU_ORG	 "\x01"
+#define DRVCRYPT_OID_ITU_ORG_GOV DRVCRYPT_OID_ITU_ORG "\x65"
+
+/*
+ * Digest Algorithm
+ *
+ * digestAlgorithm(2)
+ * csor(3) nistalgotrithm(4)
+ */
+#define DRVCRYPT_OID_DIGEST	      "\x02"
+#define DRVCRYPT_OID_DIGEST_CSOR_NIST "\x03\x04"
+
+/*
+ * Definition of the Hash OID String
+ *
+ * id-md5 OBJECT IDENTIFIER ::= {
+ *   iso(1) member-body(2) us(840) rsadsi(113549) digestAlgorithm(2) 5
+ * }
+ * id-sha1 OBJECT IDENTIFIER ::= {
+ *   iso(1) identified-organization(3) oiw(14) secsig(3) algorithms(2) 26
+ * }
+ * id-sha224 OBJECT IDENTIFIER ::= {
+ *   joint-iso-itu-t(2) country(16) us(840) organization(1) gov(101)
+ *   csor(3) nistalgorithm(4) hashalgs(2) 4
+ * }
+ * id-sha256 OBJECT IDENTIFIER ::= {
+ *   joint-iso-itu-t(2) country(16) us(840) organization(1) gov(101)
+ *   csor(3) nistalgorithm(4) hashalgs(2) 1
+ * }
+ * id-sha384 OBJECT IDENTIFIER ::= {
+ *   joint-iso-itu-t(2) country(16) us(840) organization(1) gov(101)
+ *   csor(3) nistalgorithm(4) hashalgs(2) 2
+ * }
+ * id-sha512 OBJECT IDENTIFIER ::= {
+ *   joint-iso-itu-t(2) country(16) us(840) organization(1) gov(101)
+ *   csor(3) nistalgorithm(4) hashalgs(2) 3
+ * }
+ *
+ */
+#define DRVCRYPT_OID_ID_MD5                                                    \
+	DRVCRYPT_OID_ISO_MEMBER_BODY DRVCRYPT_OID_MB_US_RSADSI                 \
+		DRVCRYPT_OID_DIGEST "\x05"
+
+#define DRVCRYPT_OID_ID_SHA1                                                   \
+	DRVCRYPT_OID_ISO_ID_ORG DRVCRYPT_OID_IO_OIW_SECSIG DRVCRYPT_OID_DIGEST \
+		"\x1a"
+
+#define DRVCRYPT_OID_ID_SHA224                                                 \
+	DRVCRYPT_OID_ISO_ITU_COUNTRY DRVCRYPT_OID_MB_US                        \
+		DRVCRYPT_OID_ITU_ORG_GOV DRVCRYPT_OID_DIGEST_CSOR_NIST         \
+			DRVCRYPT_OID_DIGEST "\x04"
+
+#define DRVCRYPT_OID_ID_SHA256                                                 \
+	DRVCRYPT_OID_ISO_ITU_COUNTRY DRVCRYPT_OID_MB_US                        \
+		DRVCRYPT_OID_ITU_ORG_GOV DRVCRYPT_OID_DIGEST_CSOR_NIST         \
+			DRVCRYPT_OID_DIGEST "\x01"
+
+#define DRVCRYPT_OID_ID_SHA384                                                 \
+	DRVCRYPT_OID_ISO_ITU_COUNTRY DRVCRYPT_OID_MB_US                        \
+		DRVCRYPT_OID_ITU_ORG_GOV DRVCRYPT_OID_DIGEST_CSOR_NIST         \
+			DRVCRYPT_OID_DIGEST "\x02"
+
+#define DRVCRYPT_OID_ID_SHA512                                                 \
+	DRVCRYPT_OID_ISO_ITU_COUNTRY DRVCRYPT_OID_MB_US                        \
+		DRVCRYPT_OID_ITU_ORG_GOV DRVCRYPT_OID_DIGEST_CSOR_NIST         \
+			DRVCRYPT_OID_DIGEST "\x03"
+
+#define DRVCRYPT_OID_LEN(_id) (sizeof(_id) - 1)
+
+/*
+ * Definition of the ASN1 OID structure
+ */
+struct drvcrypt_oid {
+	const char *asn1;	  /* OID ASN1 string */
+	const size_t asn1_length; /* OID ASN1 string length */
+};
+
+/*
+ * Hash OID constant array
+ */
+extern const struct drvcrypt_oid drvcrypt_hash_oid[];
+
+/*
+ * Return the Hash OID value registered in the Hash OID table.
+ *
+ * @algo	Hash algorithm identifier
+ */
+struct drvcrypt_oid *drvcrypt_get_alg_hash_oid(uint32_t algo);
+
+#endif /* __ASN1_OID_H__ */
diff --git a/core/drivers/crypto/crypto_api/include/drvcrypt_hash.h b/core/drivers/crypto/crypto_api/include/drvcrypt_hash.h
index 62cc8b59..2be4573c 100644
--- a/core/drivers/crypto/crypto_api/include/drvcrypt_hash.h
+++ b/core/drivers/crypto/crypto_api/include/drvcrypt_hash.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright 2018-2019 NXP
+ * Copyright 2018-2020 NXP
  *
  * Brief   Hash interface calling the HW crypto driver.
  */
@@ -8,6 +8,7 @@
 #define __DRVCRYPT_HASH_H__
 
 #include <crypto/crypto_impl.h>
+#include <drvcrypt.h>
 #include <tee_api_types.h>
 
 /*
diff --git a/core/drivers/crypto/crypto_api/include/drvcrypt_math.h b/core/drivers/crypto/crypto_api/include/drvcrypt_math.h
new file mode 100644
index 00000000..3069a296
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/include/drvcrypt_math.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Brief   Cryptographic library using the HW crypto driver.
+ *         Mathematical operation using HW if available.
+ */
+#ifndef __DRVCRYPT_MATH_H__
+#define __DRVCRYPT_MATH_H__
+
+#include <drvcrypt.h>
+
+/*
+ * Binary Modular operation data
+ */
+struct drvcrypt_mod_op {
+	struct drvcrypt_buf n;	    /* Modulus N */
+	struct drvcrypt_buf a;	    /* Operand A */
+	struct drvcrypt_buf b;	    /* Operand B */
+	struct drvcrypt_buf result; /* Result of operation */
+};
+
+/*
+ * Operation (A xor B) mod N
+ *
+ * @data   [in/out] Data operation
+ */
+TEE_Result drvcrypt_xor_mod_n(struct drvcrypt_mod_op *data);
+
+/*
+ * Crypto Library Binaries Modular driver operations
+ */
+struct drvcrypt_math {
+	/* (A xor B) mod N */
+	TEE_Result (*xor_mod_n)(struct drvcrypt_mod_op *op_data);
+};
+
+/*
+ * Register a mathematical processing driver in the crypto API
+ *
+ * @ops - Driver operations in the HW layer
+ */
+static inline TEE_Result drvcrypt_register_math(const struct drvcrypt_math *ops)
+{
+	return drvcrypt_register(CRYPTO_MATH, (void *)ops);
+}
+#endif /* __DRVCRYPT_MATH_H__ */
diff --git a/core/drivers/crypto/crypto_api/math/modulus.c b/core/drivers/crypto/crypto_api/math/modulus.c
new file mode 100644
index 00000000..e042b8f2
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/math/modulus.c
@@ -0,0 +1,63 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Brief   Cryptographic library using the NXP CAAM driver.
+ *         Mathematical Modulus operation implementation.
+ */
+#include <drvcrypt.h>
+#include <drvcrypt_math.h>
+#include <string.h>
+#include <utee_defines.h>
+#include <util.h>
+
+TEE_Result drvcrypt_xor_mod_n(struct drvcrypt_mod_op *data)
+{
+	TEE_Result ret = TEE_ERROR_GENERIC;
+	struct drvcrypt_math *math = NULL;
+
+	if (!data->a.data || !data->a.length || !data->b.data ||
+	    !data->b.length || !data->result.data || !data->result.length ||
+	    !data->n.length)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (data->result.length < data->n.length)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	math = drvcrypt_get_ops(CRYPTO_MATH);
+	if (math) {
+		/* Operation done by Math driver */
+		ret = math->xor_mod_n(data);
+	} else {
+		/* Operation done by Software */
+		size_t min = 0, idx = 0;
+
+		/* Calculate the minimum size to do A xor B */
+		min = MIN(data->a.length, data->b.length);
+		min = MIN(min, data->n.length);
+
+		for (; idx < min; idx++)
+			data->result.data[idx] =
+				data->a.data[idx] ^ data->b.data[idx];
+
+		if (min < data->n.length) {
+			/* Complete result to make a N modulus number */
+			if (data->a.length > min) {
+				memcpy(&data->result.data[idx],
+				       &data->a.data[idx],
+				       data->n.length - min);
+			} else if (data->b.length > min) {
+				memcpy(&data->result.data[idx],
+				       &data->b.data[idx],
+				       data->n.length - min);
+			} else {
+				memset(&data->result.data[idx], 0,
+				       data->n.length - min);
+			}
+		}
+
+		ret = TEE_SUCCESS;
+	}
+
+	return ret;
+}
diff --git a/core/drivers/crypto/crypto_api/math/sub.mk b/core/drivers/crypto/crypto_api/math/sub.mk
new file mode 100644
index 00000000..e718dffa
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/math/sub.mk
@@ -0,0 +1 @@
+srcs-y += modulus.c
diff --git a/core/drivers/crypto/crypto_api/oid/hash_oid.c b/core/drivers/crypto/crypto_api/oid/hash_oid.c
new file mode 100644
index 00000000..3d068a9e
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/oid/hash_oid.c
@@ -0,0 +1,39 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright 2018-2020 NXP
+ *
+ * Brief   Definition of the Hash's OID
+ */
+
+/* Driver Crypto includes */
+#include <drvcrypt_asn1_oid.h>
+
+/*
+ * Hash OID values
+ */
+const struct drvcrypt_oid drvcrypt_hash_oid[] = {
+	/* empty entry */
+	{ NULL, 0 },
+	/* MD5 */
+	{ DRVCRYPT_OID_ID_MD5, DRVCRYPT_OID_LEN(DRVCRYPT_OID_ID_MD5) },
+	/* SHA1 */
+	{ DRVCRYPT_OID_ID_SHA1, DRVCRYPT_OID_LEN(DRVCRYPT_OID_ID_SHA1) },
+	/* SHA224 */
+	{ DRVCRYPT_OID_ID_SHA224, DRVCRYPT_OID_LEN(DRVCRYPT_OID_ID_SHA224) },
+	/* SHA256 */
+	{ DRVCRYPT_OID_ID_SHA256, DRVCRYPT_OID_LEN(DRVCRYPT_OID_ID_SHA256) },
+	/* SHA384 */
+	{ DRVCRYPT_OID_ID_SHA384, DRVCRYPT_OID_LEN(DRVCRYPT_OID_ID_SHA384) },
+	/* SHA512 */
+	{ DRVCRYPT_OID_ID_SHA512, DRVCRYPT_OID_LEN(DRVCRYPT_OID_ID_SHA512) },
+};
+
+struct drvcrypt_oid *drvcrypt_get_alg_hash_oid(uint32_t algo)
+{
+	uint32_t main_alg = TEE_ALG_GET_MAIN_ALG(algo);
+
+	if (main_alg < ARRAY_SIZE(drvcrypt_hash_oid))
+		return &drvcrypt_hash_oid[main_alg];
+
+	return NULL;
+}
diff --git a/core/drivers/crypto/crypto_api/oid/sub.mk b/core/drivers/crypto/crypto_api/oid/sub.mk
new file mode 100644
index 00000000..1b051f33
--- /dev/null
+++ b/core/drivers/crypto/crypto_api/oid/sub.mk
@@ -0,0 +1 @@
+srcs-y += hash_oid.c
diff --git a/core/drivers/crypto/crypto_api/sub.mk b/core/drivers/crypto/crypto_api/sub.mk
index fa802db4..8e7dc961 100644
--- a/core/drivers/crypto/crypto_api/sub.mk
+++ b/core/drivers/crypto/crypto_api/sub.mk
@@ -1,3 +1,7 @@
 srcs-y += drvcrypt.c
 
-subdirs-$(CFG_CRYPTO_DRV_HASH) += hash
+subdirs-y += math
+
+subdirs-$(CFG_CRYPTO_DRV_HASH)    += hash
+subdirs-$(CFG_CRYPTO_DRV_ACIPHER) += acipher
+subdirs-$(CFG_CRYPTO_DRV_ACIPHER) += oid