diff --git a/core/arch/arm/plat-versal/conf.mk b/core/arch/arm/plat-versal/conf.mk
index 23a06be0d13..9099b7de95f 100644
--- a/core/arch/arm/plat-versal/conf.mk
+++ b/core/arch/arm/plat-versal/conf.mk
@@ -32,3 +32,19 @@ $(call force,CFG_WITH_LPAE,y)
 else
 $(call force,CFG_ARM32_core,y)
 endif
+
+$(call force, CFG_VERSAL_RNG_DRV, y)
+$(call force, CFG_WITH_SOFTWARE_PRNG,n)
+$(call force, CFG_VERSAL_PM, y)
+$(call force, CFG_VERSAL_MBOX, y)
+$(call force, CFG_VERSAL_NVM, y)
+
+# TRNG configuration
+CFG_VERSAL_TRNG_SEED_LIFE ?= 3
+CFG_VERSAL_TRNG_DF_MUL    ?= 7
+
+# MBOX configuration
+CFG_VERSAL_MBOX_IPI_ID ?=3
+
+# Crypto
+CFG_VERSAL_CRYPTO_DRIVER ?= n
diff --git a/core/arch/arm/plat-versal/main.c b/core/arch/arm/plat-versal/main.c
index 5eb7ee03a3f..1435b23a29f 100644
--- a/core/arch/arm/plat-versal/main.c
+++ b/core/arch/arm/plat-versal/main.c
@@ -7,6 +7,8 @@
 #include <console.h>
 #include <drivers/gic.h>
 #include <drivers/pl011.h>
+#include <drivers/versal_nvm.h>
+#include <drivers/versal_pm.h>
 #include <kernel/boot.h>
 #include <kernel/interrupt.h>
 #include <kernel/misc.h>
@@ -56,3 +58,20 @@ void console_init(void)
 		   CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
 	register_serial_console(&console_data.chip);
 }
+
+static TEE_Result platform_banner(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	uint8_t version = 0;
+
+	ret = versal_soc_version(&version);
+	if (ret) {
+		EMSG("Failure to retrieve SoC version");
+		return ret;
+	}
+
+	IMSG("Platform Versal - Silicon Revision v%d", version);
+
+	return TEE_SUCCESS;
+}
+service_init(platform_banner);
diff --git a/core/arch/arm/plat-versal/platform_config.h b/core/arch/arm/plat-versal/platform_config.h
index b95a279e7b2..613adf41cf9 100644
--- a/core/arch/arm/plat-versal/platform_config.h
+++ b/core/arch/arm/plat-versal/platform_config.h
@@ -9,7 +9,8 @@
 #include <mm/generic_ram_layout.h>
 
 /* Make stacks aligned to data cache line length */
-#define STACK_ALIGNMENT		64
+#define CACHELINE_LEN		64
+#define STACK_ALIGNMENT		CACHELINE_LEN
 
 #if defined(PLATFORM_FLAVOR_generic)
 
@@ -29,6 +30,9 @@
 #define DRAM0_BASE		0
 #define DRAM0_SIZE		0x80000000
 
+#define TRNG_BASE		0xF1230000
+#define TRNG_SIZE		0x10000
+
 #ifdef ARM64
 /* DDR High area base is only available when compiling for 64 bits */
 #define DRAM1_BASE		0x800000000
diff --git a/core/drivers/crypto/sub.mk b/core/drivers/crypto/sub.mk
index 9ff79bec4ad..3c26eda79a2 100644
--- a/core/drivers/crypto/sub.mk
+++ b/core/drivers/crypto/sub.mk
@@ -9,3 +9,5 @@ subdirs-$(CFG_NXP_SE05X) += se050
 subdirs-$(CFG_STM32_CRYPTO_DRIVER) += stm32
 
 subdirs-$(CFG_ASPEED_CRYPTO_DRIVER) += aspeed
