drivers: implement se050 driver

Add AES_CTR/RSA/RNG/HUK support for NXP SE050 via the Plug And Trust
library.

Tested on imx8mm LPDDR EVK and imx6ull EVK.

Signed-off-by: Jorge Ramirez-Ortiz <jorge@foundries.io>
Acked-by: Etienne Carriere <etienne.carriere@linaro.org>

25 files changed, 3106 insertions, 0 deletions

diff --git a/core/drivers/crypto/se050/adaptors/apis/apdu.c b/core/drivers/crypto/se050/adaptors/apis/apdu.c
new file mode 100644
index 00000000..21ac2859
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/apis/apdu.c
@@ -0,0 +1,526 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <adaptors.h>
+#include <fsl_sss_api.h>
+#include <scp.h>
+#include <se050_apdu_apis.h>
+#include <string.h>
+
+sss_status_t se050_factory_reset(pSe05xSession_t ctx)
+{
+	if (!ctx)
+		return kStatus_SSS_Fail;
+
+	if (Se05x_API_DeleteAll_Iterative(ctx) == SM_OK)
+		return kStatus_SSS_Success;
+
+	return kStatus_SSS_Fail;
+}
+
+bool se050_key_exists(uint32_t key_id, pSe05xSession_t ctx)
+{
+	SE05x_Result_t inuse = kSE05x_Result_FAILURE;
+	smStatus_t status = SM_OK;
+
+	if (!ctx)
+		return false;
+
+	status = Se05x_API_CheckObjectExists(ctx, key_id, &inuse);
+	if (status != SM_OK)
+		return false;
+
+	if (inuse == kSE05x_Result_SUCCESS)
+		return true;
+
+	return false;
+}
+
+static sss_status_t set_rsa_public(Se05xSession_t *s_ctx,
+				   Se05xPolicy_t *policy,
+				   sss_se05x_object_t *k_object,
+				   struct se050_rsa_keypub *keypub,
+				   size_t key_bit_len)
+{
+	SE05x_TransientType_t type = kSE05x_TransientType_Transient;
+	SE05x_RSAKeyFormat_t rsa_format = kSE05x_RSAKeyFormat_RAW;
+	smStatus_t status = SM_OK;
+
+	if (k_object->isPersistant)
+		type = kSE05x_TransientType_Persistent;
+
+	switch (k_object->cipherType) {
+	case kSSS_CipherType_RSA:
+		rsa_format = kSE05x_RSAKeyFormat_RAW;
+		break;
+	case kSSS_CipherType_RSA_CRT:
+		rsa_format = kSE05x_RSAKeyFormat_CRT;
+		break;
+	default:
+		return kStatus_SSS_Fail;
+	}
+
+	status = Se05x_API_WriteRSAKey(s_ctx, policy, k_object->keyId,
+				       (U16)key_bit_len,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       keypub->e, keypub->e_len,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_Public,
+				       rsa_format);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       keypub->n, keypub->n_len,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       rsa_format);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t set_rsa_private_rsa(Se05xSession_t *s_ctx,
+					Se05xPolicy_t *policy,
+					sss_se05x_object_t *k_object,
+					struct se050_rsa_keypair *keypair,
+					size_t key_bit_len)
+{
+	SE05x_TransientType_t type = kSE05x_TransientType_Transient;
+	smStatus_t status = SM_OK;
+
+	if (k_object->isPersistant)
+		type = kSE05x_TransientType_Persistent;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, policy, k_object->keyId,
+				       (U16)key_bit_len,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       keypair->d, keypair->d_len,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_Pair,
+				       kSE05x_RSAKeyFormat_RAW);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       keypair->n, keypair->n_len,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_RAW);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t set_rsa_private_rsa_crt(Se05xSession_t *s_ctx,
+					    Se05xPolicy_t *policy,
+					    sss_se05x_object_t *k_object,
+					    struct se050_rsa_keypair *keypair,
+					    size_t key_bit_len)
+{
+	SE05x_TransientType_t type = kSE05x_TransientType_Transient;
+	smStatus_t status = SM_OK;
+
+	if (k_object->isPersistant)
+		type = kSE05x_TransientType_Persistent;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, policy, k_object->keyId,
+				       (U16)key_bit_len,
+				       keypair->p,
+				       keypair->p_len,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_Private,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       keypair->q,
+				       keypair->q_len,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       keypair->dp,
+				       keypair->dp_len,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       keypair->dq,
+				       keypair->dq_len,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       keypair->qp,
+				       keypair->qp_len,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t set_rsa_keypair_rsa(Se05xSession_t *s_ctx,
+					Se05xPolicy_t *policy,
+					sss_se05x_object_t *k_object,
+					struct se050_rsa_keypair *keypair,
+					size_t key_bit_len)
+{
+	SE05x_TransientType_t type = kSE05x_TransientType_Transient;
+	smStatus_t status = SM_OK;
+
+	if (k_object->isPersistant)
+		type = kSE05x_TransientType_Persistent;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, policy, k_object->keyId,
+				       (U16)key_bit_len,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       keypair->e, keypair->e_len,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_Pair,
+				       kSE05x_RSAKeyFormat_RAW);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       keypair->d, keypair->d_len,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_RAW);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       keypair->n, keypair->n_len,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_RAW);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t set_rsa_keypair_rsa_crt(Se05xSession_t *s_ctx,
+					    Se05xPolicy_t *policy,
+					    sss_se05x_object_t *k_object,
+					    struct se050_rsa_keypair *keypair,
+					    size_t key_bit_len)
+{
+	SE05x_TransientType_t type = kSE05x_TransientType_Transient;
+	smStatus_t status = SM_OK;
+
+	if (k_object->isPersistant)
+		type = kSE05x_TransientType_Persistent;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, policy, k_object->keyId,
+				       (U16)key_bit_len,
+				       keypair->p, keypair->p_len,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_Pair,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       keypair->q, keypair->q_len,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       keypair->dp, keypair->dp_len,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       keypair->dq, keypair->dq_len,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       keypair->qp, keypair->qp_len,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       keypair->e, keypair->e_len,
+				       SE05X_RSA_NO_priv,
+				       SE05X_RSA_NO_pubMod,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	status = Se05x_API_WriteRSAKey(s_ctx, NULL, k_object->keyId,
+				       0,
+				       SE05X_RSA_NO_p,
+				       SE05X_RSA_NO_q,
+				       SE05X_RSA_NO_dp,
+				       SE05X_RSA_NO_dq,
+				       SE05X_RSA_NO_qInv,
+				       SE05X_RSA_NO_pubExp,
+				       SE05X_RSA_NO_priv,
+				       keypair->n, keypair->n_len,
+				       (SE05x_INS_t)type,
+				       kSE05x_KeyPart_NA,
+				       kSE05x_RSAKeyFormat_CRT);
+	if (status != SM_OK)
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
+
+sss_status_t se050_key_store_set_rsa_key_bin(sss_se05x_key_store_t *store,
+					     sss_se05x_object_t *k_object,
+					     struct se050_rsa_keypair *keypair,
+					     struct se050_rsa_keypub *keypub,
+					     size_t key_bit_len)
+{
+	Se05xPolicy_t policy = { };
+
+	if (!store || !store->session || !k_object)
+		return kStatus_SSS_Fail;
+
+	if (se050_key_exists(k_object->keyId, &store->session->s_ctx))
+		key_bit_len = 0;
+
+	switch (k_object->objectType) {
+	case kSSS_KeyPart_Public:
+		return set_rsa_public(&store->session->s_ctx,
+				      &policy, k_object,
+				      keypub, key_bit_len);
+	case kSSS_KeyPart_Private:
+		if (k_object->cipherType == kSSS_CipherType_RSA)
+			return set_rsa_private_rsa(&store->session->s_ctx,
+						   &policy, k_object,
+						   keypair, key_bit_len);
+
+		if (k_object->cipherType == kSSS_CipherType_RSA_CRT)
+			return set_rsa_private_rsa_crt(&store->session->s_ctx,
+						       &policy, k_object,
+						       keypair, key_bit_len);
+		return kStatus_SSS_Fail;
+	case kSSS_KeyPart_Pair:
+		if (k_object->cipherType == kSSS_CipherType_RSA)
+			return set_rsa_keypair_rsa(&store->session->s_ctx,
+						   &policy, k_object,
+						   keypair, key_bit_len);
+
+		if (k_object->cipherType == kSSS_CipherType_RSA_CRT)
+			return set_rsa_keypair_rsa_crt(&store->session->s_ctx,
+						       &policy, k_object,
+						       keypair, key_bit_len);
+		return kStatus_SSS_Fail;
+	default:
+		return kStatus_SSS_Fail;
+	}
+}
+
+sss_status_t  se050_get_free_memory(pSe05xSession_t ctx, uint16_t *p,
+				    SE05x_MemoryType_t type)
+{
+	if (p && ctx && Se05x_API_GetFreeMemory(ctx, type, p) == SM_OK)
+		return kStatus_SSS_Success;
+
+	return kStatus_SSS_Fail;
+}
+
+sss_status_t se050_scp03_send_rotate_cmd(pSe05xSession_t ctx,
+					 struct s050_scp_rotate_cmd *cmd)
+{
+	uint8_t rsp[64] = { 0 };
+	size_t rsp_len = sizeof(rsp);
+	tlvHeader_t hdr = {
+		.hdr = {
+			[0] = 0x80,
+			[1] = 0xd8,
+			[2] = 0,
+			[3] = PUT_KEYS_KEY_IDENTIFIER,
+		},
+	};
+	smStatus_t st = SM_NOT_OK;
+
+	if (!ctx || !cmd)
+		return kStatus_SSS_Fail;
+
+	hdr.hdr[2] = cmd->cmd[0];
+	st = DoAPDUTxRx_s_Case4(ctx, &hdr, cmd->cmd, cmd->cmd_len,
+				rsp, &rsp_len);
+
+	if ((rsp_len - 1 > sizeof(rsp)) || rsp_len < 2)
+		return kStatus_SSS_Fail;
+
+	st = (rsp[rsp_len - 2] << 8) + rsp[rsp_len - 1];
+	if (st != SM_OK)
+		return kStatus_SSS_Fail;
+
+	if (!memcmp(rsp, cmd->kcv, cmd->kcv_len))
+		return kStatus_SSS_Success;
+
+	return kStatus_SSS_Fail;
+}
diff --git a/core/drivers/crypto/se050/adaptors/apis/sss.c b/core/drivers/crypto/se050/adaptors/apis/sss.c
new file mode 100644
index 00000000..6c5b7417
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/apis/sss.c
@@ -0,0 +1,177 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <config.h>
+#include <crypto/crypto.h>
+#include <se050.h>
+#include <se050_utils.h>
+#include <string.h>
+
+static const sss_policy_u asym_key = {
+	.type = KPolicy_Asym_Key,
+	.auth_obj_id = 0,
+	.policy = {
+		.asymmkey = {
+			.can_Sign = 1,
+			.can_Verify = 1,
+			.can_Encrypt = 1,
+			.can_Decrypt = 1,
+			.can_KD = 1,
+			.can_Wrap = 1,
+			.can_Write = 1,
+			.can_Gen = 1,
+			.can_Import_Export = 1,
+			.can_KA = 1,
+			.can_Read = 1,
+			.can_Attest = 1,
+		}
+	}
+};
+
+static const sss_policy_u common = {
+	.type = KPolicy_Common,
+	.auth_obj_id = 0,
+	.policy = {
+		.common = {
+			.can_Delete = 1,
+			.req_Sm = 1,
+		},
+	},
+};
+
+sss_policy_t se050_asym_policy = {
+	.nPolicies = 2,
+	.policies = { &asym_key, &common },
+};
+
+sss_status_t se050_rotate_scp03_keys(struct sss_se05x_ctx *ctx)
+{
+	struct s050_scp_rotate_cmd cmd = { };
+	sss_status_t status = kStatus_SSS_Fail;
+	struct se050_scp_key cur_keys = { };
+	struct se050_scp_key new_keys = { };
+	SE_Connect_Ctx_t *connect_ctx = NULL;
+	sss_se05x_session_t *session = NULL;
+
+	if (!ctx)
+		return kStatus_SSS_Fail;
+
+	if (IS_ENABLED(CFG_CORE_SE05X_SCP03_EARLY))
+		return kStatus_SSS_Fail;
+
+	if (crypto_rng_read(new_keys.dek, sizeof(new_keys.dek)))
+		return kStatus_SSS_Fail;
+
+	if (crypto_rng_read(new_keys.mac, sizeof(new_keys.mac)))
+		return kStatus_SSS_Fail;
+
+	if (crypto_rng_read(new_keys.enc, sizeof(new_keys.enc)))
+		return kStatus_SSS_Fail;
+
+	status = se050_scp03_put_keys(&new_keys, &cur_keys);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	connect_ctx = &ctx->open_ctx;
+	session = &ctx->session;
+
+	status = se050_scp03_prepare_rotate_cmd(ctx, &cmd, &new_keys);
+	if (status != kStatus_SSS_Success)
+		goto restore;
+
+	sss_se05x_refresh_session(se050_session, NULL);
+	sss_se05x_session_close(session);
+
+	connect_ctx->skip_select_applet = 1;
+	status = sss_se05x_session_open(session, kType_SSS_SE_SE05x, 0,
+					kSSS_ConnectionType_Encrypted,
+					connect_ctx);
+	if (status != kStatus_SSS_Success)
+		goto restore;
+
+	status = se050_scp03_send_rotate_cmd(&session->s_ctx, &cmd);
+	if (status != kStatus_SSS_Success)
+		goto restore;
+
+	sss_host_session_close(&ctx->host_session);
+	sss_se05x_session_close(se050_session);
+	memset(ctx, 0, sizeof(*ctx));
+
+	if (se050_core_early_init(&new_keys))
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+
+restore:
+	se050_scp03_put_keys(&cur_keys, NULL);
+	return status;
+}
+
+sss_status_t se050_enable_scp03(sss_se05x_session_t *session)
+{
+	struct se050_scp_key keys = { };
+	sss_status_t status = kStatus_SSS_Success;
+	static bool enabled;
+
+	if (enabled)
+		return kStatus_SSS_Success;
+
+	status = se050_scp03_get_keys(&keys);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	sss_se05x_session_close(session);
+
+	if (se050_core_early_init(&keys))
+		return kStatus_SSS_Fail;
+
+	enabled = true;
+
+	return kStatus_SSS_Success;
+}
+
+sss_status_t se050_session_open(struct sss_se05x_ctx *ctx,
+				struct se050_scp_key *current_keys)
+{
+	sss_status_t status = kStatus_SSS_Fail;
+	SE_Connect_Ctx_t *connect_ctx = NULL;
+	sss_se05x_session_t *session = NULL;
+
+	if (!ctx)
+		return kStatus_SSS_Fail;
+
+	connect_ctx = &ctx->open_ctx;
+	session = &ctx->session;
+	connect_ctx->connType = kType_SE_Conn_Type_T1oI2C;
+	connect_ctx->portName = NULL;
+
+	if (!current_keys) {
+		return sss_se05x_session_open(session, kType_SSS_SE_SE05x, 0,
+					      kSSS_ConnectionType_Plain,
+					      connect_ctx);
+	}
+
+	status = se050_configure_host(&ctx->host_session,
+				      &ctx->host_ks,
+				      &ctx->open_ctx,
+				      &ctx->se05x_auth,
+				      kSSS_AuthType_SCP03,
+				      current_keys);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	return sss_se05x_session_open(session, kType_SSS_SE_SE05x, 0,
+				      kSSS_ConnectionType_Encrypted,
+				      connect_ctx);
+}
+
+sss_status_t se050_key_store_and_object_init(struct sss_se05x_ctx *ctx)
+{
+	if (!ctx)
+		return kStatus_SSS_Fail;
+
+	return sss_se05x_key_store_context_init(&ctx->ks, &ctx->session);
+}
diff --git a/core/drivers/crypto/se050/adaptors/apis/user.c b/core/drivers/crypto/se050/adaptors/apis/user.c
new file mode 100644
index 00000000..e3a4eaaf
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/apis/user.c
@@ -0,0 +1,152 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <nxScp03_Apis.h>
+#include <se050_user_apis.h>
+#include <se050_utils.h>
+#include <string.h>
+
+static sss_status_t alloc_scp_key_to_auth(sss_object_t *k_object,
+					  sss_key_store_t *k_store,
+					  uint32_t key_id)
+{
+	sss_status_t status = kStatus_SSS_Fail;
+
+	if (!k_object || !k_store)
+		return kStatus_SSS_Fail;
+
+	status = sss_host_key_object_init(k_object, k_store);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	return sss_host_key_object_allocate_handle(k_object, key_id,
+						   kSSS_KeyPart_Default,
+						   kSSS_CipherType_AES, 16,
+						   kKeyObject_Mode_Transient);
+}
+
+static sss_status_t prepare_host_scp(NXSCP03_AuthCtx_t *scp,
+				     struct se050_auth_ctx *auth,
+				     sss_key_store_t *k_store,
+				     struct se050_scp_key *keys,
+				     uint32_t oid)
+{
+	sss_status_t status = kStatus_SSS_Fail;
+	NXSCP03_StaticCtx_t *pStatic_ctx = NULL;
+	NXSCP03_DynCtx_t *pDyn_ctx = NULL;
+	size_t len = 0;
+
+	if (!scp || !auth || !k_store)
+		return kStatus_SSS_Fail;
+
+	pStatic_ctx = &auth->static_ctx;
+	pDyn_ctx = &auth->dynamic_ctx;
+
+	scp->pStatic_ctx = pStatic_ctx;
+	scp->pDyn_ctx = pDyn_ctx;
+	pStatic_ctx->keyVerNo = 0x0B;
+
+	status = alloc_scp_key_to_auth(&pStatic_ctx->Enc, k_store, oid++);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	len = sizeof(keys->enc);
+	status = sss_host_key_store_set_key(k_store, &pStatic_ctx->Enc,
+					    keys->enc, len, len * 8, NULL, 0);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	status = alloc_scp_key_to_auth(&pStatic_ctx->Mac, k_store, oid++);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	len = sizeof(keys->mac);
+	status = sss_host_key_store_set_key(k_store, &pStatic_ctx->Mac,
+					    keys->mac, len, len * 8, NULL, 0);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	status = alloc_scp_key_to_auth(&pStatic_ctx->Dek, k_store, oid++);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	len = sizeof(keys->dek);
+	status = sss_host_key_store_set_key(k_store, &pStatic_ctx->Dek,
+					    keys->dek, len, len * 8, NULL, 0);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	status = alloc_scp_key_to_auth(&pDyn_ctx->Enc, k_store, oid++);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	status = alloc_scp_key_to_auth(&pDyn_ctx->Mac, k_store, oid++);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	return alloc_scp_key_to_auth(&pDyn_ctx->Rmac, k_store, oid++);
+}
+
+sss_status_t se050_configure_host(sss_user_impl_session_t *host_session,
+				  sss_key_store_t *host_ks,
+				  SE_Connect_Ctx_t *open_ctx,
+				  struct se050_auth_ctx *auth,
+				  SE_AuthType_t auth_type,
+				  struct se050_scp_key *keys)
+{
+	sss_status_t status = kStatus_SSS_Fail;
+	uint32_t host_oid = 0;
+
+	if (!host_session || !host_ks || !open_ctx || !auth)
+		return kStatus_SSS_Fail;
+
+	if (host_session->subsystem != kType_SSS_SubSystem_NONE)
+		goto prepare;
+
+	status = sss_host_session_open(host_session, kType_SSS_Software, 0,
+				       kSSS_ConnectionType_Plain, NULL);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	status = sss_host_key_store_context_init(host_ks, host_session);
+	if (status != kStatus_SSS_Success)
+		goto error;
+
+	status = sss_host_key_store_allocate(host_ks, host_oid++);
+	if (status != kStatus_SSS_Success)
+		goto error;
+prepare:
+	status = prepare_host_scp(&open_ctx->auth.ctx.scp03, auth, host_ks,
+				  keys, host_oid);
+	if (status != kStatus_SSS_Success)
+		goto error;
+
+	open_ctx->auth.authType = auth_type;
+	return status;
+
+error:
+	sss_host_session_close(host_session);
+	return status;
+}
+
+TEE_Result se050_host_key_store_get_key(sss_key_store_t *ks __unused,
+					sss_object_t *ko, uint8_t *data,
+					size_t *byte_len, size_t *bit_len)
+{
+	sss_user_impl_object_t *key_object = (sss_user_impl_object_t *)ko;
+
+	if (!ko)
+		return TEE_ERROR_GENERIC;
+
+	if (*byte_len < sizeof(key_object->key))
+		return TEE_ERROR_EXCESS_DATA;
+
+	memcpy(data, key_object->key, sizeof(key_object->key));
+	*byte_len = sizeof(key_object->key);
+	*bit_len = 8 * sizeof(key_object->key);
+
+	return TEE_SUCCESS;
+}
diff --git a/core/drivers/crypto/se050/adaptors/include/se050.h b/core/drivers/crypto/se050/adaptors/include/se050.h
new file mode 100644
index 00000000..e023930c
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/include/se050.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef SE050_H_
+#define SE050_H_
+
+#include <fsl_sss_util_asn1_der.h>
+#include <fsl_sss_se05x_apis.h>
+#include <se05x_APDU.h>
+#include <se050_sss_apis.h>
+#include <se050_apdu_apis.h>
+#include <se050_user_apis.h>
+#include <se050_utils.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+TEE_Result se050_core_early_init(struct se050_scp_key *keys);
+
+extern sss_se05x_key_store_t *se050_kstore;
+extern sss_se05x_session_t *se050_session;
+extern struct sss_se05x_ctx se050_ctx;
+
+#endif /* SE050_H_ */
diff --git a/core/drivers/crypto/se050/adaptors/include/se050_apdu_apis.h b/core/drivers/crypto/se050/adaptors/include/se050_apdu_apis.h
new file mode 100644
index 00000000..b6247561
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/include/se050_apdu_apis.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef SE050_APDU_APIS_H_
+#define SE050_APDU_APIS_H_
+
+#include <se050.h>
+
+struct s050_scp_rotate_cmd;
+
+sss_status_t se050_factory_reset(pSe05xSession_t ctx);
+
+bool se050_key_exists(uint32_t k_id, pSe05xSession_t ctx);
+
+struct se050_rsa_keypair {
+	uint8_t *e;
+	size_t e_len;
+	uint8_t *d;
+	size_t d_len;
+	uint8_t *n;
+	size_t n_len;
+
+	uint8_t *p;
+	size_t p_len;
+	uint8_t *q;
+	size_t q_len;
+	uint8_t *qp;
+	size_t qp_len;
+	uint8_t *dp;
+	size_t dp_len;
+	uint8_t *dq;
+	size_t dq_len;
+};
+
+struct se050_rsa_keypub {
+	uint8_t *e;
+	size_t e_len;
+	uint8_t *n;
+	size_t n_len;
+};
+
+sss_status_t se050_key_store_set_rsa_key_bin(sss_se05x_key_store_t *k_store,
+					     sss_se05x_object_t *k_object,
+					     struct se050_rsa_keypair *k_pair,
+					     struct se050_rsa_keypub *k_pub,
+					     size_t k_bit_len);
+
+sss_status_t se050_get_free_memory(pSe05xSession_t ctx, uint16_t *t,
+				   SE05x_MemoryType_t type);
+
+sss_status_t se050_scp03_send_rotate_cmd(pSe05xSession_t ctx,
+					 struct s050_scp_rotate_cmd *cmd);
+
+#endif /* SE050_APDU_APIS_H_ */
diff --git a/core/drivers/crypto/se050/adaptors/include/se050_sss_apis.h b/core/drivers/crypto/se050/adaptors/include/se050_sss_apis.h
new file mode 100644
index 00000000..dac39073
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/include/se050_sss_apis.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef SE050_SSS_APIS_H_
+#define SE050_SSS_APIS_H_
+
+#include <fsl_sss_se05x_types.h>
+#include <nxScp03_Types.h>
+
+extern sss_policy_t se050_asym_policy;
+struct se050_scp_key;
+
+struct sss_se05x_ctx {
+	SE_Connect_Ctx_t open_ctx;
+	sss_se05x_session_t session;
+	sss_se05x_key_store_t ks;
+
+	struct se050_auth_ctx {
+		NXSCP03_StaticCtx_t static_ctx;
+		NXSCP03_DynCtx_t dynamic_ctx;
+	} se05x_auth;
+	sss_user_impl_session_t host_session;
+	sss_key_store_t host_ks;
+};
+
+sss_status_t se050_key_store_and_object_init(struct sss_se05x_ctx *ctx);
+sss_status_t se050_enable_scp03(sss_se05x_session_t *session);
+sss_status_t se050_rotate_scp03_keys(struct sss_se05x_ctx *ctx);
+sss_status_t se050_session_open(struct sss_se05x_ctx *ctx,
+				struct se050_scp_key *key);
+#endif /* SE050_SSS_APIS_H_ */
diff --git a/core/drivers/crypto/se050/adaptors/include/se050_user_apis.h b/core/drivers/crypto/se050/adaptors/include/se050_user_apis.h
new file mode 100644
index 00000000..ad833880
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/include/se050_user_apis.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef SE050_USER_APIS_H_
+#define SE050_USER_APIS_H_
+
+#include <fsl_sss_se05x_apis.h>
+#include <fsl_sss_se05x_types.h>
+#include <nxScp03_Types.h>
+#include <se050_sss_apis.h>
+#include <se050_utils.h>
+
+sss_status_t se050_configure_host(sss_user_impl_session_t *host_session,
+				  sss_key_store_t *host_ks,
+				  SE_Connect_Ctx_t *open_ctx,
+				  struct se050_auth_ctx *auth_ctx,
+				  SE_AuthType_t auth_type,
+				  struct se050_scp_key *keys);
+
+TEE_Result se050_host_key_store_get_key(sss_key_store_t *ks __unused,
+					sss_object_t *ko, uint8_t *data,
+					size_t *byte_len, size_t *bit_len);
+#endif /* SE050_USER_APIS_H_ */
diff --git a/core/drivers/crypto/se050/adaptors/include/se050_utils.h b/core/drivers/crypto/se050/adaptors/include/se050_utils.h
new file mode 100644
index 00000000..95ecad61
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/include/se050_utils.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef SE050_UTILS_H_
+#define SE050_UTILS_H_
+
+#include <se050.h>
+#include <tee_api_types.h>
+
+struct se050_scp_key {
+	uint8_t enc[16];
+	uint8_t mac[16];
+	uint8_t dek[16];
+};
+
+struct s050_scp_rotate_cmd {
+	uint8_t cmd[128];
+	size_t cmd_len;
+	uint8_t kcv[16];
+	size_t kcv_len;
+};
+
+#define OID_MIN			((uint32_t)(0x00000001))
+#define OID_MAX			((uint32_t)(OID_MIN + 0x7BFFFFFE))
+
+#define SE050_KEY_WATERMARK	0x57721566
+#define WATERMARKED(x)	\
+	((uint64_t)(((uint64_t)SE050_KEY_WATERMARK) << 32) + (x))
+
+sss_status_t se050_get_oid(sss_key_object_mode_t type, uint32_t *val);
+
+struct rsa_keypair;
+
+uint32_t se050_rsa_keypair_from_nvm(struct rsa_keypair *key);
+uint64_t se050_generate_private_key(uint32_t oid);
+
+void se050_refcount_init_ctx(uint8_t **cnt);
+int se050_refcount_final_ctx(uint8_t *cnt);
+
+void se050_display_board_info(sss_se05x_session_t *session);
+
+sss_status_t se050_scp03_get_keys(struct se050_scp_key *keys);
+sss_status_t se050_scp03_put_keys(struct se050_scp_key *new_keys,
+				  struct se050_scp_key *cur_keys);
+sss_status_t se050_scp03_prepare_rotate_cmd(struct sss_se05x_ctx *ctx,
+					    struct s050_scp_rotate_cmd *cmd,
+					    struct se050_scp_key *keys);
+#endif /* SE050_UTILS_H_ */
diff --git a/core/drivers/crypto/se050/adaptors/sub.mk b/core/drivers/crypto/se050/adaptors/sub.mk
new file mode 100644
index 00000000..252d3dc9
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/sub.mk
@@ -0,0 +1,15 @@
+cflags-y += -Wno-strict-aliasing
+cflags-y += -DAX_EMBEDDED=1
+cflags-y += -DVERBOSE_APDU_LOGS=0
+cflags-y += -DT1oI2C_UM11225
+cflags-y += -DT1oI2C
+cflags-y += -DSSS_USE_FTR_FILE
+
+incdirs-y += ./include
+
+srcs-y += utils/scp_config.c
+srcs-y += utils/utils.c
+srcs-y += utils/info.c
+srcs-y += apis/apdu.c
+srcs-y += apis/user.c
+srcs-y += apis/sss.c
diff --git a/core/drivers/crypto/se050/adaptors/utils/info.c b/core/drivers/crypto/se050/adaptors/utils/info.c
new file mode 100644
index 00000000..db46583c
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/utils/info.c
@@ -0,0 +1,208 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <fsl_sss_se05x_apis.h>
+#include <global_platf.h>
+#include <se050.h>
+#include <se05x_const.h>
+#include <se05x_tlv.h>
+#include <smCom.h>
+#include <string.h>
+
+/* Force the output until the P&T stack fixes its verbosity */
+#define LOG_MAU8_I(msg, buf, len) nLog_au8("Info", 0xff, msg, buf, len)
+#define LOG_I(format, ...) nLog("Info", 0xff, format, ##__VA_ARGS__)
+
+#define LOG_E(format, ...) nLog("Info", NX_LEVEL_ERROR, format, ##__VA_ARGS__)
+
+#define LOG_MAU8_E(msg, buf, len) \
+	nLog_au8("Info", NX_LEVEL_ERROR, msg, buf, len)
+
+static sss_status_t jcop4_get_id(void *ctx)
+{
+	char jcop_platform_id[17] = { 0 };
+	smStatus_t ret = SM_OK;
+	unsigned char cmd[] = {
+		0x80, /* CLA '80' / '00' GlobalPlatform / ISO / IEC	*/
+		0xCA, /* INS 'CA' GET DATA(IDENTIFY)			*/
+		0x00, /* P1 '00' High order tag value			*/
+		0xFE, /* P2 'FE' Low order tag value - proprietary data	*/
+		0x02, /* Lc '02' Length of data field			*/
+		0xDF,
+		0x28, /* Data 'DF28' Card identification data		*/
+		0x00  /* Le '00' Length of response data		*/
+	};
+	struct msg_rsp {
+		uint8_t vTag_value_proprietary_data;
+		uint8_t vLength_of_following_data;
+		uint8_t vTag_card_identification_data[0x02];
+		uint8_t vLength_of_card_identification_data;
+		uint8_t vTag_configuration_ID;
+		uint8_t vLength_configuration_ID;
+		uint8_t vConfiguration_ID[0x0C];
+		uint8_t vTag_patch_ID;
+		uint8_t vLength_patch_ID;
+		uint8_t vPatch_ID[0x08];
+		uint8_t vTag_platform_build_ID1;
+		uint8_t vLength_platform_build_ID;
+		uint8_t vPlatform_build_ID[0x18];
+		uint8_t vTag_FIPS_mode;
+		uint8_t vLength_FIPS_mode;
+		uint8_t vFIPS_mode;
+		uint8_t vTag_pre_perso_state;
+		uint8_t vLength_pre_perso_state;
+		uint8_t vBit_mask_of_pre_perso_state;
+		uint8_t vTag_ROM_ID;
+		uint8_t vLength_ROM_ID;
+		uint8_t vROM_ID[0x08];
+		uint8_t vStatus_Word_SW_[0x02];
+	} rsp = { 0 };
+	uint8_t *p = (uint8_t *)&rsp;
+	uint32_t len = sizeof(struct msg_rsp);
+	uint16_t dummy = sizeof(struct msg_rsp);
+
+	ret = GP_Select(ctx, p, 0, p, &dummy);
+	if (ret != SM_OK) {
+		LOG_E("Could not select ISD.");
+		return kStatus_SSS_Fail;
+	}
+
+	ret = smCom_TransceiveRaw(ctx, cmd, sizeof(cmd), p, &len);
+	if (ret != SM_OK || len != sizeof(rsp)) {
+		LOG_MAU8_E("Error reading JCOP ID", p, sizeof(rsp));
+		return kStatus_SSS_Fail;
+	}
+
+	LOG_I("SE050 JCOP4 Information:");
+	LOG_I("%s = 0x%02X", "Tag value - proprietary data 0xFE",
+	      rsp.vTag_value_proprietary_data);
+	LOG_I("%s = 0x%02X", "Length of following data 0x45",
+	      rsp.vLength_of_following_data);
+	LOG_MAU8_I("Tag card identification data",
+		   rsp.vTag_card_identification_data,
+		   sizeof(rsp.vTag_card_identification_data));
+	LOG_I("%s = 0x%02X", "Length of card identification data",
+	      rsp.vLength_of_card_identification_data);
+	LOG_I("%s = 0x%02X", "Tag configuration ID (Must be 0x01)",
+	      rsp.vTag_configuration_ID);
+	LOG_I("%s = 0x%02X", "Length configuration ID 0x0C",
+	      rsp.vLength_configuration_ID);
+	LOG_MAU8_I("Configuration ID",
+		   rsp.vConfiguration_ID, sizeof(rsp.vConfiguration_ID));
+
+	LOG_MAU8_I("OEF ID", &rsp.vConfiguration_ID[2], 2);
+	LOG_I("%s = 0x%02X", "Tag patch ID (Must be 0x02)", rsp.vTag_patch_ID);
+	LOG_I("%s = 0x%02X", "Length patch ID 0x08", rsp.vLength_patch_ID);
+	LOG_MAU8_I("Patch ID", rsp.vPatch_ID, sizeof(rsp.vPatch_ID));
+	LOG_I("%s = 0x%02X", "Tag platform build ID1 (Must be 0x03)",
+	      rsp.vTag_platform_build_ID1);
+	LOG_I("%s = 0x%02X", "Length platform build ID 0x18",
+	      rsp.vLength_platform_build_ID);
+	LOG_MAU8_I("Platform build ID",
+		   rsp.vPlatform_build_ID, sizeof(rsp.vPlatform_build_ID));
+	memcpy(jcop_platform_id, rsp.vPlatform_build_ID, 16);
+
+	LOG_I("%s = %s", "JCOP Platform ID", jcop_platform_id);
+	LOG_I("%s = 0x%02X", "Tag FIPS mode (Must be 0x05)",
+	      rsp.vTag_FIPS_mode);
+	LOG_I("%s = 0x%02X", "Length FIPS mode 0x01", rsp.vLength_FIPS_mode);
+	LOG_I("%s = 0x%02X", "FIPS mode var", rsp.vFIPS_mode);
+	LOG_I("%s = 0x%02X", "Tag pre-perso state (Must be 0x07)",
+	      rsp.vTag_pre_perso_state);
+	LOG_I("%s = 0x%02X", "Length pre-perso state 0x01",
+	      rsp.vLength_pre_perso_state);
+	LOG_I("%s = 0x%02X", "Bit mask of pre-perso state var",
+	      rsp.vBit_mask_of_pre_perso_state);
+
+	LOG_I("%s = 0x%02X", "Tag ROM ID (Must be 0x08)", rsp.vTag_ROM_ID);
+	LOG_I("%s = 0x%02X", "Length ROM ID 0x08", rsp.vLength_ROM_ID);
+	LOG_MAU8_I("ROM ID", rsp.vROM_ID, sizeof(rsp.vROM_ID));
+	LOG_MAU8_I("Status Word (SW)", rsp.vStatus_Word_SW_,
+		   sizeof(rsp.vStatus_Word_SW_));
+
+	return kStatus_SSS_Success;
+}
+
+#define ITEM(__x)  {					\
+		.name  = #__x,				\
+		.val = (kSE05x_AppletConfig_##__x),	\
+	}
+
+static void show_config(uint16_t cfg)
+{
+	struct items {
+		uint16_t val;
+		const char *name;
+	} features[] = {
+		ITEM(ECDAA), ITEM(ECDSA_ECDH_ECDHE), ITEM(EDDSA), ITEM(DH_MONT),
+		ITEM(HMAC), ITEM(RSA_PLAIN), ITEM(RSA_CRT), ITEM(AES),
+		ITEM(DES), ITEM(PBKDF), ITEM(TLS), ITEM(MIFARE), ITEM(I2CM),
+	};
+	size_t i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(features); i++) {
+		LOG_I("\t%s%s", cfg & features[i].val ? "with\t" : "without\t",
+		      features[i].name);
+	}
+}
+
+static sss_status_t applet_get_id(sss_se05x_session_t *session)
+{
+	SE05x_Result_t result = kSE05x_Result_NA;
+	smStatus_t ret = SM_OK;
+	uint8_t uid[SE050_MODULE_UNIQUE_ID_LEN] = { 0 };
+	size_t uidLen = sizeof(uid);
+	uint8_t applet_version[7] = { 0 };
+	size_t applet_versionLen = sizeof(applet_version);
+
+	ret = Se05x_API_CheckObjectExists(&session->s_ctx,
+					  kSE05x_AppletResID_UNIQUE_ID,
+					  &result);
+	if (ret != SM_OK)
+		return kStatus_SSS_Fail;
+
+	ret = Se05x_API_ReadObject(&session->s_ctx,
+				   kSE05x_AppletResID_UNIQUE_ID, 0,
+				   (uint16_t)uidLen, uid, &uidLen);
+	if (ret != SM_OK)
+		return kStatus_SSS_Fail;
+
+	LOG_MAU8_I("Applet ID", uid, uidLen);
+
+	/*
+	 * VersionInfo is a 7 - byte value consisting of:
+	 * - 1 - byte Major applet version
+	 * - 1 - byte Minor applet version
+	 * - 1 - byte patch applet version
+	 * - 2 - byte AppletConfig, indicating the supported applet features
+	 * - 2-byte Secure Box version: major version (MSB) concatenated with
+	 *   minor version (LSB).
+	 */
+
+	ret = Se05x_API_GetVersion(&session->s_ctx, applet_version,
+				   &applet_versionLen);
+	if (ret != SM_OK) {
+		LOG_E("Failed Se05x_API_GetVersion");
+		return kStatus_SSS_Fail;
+	}
+
+	LOG_I("Applet Major = %d", applet_version[0]);
+	LOG_I("Applet Minor = %d", applet_version[1]);
+	LOG_I("Applet patch = %d", applet_version[2]);
+	LOG_I("AppletConfig = %02X%02X", applet_version[3], applet_version[4]);
+	show_config(applet_version[3] << 8 | applet_version[4]);
+	LOG_I("Internal = %02X%02X", applet_version[5], applet_version[6]);
+
+	return kStatus_SSS_Success;
+}
+
+void se050_display_board_info(sss_se05x_session_t *session)
+{
+	if (session) {
+		applet_get_id(session);
+		jcop4_get_id(session->s_ctx.conn_ctx);
+	}
+}
diff --git a/core/drivers/crypto/se050/adaptors/utils/scp_config.c b/core/drivers/crypto/se050/adaptors/utils/scp_config.c
new file mode 100644
index 00000000..9a8b1d9d
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/utils/scp_config.c
@@ -0,0 +1,435 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+#include <assert.h>
+#include <bitstring.h>
+#include <config.h>
+#include <crypto/crypto.h>
+#include <kernel/mutex.h>
+#include <kernel/refcount.h>
+#include <kernel/thread.h>
+#include <mm/mobj.h>
+#include <optee_rpc_cmd.h>
+#include <se050.h>
+#include <se050_utils.h>
+#include <scp.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <tee_api_defines_extensions.h>
+#include <tee/tadb.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_rpc.h>
+#include <tee/tee_pobj.h>
+#include <tee/tee_svc_storage.h>
+#include <utee_defines.h>
+
+#define SE050A1_ID 0xA204
+#define SE050A2_ID 0xA205
+#define SE050B1_ID 0xA202
+#define SE050B2_ID 0xA203
+#define SE050C1_ID 0xA200
+#define SE050C2_ID 0xA201
+#define SE050DV_ID 0xA1F4
+
+#define SE050A1 0
+#define SE050A2 1
+#define SE050B1 2
+#define SE050B2 3
+#define SE050C1 4
+#define SE050C2 5
+#define SE050DV 6
+
+static const struct se050_scp_key se050_default_keys[] = {
+	[SE050A1] = {
+		.enc = { 0x34, 0xae, 0x09, 0x67, 0xe3, 0x29, 0xe9, 0x51,
+			0x8e, 0x72, 0x65, 0xd5, 0xad, 0xcc, 0x01, 0xc2 },
+		.mac = { 0x52, 0xb2, 0x53, 0xca, 0xdf, 0x47, 0x2b, 0xdb,
+			0x3d, 0x0f, 0xb3, 0x8e, 0x09, 0x77, 0x00, 0x99 },
+		.dek = { 0xac, 0xc9, 0x14, 0x31, 0xfe, 0x26, 0x81, 0x1b,
+			0x5e, 0xcb, 0xc8, 0x45, 0x62, 0x0d, 0x83, 0x44 },
+	},
+	[SE050A2] = {
+		.enc = { 0x46, 0xa9, 0xc4, 0x8c, 0x34, 0xef, 0xe3, 0x44,
+			0xa5, 0x22, 0xe6, 0x67, 0x44, 0xf8, 0x99, 0x6a },
+		.mac = { 0x12, 0x03, 0xff, 0x61, 0xdf, 0xbc, 0x9c, 0x86,
+			0x19, 0x6a, 0x22, 0x74, 0xae, 0xf4, 0xed, 0x28 },
+		.dek = { 0xf7, 0x56, 0x1c, 0x6f, 0x48, 0x33, 0x61, 0x19,
+			0xee, 0x39, 0x43, 0x9a, 0xab, 0x34, 0x09, 0x8e },
+	},
+	[SE050B1] = {
+		.enc = { 0xd4, 0x99, 0xbc, 0x90, 0xde, 0xa5, 0x42, 0xcf,
+			0x78, 0xd2, 0x5e, 0x13, 0xd6, 0x4c, 0xbb, 0x1f },
+		.mac = { 0x08, 0x15, 0x55, 0x96, 0x43, 0xfb, 0x79, 0xeb,
+			0x85, 0x01, 0xa0, 0xdc, 0x83, 0x3d, 0x90, 0x1f },
+		.dek = { 0xbe, 0x7d, 0xdf, 0xb4, 0x06, 0xe8, 0x1a, 0xe4,
+			0xe9, 0x66, 0x5a, 0x9f, 0xed, 0x64, 0x26, 0x7c },
+	},
+	[SE050B2] = {
+		.enc = { 0x5f, 0xa4, 0x3d, 0x82, 0x02, 0xd2, 0x5e, 0x9a,
+			0x85, 0xb1, 0xfe, 0x7e, 0x2d, 0x26, 0x47, 0x8d },
+		.mac = { 0x10, 0x5c, 0xea, 0x22, 0x19, 0xf5, 0x2b, 0xd1,
+			0x67, 0xa0, 0x74, 0x63, 0xc6, 0x93, 0x79, 0xc3 },
+		.dek = { 0xd7, 0x02, 0x81, 0x57, 0xf2, 0xad, 0x37, 0x2c,
+			0x74, 0xbe, 0x96, 0x9b, 0xcc, 0x39, 0x06, 0x27 },
+	},
+	[SE050C1] = {
+		.enc = { 0x85, 0x2b, 0x59, 0x62, 0xe9, 0xcc, 0xe5, 0xd0,
+			0xbe, 0x74, 0x6b, 0x83, 0x3b, 0xcc, 0x62, 0x87 },
+		.mac = { 0xdb, 0x0a, 0xa3, 0x19, 0xa4, 0x08, 0x69, 0x6c,
+			0x8e, 0x10, 0x7a, 0xb4, 0xe3, 0xc2, 0x6b, 0x47 },
+		.dek = { 0x4c, 0x2f, 0x75, 0xc6, 0xa2, 0x78, 0xa4, 0xae,
+			0xe5, 0xc9, 0xaf, 0x7c, 0x50, 0xee, 0xa8, 0x0c },
+	},
+	[SE050C2] = {
+		.enc = { 0xbd, 0x1d, 0xe2, 0x0a, 0x81, 0xea, 0xb2, 0xbf,
+			0x3b, 0x70, 0x9a, 0x9d, 0x69, 0xa3, 0x12, 0x54 },
+		.mac = { 0x9a, 0x76, 0x1b, 0x8d, 0xba, 0x6b, 0xed, 0xf2,
+			0x27, 0x41, 0xe4, 0x5d, 0x8d, 0x42, 0x36, 0xf5 },
+		.dek = { 0x9b, 0x99, 0x3b, 0x60, 0x0f, 0x1c, 0x64, 0xf5,
+			0xad, 0xc0, 0x63, 0x19, 0x2a, 0x96, 0xc9, 0x47 },
+	},
+	[SE050DV] = {
+		.enc = { 0x35, 0xc2, 0x56, 0x45, 0x89, 0x58, 0xa3, 0x4f,
+			0x61, 0x36, 0x15, 0x5f, 0x82, 0x09, 0xd6, 0xcd },
+		.mac = { 0xaf, 0x17, 0x7d, 0x5d, 0xbd, 0xf7, 0xc0, 0xd5,
+			0xc1, 0x0a, 0x05, 0xb9, 0xf1, 0x60, 0x7f, 0x78 },
+		.dek = { 0xa1, 0xbc, 0x84, 0x38, 0xbf, 0x77, 0x93, 0x5b,
+			0x36, 0x1a, 0x44, 0x25, 0xfe, 0x79, 0xfa, 0x29 },
+	},
+};
+
+struct tee_scp03db_dir {
+	const struct tee_file_operations *ops;
+	struct tee_file_handle *fh;
+};
+
+static const char scp03db_obj_id[] = "scp03.db";
+static struct tee_pobj po = {
+	.obj_id_len = sizeof(scp03db_obj_id),
+	.obj_id = (void *)scp03db_obj_id,
+};
+
+static TEE_Result __maybe_unused scp03db_delete_keys(void)
+{
+	struct tee_scp03db_dir *db = calloc(1, sizeof(struct tee_scp03db_dir));
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!db)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	db->ops = tee_svc_storage_file_ops(TEE_STORAGE_PRIVATE);
+	res = db->ops->open(&po, NULL, &db->fh);
+	if (res == TEE_ERROR_ITEM_NOT_FOUND) {
+		free(db);
+		return TEE_SUCCESS;
+	}
+
+	if (res) {
+		free(db);
+		return res;
+	}
+
+	db->ops->close(&db->fh);
+	db->ops->remove(&po);
+	free(db);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result scp03db_write_keys(struct se050_scp_key *keys)
+{
+	struct tee_scp03db_dir *db = calloc(1, sizeof(struct tee_scp03db_dir));
+	TEE_Result res = TEE_SUCCESS;
+	size_t len = sizeof(*keys);
+
+	if (!db)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	db->ops = tee_svc_storage_file_ops(TEE_STORAGE_PRIVATE);
+
+	res = db->ops->open(&po, NULL, &db->fh);
+	if (res && res != TEE_ERROR_ITEM_NOT_FOUND) {
+		free(db);
+		return TEE_ERROR_STORAGE_NOT_AVAILABLE;
+	}
+
+	res = db->ops->create(&po, true, NULL, 0, NULL, 0, keys, len, &db->fh);
+	db->ops->close(&db->fh);
+	free(db);
+
+	return res;
+}
+
+static TEE_Result scp03db_read_keys(struct se050_scp_key *keys)
+{
+	struct tee_scp03db_dir *db = calloc(1, sizeof(struct tee_scp03db_dir));
+	TEE_Result res = TEE_SUCCESS;
+	size_t len = sizeof(*keys);
+
+	if (!db)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	db->ops = tee_svc_storage_file_ops(TEE_STORAGE_PRIVATE);
+
+	res = db->ops->open(&po, NULL, &db->fh);
+	if (res) {
+		free(db);
+		return TEE_ERROR_STORAGE_NOT_AVAILABLE;
+	}
+
+	res = db->ops->read(db->fh, 0, keys, &len);
+	if (res)
+		goto close;
+
+	if (len != sizeof(*keys))
+		res = TEE_ERROR_GENERIC;
+close:
+	db->ops->close(&db->fh);
+	free(db);
+
+	return res;
+}
+
+static sss_status_t get_id_from_ofid(uint32_t ofid, uint32_t *id)
+{
+	switch (ofid) {
+	case SE050A1_ID:
+		*id = SE050A1;
+		break;
+	case SE050A2_ID:
+		*id = SE050A2;
+		break;
+	case SE050B1_ID:
+		*id = SE050B1;
+		break;
+	case SE050B2_ID:
+		*id = SE050B2;
+		break;
+	case SE050C1_ID:
+		*id = SE050C1;
+		break;
+	case SE050C2_ID:
+		*id = SE050C2;
+		break;
+	case SE050DV_ID:
+		*id = SE050DV;
+		break;
+	default:
+		return kStatus_SSS_Fail;
+	}
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t encrypt_key_and_get_kcv(uint8_t *enc, uint8_t *kc,
+					    uint8_t *key,
+					    struct sss_se05x_ctx *ctx,
+					    uint32_t id)
+{
+	static const uint8_t ones[] = { [0 ... AES_KEY_LEN_nBYTE - 1] = 1 };
+	uint8_t enc_len = AES_KEY_LEN_nBYTE;
+	uint8_t kc_len = AES_KEY_LEN_nBYTE;
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_object_t *dek_object = NULL;
+	sss_se05x_symmetric_t symm = { };
+	sss_se05x_object_t ko = { };
+	uint8_t dek[AES_KEY_LEN_nBYTE] = { 0 };
+	size_t dek_len = sizeof(dek);
+	size_t dek_bit_len = dek_len * 8;
+
+	st = sss_se05x_key_object_init(&ko, &ctx->ks);
+	if (st != kStatus_SSS_Success)
+		return kStatus_SSS_Fail;
+
+	st = sss_se05x_key_object_allocate_handle(&ko, id,
+						  kSSS_KeyPart_Default,
+						  kSSS_CipherType_AES,
+						  AES_KEY_LEN_nBYTE,
+						  kKeyObject_Mode_Transient);
+	if (st != kStatus_SSS_Success)
+		return kStatus_SSS_Fail;
+
+	st = sss_se05x_key_store_set_key(&ctx->ks, &ko, key, AES_KEY_LEN_nBYTE,
+					 AES_KEY_LEN_nBYTE * 8, NULL, 0);
+	if (st != kStatus_SSS_Success)
+		goto out;
+
+	st = sss_se05x_symmetric_context_init(&symm, &ctx->session, &ko,
+					      kAlgorithm_SSS_AES_ECB,
+					      kMode_SSS_Encrypt);
+	if (st != kStatus_SSS_Success)
+		goto out;
+
+	st = sss_se05x_cipher_one_go(&symm, NULL, 0, ones, kc, kc_len);
+	if (st != kStatus_SSS_Success)
+		goto out;
+
+	dek_object = &ctx->open_ctx.auth.ctx.scp03.pStatic_ctx->Dek;
+	if (se050_host_key_store_get_key(&ctx->host_ks, dek_object,
+					 dek, &dek_len, &dek_bit_len))
+		goto out;
+
+	st = sss_se05x_key_store_set_key(&ctx->ks, &ko, dek, AES_KEY_LEN_nBYTE,
+					 AES_KEY_LEN_nBYTE * 8, NULL, 0);
+	if (st != kStatus_SSS_Success)
+		goto out;
+
+	st = sss_se05x_cipher_one_go(&symm, NULL, 0, key, enc, enc_len);
+out:
+	if (symm.keyObject)
+		sss_se05x_symmetric_context_free(&symm);
+
+	sss_se05x_key_object_free(&ko);
+
+	Se05x_API_DeleteSecureObject(&ctx->session.s_ctx, id);
+
+	return st;
+}
+
+static sss_status_t prepare_key_data(uint8_t *key, uint8_t *cmd,
+				     struct sss_se05x_ctx *ctx, uint32_t id)
+{
+	uint8_t kc[AES_KEY_LEN_nBYTE] = { 0 };
+	sss_status_t status = kStatus_SSS_Fail;
+
+	cmd[0] = PUT_KEYS_KEY_TYPE_CODING_AES;
+	cmd[1] = AES_KEY_LEN_nBYTE + 1;
+	cmd[2] = AES_KEY_LEN_nBYTE;
+	cmd[3 + AES_KEY_LEN_nBYTE] = CRYPTO_KEY_CHECK_LEN;
+
+	status = encrypt_key_and_get_kcv(&cmd[3], kc, key, ctx, id);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	memcpy(&cmd[3 + AES_KEY_LEN_nBYTE + 1], kc, CRYPTO_KEY_CHECK_LEN);
+
+	return kStatus_SSS_Success;
+}
+
+sss_status_t se050_scp03_prepare_rotate_cmd(struct sss_se05x_ctx *ctx,
+					    struct s050_scp_rotate_cmd *cmd,
+					    struct se050_scp_key *keys)
+
+{
+	sss_status_t status = kStatus_SSS_Fail;
+	size_t kcv_len = 0;
+	size_t cmd_len = 0;
+	uint8_t key_version = 0;
+	uint8_t *key[] = {
+		[0] = keys->enc,
+		[1] = keys->mac,
+		[2] = keys->dek,
+	};
+	uint32_t oid = 0;
+	size_t i = 0;
+
+	key_version = ctx->open_ctx.auth.ctx.scp03.pStatic_ctx->keyVerNo;
+	cmd->cmd[cmd_len] = key_version;
+	cmd_len += 1;
+
+	cmd->kcv[kcv_len] = key_version;
+	kcv_len += 1;
+
+	for (i = 0; i < ARRAY_SIZE(key); i++) {
+		status = se050_get_oid(kKeyObject_Mode_Transient, &oid);
+		if (status != kStatus_SSS_Success)
+			return kStatus_SSS_Fail;
+
+		status = prepare_key_data(key[i], &cmd->cmd[cmd_len], ctx, oid);
+		if (status != kStatus_SSS_Success)
+			return kStatus_SSS_Fail;
+
+		memcpy(&cmd->kcv[kcv_len],
+		       &cmd->cmd[cmd_len + 3 + AES_KEY_LEN_nBYTE + 1],
+		       CRYPTO_KEY_CHECK_LEN);
+
+		cmd_len += 3 + AES_KEY_LEN_nBYTE + 1 + CRYPTO_KEY_CHECK_LEN;
+		kcv_len += CRYPTO_KEY_CHECK_LEN;
+	}
+
+	cmd->cmd_len = cmd_len;
+	cmd->kcv_len = kcv_len;
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t get_ofid_key(struct se050_scp_key *keys)
+{
+	sss_status_t status = kStatus_SSS_Success;
+	uint32_t id = 0;
+
+	status = get_id_from_ofid(CFG_CORE_SE05X_OEFID, &id);
+	if (status != kStatus_SSS_Success)
+		return status;
+
+	memcpy(keys, &se050_default_keys[id], sizeof(*keys));
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t get_db_key(struct se050_scp_key *keys)
+{
+	if (IS_ENABLED(CFG_CORE_SE05X_SCP03_EARLY)) {
+		/*
+		 * File system access requires the REE or RPMB to be ready to
+		 * respond to RPC calls (memory allocation and so forth).
+		 * TODO.
+		 */
+		return kStatus_SSS_Fail;
+	}
+
+	if (scp03db_read_keys(keys))
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
+
+static sss_status_t get_config_key(struct se050_scp_key *keys __maybe_unused)
+{
+#ifdef CFG_CORE_SE05X_SCP03_CURRENT_DEK
+	struct se050_scp_key current_keys = {
+		.dek = { CFG_CORE_SE05X_SCP03_CURRENT_DEK },
+		.mac = { CFG_CORE_SE05X_SCP03_CURRENT_MAC },
+		.enc = { CFG_CORE_SE05X_SCP03_CURRENT_ENC },
+	};
+
+	memcpy(keys, &current_keys, sizeof(*keys));
+	return kStatus_SSS_Success;
+#else
+	return kStatus_SSS_Fail;
+#endif
+}
+
+sss_status_t se050_scp03_get_keys(struct se050_scp_key *keys)
+{
+	sss_status_t (*get_keys[])(struct se050_scp_key *) = {
+		&get_config_key, &get_db_key, &get_ofid_key,
+	};
+	size_t i = 0;
+
+	for (i = 0; i < ARRAY_SIZE(get_keys); i++)
+		if ((*get_keys[i])(keys) == kStatus_SSS_Success)
+			return kStatus_SSS_Success;
+
+	return kStatus_SSS_Fail;
+}
+
+sss_status_t se050_scp03_put_keys(struct se050_scp_key *keys,
+				  struct se050_scp_key *cur_keys)
+
+{
+	sss_status_t status = kStatus_SSS_Success;
+
+	if (cur_keys) {
+		status = se050_scp03_get_keys(cur_keys);
+		if (status != kStatus_SSS_Success)
+			return status;
+	}
+
+	if (scp03db_write_keys(keys))
+		return kStatus_SSS_Fail;
+
+	return kStatus_SSS_Success;
+}
diff --git a/core/drivers/crypto/se050/adaptors/utils/utils.c b/core/drivers/crypto/se050/adaptors/utils/utils.c
new file mode 100644
index 00000000..34dbdd05
--- /dev/null
+++ b/core/drivers/crypto/se050/adaptors/utils/utils.c
@@ -0,0 +1,168 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <crypto/crypto.h>
+#include <se050.h>
+#include <se050_utils.h>
+#include <string.h>
+#include <util.h>
+
+/* exp: minimal amount of transient memory required to generate an RSA key */
+#define TRANSIENT_MEMORY_THRESHOLD	0x140
+
+#define NBR_OID			((uint32_t)(OID_MAX - OID_MIN))
+#define IS_WATERMARKED(x)	(((x) & WATERMARKED(0)) == WATERMARKED(0))
+
+static void delete_transient_objects(void)
+{
+	Se05xSession_t *ctx = NULL;
+	uint8_t more = kSE05x_MoreIndicator_NA;
+	uint8_t *list = NULL;
+	size_t len = 1024;
+	smStatus_t status  = SM_NOT_OK;
+	uint16_t offset = 0;
+	uint32_t id = 0;
+	size_t i = 0;
+
+	if (!se050_session)
+		return;
+
+	ctx = &se050_session->s_ctx;
+
+	list = calloc(1, len);
+	if (!list)
+		return;
+	do {
+		status = Se05x_API_ReadIDList(ctx, offset, 0xFF, &more,
+					      list, &len);
+		if (status != SM_OK)
+			break;
+
+		offset = len;
+		for (i = 0; i < len; i += 4) {
+			id = (list[i + 0] << (3 * 8)) |
+			     (list[i + 1] << (2 * 8)) |
+			     (list[i + 2] << (1 * 8)) |
+			     (list[i + 3] << (0 * 8));
+
+			if (id >= OID_MAX || id == 0)
+				continue;
+
+			if (id & BIT(0))
+				Se05x_API_DeleteSecureObject(ctx, id);
+		}
+	} while (more == kSE05x_MoreIndicator_MORE);
+
+	free(list);
+}
+
+static sss_status_t generate_oid(sss_key_object_mode_t mode, uint32_t *val)
+{
+	uint32_t oid = OID_MIN;
+	uint32_t random = 0;
+	size_t i = 0;
+
+	for (i = 0; i < NBR_OID; i++) {
+		if (crypto_rng_read(&random, sizeof(random)))
+			return kStatus_SSS_Fail;
+
+		oid = OID_MIN + (random & OID_MAX);
+		if (oid > OID_MAX)
+			continue;
+
+		if (mode == kKeyObject_Mode_Transient)
+			oid |= BIT(0);
+		else
+			oid &= ~BIT(0);
+
+		if (!se050_key_exists(oid, &se050_session->s_ctx)) {
+			*val = oid;
+			return kStatus_SSS_Success;
+		}
+	}
+
+	return kStatus_SSS_Fail;
+}
+
+sss_status_t se050_get_oid(sss_key_object_mode_t mode, uint32_t *val)
+{
+	sss_status_t status = kStatus_SSS_Success;
+	uint16_t mem_t = 0;
+
+	if (!val)
+		return kStatus_SSS_Fail;
+
+	status = se050_get_free_memory(&se050_session->s_ctx, &mem_t,
+				       kSE05x_MemoryType_TRANSIENT_DESELECT);
+	if (status != kStatus_SSS_Success)
+		return kStatus_SSS_Fail;
+
+	if (mem_t < TRANSIENT_MEMORY_THRESHOLD)
+		delete_transient_objects();
+
+	return generate_oid(mode, val);
+}
+
+static uint32_t se050_key(uint64_t key)
+{
+	uint32_t oid = (uint32_t)key;
+
+	if (!IS_WATERMARKED(key))
+		return 0;
+
+	if (oid < OID_MIN || oid > OID_MAX)
+		return 0;
+
+	return oid;
+}
+
+uint32_t se050_rsa_keypair_from_nvm(struct rsa_keypair *key)
+{
+	uint64_t key_id = 0;
+
+	if (!key)
+		return 0;
+
+	if (crypto_bignum_num_bytes(key->d) != sizeof(uint64_t))
+		return 0;
+
+	crypto_bignum_bn2bin(key->d, (uint8_t *)&key_id);
+
+	return se050_key(key_id);
+}
+
+uint64_t se050_generate_private_key(uint32_t oid)
+{
+	return WATERMARKED(oid);
+}
+
+void se050_refcount_init_ctx(uint8_t **cnt)
+{
+	if (!*cnt) {
+		*cnt = calloc(1, sizeof(uint8_t));
+		if (*cnt)
+			**cnt = 1;
+		else
+			EMSG("can't allocate refcount");
+	} else {
+		**cnt = **cnt + 1;
+	}
+}
+
+int se050_refcount_final_ctx(uint8_t *cnt)
+{
+	if (!cnt)
+		return 1;
+
+	if (!*cnt) {
+		free(cnt);
+		return 1;
+	}
+
+	*cnt = *cnt - 1;
+
+	return 0;
+}
diff --git a/core/drivers/crypto/se050/core/cipher.c b/core/drivers/crypto/se050/core/cipher.c
new file mode 100644
index 00000000..07508013
--- /dev/null
+++ b/core/drivers/crypto/se050/core/cipher.c
@@ -0,0 +1,80 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <se050_cipher_algorithms.h>
+#include <crypto/crypto_impl.h>
+#include <drvcrypt.h>
+#include <drvcrypt_cipher.h>
+#include <initcall.h>
+#include <string.h>
+#include <utee_defines.h>
+#include <util.h>
+
+static TEE_Result do_init(struct drvcrypt_cipher_init *dinit)
+{
+	struct crypto_cipher_ctx *ctx = dinit->ctx;
+
+	return ctx->ops->init(dinit->ctx, dinit->encrypt,
+			      dinit->key1.data, dinit->key1.length,
+			      dinit->key2.data, dinit->key2.length,
+			      dinit->iv.data, dinit->iv.length);
+}
+
+static TEE_Result do_update(struct drvcrypt_cipher_update *dupdate)
+{
+	struct crypto_cipher_ctx *ctx = dupdate->ctx;
+
+	return ctx->ops->update(ctx, dupdate->last, dupdate->src.data,
+				dupdate->src.length, dupdate->dst.data);
+}
+
+static void do_final(void *context)
+{
+	struct crypto_cipher_ctx *ctx = context;
+
+	ctx->ops->final(ctx);
+}
+
+static void do_free(void *context)
+{
+	struct crypto_cipher_ctx *ctx = context;
+
+	ctx->ops->free_ctx(ctx);
+}
+
+static void do_copy_state(void *out, void *in)
+{
+	struct crypto_cipher_ctx *dst_ctx = out;
+	struct crypto_cipher_ctx *src_ctx = in;
+
+	src_ctx->ops->copy_state(dst_ctx, src_ctx);
+}
+
+static TEE_Result do_allocate(void **ctx, uint32_t algo)
+{
+	switch (algo) {
+	case TEE_ALG_AES_CTR:
+		return se050_aes_ctr_allocate(ctx);
+	default:
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	}
+}
+
+static struct drvcrypt_cipher driver_cipher = {
+	.alloc_ctx = &do_allocate,
+	.free_ctx = &do_free,
+	.init = &do_init,
+	.update = &do_update,
+	.final = &do_final,
+	.copy_state = &do_copy_state,
+};
+
+static TEE_Result se050_cipher_init(void)
+{
+	return drvcrypt_register_cipher(&driver_cipher);
+}
+
+driver_init_late(se050_cipher_init);
diff --git a/core/drivers/crypto/se050/core/ctr.c b/core/drivers/crypto/se050/core/ctr.c
new file mode 100644
index 00000000..f4a1a5f3
--- /dev/null
+++ b/core/drivers/crypto/se050/core/ctr.c
@@ -0,0 +1,166 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <assert.h>
+#include <crypto/crypto_impl.h>
+#include <se050.h>
+#include <se050_cipher_algorithms.h>
+#include <string.h>
+#include <utee_defines.h>
+#include <util.h>
+
+struct se050_aes_ctr_ctx {
+	struct crypto_cipher_ctx ctx;
+	sss_se05x_symmetric_t aes_ctx;
+	sss_se05x_object_t key_obj;
+	uint8_t *cnt; /* shared reference counter for duplicated ciphers */
+	int nc_off;
+	unsigned char counter[TEE_AES_BLOCK_SIZE];
+	unsigned char block[TEE_AES_BLOCK_SIZE];
+};
+
+static struct se050_aes_ctr_ctx *to_aes_ctr_ctx(struct crypto_cipher_ctx *ctx)
+{
+	return container_of(ctx, struct se050_aes_ctr_ctx, ctx);
+}
+
+static TEE_Result se050_aes_ctr_init(struct crypto_cipher_ctx *ctx,
+				     TEE_OperationMode mode __unused,
+				     const uint8_t *key1,
+				     size_t key1_len,
+				     const uint8_t *key2 __unused,
+				     size_t key2_len __unused,
+				     const uint8_t *iv, size_t iv_len __unused)
+{
+	struct se050_aes_ctr_ctx *c = to_aes_ctr_ctx(ctx);
+	sss_status_t st = kStatus_SSS_Success;
+	uint32_t oid = 0;
+
+	if (c->key_obj.keyId)
+		goto init;
+
+	memcpy(c->counter, iv, sizeof(c->counter));
+
+	st = sss_se05x_key_object_init(&c->key_obj, se050_kstore);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = se050_get_oid(kKeyObject_Mode_Transient, &oid);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = sss_se05x_key_object_allocate_handle(&c->key_obj, oid,
+						  kSSS_KeyPart_Default,
+						  kSSS_CipherType_AES, 0,
+						  kKeyObject_Mode_Transient);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = sss_se05x_key_store_set_key(se050_kstore, &c->key_obj,
+					 key1, key1_len, key1_len * 8, NULL, 0);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = sss_se05x_symmetric_context_init(&c->aes_ctx, se050_session,
+					      &c->key_obj,
+					      kAlgorithm_SSS_AES_ECB,
+					      kMode_SSS_Encrypt);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+init:
+	st = sss_se05x_cipher_init(&c->aes_ctx, (uint8_t *)NULL, 0);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result se050_aes_ctr_update(struct crypto_cipher_ctx *ctx,
+				       bool last_block __unused,
+				       const uint8_t *data, size_t len,
+				       uint8_t *dst)
+{
+	struct se050_aes_ctr_ctx *c = to_aes_ctr_ctx(ctx);
+	sss_status_t st = kStatus_SSS_Success;
+	size_t dst_len = len;
+	int i = 0;
+	int n = 0;
+	int j = 0;
+
+	n = c->nc_off;
+	while (len--) {
+		dst_len = sizeof(c->counter);
+		if (n == 0) {
+			st = sss_se05x_cipher_update(&c->aes_ctx,
+						     c->counter, 16,
+						     c->block, &dst_len);
+			if (st != kStatus_SSS_Success)
+				return TEE_ERROR_BAD_STATE;
+
+			for (i = 16; i > 0; i--)
+				if (++c->counter[i - 1] != 0)
+					break;
+		}
+		j = *data++;
+		*dst++ = (unsigned char)(j ^ c->block[n]);
+		n = (n + 1) & 0x0F;
+	}
+
+	c->nc_off = n;
+	return TEE_SUCCESS;
+}
+
+static void do_final(struct crypto_cipher_ctx *ctx)
+{
+	struct se050_aes_ctr_ctx *c = to_aes_ctr_ctx(ctx);
+
+	if (!se050_refcount_final_ctx(c->cnt)) {
+		memset(c->block, 0, sizeof(c->block));
+		return;
+	}
+
+	if (c->key_obj.keyId)
+		sss_se05x_key_store_erase_key(se050_kstore, &c->key_obj);
+
+	sss_se05x_symmetric_context_free(&c->aes_ctx);
+}
+
+static void do_free(struct crypto_cipher_ctx *ctx)
+{
+	free(to_aes_ctr_ctx(ctx));
+}
+
+static void do_copy_state(struct crypto_cipher_ctx *dst_ctx,
+			  struct crypto_cipher_ctx *src_ctx)
+{
+	struct se050_aes_ctr_ctx *src = to_aes_ctr_ctx(src_ctx);
+	struct se050_aes_ctr_ctx *dst = to_aes_ctr_ctx(dst_ctx);
+
+	se050_refcount_init_ctx(&src->cnt);
+	memcpy(dst, src, sizeof(*dst));
+}
+
+static struct crypto_cipher_ops aes_ctr_ops = {
+	.update = &se050_aes_ctr_update,
+	.copy_state = &do_copy_state,
+	.init = &se050_aes_ctr_init,
+	.free_ctx = &do_free,
+	.final = &do_final,
+};
+
+TEE_Result se050_aes_ctr_allocate(void **ctx)
+{
+	struct se050_aes_ctr_ctx *c = NULL;
+
+	c = calloc(1, sizeof(*c));
+	if (!c)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	c->ctx.ops = &aes_ctr_ops;
+	*ctx = &c->ctx;
+
+	return TEE_SUCCESS;
+}
diff --git a/core/drivers/crypto/se050/core/huk.c b/core/drivers/crypto/se050/core/huk.c
new file mode 100644
index 00000000..4e474e16
--- /dev/null
+++ b/core/drivers/crypto/se050/core/huk.c
@@ -0,0 +1,30 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <kernel/tee_common_otp.h>
+#include <se050.h>
+#include <string.h>
+#include <tee/tee_cryp_utl.h>
+#include <util.h>
+
+int tee_otp_get_die_id(uint8_t *buffer, size_t len)
+{
+	uint8_t se050_huk[SE050_MODULE_UNIQUE_ID_LEN] = { 0 };
+	size_t se050_huk_len = sizeof(se050_huk);
+	sss_status_t status = kStatus_SSS_Fail;
+
+	memset(buffer, 0, len);
+
+	status = sss_se05x_session_prop_get_au8(se050_session,
+						kSSS_SessionProp_UID,
+						se050_huk, &se050_huk_len);
+	if (status != kStatus_SSS_Success)
+		return -1;
+
+	memcpy(buffer, se050_huk, MIN(len, se050_huk_len));
+
+	return 0;
+}
diff --git a/core/drivers/crypto/se050/core/include/se050_cipher_algorithms.h b/core/drivers/crypto/se050/core/include/se050_cipher_algorithms.h
new file mode 100644
index 00000000..2fa3b337
--- /dev/null
+++ b/core/drivers/crypto/se050/core/include/se050_cipher_algorithms.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#ifndef SE050_CIPHER_ALGORITHMS_H_
+#define SE050_CIPHER_ALGORITHMS_H_
+
+#include <tee_api_types.h>
+
+#if defined(CFG_NXP_SE05X_CTR_DRV)
+TEE_Result se050_aes_ctr_allocate(void **ctx);
+#else
+static inline TEE_Result se050_aes_ctr_allocate(void **ctx __unused)
+{
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+#endif
+
+#endif /* SE050_CIPHER_ALGORITHMS_H_ */
diff --git a/core/drivers/crypto/se050/core/rng.c b/core/drivers/crypto/se050/core/rng.c
new file mode 100644
index 00000000..a3e6be35
--- /dev/null
+++ b/core/drivers/crypto/se050/core/rng.c
@@ -0,0 +1,47 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <crypto/crypto.h>
+#include <rng_support.h>
+#include <se050.h>
+#include <tee/tee_cryp_utl.h>
+
+static TEE_Result do_rng_read(void *buf, size_t blen)
+{
+	sss_status_t status = kStatus_SSS_Success;
+	sss_se05x_rng_context_t rng = { };
+
+	sss_se05x_rng_context_init(&rng, se050_session);
+	status = sss_se05x_rng_get_random(&rng, buf, blen);
+	sss_se05x_rng_context_free(&rng);
+
+	if (status != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+void plat_rng_init(void)
+{
+}
+
+TEE_Result crypto_rng_read(void *buf, size_t blen)
+{
+	if (!buf)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return do_rng_read(buf, blen);
+}
+
+uint8_t hw_get_random_byte(void)
+{
+	uint8_t data = 0;
+
+	if (do_rng_read(&data, 1))
+		return 0;
+
+	return data;
+}
diff --git a/core/drivers/crypto/se050/core/rsa.c b/core/drivers/crypto/se050/core/rsa.c
new file mode 100644
index 00000000..ee1792df
--- /dev/null
+++ b/core/drivers/crypto/se050/core/rsa.c
@@ -0,0 +1,684 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <assert.h>
+#include <drvcrypt.h>
+#include <drvcrypt_acipher.h>
+#include <drvcrypt_math.h>
+#include <initcall.h>
+#include <se050.h>
+#include <string.h>
+#include <tee/cache.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee_api_defines_extensions.h>
+
+static uint32_t tee2se050(uint32_t algo)
+{
+	switch (algo) {
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+		return kAlgorithm_SSS_RSASSA_PKCS1_V1_5_SHA1;
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+		return kAlgorithm_SSS_RSASSA_PKCS1_V1_5_SHA224;
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+		return kAlgorithm_SSS_RSASSA_PKCS1_V1_5_SHA256;
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+		return kAlgorithm_SSS_RSASSA_PKCS1_V1_5_SHA384;
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+		return kAlgorithm_SSS_RSASSA_PKCS1_V1_5_SHA512;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+		return kAlgorithm_SSS_RSASSA_PKCS1_PSS_MGF1_SHA1;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+		return kAlgorithm_SSS_RSASSA_PKCS1_PSS_MGF1_SHA224;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+		return kAlgorithm_SSS_RSASSA_PKCS1_PSS_MGF1_SHA256;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+		return kAlgorithm_SSS_RSASSA_PKCS1_PSS_MGF1_SHA384;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		return kAlgorithm_SSS_RSASSA_PKCS1_PSS_MGF1_SHA512;
+	case TEE_ALG_RSAES_PKCS1_V1_5:
+		return kAlgorithm_SSS_RSAES_PKCS1_V1_5_SHA256;
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA1:
+		return kAlgorithm_SSS_RSAES_PKCS1_OAEP_SHA1;
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA224:
+		return kAlgorithm_SSS_RSAES_PKCS1_OAEP_SHA224;
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256:
+		return kAlgorithm_SSS_RSAES_PKCS1_OAEP_SHA256;
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA384:
+		return kAlgorithm_SSS_RSAES_PKCS1_OAEP_SHA384;
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA512:
+		return kAlgorithm_SSS_RSAES_PKCS1_OAEP_SHA512;
+	case TEE_ALG_RSA_NOPAD:
+		return kAlgorithm_SSS_RSASSA_NO_PADDING;
+#ifdef CFG_CRYPTO_RSASSA_NA1
+	case TEE_ALG_RSASSA_PKCS1_V1_5:
+		return kAlgorithm_SSS_RSASSA_PKCS1_V1_5_NO_HASH;
+#endif
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5SHA1:
+	default:
+		return kAlgorithm_None;
+	}
+}
+
+static bool bn_alloc_max(struct bignum **s)
+{
+	*s = crypto_bignum_allocate(4096);
+
+	return *s;
+}
+
+static TEE_Result set_binary_data(struct bignum *b, uint8_t **p, size_t *len)
+{
+	*len = crypto_bignum_num_bytes(b);
+	if (*len) {
+		*p = (uint8_t *)calloc(1, *len);
+		if (!*p)
+			return TEE_ERROR_OUT_OF_MEMORY;
+		crypto_bignum_bn2bin(b, *p);
+	}
+	return TEE_SUCCESS;
+}
+
+static TEE_Result se050_inject_public_key(sss_se05x_object_t *k_object,
+					  struct rsa_public_key *key)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	struct se050_rsa_keypub key_bin = { };
+	uint32_t oid = 0;
+
+	st = sss_se05x_key_object_init(k_object, se050_kstore);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	st = se050_get_oid(kKeyObject_Mode_Persistent, &oid);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = sss_se05x_key_object_allocate_handle(k_object, oid,
+						  kSSS_KeyPart_Public,
+						  kSSS_CipherType_RSA, 0,
+						  kKeyObject_Mode_Persistent);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	set_binary_data(key->e, &key_bin.e, &key_bin.e_len);
+	set_binary_data(key->n, &key_bin.n, &key_bin.n_len);
+	st = se050_key_store_set_rsa_key_bin(se050_kstore, k_object, NULL,
+					     &key_bin, key_bin.n_len * 8);
+	free(key_bin.n);
+	free(key_bin.e);
+
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, k_object);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result se050_inject_keypair(sss_se05x_object_t *k_object,
+				       struct rsa_keypair *key)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	struct se050_rsa_keypair key_bin = { };
+	uint32_t key_id = 0;
+	uint32_t oid = 0;
+
+	st = sss_se05x_key_object_init(k_object, se050_kstore);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	key_id = se050_rsa_keypair_from_nvm(key);
+	if (key_id) {
+		st = sss_se05x_key_object_get_handle(k_object, key_id);
+		if (st != kStatus_SSS_Success)
+			return TEE_ERROR_BAD_PARAMETERS;
+		return TEE_SUCCESS;
+	}
+
+	st = se050_get_oid(kKeyObject_Mode_Transient, &oid);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = sss_se05x_key_object_allocate_handle(k_object, oid,
+						  kSSS_KeyPart_Pair,
+						  kSSS_CipherType_RSA, 0,
+						  kKeyObject_Mode_Transient);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	set_binary_data(key->e, &key_bin.e, &key_bin.e_len);
+	set_binary_data(key->d, &key_bin.d, &key_bin.d_len);
+	set_binary_data(key->n, &key_bin.n, &key_bin.n_len);
+	set_binary_data(key->p, &key_bin.p, &key_bin.p_len);
+	set_binary_data(key->q, &key_bin.q, &key_bin.q_len);
+	set_binary_data(key->qp, &key_bin.qp, &key_bin.qp_len);
+	set_binary_data(key->dp, &key_bin.dp, &key_bin.dp_len);
+	set_binary_data(key->dq, &key_bin.dq, &key_bin.dq_len);
+	st = se050_key_store_set_rsa_key_bin(se050_kstore, k_object,
+					     &key_bin, NULL,
+					     crypto_bignum_num_bits(key->n));
+	free(key_bin.e);
+	free(key_bin.d);
+	free(key_bin.n);
+	free(key_bin.p);
+	free(key_bin.q);
+	free(key_bin.qp);
+	free(key_bin.dp);
+	free(key_bin.dq);
+
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, k_object);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result decrypt_es(uint32_t algo, struct rsa_keypair *key,
+			     const uint8_t *src, size_t src_len,
+			     uint8_t *dst, size_t *dst_len)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_asymmetric_t ctx = { };
+	sss_se05x_object_t kobject = { };
+	TEE_Result res = TEE_SUCCESS;
+
+	res = se050_inject_keypair(&kobject, key);
+	if (res)
+		return res;
+
+	st = sss_se05x_asymmetric_context_init(&ctx, se050_session, &kobject,
+					       tee2se050(algo),
+					       kMode_SSS_Decrypt);
+	if (st != kStatus_SSS_Success) {
+		if (!se050_rsa_keypair_from_nvm(key))
+			sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	st = sss_se05x_asymmetric_decrypt(&ctx, src, src_len, dst, dst_len);
+	if (st != kStatus_SSS_Success)
+		res = TEE_ERROR_BAD_PARAMETERS;
+
+	if (!se050_rsa_keypair_from_nvm(key))
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+
+	sss_se05x_asymmetric_context_free(&ctx);
+
+	return res;
+}
+
+static TEE_Result decrypt_nopad(struct rsa_keypair *key, const uint8_t *src,
+				size_t src_len, uint8_t *dst, size_t *dst_len)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_asymmetric_t ctx = { };
+	sss_se05x_object_t kobject = { };
+	TEE_Result res = TEE_SUCCESS;
+	uint8_t *buf = NULL;
+	size_t offset = 0;
+	size_t blen = 0;
+	size_t rsa_len = 0;
+
+	res = se050_inject_keypair(&kobject, key);
+	if (res)
+		return res;
+
+	st = sss_se05x_asymmetric_context_init(&ctx, se050_session, &kobject,
+					       kAlgorithm_SSS_RSASSA_NO_PADDING,
+					       kMode_SSS_Decrypt);
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	blen = CFG_CORE_BIGNUM_MAX_BITS / 8;
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	rsa_len = crypto_bignum_num_bytes(key->n);
+	memset(buf, 0, blen);
+	memcpy(buf + rsa_len - src_len, src, src_len);
+
+	st = sss_se05x_asymmetric_decrypt(&ctx, buf, src_len, buf, &blen);
+	if (st != kStatus_SSS_Success) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	offset = 0;
+	while ((offset < rsa_len - 1) && (buf[offset] == 0))
+		offset++;
+
+	if (*dst_len < rsa_len - offset) {
+		*dst_len = rsa_len - offset;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+	*dst_len = rsa_len - offset;
+	memcpy(dst, (char *)buf + offset, *dst_len);
+out:
+	free(buf);
+	if (!se050_rsa_keypair_from_nvm(key))
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+	sss_se05x_asymmetric_context_free(&ctx);
+
+	return res;
+}
+
+static TEE_Result encrypt_es(uint32_t algo, struct rsa_public_key *key,
+			     const uint8_t *src, size_t src_len,
+			     uint8_t *dst, size_t *dst_len)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_asymmetric_t ctx = { };
+	sss_se05x_object_t kobject = { };
+	TEE_Result res = TEE_SUCCESS;
+
+	if (*dst_len < crypto_bignum_num_bytes(key->n)) {
+		*dst_len = crypto_bignum_num_bytes(key->n);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	if (se050_inject_public_key(&kobject, key))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	st = sss_se05x_asymmetric_context_init(&ctx, se050_session, &kobject,
+					       tee2se050(algo),
+					       kMode_SSS_Encrypt);
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	st = sss_se05x_asymmetric_encrypt(&ctx, src, src_len, dst, dst_len);
+	if (st != kStatus_SSS_Success)
+		res = TEE_ERROR_BAD_PARAMETERS;
+
+	sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+	sss_se05x_asymmetric_context_free(&ctx);
+
+	return res;
+}
+
+static TEE_Result encrypt_nopad(struct rsa_public_key *key, const uint8_t *src,
+				size_t src_len, uint8_t *dst, size_t *dst_len)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_asymmetric_t ctx = { };
+	sss_se05x_object_t kobject = { };
+	TEE_Result res = TEE_SUCCESS;
+	uint8_t *buf = NULL;
+	size_t offset = 0;
+	size_t blen = 0;
+	size_t rsa_len = 0;
+
+	if (se050_inject_public_key(&kobject, key))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	st = sss_se05x_asymmetric_context_init(&ctx, se050_session, &kobject,
+					       kAlgorithm_SSS_RSASSA_NO_PADDING,
+					       kMode_SSS_Encrypt);
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	blen = CFG_CORE_BIGNUM_MAX_BITS / 8;
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	rsa_len = crypto_bignum_num_bytes(key->n);
+	memset(buf, 0, blen);
+	memcpy(buf + rsa_len - src_len, src, src_len);
+
+	st = sss_se05x_asymmetric_encrypt(&ctx, buf, src_len, buf, &blen);
+	if (st != kStatus_SSS_Success) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	offset = 0;
+	while ((offset < rsa_len - 1) && (buf[offset] == 0))
+		offset++;
+
+	if (*dst_len < rsa_len - offset) {
+		*dst_len = rsa_len - offset;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	*dst_len = rsa_len - offset;
+	memcpy(dst, buf + offset, *dst_len);
+out:
+	free(buf);
+	sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+	sss_se05x_asymmetric_context_free(&ctx);
+
+	return res;
+}
+
+static TEE_Result sign_ssa(uint32_t algo, struct rsa_keypair *key,
+			   const uint8_t *msg, size_t msg_len,
+			   uint8_t *sig, size_t *sig_len)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_asymmetric_t ctx = { };
+	sss_se05x_object_t kobject = { };
+	TEE_Result res = TEE_SUCCESS;
+
+	if (*sig_len < crypto_bignum_num_bytes(key->n)) {
+		*sig_len = crypto_bignum_num_bytes(key->n);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	res = se050_inject_keypair(&kobject, key);
+	if (res)
+		return res;
+
+	st = sss_se05x_asymmetric_context_init(&ctx, se050_session, &kobject,
+					       tee2se050(algo), kMode_SSS_Sign);
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	st = sss_se05x_asymmetric_sign_digest(&ctx, (uint8_t *)msg, msg_len,
+					      sig, sig_len);
+	if (st != kStatus_SSS_Success)
+		res = TEE_ERROR_BAD_PARAMETERS;
+
+	if (!se050_rsa_keypair_from_nvm(key))
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+
+	sss_se05x_asymmetric_context_free(&ctx);
+
+	return res;
+}
+
+static TEE_Result verify_ssa(uint32_t algo, struct rsa_public_key *key,
+			     const uint8_t *msg, size_t msg_len,
+			     const uint8_t *sig, size_t sig_len)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_asymmetric_t ctx = { };
+	sss_se05x_object_t kobject = { };
+	TEE_Result res = TEE_SUCCESS;
+
+	if (se050_inject_public_key(&kobject, key))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	st = sss_se05x_asymmetric_context_init(&ctx, se050_session, &kobject,
+					       tee2se050(algo),
+					       kMode_SSS_Verify);
+	if (st != kStatus_SSS_Success) {
+		sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	st = sss_se05x_asymmetric_verify_digest(&ctx, (uint8_t *)msg, msg_len,
+						(uint8_t *)sig, sig_len);
+	if (st != kStatus_SSS_Success)
+		res = TEE_ERROR_SIGNATURE_INVALID;
+
+	sss_se05x_key_store_erase_key(se050_kstore, &kobject);
+	sss_se05x_asymmetric_context_free(&ctx);
+
+	return res;
+}
+
+static TEE_Result do_alloc_keypair(struct rsa_keypair *s,
+				   size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->e))
+		return TEE_ERROR_OUT_OF_MEMORY;
+	if (!bn_alloc_max(&s->d))
+		goto err;
+	if (!bn_alloc_max(&s->n))
+		goto err;
+	if (!bn_alloc_max(&s->p))
+		goto err;
+	if (!bn_alloc_max(&s->q))
+		goto err;
+	if (!bn_alloc_max(&s->qp))
+		goto err;
+	if (!bn_alloc_max(&s->dp))
+		goto err;
+	if (!bn_alloc_max(&s->dq))
+		goto err;
+
+	return TEE_SUCCESS;
+err:
+	crypto_bignum_free(s->e);
+	crypto_bignum_free(s->d);
+	crypto_bignum_free(s->n);
+	crypto_bignum_free(s->p);
+	crypto_bignum_free(s->q);
+	crypto_bignum_free(s->qp);
+	crypto_bignum_free(s->dp);
+	crypto_bignum_free(s->dq);
+
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static TEE_Result do_alloc_publickey(struct rsa_public_key *s,
+				     size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->e))
+		return TEE_ERROR_OUT_OF_MEMORY;
+	if (!bn_alloc_max(&s->n)) {
+		crypto_bignum_free(s->e);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static void do_free_publickey(struct rsa_public_key *s)
+{
+	if (s) {
+		crypto_bignum_free(s->n);
+		crypto_bignum_free(s->e);
+	}
+}
+
+static void do_free_keypair(struct rsa_keypair *s)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_object_t k_object = { };
+	uint32_t key_id;
+
+	if (!s)
+		return;
+
+	key_id = se050_rsa_keypair_from_nvm(s);
+	if (key_id) {
+		st = sss_se05x_key_object_get_handle(&k_object, key_id);
+		if (st == kStatus_SSS_Success)
+			sss_se05x_key_store_erase_key(se050_kstore, &k_object);
+	}
+
+	crypto_bignum_free(s->e);
+	crypto_bignum_free(s->d);
+	crypto_bignum_free(s->n);
+	crypto_bignum_free(s->p);
+	crypto_bignum_free(s->q);
+	crypto_bignum_free(s->qp);
+	crypto_bignum_free(s->dp);
+	crypto_bignum_free(s->dq);
+}
+
+static TEE_Result do_gen_keypair(struct rsa_keypair *key, size_t kb)
+{
+	sss_status_t st = kStatus_SSS_Fail;
+	sss_se05x_object_t k_object = { };
+	uint32_t oid = 0;
+	uint64_t kid = 0;
+	uint8_t k[2048] = { 0 };
+	uint8_t *n = NULL;
+	uint8_t *e = NULL;
+	size_t n_len = 0;
+	size_t e_len = 0;
+	size_t k_len = sizeof(k);
+
+	st = sss_se05x_key_object_init(&k_object, se050_kstore);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	st = se050_get_oid(kKeyObject_Mode_Persistent, &oid);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	st = sss_se05x_key_object_allocate_handle(&k_object, oid,
+						  kSSS_KeyPart_Pair,
+						  kSSS_CipherType_RSA, 0,
+						  kKeyObject_Mode_Persistent);
+	if (st != kStatus_SSS_Success)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	st = sss_se05x_key_store_generate_key(se050_kstore, &k_object, kb,
+					      &se050_asym_policy);
+	if (st != kStatus_SSS_Success)
+		goto error;
+
+	st = sss_se05x_key_store_get_key(se050_kstore, &k_object, k, &k_len,
+					 &kb);
+	if (st != kStatus_SSS_Success)
+		goto error;
+
+	st = sss_util_asn1_rsa_parse_public(k, k_len, &n, &n_len, &e, &e_len);
+	if (st != kStatus_SSS_Success)
+		goto error;
+
+	crypto_bignum_bin2bn(n, n_len, key->n);
+	crypto_bignum_bin2bn(e, e_len, key->e);
+	kid = se050_generate_private_key(oid);
+	crypto_bignum_bin2bn((uint8_t *)&kid, sizeof(kid), (key->d));
+	crypto_bignum_bin2bn((uint8_t *)&oid, sizeof(oid), key->p);
+	crypto_bignum_bin2bn((uint8_t *)&oid, sizeof(oid), key->q);
+	crypto_bignum_bin2bn((uint8_t *)&oid, sizeof(oid), key->qp);
+	crypto_bignum_bin2bn((uint8_t *)&oid, sizeof(oid), key->dp);
+	crypto_bignum_bin2bn((uint8_t *)&oid, sizeof(oid), key->dq);
+	free(n);
+	free(e);
+
+	return TEE_SUCCESS;
+error:
+	sss_se05x_key_store_erase_key(se050_kstore, &k_object);
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+static TEE_Result do_encrypt(struct drvcrypt_rsa_ed *rsa_data)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+
+	switch (rsa_data->rsa_id) {
+	case DRVCRYPT_RSA_NOPAD:
+	case DRVCRYPT_RSASSA_PKCS_V1_5:
+	case DRVCRYPT_RSASSA_PSS:
+		ret = encrypt_nopad(rsa_data->key.key,
+				    rsa_data->message.data,
+				    rsa_data->message.length,
+				    rsa_data->cipher.data,
+				    &rsa_data->cipher.length);
+		break;
+
+	case DRVCRYPT_RSA_PKCS_V1_5:
+		ret = encrypt_es(rsa_data->hash_algo,
+				 rsa_data->key.key,
+				 rsa_data->message.data,
+				 rsa_data->message.length,
+				 rsa_data->cipher.data,
+				 &rsa_data->cipher.length);
+		break;
+
+	case DRVCRYPT_RSA_OAEP:
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static TEE_Result do_decrypt(struct drvcrypt_rsa_ed *rsa_data)
+{
+	TEE_Result ret = TEE_ERROR_NOT_IMPLEMENTED;
+
+	switch (rsa_data->rsa_id) {
+	case DRVCRYPT_RSA_NOPAD:
+	case DRVCRYPT_RSASSA_PKCS_V1_5:
+	case DRVCRYPT_RSASSA_PSS:
+		ret = decrypt_nopad(rsa_data->key.key,
+				    rsa_data->message.data,
+				    rsa_data->message.length,
+				    rsa_data->cipher.data,
+				    &rsa_data->cipher.length);
+		break;
+
+	case DRVCRYPT_RSA_PKCS_V1_5:
+		ret = decrypt_es(rsa_data->hash_algo,
+				 rsa_data->key.key,
+				 rsa_data->message.data,
+				 rsa_data->message.length,
+				 rsa_data->cipher.data,
+				 &rsa_data->cipher.length);
+		break;
+
+	case DRVCRYPT_RSA_OAEP:
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static TEE_Result do_ssa_sign(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	return sign_ssa(ssa_data->algo,
+			ssa_data->key.key,
+			ssa_data->message.data,
+			ssa_data->message.length,
+			ssa_data->signature.data,
+			&ssa_data->signature.length);
+}
+
+static TEE_Result do_ssa_verify(struct drvcrypt_rsa_ssa *ssa_data)
+{
+	return verify_ssa(ssa_data->algo,
+			ssa_data->key.key,
+			ssa_data->message.data,
+			ssa_data->message.length,
+			ssa_data->signature.data,
+			ssa_data->signature.length);
+}
+
+static const 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)
+{
+	return drvcrypt_register_rsa(&driver_rsa);
+}
+
+driver_init_late(rsa_init);
diff --git a/core/drivers/crypto/se050/core/scp03.c b/core/drivers/crypto/se050/core/scp03.c
new file mode 100644
index 00000000..0422f4d4
--- /dev/null
+++ b/core/drivers/crypto/se050/core/scp03.c
@@ -0,0 +1,30 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <crypto/crypto.h>
+#include <config.h>
+#include <se050.h>
+
+TEE_Result crypto_enable_scp03(unsigned int rotate_keys)
+{
+	sss_status_t status = kStatus_SSS_Success;
+
+	status = se050_enable_scp03(se050_session);
+	if (status != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	if (rotate_keys) {
+		if (IS_ENABLED(CFG_CORE_SE05X_SCP03_PROVISION)) {
+			status = se050_rotate_scp03_keys(&se050_ctx);
+			if (status != kStatus_SSS_Success)
+				return TEE_ERROR_GENERIC;
+
+			return TEE_SUCCESS;
+		}
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+	return TEE_SUCCESS;
+}
diff --git a/core/drivers/crypto/se050/core/storage.c b/core/drivers/crypto/se050/core/storage.c
new file mode 100644
index 00000000..650eb373
--- /dev/null
+++ b/core/drivers/crypto/se050/core/storage.c
@@ -0,0 +1,50 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <crypto/crypto.h>
+#include <se050.h>
+#include <se050_utils.h>
+#include <string.h>
+
+void crypto_storage_obj_del(uint8_t *data, size_t len)
+{
+	sss_status_t status = kStatus_SSS_Success;
+	uint32_t val = SE050_KEY_WATERMARK;
+	sss_se05x_object_t k_object = { };
+	bool found = false;
+	uint8_t *p = data;
+
+	if (!p)
+		return;
+
+	while (len > sizeof(uint64_t) && !found) {
+		if (memcmp(p, &val, sizeof(val)) != 0) {
+			p++;
+			len--;
+			continue;
+		}
+		found = true;
+	}
+
+	if (!found)
+		return;
+
+	p = p - 4;
+	memcpy((void *)&val, p, sizeof(val));
+
+	if (val < OID_MIN || val > OID_MAX)
+		return;
+
+	status = sss_se05x_key_object_init(&k_object, se050_kstore);
+	if (status != kStatus_SSS_Success)
+		return;
+
+	status = sss_se05x_key_object_get_handle(&k_object, val);
+	if (status != kStatus_SSS_Success)
+		return;
+
+	sss_se05x_key_store_erase_key(se050_kstore, &k_object);
+}
diff --git a/core/drivers/crypto/se050/core/sub.mk b/core/drivers/crypto/se050/core/sub.mk
new file mode 100644
index 00000000..31503151
--- /dev/null
+++ b/core/drivers/crypto/se050/core/sub.mk
@@ -0,0 +1,16 @@
+cflags-y += -DAX_EMBEDDED=1
+cflags-y += -DVERBOSE_APDU_LOGS=0
+cflags-y += -DT1oI2C_UM1225_SE050
+cflags-y += -DT1oI2C
+cflags-y += -DSSS_USE_FTR_FILE
+
+incdirs-y += ../adaptors/include
+incdirs-y += include
+
+srcs-y += scp03.c
+srcs-y += storage.c
+srcs-$(CFG_NXP_SE05X_RSA_DRV) += rsa.c
+srcs-$(CFG_NXP_SE05X_CTR_DRV) += ctr.c
+srcs-$(CFG_NXP_SE05X_HUK_DRV) += huk.c
+srcs-$(CFG_NXP_SE05X_RNG_DRV) += rng.c
+srcs-$(CFG_NXP_SE05X_CIPHER_DRV) += cipher.c
diff --git a/core/drivers/crypto/se050/session.c b/core/drivers/crypto/se050/session.c
new file mode 100644
index 00000000..dfe11670
--- /dev/null
+++ b/core/drivers/crypto/se050/session.c
@@ -0,0 +1,85 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (C) Foundries Ltd. 2020 - All Rights Reserved
+ * Author: Jorge Ramirez <jorge@foundries.io>
+ */
+
+#include <config.h>
+#include <initcall.h>
+#include <se050.h>
+
+sss_se05x_key_store_t *se050_kstore;
+sss_se05x_session_t *se050_session;
+struct sss_se05x_ctx se050_ctx;
+
+TEE_Result se050_core_early_init(struct se050_scp_key *keys)
+{
+	sss_status_t status = kStatus_SSS_Success;
+
+	status = se050_session_open(&se050_ctx, keys);
+	if (status != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	if (IS_ENABLED(CFG_CORE_SE05X_INIT_NVM)) {
+		status = se050_factory_reset(&se050_ctx.session.s_ctx);
+		if (status != kStatus_SSS_Success)
+			return TEE_ERROR_GENERIC;
+	}
+
+	if (se050_ctx.session.subsystem == kType_SSS_SubSystem_NONE)
+		return TEE_ERROR_GENERIC;
+
+	status = se050_key_store_and_object_init(&se050_ctx);
+	if (status != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	se050_session = (sss_se05x_session_t *)((void *)&se050_ctx.session);
+	se050_kstore = (sss_se05x_key_store_t *)((void *)&se050_ctx.ks);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result display_info(void)
+{
+	se050_display_board_info(se050_session);
+	/* the session must be closed after accessing board information */
+	sss_se05x_session_close(se050_session);
+	return se050_core_early_init(NULL);
+}
+
+static TEE_Result enable_scp03(void)
+{
+	if (se050_enable_scp03(se050_session) != kStatus_SSS_Success)
+		return TEE_ERROR_GENERIC;
+
+	/*
+	 * Do not provision the keys at this point unless there is guaranteed
+	 * access to trusted storage so the new keys can be written.
+	 *
+	 * This can be done once RPMB is accessible and we can test it
+	 *
+	 * #if defined(CFG_CORE_SE05X_SCP03_PROVISION)
+	 *	if (se050_rotate_scp03_keys(&se050_ctx) != kStatus_SSS_Success)
+	 *		return TEE_ERROR_GENERIC;
+	 * #endif
+	 */
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result se050_early_init(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+
+	ret = se050_core_early_init(NULL);
+
+	if (!ret && IS_ENABLED(CFG_CORE_SE05X_DISPLAY_INFO))
+		ret = display_info();
+
+	if (!ret && IS_ENABLED(CFG_CORE_SE05X_SCP03_EARLY))
+		return enable_scp03();
+
+	return ret;
+}
+
+driver_init(se050_early_init);
diff --git a/core/drivers/crypto/se050/sub.mk b/core/drivers/crypto/se050/sub.mk
new file mode 100644
index 00000000..407dc93a
--- /dev/null
+++ b/core/drivers/crypto/se050/sub.mk
@@ -0,0 +1,16 @@
+core-platform-cflags += "-I${CFG_NXP_SE05X_PLUG_AND_TRUST}/optee_lib/include"
+
+cflags-y += -Wno-error
+cflags-y += -Wno-implicit-function-declaration
+cflags-y += -DAX_EMBEDDED=1
+cflags-y += -DVERBOSE_APDU_LOGS=0
+cflags-y += -DT1oI2C_UM11225
+cflags-y += -DT1oI2C
+cflags-y += -DSSS_USE_FTR_FILE
+
+incdirs-y += adaptors/include
+
+subdirs-y += adaptors
+subdirs-y += core
+
+srcs-y += session.c
diff --git a/core/drivers/crypto/sub.mk b/core/drivers/crypto/sub.mk
index f61f4557..380f074e 100644
--- a/core/drivers/crypto/sub.mk
+++ b/core/drivers/crypto/sub.mk
@@ -3,3 +3,5 @@ global-incdirs-$(CFG_CRYPTO_DRIVER) += crypto_api/include
 subdirs-$(CFG_CRYPTO_DRIVER) += crypto_api
 
 subdirs-$(CFG_NXP_CAAM) += caam
+
+subdirs-$(CFG_NXP_SE05X) += se050
diff --git a/core/include/crypto/crypto.h b/core/include/crypto/crypto.h
index e59836c1..c7b0eae5 100644
--- a/core/include/crypto/crypto.h
+++ b/core/include/crypto/crypto.h
@@ -81,6 +81,10 @@ void crypto_authenc_final(void *ctx);
 void crypto_authenc_free_ctx(void *ctx);
 void crypto_authenc_copy_state(void *dst_ctx, void *src_ctx);
 
+#if defined(CFG_NXP_SE05X)
+TEE_Result crypto_enable_scp03(unsigned int rotate_keys);
+#endif
+
 /* Implementation-defined big numbers */
 
 /*