feat(intel): extending to support SMMU in FCS

This patch is to extend support SMMU in FCS GET_DIGEST, MAC_VERIFY,
ECDSA_SHA2_DATA_SIGNING and ECDSA_SHA2_DATA_SIGNATURE_VERIFY.
It also will change to use asynchronous mailbox send command to improve
fcs_client timing performance.
Increase the SIP_SVC_VERSION_MAJOR because SMMU support is not backward
compatible.
Increase the SIP_SVC_VERSION_MINOR because 8 news function IDs are
introduced.

Signed-off-by: Sieu Mun Tang <sieu.mun.tang@intel.com>
Change-Id: I15e619e246531b065451f9b201646f3c50e26307
diff --git a/plat/intel/soc/agilex/bl31_plat_setup.c b/plat/intel/soc/agilex/bl31_plat_setup.c
index b1b9514..26ed7ef 100644
--- a/plat/intel/soc/agilex/bl31_plat_setup.c
+++ b/plat/intel/soc/agilex/bl31_plat_setup.c
@@ -17,6 +17,7 @@
 #include "ccu/ncore_ccu.h"
 #include "socfpga_mailbox.h"
 #include "socfpga_private.h"
+#include "socfpga_sip_svc.h"
 
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
@@ -35,6 +36,25 @@
 		return NULL;
 }
 
+void setup_smmu_secure_context(void)
+{
+	/*
+	 * Program SCR0 register (0xFA000000)
+	 * to set SMCFCFG bit[21] to 0x1 which raise stream match conflict fault
+	 * to set CLIENTPD bit[0] to 0x0 which enables SMMU for secure context
+	 */
+	mmio_write_32(0xFA000000, 0x00200000);
+
+	/*
+	 * Program SCR1 register (0xFA000004)
+	 * to set NSNUMSMRGO bit[14:8] to 0x4 which stream mapping register
+	 * for non-secure context and the rest will be secure context
+	 * to set NSNUMCBO bit[5:0] to 0x4 which allocate context bank
+	 * for non-secure context and the rest will be secure context
+	 */
+	mmio_write_32(0xFA000004, 0x00000404);
+}
+
 void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				u_register_t arg2, u_register_t arg3)
 {
@@ -109,6 +129,7 @@
 	gicv2_distif_init();
 	gicv2_pcpu_distif_init();
 	gicv2_cpuif_enable();
+	setup_smmu_secure_context();
 
 	/* Signal secondary CPUs to jump to BL31 (BL2 = U-boot SPL) */
 	mmio_write_64(PLAT_CPU_RELEASE_ADDR,
diff --git a/plat/intel/soc/common/include/socfpga_fcs.h b/plat/intel/soc/common/include/socfpga_fcs.h
index 893551d..c804313 100644
--- a/plat/intel/soc/common/include/socfpga_fcs.h
+++ b/plat/intel/soc/common/include/socfpga_fcs.h
@@ -235,6 +235,11 @@
 				uint32_t src_addr, uint32_t src_size,
 				uint64_t dst_addr, uint32_t *dst_size,
 				uint8_t is_finalised, uint32_t *mbox_error);
+int intel_fcs_get_digest_smmu_update_finalize(uint32_t session_id, uint32_t context_id,
+				uint32_t src_addr, uint32_t src_size,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id);
 
 int intel_fcs_mac_verify_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
@@ -244,6 +249,11 @@
 				uint64_t dst_addr, uint32_t *dst_size,
 				uint32_t data_size, uint8_t is_finalised,
 				uint32_t *mbox_error);
+int intel_fcs_mac_verify_smmu_update_finalize(uint32_t session_id, uint32_t context_id,
+				uint32_t src_addr, uint32_t src_size,
+				uint64_t dst_addr, uint32_t *dst_size,
+				uint32_t data_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id);
 
 int intel_fcs_ecdsa_hash_sign_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
@@ -270,6 +280,11 @@
 				uint32_t src_size, uint64_t dst_addr,
 				uint32_t *dst_size, uint8_t is_finalised,
 				uint32_t *mbox_error);
