libmbedtls: support mbedtls acipher RSA function

Support RSA:
	RSASSA_PKCS1_V1_5
	RSASSA_PKCS1_PSS_MGF1
	RSAES_PKCS1_V1_5
	RSAES_PKCS1_OAEP_MGF1

Acked-by: Etienne Carriere <etienne.carriere@linaro.org>
Signed-off-by: Edison Ai <edison.ai@arm.com>
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

5 files changed, 714 insertions, 88 deletions

diff --git a/lib/libmbedtls/core/mbd_rand.h b/lib/libmbedtls/core/mbd_rand.h
new file mode 100644
index 00000000..21e32787
--- /dev/null
+++ b/lib/libmbedtls/core/mbd_rand.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2018, ARM Limited
+ * Copyright (C) 2019, Linaro Limited
+ */
+
+#ifndef __MBD_RAND_H
+#define __MBD_RAND_H
+
+#include <crypto/crypto.h>
+#include <mbedtls/ctr_drbg.h>
+
+static inline int mbd_rand(void *rng_state __unused, unsigned char *output,
+			size_t len)
+{
+	if (crypto_rng_read(output, len))
+		return MBEDTLS_ERR_CTR_DRBG_ENTROPY_SOURCE_FAILED;
+	return 0;
+}
+
+#endif /*__MBD_RAND_H*/
diff --git a/lib/libmbedtls/core/rsa.c b/lib/libmbedtls/core/rsa.c
new file mode 100644
index 00000000..a323a3ba
--- /dev/null
+++ b/lib/libmbedtls/core/rsa.c
@@ -0,0 +1,676 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) 2018, ARM Limited
+ * Copyright (C) 2019, Linaro Limited
+ */
+
+#include <assert.h>
+#include <crypto/crypto.h>
+#include <mbedtls/ctr_drbg.h>
+#include <mbedtls/entropy.h>
+#include <mbedtls/pk.h>
+#include <mbedtls/pk_internal.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+
+#include "mbd_rand.h"
+
+static TEE_Result get_tee_result(int lmd_res)
+{
+	switch (lmd_res) {
+	case 0:
+		return TEE_SUCCESS;
+	case MBEDTLS_ERR_RSA_BAD_INPUT_DATA:
+	case MBEDTLS_ERR_RSA_INVALID_PADDING:
+	case MBEDTLS_ERR_PK_TYPE_MISMATCH:
+		return TEE_ERROR_BAD_PARAMETERS;
+	case MBEDTLS_ERR_RSA_OUTPUT_TOO_LARGE:
+		return TEE_ERROR_SHORT_BUFFER;
+	default:
+		return TEE_ERROR_BAD_STATE;
+	}
+}
+
+static uint32_t tee_algo_to_mbedtls_hash_algo(uint32_t algo)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_SHA1)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA1:
+	case TEE_ALG_SHA1:
+	case TEE_ALG_DSA_SHA1:
+	case TEE_ALG_HMAC_SHA1:
+		return MBEDTLS_MD_SHA1;
+#endif
+#if defined(CFG_CRYPTO_MD5)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_MD5:
+	case TEE_ALG_HMAC_MD5:
+		return MBEDTLS_MD_MD5;
+#endif
+#if defined(CFG_CRYPTO_SHA224)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA224:
+	case TEE_ALG_SHA224:
+	case TEE_ALG_DSA_SHA224:
+	case TEE_ALG_HMAC_SHA224:
+		return MBEDTLS_MD_SHA224;
+#endif
+#if defined(CFG_CRYPTO_SHA256)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256:
+	case TEE_ALG_SHA256:
+	case TEE_ALG_DSA_SHA256:
+	case TEE_ALG_HMAC_SHA256:
+		return MBEDTLS_MD_SHA256;
+#endif
+#if defined(CFG_CRYPTO_SHA384)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA384:
+	case TEE_ALG_SHA384:
+	case TEE_ALG_HMAC_SHA384:
+		return MBEDTLS_MD_SHA384;
+#endif
+#if defined(CFG_CRYPTO_SHA512)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA512:
+	case TEE_ALG_SHA512:
+	case TEE_ALG_HMAC_SHA512:
+		return MBEDTLS_MD_SHA512;
+#endif
+	default:
+		return MBEDTLS_MD_NONE;
+	}
+}
+
+static void rsa_init_from_key_pair(mbedtls_rsa_context *rsa,
+				struct rsa_keypair *key)
+{
+	mbedtls_rsa_init(rsa, 0, 0);
+
+	rsa->E = *(mbedtls_mpi *)key->e;
+	rsa->N = *(mbedtls_mpi *)key->n;
+	rsa->D = *(mbedtls_mpi *)key->d;
+	if (key->p && crypto_bignum_num_bytes(key->p)) {
+		rsa->P = *(mbedtls_mpi *)key->p;
+		rsa->Q = *(mbedtls_mpi *)key->q;
+		rsa->QP = *(mbedtls_mpi *)key->qp;
+		rsa->DP = *(mbedtls_mpi *)key->dp;
+		rsa->DQ = *(mbedtls_mpi *)key->dq;
+	}
+	rsa->len = mbedtls_mpi_size(&rsa->N);
+}
+
+static void mbd_rsa_free(mbedtls_rsa_context *rsa)
+{
+	/* Reset mpi to skip freeing here, those mpis will be freed with key */
+	mbedtls_mpi_init(&rsa->E);
+	mbedtls_mpi_init(&rsa->N);
+	mbedtls_mpi_init(&rsa->D);
+	if (mbedtls_mpi_size(&rsa->P)) {
+		mbedtls_mpi_init(&rsa->P);
+		mbedtls_mpi_init(&rsa->Q);
+		mbedtls_mpi_init(&rsa->QP);
+		mbedtls_mpi_init(&rsa->DP);
+		mbedtls_mpi_init(&rsa->DQ);
+	}
+	mbedtls_rsa_free(rsa);
+}
+
+TEE_Result crypto_acipher_alloc_rsa_keypair(struct rsa_keypair *s,
+					    size_t key_size_bits)
+{
+	memset(s, 0, sizeof(*s));
+	s->e = crypto_bignum_allocate(key_size_bits);
+	if (!s->e)
+		goto err;
+	s->d = crypto_bignum_allocate(key_size_bits);
+	if (!s->d)
+		goto err;
+	s->n = crypto_bignum_allocate(key_size_bits);
+	if (!s->n)
+		goto err;
+	s->p = crypto_bignum_allocate(key_size_bits);
+	if (!s->p)
+		goto err;
+	s->q = crypto_bignum_allocate(key_size_bits);
+	if (!s->q)
+		goto err;
+	s->qp = crypto_bignum_allocate(key_size_bits);
+	if (!s->qp)
+		goto err;
+	s->dp = crypto_bignum_allocate(key_size_bits);
+	if (!s->dp)
+		goto err;
+	s->dq = crypto_bignum_allocate(key_size_bits);
+	if (!s->dq)
+		goto err;
+
+	return TEE_SUCCESS;
+err:
+	crypto_bignum_free(s->e);
+	crypto_bignum_free(s->d);
+	crypto_bignum_free(s->n);
+	crypto_bignum_free(s->p);
+	crypto_bignum_free(s->q);
+	crypto_bignum_free(s->qp);
+	crypto_bignum_free(s->dp);
+
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+TEE_Result crypto_acipher_alloc_rsa_public_key(struct rsa_public_key *s,
+					       size_t key_size_bits)
+{
+	memset(s, 0, sizeof(*s));
+	s->e = crypto_bignum_allocate(key_size_bits);
+	if (!s->e)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	s->n = crypto_bignum_allocate(key_size_bits);
+	if (!s->n)
+		goto err;
+	return TEE_SUCCESS;
+err:
+	crypto_bignum_free(s->e);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+void crypto_acipher_free_rsa_public_key(struct rsa_public_key *s)
+{
+	if (!s)
+		return;
+	crypto_bignum_free(s->n);
+	crypto_bignum_free(s->e);
+}
+
+TEE_Result crypto_acipher_gen_rsa_key(struct rsa_keypair *key, size_t key_size)
+{
+	TEE_Result res = TEE_SUCCESS;
+	mbedtls_rsa_context rsa;
+	int lmd_res = 0;
+	uint32_t e = 0;
+
+	memset(&rsa, 0, sizeof(rsa));
+	mbedtls_rsa_init(&rsa, 0, 0);
+
+	/* get the public exponent */
+	mbedtls_mpi_write_binary((mbedtls_mpi *)key->e,
+				 (unsigned char *)&e, sizeof(uint32_t));
+
+	e = TEE_U32_FROM_BIG_ENDIAN(e);
+	lmd_res = mbedtls_rsa_gen_key(&rsa, mbd_rand, NULL, key_size, (int)e);
+	if (lmd_res != 0) {
+		res = get_tee_result(lmd_res);
+	} else if ((size_t)mbedtls_mpi_bitlen(&rsa.N) != key_size) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+	} else {
+		/* Copy the key */
+		crypto_bignum_copy(key->e, (void *)&rsa.E);
+		crypto_bignum_copy(key->d, (void *)&rsa.D);
+		crypto_bignum_copy(key->n, (void *)&rsa.N);
+		crypto_bignum_copy(key->p, (void *)&rsa.P);
+
+		crypto_bignum_copy(key->q, (void *)&rsa.Q);
+		crypto_bignum_copy(key->qp, (void *)&rsa.QP);
+		crypto_bignum_copy(key->dp, (void *)&rsa.DP);
+		crypto_bignum_copy(key->dq, (void *)&rsa.DQ);
+
+		res = TEE_SUCCESS;
+	}
+
+	mbedtls_rsa_free(&rsa);
+
+	return res;
+}
+
+TEE_Result crypto_acipher_rsanopad_encrypt(struct rsa_public_key *key,
+					   const uint8_t *src, size_t src_len,
+					   uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	mbedtls_rsa_context rsa;
+	int lmd_res = 0;
+	uint8_t *buf = NULL;
+	unsigned long blen = 0;
+	unsigned long offset = 0;
+
+	memset(&rsa, 0, sizeof(rsa));
+	mbedtls_rsa_init(&rsa, 0, 0);
+
+	rsa.E = *(mbedtls_mpi *)key->e;
+	rsa.N = *(mbedtls_mpi *)key->n;
+
+	rsa.len = crypto_bignum_num_bytes((void *)&rsa.N);
+
+	blen = CFG_CORE_BIGNUM_MAX_BITS / 8;
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	memset(buf, 0, blen);
+	memcpy(buf + rsa.len - src_len, src, src_len);
+
+	lmd_res = mbedtls_rsa_public(&rsa, buf, buf);
+	if (lmd_res != 0) {
+		FMSG("mbedtls_rsa_public() returned 0x%x", -lmd_res);
+		res = get_tee_result(lmd_res);
+		goto out;
+	}
+
+	/* Remove the zero-padding (leave one zero if buff is all zeroes) */
+	offset = 0;
+	while ((offset < rsa.len - 1) && (buf[offset] == 0))
+		offset++;
+
+	if (*dst_len < rsa.len - offset) {
+		*dst_len = rsa.len - offset;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+	*dst_len = rsa.len - offset;
+	memcpy(dst, buf + offset, *dst_len);
+out:
+	free(buf);
+	/* Reset mpi to skip freeing here, those mpis will be freed with key */
+	mbedtls_mpi_init(&rsa.E);
+	mbedtls_mpi_init(&rsa.N);
+	mbedtls_rsa_free(&rsa);
+
+	return res;
+}
+
+TEE_Result crypto_acipher_rsanopad_decrypt(struct rsa_keypair *key,
+					   const uint8_t *src, size_t src_len,
+					   uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	mbedtls_rsa_context rsa;
+	int lmd_res = 0;
+	uint8_t *buf = NULL;
+	unsigned long blen = 0;
+	unsigned long offset = 0;
+
+	memset(&rsa, 0, sizeof(rsa));
+	rsa_init_from_key_pair(&rsa, key);
+
+	blen = CFG_CORE_BIGNUM_MAX_BITS / 8;
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	memset(buf, 0, blen);
+	memcpy(buf + rsa.len - src_len, src, src_len);
+
+	lmd_res = mbedtls_rsa_private(&rsa, NULL, NULL, buf, buf);
+	if (lmd_res != 0) {
+		FMSG("mbedtls_rsa_private() returned 0x%x", -lmd_res);
+		res = get_tee_result(lmd_res);
+		goto out;
+	}
+
+	/* Remove the zero-padding (leave one zero if buff is all zeroes) */
+	offset = 0;
+	while ((offset < rsa.len - 1) && (buf[offset] == 0))
+		offset++;
+
+	if (*dst_len < rsa.len - offset) {
+		*dst_len = rsa.len - offset;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+	*dst_len = rsa.len - offset;
+	memcpy(dst, (char *)buf + offset, *dst_len);
+out:
+	if (buf)
+		free(buf);
+	mbd_rsa_free(&rsa);
+	return res;
+}
+
+TEE_Result crypto_acipher_rsaes_decrypt(uint32_t algo, struct rsa_keypair *key,
+					const uint8_t *label __unused,
+					size_t label_len __unused,
+					const uint8_t *src, size_t src_len,
+					uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	int lmd_res = 0;
+	int lmd_padding = 0;
+	size_t blen = 0;
+	size_t mod_size = 0;
+	void *buf = NULL;
+	mbedtls_rsa_context rsa;
+	const mbedtls_pk_info_t *pk_info = NULL;
+	uint32_t md_algo = MBEDTLS_MD_NONE;
+
+	memset(&rsa, 0, sizeof(rsa));
+	rsa_init_from_key_pair(&rsa, key);
+
+	/*
+	 * Use a temporary buffer since we don't know exactly how large
+	 * the required size of the out buffer without doing a partial
+	 * decrypt. We know the upper bound though.
+	 */
+	if (algo == TEE_ALG_RSAES_PKCS1_V1_5) {
+		mod_size = crypto_bignum_num_bytes(key->n);
+		blen = mod_size - 11;
+		lmd_padding = MBEDTLS_RSA_PKCS_V15;
+	} else {
+		/* Decoded message is always shorter than encrypted message */
+		blen = src_len;
+		lmd_padding = MBEDTLS_RSA_PKCS_V21;
+	}
+
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	pk_info = mbedtls_pk_info_from_type(MBEDTLS_PK_RSA);
+	if (!pk_info) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto out;
+	}
+
+	/*
+	 * TEE_ALG_RSAES_PKCS1_V1_5 is invalid in hash. But its hash algo will
+	 * not be used in rsa, so skip it here.
+	 */
+	if (algo != TEE_ALG_RSAES_PKCS1_V1_5) {
+		md_algo = tee_algo_to_mbedtls_hash_algo(algo);
+		if (md_algo == MBEDTLS_MD_NONE) {
+			res = TEE_ERROR_NOT_SUPPORTED;
+			goto out;
+		}
+	}
+
+	mbedtls_rsa_set_padding(&rsa, lmd_padding, md_algo);
+
+	if (lmd_padding == MBEDTLS_RSA_PKCS_V15)
+		lmd_res = pk_info->decrypt_func(&rsa, src, src_len, buf, &blen,
+						blen, NULL, NULL);
+	else
+		lmd_res = pk_info->decrypt_func(&rsa, src, src_len, buf, &blen,
+						blen, mbd_rand, NULL);
+	if (lmd_res != 0) {
+		FMSG("decrypt_func() returned 0x%x", -lmd_res);
+		res = get_tee_result(lmd_res);
+		goto out;
+	}
+
+	if (*dst_len < blen) {
+		*dst_len = blen;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = TEE_SUCCESS;
+	*dst_len = blen;
+	memcpy(dst, buf, blen);
+out:
+	if (buf)
+		free(buf);
+	mbd_rsa_free(&rsa);
+	return res;
+}
+
+TEE_Result crypto_acipher_rsaes_encrypt(uint32_t algo,
+					struct rsa_public_key *key,
+					const uint8_t *label __unused,
+					size_t label_len __unused,
+					const uint8_t *src, size_t src_len,
+					uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	int lmd_res = 0;
+	int lmd_padding = 0;
+	size_t mod_size = 0;
+	mbedtls_rsa_context rsa;
+	const mbedtls_pk_info_t *pk_info = NULL;
+	uint32_t md_algo = MBEDTLS_MD_NONE;
+
+	memset(&rsa, 0, sizeof(rsa));
+	mbedtls_rsa_init(&rsa, 0, 0);
+
+	rsa.E = *(mbedtls_mpi *)key->e;
+	rsa.N = *(mbedtls_mpi *)key->n;
+
+	mod_size = crypto_bignum_num_bytes(key->n);
+	if (*dst_len < mod_size) {
+		*dst_len = mod_size;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+	*dst_len = mod_size;
+	rsa.len = mod_size;
+
+	if (algo == TEE_ALG_RSAES_PKCS1_V1_5)
+		lmd_padding = MBEDTLS_RSA_PKCS_V15;
+	else
+		lmd_padding = MBEDTLS_RSA_PKCS_V21;
+
+	pk_info = mbedtls_pk_info_from_type(MBEDTLS_PK_RSA);
+	if (!pk_info) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto out;
+	}
+
+	/*
+	 * TEE_ALG_RSAES_PKCS1_V1_5 is invalid in hash. But its hash algo will
+	 * not be used in rsa, so skip it here.
+	 */
+	if (algo != TEE_ALG_RSAES_PKCS1_V1_5) {
+		md_algo = tee_algo_to_mbedtls_hash_algo(algo);
+		if (md_algo == MBEDTLS_MD_NONE) {
+			res = TEE_ERROR_NOT_SUPPORTED;
+			goto out;
+		}
+	}
+
+	mbedtls_rsa_set_padding(&rsa, lmd_padding, md_algo);
+
+	lmd_res = pk_info->encrypt_func(&rsa, src, src_len, dst, dst_len,
+					*dst_len, mbd_rand, NULL);
+	if (lmd_res != 0) {
+		FMSG("encrypt_func() returned 0x%x", -lmd_res);
+		res = get_tee_result(lmd_res);
+		goto out;
+	}
+	res = TEE_SUCCESS;
+out:
+	/* Reset mpi to skip freeing here, those mpis will be freed with key */
+	mbedtls_mpi_init(&rsa.E);
+	mbedtls_mpi_init(&rsa.N);
+	mbedtls_rsa_free(&rsa);
+	return res;
+}
+
+TEE_Result crypto_acipher_rsassa_sign(uint32_t algo, struct rsa_keypair *key,
+				      int salt_len __unused, const uint8_t *msg,
+				      size_t msg_len, uint8_t *sig,
+				      size_t *sig_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	int lmd_res = 0;
+	int lmd_padding = 0;
+	size_t mod_size = 0;
+	size_t hash_size = 0;
+	mbedtls_rsa_context rsa;
+	const mbedtls_pk_info_t *pk_info = NULL;
+	uint32_t md_algo = 0;
+
+	memset(&rsa, 0, sizeof(rsa));
+	rsa_init_from_key_pair(&rsa, key);
+
+	switch (algo) {
+	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:
+		lmd_padding = MBEDTLS_RSA_PKCS_V15;
+		break;
+	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:
+		lmd_padding = MBEDTLS_RSA_PKCS_V21;
+		break;
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	res = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+				       &hash_size);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	if (msg_len != hash_size) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	mod_size = crypto_bignum_num_bytes(key->n);
+	if (*sig_len < mod_size) {
+		*sig_len = mod_size;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto err;
+	}
+	rsa.len = mod_size;
+
+	md_algo = tee_algo_to_mbedtls_hash_algo(algo);
+	if (md_algo == MBEDTLS_MD_NONE) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto err;
+	}
+
+	pk_info = mbedtls_pk_info_from_type(MBEDTLS_PK_RSA);
+	if (!pk_info) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto err;
+	}
+
+	mbedtls_rsa_set_padding(&rsa, lmd_padding, md_algo);
+
+	if (lmd_padding == MBEDTLS_RSA_PKCS_V15)
+		lmd_res = pk_info->sign_func(&rsa, md_algo, msg, msg_len, sig,
+					     sig_len, NULL, NULL);
+	else
+		lmd_res = pk_info->sign_func(&rsa, md_algo, msg, msg_len, sig,
+					     sig_len, mbd_rand, NULL);
+	if (lmd_res != 0) {
+		FMSG("sign_func failed, returned 0x%x", -lmd_res);
+		res = get_tee_result(lmd_res);
+		goto err;
+	}
+	res = TEE_SUCCESS;
+err:
+	mbd_rsa_free(&rsa);
+	return res;
+}
+
+TEE_Result crypto_acipher_rsassa_verify(uint32_t algo,
+					struct rsa_public_key *key,
+					int salt_len __unused,
+					const uint8_t *msg,
+					size_t msg_len, const uint8_t *sig,
+					size_t sig_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	int lmd_res = 0;
+	int lmd_padding = 0;
+	size_t hash_size = 0;
+	size_t bigint_size = 0;
+	mbedtls_rsa_context rsa;
+	const mbedtls_pk_info_t *pk_info = NULL;
+	uint32_t md_algo = 0;
+
+	memset(&rsa, 0, sizeof(rsa));
+	mbedtls_rsa_init(&rsa, 0, 0);
+
+	rsa.E = *(mbedtls_mpi *)key->e;
+	rsa.N = *(mbedtls_mpi *)key->n;
+
+	res = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+				       &hash_size);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	if (msg_len != hash_size) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	bigint_size = crypto_bignum_num_bytes(key->n);
+	if (sig_len < bigint_size) {
+		res = TEE_ERROR_MAC_INVALID;
+		goto err;
+	}
+
+	rsa.len = bigint_size;
+
+	switch (algo) {
+	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:
+		lmd_padding = MBEDTLS_RSA_PKCS_V15;
+		break;
+	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:
+		lmd_padding = MBEDTLS_RSA_PKCS_V21;
+		break;
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	md_algo = tee_algo_to_mbedtls_hash_algo(algo);
+	if (md_algo == MBEDTLS_MD_NONE) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto err;
+	}
+
+	pk_info = mbedtls_pk_info_from_type(MBEDTLS_PK_RSA);
+	if (!pk_info) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto err;
+	}
+
+	mbedtls_rsa_set_padding(&rsa, lmd_padding, md_algo);
+
+	lmd_res = pk_info->verify_func(&rsa, md_algo, msg, msg_len,
+				       sig, sig_len);
+	if (lmd_res != 0) {
+		FMSG("verify_func failed, returned 0x%x", -lmd_res);
+		res = TEE_ERROR_SIGNATURE_INVALID;
+		goto err;
+	}
+	res = TEE_SUCCESS;
+err:
+	/* Reset mpi to skip freeing here, those mpis will be freed with key */
+	mbedtls_mpi_init(&rsa.E);
+	mbedtls_mpi_init(&rsa.N);
+	mbedtls_rsa_free(&rsa);
+	return res;
+}
diff --git a/lib/libmbedtls/core/stubbed.c b/lib/libmbedtls/core/stubbed.c
index b7e393cd..c5682f52 100644
--- a/lib/libmbedtls/core/stubbed.c
+++ b/lib/libmbedtls/core/stubbed.c
@@ -15,94 +15,6 @@
  * Asymmetric algorithms
  ******************************************************************************/
 