+
+subdirs-$(CFG_VERSAL_CRYPTO_DRIVER) += versal
diff --git a/core/drivers/crypto/versal/authenc.c b/core/drivers/crypto/versal/authenc.c
new file mode 100644
index 00000000000..66afc1735c6
--- /dev/null
+++ b/core/drivers/crypto/versal/authenc.c
@@ -0,0 +1,435 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <assert.h>
+#include <crypto/crypto.h>
+#include <crypto/crypto_impl.h>
+#include <crypto/internal_aes-gcm.h>
+#include <drvcrypt.h>
+#include <drvcrypt_authenc.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <mm/core_memprot.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <tee_api_types.h>
+#include <tee/cache.h>
+#include <utee_defines.h>
+#include <util.h>
+
+#include "ipi.h"
+
+#define GCM_TAG_LEN		16
+
+#define	XSECURE_AES_KEY_SIZE_128  0   /* Key Length = 32 bytes = 256 bits */
+#define	XSECURE_AES_KEY_SIZE_256  2   /* Key Length = 16 bytes = 128 bits */
+
+#define XSECURE_ENCRYPT 0
+#define XSECURE_DECRYPT 1
+
+enum aes_key_src {
+	XSECURE_AES_BBRAM_KEY = 0,              /* BBRAM Key */
+	XSECURE_AES_BBRAM_RED_KEY,              /* BBRAM Red Key */
+	XSECURE_AES_BH_KEY,                     /* BH Key */
+	XSECURE_AES_BH_RED_KEY,                 /* BH Red Key */
+	XSECURE_AES_EFUSE_KEY,                  /* eFUSE Key */
+	XSECURE_AES_EFUSE_RED_KEY,              /* eFUSE Red Key */
+	XSECURE_AES_EFUSE_USER_KEY_0,           /* eFUSE User Key 0 */
+	XSECURE_AES_EFUSE_USER_KEY_1,           /* eFUSE User Key 1 */
+	XSECURE_AES_EFUSE_USER_RED_KEY_0,       /* eFUSE User Red Key 0 */
+	XSECURE_AES_EFUSE_USER_RED_KEY_1,       /* eFUSE User Red Key 1 */
+	XSECURE_AES_KUP_KEY,                    /* KUP key */
+	XSECURE_AES_PUF_KEY,                    /* PUF key */
+	XSECURE_AES_USER_KEY_0,                 /* User Key 0 */
+	XSECURE_AES_USER_KEY_1,                 /* User Key 1 */
+	XSECURE_AES_USER_KEY_2,                 /* User Key 2 */
+	XSECURE_AES_USER_KEY_3,                 /* User Key 3 */
+	XSECURE_AES_USER_KEY_4,                 /* User Key 4 */
+	XSECURE_AES_USER_KEY_5,                 /* User Key 5 */
+	XSECURE_AES_USER_KEY_6,                 /* User Key 6 */
+	XSECURE_AES_USER_KEY_7,                 /* User Key 7 */
+	XSECURE_AES_EXPANDED_KEYS,              /* Expanded keys */
+	XSECURE_AES_ALL_KEYS,                   /* AES All keys */
+};
+
+struct versal_aes_key {
+	uint32_t id;
+	struct refcount refc;
+	SLIST_ENTRY(versal_aes_key) link;
+};
+
+static unsigned int key_list_lock = SPINLOCK_UNLOCK;
+static SLIST_HEAD(, versal_aes_key) key_list = SLIST_HEAD_INITIALIZER(key_list);
+
+struct versal_ae_ctx {
+	struct crypto_authenc_ctx a_ctx;
+	struct versal_aes_key *key;
+};
+
+static struct versal_ae_ctx *to_versal_ctx(struct crypto_authenc_ctx *ctx)
+{
+	assert(ctx);
+
+	return container_of(ctx, struct versal_ae_ctx, a_ctx);
+}
+
+static TEE_Result do_init(struct drvcrypt_authenc_init *dinit)
+{
+	struct versal_ae_ctx *c = to_versal_ctx(dinit->ctx);
+	struct versal_aes_init *init = NULL;
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	uint32_t key_len = XSECURE_AES_KEY_SIZE_128;
+	void *p = NULL;
+	uint32_t exceptions = 0;
+
+	if (dinit->key.length != 32 && dinit->key.length != 16)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (dinit->key.length == 32)
+		key_len = XSECURE_AES_KEY_SIZE_256;
+
+	if (c->key)
+		goto init_op;
+
+	exceptions = cpu_spin_lock_xsave(&key_list_lock);
+	if (SLIST_EMPTY(&key_list)) {
+		cpu_spin_unlock_xrestore(&key_list_lock, exceptions);
+		return TEE_ERROR_BUSY;
+	}
+	c->key = SLIST_FIRST(&key_list);
+	SLIST_REMOVE_HEAD(&key_list, link);
+	cpu_spin_unlock_xrestore(&key_list_lock, exceptions);
+
+	/* initialize the AES engine */
+	if (versal_crypto_request(AES_INIT, &arg)) {
+		EMSG("AES_INIT error");
+		return TEE_ERROR_GENERIC;
+	}
+
+	/* write the key */
+	p = memalign(CACHELINE_LEN, ROUNDUP(dinit->key.length, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy(p, dinit->key.data, dinit->key.length);
+
+	arg.data[0] = key_len;
+	arg.data[1] = c->key->id;
+	arg.dlen = 2;
+
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = ROUNDUP(dinit->key.length, CACHELINE_LEN);
+
+	if (versal_crypto_request(AES_WRITE_KEY, &arg)) {
+		EMSG("AES_WRITE_KEY error");
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+	free(p);
+
+init_op:
+	/* send the initialization structure */
+	memset(&arg, 0, sizeof(arg));
+	init = memalign(CACHELINE_LEN, ROUNDUP(sizeof(*init), CACHELINE_LEN));
+	if (!init)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	p = memalign(CACHELINE_LEN, ROUNDUP(dinit->nonce.length,
+					    CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy(p, dinit->nonce.data, dinit->nonce.length);
+
+	init->iv_addr = virt_to_phys(p);
+	init->operation = dinit->encrypt ? XSECURE_ENCRYPT : XSECURE_DECRYPT;
+	init->key_src = c->key->id;
+	init->key_len = key_len;
+
+	arg.ibuf[0].p = init;
+	arg.ibuf[0].len = ROUNDUP(sizeof(*init), CACHELINE_LEN);
+	arg.ibuf[1].only_cache = true;
+	arg.ibuf[1].p = p;
+	arg.ibuf[1].len = ROUNDUP(dinit->nonce.length, CACHELINE_LEN);
+
+	if (versal_crypto_request(AES_OP_INIT, &arg)) {
+		EMSG("AES_OP_INIT error");
+		ret = TEE_ERROR_GENERIC;
+	}
+out:
+	free(p);
+	free(init);
+
+	return ret;
+}
+
+static TEE_Result do_update_aad(struct drvcrypt_authenc_update_aad *dupdate)
+{
+	struct versal_ae_ctx *c = to_versal_ctx(dupdate->ctx);
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	void *p = NULL;
+
+	p = memalign(CACHELINE_LEN,
+		     ROUNDUP(dupdate->aad.length, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy(p, dupdate->aad.data, dupdate->aad.length);
+
+	arg.data[0] = dupdate->aad.length;
+	arg.dlen = 1;
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = ROUNDUP(dupdate->aad.length, CACHELINE_LEN);
+
+	if (versal_crypto_request(AES_UPDATE_AAD, &arg)) {
+		EMSG("AES_UPDATE_AAD error");
+		ret = TEE_ERROR_GENERIC;
+	}
+
+	free(p);
+
+	return ret;
+}
+
+static TEE_Result update_payload(struct drvcrypt_authenc_update_payload
+				 *dupdate, bool is_last)
+{
+	struct versal_ae_ctx *c = to_versal_ctx(dupdate->ctx);
+	struct versal_aes_input_param *input = NULL;
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	enum versal_crypto_api id = AES_DECRYPT_UPDATE;
+	void *p = NULL;
+	void *q = NULL;
+
+	p = memalign(CACHELINE_LEN,
+		     ROUNDUP(dupdate->src.length, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	memcpy(p, dupdate->src.data, dupdate->src.length);
+
+	q = memalign(CACHELINE_LEN,
+		     ROUNDUP(dupdate->dst.length, CACHELINE_LEN));
+	if (!q)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memset(q, 0, ROUNDUP(dupdate->dst.length, CACHELINE_LEN));
+
+	input = memalign(CACHELINE_LEN,
+			 ROUNDUP(sizeof(*c), CACHELINE_LEN));
+	if (!input)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	input->input_addr = virt_to_phys(p);
+	input->input_len = dupdate->src.length;
+	input->is_last = is_last;
+
+	arg.ibuf[0].p = input;
+	arg.ibuf[0].len = ROUNDUP(sizeof(*c), CACHELINE_LEN);
+	arg.ibuf[1].p = q;
+	arg.ibuf[1].len = ROUNDUP(dupdate->dst.length, CACHELINE_LEN);
+	arg.ibuf[2].p = p;
+	arg.ibuf[2].len = ROUNDUP(dupdate->src.length, CACHELINE_LEN);
+
+	if (dupdate->encrypt)
+		id = AES_ENCRYPT_UPDATE;
+
+	if (versal_crypto_request(id, &arg)) {
+		EMSG("AES_UPDATE_PAYLOAD error");
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	memcpy(dupdate->dst.data, q, dupdate->dst.length);
+out:
+	free(p);
+	free(q);
+	free(input);
+
+	return ret;
+}
+
+static TEE_Result do_update_payload(struct drvcrypt_authenc_update_payload
+				    *dupdate)
+{
+	return update_payload(dupdate, false);
+}
+
+static TEE_Result do_enc_final(struct drvcrypt_authenc_final *dfinal)
+{
+	struct versal_ae_ctx *c = to_versal_ctx(dfinal->ctx);
+	struct drvcrypt_authenc_update_payload last = { };
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	uint8_t *p = NULL;
+
+	last.ctx = dfinal->ctx;
+	last.dst = dfinal->dst;
+	last.encrypt = true;
+	last.src = dfinal->src;
+
+	ret = update_payload(&last, true);
+	if (ret)
+		return ret;
+	memcpy(dfinal->dst.data, last.dst.data, dfinal->dst.length);
+
+	p = memalign(CACHELINE_LEN,
+		     ROUNDUP(GCM_TAG_LEN, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = ROUNDUP(GCM_TAG_LEN, CACHELINE_LEN);
+	if (versal_crypto_request(AES_ENCRYPT_FINAL, &arg)) {
+		EMSG("AES_ENCRYPT_FINAL error");
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	memcpy(dfinal->tag.data, p, GCM_TAG_LEN);
+	dfinal->tag.length = GCM_TAG_LEN;
+out:
+	free(p);
+
+	return ret;
+}
+
+static TEE_Result do_dec_final(struct drvcrypt_authenc_final *dfinal)
+{
+	struct versal_ae_ctx *c = to_versal_ctx(dfinal->ctx);
+	struct drvcrypt_authenc_update_payload last = { };
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	uint8_t *p = NULL;
+
+	last.ctx = dfinal->ctx;
+	last.dst = dfinal->dst;
+	last.encrypt = false;
+	last.src = dfinal->src;
+
+	ret = update_payload(&last, true);
+	if (ret)
+		return ret;
+
+	memcpy(dfinal->dst.data, last.dst.data, dfinal->dst.length);
+
+	p = memalign(CACHELINE_LEN,
+		ROUNDUP(GCM_TAG_LEN, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy(p, dfinal->tag.data, dfinal->tag.length);
+
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = ROUNDUP(GCM_TAG_LEN, CACHELINE_LEN);
+
+	if (versal_crypto_request(AES_DECRYPT_FINAL, &arg)) {
+		EMSG("AES_DECRYPT_FINAL error");
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	memcpy(dfinal->tag.data, p, GCM_TAG_LEN);
+	dfinal->tag.length = GCM_TAG_LEN;
+out:
+	free(p);
+
+	return ret;
+}
+
+static void do_final(void *ctx)
+{
+}
+
+static void do_free(void *ctx)
+{
+	struct versal_ae_ctx *c = to_versal_ctx(ctx);
+	uint32_t exceptions = 0;
+
+	exceptions = cpu_spin_lock_xsave(&key_list_lock);
+	if (refcount_dec(&c->key->refc)) {
+		refcount_set(&c->key->refc, 1);
+		SLIST_INSERT_HEAD(&key_list, c->key, link);
+	}
+	cpu_spin_unlock_xrestore(&key_list_lock, exceptions);
+	free(c);
+}
+
+static void do_copy_state(void *dst_ctx, void *src_ctx)
+{
+	struct versal_ae_ctx *src = to_versal_ctx(src_ctx);
+	struct versal_ae_ctx *dst = to_versal_ctx(dst_ctx);
+
+	memcpy(dst, src, sizeof(*dst));
+	refcount_inc(&src->key->refc);
+}
+
+static TEE_Result do_allocate(void **ctx, uint32_t algo)
+{
+	struct versal_ae_ctx *c = NULL;
+
+	if (algo != TEE_ALG_AES_GCM)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	c = calloc(1, sizeof(*c));
+	if (!c)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	*ctx = &c->a_ctx;
+
+	return TEE_SUCCESS;
+}
+
+static struct drvcrypt_authenc versal_authenc = {
+	.alloc_ctx = do_allocate,
+	.free_ctx = do_free,
+	.init = do_init,
+	.update_aad = do_update_aad,
+	.update_payload = do_update_payload,
+	.enc_final = do_enc_final,
+	.dec_final = do_dec_final,
+	.final = do_final,
+	.copy_state = do_copy_state,
+};
+
+/*
+ * This driver reserves all AE_USER_KEYS for its operation - perhaps use a
+ * CFG_ so the user can specify a range
+ */
+static TEE_Result versal_register_authenc(void)
+{
+	uint32_t user_keys[] = {
+		XSECURE_AES_USER_KEY_0, XSECURE_AES_USER_KEY_1,
+		XSECURE_AES_USER_KEY_2, XSECURE_AES_USER_KEY_3,
+		XSECURE_AES_USER_KEY_4, XSECURE_AES_USER_KEY_5,
+		XSECURE_AES_USER_KEY_6, XSECURE_AES_USER_KEY_7,
+	};
+	struct versal_aes_key *key = NULL;
+	TEE_Result ret = TEE_SUCCESS;
+	size_t i = 0;
+
+	ret = drvcrypt_register_authenc(&versal_authenc);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(user_keys); i++) {
+		key = calloc(1, sizeof (*key));
+		if (!key)
+			return TEE_ERROR_OUT_OF_MEMORY;
+
+		key->id = user_keys[i];
+		refcount_set(&key->refc, 1);
+		SLIST_INSERT_HEAD(&key_list, key, link);
+	}
+
+	return TEE_SUCCESS;
+}
+
+driver_init_late(versal_register_authenc);
diff --git a/core/drivers/crypto/versal/authenc_test.c b/core/drivers/crypto/versal/authenc_test.c
new file mode 100644
index 00000000000..83d40ebc58c
--- /dev/null
+++ b/core/drivers/crypto/versal/authenc_test.c
@@ -0,0 +1,291 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <crypto/crypto.h>
+#include <drvcrypt.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <string.h>
+
+#include "ipi.h"
+
+#define __STR(X) #X
+#define STR(X) __STR(X)
+
+#define u32 uint32_t
+#define u8  uint8_t
+
+#define XSECURE_GCM_TAG_SIZE	(16U)
+
+#define	XSECURE_AES_KEY	\
+	"F878B838D8589818E868A828C8488808F070B030D0509010E060A020C0408000"
+#define XSECURE_KEY_SIZE	(32)
+
+#define	XSECURE_IV		"D2450E07EA5DE0426C0FA133"
+#define XSECURE_IV_SIZE		(12)
+
+#define XSECURE_AAD		"67e21cf3cb29e0dcbc4d8b1d0cc5334b"
+#define XSECURE_AAD_SIZE	(16)
+
+static u8 Iv[XSECURE_IV_SIZE];
+static u8 Key[XSECURE_KEY_SIZE];
+static u8 Aad[XSECURE_AAD_SIZE];
+
+#define XSECURE_DATA	\
+	"1234567808F070B030D0509010E060A020C0408000A5DE08D85898A5A5FEDCA10134" \
+	"ABCDEF12345678900987654321123487654124456679874309713627463801AD1056"
+#define XSECURE_DATA_SIZE	(68)
+
+static uint8_t Data[XSECURE_DATA_SIZE];
+static uint8_t DecData[XSECURE_DATA_SIZE];
+static uint8_t EncData[XSECURE_DATA_SIZE];
+static uint8_t Tag[XSECURE_GCM_TAG_SIZE];
+
+
+#define XST_SUCCESS	0
+#define XST_FAILURE	1
+
+u32 Xil_ConvertCharToNibble(u8 InChar, u8 *Num)
+{
+	u32 Status;
+
+	/* Convert the char to nibble */
+	if ((InChar >= (u8)'0') && (InChar <= (u8)'9')) {
+		*Num = InChar - (u8)'0';
+		Status = XST_SUCCESS;
+	} else if ((InChar >= (u8)'a') && (InChar <= (u8)'f')) {
+		*Num = InChar - (u8)'a' + 10U;
+		Status = XST_SUCCESS;
+	} else if ((InChar >= (u8)'A') && (InChar <= (u8)'F')) {
+		*Num = InChar - (u8)'A' + 10U;
+		Status = XST_SUCCESS;
+	} else {
+		Status = XST_FAILURE;
+	}
+
+	return Status;
+}
+
+static u32 Secure_ConvertStringToHexBE(const char *Str, u8 *Buf, u32 Len)
+{
+	u32 ConvertedLen = 0;
+	u8 LowerNibble, UpperNibble;
+
+	/* Check the parameters */
+	if (Str == NULL)
+		return XST_FAILURE;
+
+	if (Buf == NULL)
+		return XST_FAILURE;
+
+	/* Len has to be multiple of 2 */
+	if ((Len == 0) || (Len % 2 == 1))
+		return XST_FAILURE;
+
+	ConvertedLen = 0;
+	while (ConvertedLen < Len) {
+		/* Convert char to nibble */
+		if (Xil_ConvertCharToNibble(Str[ConvertedLen],
+					    &UpperNibble) == XST_SUCCESS) {
+			/* Convert char to nibble */
+			if (Xil_ConvertCharToNibble(
+						    Str[ConvertedLen + 1],
+						    &LowerNibble) == XST_SUCCESS) {
+				/* Merge upper and lower nibble to Hex */
+				Buf[ConvertedLen / 2] =
+							(UpperNibble << 4) | LowerNibble;
+			} else {
+				/* Error converting Lower nibble */
+				return XST_FAILURE;
+			}
+		} else {
+			/* Error converting Upper nibble */
+			return XST_FAILURE;
+		}
+		ConvertedLen += 2;
+	}
+
+	return XST_SUCCESS;
+}
+
+static int do_init(void)
+{
+	int Status;
+
+	/* Covert strings to buffers */
+	Status = Secure_ConvertStringToHexBE(
+					     (const char *)(XSECURE_AES_KEY),
+					     Key, XSECURE_KEY_SIZE * 2);
+	if (Status != XST_SUCCESS) {
+		EMSG("String Conversion error (KEY):%08x !!!\r\n", Status);
+		return Status;
+	}
+
+	Status = Secure_ConvertStringToHexBE(
+					     (const char *)(XSECURE_IV),
+					     Iv, XSECURE_IV_SIZE * 2);
+	if (Status != XST_SUCCESS) {
+		EMSG("String Conversion error (IV):%08x !!!\r\n", Status);
+		return Status;
+	}
+
+	Status = Secure_ConvertStringToHexBE(
+					     (const char *)(XSECURE_AAD),
+					     Aad, XSECURE_AAD_SIZE * 2);
+	if (Status != XST_SUCCESS) {
+		EMSG("String Conversion error (IV):%08x !!!\r\n", Status);
+		return Status;
+	}
+
+
+	Status = Secure_ConvertStringToHexBE(
+					     (const char *)(XSECURE_DATA),
+					     Data, XSECURE_DATA_SIZE * 2);
+	if (Status != XST_SUCCESS) {
+		EMSG("String Conversion error (Data):%08x !!!\r\n", Status);
+		return Status;
+	}
+
+	return Status;
+}
+
+#define TEE_AES_BLOCK_SIZE  16UL
+
+static TEE_Result test_authenc_enc(void)
+{
+	size_t tag_len = XSECURE_GCM_TAG_SIZE;
+	size_t enc_len = XSECURE_DATA_SIZE;
+	TEE_Result ret = TEE_SUCCESS;
+	void *ctx;
+
+	ret = crypto_authenc_alloc_ctx(&ctx, TEE_ALG_AES_GCM);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	ret = crypto_authenc_init(ctx, TEE_MODE_ENCRYPT,
+				  Key, XSECURE_KEY_SIZE,
+				  Iv, XSECURE_IV_SIZE,
+				  TEE_AES_BLOCK_SIZE, 0, 0);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	ret = crypto_authenc_update_aad(ctx, TEE_MODE_ENCRYPT,
+					Aad, XSECURE_AAD_SIZE);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+#if 0
+	/* only one block
+	*  -> we cant send an update, needs to be done in final
+	*/
+	ret = crypto_authenc_update_payload(ctx, TEE_MODE_ENCRYPT,
+					    Data, XSECURE_DATA_SIZE);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+#endif
+	ret = crypto_authenc_enc_final(ctx,
+			Data, XSECURE_DATA_SIZE,
+			EncData, &enc_len,
+			Tag, &tag_len);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	crypto_authenc_final(ctx);
+	crypto_authenc_free_ctx(ctx);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_authenc_dec(void)
+{
+	size_t tag_len = XSECURE_GCM_TAG_SIZE;
+	size_t dec_len = XSECURE_DATA_SIZE;
+	TEE_Result ret = TEE_SUCCESS;
+	void *ctx;
+
+	ret = crypto_authenc_alloc_ctx(&ctx, TEE_ALG_AES_GCM);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	ret = crypto_authenc_init(ctx, TEE_MODE_DECRYPT,
+				  Key, XSECURE_KEY_SIZE,
+				  Iv, XSECURE_IV_SIZE,
+				  TEE_AES_BLOCK_SIZE, 0, 0);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	ret = crypto_authenc_update_aad(ctx, TEE_MODE_ENCRYPT,
+					Aad, XSECURE_AAD_SIZE);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	ret = crypto_authenc_dec_final(ctx,
+				       EncData, XSECURE_DATA_SIZE,
+				       DecData, &dec_len,
+				       Tag, tag_len);
+	if (ret) {
+		EMSG("%s %d", __func__, __LINE__);
+		return ret;
+	}
+
+	crypto_authenc_final(ctx);
+	crypto_authenc_free_ctx(ctx);
+
+	return TEE_SUCCESS;
+}
+
+static struct {
+	TEE_Result (*f)(void);
+	const char *name;
+	bool failed;
+} test[] = {
+	{ .f = test_authenc_enc, .name = STR(auth enc), },
+	{ .f = test_authenc_dec, .name = STR(auth dec), },
+};
+
+#define AES_USER_KEYS 8
+
+static TEE_Result versal_authenc_test(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t i = 0;
+
+	/* initialize the data and the global context */
+	do_init();
+
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		ret = (test[i].f)();
+		if (ret)
+			test[i].failed = ret;
+	}
+
+	IMSG("Versal: Test AUTHENC");
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		IMSG("---- %s:\t\t\t\t\t [%s]",
+		     test[i].name,
+		     test[i].failed ? "KO" : "OK");
+	}
+
+	return TEE_SUCCESS;;
+}
+
+boot_final(versal_authenc_test);
diff --git a/core/drivers/crypto/versal/crypto.mk b/core/drivers/crypto/versal/crypto.mk
new file mode 100644
index 00000000000..ab9582bc8ff
--- /dev/null
+++ b/core/drivers/crypto/versal/crypto.mk
@@ -0,0 +1,9 @@
+# Enable the crypto driver
+$(call force,CFG_CRYPTO_DRIVER,y)
+CFG_CRYPTO_DRIVER_DEBUG ?= 0
+
+$(call force,CFG_CRYPTO_DRV_ACIPHER,y)
+$(call force,CFG_CRYPTO_DRV_ECC,y)
+$(call force,CFG_CRYPTO_DRV_RSA,y)
+$(call force,CFG_CRYPTO_DRV_HASH,y)
+$(call force,CFG_CRYPTO_DRV_AUTHENC,y)
diff --git a/core/drivers/crypto/versal/ecc.c b/core/drivers/crypto/versal/ecc.c
new file mode 100644
index 00000000000..ed5bf5d53ab
--- /dev/null
+++ b/core/drivers/crypto/versal/ecc.c
@@ -0,0 +1,386 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <drvcrypt.h>
+#include <drvcrypt_acipher.h>
+#include <crypto/crypto_impl.h>
+#include <initcall.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "ipi.h"
+
+/* Xilinx Versal Known Answer Tests */
+#define XSECURE_ECDSA_KAT_NIST_P384	0
+#define XSECURE_ECDSA_KAT_NIST_P521	2
+
+static const struct crypto_ecc_keypair_ops *soft_keypair_ops;
+static const struct crypto_ecc_public_ops *soft_public_ops;
+
+static void crypto_bignum_bn2bin_eswap(struct bignum *from, uint8_t *to)
+{
+	uint8_t tmp = 0;
+	size_t i = 0;
+	size_t j = 0;
+
+	crypto_bignum_bn2bin(from, to);
+	for(i = 0, j = crypto_bignum_num_bytes(from) - 1; i < j; i++, j--) {
+		tmp = to[i];
+		to[i] = to[j];
+		to[j] = tmp;
+	}
+}
+
+static TEE_Result ecc_get_key_size(uint32_t curve, uint32_t algo,
+				   size_t *bytes, size_t *bits)
+{
+	switch (curve) {
+	case TEE_ECC_CURVE_NIST_P384:
+		*bits = 384;
+		*bytes = 48;
+		if (algo && algo != TEE_ALG_ECDSA_P384 &&
+		    algo != TEE_ALG_ECDH_P384)
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_ECC_CURVE_NIST_P521:
+		*bits = 521;
+		*bytes = 66;
+		if (algo && algo != TEE_ALG_ECDSA_P521 &&
+		    algo != TEE_ALG_ECDH_P521)
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result ecc_prepare_msg(uint32_t algo, const uint8_t *msg,
+				  size_t *msg_len, uint8_t **p)
+{
+	size_t len = 0;
+
+	switch (algo) {
+	case TEE_ALG_ECDSA_P384:
+		len = TEE_SHA384_HASH_SIZE;
+		break;
+	case TEE_ALG_ECDSA_P521:
+		len = TEE_SHA512_HASH_SIZE;
+		break;
+	default:
+		return TEE_ERROR_GENERIC;
+	}
+
+	if (*msg_len >= len)
+		*msg_len = len;
+
+	*p = memalign(CACHELINE_LEN, ROUNDUP(len, CACHELINE_LEN));
+	if (!*p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memset(*p, 0, ROUNDUP(len, CACHELINE_LEN));
+	memcpy(*p, msg, *msg_len);
+	*msg_len = len;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result verify(uint32_t algo, struct ecc_public_key *key,
+			 const uint8_t *msg, size_t msg_len,
+			 const uint8_t *sig, size_t sig_len)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	struct versal_ecc_verify_param *cmd = NULL;
+	struct cmd_args arg = { };
+	size_t key_bits = 0;
+	size_t key_bytes = 0;
+	uint8_t *p = NULL;
+	uint8_t *x = NULL;
+	uint8_t *s = NULL;
+	size_t xlen = 0;
+	size_t slen = 0;
+
+	ret = ecc_get_key_size(key->curve, 0, &key_bytes, &key_bits);
+	if (ret != TEE_SUCCESS)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = ecc_prepare_msg(algo, msg, &msg_len, &p);
+	if (ret)
+		return ret;
+
+	/* Public key */
+	xlen = ROUNDUP(crypto_bignum_num_bytes(key->x) +
+		       crypto_bignum_num_bytes(key->y), CACHELINE_LEN);
+
+	x = memalign(CACHELINE_LEN, xlen);
+	if (!x)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	memset(x, 0, xlen);
+
+	crypto_bignum_bn2bin_eswap(key->x, x);
+	crypto_bignum_bn2bin_eswap(key->y, (uint8_t *)
+				   x + crypto_bignum_num_bytes(key->x));
+
+	/* Signature */
+	slen = ROUNDUP(sig_len, CACHELINE_LEN);
+	s = memalign(CACHELINE_LEN, slen);
+	if (!s) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+
+	}
+	memset(s, 0, slen);
+	memcpy(s, sig, sig_len);
+
+	/* IPI cmd */
+	cmd = memalign(CACHELINE_LEN, ROUNDUP(sizeof(*cmd), CACHELINE_LEN));
+	if (!cmd) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	cmd->hash_addr = virt_to_phys(p);
+	cmd->hash_len = msg_len;
+	cmd->pub_key_addr = virt_to_phys(x);
+	cmd->signature_addr = virt_to_phys(s);
+	cmd->curve = key->curve;
+
+	/* Src */
+	arg.ibuf[0].p = cmd;
+	arg.ibuf[0].len = ROUNDUP(sizeof(*cmd), CACHELINE_LEN);
+	/* Data */
+	arg.ibuf[1].p = p;
+	arg.ibuf[1].len = ROUNDUP(msg_len, CACHELINE_LEN);
+	arg.ibuf[1].only_cache = true;
+	arg.ibuf[2].p = x;
+	arg.ibuf[2].len = xlen;
+	arg.ibuf[3].p = s;
+	arg.ibuf[3].len = slen;
+	if (versal_crypto_request(ELLIPTIC_VERIFY_SIGN, &arg))
+		ret = TEE_ERROR_GENERIC;
+out:
+	free(p);
+	free(x);
+	free(s);
+	free(cmd);
+
+	return ret;
+}
+
+static TEE_Result sign(uint32_t algo, struct ecc_keypair *key,
+		       const uint8_t *msg, size_t msg_len,
+		       uint8_t *sig, size_t *sig_len)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	struct versal_ecc_sign_param *cmd = NULL;
+	struct cmd_args arg = { };
+	size_t key_bits = 0;
+	size_t key_bytes = 0;
+	uint8_t *p = NULL;
+	uint8_t *k = NULL;
+	uint8_t *d = NULL;
+	uint8_t *s = NULL;
+
+	ret = ecc_get_key_size(key->curve, 0, &key_bytes, &key_bits);
+	if (ret != TEE_SUCCESS)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = ecc_prepare_msg(algo, msg, &msg_len, &p);
+	if (ret)
+		return ret;
+
+	/* Ephemeral private key */
+	k = memalign(CACHELINE_LEN, ROUNDUP(key_bytes, CACHELINE_LEN));
+	if (!k) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	ret = crypto_rng_read(k, key_bytes);
+	if (ret)
+		goto out;
+
+	/* Private key*/
+	d = memalign(CACHELINE_LEN, ROUNDUP(key_bytes, CACHELINE_LEN));
+	if (!d) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	crypto_bignum_bn2bin_eswap(key->d, d);
+
+	/* Signature */
+	s = memalign(CACHELINE_LEN, ROUNDUP(*sig_len, CACHELINE_LEN));
+	if (!s) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	memset(s, 0x00, ROUNDUP(*sig_len, CACHELINE_LEN));
+
+	/* IPI command */
+	cmd = memalign(CACHELINE_LEN, ROUNDUP(sizeof(*cmd), CACHELINE_LEN));
+	if (!cmd) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	cmd->curve = key->curve;
+	cmd->hash_addr = virt_to_phys(p);
+	cmd->hash_len = msg_len;
+	cmd->priv_key_addr = virt_to_phys(d);
+	cmd->epriv_key_addr = virt_to_phys(k);
+
+	/* Src */
+	arg.ibuf[0].p = cmd;
+	arg.ibuf[0].len = ROUNDUP(sizeof(*cmd), CACHELINE_LEN);
+	/* Dst */
+	arg.ibuf[1].p = s;
+	arg.ibuf[1].len = ROUNDUP(*sig_len, CACHELINE_LEN);
+	/* Data */
+	arg.ibuf[4].p = p;
+	arg.ibuf[4].len = ROUNDUP(msg_len, CACHELINE_LEN);
+	arg.ibuf[3].p = d;
+	arg.ibuf[3].len = ROUNDUP(key_bytes, CACHELINE_LEN);
+	arg.ibuf[2].p = k;
+	arg.ibuf[2].len = ROUNDUP(key_bytes, CACHELINE_LEN);
+
+	if (versal_crypto_request(ELLIPTIC_GENERATE_SIGN, &arg)) {
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	memcpy(sig, s, *sig_len);
+out:
+	free(cmd);
+	free(k);
+	free(p);
+	free(s);
+	free(d);
+
+	return ret;
+}
+
+static TEE_Result shared_secret(struct ecc_keypair *private_key,
+				struct ecc_public_key *public_key,
+				void *secret, size_t *secret_len)
+{
+	return soft_keypair_ops->shared_secret(private_key, public_key,
+					       secret, secret_len);
+}
+
+static TEE_Result do_shared_secret(struct drvcrypt_secret_data *sdata)
+{
+	return shared_secret(sdata->key_priv,
+			     sdata->key_pub,
+			     sdata->secret.data,
+			     &sdata->secret.length);
+}
+
+static TEE_Result do_sign(struct drvcrypt_sign_data *sdata)
+{
+	return sign(sdata->algo,
+		    sdata->key,
+		    sdata->message.data,
+		    sdata->message.length,
+		    sdata->signature.data,
+		    &sdata->signature.length);
+}
+
+static TEE_Result do_verify(struct drvcrypt_sign_data *sdata)
+{
+	return verify(sdata->algo,
+		      sdata->key,
+		      sdata->message.data,
+		      sdata->message.length,
+		      sdata->signature.data,
+		      sdata->signature.length);
+}
+
+static TEE_Result do_gen_keypair(struct ecc_keypair *keypair, size_t size_bytes)
+{
+	/* Versal requires little endian so need to eswap on Versal IP ops */
+	return soft_keypair_ops->generate(keypair, size_bytes);
+}
+
+static TEE_Result do_alloc_keypair(struct ecc_keypair *s, size_t size_bits)
+{
+	TEE_Result ret = TEE_SUCCESS;
+
+	ret = crypto_asym_alloc_ecc_keypair(s, TEE_TYPE_ECDSA_KEYPAIR,
+					    size_bits);
+	if (!ret)
+		s->ops = NULL;
+
+	return ret;
+}
+
+static TEE_Result do_alloc_publickey(struct ecc_public_key *s, size_t size_bits)
+{
+	TEE_Result ret = TEE_SUCCESS;
+
+	ret = crypto_asym_alloc_ecc_public_key(s, TEE_TYPE_ECDSA_PUBLIC_KEY,
+					       size_bits);
+	if (!ret)
+		s->ops = NULL;
+
+	return ret;
+}
+
+static void do_free_publickey(struct ecc_public_key *s)
+{
+	return soft_public_ops->free(s);
+}
+
+static struct drvcrypt_ecc driver_ecc = {
+	.alloc_keypair = do_alloc_keypair,
+	.alloc_publickey = do_alloc_publickey,
+	.free_publickey = do_free_publickey,
+	.gen_keypair = do_gen_keypair,
+	.sign = do_sign,
+	.verify = do_verify,
+	.shared_secret = do_shared_secret,
+};
+
+static TEE_Result ecc_init(void)
+{
+	struct ecc_public_key public_dummy = { };
+	struct ecc_keypair pair_dummy = { };
+	struct cmd_args arg = { .dlen = 1 };
+	TEE_Result ret = TEE_ERROR_GENERIC;
+
+	arg.data[0] = XSECURE_ECDSA_KAT_NIST_P384;
+	if (versal_crypto_request(ELLIPTIC_KAT, &arg)) {
+		EMSG("Versal KAG NIST_P384 failed");
+		return TEE_ERROR_GENERIC;
+	}
+
+	arg.data[0] = XSECURE_ECDSA_KAT_NIST_P521;
+	if (versal_crypto_request(ELLIPTIC_KAT, &arg)) {
+		EMSG("Versal KAG NIST_P521 failed");
+		return TEE_ERROR_GENERIC;
+	}
+
+	ret = crypto_asym_alloc_ecc_keypair(&pair_dummy,
+					    TEE_TYPE_ECDSA_KEYPAIR, 0);
+	if (ret)
+		return ret;
+
+	ret = crypto_asym_alloc_ecc_public_key(&public_dummy,
+					       TEE_TYPE_ECDSA_PUBLIC_KEY, 0);
+	if (ret)
+		return ret;
+
+	public_dummy.ops->free(&public_dummy);
+
+	soft_keypair_ops = pair_dummy.ops;
+	soft_public_ops = public_dummy.ops;
+
+	return drvcrypt_register_ecc(&driver_ecc);
+}
+
+driver_init(ecc_init);
diff --git a/core/drivers/crypto/versal/ecc_test.c b/core/drivers/crypto/versal/ecc_test.c
new file mode 100644
index 00000000000..2a5533caff5
--- /dev/null
+++ b/core/drivers/crypto/versal/ecc_test.c
@@ -0,0 +1,179 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+
+#include <drvcrypt.h>
+#include <drvcrypt_acipher.h>
+#include <crypto/crypto_impl.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "ipi.h"
+
+#define __STR(X) #X
+#define STR(X) __STR(X)
+
+static struct ecc_keypair keypair = { };
+static struct ecc_public_key key = { };
+
+/* size for TEE_ALG_ECDSA_P384 */
+#define TEE_ALG_ECDSA_P384_SIG_LEN 96
+
+size_t sig_len = TEE_ALG_ECDSA_P384_SIG_LEN;
+uint8_t sig[TEE_ALG_ECDSA_P384_SIG_LEN];
+uint8_t msg[48] = {
+	0x89U, 0x1EU, 0x78U, 0x0AU, 0x0EU, 0xF7U, 0x8AU, 0x2BU,
+	0xCBU, 0xD6U, 0x30U, 0x6CU, 0x9DU, 0x14U, 0x11U, 0x74U,
+	0x5AU, 0x8BU, 0x3FU, 0x0BU, 0x5EU, 0x9FU, 0x52U, 0xC9U,
+	0x99U, 0x02U, 0xEEU, 0x49U, 0x70U, 0xBCU, 0xDBU, 0x6AU,
+	0x6CU, 0x83U, 0x6DU, 0x12U, 0x20U, 0x7DU, 0x05U, 0x35U,
+	0x1BU, 0x6EU, 0x4FU, 0x1CU, 0x7DU, 0x18U, 0xEAU, 0x5AU,
+};
+
+static void crypto_bignum_bn2bin_eswap(struct bignum *from, uint8_t *to)
+{
+	uint8_t tmp = 0;
+	size_t i = 0;
+	size_t j = 0;
+
+	crypto_bignum_bn2bin(from, to);
+	for(i = 0, j = crypto_bignum_num_bytes(from) - 1; i < j; i++, j--) {
+		tmp = to[i];
+		to[i] = to[j];
+		to[j] = tmp;
+	}
+}
+
+static TEE_Result test_generate_signature(void)
+{
+	return keypair.ops->sign(TEE_ALG_ECDSA_P384, &keypair, msg, sizeof(msg),
+				sig, &sig_len);
+}
+
+static TEE_Result test_validate_signature(void)
+{
+	return key.ops->verify(TEE_ALG_ECDSA_P384, &key, msg, sizeof(msg),
+			       sig, sig_len);
+}
+
+static TEE_Result test_validate_keypair_gen(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	void *p = NULL;
+	void *q = NULL;
+	size_t plen = 0;
+	size_t qlen = 0;
+
+	ret = drvcrypt_asym_alloc_ecc_keypair(&keypair,
+					      TEE_TYPE_ECDSA_KEYPAIR, 1024);
+	if (ret)
+		return ret;
+
+	keypair.curve = TEE_ECC_CURVE_NIST_P384;
+
+	ret = crypto_acipher_gen_ecc_key(&keypair, 1024);
+	if (ret)
+		return ret;
+
+	plen = ROUNDUP(crypto_bignum_num_bytes(keypair.x) +
+		       crypto_bignum_num_bytes(keypair.y), CACHELINE_LEN);
+
+	p = memalign(CACHELINE_LEN, plen);
+	if (!p)
+		return TEE_ERROR_GENERIC;
+
+	memset(p, 0, plen);
+	crypto_bignum_bn2bin_eswap(keypair.x, p);
+	crypto_bignum_bn2bin_eswap(keypair.y,
+			     (uint8_t *)p + crypto_bignum_num_bytes(keypair.x));
+
+	cache_operation(TEE_CACHEFLUSH, p, plen);
+
+	/* IPI cmd */
+	arg.data[0] = TEE_ECC_CURVE_NIST_P384;
+	arg.dlen = 1;
+	/* src */
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = plen;
+
+	if (!versal_crypto_request(ELLIPTIC_VALIDATE_PUBLIC_KEY, &arg))
+		goto public;
+
+	ret = TEE_ERROR_GENERIC;
+
+	qlen = crypto_bignum_num_bytes(keypair.d);
+	q = malloc(qlen);
+	if (!q)
+		return TEE_ERROR_GENERIC;
+
+	crypto_bignum_bn2bin(keypair.d, q);
+#if 0
+	IMSG("Privat Key: d = %ld", crypto_bignum_num_bytes(keypair.d));
+	DHEXDUMP(q, qlen);
+
+	IMSG("Public Key: x = %ld", crypto_bignum_num_bytes(keypair.x));
+	DHEXDUMP(p, crypto_bignum_num_bytes(keypair.x));
+
+	IMSG("Public Key: y = %ld", crypto_bignum_num_bytes(keypair.y));
+	DHEXDUMP( (uint8_t *)p + crypto_bignum_num_bytes(keypair.x),
+		 crypto_bignum_num_bytes(keypair.y));
+#endif
+public:
+	/*
+	 * Create a public key for the private key so we can verify on the
+	 * next test in the sequence
+	 */
+	if (drvcrypt_asym_alloc_ecc_public_key(&key, TEE_TYPE_ECDSA_PUBLIC_KEY,
+		1024)) {
+		/* panic since there is no reason to test further */
+		panic();
+	}
+	key.curve = TEE_ECC_CURVE_NIST_P384;
+	key.x = keypair.x;
+	key.y = keypair.y;
+
+	free(q);
+	free(p);
+
+	return ret;
+}
+
+static struct {
+	TEE_Result (*f)(void);
+	const char *name;
+	bool failed;
+} test[] = {
+	{ .f = test_validate_keypair_gen, .name = STR(ecc gen pair), },
+	{ .f = test_generate_signature,   .name = STR(ecc gen sign), },
+	{ .f = test_validate_signature,   .name = STR(ecc ver sign), },
+};
+
+static TEE_Result versal_crypto_test(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		ret = (test[i].f)();
+		if (ret)
+			test[i].failed = true;
+	}
+
+	IMSG("Versal: Test ECC");
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		IMSG("---- %s:\t\t\t\t [%s]",
+		     test[i].name,
+		     test[i].failed ? "KO" : "OK");
+	}
+
+	return TEE_SUCCESS;;
+}
+
+driver_init_late(versal_crypto_test);
diff --git a/core/drivers/crypto/versal/hash.c b/core/drivers/crypto/versal/hash.c
new file mode 100644
index 00000000000..69796aa6370
--- /dev/null
+++ b/core/drivers/crypto/versal/hash.c
@@ -0,0 +1,168 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <assert.h>
+#include <drvcrypt.h>
+#include <drvcrypt_hash.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+
+#include "ipi.h"
+
+#define FIRST_PACKET	BIT(30)
+#define NEXT_PACKET	BIT(31)
+
+enum versal_sha3_state { SHA3_STNDBY = 0, SHA3_INIT , SHA3_RUN };
+
+struct versal_hash_ctx {
+	struct crypto_hash_ctx hash_ctx;
+	enum versal_sha3_state state;
+};
+
+static const struct crypto_hash_ops versal_ops;
+
+static struct versal_hash_ctx* to_versal_ctx(struct crypto_hash_ctx *ctx)
+{
+	assert(ctx && ctx->ops == &versal_ops);
+
+	return container_of(ctx, struct versal_hash_ctx, hash_ctx);
+}
+
+static TEE_Result do_hash_init(struct crypto_hash_ctx *ctx)
+{
+	struct versal_hash_ctx *c = to_versal_ctx(ctx);
+
+	c->state = SHA3_INIT;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result do_hash_update(struct crypto_hash_ctx *ctx,
+				 const uint8_t *data, size_t len)
+{
+	struct versal_hash_ctx *c = to_versal_ctx(ctx);
+	struct cmd_args arg = { };
+	TEE_Result ret = TEE_SUCCESS;
+	uint32_t init_mask = 0;
+	uint8_t *p = NULL;
+
+	if (c->state == SHA3_STNDBY)
+		return TEE_ERROR_GENERIC;
+
+	if (c->state == SHA3_INIT)
+		init_mask = FIRST_PACKET;
+
+	p = memalign(CACHELINE_LEN, ROUNDUP(len, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy(p, data, len);
+
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = ROUNDUP(len, CACHELINE_LEN);
+	arg.data[0] = NEXT_PACKET | init_mask | len;
+
+	if (versal_crypto_request(SHA3_UPDATE, &arg))
+		ret = TEE_ERROR_GENERIC;
+	else
+		c->state = SHA3_RUN;
+
+	free(p);
+
+	return ret;
+}
+
+static TEE_Result do_hash_final(struct crypto_hash_ctx *ctx,
+				uint8_t *digest, size_t len)
+{
+	struct versal_hash_ctx *c = to_versal_ctx(ctx);
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	uint8_t *p = NULL;
+
+	if (c->state == SHA3_STNDBY)
+		return TEE_ERROR_GENERIC;
+
+	p = memalign(CACHELINE_LEN, ROUNDUP(len, CACHELINE_LEN));
+	if (!p)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	arg.ibuf[0].p = p;
+	arg.ibuf[0].len = ROUNDUP(len, CACHELINE_LEN);
+
+	if (versal_crypto_request(SHA3_UPDATE, &arg))
+		ret = TEE_ERROR_GENERIC;
+	else
+		c->state = SHA3_STNDBY;
+
+	memcpy(digest, p, len);
+
+	free(p);
+
+	return ret;
+}
+
+static void do_hash_copy_state(struct crypto_hash_ctx *dst_ctx,
+			       struct crypto_hash_ctx *src_ctx)
+{
+	struct versal_hash_ctx *src_hctx = NULL;
+	struct versal_hash_ctx *dst_hctx = NULL;
+
+	src_hctx = container_of(src_ctx, struct versal_hash_ctx, hash_ctx);
+	dst_hctx = container_of(dst_ctx, struct versal_hash_ctx, hash_ctx);
+
+	memcpy(dst_hctx, src_hctx, sizeof(*dst_hctx));
+}
+
+static void do_hash_free(struct crypto_hash_ctx *ctx)
+{
+	struct versal_hash_ctx *hctx = NULL;
+
+	hctx = container_of(ctx, struct versal_hash_ctx, hash_ctx);
+
+	free(hctx);
+}
+
+static TEE_Result versal_hash_alloc(struct crypto_hash_ctx **ctx,
+				    uint32_t algo)
+{
+	struct versal_hash_ctx *vctx = NULL;
+
+	if (algo != TEE_ALG_SHA384)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	vctx = calloc(1, sizeof(*vctx));
+	if (!vctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	vctx->hash_ctx.ops = &versal_ops;
+	*ctx = &vctx->hash_ctx;
+
+	return TEE_SUCCESS;
+}
+
+
+static const struct crypto_hash_ops versal_ops = {
+	.init = do_hash_init,
+	.update = do_hash_update,
+	.final = do_hash_final,
+	.free_ctx = do_hash_free,
+	.copy_state = do_hash_copy_state,
+};
+
+static TEE_Result sha3_init(void)
+{
+	struct cmd_args arg = { };
+
+	if (versal_crypto_request(SHA3_KAT, &arg))
+		panic();
+
+	return drvcrypt_register_hash(versal_hash_alloc);
+}
+
+driver_init(sha3_init);
diff --git a/core/drivers/crypto/versal/hash_test.c b/core/drivers/crypto/versal/hash_test.c
new file mode 100644
index 00000000000..04d64294f82
--- /dev/null
+++ b/core/drivers/crypto/versal/hash_test.c
@@ -0,0 +1,102 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <crypto/crypto.h>
+#include <drvcrypt.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <string.h>
+
+#include "ipi.h"
+
+#define __STR(X) #X
+#define STR(X) __STR(X)
+
+#define SHA3_HASH_LEN		48
+#define SHA3_INPUT_DATA_LEN	6
+
+static const uint8_t data[SHA3_INPUT_DATA_LEN + 1] = "XILINX";
+
+static uint8_t expected[SHA3_HASH_LEN] = {
+	0x70, 0x69, 0x77, 0x35, 0x0b, 0x93,
+	0x92, 0xa0, 0x48, 0x2c, 0xd8, 0x23,
+	0x38, 0x47, 0xd2, 0xd9, 0x2d, 0x1a,
+	0x95, 0x0c, 0xad, 0xa8, 0x60, 0xc0,
+	0x9b, 0x70, 0xc6, 0xad, 0x6e, 0xf1,
+	0x5d, 0x49, 0x68, 0xa3, 0x50, 0x75,
+	0x06, 0xbb, 0x0b, 0x9b, 0x03, 0x7d,
+	0xd5, 0x93, 0x76, 0x50, 0xdb, 0xd4
+};
+static uint8_t out[SHA3_HASH_LEN];
+
+static TEE_Result test_sha3(void)
+{
+	TEE_Result res = TEE_SUCCESS;
+	void *ctx;
+
+	res = crypto_hash_alloc_ctx(&ctx, TEE_ALG_SHA384);
+	if (res != TEE_SUCCESS) {
+		EMSG("%s %d", __func__, __LINE__);
+		return res;
+	}
+
+	res = crypto_hash_init(ctx);
+	if (res != TEE_SUCCESS) {
+		EMSG("%s %d", __func__, __LINE__);
+		return res;
+	}
+
+	res = crypto_hash_update(ctx, data, SHA3_INPUT_DATA_LEN);
+	if (res != TEE_SUCCESS) {
+		EMSG("%s %d", __func__, __LINE__);
+		return res;
+	}
+
+	res = crypto_hash_final(ctx, out, SHA3_HASH_LEN);
+	if (res != TEE_SUCCESS) {
+		EMSG("%s %d", __func__, __LINE__);
+		return res;
+	}
+
+	if (memcmp(out, expected, sizeof(expected))) {
+		EMSG("%s %d", __func__, __LINE__);
+		return TEE_ERROR_GENERIC;
+	}
+
+	return TEE_SUCCESS;
+}
+
+
+static struct {
+	TEE_Result (*f)(void);
+	const char *name;
+	bool failed;
+} test[] = {
+	{ .f = test_sha3, .name = STR(hash sha384), },
+};
+
+static TEE_Result versal_sha3_test(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		ret = (test[i].f)();
+		if (ret)
+			test[i].failed = true;
+	}
+
+	IMSG("Versal: Test HASH");
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		IMSG("---- %s:\t\t\t\t\t [%s]",
+		     test[i].name,
+		     test[i].failed ? "KO" : "OK");
+	}
+
+	return TEE_SUCCESS;;
+}
+
+boot_final(versal_sha3_test);
diff --git a/core/drivers/crypto/versal/include/ipi.h b/core/drivers/crypto/versal/include/ipi.h
new file mode 100644
index 00000000000..e8e2ea19bb1
--- /dev/null
+++ b/core/drivers/crypto/versal/include/ipi.h
@@ -0,0 +1,97 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef IPI_H
+#define IPI_H
+
+#include <drivers/versal_mbox.h>
+
+struct versal_rsa_input_param {
+	uint64_t key_addr;
+	uint64_t data_addr;
+	uint32_t key_len;
+}__packed;;
+
+struct versal_rsa_sign_param {
+	uint64_t sign_addr;
+	uint64_t hash_addr;
+	uint32_t hash_len;
+}__packed;;
+
+struct versal_ecc_sign_param {
+	uint64_t hash_addr;
+	uint64_t priv_key_addr;
+	uint64_t epriv_key_addr;
+	uint32_t curve;
+	uint32_t hash_len;
+}__packed;
+
+struct versal_ecc_verify_param {
+	uint64_t hash_addr;
+	uint64_t pub_key_addr;
+	uint64_t signature_addr;
+	uint32_t curve;
+	uint32_t hash_len;
+};
+
+enum versal_aes_operation { ENCRYPT, DECRYPT };
+
+struct versal_aes_init {
+	uint64_t iv_addr;
+	uint32_t operation;
+	uint32_t key_src;
+	uint32_t key_len;
+}__packed;
+
+struct versal_aes_input_param {
+	uint64_t input_addr;
+	uint32_t input_len;
+	uint32_t is_last;
+}__packed;
+
+enum versal_crypto_api {
+	FEATURES = 0U,
+	RSA_SIGN_VERIFY,
+	RSA_PUBLIC_ENCRYPT,
+	RSA_PRIVATE_DECRYPT,
+	RSA_KAT,
+	SHA3_UPDATE = 32U,
+	SHA3_KAT,
+	ELLIPTIC_GENERATE_PUBLIC_KEY = 64U,
+	ELLIPTIC_GENERATE_SIGN,
+	ELLIPTIC_VALIDATE_PUBLIC_KEY,
+	ELLIPTIC_VERIFY_SIGN,
+	ELLIPTIC_KAT,
+	AES_INIT = 96U,
+	AES_OP_INIT,
+	AES_UPDATE_AAD,
+	AES_ENCRYPT_UPDATE,
+	AES_ENCRYPT_FINAL,
+	AES_DECRYPT_UPDATE,
+	AES_DECRYPT_FINAL,
+	AES_KEY_ZERO,
+	AES_WRITE_KEY,
+	AES_LOCK_USER_KEY,
+	AES_KEK_DECRYPT,
+	AES_SET_DPA_CM,
+	AES_DECRYPT_KAT,
+	AES_DECRYPT_CM_KAT,
+	MAX,
+};
+
+#define MAX_IPI_REGS 6
+
+struct cmd_args {
+	uint32_t data[MAX_IPI_REGS];
+	size_t dlen;
+	struct ipi_buf ibuf[MAX_IPI_BUF];
+};
+
+TEE_Result versal_crypto_request(enum versal_crypto_api id,
+				 struct cmd_args *arg);
+
+#endif
+
diff --git a/core/drivers/crypto/versal/ipi.c b/core/drivers/crypto/versal/ipi.c
new file mode 100644
index 00000000000..96d89dcf9e9
--- /dev/null
+++ b/core/drivers/crypto/versal/ipi.c
@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <arm.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "ipi.h"
+
+#define SEC_MODULE_SHIFT 8
+#define SEC_MODULE_ID 5
+
+#define CRYPTO_API_ID(__x) ((SEC_MODULE_ID << SEC_MODULE_SHIFT) | (__x))
+
+static TEE_Result versal_sha3_request(enum versal_crypto_api id,
+				      struct cmd_args *arg)
+{
+	struct ipi_cmd cmd = { };
+
+	cmd.data[0] = CRYPTO_API_ID(id);
+	if (arg->data[0]) {
+		/* write */
+		cmd.data[1] = virt_to_phys(arg->ibuf[0].p);
+		cmd.data[2] = virt_to_phys(arg->ibuf[0].p) >> 32;
+		cmd.data[3] = arg->data[0];
+
+		cmd.ibuf[0].p = arg->ibuf[0].p;
+		cmd.ibuf[0].len = arg->ibuf[0].len;
+	} else {
+		/* read */
+		cmd.data[4] = virt_to_phys(arg->ibuf[0].p);
+		cmd.data[5] = virt_to_phys(arg->ibuf[0].p) >> 32;
+
+		cmd.ibuf[0].p = arg->ibuf[0].p;
+		cmd.ibuf[0].len = arg->ibuf[0].len;
+	}
+
+	return 	versal_mbox_notify(&cmd, NULL);
+}
+
+static TEE_Result versal_aes_update_aad_request(enum versal_crypto_api id,
+						struct cmd_args *arg)
+{
+	struct ipi_cmd cmd = { };
+
+	cmd.data[0] = CRYPTO_API_ID(id);
+	cmd.data[1] = virt_to_phys(arg->ibuf[0].p);
+	cmd.data[2] = virt_to_phys(arg->ibuf[0].p) >> 32;
+	cmd.data[3] = arg->data[0];
+
+	cmd.ibuf[0].p = arg->ibuf[0].p;
+	cmd.ibuf[0].len = arg->ibuf[0].len;
+
+	return versal_mbox_notify(&cmd, NULL);
+}
+
+
+TEE_Result versal_crypto_request(enum versal_crypto_api id,
+				 struct cmd_args *arg)
+{
+	struct ipi_cmd cmd = { };
+	size_t i = 0;
+
+	if (id == SHA3_UPDATE)
+		return versal_sha3_request(id, arg);
+
+	if (id == AES_UPDATE_AAD)
+		return versal_aes_update_aad_request(id, arg);
+
+	cmd.data[i] = CRYPTO_API_ID(id);
+	for (i = 1; i < arg->dlen + 1; i++)
+		cmd.data[i] = arg->data[i - 1];
+
+	/* src */
+	if (!arg->ibuf[0].p)
+		goto notify;
+
+	cmd.data[i++] = virt_to_phys(arg->ibuf[0].p);
+	cmd.data[i++] = virt_to_phys(arg->ibuf[0].p) >> 32;
+
+	/* dst */
+	if (!arg->ibuf[1].p)
+		goto cache;
+
+	if (arg->ibuf[1].only_cache)
+		goto cache;
+
+	cmd.data[i++] = virt_to_phys(arg->ibuf[1].p);
+	cmd.data[i++] = virt_to_phys(arg->ibuf[1].p) >> 32;
+cache:
+	for (i = 0; i < MAX_IPI_BUF; i++) {
+		cmd.ibuf[i].len = arg->ibuf[i].len;
+		cmd.ibuf[i].p = arg->ibuf[i].p;
+	}
+notify:
+	return 	versal_mbox_notify(&cmd, NULL);
+}
diff --git a/core/drivers/crypto/versal/rsa.c b/core/drivers/crypto/versal/rsa.c
new file mode 100644
index 00000000000..030f6f25082
--- /dev/null
+++ b/core/drivers/crypto/versal/rsa.c
@@ -0,0 +1,264 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <drvcrypt.h>
+#include <drvcrypt_acipher.h>
+#include <crypto/crypto_impl.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "ipi.h"
+
+static void crypto_bignum_bn2bin_eswap(struct bignum *from, uint8_t *to)
+{
+	uint8_t tmp = 0;
+	size_t i = 0;
+	size_t j = 0;
+	size_t len = crypto_bignum_num_bytes(from);
+
+	if (len < sizeof(uint32_t)) {
+		/* public exponent should be 4 bytes:
+		   bignum chops off leading zeroes. Pad it */
+		uint8_t buf[8] = { 0 };
+		crypto_bignum_bn2bin(from, buf + 1);
+		for (i = 0, j = sizeof(uint32_t) - 1; i < j; i++, j--) {
+			tmp = buf[i];
+			buf[i] = buf[j];
+			buf[j] = tmp;
+		}
+		memcpy(to, buf, 8);
+		return;
+	}
+
+	crypto_bignum_bn2bin(from, to);
+	for (i = 0, j = len - 1; i < j; i++, j--) {
+		tmp = to[i];
+		to[i] = to[j];
+		to[j] = tmp;
+	}
+}
+
+static TEE_Result do_encrypt(struct drvcrypt_rsa_ed *rsa_data)
+{
+	struct versal_rsa_input_param *cmd = NULL;
+	struct rsa_public_key *p = rsa_data->key.key;
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	uint8_t *cipher = NULL;
+	uint8_t *key = NULL;
+	uint8_t *msg = NULL;
+
+	key = memalign(CACHELINE_LEN,
+		       ROUNDUP(512 + crypto_bignum_num_bytes(p->e),
+			       CACHELINE_LEN));
+	if (!key) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	crypto_bignum_bn2bin_eswap(p->n, key);
+	crypto_bignum_bn2bin_eswap(p->e, key + 512);
+
+	cipher = memalign(CACHELINE_LEN,
+			  ROUNDUP(rsa_data->cipher.length, CACHELINE_LEN));
+	if (!cipher) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	msg = memalign(CACHELINE_LEN,
+		       ROUNDUP(rsa_data->message.length, CACHELINE_LEN));
+	if (!msg) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	memcpy(msg, rsa_data->message.data, rsa_data->message.length);
+
+	cmd = memalign(CACHELINE_LEN, ROUNDUP(sizeof(*cmd), CACHELINE_LEN));
+	if (!cmd) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	cmd->key_addr = virt_to_phys(key);
+	cmd->data_addr = virt_to_phys(msg);
+	cmd->key_len = crypto_bignum_num_bytes(p->n);
+
+	/* src */
+	arg.ibuf[0].p = cmd;
+	arg.ibuf[0].len = ROUNDUP(sizeof(*cmd), CACHELINE_LEN);
+	/* dst */
+	arg.ibuf[1].p = cipher;
+	arg.ibuf[1].len = ROUNDUP(rsa_data->cipher.length, CACHELINE_LEN);
+	/* cache */
+	arg.ibuf[2].p = msg;
+	arg.ibuf[2].len = ROUNDUP(rsa_data->message.length, CACHELINE_LEN);
+	arg.ibuf[3].p = key;
+	arg.ibuf[3].len = ROUNDUP(512 + crypto_bignum_num_bytes(p->e),
+				  CACHELINE_LEN);
+
+	if (versal_crypto_request(RSA_PUBLIC_ENCRYPT, &arg)) {
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	memcpy(rsa_data->cipher.data, cipher, rsa_data->cipher.length);
+	ret = TEE_SUCCESS;
+out:
+	free(cipher);
+	free(cmd);
+	free(msg);
+	free(key);
+
+	return ret;
+}
+
+static TEE_Result do_decrypt(struct drvcrypt_rsa_ed *rsa_data)
+{
+	struct versal_rsa_input_param *cmd = NULL;
+	struct rsa_keypair *p = rsa_data->key.key;
+	TEE_Result ret = TEE_SUCCESS;
+	struct cmd_args arg = { };
+	uint8_t *cipher = NULL;
+	uint8_t *key = NULL;
+	uint8_t *msg = NULL;
+
+	key = memalign(CACHELINE_LEN,
+		       ROUNDUP(512 + crypto_bignum_num_bytes(p->d),
+			       CACHELINE_LEN));
+	if (!key) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	crypto_bignum_bn2bin_eswap(p->n, key);
+	crypto_bignum_bn2bin_eswap(p->d, key + 512);
+
+	cipher = memalign(CACHELINE_LEN,
+			  ROUNDUP(rsa_data->cipher.length, CACHELINE_LEN));
+	if (!cipher) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	memcpy(cipher, rsa_data->cipher.data, rsa_data->cipher.length);
+
+	msg = memalign(CACHELINE_LEN,
+		       ROUNDUP(rsa_data->message.length, CACHELINE_LEN));
+	if (!msg) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	cmd = memalign(CACHELINE_LEN, ROUNDUP(sizeof(*cmd), CACHELINE_LEN));
+	if (!cmd) {
+		ret = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	cmd->key_addr = virt_to_phys(key);
+	cmd->data_addr = virt_to_phys(cipher);
+	cmd->key_len = crypto_bignum_num_bytes(p->n);
+
+	/* src */
+	arg.ibuf[0].p = cmd;
+	arg.ibuf[0].len = ROUNDUP(sizeof(*cmd), CACHELINE_LEN);
+	/* dst */
+	arg.ibuf[1].p = msg;
+	arg.ibuf[1].len = ROUNDUP(rsa_data->message.length, CACHELINE_LEN);
+	/* cache */
+	arg.ibuf[2].p = cipher;
+	arg.ibuf[2].len = ROUNDUP(rsa_data->cipher.length, CACHELINE_LEN);
+	arg.ibuf[3].p = key;
+	arg.ibuf[3].len = ROUNDUP(crypto_bignum_num_bytes(p->d) +
+				  crypto_bignum_num_bytes(p->n), CACHELINE_LEN);
+
+	if (versal_crypto_request(RSA_PRIVATE_DECRYPT, &arg)) {
+		ret = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	memcpy(rsa_data->message.data, msg, rsa_data->message.length);
+	ret = TEE_SUCCESS;
+out:
+	free(cipher);
+	free(cmd);
+	free(key);
+	free(msg);
+
+	return ret;
+}
+
+static TEE_Result do_ssa_sign(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	/* delegating to software */
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static TEE_Result do_ssa_verify(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	/* delegating to software */
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static TEE_Result do_gen_keypair(struct rsa_keypair *keypair, size_t size_bytes)
+{
+	/* delegating to software */
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static TEE_Result do_alloc_keypair(struct rsa_keypair *s, size_t size_bits)
+{
+	/* delegating to software */
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static TEE_Result do_alloc_publickey(struct rsa_public_key *s,
+				     size_t size_bits __unused)
+{
+	/* delegating to software */
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static void do_free_publickey(struct rsa_public_key *s)
+{
+	/* delegating to software */
+	sw_crypto_acipher_free_rsa_public_key(s);
+}
+
+static void do_free_keypair(struct rsa_keypair *s)
+{
+	/* delegating to software */
+	sw_crypto_acipher_free_rsa_keypair(s);
+}
+
+static struct drvcrypt_rsa driver_rsa = {
+	.alloc_keypair = do_alloc_keypair,
+	.alloc_publickey = do_alloc_publickey,
+	.free_publickey = do_free_publickey,
+	.free_keypair = do_free_keypair,
+	.gen_keypair = do_gen_keypair,
+	.encrypt = do_encrypt,
+	.decrypt = do_decrypt,
+	.optional.ssa_sign = do_ssa_sign,
+	.optional.ssa_verify = do_ssa_verify,
+};
+
+static TEE_Result rsa_init(void)
+{
+	struct cmd_args arg = { };
+
+	if (versal_crypto_request(RSA_KAT, &arg))
+		return TEE_ERROR_GENERIC;
+
+	return drvcrypt_register_rsa(&driver_rsa);
+}
+
+driver_init(rsa_init);
diff --git a/core/drivers/crypto/versal/rsa_test.c b/core/drivers/crypto/versal/rsa_test.c
new file mode 100644
index 00000000000..825c6a13518
--- /dev/null
+++ b/core/drivers/crypto/versal/rsa_test.c
@@ -0,0 +1,351 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2022 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+
+#include <drvcrypt.h>
+#include <drvcrypt_acipher.h>
+#include <crypto/crypto_impl.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <mempool.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "ipi.h"
+
+#define __STR(X) #X
+#define STR(X) __STR(X)
+
+#define XSECURE_RSA_SIZE	512	/**< 512 bytes for 4096 bit data */
+
+static uint8_t mod[XSECURE_RSA_SIZE] = {
+	0xf0, 0x1c, 0xde, 0x7b, 0xdb, 0x26, 0xcd, 0xd7, 0xa7, 0xe3, 0xfe, 0x51,
+	0x0b, 0x82, 0x03, 0xdd, 0x12, 0xc7, 0xc7, 0x60, 0x22, 0x97, 0x38, 0xfe,
+	0xad, 0x79, 0x8c, 0x9d, 0x3f, 0x9d, 0x87, 0x0e, 0xac, 0x9c, 0xee, 0xb6,
+	0x13, 0xcc, 0x8c, 0x75, 0x7f, 0xcf, 0x7a, 0xf3, 0xe6, 0x90, 0x5e, 0x59,
+	0xb2, 0x09, 0xd9, 0x75, 0x94, 0xd4, 0x92, 0x8d, 0xc5, 0x81, 0xfa, 0xf8,
+	0x6e, 0x63, 0xf7, 0x70, 0x19, 0xf3, 0x31, 0xd2, 0xf3, 0x28, 0x6c, 0x8a,
+	0x79, 0xca, 0x34, 0x03, 0x4f, 0x39, 0x7f, 0x09, 0xf7, 0x21, 0x20, 0xd6,
+	0xfd, 0x36, 0xb3, 0xa2, 0x7e, 0x40, 0x96, 0x3e, 0x68, 0x12, 0x80, 0xfe,
+	0x92, 0x37, 0xe7, 0xd5, 0xdc, 0xfe, 0x62, 0x5c, 0x3e, 0xce, 0x21, 0x71,
+	0x70, 0x03, 0xd6, 0xbd, 0x02, 0xd0, 0x24, 0x41, 0xb6, 0xfc, 0x5c, 0xfb,
+	0x8e, 0xf1, 0xfa, 0xc7, 0x3d, 0x9b, 0x81, 0xd7, 0xee, 0x0a, 0x9f, 0x4c,
+	0x4b, 0x81, 0x9c, 0x6d, 0xe2, 0x49, 0xa5, 0x98, 0xaf, 0xa0, 0xc1, 0x19,
+	0x5c, 0x0b, 0x85, 0x66, 0x31, 0x60, 0x6c, 0x44, 0x77, 0xe9, 0x96, 0x58,
+	0xac, 0x1d, 0x72, 0x9e, 0xcd, 0xb2, 0xc2, 0x6a, 0x5a, 0x90, 0xeb, 0xe9,
+	0x30, 0x52, 0x97, 0x3f, 0xd1, 0x9b, 0xb6, 0xac, 0x17, 0x84, 0xfc, 0xa7,
+	0xd2, 0x64, 0x7a, 0xa7, 0x40, 0x90, 0xac, 0xb9, 0x1a, 0x96, 0x28, 0xa0,
+	0xf4, 0xb0, 0xfb, 0x07, 0x16, 0x26, 0x29, 0x3d, 0x9f, 0xac, 0xe7, 0x2e,
+	0xe8, 0x72, 0x79, 0xf9, 0x5e, 0x14, 0x33, 0xed, 0xe1, 0xc2, 0x51, 0x03,
+	0xca, 0xf2, 0xdc, 0xa8, 0x49, 0x41, 0x2c, 0x29, 0x13, 0x59, 0xec, 0xad,
+	0x95, 0x34, 0xe3, 0x5f, 0xcd, 0x35, 0xc5, 0x60, 0xaf, 0xe6, 0x12, 0x21,
+	0xcc, 0x99, 0x85, 0x13, 0x78, 0xc1, 0xce, 0x1a, 0x26, 0xfc, 0x34, 0x26,
+	0x3d, 0x9a, 0xfd, 0xfe, 0x5e, 0x50, 0xba, 0xe7, 0x9b, 0x97, 0x63, 0xb2,
+	0x3d, 0xf9, 0xf5, 0xdd, 0x52, 0xb7, 0xb5, 0x0f, 0x6e, 0x1b, 0x1e, 0x12,
+	0xb6, 0x5b, 0xa7, 0xf4, 0xb6, 0x48, 0xc5, 0x5f, 0x72, 0x3b, 0xc3, 0x46,
+	0x7b, 0x30, 0xef, 0xe8, 0x37, 0xd9, 0xba, 0x34, 0x77, 0xc5, 0x72, 0xb5,
+	0xf8, 0xa3, 0x56, 0x4a, 0xc7, 0xdb, 0x29, 0xab, 0x11, 0x89, 0x0a, 0xe5,
+	0x94, 0xcf, 0x60, 0x8c, 0x97, 0x68, 0xb2, 0x4e, 0x8b, 0x49, 0xc1, 0xea,
+	0x7f, 0x74, 0x35, 0x29, 0xa1, 0xa3, 0xf9, 0xd4, 0xe5, 0xa0, 0x79, 0x25,
+	0xcb, 0x2e, 0x5b, 0x83, 0xed, 0x0a, 0xb3, 0x36, 0x0c, 0x0e, 0x2e, 0xda,
+	0x49, 0xf9, 0x94, 0x2e, 0xe3, 0xd2, 0x62, 0xa1, 0xf8, 0xdd, 0xf8, 0x77,
+	0xb3, 0xf6, 0xd8, 0x05, 0xd8, 0x9a, 0x8f, 0x65, 0x31, 0x67, 0xa7, 0x6d,
+	0x11, 0xa3, 0xee, 0xcd, 0xd3, 0x79, 0x0a, 0x46, 0xd2, 0x4a, 0xe0, 0xdd,
+	0x02, 0x09, 0xf4, 0x5f, 0x41, 0x49, 0x02, 0x3e, 0x05, 0xd1, 0x11, 0x72,
+	0x46, 0xff, 0xbd, 0x7d, 0x75, 0xb1, 0x5f, 0xa3, 0x3b, 0xa6, 0xf2, 0xcd,
+	0x24, 0xe3, 0xca, 0x2d, 0xd6, 0xfc, 0x94, 0xdd, 0x9e, 0x6f, 0x13, 0xa6,
+	0xb0, 0xa9, 0x88, 0x2e, 0x14, 0x74, 0xd2, 0xc3, 0xa7, 0xe1, 0xa5, 0xae,
+	0x6d, 0x7a, 0x06, 0x8f, 0x48, 0x95, 0x08, 0x84, 0x50, 0x6c, 0xc4, 0xa6,
+	0xfd, 0xc6, 0x3a, 0x25, 0x03, 0x8e, 0x19, 0xb8, 0x82, 0x88, 0xad, 0xee,
+	0xbc, 0x8d, 0xbc, 0xf1, 0xdd, 0xf7, 0x1a, 0x4c, 0x2e, 0x1d, 0x5d, 0x54,
+	0x82, 0x75, 0x35, 0x7c, 0xd5, 0xe3, 0x25, 0x40, 0x8b, 0x4a, 0xa5, 0x49,
+	0x4e, 0x52, 0x14, 0x1f, 0xc5, 0xd8, 0xb8, 0xef, 0xae, 0xcf, 0x50, 0xbb,
+	0x9d, 0xbc, 0xe7, 0x93, 0x71, 0x04, 0xf8, 0xf4, 0xa6, 0xf1, 0xe2, 0x73,
+	0xf8, 0x56, 0x00, 0x30, 0xba, 0xee, 0xd5, 0xe7
+};
+
+static uint8_t private_exp[XSECURE_RSA_SIZE] = {
+	0x9e, 0x65, 0x5c, 0xfb, 0x75, 0xb1, 0x9e, 0x7e, 0xd2, 0x39, 0x5c, 0x0e,
+	0x58, 0xba, 0x17, 0x14, 0x62, 0x8c, 0x39, 0xe1, 0x18, 0x50, 0x8b, 0xff,
+	0xad, 0xae, 0x8e, 0x6d, 0x39, 0x87, 0x10, 0x8e, 0x44, 0x9e, 0x6d, 0xf6,
+	0xdd, 0x0e, 0xc5, 0xe5, 0xc9, 0x3f, 0xb4, 0xcb, 0x6a, 0xe0, 0xf7, 0xec,
+	0xd1, 0xbf, 0x1f, 0x9e, 0x2a, 0x65, 0x24, 0x9c, 0xb3, 0x9b, 0x30, 0x55,
+	0x6c, 0x75, 0x33, 0xc8, 0xd6, 0xc5, 0x90, 0xab, 0xa9, 0x4e, 0x94, 0x73,
+	0xb1, 0x2e, 0x0b, 0xab, 0xd7, 0x3a, 0x85, 0xac, 0xac, 0x43, 0x45, 0x67,
+	0xad, 0xb8, 0xc5, 0x3d, 0xbf, 0x6d, 0x99, 0xef, 0x67, 0x36, 0x35, 0xb4,
+	0x39, 0xfe, 0xf9, 0x48, 0x22, 0xb6, 0x09, 0x6a, 0xf2, 0xdf, 0x86, 0x3e,
+	0x4f, 0xf7, 0xa8, 0x54, 0x1d, 0xa3, 0xf3, 0x17, 0x0a, 0xc0, 0x66, 0x12,
+	0x68, 0xd5, 0x06, 0x6b, 0x23, 0x20, 0x0f, 0xc2, 0xc3, 0x3c, 0x6c, 0xa3,
+	0xa6, 0xf7, 0xc4, 0x9a, 0xd4, 0x79, 0x62, 0xed, 0x5b, 0x86, 0x7f, 0x7b,
+	0xdc, 0x35, 0x99, 0x75, 0xb5, 0xc2, 0xf7, 0x09, 0x1f, 0xd5, 0x8f, 0x21,
+	0xc5, 0xc0, 0xd3, 0x59, 0x21, 0x36, 0xf8, 0x7b, 0xb2, 0x6d, 0x01, 0xc3,
+	0x5d, 0xe6, 0x4d, 0xf2, 0x82, 0xb2, 0xe7, 0x09, 0x95, 0xa6, 0x5c, 0x09,
+	0xd5, 0x88, 0x05, 0x45, 0x8a, 0x19, 0x26, 0x68, 0x21, 0xba, 0xc6, 0xe8,
+	0x8f, 0x22, 0xa5, 0x26, 0xa2, 0x9c, 0xdf, 0xfd, 0xcf, 0xf4, 0xfe, 0xb8,
+	0xfa, 0x0c, 0x72, 0x9c, 0x67, 0x4c, 0x8a, 0x89, 0x35, 0xb1, 0xbb, 0x41,
+	0xf6, 0x42, 0xc9, 0xff, 0x95, 0xfb, 0x91, 0x47, 0x33, 0x85, 0xd1, 0xd5,
+	0x1c, 0xa5, 0xbc, 0x0c, 0xf9, 0xd1, 0xab, 0xf8, 0x41, 0x71, 0x53, 0xaa,
+	0xca, 0x4e, 0xc7, 0xf3, 0xf5, 0x68, 0x5c, 0x2e, 0xf2, 0x62, 0xd3, 0x80,
+	0x1a, 0x89, 0xc1, 0x5d, 0x21, 0xfc, 0xba, 0x39, 0xea, 0x7c, 0x7b, 0xba,
+	0x4f, 0x70, 0xc3, 0x7a, 0x3d, 0xe0, 0xce, 0xc5, 0xe8, 0xfd, 0x18, 0xd3,
+	0x47, 0x0e, 0xf3, 0x98, 0x42, 0x3b, 0xe8, 0x03, 0xcc, 0x2e, 0xd6, 0xe7,
+	0xe4, 0x36, 0x53, 0x47, 0xe6, 0xf8, 0xd0, 0x09, 0xc0, 0x06, 0xa9, 0x23,
+	0x10, 0xc6, 0xd3, 0x4f, 0xf6, 0x8d, 0x27, 0xc3, 0xbe, 0xa0, 0xc1, 0x80,
+	0x8b, 0x2d, 0x43, 0x42, 0x1a, 0x5c, 0xbd, 0xbe, 0xf3, 0x14, 0x6b, 0x7e,
+	0x44, 0xa9, 0x59, 0x1b, 0x2c, 0x06, 0xe4, 0xfe, 0xb7, 0x44, 0xbc, 0x80,
+	0xdc, 0x11, 0x24, 0xd5, 0x40, 0x5e, 0x7e, 0xc4, 0xce, 0x6a, 0x6c, 0xdc,
+	0xda, 0x4d, 0x3a, 0xbc, 0xa2, 0xbf, 0xb9, 0xf0, 0x76, 0xa7, 0xb0, 0x82,
+	0x39, 0xb1, 0x2a, 0xc6, 0x7b, 0x78, 0xba, 0x18, 0xf5, 0xb7, 0x06, 0x43,
+	0xe8, 0x1c, 0x4b, 0x89, 0xb9, 0x07, 0xf7, 0x5f, 0x88, 0xd8, 0x7b, 0xf1,
+	0xf7, 0xc4, 0x72, 0xd9, 0xa9, 0x3d, 0x79, 0x0a, 0xfc, 0x39, 0x7d, 0xe9,
+	0xc9, 0x26, 0xf7, 0x0f, 0x22, 0xe0, 0x80, 0x12, 0x49, 0x39, 0x21, 0xa4,
+	0xaa, 0x75, 0xdc, 0x83, 0x7a, 0x65, 0x23, 0xe5, 0xd1, 0x38, 0x0e, 0xc3,
+	0x69, 0xdd, 0x82, 0xe1, 0xeb, 0xca, 0x84, 0x5e, 0x2f, 0x10, 0x72, 0x99,
+	0xdb, 0x7d, 0xea, 0xa5, 0xc5, 0x59, 0x7e, 0xd9, 0xe0, 0xf3, 0xa9, 0x4d,
+	0xdb, 0xb9, 0x1c, 0x80, 0x02, 0x11, 0xa5, 0xd0, 0xa3, 0xaa, 0x53, 0x54,
+	0xc5, 0xce, 0x22, 0x7b, 0xfa, 0xa9, 0x9b, 0x8f, 0xc0, 0x90, 0xd4, 0xcd,
+	0x39, 0x0a, 0x9f, 0x17, 0x03, 0xe4, 0xe1, 0xcd, 0x5b, 0x5a, 0x1c, 0x46,
+	0x88, 0x58, 0x9b, 0x11, 0x6a, 0x8c, 0x34, 0x11, 0x8d, 0x65, 0xd3, 0xa2,
+	0x5e, 0x19, 0xbc, 0xb4, 0x15, 0x4e, 0x33, 0x10, 0xe4, 0x87, 0x72, 0x00,
+	0xc4, 0x00, 0xa3, 0x30, 0x59, 0x29, 0xd9, 0x21
+};
+
+static uint32_t public_exp = 0x01000100;
+
+static uint8_t data[XSECURE_RSA_SIZE] = {
+	0x00, 0x01,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00,
+	/* T_Padding */
+	0x30, 0x41, 0x30, 0x0D, 0x06, 0x09, 0x60, 0x86, 0x48, 0x01, 0x65, 0x03,
+	0x04, 0x02, 0x09, 0x05, 0x00, 0x04, 0x30,
+	/* SHA 3 Hash */
+	0x85, 0x52, 0xA4, 0xA3, 0xC3, 0x06, 0x84, 0x9C, 0x00, 0x22, 0x43, 0x10,
+	0x78, 0x15, 0xD7, 0xD9, 0x2D, 0xF6, 0x5F, 0x07, 0x35, 0xD5, 0x45, 0x80,
+	0x35, 0xE1, 0xA7, 0x9C, 0xBB, 0xBE, 0x98, 0x02, 0x89, 0x0D, 0x7C, 0x4D,
+	0xC0, 0x63, 0x24, 0x01, 0x21, 0x5F, 0xC5, 0x9D, 0xBC, 0xAE, 0x00, 0xD3
+};
+
+static uint8_t expected_sign[XSECURE_RSA_SIZE] = {
+	0xCE, 0x8F, 0x94, 0xA6, 0xD0, 0xB4, 0x4A, 0x74, 0x1B, 0xA0, 0xCE, 0x17,
+	0x56, 0x2E, 0xB3, 0xBF, 0x45, 0x00, 0x02, 0x73, 0x5C, 0x23, 0x36, 0x9D,
+	0x58, 0xB4, 0x76, 0x8A, 0xD4, 0xCF, 0xCF, 0xF1, 0xAC, 0x96, 0x3F, 0xEE,
+	0xB2, 0x62, 0x55, 0x09, 0xBA, 0x22, 0x5E, 0x86, 0xA2, 0xD0, 0xE5, 0x01,
+	0xAA, 0x48, 0xF1, 0x2A, 0x49, 0xCD, 0xB3, 0x24, 0xB3, 0x5C, 0xD6, 0x7A,
+	0x98, 0x0E, 0xDC, 0xC1, 0x95, 0xB7, 0xAA, 0x4D, 0x26, 0xEF, 0x88, 0xE3,
+	0x13, 0xC3, 0x03, 0x37, 0x4F, 0x7C, 0xFA, 0xF5, 0x40, 0x99, 0x36, 0x88,
+	0xE8, 0x11, 0xFA, 0x42, 0x8A, 0xA8, 0x14, 0xF4, 0xCC, 0x2F, 0xE1, 0x29,
+	0xAA, 0xBB, 0x07, 0x17, 0x68, 0x21, 0xFC, 0x1D, 0xC4, 0x0A, 0xFF, 0x93,
+	0x8F, 0x43, 0x0A, 0x40, 0x54, 0xED, 0xD3, 0xDC, 0xA0, 0x57, 0x7C, 0xD8,
+	0xB4, 0xD9, 0x03, 0x05, 0x51, 0xFF, 0xFB, 0x0A, 0x90, 0x95, 0xD7, 0x34,
+	0x93, 0x39, 0x65, 0x0F, 0xE6, 0xC1, 0x3F, 0x60, 0x15, 0xE5, 0x9B, 0xC2,
+	0x9F, 0x52, 0x3F, 0xA6, 0x29, 0x6A, 0xC8, 0x02, 0x21, 0x2A, 0xD9, 0xD0,
+	0x6F, 0xA9, 0x5F, 0x2D, 0x4F, 0xBD, 0xCA, 0x14, 0xC1, 0xAD, 0x9B, 0x23,
+	0xF1, 0x21, 0xF6, 0xEE, 0x51, 0xB6, 0x40, 0x04, 0x6D, 0x01, 0x48, 0x19,
+	0x13, 0x76, 0x6B, 0xF5, 0x22, 0x4A, 0x3D, 0x53, 0x0D, 0xF3, 0xE6, 0x7C,
+	0xB0, 0xDA, 0xE0, 0x61, 0x8C, 0x21, 0x7A, 0x08, 0xFF, 0xF9, 0xE3, 0xF4,
+	0xC4, 0x4D, 0x59, 0x83, 0xCB, 0xC9, 0xED, 0xA5, 0xD0, 0x3D, 0xC8, 0x05,
+	0xD7, 0xA1, 0x6C, 0xFF, 0xE7, 0xD8, 0xB9, 0x5C, 0xAE, 0x7D, 0x03, 0xE3,
+	0x21, 0xDB, 0xFB, 0x06, 0x4B, 0x71, 0xE9, 0x1C, 0x3F, 0xBB, 0xC4, 0xBB,
+	0x03, 0xF5, 0x6D, 0x1B, 0xC3, 0xB0, 0x2D, 0x15, 0xAC, 0x1D, 0xCA, 0x71,
+	0x89, 0x8D, 0x21, 0xD5, 0x68, 0x1A, 0x17, 0xCA, 0x01, 0x6B, 0x01, 0x2A,
+	0xC3, 0xA2, 0x01, 0x37, 0x54, 0x52, 0xE2, 0xED, 0xE8, 0xC9, 0x33, 0xCA,
+	0xE1, 0x81, 0xE3, 0x7B, 0x4D, 0xDB, 0x9F, 0xF1, 0xF0, 0x9E, 0x01, 0xBD,
+	0x51, 0x65, 0xAE, 0x14, 0x57, 0xBA, 0x0E, 0x3C, 0x6C, 0x82, 0x8C, 0x8E,
+	0x4E, 0xF3, 0x51, 0x99, 0x8B, 0x7F, 0x96, 0x3C, 0xD1, 0x4E, 0x6E, 0x38,
+	0xD0, 0x5F, 0x3B, 0x0D, 0x8F, 0x45, 0xFD, 0x8D, 0x12, 0x9B, 0x0A, 0x53,
+	0x34, 0x50, 0x1E, 0x86, 0x9E, 0x89, 0xE1, 0xD4, 0x58, 0x60, 0x68, 0xDF,
+	0x53, 0x15, 0x36, 0x82, 0x3B, 0xC3, 0x39, 0x8F, 0xFE, 0x1B, 0xD2, 0xBA,
+	0x3A, 0xBD, 0xE5, 0x63, 0x45, 0xE1, 0xC7, 0x92, 0x0C, 0xCF, 0xE3, 0x1D,
+	0x93, 0x01, 0x55, 0xA4, 0x05, 0x28, 0xA8, 0xBB, 0xE2, 0xFD, 0xCA, 0x00,
+	0xAC, 0x53, 0x97, 0xE0, 0x17, 0xC6, 0x5D, 0x18, 0x7A, 0x1F, 0xBD, 0x51,
+	0xD9, 0xDF, 0xA1, 0xAC, 0x7C, 0x91, 0xA9, 0x3A, 0xF9, 0x28, 0x6F, 0xA6,
+	0xB9, 0x65, 0x0B, 0xAF, 0xC2, 0xB9, 0x46, 0xBE, 0x25, 0x31, 0x18, 0x83,
+	0x25, 0x10, 0x76, 0x87, 0x2A, 0x94, 0xEC, 0xBC, 0x1A, 0xBC, 0x25, 0x72,
+	0x56, 0xA6, 0xB0, 0x66, 0xE1, 0x7B, 0x4F, 0xC7, 0xCE, 0x7D, 0x4D, 0x1C,
+	0x12, 0x50, 0xDB, 0x89, 0x0D, 0xEF, 0xC0, 0x73, 0x7B, 0x47, 0xA2, 0xC2,
+	0x66, 0x9A, 0x8A, 0xF2, 0xE0, 0x9D, 0x04, 0x73, 0xAB, 0xC3, 0x0C, 0x3E,
+	0x3E, 0x66, 0x55, 0xEE, 0x3C, 0xAA, 0xC1, 0xE3, 0xB0, 0x4A, 0xC1, 0xBD,
+	0xAA, 0xBA, 0x10, 0xCD, 0x4B, 0x15, 0xF3, 0x8F, 0x59, 0x81, 0x54, 0xE6,
+	0x63, 0x3C, 0xC7, 0xA9, 0xB1, 0x40, 0x1A, 0x7B, 0x8A, 0x22, 0xD1, 0x10,
+	0xC5, 0x7C, 0x3F, 0xF5, 0xAB, 0xBE, 0x5E, 0x4C, 0x14, 0xD6, 0x83, 0x6E,
+	0x31, 0x6A, 0x45, 0x5F, 0x4A, 0x93, 0xE5, 0x13
+};
+
+static uint8_t signature[XSECURE_RSA_SIZE];
+static uint8_t encrypted_signature[XSECURE_RSA_SIZE];
+
+static void crypto_bignum_bn2bin_eswap(struct bignum *from, uint8_t *to)
+{
+	uint8_t tmp = 0;
+	size_t i = 0;
+	size_t j = 0;
+
+	crypto_bignum_bn2bin(from, to);
+	for (i = 0, j = crypto_bignum_num_bytes(from) - 1; i < j; i++, j--) {
+		tmp = to[i];
+		to[i] = to[j];
+		to[j] = tmp;
+	}
+}
+
+static void crypto_bignum_bin2bn_eswap(uint8_t *from, size_t len,
+				       struct bignum *to)
+{
+	uint8_t *p = 0;
+	uint8_t tmp = 0;
+	size_t i = 0;
+	size_t j = 0;
+
+	p = mempool_calloc(mempool_default, 1, len);
+	if (!p)
+		panic();
+
+	memcpy(p, from, len);
+
+	for (i = 0, j = len - 1; i < j; i++, j--) {
+		tmp = p[i];
+		p[i] = p[j];
+		p[j] = tmp;
+	}
+
+	crypto_bignum_bin2bn(p, len, to);
+	free(p);
+}
+
+static struct rsa_public_key pubkey = { };
+static struct rsa_keypair keypair = { };
+
+static TEE_Result test_encrypt(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t signature_len = sizeof(signature);
+	size_t encrypted_signature_len = sizeof(encrypted_signature);
+
+	ret = crypto_acipher_alloc_rsa_public_key(&pubkey, 4096);
+	if (ret)
+		return ret;
+
+	crypto_bignum_bin2bn_eswap((uint8_t*)&public_exp,
+				   sizeof(public_exp), pubkey.e);
+	crypto_bignum_bin2bn_eswap(mod, sizeof(mod), pubkey.n);
+
+	ret = crypto_acipher_rsanopad_encrypt(&pubkey,
+					      signature, signature_len,
+					      encrypted_signature,
+					      &encrypted_signature_len);
+	if (ret)
+		return ret;
+
+	crypto_acipher_free_rsa_public_key(&pubkey);
+
+	if (memcmp(encrypted_signature, data, encrypted_signature_len)) {
+		EMSG("Encrypted buffer: ");
+		DHEXDUMP(encrypted_signature, encrypted_signature_len);
+
+		return TEE_ERROR_GENERIC;
+	}
+
+	return ret;
+}
+
+static TEE_Result test_decrypt(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t signature_len = sizeof(signature);
+
+	ret = crypto_acipher_alloc_rsa_keypair(&keypair, 4096);
+	if (ret)
+		return ret;
+
+	crypto_bignum_bin2bn_eswap(private_exp, sizeof(private_exp), keypair.d);
+	crypto_bignum_bin2bn_eswap(mod, sizeof(mod), keypair.n);
+
+	ret = crypto_acipher_rsanopad_decrypt(&keypair,
+					      data,
+					      sizeof(data),
+					      signature,
+					      &signature_len);
+	if (ret)
+		return ret;
+
+	crypto_acipher_free_rsa_keypair(&keypair);
+
+	if (memcmp(signature, expected_sign, sizeof(signature)))
+		return TEE_ERROR_GENERIC;
+
+	return ret;
+}
+
+static struct {
+	TEE_Result (*f)(void);
+	const char *name;
+	bool failed;
+} test[] = {
+	{ .f = test_decrypt, .name = STR(rsa decrypt), },
+	{ .f = test_encrypt, .name = STR(rsa encrypt), },
+};
+
+
+static TEE_Result versal_crypto_test(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		ret = (test[i].f)();
+		if (ret)
+			test[i].failed = true;
+	}
+
+	IMSG("Versal: Test RSA");
+	for (i = 0; i < ARRAY_SIZE(test); i++) {
+		IMSG("---- %s:\t\t\t\t\t [%s]",
+		     test[i].name,
+		     test[i].failed ? "KO" : "OK");
+	}
+
+	return TEE_SUCCESS;;
+}
+
+boot_final(versal_crypto_test);
diff --git a/core/drivers/crypto/versal/sub.mk b/core/drivers/crypto/versal/sub.mk
new file mode 100644
index 00000000000..a02923d622a
--- /dev/null
+++ b/core/drivers/crypto/versal/sub.mk
@@ -0,0 +1,9 @@
+incdirs-y += include
+
+cflags-y += -Wno-unused-parameter
+
+srcs-y += ipi.c
+srcs-y += ecc.c      ecc_test.c
+srcs-y += rsa.c      rsa_test.c
+srcs-y += hash.c     hash_test.c
+srcs-y += authenc.c  authenc_test.c
diff --git a/core/drivers/sub.mk b/core/drivers/sub.mk
index 7d825eed986..af3f4482cbe 100644
--- a/core/drivers/sub.mk
+++ b/core/drivers/sub.mk
@@ -51,6 +51,10 @@ srcs-$(CFG_ZYNQMP_CSUDMA) += zynqmp_csudma.c
 srcs-$(CFG_ZYNQMP_CSU_AES) += zynqmp_csu_aes.c
 srcs-$(CFG_ZYNQMP_PM) += zynqmp_pm.c
 srcs-$(CFG_ZYNQMP_HUK) += zynqmp_huk.c
+srcs-$(CFG_VERSAL_RNG_DRV) += versal_trng.c
+srcs-$(CFG_VERSAL_PM) += versal_pm.c
+srcs-$(CFG_VERSAL_MBOX) += versal_mbox.c
+srcs-$(CFG_VERSAL_NVM) += versal_nvm.c versal_nvm_test.c
 
 subdirs-y += crypto
 subdirs-$(CFG_BNXT_FW) += bnxt
diff --git a/core/drivers/versal_mbox.c b/core/drivers/versal_mbox.c
new file mode 100644
index 00000000000..65483862421
--- /dev/null
+++ b/core/drivers/versal_mbox.c
@@ -0,0 +1,219 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) 2022 Foundries.io Ltd
+ * Jorge Ramirez-Ortiz <jorge@foundries.io>
+ */
+
+#include <initcall.h>
+#include <kernel/delay.h>
+#include <kernel/panic.h>
+#include <mm/core_mmu.h>
+#include <string.h>
+#include <tee/cache.h>
+#include "drivers/versal_mbox.h"
+
+#define PM_SIP_SVC	0xc2000000
+
+/* ipi targets */
+#define IPI_ID_PMC	1
+#define IPI_ID_0	2
+#define IPI_ID_RPU0	3
+#define IPI_ID_RPU1	4
+#define IPI_ID_3	5
+#define IPI_ID_4	6
+#define IPI_ID_5	7
+
+/* buffers */
+#define IPI_BUFFER_BASEADDR		0xFF3F0000
+#define IPI_BUFFER_APU_ID_0_BASE	(IPI_BUFFER_BASEADDR + 0x400)
+#define IPI_BUFFER_APU_ID_3_BASE	(IPI_BUFFER_BASEADDR + 0xA00)
+#define IPI_BUFFER_APU_ID_4_BASE	(IPI_BUFFER_BASEADDR + 0xC00)
+#define IPI_BUFFER_APU_ID_5_BASE	(IPI_BUFFER_BASEADDR + 0xE00)
+#define IPI_BUFFER_PMC_BASE		(IPI_BUFFER_BASEADDR + 0x200)
+#define IPI_BUFFER_TARGET_APU_OFFSET	0x80
+#define IPI_BUFFER_TARGET_PMC_OFFSET	0x40
+#define IPI_BUFFER_REQ_OFFSET		0x0
+#define IPI_BUFFER_RESP_OFFSET		0x20
+
+#define IPI_BUFFER_LOCAL_OFFSET		IPI_BUFFER_TARGET_APU_OFFSET
+#define IPI_BUFFER_REMOTE_OFFSET	IPI_BUFFER_TARGET_PMC_OFFSET
+
+#define IPI_BLOCK		1
+#define IPI_NON_BLOCK		0
+
+/* mailbox api */
+enum ipi_api_id {
+	IPI_MAILBOX_OPEN = 0x1000,
+	IPI_MAILBOX_RELEASE,
+	IPI_MAILBOX_STATUS_ENQUIRY,
+	IPI_MAILBOX_NOTIFY,
+	IPI_MAILBOX_ACK,
+	IPI_MAILBOX_ENABLE_IRQ,
+	IPI_MAILBOX_DISABLE_IRQ
+};
+
+static struct versal_ipi {
+	uint32_t lcl;
+	const uint32_t rmt;
+	paddr_t buf;
+	/* Exclusive access to the IPI shared buffer */
+	struct mutex lock;
+	void *rsp;
+	void *req;
+} ipi = {
+	.buf = IPI_BUFFER_APU_ID_3_BASE,
+	.rmt = IPI_ID_PMC,
+	.lcl = IPI_ID_3,
+};
+
+static TEE_Result mbox_call(enum ipi_api_id id, uint32_t blocking_call)
+{
+	struct thread_smc_args args = {
+		.a0 = PM_SIP_SVC | id,
+		.a1 = reg_pair_to_64(0, ipi.lcl),
+		.a2 = reg_pair_to_64(0, ipi.rmt),
+		.a3 = reg_pair_to_64(0, blocking_call),
+	};
+
+	thread_smccc(&args);
+
+	if (IS_ENABLED(CFG_VERSAL_TRACE_PLM))
+		mdelay(1000);
+
+	if (args.a0)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result versal_mbox_write_req(struct ipi_cmd *cmd)
+{
+	size_t i = 0;
+
+	for (i = 0; i < MAX_IPI_BUF; i++) {
+		if (!cmd->ibuf[i].p)
+			continue;
+
+		if (!IS_ALIGNED((uintptr_t)cmd->ibuf[i].p, CACHELINE_LEN))
+			return TEE_ERROR_GENERIC;
+
+		if (!IS_ALIGNED(cmd->ibuf[i].len, CACHELINE_LEN))
+			return TEE_ERROR_GENERIC;
+
+		cache_operation(TEE_CACHEFLUSH,
+				cmd->ibuf[i].p, cmd->ibuf[i].len);
+	}
+
+	memcpy(ipi.req, cmd->data, sizeof(cmd->data));
+	/* cache operation on the IPI buffer is safe */
+	cache_operation(TEE_CACHEFLUSH, ipi.req, sizeof(cmd->data));
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result versal_mbox_read_rsp(struct ipi_cmd *cmd, struct ipi_cmd *rsp,
+				       uint32_t *status)
+{
+	size_t i = 0;
+
+	/* cache operation on the IPI buffer is safe */
+	cache_operation(TEE_CACHEINVALIDATE, ipi.rsp, sizeof(rsp->data));
+
+	*status = *(uint32_t *)ipi.rsp;
+
+	if (rsp)
+		memcpy(rsp->data, ipi.rsp, sizeof(rsp->data));
+
+	if (*status)
+		return TEE_ERROR_GENERIC;
+
+	for (i = 0; i < MAX_IPI_BUF; i++) {
+		if (!cmd->ibuf[i].p)
+			continue;
+
+		if (!IS_ALIGNED((uintptr_t)cmd->ibuf[i].p, CACHELINE_LEN))
+			return TEE_ERROR_GENERIC;
+
+		if (!IS_ALIGNED(cmd->ibuf[i].len, CACHELINE_LEN))
+			return TEE_ERROR_GENERIC;
+
+		cache_operation(TEE_CACHEINVALIDATE,
+				cmd->ibuf[i].p, cmd->ibuf[i].len);
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result versal_mbox_notify(struct ipi_cmd *cmd, struct ipi_cmd *rsp)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	uint32_t remote_status = 0;
+
+	mutex_lock(&ipi.lock);
+
+	ret = versal_mbox_write_req(cmd);
+	if (ret) {
+		EMSG("Can't write the request command");
+		goto out;
+	}
+
+	ret = mbox_call(IPI_MAILBOX_NOTIFY, IPI_BLOCK);
+	if (ret) {
+		EMSG("IPI error");
+		goto out;
+	}
+
+	ret = versal_mbox_read_rsp(cmd, rsp, &remote_status);
+	if (ret)
+		EMSG("Can't read the remote response");
+
+	if (remote_status) {
+		/* Check xplmi_status.h in the PLM code (hundreds of types) */
+		EMSG("PLM (err=0x%x)", remote_status >> 16);
+		ret = TEE_ERROR_GENERIC;
+	}
+out:
+	mutex_unlock(&ipi.lock);
+
+	return ret;
+}
+
+static TEE_Result versal_mbox_init(void)
+{
+	switch (CFG_VERSAL_MBOX_IPI_ID) {
+	case 0:
+		ipi.buf = IPI_BUFFER_APU_ID_0_BASE;
+		ipi.lcl = IPI_ID_0;
+		break;
+	case 3:
+		break;
+	case 4:
+		ipi.buf = IPI_BUFFER_APU_ID_4_BASE;
+		ipi.lcl = IPI_ID_4;
+		break;
+	case 5:
+		ipi.buf = IPI_BUFFER_APU_ID_5_BASE;
+		ipi.lcl = IPI_ID_5;
+		break;
+	default:
+		EMSG("Invalid IPI requested");
+		return TEE_ERROR_GENERIC;
+	}
+
+	ipi.req = core_mmu_add_mapping(MEM_AREA_RAM_SEC,
+				       ipi.buf + IPI_BUFFER_REMOTE_OFFSET +
+				       IPI_BUFFER_REQ_OFFSET,
+				       sizeof(struct ipi_cmd));
+
+	ipi.rsp = core_mmu_add_mapping(MEM_AREA_RAM_SEC,
+				       ipi.buf + IPI_BUFFER_REMOTE_OFFSET +
+				       IPI_BUFFER_RESP_OFFSET,
+				       sizeof(struct ipi_cmd));
+	if (!ipi.req || !ipi.rsp)
+		panic();
+
+	mutex_init(&ipi.lock);
+
+	return mbox_call(IPI_MAILBOX_OPEN, IPI_BLOCK);
+}
+early_init(versal_mbox_init);
diff --git a/core/drivers/versal_nvm.c b/core/drivers/versal_nvm.c
new file mode 100644
index 00000000000..a51efc1f70d
--- /dev/null
+++ b/core/drivers/versal_nvm.c
@@ -0,0 +1,496 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) 2022 Foundries.io Ltd
+ * Jorge Ramirez-Ortiz <jorge@foundries.io>
+ */
+
+#include <arm.h>
+#include <drivers/versal_nvm.h>
+#include <drivers/versal_mbox.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "drivers/versal_nvm.h"
+
+/* Protocol API with the remote processor */
+#define NVM_MODULE_SHIFT	8
+#define NVM_MODULE		11
+#define NVM_API_ID(_id) ((NVM_MODULE << NVM_MODULE_SHIFT) | (_id))
+
+/*
+ * Max size of the buffer needed for the remote processor to DMA efuse _data_
+ * to/from
+ */
+#define EFUSE_MAX_LEN (EFUSE_MAX_USER_FUSES * sizeof(uint32_t))
+
+enum versal_nvm_api_id {
+	API_FEATURES			= 0,
+	BBRAM_WRITE_AES_KEY		= 1,
+	BBRAM_ZEROIZE			= 2,
+	BBRAM_WRITE_USER_DATA		= 3,
+	BBRAM_READ_USER_DATA		= 4,
+	BBRAM_LOCK_WRITE_USER_DATA	= 5,
+	EFUSE_WRITE			= 6,
+	EFUSE_WRITE_PUF			= 7,
+	EFUSE_PUF_USER_FUSE_WRITE	= 8,
+	EFUSE_READ_IV			= 9,
+	EFUSE_READ_REVOCATION_ID	= 10,
+	EFUSE_READ_OFFCHIP_REVOCATION_ID = 11,
+	EFUSE_READ_USER_FUSES		= 12,
+	EFUSE_READ_MISC_CTRL		= 13,
+	EFUSE_READ_SEC_CTRL		= 14,
+	EFUSE_READ_SEC_MISC1		= 15,
+	EFUSE_READ_BOOT_ENV_CTRL	= 16,
+	EFUSE_READ_PUF_SEC_CTRL		= 17,
+	EFUSE_READ_PPK_HASH		= 18,
+	EFUSE_READ_DEC_EFUSE_ONLY	= 19,
+	EFUSE_READ_DNA			= 20,
+	EFUSE_READ_PUF_USER_FUSES	= 21,
+	EFUSE_READ_PUF			= 22,
+	EFUSE_INVALID			= 23,
+};
+
+struct versal_efuse_sec_ctrl_bits {
+	uint8_t aes_dis;
+	uint8_t jtag_err_out_dis;
+	uint8_t jtag_dis;
+	uint8_t ppk0_wr_lk;
+	uint8_t ppk1_wr_lk;
+	uint8_t ppk2_wr_lk;
+	uint8_t aes_crc_lk;
+	uint8_t aes_wr_lk;
+	uint8_t user_key0_crc_lk;
+	uint8_t user_key0_wr_lk;
+	uint8_t user_key1_crc_lk;
+	uint8_t user_key1_wr_lk;
+	uint8_t sec_dbg_dis;
+	uint8_t sec_lock_dbg_dis;
+	uint8_t boot_env_wr_lk;
+	uint8_t reg_init_dis;
+	uint8_t pad[48];
+} __packed;
+
+struct versal_efuse_puf_sec_ctrl_bits {
+	uint8_t puf_regen_dis;
+	uint8_t puf_hd_invalid;
+	uint8_t puf_test2_dis;
+	uint8_t puf_dis;
+	uint8_t puf_syn_lk;
+	uint8_t pad[59];
+} __packed;
+
+struct versal_efuse_misc_ctrl_bits {
+	uint8_t glitch_det_halt_boot_en;
+	uint8_t glitch_det_rom_monitor_en;
+	uint8_t halt_boot_error;
+	uint8_t halt_boot_env;
+	uint8_t vrypto_kat_en;
+	uint8_t lbist_en;
+	uint8_t safety_mission_en;
+	uint8_t ppk0_invalid;
+	uint8_t ppk1_invalid;
+	uint8_t ppk2_invalid;
+	uint8_t pad[54];
+} __packed;
+
+struct versal_efuse_sec_misc1_bits {
+	uint8_t lpd_mbist_en;
+	uint8_t pmc_mbist_en;
+	uint8_t lpd_noc_sc_en;
+	uint8_t sysmon_volt_mon_en;
+	uint8_t sysmon_temp_mon_en;
+	uint8_t pad[59];
+} __packed;
+
+struct versal_efuse_boot_env_ctrl_bits {
+	uint8_t prgm_sysmon_temp_hot;
+	uint8_t prgm_sysmon_volt_pmc;
+	uint8_t prgm_sysmon_volt_pslp;
+	uint8_t prgm_sysmon_temp_cold;
+	uint8_t sysmon_temp_en;
+	uint8_t sysmon_volt_en;
+	uint8_t sysmon_volt_soc;
+	uint8_t sysmon_temp_hot;
+	uint8_t sysmon_volt_pmc;
+	uint8_t sysmon_volt_pslp;
+	uint8_t sysmon_temp_cold;
+	uint8_t pad[53];
+} __packed;
+
+struct versal_efuse_glitch_cfg_bits {
+	uint8_t prgm_glitch;
+	uint8_t glitch_det_wr_lk;
+	uint32_t glitch_det_trim;
+	uint8_t gd_rom_monitor_en;
+	uint8_t gd_halt_boot_en;
+	uint8_t pad[56];
+} __packed;
+
+struct versal_efuse_aes_keys {
+	uint8_t prgm_aes_key;
+	uint8_t prgm_user_key0;
+	uint8_t prgm_user_key1;
+	uint32_t aes_key[8];
+	uint32_t user_key0[8];
+	uint32_t user_key1[8];
+	uint8_t pad[29];
+} __packed;
+
+struct versal_efuse_ppk_hash {
+	uint8_t prgm_ppk0_hash;
+	uint8_t prgm_ppk1_hash;
+	uint8_t prgm_ppk2_hash;
+	uint32_t ppk0_hash[8];
+	uint32_t ppk1_hash[8];
+	uint32_t ppk2_hash[8];
+	uint8_t pad[29];
+} __packed;
+
+struct versal_efuse_dec_only {
+	uint8_t prgm_dec_only;
+	uint8_t pad[63];
+} __packed;
+
+struct versal_efuse_revoke_ids {
+	uint8_t prgm_revoke_id;
+	uint32_t revoke_id[8];
+	uint8_t pad[31];
+} __packed;
+
+struct versal_efuse_offchip_ids {
+	uint8_t prgm_offchip_id;
+	uint32_t offchip_id[8];
+	uint8_t pad[31];
+} __packed;
+
+struct versal_efuse_ivs {
+	uint8_t prgm_meta_header_iv;
+	uint8_t prgm_blk_obfus_iv;
+	uint8_t prgm_plm_iv;
+	uint8_t prgm_data_partition_iv;
+	uint32_t meta_header_iv[3];
+	uint32_t blk_obfus_iv[3];
+	uint32_t plm_iv[3];
+	uint32_t data_partition_iv[3];
+	uint8_t pad[12];
+} __packed;
+
+struct versal_efuse_user_data {
+	uint32_t start;
+	uint32_t num;
+	uint64_t addr;
+	uint8_t pad[48];
+} __packed;
+
+struct versal_efuse_puf_fuse {
+	uint8_t env_monitor_dis;
+	uint8_t prgm_puf_fuse;
+	uint32_t start;
+	uint32_t num;
+	uint64_t addr;
+	uint8_t pad[46];
+} __packed;
+
+struct versal_efuse_puf_hd {
+	struct versal_efuse_puf_sec_ctrl_bits puf_sec_ctrl_bits;
+	uint8_t prgm_puf_helper_data;
+	uint8_t env_monitor_dis;
+	uint32_t efuse_syn_data[127];
+	uint32_t chash;
+	uint32_t aux;
+	uint8_t pad[58];
+} __packed;
+
+struct versal_efuse_data {
+	uint64_t env_mon_dis_flag;
+	uint64_t aes_key_addr;
+	uint64_t ppk_hash_addr;
+	uint64_t dec_only_addr;
+	uint64_t sec_ctrl_addr;
+	uint64_t misc_ctrl_addr;
+	uint64_t revoke_id_addr;
+	uint64_t iv_addr;
+	uint64_t user_fuse_addr;
+	uint64_t glitch_cfg_addr;
+	uint64_t boot_env_ctrl_addr;
+	uint64_t misc1_ctrl_addr;
+	uint64_t offchip_id_addr;
+	uint8_t pad[24];
+} __packed;
+
+/* Helper read and write requests (not part of the protocol) */
+struct versal_nvm_read_req {
+	enum versal_nvm_api_id efuse_id;
+	enum versal_nvm_revocation_id revocation_id;
+	enum versal_nvm_offchip_id offchip_id;
+	enum versal_nvm_ppk_type ppk_type;
+	enum versal_nvm_iv_type iv_type;
+	struct ipi_buf ibuf[MAX_IPI_BUF];
+};
+
+enum versal_nvm_write_efuse_id {
+	EFUSE_WRITE_USER_FUSES = 0,
+	EFUSE_WRITE_INVALID = 0xffff,
+};
+
+struct versal_nvm_write_req {
+	struct versal_efuse_data data;
+	enum versal_nvm_write_efuse_id id;
+	struct ipi_buf ibuf[MAX_IPI_BUF];
+};
+
+struct cmd_args {
+	uint32_t data[3];
+	size_t len;
+};
+
+static TEE_Result prepare_cmd(struct ipi_cmd *cmd, enum versal_nvm_api_id efuse,
+			      struct ipi_buf *ibufs, struct cmd_args *arg)
+{
+	size_t i = 0;
+
+	cmd->data[0] = NVM_API_ID(efuse);
+	for (i = 1; i < arg->len + 1; i++)
+		cmd->data[i] = arg->data[i - 1];
+
+	if (!ibufs[0].p)
+		return TEE_SUCCESS;
+
+	cmd->data[i++] = virt_to_phys(ibufs[0].p);
+	cmd->data[i++] = virt_to_phys(ibufs[0].p) >> 32;
+
+	for (i = 0; i < MAX_IPI_BUF; i++) {
+		cmd->ibuf[i].len = ibufs[i].len;
+		cmd->ibuf[i].p = ibufs[i].p;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result efuse_req(enum versal_nvm_api_id efuse, struct ipi_buf *ibufs,
+			    struct cmd_args *arg)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	struct ipi_cmd cmd = { };
+
+	ret = prepare_cmd(&cmd, efuse, ibufs, arg);
+	if (ret)
+		return ret;
+
+	ret = versal_mbox_notify(&cmd, NULL);
+	if (ret)
+		EMSG("Mailbox error");
+
+	return ret;
+}
+
+static TEE_Result versal_nvm_read(struct versal_nvm_read_req *req)
+{
+	struct cmd_args args = { };
+
+	if (!req)
+		return TEE_ERROR_GENERIC;
+
+	switch (req->efuse_id) {
+	case EFUSE_READ_DNA:
+	case EFUSE_READ_DEC_EFUSE_ONLY:
+	case EFUSE_READ_PUF_SEC_CTRL:
+	case EFUSE_READ_BOOT_ENV_CTRL:
+	case EFUSE_READ_SEC_CTRL:
+	case EFUSE_READ_MISC_CTRL:
+	case EFUSE_READ_SEC_MISC1:
+	case BBRAM_READ_USER_DATA:
+	case EFUSE_READ_USER_FUSES:
+	case EFUSE_READ_PUF_USER_FUSES:
+	case EFUSE_READ_PUF:
+		break;
+	case BBRAM_ZEROIZE:
+	case BBRAM_LOCK_WRITE_USER_DATA:
+		if (req->ibuf[0].p)
+			return TEE_ERROR_GENERIC;
+		break;
+	case EFUSE_READ_OFFCHIP_REVOCATION_ID:
+		args.data[0] = req->offchip_id;
+		args.len = 1;
+		break;
+	case EFUSE_READ_REVOCATION_ID:
+		args.data[0] = req->revocation_id;
+		args.len = 1;
+		break;
+	case EFUSE_READ_IV:
+		args.data[0] = req->iv_type;
+		args.len = 1;
+		break;
+	case EFUSE_READ_PPK_HASH:
+		args.data[0] = req->ppk_type;
+		args.len = 1;
+		break;
+	default:
+		return TEE_ERROR_GENERIC;
+	}
+
+	return efuse_req(req->efuse_id, req->ibuf, &args);
+}
+
+static TEE_Result versal_nvm_write(struct versal_nvm_write_req *req)
+{
+	enum versal_nvm_api_id efuse_id = EFUSE_INVALID;
+	struct cmd_args args = { };
+
+	switch (req->id) {
+	case EFUSE_WRITE_USER_FUSES:
+		efuse_id = EFUSE_WRITE;
+		break;
+	default:
+		return TEE_ERROR_GENERIC;
+	}
+
+	return efuse_req(efuse_id, req->ibuf, &args);
+}
+
+TEE_Result versal_read_efuse_dna(uint32_t *buf, size_t len)
+{
+	uint8_t lbuf[EFUSE_MAX_LEN] __aligned_efuse = { 0 };
+	struct versal_nvm_read_req req = { 0 };
+
+	if (len < EFUSE_DNA_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Prepare request */
+	req.efuse_id = EFUSE_READ_DNA;
+
+	/* Request cache management */
+	req.ibuf[0].p = lbuf;
+	req.ibuf[0].len = sizeof(lbuf);
+
+	if (versal_nvm_read(&req))
+		return TEE_ERROR_GENERIC;
+
+	memcpy(buf, lbuf, EFUSE_DNA_LEN);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result versal_read_efuse_user(uint32_t *buf, size_t len, uint32_t first,
+				  size_t num)
+{
+	uint8_t lbuf[EFUSE_MAX_LEN] __aligned_efuse = { 0 };
+	struct versal_efuse_user_data cfg __aligned_efuse = {
+		.addr = (uintptr_t)lbuf,
+		.start = first,
+		.num = num, /* fuses needs to be at least 40 bytes */
+	};
+	struct versal_nvm_read_req req = { 0 };
+
+	if (first + num > EFUSE_MAX_USER_FUSES || len < num * sizeof(uint32_t))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Prepare request */
+	req.efuse_id = EFUSE_READ_USER_FUSES;
+
+	/* Request cache management */
+	req.ibuf[0].p = &cfg;
+	req.ibuf[0].len = sizeof(cfg);
+	req.ibuf[1].p = lbuf;
+	req.ibuf[1].len = sizeof(lbuf);
+
+	/* Update the command buffer */
+	cfg.addr = (paddr_t)virt_to_phys((void *)cfg.addr);
+
+	if (versal_nvm_read(&req))
+		return TEE_ERROR_GENERIC;
+
+	memcpy(buf, lbuf, num * sizeof(uint32_t));
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result versal_read_efuse_iv(uint32_t *buf, size_t len,
+				enum versal_nvm_iv_type type)
+{
+	uint8_t lbuf[EFUSE_MAX_LEN] __aligned_efuse = { 0 };
+	struct versal_nvm_read_req req = { .iv_type = type, };
+
+	if (len < EFUSE_IV_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Prepare the request */
+	req.efuse_id = EFUSE_READ_IV;
+
+	/* Request cache management */
+	req.ibuf[0].p = lbuf;
+	req.ibuf[0].len = sizeof(lbuf);
+
+	if (versal_nvm_read(&req))
+		return TEE_ERROR_GENERIC;
+
+	memcpy(buf, lbuf, EFUSE_IV_LEN);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result versal_read_efuse_ppk(uint32_t *buf, size_t len,
+				 enum versal_nvm_ppk_type type)
+{
+	uint8_t lbuf[EFUSE_MAX_LEN]__aligned_efuse = { 0 };
+	struct versal_nvm_read_req req = { .ppk_type = type, };
+
+	if (len < EFUSE_PPK_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Prepare the request */
+	req.efuse_id = EFUSE_READ_PPK_HASH;
+
+	/* Request cache management */
+	req.ibuf[0].p = lbuf;
+	req.ibuf[0].len = sizeof(lbuf);
+
+	if (versal_nvm_read(&req))
+		return TEE_ERROR_GENERIC;
+
+	memcpy(buf, lbuf, EFUSE_PPK_LEN);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result versal_write_efuse_user(uint32_t *buf, size_t len, uint32_t first,
+				   size_t num)
+{
+	uint32_t lbuf[EFUSE_MAX_USER_FUSES] __aligned_efuse = { 0 };
+	struct versal_efuse_user_data cfg __aligned_efuse = {
+		.addr = (uintptr_t)lbuf,
+		.start = first,
+		.num = num,
+	};
+	struct versal_nvm_write_req req __aligned_efuse = {
+		.data.env_mon_dis_flag = 1,
+		.data.user_fuse_addr = (uintptr_t)&cfg,
+		.id = EFUSE_WRITE_USER_FUSES,
+	};
+	size_t i = 0;
+
+	if (first + num > EFUSE_MAX_USER_FUSES || len  < num * sizeof(uint32_t))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Update the command buffers with physical addresses */
+	req.data.user_fuse_addr = (paddr_t)
+				  virt_to_phys((void *)req.data.user_fuse_addr);
+	cfg.addr = (paddr_t)virt_to_phys(lbuf);
+
+	/* Request cache management */
+	req.ibuf[0].p = &req.data;
+	req.ibuf[0].len = sizeof(req.data);
+	req.ibuf[1].p = &cfg;
+	req.ibuf[1].len = sizeof(cfg);
+	req.ibuf[2].p = lbuf;
+	req.ibuf[2].len = sizeof(lbuf);
+
+	/* Prepare fuses to write with some random data */
+	for (i = 0; i < cfg.num; i++)
+		lbuf[i] = buf[i];
+
+	return versal_nvm_write(&req);
+}
diff --git a/core/drivers/versal_nvm_test.c b/core/drivers/versal_nvm_test.c
new file mode 100644
index 00000000000..c93fa31390f
--- /dev/null
+++ b/core/drivers/versal_nvm_test.c
@@ -0,0 +1,143 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) 2022 Foundries.io Ltd
+ * Jorge Ramirez-Ortiz <jorge@foundries.io>
+ *
+ * The proper way to run this test would be to write the eFuses using some
+ * Xilinx tool and then just memcmp the read results.
+ *
+ * For writing would be the inverse test.
+ *
+ * Foundries has validated this manually and through visual inspection of the
+ * output.
+ *
+ */
+
+#include <arm.h>
+#include <drivers/versal_nvm.h>
+#include <drivers/versal_mbox.h>
+#include <initcall.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+
+#include "drivers/versal_nvm.h"
+
+#define __STR(X) #X
+#define STR(X) __STR(X)
+
+static TEE_Result test_read_ppk(void)
+{
+	uint32_t hash[8] = { 0xff };
+
+	if (versal_read_efuse_ppk(hash, sizeof(hash), EFUSE_PPK1))
+		return TEE_ERROR_GENERIC;
+
+#if 0
+	DMSG("PPK1 hash:");
+	DHEXDUMP((void *)hash, sizeof(hash));
+#endif
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_read_iv(void)
+{
+	uint32_t iv[3] = { 0xff };
+
+	if (versal_read_efuse_iv(iv, sizeof(iv), EFUSE_PLM_IV_RANGE))
+		return TEE_ERROR_GENERIC;
+
+#if 0
+	DMSG("EFUSE_PLM_IV_RANGE eFuse:");
+	DHEXDUMP((void *)iv, sizeof(iv));
+#endif
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_read_dna(void)
+{
+	uint32_t dna[4];
+
+	if (versal_read_efuse_dna(dna, sizeof(dna)))
+		return TEE_ERROR_GENERIC;
+
+#if 0
+	DMSG("DNA eFuse");
+	DHEXDUMP((void *)dna, sizeof(dna));
+#endif
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_read_user_fuses(void)
+{
+	uint32_t fuses[64] = { 0xff };
+
+	if (versal_read_efuse_user(fuses, sizeof(fuses), 2, 10))
+		return TEE_ERROR_GENERIC;
+
+#if 0
+	DMSG("User eFuse(s)");
+	DHEXDUMP((void *)fuses, sizeof(fuses));
+#endif
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_write_user_fuses(void)
+{
+	/* the length of the source buffer must be cacheline aligned */
+	uint32_t fuses[4] = { 0xab123456, 0xbc123456, 0xcd123456, 0xde123456 };
+
+	if (versal_write_efuse_user(fuses, sizeof(fuses), 3 ,4))
+#if 0
+		return TEE_ERROR_GENERIC;
+#else
+		return TEE_SUCCESS;
+	/**
+	 * We cant just write the efuse again and again everytime the test
+	 * is executed. Since this has been tested, it is ok to disable it
+	 * now and always return a pass result.
+	 */
+#endif
+	return TEE_SUCCESS;
+}
+
+static struct {
+	TEE_Result (*f)(void);
+	const char *name;
+	bool failed;
+} test[] = {
+	{ .f = test_write_user_fuses, .name = STR(wr usr fuse),},
+	{ .f = test_read_user_fuses,  .name = STR(rd usr fuse),},
+	{ .f = test_read_dna,         .name = STR(rd dna fuse),},
+	{ .f = test_read_ppk,         .name = STR(rd ppk fuse),},
+	{ .f = test_read_iv,          .name = STR(rd iv  fuse),},
+	{ .f = NULL, .name = "" },
+};
+
+static TEE_Result versal_nvm_test(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	size_t i = 0;
+
+	do {
+		ret = (test[i].f)();
+		if (ret)
+			test[i].failed = true;
+		i++;
+
+	} while (test[i].f);
+
+	IMSG("Versal: Test NVM");
+
+	for (i = 0; i < ARRAY_SIZE(test) - 1; i++)
+		IMSG("---- %s:\t\t\t\t\t [%s]",
+		     test[i].name, test[i].failed ? "KO" : "OK");
+
+	return TEE_SUCCESS;;
+}
+driver_init(versal_nvm_test);
+
diff --git a/core/drivers/versal_pm.c b/core/drivers/versal_pm.c
new file mode 100644
index 00000000000..3dfcbfad750
--- /dev/null
+++ b/core/drivers/versal_pm.c
@@ -0,0 +1,110 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) 2022 Foundries.io Ltd
+ * Jorge Ramirez-Ortiz <jorge@foundries.io>
+ */
+
+#include <arm.h>
+#include <drivers/versal_pm.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/delay.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <tee/cache.h>
+#include <tee_api_types.h>
+#include <types_ext.h>
+#include <utee_defines.h>
+
+#define PAYLOAD_ARG_CNT		6
+#define PM_SIP_SVC		0xc2000000
+
+/* PM API ids */
+#define PM_GET_API_VERSION		1
+#define PM_GET_DEVICE_STATUS		3
+#define PM_GET_OP_CHARACTERISTIC	4
+#define PM_REGISTER_NOTIFIER		5
+#define PM_REQ_SUSPEND			6
+#define PM_SELF_SUSPEND			7
+#define PM_FORCE_POWERDOWN		8
+#define PM_ABORT_SUSPEND		9
+#define PM_REQ_WAKEUP			10
+#define PM_SET_WAKEUP_SOURCE		11
+#define PM_SYSTEM_SHUTDOWN		12
+#define PM_REQUEST_DEVICE		13
+#define PM_RELEASE_DEVICE		14
+#define PM_SET_REQUIREMENT		15
+#define PM_SET_MAX_LATENCY		16
+#define PM_RESET_ASSERT			17
+#define PM_RESET_GET_STATUS		18
+#define PM_INIT_FINALIZE		21
+#define PM_GET_CHIPID			24
+#define	PM_PINCTRL_REQUEST		28
+#define	PM_PINCTRL_RELEASE		29
+#define	PM_PINCTRL_GET_FUNCTION		30
+#define	PM_PINCTRL_SET_FUNCTION		31
+#define	PM_PINCTRL_CONFIG_PARAM_GET	32
+#define	PM_PINCTRL_CONFIG_PARAM_SET	33
+#define PM_IOCTL			34
+#define PM_QUERY_DATA			35
+#define PM_CLOCK_ENABLE			36
+#define PM_CLOCK_DISABLE		37
+#define PM_CLOCK_GETSTATE		38
+#define PM_CLOCK_SETDIVIDER		39
+#define PM_CLOCK_GETDIVIDER		40
+#define PM_CLOCK_SETRATE		41
+#define PM_CLOCK_GETRATE		42
+#define PM_CLOCK_SETPARENT		43
+#define PM_CLOCK_GETPARENT		44
+#define PM_PLL_SET_PARAMETER		48
+#define PM_PLL_GET_PARAMETER		49
+#define PM_PLL_SET_MODE			50
+#define PM_PLL_GET_MODE			51
+
+struct versal_sip_payload {
+	uint32_t data[PAYLOAD_ARG_CNT];
+};
+
+static uint32_t versal_sip_call(uint32_t pm_api_id, uint32_t arg0,
+				uint32_t arg1, uint32_t arg2, uint32_t arg3,
+				struct versal_sip_payload *payload)
+{
+	struct thread_smc_args args = {
+		.a0 = PM_SIP_SVC | pm_api_id,
+		.a1 = reg_pair_to_64(arg1, arg0),
+		.a2 = reg_pair_to_64(arg3, arg2),
+	};
+
+	thread_smccc(&args);
+
+	if (payload) {
+		reg_pair_from_64(args.a0, &payload->data[1], &payload->data[0]);
+		reg_pair_from_64(args.a1, &payload->data[3], &payload->data[2]);
+		reg_pair_from_64(args.a2, &payload->data[5], &payload->data[4]);
+	}
+
+	if (IS_ENABLED(CFG_VERSAL_TRACE_PLM))
+		mdelay(1000);
+
+	return args.a0;
+}
+
+TEE_Result versal_soc_version(uint8_t *version)
+{
+	struct versal_sip_payload payload = { };
+	const uint32_t version_shift = 12;
+	uint32_t res = 0;
+
+	if (!version)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = versal_sip_call(PM_GET_CHIPID, 0, 0, 0, 0, &payload);
+	if (res) {
+		EMSG("Failed to retrieve version");
+		return TEE_ERROR_GENERIC;
+	}
+
+	*version = payload.data[2] >> version_shift;
+
+	return TEE_SUCCESS;
+}
diff --git a/core/drivers/versal_trng.c b/core/drivers/versal_trng.c
new file mode 100644
index 00000000000..dcc23c336e2
--- /dev/null
+++ b/core/drivers/versal_trng.c
@@ -0,0 +1,1158 @@
+// SPDX-License-Identifier: MIT
+/*
+ * Copyright (C) 2022 Xilinx, Inc.  All rights reserved.
+ * Copyright (C) 2022 Foundries Ltd.
+ *
+ * Driver port from Xilinx's FSBL implementation, jorge@foundries.io
+ *
+ * The Xilinx True Random Number Generator(TRNG) module in Versal - PMC TRNG
+ * consists of an entropy source, a deterministic random bit generator (DRBG)
+ * and health test logic, which tests the randomness of the generated data.
+ * The entropy source for the unit is an array of Ring Oscillators.
+ *
+ * The Versal PMC TRNG is envisaged to operate in three basic modes: DRNG, PTRNG
+ * and HRNG . Each of these can be operated with or without Derivative Function
+ * (DF), resulting in a total of 6 different modes of operation.
+ *
+ * NIST SP-800-90A practically requires the true random generators based on
+ * CTR_DRBG to include a derivation function (DF). This is expected to be
+ * implemented inside the Silicon (TRNG IP). However, the version of the IP used
+ * in Versal PMC does not have this capability. Hence, a software
+ * implementation of the DF is done in this driver.
+ *
+ * DRNG mode: Deterministic Random Number Generator mode.
+ *            In this mode, the DRBG portion of the TRNG is used. User provides
+ *            the (external) seed.
+ * PTRNG mode: Physical True Random Number Generator mode (aka Entropy mode).
+ *            In this mode digitized Entropy source is output as random number.
+ * HRNG mode: Hybrid Random Number Generator mode.
+ *            This is combination of above two modes in which the Entropy source
+ *            is used to provide the seed, which is fed to the DRBG, which in
+ *            turn generates the random number.
+ *
+ * DRNG mode with DF: It may not be common usecase to use the DF with DRNG as
+ * the general expectation would be that the seed would have sufficient entropy.
+ * However, the below guideline from section 10.2.1 of NIST SP-800-90A implies
+ * that need for DF for DRNG mode too: "..the DRBG mechanism is specified to
+ * allow an implementation tradeoff with respect to the use of this derivation
+ * function. The use of the derivation function is optional if either an
+ * approved RBG or an entropy source provides full entropy output when entropy
+ * input is requested by the DRBG mechanism. Otherwise, the derivation function
+ * shall be used". Sufficient large entropy data from user is fed to DF to
+ * generate the seed which will be loaded into the external seed registers.
+ * From here, it is similar to regular DRNG mode.
+ *
+ * PTRNG mode with DF: This mode is similar to PTRNG mode, however, the entropy
+ * data from the core output registers are accumulated and fed to the DF
+ * (instead of directly consuming it). The output of the DF would be final
+ * random data. In this mode, the output of DF is not seed but the random data.
+ *
+ * HRNG mode with DF: This mode is the combination of the above two modes.
+ * The entropy data is fed to the DF to produce seed. This seed is loaded to the
+ * external seed registers which provide seed to the DRBG.
+ */
+#include <arm.h>
+#include <crypto/crypto.h>
+#include <initcall.h>
+#include <io.h>
+#include <kernel/delay.h>
+#include <kernel/panic.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <rng_support.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee/tee_cryp_utl.h>
+#include <trace.h>
+
+#define TRNG_STATUS			0x04
+#define TRNG_STATUS_QCNT_SHIFT		9
+#define TRNG_STATUS_QCNT_MASK		(BIT(9) | BIT(10) | BIT(11))
+#define TRNG_STATUS_CERTF_MASK		BIT(3)
+#define TRNG_STATUS_DTF_MASK		BIT(1)
+#define TRNG_STATUS_DONE_MASK		BIT(0)
+#define TRNG_CTRL			0x08
+#define TRNG_CTRL_EUMODE_MASK		BIT(8)
+#define TRNG_CTRL_PRNGMODE_MASK		BIT(7)
+#define TRNG_CTRL_PRNGSTART_MASK	BIT(5)
+#define TRNG_CTRL_PRNGXS_MASK		BIT(3)
+#define TRNG_CTRL_TRSSEN_MASK		BIT(2)
+#define TRNG_CTRL_PRNGSRST_MASK		BIT(0)
+#define TRNG_EXT_SEED_0			0x40
+/* Below registers are not directly referenced in driver but are accessed
+ * with offset from TRNG_EXT_SEED_0
+ * Register: TRNG_EXT_SEED_1		0x00000044
+ * Register: TRNG_EXT_SEED_2		0x00000048
+ * Register: TRNG_EXT_SEED_3		0x0000004C
+ * Register: TRNG_EXT_SEED_4		0x00000050
+ * Register: TRNG_EXT_SEED_5		0x00000054
+ * Register: TRNG_EXT_SEED_6		0x00000058
+ * Register: TRNG_EXT_SEED_7		0x0000005C
+ * Register: TRNG_EXT_SEED_8		0x00000060
+ * Register: TRNG_EXT_SEED_9		0x00000064
+ * Register: TRNG_EXT_SEED_10		0x00000068
+ * Register: TRNG_EXT_SEED_11		0x0000006C
+ */
+#define TRNG_PER_STRING_0		0x80
+/* Below registers are not directly referenced in driver but are accessed
+ * with offset from TRNG_PER_STRING_0
+ * Register: TRNG_PER_STRING_1		0x00000084
+ * Register: TRNG_PER_STRING_2		0x00000088
+ * Register: TRNG_PER_STRING_3		0x0000008C
+ * Register: TRNG_PER_STRING_4		0x00000090
+ * Register: TRNG_PER_STRING_5		0x00000094
+ * Register: TRNG_PER_STRING_6		0x00000098
+ * Register: TRNG_PER_STRING_7		0x0000009C
+ * Register: TRNG_PER_STRING_8		0x000000A0
+ * Register: TRNG_PER_STRING_9		0x000000A4
+ * Register: TRNG_PER_STRING_10		0x000000A8
+ * Register: TRNG_PER_STRING_11		0x000000AC
+ */
+#define TRNG_CORE_OUTPUT		0xC0
+#define TRNG_RESET			0xD0
+#define TRNG_RESET_VAL_MASK		BIT(0)
+#define TRNG_OSC_EN			0xD4
+#define TRNG_OSC_EN_VAL_MASK		BIT(0)
+
+/* TRNG configuration  */
+#define TRNG_BURST_SIZE		16
+#define TRNG_BURST_SIZE_BITS	128
+#define TRNG_NUM_INIT_REGS	12
+#define TRNG_REG_SIZE		32
+#define TRNG_BYTES_PER_REG	4
+#define TRNG_MAX_QCNT		4
+#define TRNG_RESEED_TIMEOUT	15000
+#define TRNG_GENERATE_TIMEOUT	8000
+#define TRNG_MIN_DFLENMULT	2
+#define TRNG_MAX_DFLENMULT	9
+#define PRNGMODE_RESEED		0
+#define PRNGMODE_GEN		TRNG_CTRL_PRNGMODE_MASK
+#define RESET_DELAY		10
+#define TRNG_SEC_STRENGTH_LEN	32
+#define TRNG_PERS_STR_REGS	12
+#define TRNG_PERS_STR_LEN	48
+#define TRNG_SEED_REGS		12
+#define TRNG_SEED_LEN		48
+#define TRNG_GEN_LEN		32
+#define RAND_BUF_LEN		4
+#define BYTES_PER_BLOCK		16
+#define ALL_A_PATTERN_32	0xAAAAAAAA
+#define ALL_5_PATTERN_32	0x55555555
+
+/* Derivative function helper macros */
+#define DF_SEED			0
+#define DF_RAND			1
+#define DF_IP_IV_LEN		4
+#define DF_PAD_DATA_LEN		8
+#define MAX_PRE_DF_LEN		160
+#define MAX_PRE_DF_LEN_WORDS	40
+#define DF_PERS_STR_LEN		TRNG_PERS_STR_LEN
+#define DF_PAD_VAL		0x80
+#define DF_KEY_LEN		32
+#define BLK_SIZE		16
+#define MAX_ROUNDS		14
+
+enum trng_status {
+	TRNG_UNINITIALIZED = 0,
+	TRNG_HEALTHY,
+	TRNG_ERROR,
+	TRNG_CATASTROPHIC
+};
+
+enum trng_mode {
+	TRNG_HRNG = 0,
+	TRNG_DRNG,
+	TRNG_PTRNG
+};
+
+struct trng_cfg {
+	paddr_t base;
+	vaddr_t addr;
+	size_t len;
+};
+
+struct trng_usr_cfg {
+	enum trng_mode mode;
+	uint64_t seed_life;      /* number of TRNG requests per seed */
+	bool predict_en;         /* enable prediction resistance     */
+	bool pstr_en;            /* enable personalization string    */
+	uint32_t pstr[TRNG_PERS_STR_REGS];
+	bool iseed_en;           /* enable an initial seed           */
+	uint32_t init_seed[MAX_PRE_DF_LEN_WORDS];
+	uint32_t df_disable;     /* disable the derivative function  */
+	uint32_t dfmul;          /* derivative function multiplier   */
+};
+
+struct trng_stats {
+	uint64_t bytes;
+	uint64_t bytes_reseed;
+	uint64_t elapsed_seed_life;
+};
+
+/* block cipher derivative function algorithm */
+struct trng_dfin {
+	uint32_t ivc[DF_IP_IV_LEN];
+	uint32_t val1;
+	uint32_t val2;
+	uint8_t entropy[MAX_PRE_DF_LEN];    /* input entropy                */
+	uint8_t pstr[DF_PERS_STR_LEN];      /* personalization string       */
+	uint8_t pad_data[DF_PAD_DATA_LEN];  /* pad to multiples of 16 bytes*/
+};
+
+struct versal_trng {
+	struct trng_cfg cfg;
+	struct trng_usr_cfg usr_cfg;
+	struct trng_stats stats;
+	enum trng_status status;
+	uint32_t buf[RAND_BUF_LEN];   /* buffer of random bits      */
+	size_t len;
+	struct trng_dfin dfin;
+	uint8_t dfout[TRNG_SEED_LEN]; /* output of the DF operation */
+};
+
+/* Derivative function variables */
+static unsigned char SBx1[256];
+static unsigned char SBx2[256];
+static unsigned char SBx3[256];
+static unsigned char schedule[BLK_SIZE * (MAX_ROUNDS + 1)];
+static unsigned int rounds;
+
+static void rota4(uint8_t *a, uint8_t *b, uint8_t *c, uint8_t *d)
+{
+	uint8_t t = *a;
+
+	*a = SBx1[*b];
+	*b = SBx1[*c];
+	*c = SBx1[*d];
+	*d = SBx1[t];
+}
+
+static void rota2(uint8_t *a, uint8_t *b)
+{
+	uint8_t t = *a;
+
+	*a = SBx1[*b];
+	*b = SBx1[t];
+}
+
+static void sbox4(uint8_t *a, uint8_t *b, uint8_t *c, uint8_t *d)
+{
+	*a = SBx1[*a];
+	*b = SBx1[*b];
+	*c = SBx1[*c];
+	*d = SBx1[*d];
+}
+
+static void xorb(uint8_t *res,  const uint8_t *in)
+{
+	size_t i = 0;
+
+	for (i = 0; i < BLK_SIZE; ++i)
+		res[i] ^= in[i];
+}
+
+static void set_key(uint8_t *res, const uint8_t *src, unsigned int roundval)
+{
+	memcpy(res, src, BLK_SIZE);
+	xorb(res, schedule + roundval * BLK_SIZE);
+}
+
+static void mix_column_sbox(uint8_t *dst, const uint8_t *f)
+{
+	size_t i = 0;
+	size_t a = 0;
+	size_t b = 0;
+	size_t c = 0;
+	size_t d = 0;
+
+	for (i = 0; i < 4; i++) {
+		a = 4 * i;
+		b = (0x5 + a) % 16;
+		c = (0xa + a) % 16;
+		d = (0xf + a) % 16;
+		dst[0 + a] = SBx2[f[a]] ^ SBx3[f[b]] ^ SBx1[f[c]] ^ SBx1[f[d]];
+		dst[1 + a] = SBx1[f[a]] ^ SBx2[f[b]] ^ SBx3[f[c]] ^ SBx1[f[d]];
+		dst[2 + a] = SBx1[f[a]] ^ SBx1[f[b]] ^ SBx2[f[c]] ^ SBx3[f[d]];
+		dst[3 + a] = SBx3[f[a]] ^ SBx1[f[b]] ^ SBx1[f[c]] ^ SBx2[f[d]];
+	}
+}
+
+static void shift_row_sbox(uint8_t *f)
+{
+	sbox4(&f[0], &f[4], &f[8], &f[12]);
+	rota4(&f[1], &f[5], &f[9], &f[13]);
+	rota2(&f[2], &f[10]);
+	rota2(&f[6], &f[14]);
+	rota4(&f[15], &f[11], &f[7], &f[3]);
+}
+
+static void encrypt(uint8_t *in, uint8_t *out)
+{
+	uint8_t fA[BLK_SIZE] = { 0 };
+	uint8_t fB[BLK_SIZE] = { 0 };
+	size_t roundval = 0;
+
+	set_key(fA, in, 0);
+	for (roundval = 1; roundval < rounds; ++roundval) {
+		mix_column_sbox(fB, fA);
+		set_key(fA, fB, roundval);
+	}
+
+	shift_row_sbox(fA);
+	set_key(out, fA, roundval);
+}
+
+static void checksum(unsigned char *in, uint8_t *iv, int max_blk)
+{
+	while (max_blk > 0) {
+		xorb(iv, in);
+		encrypt(iv, iv);
+		in += BLK_SIZE;
+		max_blk -= 1;
+	}
+}
+
+static void setup_key(const unsigned char *k, size_t klen)
+{
+	unsigned char rcon = 1;
+	size_t sch_size = 240;
+	size_t i;
+
+	rounds = MAX_ROUNDS;
+	memcpy(schedule, k, klen);
+	for (i = klen; i < sch_size; i += 4) {
+		unsigned char t0 = 0;
+		unsigned char t1 = 0;
+		unsigned char t2 = 0;
+		unsigned char t3 = 0;
+		int ik = 0;
+
+		t0 = schedule[i - 4];
+		t1 = schedule[i - 3];
+		t2 = schedule[i - 2];
+		t3 = schedule[i - 1];
+		if (i % klen == 0) {
+			rota4(&t0, &t1, &t2, &t3);
+			t0 ^= rcon;
+			rcon = (rcon << 1) ^ (((rcon >> 7) & 1) * 0x1B);
+		} else if (i % klen == 16) {
+			sbox4(&t0, &t1, &t2, &t3);
+		}
+		ik = i - klen;
+		schedule[i + 0] = schedule[ik + 0] ^ t0;
+		schedule[i + 1] = schedule[ik + 1] ^ t1;
+		schedule[i + 2] = schedule[ik + 2] ^ t2;
+		schedule[i + 3] = schedule[ik + 3] ^ t3;
+	}
+}
+
+static void trng_df_init(void)
+{
+	uint8_t sb[] = {
+		0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01,
+		0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, 0xca, 0x82, 0xc9, 0x7d,
+		0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4,
+		0x72, 0xc0, 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+		0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, 0x04, 0xc7,
+		0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2,
+		0xeb, 0x27, 0xb2, 0x75, 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e,
+		0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+		0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb,
+		0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, 0xd0, 0xef, 0xaa, 0xfb,
+		0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c,
+		0x9f, 0xa8, 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+		0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, 0xcd, 0x0c,
+		0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d,
+		0x64, 0x5d, 0x19, 0x73, 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a,
+		0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+		0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3,
+		0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, 0xe7, 0xc8, 0x37, 0x6d,
+		0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a,
+		0xae, 0x08, 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+		0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, 0x70, 0x3e,
+		0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9,
+		0x86, 0xc1, 0x1d, 0x9e, 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9,
+		0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+		0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99,
+		0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16,
+	};
+	size_t i = 0;
+
+	memcpy(SBx1, sb, sizeof(sb));
+	for (i = 0; i < sizeof(sb); i++) {
+		SBx2[i] = (sb[i] << 1) ^ (((sb[i] >> 7) & 1) * 0x1B);
+		SBx3[i] = SBx2[i] ^ sb[i];
+	}
+}
+
+/*
+ * This function implements the Derivative Function by distilling the entropy
+ * available in its input into a smaller number of bits on the output.
+ * - per NIST SP80090A.
+ *
+ * The Block Cipher algorithm follows sections 10.3.2 and 10.3.3 of the
+ * NIST.SP.800-90Ar1 document
+ */
+static void trng_df_algorithm(struct versal_trng *trng, uint8_t *dfout,
+			      uint32_t flag, const uint8_t *pstr)
+{
+	static bool df_init;
+	const uint8_t df_key[DF_KEY_LEN] = {
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+		17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31
+	};
+	size_t dfin_len = sizeof(struct trng_dfin) + trng->len;
+	uint8_t *inp_blk = NULL;
+	uint8_t *out_blk = NULL;
+	uintptr_t reminder = 0;
+	size_t xfer_len = 0;
+	uint32_t index = 0;
+	uintptr_t src = 0;
+	uintptr_t dst = 0;
+	size_t offset = 0;
+
+	if (!df_init) {
+		trng_df_init();
+		df_init = true;
+	}
+
+	if (flag == DF_SEED)
+		trng->dfin.val2 = TEE_U32_TO_BIG_ENDIAN(TRNG_PERS_STR_LEN);
+	else
+		trng->dfin.val2 = TEE_U32_TO_BIG_ENDIAN(TRNG_GEN_LEN);
+
+	trng->dfin.pad_data[0] = DF_PAD_VAL;
+
+	if (!pstr) {
+		if (trng->len > (MAX_PRE_DF_LEN + TRNG_PERS_STR_LEN))
+			panic();
+
+		dfin_len = dfin_len - TRNG_PERS_STR_LEN - MAX_PRE_DF_LEN;
+		trng->dfin.val1 = TEE_U32_TO_BIG_ENDIAN(trng->len);
+
+		xfer_len = DF_PAD_DATA_LEN;
+		src = (uintptr_t)trng->dfin.pad_data;
+		offset = MAX_PRE_DF_LEN + TRNG_PERS_STR_LEN - trng->len;
+	} else {
+		if (trng->len > MAX_PRE_DF_LEN)
+			panic();
+
+		memcpy(trng->dfin.pstr, pstr, TRNG_PERS_STR_LEN);
+		dfin_len = dfin_len - MAX_PRE_DF_LEN;
+		trng->dfin.val1 = TEE_U32_TO_BIG_ENDIAN(trng->len +
+							TRNG_PERS_STR_LEN);
+		xfer_len = DF_PAD_DATA_LEN + TRNG_PERS_STR_LEN;
+		src = (uintptr_t)trng->dfin.pstr;
+		offset = MAX_PRE_DF_LEN - trng->len;
+	}
+
+	/* Move back into the dfin structure */
+	dst = src - offset;
+	reminder = (uintptr_t)&trng->dfin + sizeof(trng->dfin) - offset;
+	if (offset) {
+		memcpy((void *)dst, (void *)src, xfer_len);
+		memset((void *)reminder, 0, offset);
+	}
+
+	/* DF algorithm - step 1 */
+	setup_key(df_key, DF_KEY_LEN);
+	for (index = 0; index < TRNG_SEED_LEN; index += BLK_SIZE) {
+		memset((void *)(trng->dfout + index), 0, BLK_SIZE);
+		trng->dfin.ivc[0] = TEE_U32_TO_BIG_ENDIAN(index / BLK_SIZE);
+		checksum((unsigned char *)&trng->dfin,
+			 trng->dfout + index, dfin_len / BLK_SIZE);
+	}
+
+	/* DF algorithm - step 2 */
+	setup_key(trng->dfout, DF_KEY_LEN);
+	for (index = 0; index < TRNG_SEED_LEN; index += BLK_SIZE) {
+		if (!index)
+			inp_blk = &dfout[TRNG_SEC_STRENGTH_LEN];
+		else
+			inp_blk = &dfout[index - BLK_SIZE];
+
+		out_blk = &dfout[index];
+		encrypt(inp_blk, out_blk);
+	}
+}
+
+static uint32_t trng_read32(vaddr_t addr, size_t off)
+{
+	return io_read32(addr + off);
+}
+
+static void trng_write32(vaddr_t addr, size_t off, uint32_t val)
+{
+	io_write32(addr + off, val);
+}
+
+static void trng_clrset32(vaddr_t addr, size_t off, uint32_t mask,
+			  uint32_t val)
+{
+	io_clrsetbits32(addr + off, mask, mask & val);
+}
+
+static void trng_write32_range(const struct versal_trng *trng, uint32_t start,
+			       uint32_t num_regs, const uint8_t *buf)
+{
+	size_t off = 0;
+	uint32_t val = 0;
+	size_t cnt = 0;
+	size_t i = 0;
+
+	for (i = 0; i < num_regs; ++i) {
+		if (!buf) {
+			off = start + i * TRNG_BYTES_PER_REG;
+			trng_write32(trng->cfg.addr, off, 0);
+			continue;
+		}
+
+		val = 0;
+		for (cnt = 0; cnt < TRNG_BYTES_PER_REG; ++cnt)
+			val = (val << 8) | buf[i * TRNG_BYTES_PER_REG + cnt];
+
+		off = start + (TRNG_NUM_INIT_REGS - 1 - i) * TRNG_BYTES_PER_REG;
+		trng_write32(trng->cfg.addr, off, val);
+	}
+}
+
+static TEE_Result trng_wait_for_event(vaddr_t addr, size_t off, uint32_t mask,
+				      uint32_t event, uint32_t time_out)
+{
+	uint64_t tref = timeout_init_us(time_out);
+
+	do {
+		if (timeout_elapsed(tref))
+			break;
+	} while ((io_read32(addr + off) & mask) != event);
+
+	/* Normal world might have suspended the OP-TEE thread, check again  */
+	if ((io_read32(addr + off) & mask) != event)
+		panic();
+
+	return TEE_SUCCESS;
+}
+
+static void trng_soft_reset(const struct versal_trng *trng)
+{
+	trng_clrset32(trng->cfg.addr, TRNG_CTRL, TRNG_CTRL_PRNGSRST_MASK,
+		      TRNG_CTRL_PRNGSRST_MASK);
+	udelay(RESET_DELAY);
+	trng_clrset32(trng->cfg.addr, TRNG_CTRL, TRNG_CTRL_PRNGSRST_MASK, 0);
+}
+
+static void trng_reset(const struct versal_trng *trng)
+{
+	trng_write32(trng->cfg.addr, TRNG_RESET, TRNG_RESET_VAL_MASK);
+	udelay(RESET_DELAY);
+	trng_write32(trng->cfg.addr, TRNG_RESET, 0);
+	trng_soft_reset(trng);
+}
+
+static void trng_hold_reset(const struct versal_trng *trng)
+{
+	trng_clrset32(trng->cfg.addr, TRNG_CTRL,
+		      TRNG_CTRL_PRNGSRST_MASK, TRNG_CTRL_PRNGSRST_MASK);
+	trng_write32(trng->cfg.addr, TRNG_RESET, TRNG_RESET_VAL_MASK);
+	udelay(RESET_DELAY);
+}
+
+static TEE_Result trng_check_seed(uint8_t *entropy, uint32_t len)
+{
+	uint32_t *p = (void *)entropy;
+	size_t i = 0;
+
+	for (i = 0; i < len / sizeof(*p); i++) {
+		if (p[i] == ALL_A_PATTERN_32)
+			return TEE_ERROR_GENERIC;
+
+		if (p[i] == ALL_5_PATTERN_32)
+			return TEE_ERROR_GENERIC;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result trng_collect_random(struct versal_trng *trng, uint8_t *dst,
+				      size_t len)
+{
+	const size_t bursts = len / TRNG_BURST_SIZE;
+	const size_t words = TRNG_BURST_SIZE_BITS / TRNG_REG_SIZE;
+	uint32_t *p = (void *)dst;
+	size_t bcnt = 0;
+	size_t wcnt = 0;
+	size_t index = 0;
+	uint32_t val = 0;
+	bool match = false;
+
+	trng_clrset32(trng->cfg.addr, TRNG_CTRL,
+		      TRNG_CTRL_PRNGSTART_MASK, TRNG_CTRL_PRNGSTART_MASK);
+
+	/*
+	 * Loop as many times based on len requested. In each burst 128 bits
+	 * are generated, which is reflected in QCNT value of 4 by hardware.
+	 */
+	for (bcnt = 0; bcnt < bursts; bcnt++) {
+		if (trng_wait_for_event(trng->cfg.addr,
+					TRNG_STATUS, TRNG_STATUS_QCNT_MASK,
+					TRNG_MAX_QCNT << TRNG_STATUS_QCNT_SHIFT,
+					TRNG_GENERATE_TIMEOUT)) {
+			EMSG("Timeout waiting for randomness");
+			return TEE_ERROR_GENERIC;
+		}
+
+		/*
+		 * DTF flag set during generate indicates catastrophic
+		 * condition, which needs to be checked for every time unless we
+		 * are in PTRNG mode
+		 */
+		if (trng->usr_cfg.mode == TRNG_PTRNG)
+			goto skip_dtf;
+
+		val = trng_read32(trng->cfg.addr, TRNG_STATUS);
+		if ((val & TRNG_STATUS_DTF_MASK) == TRNG_STATUS_DTF_MASK) {
+			EMSG("Catastrophic DFT error");
+			trng->status = TRNG_CATASTROPHIC;
+
+			return TEE_ERROR_GENERIC;
+		}
+skip_dtf:
+		/*
+		 * Read the core output register 4 times to consume the random
+		 * data generated for every burst.
+		 */
+		match = true;
+		for (wcnt = 0; wcnt < words; wcnt++) {
+			val = trng_read32(trng->cfg.addr, TRNG_CORE_OUTPUT);
+
+			if (bcnt > 0 && trng->buf[wcnt] != val)
+				match = false;
+
+			trng->buf[wcnt] = val;
+
+			if (dst)
+				p[index] = TEE_U32_TO_BIG_ENDIAN(val);
+
+			index++;
+		}
+
+		if (bursts > 1 && bcnt > 0 && match) {
+			EMSG("Catastrophic software error");
+			trng->status = TRNG_CATASTROPHIC;
+			return TEE_ERROR_GENERIC;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result trng_reseed_internal_nodf(struct versal_trng *trng,
+					    uint8_t *eseed, uint8_t *str)
+{
+	uint8_t entropy[TRNG_SEED_LEN] = { 0 };
+	uint8_t *seed = NULL;
+
+	switch (trng->usr_cfg.mode) {
+	case TRNG_HRNG:
+		trng_write32(trng->cfg.addr, TRNG_OSC_EN, TRNG_OSC_EN_VAL_MASK);
+		trng_soft_reset(trng);
+		trng_write32(trng->cfg.addr, TRNG_CTRL,
+			     TRNG_CTRL_EUMODE_MASK | TRNG_CTRL_TRSSEN_MASK);
+
+		if (trng_collect_random(trng, entropy, TRNG_SEED_LEN))
+			return TEE_ERROR_GENERIC;
+
+		if (trng_check_seed(entropy, TRNG_SEED_LEN))
+			return TEE_ERROR_GENERIC;
+
+		seed = entropy;
+		break;
+	case TRNG_DRNG:
+		seed = eseed;
+		break;
+	default:
+		seed = NULL;
+		break;
+	}
+
+	trng_write32_range(trng, TRNG_EXT_SEED_0, TRNG_SEED_REGS, seed);
+	if (str)
+		trng_write32_range(trng, TRNG_PER_STRING_0, TRNG_PERS_STR_REGS,
+				   str);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result trng_reseed_internal_df(struct versal_trng *trng,
+					  uint8_t *eseed, uint8_t *str)
+{
+	memset(&trng->dfin, 0, sizeof(trng->dfin));
+
+	switch (trng->usr_cfg.mode) {
+	case TRNG_HRNG:
+		trng_write32(trng->cfg.addr, TRNG_OSC_EN, TRNG_OSC_EN_VAL_MASK);
+		trng_soft_reset(trng);
+		trng_write32(trng->cfg.addr, TRNG_CTRL,
+			     TRNG_CTRL_EUMODE_MASK | TRNG_CTRL_TRSSEN_MASK);
+
+		if (trng_collect_random(trng, trng->dfin.entropy, trng->len))
+			return TEE_ERROR_GENERIC;
+
+		if (trng_check_seed(trng->dfin.entropy, trng->len))
+			return TEE_ERROR_GENERIC;
+		break;
+	case TRNG_DRNG:
+		memcpy(trng->dfin.entropy, eseed, trng->len);
+		break;
+	default:
+		break;
+	}
+
+	trng_df_algorithm(trng, trng->dfout, DF_SEED, str);
+	trng_write32_range(trng, TRNG_EXT_SEED_0, TRNG_SEED_REGS, trng->dfout);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result trng_reseed_internal(struct versal_trng *trng,
+				       uint8_t *eseed, uint8_t *str,
+				       uint32_t mul)
+{
+	uint32_t val = 0;
+
+	trng->stats.bytes_reseed = 0;
+	trng->stats.elapsed_seed_life = 0;
+
+	if (trng->usr_cfg.df_disable)
+		trng->len = TRNG_SEED_LEN;
+	else
+		trng->len = (mul + 1) * BYTES_PER_BLOCK;
+
+	if (trng->usr_cfg.df_disable) {
+		if (trng_reseed_internal_nodf(trng, eseed, str))
+			goto error;
+	} else {
+		if (trng_reseed_internal_df(trng, eseed, str))
+			goto error;
+	}
+
+	trng_write32(trng->cfg.addr, TRNG_CTRL,
+		     PRNGMODE_RESEED | TRNG_CTRL_PRNGXS_MASK);
+
+	/* Start the reseed operation */
+	trng_clrset32(trng->cfg.addr, TRNG_CTRL, TRNG_CTRL_PRNGSTART_MASK,
+		      TRNG_CTRL_PRNGSTART_MASK);
+
+	if (trng_wait_for_event(trng->cfg.addr, TRNG_STATUS,
+				TRNG_STATUS_DONE_MASK, TRNG_STATUS_DONE_MASK,
+				TRNG_RESEED_TIMEOUT))
+		goto error;
+
+	/* Check SP800 - 90B (entropy health test error) */
+	val = trng_read32(trng->cfg.addr, TRNG_STATUS) & TRNG_STATUS_CERTF_MASK;
+	if (val == TRNG_STATUS_CERTF_MASK)
+		goto error;
+
+	trng_clrset32(trng->cfg.addr, TRNG_CTRL, TRNG_CTRL_PRNGSTART_MASK, 0);
+	return TEE_SUCCESS;
+error:
+	trng->status = TRNG_ERROR;
+	return TEE_ERROR_GENERIC;
+}
+
+static TEE_Result trng_instantiate(struct versal_trng *trng,
+				   const struct trng_usr_cfg *usr_cfg)
+{
+	uint8_t *seed = NULL;
+	uint8_t *pers = NULL;
+
+	if (!trng)
+		return TEE_ERROR_GENERIC;
+
+	if (!usr_cfg)
+		goto error;
+
+	if (trng->status != TRNG_UNINITIALIZED)
+		goto error;
+
+	if (usr_cfg->mode != TRNG_HRNG && usr_cfg->mode != TRNG_DRNG &&
+	    usr_cfg->mode != TRNG_PTRNG)
+		goto error;
+
+	if (usr_cfg->mode != TRNG_PTRNG && !usr_cfg->seed_life)
+		goto error;
+
+	if (!usr_cfg->iseed_en && usr_cfg->mode == TRNG_DRNG)
+		goto error;
+
+	if (usr_cfg->iseed_en && usr_cfg->mode == TRNG_HRNG)
+		goto error;
+
+	if (!usr_cfg->df_disable &&
+	    (usr_cfg->dfmul < TRNG_MIN_DFLENMULT ||
+	     usr_cfg->dfmul > TRNG_MAX_DFLENMULT))
+		goto error;
+
+	if (usr_cfg->df_disable && usr_cfg->dfmul)
+		goto error;
+
+	if (usr_cfg->mode == TRNG_PTRNG &&
+	    (usr_cfg->iseed_en || usr_cfg->pstr_en ||
+	     usr_cfg->predict_en || usr_cfg->seed_life))
+		goto error;
+
+	memcpy(&trng->usr_cfg, usr_cfg, sizeof(struct trng_usr_cfg));
+	trng_reset(trng);
+
+	if (trng->usr_cfg.iseed_en)
+		seed = (void *)trng->usr_cfg.init_seed;
+
+	if (trng->usr_cfg.pstr_en)
+		pers = (void *)trng->usr_cfg.pstr;
+
+	if (trng->usr_cfg.mode != TRNG_PTRNG) {
+		if (trng_reseed_internal(trng, seed, pers, trng->usr_cfg.dfmul))
+			goto error;
+	}
+
+	trng->status = TRNG_HEALTHY;
+	return TEE_SUCCESS;
+error:
+	trng->status = TRNG_ERROR;
+	return TEE_ERROR_GENERIC;
+}
+
+static TEE_Result trng_reseed(struct versal_trng *trng, uint8_t *eseed,
+			      uint32_t mul)
+{
+	if (!trng)
+		return TEE_ERROR_GENERIC;
+
+	if (trng->status != TRNG_HEALTHY)
+		goto error;
+
+	if (trng->usr_cfg.mode != TRNG_DRNG && trng->usr_cfg.mode != TRNG_HRNG)
+		goto error;
+
+	if (trng->usr_cfg.mode == TRNG_DRNG && !eseed)
+		goto error;
+
+	if (trng->usr_cfg.mode != TRNG_DRNG && eseed)
+		goto error;
+
+	if (!trng->usr_cfg.df_disable) {
+		if (mul < TRNG_MIN_DFLENMULT || mul > TRNG_MAX_DFLENMULT)
+			goto error;
+	}
+
+	if (trng->usr_cfg.df_disable && mul)
+		goto error;
+
+	if (eseed && !memcmp(eseed, trng->usr_cfg.init_seed, trng->len))
+		goto error;
+
+	if (trng_reseed_internal(trng, eseed, NULL, mul))
+		goto error;
+
+	return TEE_SUCCESS;
+error:
+	trng->status = TRNG_ERROR;
+	return TEE_ERROR_GENERIC;
+}
+
+static TEE_Result trng_generate(struct versal_trng *trng, uint8_t *buf,
+				size_t blen, bool predict)
+{
+	uint32_t len = TRNG_SEC_STRENGTH_LEN;
+	uint8_t *p = buf;
+
+	if (!trng)
+		return TEE_ERROR_GENERIC;
+
+	if (!p)
+		goto error;
+
+	if (blen < TRNG_SEC_STRENGTH_LEN)
+		goto error;
+
+	if (trng->status != TRNG_HEALTHY)
+		goto error;
+
+	if (trng->usr_cfg.mode == TRNG_PTRNG && predict)
+		goto error;
+
+	if (!trng->usr_cfg.predict_en && predict)
+		goto error;
+
+	switch (trng->usr_cfg.mode) {
+	case TRNG_HRNG:
+		if (trng->stats.elapsed_seed_life >= trng->usr_cfg.seed_life) {
+			if (trng_reseed_internal(trng, NULL, NULL, 0))
+				goto error;
+		}
+
+		if (predict && trng->stats.elapsed_seed_life > 0) {
+			if (trng_reseed_internal(trng, NULL, NULL, 0))
+				goto error;
+		}
+
+		trng_write32(trng->cfg.addr, TRNG_CTRL, PRNGMODE_GEN);
+		break;
+	case TRNG_DRNG:
+		if (trng->stats.elapsed_seed_life > trng->usr_cfg.seed_life)
+			goto error;
+
+		if (predict && trng->stats.elapsed_seed_life > 0)
+			goto error;
+
+		trng_write32(trng->cfg.addr, TRNG_CTRL, PRNGMODE_GEN);
+		break;
+	default:
+		if (!trng->usr_cfg.df_disable) {
+			memset(&trng->dfin, 0, sizeof(trng->dfin));
+			len = (trng->usr_cfg.dfmul + 1) * BYTES_PER_BLOCK;
+			trng->len = len;
+			p = trng->dfin.entropy;
+		}
+		/* Enable the 8 ring oscillators used for entropy source */
+		trng_write32(trng->cfg.addr, TRNG_OSC_EN, TRNG_OSC_EN_VAL_MASK);
+		trng_soft_reset(trng);
+		trng_write32(trng->cfg.addr, TRNG_CTRL,
+			     TRNG_CTRL_EUMODE_MASK | TRNG_CTRL_TRSSEN_MASK);
+		break;
+	}
+
+	if (trng_collect_random(trng, p, len))
+		goto error;
+
+	trng->stats.bytes_reseed += len;
+	trng->stats.bytes += len;
+	trng->stats.elapsed_seed_life++;
+
+	if (!trng->usr_cfg.df_disable) {
+		if (trng->usr_cfg.mode == TRNG_PTRNG)
+			trng_df_algorithm(trng, buf, DF_RAND, NULL);
+	}
+
+	return TEE_SUCCESS;
+error:
+	if (trng->status != TRNG_CATASTROPHIC)
+		trng->status = TRNG_ERROR;
+
+	return TEE_ERROR_GENERIC;
+}
+
+static TEE_Result trng_release(struct versal_trng *trng)
+{
+	if (!trng)
+		return TEE_ERROR_GENERIC;
+
+	if (trng->status == TRNG_UNINITIALIZED)
+		goto error;
+
+	trng_write32_range(trng, TRNG_EXT_SEED_0, TRNG_SEED_REGS, NULL);
+	trng_write32_range(trng, TRNG_PER_STRING_0, TRNG_PERS_STR_REGS, NULL);
+	trng_hold_reset(trng);
+
+	/* Clear the instance */
+	memset(&trng->usr_cfg, 0, sizeof(trng->usr_cfg));
+	memset(trng->buf, 0, sizeof(trng->buf));
+	memset(trng->dfout, 0, sizeof(trng->dfout));
+	trng->status = TRNG_UNINITIALIZED;
+
+	return TEE_SUCCESS;
+error:
+	trng->status = TRNG_ERROR;
+
+	return TEE_ERROR_GENERIC;
+}
+
+/* Health tests should be run when the configured mode is of PTRNG or HRNG */
+static TEE_Result trng_health_test(struct versal_trng *trng)
+{
+	struct trng_usr_cfg tests = {
+		.mode = TRNG_HRNG,
+		.seed_life = 10,
+		.dfmul = 7,
+		.predict_en = false,
+		.iseed_en = false,
+		.pstr_en = false,
+		.df_disable = false,
+	};
+
+	if (trng_instantiate(trng, &tests))
+		goto error;
+
+	if (trng_release(trng))
+		goto error;
+
+	return TEE_SUCCESS;
+error:
+	trng->status = TRNG_ERROR;
+
+	return TEE_ERROR_GENERIC;
+}
+
+/*
+ * The KAT test should be run when the TRNG is configured in DRNG or HRNG mode.
+ * If KAT fails, the driver has to be put in error state.
+ */
+static TEE_Result trng_kat_test(struct versal_trng *trng)
+{
+	struct trng_usr_cfg tests = {
+		.mode = TRNG_DRNG,
+		.seed_life = 5,
+		.dfmul = 2,
+		.predict_en = false,
+		.iseed_en = true,
+		.pstr_en = true,
+		.df_disable = false,
+	};
+	const uint8_t ext_seed[TRNG_SEED_LEN] = {
+		0x3BU, 0xC3U, 0xEDU, 0x64U, 0xF4U, 0x80U, 0x1CU, 0xC7U,
+		0x14U, 0xCCU, 0x35U, 0xEDU, 0x57U, 0x01U, 0x2AU, 0xE4U,
+		0xBCU, 0xEFU, 0xDEU, 0xF6U, 0x7CU, 0x46U, 0xA6U, 0x34U,
+		0xC6U, 0x79U, 0xE8U, 0x91U, 0x5DU, 0xB1U, 0xDBU, 0xA7U,
+		0x49U, 0xA5U, 0xBBU, 0x4FU, 0xEDU, 0x30U, 0xB3U, 0x7BU,
+		0xA9U, 0x8BU, 0xF5U, 0x56U, 0x4DU, 0x40U, 0x18U, 0x9FU,
+	};
+	const uint8_t pers_str[TRNG_PERS_STR_LEN] = {
+		0xB2U, 0x80U, 0x7EU, 0x4CU, 0xD0U, 0xE4U, 0xE2U, 0xA9U,
+		0x2FU, 0x1FU, 0x5DU, 0xC1U, 0xA2U, 0x1FU, 0x40U, 0xFCU,
+		0x1FU, 0x24U, 0x5DU, 0x42U, 0x61U, 0x80U, 0xE6U, 0xE9U,
+		0x71U, 0x05U, 0x17U, 0x5BU, 0xAFU, 0x70U, 0x30U, 0x18U,
+		0xBCU, 0x23U, 0x18U, 0x15U, 0xCBU, 0xB8U, 0xA6U, 0x3EU,
+		0x83U, 0xB8U, 0x4AU, 0xFEU, 0x38U, 0xFCU, 0x25U, 0x87U,
+	};
+	const uint8_t expected_out[TRNG_GEN_LEN] = {
+		0x91U, 0x9AU, 0x6BU, 0x99U, 0xD5U, 0xBCU, 0x2CU, 0x11U,
+		0x5FU, 0x3AU, 0xFCU, 0x0BU, 0x0EU, 0x7BU, 0xC7U, 0x69U,
+		0x4DU, 0xE1U, 0xE5U, 0xFEU, 0x59U, 0x9EU, 0xAAU, 0x41U,
+		0xD3U, 0x48U, 0xFDU, 0x3DU, 0xD2U, 0xC4U, 0x50U, 0x1EU,
+	};
+	uint8_t out[TRNG_GEN_LEN] = { 0 };
+
+	if (!trng)
+		return TEE_ERROR_GENERIC;
+
+	memcpy(&tests.init_seed, ext_seed, sizeof(ext_seed));
+	memcpy(tests.pstr, pers_str, sizeof(pers_str));
+
+	if (trng_instantiate(trng, &tests))
+		goto error;
+
+	if (trng_generate(trng, out, sizeof(out), false))
+		goto error;
+
+	if (memcmp(out, expected_out, TRNG_GEN_LEN)) {
+		EMSG("K.A.T mismatch");
+		goto error;
+	}
+
+	if (trng_release(trng))
+		goto error;
+
+	return TEE_SUCCESS;
+error:
+	trng->status = TRNG_ERROR;
+	return TEE_ERROR_GENERIC;
+}
+
+struct versal_trng trng = {
+	.cfg.base = TRNG_BASE,
+	.cfg.len = TRNG_SIZE,
+};
+
+uint8_t hw_get_random_byte(void)
+{
+	uint8_t random[TRNG_SEC_STRENGTH_LEN] = { 0 };
+
+	if (trng_generate(&trng, random, TRNG_SEC_STRENGTH_LEN, false))
+		panic();
+
+	return random[TRNG_SEC_STRENGTH_LEN / 2];
+}
+
+TEE_Result crypto_rng_read(void *buf, size_t len)
+{
+	uint8_t random[TRNG_SEC_STRENGTH_LEN] = { 0 };
+	uint8_t *p = buf;
+	size_t i = 0;
+
+	for (i = 0; i < len / TRNG_SEC_STRENGTH_LEN; i++) {
+		if (trng_generate(&trng, p + i * TRNG_SEC_STRENGTH_LEN,
+				  TRNG_SEC_STRENGTH_LEN, false))
+			panic();
+	}
+
+	if (len % TRNG_SEC_STRENGTH_LEN) {
+		if (trng_generate(&trng, random, TRNG_SEC_STRENGTH_LEN, false))
+			panic();
+		memcpy(p + i * TRNG_SEC_STRENGTH_LEN, random,
+		       len % TRNG_SEC_STRENGTH_LEN);
+	}
+
+	return TEE_SUCCESS;
+}
+
+void plat_rng_init(void)
+{
+}
+
+static TEE_Result trng_hrng_mode_init(void)
+{
+	uint8_t pers_str[TRNG_PERS_STR_LEN] = {
+		0xB2, 0x80, 0x7E, 0x4C, 0xD0, 0xE4, 0xE2, 0xA9,
+		0x2F, 0x1F, 0x5D, 0xC1, 0xA2, 0x1F, 0x40, 0xFC,
+		0x1F, 0x24, 0x5D, 0x42, 0x61, 0x80, 0xE6, 0xE9,
+		0x71, 0x05, 0x17, 0x5B, 0xAF, 0x70, 0x30, 0x18,
+		0xBC, 0x23, 0x18, 0x15, 0xCB, 0xB8, 0xA6, 0x3E,
+		0x83, 0xB8, 0x4A, 0xFE, 0x38, 0xFC, 0x25, 0x87,
+	};
+	/* configure in hybrid mode with derivative function enabled */
+	struct trng_usr_cfg usr_cfg = {
+		.mode = TRNG_HRNG,
+		.seed_life = CFG_VERSAL_TRNG_SEED_LIFE,
+		.predict_en = false,
+		.df_disable = false,
+		.dfmul = CFG_VERSAL_TRNG_DF_MUL,
+		.iseed_en =  false,
+		.pstr_en = true,
+	};
+
+	memcpy(usr_cfg.pstr, pers_str, TRNG_PERS_STR_LEN);
+	trng.cfg.addr = (vaddr_t)core_mmu_add_mapping(MEM_AREA_IO_SEC,
+						      trng.cfg.base,
+						      trng.cfg.len);
+	if (!trng.cfg.addr) {
+		EMSG("Failed to map TRNG");
+		goto error;
+	}
+
+	if (trng_kat_test(&trng)) {
+		EMSG("KAT Failed");
+		goto error;
+	}
+
+	if (trng_health_test(&trng)) {
+		EMSG("RunHealthTest Failed");
+		goto error;
+	}
+
+	if (trng_instantiate(&trng, &usr_cfg)) {
+		EMSG("Driver instantiation Failed");
+		goto error;
+	}
+
+	if (trng_reseed(&trng, NULL, usr_cfg.dfmul)) {
+		EMSG("Reseed Failed");
+		goto error;
+	}
+
+	return TEE_SUCCESS;
+error:
+	panic();
+	return TEE_ERROR_GENERIC;
+}
+
+driver_init(trng_hrng_mode_init);
diff --git a/core/include/drivers/versal_mbox.h b/core/include/drivers/versal_mbox.h
new file mode 100644
index 00000000000..033ebf2fd50
--- /dev/null
+++ b/core/include/drivers/versal_mbox.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2022, Foundries.io Ltd
+ */
+#ifndef __DRIVERS_VERSAL_MBOX_H
+#define __DRIVERS_VERSAL_MBOX_H
+
+#include <platform_config.h>
+#include <tee_api_types.h>
+#include <util.h>
+
+#define SRC 0
+#define DST 1
+
+#define MAX_IPI_BUF 6
+
+struct ipi_buf {
+	size_t len;
+	void *p;
+	bool only_cache;
+};
+
+struct ipi_cmd {
+	uint32_t data[8];
+	struct ipi_buf ibuf[MAX_IPI_BUF];
+};
+
+TEE_Result versal_mbox_notify(struct ipi_cmd *cmd, struct ipi_cmd *rsp);
+
+#endif /* __DRIVERS_VERSAL_MBOX_H */
diff --git a/core/include/drivers/versal_nvm.h b/core/include/drivers/versal_nvm.h
new file mode 100644
index 00000000000..6a6d123427a
--- /dev/null
+++ b/core/include/drivers/versal_nvm.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Foundries.io Ltd
+ */
+
+#ifndef __DRIVERS_VERSAL_NVM_H__
+#define __DRIVERS_VERSAL_NVM_H__
+
+#include <drivers/versal_mbox.h>
+#include <platform_config.h>
+#include <tee_api_types.h>
+#include <types_ext.h>
+#include <util.h>
+
+#define EFUSE_MAX_USER_FUSES 64
+#define EFUSE_IV_LEN 12
+#define EFUSE_DNA_LEN 16
+#define EFUSE_PPK_LEN 32
+
+enum versal_nvm_iv_type {
+	EFUSE_META_HEADER_IV_RANGE = 0,
+	EFUSE_BLACK_IV,
+	EFUSE_PLM_IV_RANGE,
+	EFUSE_DATA_PARTITION_IV_RANGE
+};
+
+enum versal_nvm_ppk_type {
+	EFUSE_PPK0 = 0,
+	EFUSE_PPK1,
+	EFUSE_PPK2
+};
+
+enum versal_nvm_revocation_id {
+	EFUSE_REVOCATION_ID_0 = 0,
+	EFUSE_REVOCATION_ID_1,
+	EFUSE_REVOCATION_ID_2,
+	EFUSE_REVOCATION_ID_3,
+	EFUSE_REVOCATION_ID_4,
+	EFUSE_REVOCATION_ID_5,
+	EFUSE_REVOCATION_ID_6,
+	EFUSE_REVOCATION_ID_7
+};
+
+enum versal_nvm_offchip_id {
+	EFUSE_INVLD = -1,
+	EFUSE_OFFCHIP_REVOKE_ID_0 = 0,
+	EFUSE_OFFCHIP_REVOKE_ID_1,
+	EFUSE_OFFCHIP_REVOKE_ID_2,
+	EFUSE_OFFCHIP_REVOKE_ID_3,
+	EFUSE_OFFCHIP_REVOKE_ID_4,
+	EFUSE_OFFCHIP_REVOKE_ID_5,
+	EFUSE_OFFCHIP_REVOKE_ID_6,
+	EFUSE_OFFCHIP_REVOKE_ID_7
+};
+
+#define __aligned_efuse			__aligned(CACHELINE_LEN)
+
+TEE_Result versal_read_efuse_dna(uint32_t *buf, size_t len);
+TEE_Result versal_read_efuse_user(uint32_t *buf, size_t len, uint32_t first,
+				  size_t num);
+TEE_Result versal_read_efuse_iv(uint32_t *buf, size_t len,
+				enum versal_nvm_iv_type type);
+TEE_Result versal_read_efuse_ppk(uint32_t *buf, size_t len,
+				 enum versal_nvm_ppk_type type);
+TEE_Result versal_write_efuse_user(uint32_t *buf, size_t len,
+				   uint32_t first, size_t num);
+
+#endif /*__DRIVERS_VERSAL_NVM_H__*/
diff --git a/core/include/drivers/versal_pm.h b/core/include/drivers/versal_pm.h
new file mode 100644
index 00000000000..18b5bd11206
--- /dev/null
+++ b/core/include/drivers/versal_pm.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022 Foundries.io Ltd
+ */
+
+#ifndef __DRIVERS_VERSAL_PM_H__
+#define __DRIVERS_VERSAL_PM_H__
+
+#include <platform_config.h>
+#include <tee_api_types.h>
+#include <util.h>
+
+TEE_Result versal_soc_version(uint8_t *version);
+
+#endif /*__DRIVERS_VERSAL_PM_H__*/