+int intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id);
 
 int intel_fcs_ecdsa_sha2_data_sig_verify_init(uint32_t session_id,
 				uint32_t context_id, uint32_t key_id,
@@ -280,6 +295,12 @@
 				uint32_t src_size, uint64_t dst_addr,
 				uint32_t *dst_size, uint32_t data_size,
 				uint8_t is_finalised, uint32_t *mbox_error);
+int intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t data_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id);
 
 int intel_fcs_ecdsa_get_pubkey_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index 0803eb5..21169f7 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -86,49 +86,57 @@
 #define INTEL_SIP_SMC_GET_USERCODE				0xC200003D
 
 /* FPGA Crypto Services */
-#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER				0xC200005A
-#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER_EXT			0x4200008F
-#define INTEL_SIP_SMC_FCS_CRYPTION				0x4200005B
-#define INTEL_SIP_SMC_FCS_CRYPTION_EXT				0xC2000090
-#define INTEL_SIP_SMC_FCS_SERVICE_REQUEST			0x4200005C
-#define INTEL_SIP_SMC_FCS_SEND_CERTIFICATE			0x4200005D
-#define INTEL_SIP_SMC_FCS_GET_PROVISION_DATA			0x4200005E
-#define INTEL_SIP_SMC_FCS_CNTR_SET_PREAUTH			0xC200005F
-#define INTEL_SIP_SMC_FCS_PSGSIGMA_TEARDOWN			0xC2000064
-#define INTEL_SIP_SMC_FCS_CHIP_ID				0xC2000065
-#define INTEL_SIP_SMC_FCS_ATTESTATION_SUBKEY			0xC2000066
-#define INTEL_SIP_SMC_FCS_ATTESTATION_MEASUREMENTS		0xC2000067
-#define INTEL_SIP_SMC_FCS_GET_ATTESTATION_CERT			0xC2000068
-#define INTEL_SIP_SMC_FCS_CREATE_CERT_ON_RELOAD			0xC2000069
-#define INTEL_SIP_SMC_FCS_OPEN_CS_SESSION			0xC200006E
-#define INTEL_SIP_SMC_FCS_CLOSE_CS_SESSION			0xC200006F
-#define INTEL_SIP_SMC_FCS_IMPORT_CS_KEY				0x42000070
-#define INTEL_SIP_SMC_FCS_EXPORT_CS_KEY				0xC2000071
-#define INTEL_SIP_SMC_FCS_REMOVE_CS_KEY				0xC2000072
-#define INTEL_SIP_SMC_FCS_GET_CS_KEY_INFO			0xC2000073
-#define INTEL_SIP_SMC_FCS_AES_CRYPT_INIT			0xC2000074
-#define INTEL_SIP_SMC_FCS_AES_CRYPT_UPDATE			0x42000075
-#define INTEL_SIP_SMC_FCS_AES_CRYPT_FINALIZE			0x42000076
-#define INTEL_SIP_SMC_FCS_GET_DIGEST_INIT			0xC2000077
-#define INTEL_SIP_SMC_FCS_GET_DIGEST_UPDATE			0xC2000078
-#define INTEL_SIP_SMC_FCS_GET_DIGEST_FINALIZE			0xC2000079
-#define INTEL_SIP_SMC_FCS_MAC_VERIFY_INIT			0xC200007A
-#define INTEL_SIP_SMC_FCS_MAC_VERIFY_UPDATE			0xC200007B
-#define INTEL_SIP_SMC_FCS_MAC_VERIFY_FINALIZE			0xC200007C
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_INIT			0xC200007D
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_FINALIZE		0xC200007F
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_INIT		0xC2000080
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_UPDATE		0xC2000081
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_FINALIZE		0xC2000082
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_INIT		0xC2000083
-#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_FINALIZE	0xC2000085
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_INIT	0xC2000086
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_UPDATE	0xC2000087
-#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_FINALIZE	0xC2000088
-#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_INIT			0xC2000089
-#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_FINALIZE		0xC200008B
-#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_INIT			0xC200008C
-#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_FINALIZE			0xC200008E
+#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER					0xC200005A
+#define INTEL_SIP_SMC_FCS_RANDOM_NUMBER_EXT				0x4200008F
+#define INTEL_SIP_SMC_FCS_CRYPTION					0x4200005B
+#define INTEL_SIP_SMC_FCS_CRYPTION_EXT					0xC2000090
+#define INTEL_SIP_SMC_FCS_SERVICE_REQUEST				0x4200005C
+#define INTEL_SIP_SMC_FCS_SEND_CERTIFICATE				0x4200005D
+#define INTEL_SIP_SMC_FCS_GET_PROVISION_DATA				0x4200005E
+#define INTEL_SIP_SMC_FCS_CNTR_SET_PREAUTH				0xC200005F
+#define INTEL_SIP_SMC_FCS_PSGSIGMA_TEARDOWN				0xC2000064
+#define INTEL_SIP_SMC_FCS_CHIP_ID					0xC2000065
+#define INTEL_SIP_SMC_FCS_ATTESTATION_SUBKEY				0xC2000066
+#define INTEL_SIP_SMC_FCS_ATTESTATION_MEASUREMENTS			0xC2000067
+#define INTEL_SIP_SMC_FCS_GET_ATTESTATION_CERT				0xC2000068
+#define INTEL_SIP_SMC_FCS_CREATE_CERT_ON_RELOAD				0xC2000069
+#define INTEL_SIP_SMC_FCS_OPEN_CS_SESSION				0xC200006E
+#define INTEL_SIP_SMC_FCS_CLOSE_CS_SESSION				0xC200006F
+#define INTEL_SIP_SMC_FCS_IMPORT_CS_KEY					0x42000070
+#define INTEL_SIP_SMC_FCS_EXPORT_CS_KEY					0xC2000071
+#define INTEL_SIP_SMC_FCS_REMOVE_CS_KEY					0xC2000072
+#define INTEL_SIP_SMC_FCS_GET_CS_KEY_INFO				0xC2000073
+#define INTEL_SIP_SMC_FCS_AES_CRYPT_INIT				0xC2000074
+#define INTEL_SIP_SMC_FCS_AES_CRYPT_UPDATE				0x42000075
+#define INTEL_SIP_SMC_FCS_AES_CRYPT_FINALIZE				0x42000076
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_INIT				0xC2000077
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_UPDATE				0xC2000078
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_FINALIZE				0xC2000079
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_UPDATE			0x42000091
+#define INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_FINALIZE			0x42000092
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_INIT				0xC200007A
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_UPDATE				0xC200007B
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_FINALIZE				0xC200007C
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_UPDATE			0x42000093
+#define INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_FINALIZE			0x42000094
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_INIT				0xC200007D
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_FINALIZE			0xC200007F
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_INIT			0xC2000080
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_UPDATE			0xC2000081
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_FINALIZE			0xC2000082
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_UPDATE		0x42000095
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_FINALIZE		0x42000096
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_INIT			0xC2000083
+#define INTEL_SIP_SMC_FCS_ECDSA_HASH_SIG_VERIFY_FINALIZE		0xC2000085
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_INIT		0xC2000086
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_UPDATE		0xC2000087
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_FINALIZE		0xC2000088
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_UPDATE	0x42000097
+#define INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_FINALIZE	0x42000098
+#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_INIT				0xC2000089
+#define INTEL_SIP_SMC_FCS_ECDSA_GET_PUBKEY_FINALIZE			0xC200008B
+#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_INIT				0xC200008C
+#define INTEL_SIP_SMC_FCS_ECDH_REQUEST_FINALIZE				0xC200008E
 
 #define INTEL_SIP_SMC_FCS_SHA_MODE_MASK				0xF
 #define INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK			0xF