-#if defined(CFG_CRYPTO_RSA)
-TEE_Result crypto_acipher_alloc_rsa_keypair(struct rsa_keypair *s __unused,
-					    size_t key_size_bits __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result
-crypto_acipher_alloc_rsa_public_key(struct rsa_public_key *s __unused,
-				    size_t key_size_bits __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-void crypto_acipher_free_rsa_public_key(struct rsa_public_key *s __unused)
-{
-}
-
-TEE_Result crypto_acipher_gen_rsa_key(struct rsa_keypair *key __unused,
-				      size_t key_size __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result crypto_acipher_rsanopad_decrypt(struct rsa_keypair *key __unused,
-					   const uint8_t *src __unused,
-					   size_t src_len __unused,
-					   uint8_t *dst __unused,
-					   size_t *dst_len __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result crypto_acipher_rsanopad_encrypt(struct rsa_public_key *key __unused,
-					   const uint8_t *src __unused,
-					   size_t src_len __unused,
-					   uint8_t *dst __unused,
-					   size_t *dst_len __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result crypto_acipher_rsaes_decrypt(uint32_t algo __unused,
-					struct rsa_keypair *key __unused,
-					const uint8_t *label __unused,
-					size_t label_len __unused,
-					const uint8_t *src __unused,
-					size_t src_len __unused,
-					uint8_t *dst __unused,
-					size_t *dst_len __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result crypto_acipher_rsaes_encrypt(uint32_t algo __unused,
-					struct rsa_public_key *key __unused,
-					const uint8_t *label __unused,
-					size_t label_len __unused,
-					const uint8_t *src __unused,
-					size_t src_len __unused,
-					uint8_t *dst __unused,
-					size_t *dst_len __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result crypto_acipher_rsassa_sign(uint32_t algo __unused,
-				      struct rsa_keypair *key __unused,
-				      int salt_len __unused,
-				      const uint8_t *msg __unused,
-				      size_t msg_len __unused,
-				      uint8_t *sig __unused,
-				      size_t *sig_len __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-
-TEE_Result crypto_acipher_rsassa_verify(uint32_t algo __unused,
-					struct rsa_public_key *key __unused,
-					int salt_len __unused,
-					const uint8_t *msg __unused,
-					size_t msg_len __unused,
-					const uint8_t *sig __unused,
-					size_t sig_len __unused)
-{
-	return TEE_ERROR_NOT_IMPLEMENTED;
-}
-#endif /* CFG_CRYPTO_RSA */
 
 #if defined(CFG_CRYPTO_DSA)
 TEE_Result crypto_acipher_alloc_dsa_keypair(struct dsa_keypair *s __unused,
diff --git a/lib/libmbedtls/core/sub.mk b/lib/libmbedtls/core/sub.mk
index 2cc79bd6..0c02798f 100644
--- a/lib/libmbedtls/core/sub.mk
+++ b/lib/libmbedtls/core/sub.mk
@@ -24,3 +24,4 @@ srcs-$(CFG_CRYPTO_CMAC) += aes_cmac.c
 
 srcs-$(call cfg-one-enabled, CFG_CRYPTO_RSA CFG_CRYPTO_DSA \
 			     CFG_CRYPTO_DH CFG_CRYPTO_ECC) += bignum.c
+srcs-$(CFG_CRYPTO_RSA) += rsa.c
diff --git a/lib/libmbedtls/include/mbedtls_config_kernel.h b/lib/libmbedtls/include/mbedtls_config_kernel.h
index 7a803d61..b1950dbf 100644
--- a/lib/libmbedtls/include/mbedtls_config_kernel.h
+++ b/lib/libmbedtls/include/mbedtls_config_kernel.h
@@ -66,6 +66,22 @@
 #if defined(CFG_CRYPTO_RSA) || defined(CFG_CRYPTO_DSA) || \
     defined(CFG_CRYPTO_DH) || defined(CFG_CRYPTO_ECC)
 #define MBEDTLS_BIGNUM_C
+#define MBEDTLS_OID_C
+#define MBEDTLS_PKCS1_V15
+#define MBEDTLS_PKCS1_V21
+#define MBEDTLS_PK_C
+#define MBEDTLS_GENPRIME
+#define MBEDTLS_CTR_DRBG_C
+#endif
+
+#if defined(CFG_CRYPTO_RSA)
+#define MBEDTLS_RSA_C
+#define MBEDTLS_RSA_NO_CRT
+#endif
+
+#if defined(CFG_CRYPTO_RSA)
+#define MBEDTLS_ASN1_PARSE_C
+#define MBEDTLS_ASN1_WRITE_C
 #endif
 
 #endif /*CFG_CRYPTOLIB_NAME_mbedtls*/