@@ -166,8 +174,14 @@
 #define SIP_SVC_VERSION						0x8200ff03
 
 /* SiP Service Calls version numbers */
-#define SIP_SVC_VERSION_MAJOR					1
-#define SIP_SVC_VERSION_MINOR					0
+/*
+ * Increase if there is any backward compatibility impact
+ */
+#define SIP_SVC_VERSION_MAJOR					2
+/*
+ * Increase if there is new SMC function ID being added
+ */
+#define SIP_SVC_VERSION_MINOR					1
 
 
 /* Structure Definitions */
diff --git a/plat/intel/soc/common/sip/socfpga_sip_fcs.c b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
index facee0f..84ec53c 100644
--- a/plat/intel/soc/common/sip/socfpga_sip_fcs.c
+++ b/plat/intel/soc/common/sip/socfpga_sip_fcs.c
@@ -946,6 +946,104 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_get_digest_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id)
+{
+	int status;
+	uint32_t i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t resp_len;
+	uint32_t payload[FCS_GET_DIGEST_CMD_MAX_WORD_SIZE] = {0U};
+
+	/* Source data must be 8 bytes aligned */
+	if (dst_size == NULL || mbox_error == NULL ||
+		!is_8_bytes_aligned(src_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha_get_digest_param.session_id != session_id ||
+	    fcs_sha_get_digest_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		 !is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+
+	if (fcs_sha_get_digest_param.is_updated) {
+		fcs_sha_get_digest_param.crypto_param_size = 0;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_INIT;
+	}
+
+	if (is_finalised != 0U) {
+		flag |=  FCS_CS_FIELD_FLAG_FINALIZE;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha_get_digest_param.is_updated = 1;
+	}
+
+	crypto_header = ((flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			(fcs_sha_get_digest_param.crypto_param_size &
+			FCS_CS_FIELD_SIZE_MASK));
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha_get_digest_param.session_id;
+	i++;
+	payload[i] = fcs_sha_get_digest_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha_get_digest_param.key_id;
+		i++;
+		/* Crypto parameters */
+		payload[i] = fcs_sha_get_digest_param.crypto_param
+				& INTEL_SIP_SMC_FCS_SHA_MODE_MASK;
+		payload[i] |= ((fcs_sha_get_digest_param.crypto_param
+				>> INTEL_SIP_SMC_FCS_DIGEST_SIZE_OFFSET)
+				& INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK)
+				<< FCS_SHA_HMAC_CRYPTO_PARAM_SIZE_OFFSET;
+		i++;
+	}
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = src_size;
+	i++;
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_GET_DIGEST_REQ,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *)&fcs_sha_get_digest_param, 0,
+		sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_mac_verify_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
 				uint64_t param_data, uint32_t *mbox_error)
@@ -1071,6 +1169,127 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_mac_verify_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t data_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id)
+{
+	int status;
+	uint32_t i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t resp_len;
+	uint32_t payload[FCS_MAC_VERIFY_CMD_MAX_WORD_SIZE] = {0U};
+	uintptr_t mac_offset;
+
+	/*
+	 * Source data must be 4 bytes aligned
+	 * User data must be 8 bytes aligned
+	 */
+	if (dst_size == NULL || mbox_error == NULL ||
+		!is_size_4_bytes_aligned(src_size) ||
+		!is_8_bytes_aligned(data_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (data_size > src_size) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha_mac_verify_param.session_id != session_id ||
+		fcs_sha_mac_verify_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+
+	if (fcs_sha_mac_verify_param.is_updated) {
+		fcs_sha_mac_verify_param.crypto_param_size = 0;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_INIT;
+	}
+
+	if (is_finalised) {
+		flag |=  FCS_CS_FIELD_FLAG_FINALIZE;
+	} else {
+		flag |=  FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha_mac_verify_param.is_updated = 1;
+	}
+
+	crypto_header = ((flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			(fcs_sha_mac_verify_param.crypto_param_size &
+			FCS_CS_FIELD_SIZE_MASK));
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha_mac_verify_param.session_id;
+	i++;
+	payload[i] = fcs_sha_mac_verify_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha_mac_verify_param.key_id;
+		i++;
+		/* Crypto parameters */
+		payload[i] = ((fcs_sha_mac_verify_param.crypto_param
+				>> INTEL_SIP_SMC_FCS_DIGEST_SIZE_OFFSET)
+				& INTEL_SIP_SMC_FCS_DIGEST_SIZE_MASK)
+				<< FCS_SHA_HMAC_CRYPTO_PARAM_SIZE_OFFSET;
+		i++;
+	}
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = data_size;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_FINALIZE) {
+		/* Copy mac data to command
+		 * Using dst_addr (physical address) to store mac_offset
+		 * mac_offset = MAC data
+		 */
+		mac_offset = dst_addr;
+		memcpy((uint8_t *) &payload[i], (uint8_t *) mac_offset,
+		src_size - data_size);
+
+		memset((void *)&dst_addr, 0, sizeof(dst_size));
+
+		i += (src_size - data_size) / MBOX_WORD_BYTE;
+	}
+
+	status = mailbox_send_cmd_async(send_id, MBOX_FCS_MAC_VERIFY_REQ,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised) {
+		memset((void *)&fcs_sha_mac_verify_param, 0,
+		sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_ecdsa_hash_sign_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
 				uint64_t param_data, uint32_t *mbox_error)
@@ -1348,6 +1567,99 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint8_t is_finalised,
+				uint32_t *mbox_error, uint32_t *send_id)
+{
+	int status;
+	int i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t payload[FCS_ECDSA_SHA2_DATA_SIGN_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+
+	/* Source data must be 8 bytes aligned */
+	if ((dst_size == NULL) || (mbox_error == NULL ||
+		!is_8_bytes_aligned(src_size))) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha2_data_sign_param.session_id != session_id ||
+		fcs_sha2_data_sign_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+	if (fcs_sha2_data_sign_param.is_updated) {
+		fcs_sha2_data_sign_param.crypto_param_size = 0;
+	} else {
+		flag |= FCS_CS_FIELD_FLAG_INIT;
+	}
+
+	if (is_finalised != 0U) {
+		flag |= FCS_CS_FIELD_FLAG_FINALIZE;
+	} else {
+		flag |= FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha2_data_sign_param.is_updated = 1;
+	}
+	crypto_header = (flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			fcs_sha2_data_sign_param.crypto_param_size;
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha2_data_sign_param.session_id;
+	i++;
+	payload[i] = fcs_sha2_data_sign_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha2_data_sign_param.key_id;
+		/* Crypto parameters */
+		i++;
+		payload[i] = fcs_sha2_data_sign_param.crypto_param
+				& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+		i++;
+	}
+
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = src_size;
+	i++;
+
+	status = mailbox_send_cmd_async(send_id,
+					MBOX_FCS_ECDSA_SHA2_DATA_SIGN_REQ,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *)&fcs_sha2_data_sign_param, 0,
+			sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_ecdsa_sha2_data_sig_verify_init(uint32_t session_id,
 				uint32_t context_id, uint32_t key_id,
 				uint32_t param_size, uint64_t param_data,
@@ -1469,6 +1781,121 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+int intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(uint32_t session_id,
+				uint32_t context_id, uint32_t src_addr,
+				uint32_t src_size, uint64_t dst_addr,
+				uint32_t *dst_size, uint32_t data_size,
+				uint8_t is_finalised, uint32_t *mbox_error,
+				uint32_t *send_id)
+{
+	int status;
+	uint32_t i;
+	uint32_t flag;
+	uint32_t crypto_header;
+	uint32_t payload[FCS_ECDSA_SHA2_DATA_SIG_VERIFY_CMD_MAX_WORD_SIZE] = {0U};
+	uint32_t resp_len;
+	uintptr_t sig_pubkey_offset;
+
+	/*
+	 * Source data must be 4 bytes aligned
+	 * Source addrress must be 8 bytes aligned
+	 * User data must be 8 bytes aligned
+	 */
+	if ((dst_size == NULL) || (mbox_error == NULL) ||
+		!is_size_4_bytes_aligned(src_size) ||
+		!is_8_bytes_aligned(src_addr) ||
+		!is_8_bytes_aligned(data_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (fcs_sha2_data_sig_verify_param.session_id != session_id ||
+		fcs_sha2_data_sig_verify_param.context_id != context_id) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	if (!is_address_in_ddr_range(src_addr, src_size) ||
+		!is_address_in_ddr_range(dst_addr, *dst_size)) {
+		return INTEL_SIP_SMC_STATUS_REJECTED;
+	}
+
+	resp_len = *dst_size / MBOX_WORD_BYTE;
+
+	/* Prepare crypto header */
+	flag = 0;
+	if (fcs_sha2_data_sig_verify_param.is_updated)
+		fcs_sha2_data_sig_verify_param.crypto_param_size = 0;
+	else
+		flag |= FCS_CS_FIELD_FLAG_INIT;
+
+	if (is_finalised != 0U)
+		flag |= FCS_CS_FIELD_FLAG_FINALIZE;
+	else {
+		flag |= FCS_CS_FIELD_FLAG_UPDATE;
+		fcs_sha2_data_sig_verify_param.is_updated = 1;
+	}
+	crypto_header = (flag << FCS_CS_FIELD_FLAG_OFFSET) |
+			fcs_sha2_data_sig_verify_param.crypto_param_size;
+
+	/* Prepare command payload */
+	i = 0;
+	payload[i] = fcs_sha2_data_sig_verify_param.session_id;
+	i++;
+	payload[i] = fcs_sha2_data_sig_verify_param.context_id;
+	i++;
+	payload[i] = crypto_header;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_INIT) {
+		payload[i] = fcs_sha2_data_sig_verify_param.key_id;
+		i++;
+		/* Crypto parameters */
+		payload[i] = fcs_sha2_data_sig_verify_param.crypto_param
+				& INTEL_SIP_SMC_FCS_ECC_ALGO_MASK;
+		i++;
+	}
+
+	/* Data source address and size */
+	payload[i] = src_addr;
+	i++;
+	payload[i] = data_size;
+	i++;
+
+	if ((crypto_header >> FCS_CS_FIELD_FLAG_OFFSET) &
+		FCS_CS_FIELD_FLAG_FINALIZE) {
+		/* Copy mac data to command
+		 * Using dst_addr (physical address) to store sig_pubkey_offset
+		 * sig_pubkey_offset is Signature + Public Key Data
+		 */
+		sig_pubkey_offset = dst_addr;
+		memcpy((uint8_t *) &payload[i], (uint8_t *) sig_pubkey_offset,
+			src_size - data_size);
+
+		memset((void *)&dst_addr, 0, sizeof(dst_size));
+
+		i += (src_size - data_size) / MBOX_WORD_BYTE;
+	}
+
+	status = mailbox_send_cmd_async(send_id,
+					MBOX_FCS_ECDSA_SHA2_DATA_SIGN_VERIFY,
+					payload, i, CMD_INDIRECT);
+
+	if (is_finalised != 0U) {
+		memset((void *) &fcs_sha2_data_sig_verify_param, 0,
+			sizeof(fcs_crypto_service_data));
+	}
+
+	if (status < 0) {
+		*mbox_error = -status;
+		return INTEL_SIP_SMC_STATUS_ERROR;
+	}
+
+	*dst_size = resp_len * MBOX_WORD_BYTE;
+	flush_dcache_range(dst_addr, *dst_size);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
 int intel_fcs_ecdsa_get_pubkey_init(uint32_t session_id, uint32_t context_id,
 				uint32_t key_id, uint32_t param_size,
 				uint64_t param_data, uint32_t *mbox_error)
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index b57ab92..a20e61c 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -344,6 +344,28 @@
 	case(0xF8011120):	/* INTSTAT */
 	case(0xF8011124):	/* DIAGINTTEST */
 	case(0xF801112C):	/* DERRADDRA */
+	case(0xFA000000):	/* SMMU SCR0 */
+	case(0xFA000004):	/* SMMU SCR1 */
+	case(0xFA000400):	/* SMMU NSCR0 */
+	case(0xFA004000):	/* SMMU SSD0_REG */
+	case(0xFA000820):	/* SMMU SMR8 */
+	case(0xFA000c20):	/* SMMU SCR8 */
+	case(0xFA028000):	/* SMMU CB8_SCTRL */
+	case(0xFA001020):	/* SMMU CBAR8 */
+	case(0xFA028030):	/* SMMU TCR_LPAE */
+	case(0xFA028020):	/* SMMU CB8_TTBR0_LOW */
+	case(0xFA028024):	/* SMMU CB8_PRRR_HIGH */
+	case(0xFA028038):	/* SMMU CB8_PRRR_MIR0 */
+	case(0xFA02803C):	/* SMMU CB8_PRRR_MIR1 */
+	case(0xFA028010):	/* SMMU_CB8)TCR2 */
+	case(0xFFD080A4):	/* SDM SMMU STREAM ID REG */
+	case(0xFA001820):	/* SMMU_CBA2R8 */
+	case(0xFA000074):	/* SMMU_STLBGSTATUS */
+	case(0xFA0287F4):	/* SMMU_CB8_TLBSTATUS */
+	case(0xFA000060):	/* SMMU_STLBIALL */
+	case(0xFA000070):	/* SMMU_STLBGSYNC */
+	case(0xFA028618):	/* CB8_TLBALL */
+	case(0xFA0287F0):	/* CB8_TLBSYNC */
 	case(0xFFD12028):	/* SDMMCGRP_CTRL */
 	case(0xFFD12044):	/* EMAC0 */
 	case(0xFFD12048):	/* EMAC1 */
@@ -927,6 +949,22 @@
 					&mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_get_digest_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, false,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_GET_DIGEST_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_get_digest_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, true,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_MAC_VERIFY_INIT:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		status = intel_fcs_mac_verify_init(x1, x2, x3,
@@ -951,6 +989,24 @@
 					true, &mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_mac_verify_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, x7,
+					false, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_MAC_VERIFY_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_mac_verify_smmu_update_finalize(x1, x2, x3,
+					x4, x5, (uint32_t *) &x6, x7,
+					true, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_INIT:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		status = intel_fcs_ecdsa_sha2_data_sign_init(x1, x2, x3,
@@ -973,6 +1029,22 @@
 					&mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(x1,
+					x2, x3, x4, x5, (uint32_t *) &x6, false,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIGN_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		status = intel_fcs_ecdsa_sha2_data_sign_smmu_update_finalize(x1,
+					x2, x3, x4, x5, (uint32_t *) &x6, true,
+					&mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_ECDSA_HASH_SIGN_INIT:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		status = intel_fcs_ecdsa_hash_sign_init(x1, x2, x3,
@@ -1014,6 +1086,24 @@
 					x7, false, &mbox_error);
 		SMC_RET4(handle, status, mbox_error, x5, x6);
 
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_UPDATE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(
+					x1, x2, x3, x4, x5, (uint32_t *) &x6,
+					x7, false, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
+	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_SMMU_FINALIZE:
+		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
+		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
+		x7 = SMC_GET_GP(handle, CTX_GPREG_X7);
+		status = intel_fcs_ecdsa_sha2_data_sig_verify_smmu_update_finalize(
+					x1, x2, x3, x4, x5, (uint32_t *) &x6,
+					x7, true, &mbox_error, &send_id);
+		SMC_RET4(handle, status, mbox_error, x5, x6);
+
 	case INTEL_SIP_SMC_FCS_ECDSA_SHA2_DATA_SIG_VERIFY_FINALIZE:
 		x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 		x6 = SMC_GET_GP(handle, CTX_GPREG_X6);