diff --git a/plat/imx/imx8m/imx8m_csu.c b/plat/imx/imx8m/imx8m_csu.c
new file mode 100644
index 000000000..2b3a7d97b
--- /dev/null
+++ b/plat/imx/imx8m/imx8m_csu.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+
+#include <imx8m_csu.h>
+
+void imx_csu_init(const struct imx_csu_cfg *csu_cfg)
+{
+	const struct imx_csu_cfg *csu = csu_cfg;
+	uint32_t val;
+
+	while (csu->type != CSU_INVALID) {
+		switch (csu->type) {
+		case CSU_CSL:
+			val = mmio_read_32(CSLx_REG(csu->idx));
+			if (val & CSLx_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSLx_REG(csu->idx), CSLx_CFG(0xff, csu->idx),
+				CSLx_CFG(csu->csl_level | (csu->lock << 8), csu->idx));
+			break;
+		case CSU_HP:
+			val = mmio_read_32(CSU_HP_REG(csu->idx));
+			if (val & CSU_HP_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_HP_REG(csu->idx), CSU_HP_CFG(0x1, csu->idx),
+				CSU_HP_CFG(csu->hp | (csu->lock << 0x1), csu->idx));
+			break;
+		case CSU_SA:
+			val = mmio_read_32(CSU_SA_REG(csu->idx));
+			if (val & CSU_SA_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_SA_REG(csu->idx), CSU_SA_CFG(0x1, csu->idx),
+				CSU_SA_CFG(csu->sa | (csu->lock << 0x1), csu->idx));
+			break;
+		case CSU_HPCONTROL:
+			val = mmio_read_32(CSU_HPCONTROL_REG(csu->idx));
+			if (val & CSU_HPCONTROL_LOCK(csu->idx)) {
+				break;
+			}
+			mmio_clrsetbits_32(CSU_HPCONTROL_REG(csu->idx), CSU_HPCONTROL_CFG(0x1, csu->idx),
+				CSU_HPCONTROL_CFG(csu->hpctrl | (csu->lock << 0x1), csu->idx));
+			break;
+		default:
+			break;
+		}
+
+		csu++;
+	}
+}
diff --git a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
index 40110d778..debede1fd 100644
--- a/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022 ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,7 +18,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <gpc.h>
@@ -26,8 +26,11 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	MAP_REGION_FLAT(IMX_GIC_BASE, IMX_GIC_SIZE, MT_DEVICE | MT_RW),
 	MAP_REGION_FLAT(IMX_AIPS_BASE, IMX_AIPS_SIZE, MT_DEVICE | MT_RW), /* AIPS map */
@@ -44,9 +47,11 @@ static const struct aipstz_cfg aipstz[] = {
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M4, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART4, D1R | D1W),
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 
@@ -54,6 +59,20 @@ static const struct imx_rdc_cfg rdc[] = {
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(0x1, CSU_SEC_LEVEL_0, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -109,6 +128,8 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -124,7 +145,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -134,6 +155,16 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -150,6 +181,9 @@ void bl31_plat_arch_setup(void)
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -184,3 +218,12 @@ unsigned int plat_get_syscnt_freq2(void)
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mm/include/imx_sec_def.h b/plat/imx/imx8m/imx8mm/include/imx_sec_def.h
new file mode 100644
index 000000000..62159837d
--- /dev/null
+++ b/plat/imx/imx8m/imx8mm/include/imx_sec_def.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M4 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_VPU_Decoders = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_CSI1 = 6,
+	RDC_MDA_SDMA3b = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_NAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2p = 24,
+	RDC_MDA_SDMA2b = 24,
+	RDC_MDA_SDMA2_to_SPBA2 = 24,
+	RDC_MDA_SDMA3_to_SPBA2 = 25,
+	RDC_MDA_SDMA1_to_SPBA1 = 26,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA3 = 11,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_LCDIF = 32,
+	RDC_PDAP_MIPI_DSI = 33,
+	RDC_PDAP_CSI = 34,
+	RDC_PDAP_MIPI_CSI = 35,
+	RDC_PDAP_USB1 = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_USB2 = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_PCIE_PHY1 = 88,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_MICFIL = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_SPDIF2 = 106,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_DC_MST0 = 32,
+	CSU_CSL_DC_MST1 = 33,
+	CSU_CSL_DC_MST2 = 34,
+	CSU_CSL_DC_MST3 = 35,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_CAAM = 114,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mm/include/platform_def.h b/plat/imx/imx8m/imx8mm/include/platform_def.h
index 300ef9e62..ed693b9fc 100644
--- a/plat/imx/imx8m/imx8mm/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mm/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -59,6 +59,8 @@
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 #define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)
diff --git a/plat/imx/imx8m/imx8mm/platform.mk b/plat/imx/imx8m/imx8mm/platform.mk
index cd8de891b..0cce7ca57 100644
--- a/plat/imx/imx8m/imx8mm/platform.mk
+++ b/plat/imx/imx8m/imx8mm/platform.mk
@@ -1,8 +1,11 @@
 #
-# Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2019-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
+#
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
 
 PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
 				-Iplat/imx/imx8m/include		\
@@ -25,6 +28,7 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/imx8m/gpc_common.c			\
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mm/imx8mm_bl31_setup.c	\
@@ -34,14 +38,11 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
 ifeq (${NEED_BL2},yes)
@@ -150,8 +151,11 @@ $(eval $(call add_define,BL32_SIZE))
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
 
 ifeq (${MEASURED_BOOT},1)
     MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
@@ -161,5 +165,8 @@ ifeq (${MEASURED_BOOT},1)
 BL2_SOURCES		+=	plat/imx/imx8m/imx8m_measured_boot.c	\
 				plat/imx/imx8m/imx8m_dyn_cfg_helpers.c	\
 				${EVENT_LOG_SOURCES}
-
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
 endif
diff --git a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
index d4705eeeb..8147792c6 100644
--- a/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2019-2020 NXP
+ * Copyright 2019-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,9 +24,12 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	GIC_MAP, AIPS_MAP, OCRAM_S_MAP, DDRC_MAP, {0},
 };
@@ -41,9 +44,11 @@ static const struct aipstz_cfg aipstz[] = {
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M7, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART4, D1R | D1W),
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 	RDC_MEM_REGIONn(16, 0x0, 0x0, 0xff),
@@ -54,6 +59,22 @@ static const struct imx_rdc_cfg rdc[] = {
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(CSU_CSL_OCRAM, CSU_SEC_LEVEL_2, UNLOCKED),
+	CSU_CSLx(CSU_CSL_OCRAM_S, CSU_SEC_LEVEL_2, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -98,6 +119,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 		u_register_t arg2, u_register_t arg3)
 {
 	static console_t console;
+	unsigned int val;
 	int i;
 
 	/* Enable CSU NS access permission */
@@ -109,6 +131,13 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
+	/* config the ocram memory range for secure access */
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, 0x4c1);
+	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -124,7 +153,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -134,6 +163,16 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -150,6 +189,10 @@ void bl31_plat_arch_setup(void)
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -184,3 +227,12 @@ unsigned int plat_get_syscnt_freq2(void)
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mn/include/imx_sec_def.h b/plat/imx/imx8m/imx8mn/include/imx_sec_def.h
new file mode 100644
index 000000000..0ef14a90b
--- /dev/null
+++ b/plat/imx/imx8m/imx8mn/include/imx_sec_def.h
@@ -0,0 +1,210 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M7 = 1,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_ISI = 6,
+	RDC_MDA_SDMA3b = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA3 = 11,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_LCDIF = 32,
+	RDC_PDAP_MIPI_DSI = 33,
+	RDC_PDAP_ISI = 34,
+	RDC_PDAP_MIPI_CSI = 35,
+	RDC_PDAP_USB1 = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_SAI7 = 87,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_MICFIL = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_ASRC = 107,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_DC_MST0 = 32,
+	CSU_CSL_DC_MST1 = 33,
+	CSU_CSL_DC_MST2 = 34,
+	CSU_CSL_DC_MST3 = 35,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_CAAM = 114,
+	CSU_CSL_OCRAM = 118,
+	CSU_CSL_OCRAM_S = 119,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mn/include/platform_def.h b/plat/imx/imx8m/imx8mn/include/platform_def.h
index 9c46d8d27..8d39ea6a5 100644
--- a/plat/imx/imx8m/imx8mn/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mn/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -45,6 +45,8 @@
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)
diff --git a/plat/imx/imx8m/imx8mn/platform.mk b/plat/imx/imx8m/imx8mn/platform.mk
index 208708918..54be41b7d 100644
--- a/plat/imx/imx8m/imx8mn/platform.mk
+++ b/plat/imx/imx8m/imx8mn/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2019-2020 NXP
+# Copyright 2019-2022 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -23,6 +23,7 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mn/imx8mn_bl31_setup.c	\
 				plat/imx/imx8m/imx8mn/imx8mn_psci.c		\
@@ -31,8 +32,6 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
@@ -57,5 +56,12 @@ $(eval $(call add_define,BL32_SIZE))
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif
diff --git a/plat/imx/imx8m/imx8mp/gpc.c b/plat/imx/imx8m/imx8mp/gpc.c
index d660e3d88..3d68b94a3 100644
--- a/plat/imx/imx8m/imx8mp/gpc.c
+++ b/plat/imx/imx8m/imx8mp/gpc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2019-2020 NXP
+ * Copyright 2019-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -69,10 +69,11 @@ enum pu_domain_id {
 	HDMIMIX,
 	HDMI_PHY,
 	DDRMIX,
+	MAX_DOMAINS,
 };
 
 /* PU domain, add some hole to minimize the uboot change */
-static struct imx_pwr_domain pu_domains[20] = {
+static struct imx_pwr_domain pu_domains[MAX_DOMAINS] = {
 	[MIPI_PHY1] = IMX_PD_DOMAIN(MIPI_PHY1, false),
 	[PCIE_PHY] = IMX_PD_DOMAIN(PCIE_PHY, false),
 	[USB1_PHY] = IMX_PD_DOMAIN(USB1_PHY, true),
@@ -174,6 +175,11 @@ static void imx_gpc_pm_domain_enable(uint32_t domain_id, bool on)
 	struct imx_pwr_domain *pwr_domain = &pu_domains[domain_id];
 	unsigned int i;
 
+	/* validate the domain id */
+	if (domain_id >= MAX_DOMAINS) {
+		return;
+	}
+
 	if (domain_id == HSIOMIX) {
 		for (i = 0; i < ARRAY_SIZE(hsiomix_clk); i++) {
 			hsiomix_clk[i].val = mmio_read_32(IMX_CCM_BASE + hsiomix_clk[i].offset);
diff --git a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
index 22fbd5e4b..57e5c5167 100644
--- a/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,9 +24,12 @@
 #include <imx_uart.h>
 #include <imx_rdc.h>
 #include <imx8m_caam.h>
+#include <imx8m_csu.h>
 #include <platform_def.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	GIC_MAP, AIPS_MAP, OCRAM_S_MAP, DDRC_MAP,
 	NOC_MAP, {0},
@@ -42,9 +45,10 @@ static const struct aipstz_cfg aipstz[] = {
 
 static const struct imx_rdc_cfg rdc[] = {
 	/* Master domain assignment */
-	RDC_MDAn(0x1, DID1),
+	RDC_MDAn(RDC_MDA_M7, DID1),
 
 	/* peripherals domain permission */
+	RDC_PDAPn(RDC_PDAP_UART2, D0R | D0W),
 
 	/* memory region */
 
@@ -52,6 +56,21 @@ static const struct imx_rdc_cfg rdc[] = {
 	{0},
 };
 
+static const struct imx_csu_cfg csu_cfg[] = {
+	/* peripherals csl setting */
+	CSU_CSLx(CSU_CSL_OCRAM, CSU_SEC_LEVEL_2, UNLOCKED),
+	CSU_CSLx(CSU_CSL_OCRAM_S, CSU_SEC_LEVEL_2, UNLOCKED),
+
+	/* master HP0~1 */
+
+	/* SA setting */
+
+	/* HP control setting */
+
+	/* Sentinel */
+	{0}
+};
+
 static entry_point_info_t bl32_image_ep_info;
 static entry_point_info_t bl33_image_ep_info;
 
@@ -96,6 +115,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 		u_register_t arg2, u_register_t arg3)
 {
 	static console_t console;
+	unsigned int val;
 	unsigned int i;
 
 	/* Enable CSU NS access permission */
@@ -107,6 +127,13 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 
 	imx_rdc_init(rdc);
 
+	imx_csu_init(csu_cfg);
+
+	/* config the ocram memory range for secure access */
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, 0x4E1);
+	val = mmio_read_32(IMX_IOMUX_GPR_BASE + 0x2c);
+	mmio_write_32(IMX_IOMUX_GPR_BASE + 0x2c, val | 0x3DFF0000);
+
 	imx8m_caam_init();
 
 	console_imx_uart_register(IMX_BOOT_UART_BASE, IMX_BOOT_UART_CLK_IN_HZ,
@@ -122,7 +149,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -132,6 +159,16 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tzc380_setup();
@@ -148,6 +185,10 @@ void bl31_plat_arch_setup(void)
 		(BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE),
 		MT_DEVICE | MT_RW | MT_SECURE);
 #endif
+
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 	init_xlat_tables();
@@ -185,3 +226,12 @@ unsigned int plat_get_syscnt_freq2(void)
 {
 	return COUNTER_FREQUENCY;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mp/include/imx_sec_def.h b/plat/imx/imx8m/imx8mp/include/imx_sec_def.h
new file mode 100644
index 000000000..ba248b592
--- /dev/null
+++ b/plat/imx/imx8m/imx8mp/include/imx_sec_def.h
@@ -0,0 +1,274 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M7 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_SDMA3p = 3,
+	RDC_MDA_SDMA3b = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_ISI = 6,
+	RDC_MDA_NPU = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMA1p = 11,
+	RDC_MDA_SDMA1b = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_uSDHC3 = 17,
+	RDC_MDA_AUDIO_PROCESSOR = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+	RDC_MDA_SDMA3_to_SPBA2 = 25,
+	RDC_MDA_SDMA1_to_SPBA1 = 26,
+	RDC_MDA_LCDIF2 = 27,
+	RDC_MDA_HDMI_TX = 28,
+	RDC_MDA_ENET2 = 29,
+	RDC_MDA_GPU3D = 30,
+	RDC_MDA_GPU2D = 31,
+	RDC_MDA_VPU_G1 = 32,
+	RDC_MDA_VPU_G2 = 33,
+	RDC_MDA_VPU_VC8000E = 34,
+	RDC_MDA_AUDIO_EDMA = 35,
+	RDC_MDA_ISP1 = 36,
+	RDC_MDA_ISP2 = 37,
+	RDC_MDA_DEWARP = 38,
+	RDC_MDA_GIC500 = 39,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_MU_2_A = 5,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_MU_2_B = 16,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_MU_3_A = 18,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANA_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_MU_3_B = 31,
+	RDC_PDAP_ISI = 32,
+	RDC_PDAP_ISP0 = 33,
+	RDC_PDAP_ISP1 = 34,
+	RDC_PDAP_IPS_Dewarp = 35,
+	RDC_PDAP_MIPI_CSI0 = 36,
+	RDC_PDAP_HSIOMIX_BLK_CTL = 37,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_System_Counter_RD = 42,
+	RDC_PDAP_System_Counter_CMP = 43,
+	RDC_PDAP_System_Counter_CTRL = 44,
+	RDC_PDAP_I2C5 = 45,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_MIPI_CSI1 = 49,
+	RDC_PDAP_MIPI_DSI0 = 50,
+	RDC_PDAP_MEDIAMIX_BLK_CTL = 51,
+	RDC_PDAP_LCDIF1 = 52,
+	RDC_PDAP_eDMA_Management_Page = 53,
+	RDC_PDAP_eDMA_Channels_15_0 = 54,
+	RDC_PDAP_eDMA_Channels_31_16 = 55,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_I2C6 = 57,
+	RDC_PDAP_CAAM = 58,
+	RDC_PDAP_LCDIF2 = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_NOC_BLK_CTL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_LVDS0 = 64,
+	RDC_PDAP_LVDS1 = 65,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_HDMI_TX = 71,
+	RDC_PDAP_IRQ_STEER_Audio_Processor = 72,
+	RDC_PDAP_SDMA2 = 73,
+	RDC_PDAP_MU_1_A = 74,
+	RDC_PDAP_MU_1_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI2 = 79,
+	RDC_PDAP_SAI3 = 80,
+	RDC_PDAP_CAN_FD1 = 81,
+	RDC_PDAP_SAI5 = 82,
+	RDC_PDAP_SAI6 = 83,
+	RDC_PDAP_uSDHC1 = 84,
+	RDC_PDAP_uSDHC2 = 85,
+	RDC_PDAP_uSDHC3 = 86,
+	RDC_PDAP_PCIE_PHY1 = 87,
+	RDC_PDAP_HDMI_TX_AUDLNK_MSTR = 88,
+	RDC_PDAP_CAN_FD2 = 89,
+	RDC_PDAP_SPBA2 = 90,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_AUDIO_BLK_CTRL = 92,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_ENET2_TSN = 95,
+	RDC_PDAP_ASRC = 97,
+	RDC_PDAP_eCSPI1 = 98,
+	RDC_PDAP_eCSPI2 = 99,
+	RDC_PDAP_eCSPI3 = 100,
+	RDC_PDAP_SAI7 = 101,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_PDM_MICFIL = 106,
+	RDC_PDAP_AUDIO_XCVR_RX_eARC = 107,
+	RDC_PDAP_SDMA3 = 109,
+	RDC_PDAP_SPBA1 = 111,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_MU_2_A = 5,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_MU_2_B = 16,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_MU_3_A = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANA_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_MU_3_B = 31,
+	CSU_CSL_ISI = 32,
+	CSU_CSL_ISP0 = 33,
+	CSU_CSL_ISP1 = 34,
+	CSU_CSL_IPS_Dewarp = 35,
+	CSU_CSL_MIPI_CSI0 = 36,
+	CSU_CSL_HSIOMIX_BLK_CTL	= 37,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_System_Counter_RD = 42,
+	CSU_CSL_System_Counter_CMP = 43,
+	CSU_CSL_System_Counter_CTRL = 44,
+	CSU_CSL_I2C5 = 45,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_MIPI_CSI1 = 49,
+	CSU_CSL_MIPI_DSI0 = 50,
+	CSU_CSL_MEDIAMIX_BLK_CTL = 51,
+	CSU_CSL_LCDIF1 = 52,
+	CSU_CSL_eDMA_Management_Page = 53,
+	CSU_CSL_eDMA_Channels_15_0 = 54,
+	CSU_CSL_eDMA_Channels_31_16 = 55,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_I2C6 = 57,
+	CSU_CSL_CAAM = 58,
+	CSU_CSL_LCDIF2 = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_NOC_BLK_CTL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_LVDS0 = 64,
+	CSU_CSL_LVDS1 = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_HDMI_TX = 71,
+	CSU_CSL_IRQ_STEER_Audio_Processor = 72,
+	CSU_CSL_SDMA2 = 73,
+	CSU_CSL_MU_1_A = 74,
+	CSU_CSL_MU_1_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI2 = 79,
+	CSU_CSL_SAI3 = 80,
+	CSU_CSL_CAN_FD1 = 81,
+	CSU_CSL_SAI5 = 82,
+	CSU_CSL_SAI6 = 83,
+	CSU_CSL_uSDHC1 = 84,
+	CSU_CSL_uSDHC2 = 85,
+	CSU_CSL_uSDHC3 = 86,
+	CSU_CSL_PCIE_PHY1 = 87,
+	CSU_CSL_HDMI_TX_AUDLNK_MSTR = 88,
+	CSU_CSL_CAN_FD2 = 89,
+	CSU_CSL_SPBA2 = 90,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_AUDIO_BLK_CTRL = 92,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_ENET2_TSN = 95,
+	CSU_CSL_ASRC = 97,
+	CSU_CSL_eCSPI1 = 98,
+	CSU_CSL_eCSPI2 = 99,
+	CSU_CSL_eCSPI3 = 100,
+	CSU_CSL_SAI7 = 101,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_PDM_MICFIL = 106,
+	CSU_CSL_AUDIO_XCVR_RX_eARC = 107,
+	CSU_CSL_SDMA3 = 109,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_OCRAM_A = 113,
+	CSU_CSL_OCRAM = 118,
+	CSU_CSL_OCRAM_S = 119,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mp/include/platform_def.h b/plat/imx/imx8m/imx8mp/include/platform_def.h
index 486c1eec5..8807f5d2d 100644
--- a/plat/imx/imx8m/imx8mp/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 NXP
+ * Copyright 2020-2022 NXP
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -62,6 +62,8 @@
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
 #define PLAT_NS_IMAGE_SIZE		U(0x00200000)
 
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
+
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
 #define PLAT_GICR_BASE			U(0x38880000)
diff --git a/plat/imx/imx8m/imx8mp/platform.mk b/plat/imx/imx8m/imx8mp/platform.mk
index 823b5d6d4..73fbd872b 100644
--- a/plat/imx/imx8m/imx8mp/platform.mk
+++ b/plat/imx/imx8m/imx8mp/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright 2019-2020 NXP
+# Copyright 2019-2022 NXP
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -25,13 +25,12 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/imx8m/imx_aipstz.c			\
 				plat/imx/imx8m/imx_rdc.c			\
 				plat/imx/imx8m/imx8m_caam.c			\
+				plat/imx/imx8m/imx8m_csu.c			\
 				plat/imx/imx8m/imx8m_psci_common.c		\
 				plat/imx/imx8m/imx8mp/imx8mp_bl31_setup.c	\
 				plat/imx/imx8m/imx8mp/imx8mp_psci.c		\
 				plat/imx/imx8m/imx8mp/gpc.c			\
 				plat/imx/common/imx8_topology.c			\
-				plat/imx/common/imx_ehf.c                       \
-				plat/imx/common/imx_sdei.c                      \
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
@@ -149,5 +148,12 @@ $(eval $(call add_define,BL32_SIZE))
 IMX_BOOT_UART_BASE	?=	0x30890000
 $(eval $(call add_define,IMX_BOOT_UART_BASE))
 
-EL3_EXCEPTION_HANDLING := 1
-SDEI_SUPPORT := 1
+EL3_EXCEPTION_HANDLING := $(SDEI_SUPPORT)
+ifeq (${SDEI_SUPPORT}, 1)
+BL31_SOURCES 		+= 	plat/imx/common/imx_ehf.c	\
+				plat/imx/common/imx_sdei.c
+endif
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif
diff --git a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
index 05b59705f..e998a165c 100644
--- a/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
+++ b/plat/imx/imx8m/imx8mq/imx8mq_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -18,7 +18,7 @@
 #include <drivers/generic_delay_timer.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <gpc.h>
@@ -27,6 +27,8 @@
 #include <imx8m_caam.h>
 #include <plat_imx8.h>
 
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
+
 static const mmap_region_t imx_mmap[] = {
 	MAP_REGION_FLAT(GPV_BASE, GPV_SIZE, MT_DEVICE | MT_RW), /* GPV map */
 	MAP_REGION_FLAT(IMX_ROM_BASE, IMX_ROM_SIZE, MT_MEMORY | MT_RO), /* ROM map */
@@ -146,7 +148,7 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
-#ifdef SPD_opteed
+#if defined(SPD_opteed) || defined(SPD_trusty)
 	/* Populate entry point information for BL32 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
@@ -156,6 +158,16 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 	/* Pass TEE base and size to bl33 */
 	bl33_image_ep_info.args.arg1 = BL32_BASE;
 	bl33_image_ep_info.args.arg2 = BL32_SIZE;
+
+#ifdef SPD_trusty
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#else
+	/* Make sure memory is clean */
+	mmio_write_32(BL32_FDT_OVERLAY_ADDR, 0);
+	bl33_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+	bl32_image_ep_info.args.arg3 = BL32_FDT_OVERLAY_ADDR;
+#endif
 #endif
 
 	bl31_tz380_setup();
@@ -168,6 +180,9 @@ void bl31_plat_arch_setup(void)
 	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE, (BL_CODE_END - BL_CODE_BASE),
 		MT_MEMORY | MT_RO | MT_SECURE);
 
+	/* Map TEE memory */
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+
 	mmap_add(imx_mmap);
 
 #if USE_COHERENT_MEM
@@ -215,3 +230,12 @@ void bl31_plat_runtime_setup(void)
 {
 	return;
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = BL32_SIZE;
+	args->arg1 = BL32_BASE;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
diff --git a/plat/imx/imx8m/imx8mq/include/imx_sec_def.h b/plat/imx/imx8m/imx8mq/include/imx_sec_def.h
new file mode 100644
index 000000000..0f771415f
--- /dev/null
+++ b/plat/imx/imx8m/imx8mq/include/imx_sec_def.h
@@ -0,0 +1,249 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_SEC_DEF_H
+#define IMX_SEC_DEF_H
+
+/* RDC MDA index */
+enum rdc_mda_idx {
+	RDC_MDA_A53 = 0,
+	RDC_MDA_M4 = 1,
+	RDC_MDA_PCIE_CTRL1 = 2,
+	RDC_MDA_PCIE_CTRL2 = 3,
+	RDC_MDA_VPU_DEC = 4,
+	RDC_MDA_LCDIF = 5,
+	RDC_MDA_CSI1 = 6,
+	RDC_MDA_CSI2 = 7,
+	RDC_MDA_Coresight = 8,
+	RDC_MDA_DAP = 9,
+	RDC_MDA_CAAM = 10,
+	RDC_MDA_SDMAp = 11,
+	RDC_MDA_SDMAb = 12,
+	RDC_MDA_APBHDMA = 13,
+	RDC_MDA_RAWNAND = 14,
+	RDC_MDA_uSDHC1 = 15,
+	RDC_MDA_uSDHC2 = 16,
+	RDC_MDA_DCSS = 17,
+	RDC_MDA_GPU = 18,
+	RDC_MDA_USB1 = 19,
+	RDC_MDA_USB2 = 20,
+	RDC_MDA_TESTPORT = 21,
+	RDC_MDA_ENET1_TX = 22,
+	RDC_MDA_ENET1_RX = 23,
+	RDC_MDA_SDMA2 = 24,
+	RDC_MDA_SDMA1 = 26,
+};
+
+/* RDC Peripherals index */
+enum rdc_pdap_idx {
+	RDC_PDAP_GPIO1 = 0,
+	RDC_PDAP_GPIO2 = 1,
+	RDC_PDAP_GPIO3 = 2,
+	RDC_PDAP_GPIO4 = 3,
+	RDC_PDAP_GPIO5 = 4,
+	RDC_PDAP_ANA_TSENSOR = 6,
+	RDC_PDAP_ANA_OSC = 7,
+	RDC_PDAP_WDOG1 = 8,
+	RDC_PDAP_WDOG2 = 9,
+	RDC_PDAP_WDOG3 = 10,
+	RDC_PDAP_SDMA2 = 12,
+	RDC_PDAP_GPT1 = 13,
+	RDC_PDAP_GPT2 = 14,
+	RDC_PDAP_GPT3 = 15,
+	RDC_PDAP_ROMCP = 17,
+	RDC_PDAP_LCDIF = 18,
+	RDC_PDAP_IOMUXC = 19,
+	RDC_PDAP_IOMUXC_GPR = 20,
+	RDC_PDAP_OCOTP_CTRL = 21,
+	RDC_PDAP_ANATOP_PLL = 22,
+	RDC_PDAP_SNVS_HP = 23,
+	RDC_PDAP_CCM = 24,
+	RDC_PDAP_SRC = 25,
+	RDC_PDAP_GPC = 26,
+	RDC_PDAP_SEMAPHORE1 = 27,
+	RDC_PDAP_SEMAPHORE2 = 28,
+	RDC_PDAP_RDC = 29,
+	RDC_PDAP_CSU = 30,
+	RDC_PDAP_MST0 = 32,
+	RDC_PDAP_MST1 = 33,
+	RDC_PDAP_MST2 = 34,
+	RDC_PDAP_MST3 = 35,
+	RDC_PDAP_HDMI_SEC = 36,
+	RDC_PDAP_PWM1 = 38,
+	RDC_PDAP_PWM2 = 39,
+	RDC_PDAP_PWM3 = 40,
+	RDC_PDAP_PWM4 = 41,
+	RDC_PDAP_SysCounter_RD = 42,
+	RDC_PDAP_SysCounter_CMP = 43,
+	RDC_PDAP_SysCounter_CTRL = 44,
+	RDC_PDAP_HDMI_CTRL = 45,
+	RDC_PDAP_GPT6 = 46,
+	RDC_PDAP_GPT5 = 47,
+	RDC_PDAP_GPT4 = 48,
+	RDC_PDAP_TZASC = 56,
+	RDC_PDAP_MTR = 59,
+	RDC_PDAP_PERFMON1 = 60,
+	RDC_PDAP_PERFMON2 = 61,
+	RDC_PDAP_PLATFORM_CTRL = 62,
+	RDC_PDAP_QoSC = 63,
+	RDC_PDAP_MIPI_PHY = 64,
+	RDC_PDAP_MIPI_DSI = 65,
+	RDC_PDAP_I2C1 = 66,
+	RDC_PDAP_I2C2 = 67,
+	RDC_PDAP_I2C3 = 68,
+	RDC_PDAP_I2C4 = 69,
+	RDC_PDAP_UART4 = 70,
+	RDC_PDAP_MIPI_CSI1 = 71,
+	RDC_PDAP_MIPI_CSI_PHY1 = 72,
+	RDC_PDAP_CSI1 = 73,
+	RDC_PDAP_MU_A = 74,
+	RDC_PDAP_MU_B = 75,
+	RDC_PDAP_SEMAPHORE_HS = 76,
+	RDC_PDAP_SAI1 = 78,
+	RDC_PDAP_SAI6 = 80,
+	RDC_PDAP_SAI5 = 81,
+	RDC_PDAP_SAI4 = 82,
+	RDC_PDAP_USDHC1 = 84,
+	RDC_PDAP_USDHC2 = 85,
+	RDC_PDAP_MIPI_CSI2 = 86,
+	RDC_PDAP_MIPI_CSI_PHY2 = 87,
+	RDC_PDAP_CSI2 = 88,
+	RDC_PDAP_QSPI = 91,
+	RDC_PDAP_SDMA1 = 93,
+	RDC_PDAP_ENET1 = 94,
+	RDC_PDAP_SPDIF1 = 97,
+	RDC_PDAP_ECSPI1 = 98,
+	RDC_PDAP_ECSPI2 = 99,
+	RDC_PDAP_ECSPI3 = 100,
+	RDC_PDAP_UART1 = 102,
+	RDC_PDAP_UART3 = 104,
+	RDC_PDAP_UART2 = 105,
+	RDC_PDAP_SPDIF2 = 106,
+	RDC_PDAP_SAI2 = 107,
+	RDC_PDAP_SAI3 = 108,
+	RDC_PDAP_SPBA1 = 111,
+	RDC_PDAP_CAAM = 114,
+	RDC_PDAP_DDRC_SEC = 115,
+	RDC_PDAP_GIC_EXSC = 116,
+	RDC_PDAP_USB_EXSC = 117,
+	RDC_PDAP_OCRAM_TZ = 118,
+	RDC_PDAP_OCRAM_S_TZ = 119,
+	RDC_PDAP_VPU_SEC = 120,
+	RDC_PDAP_DAP_EXSC = 121,
+	RDC_PDAP_ROMCP_SEC = 122,
+	RDC_PDAP_APBHDMA_SEC = 123,
+	RDC_PDAP_M4_SEC = 124,
+	RDC_PDAP_QSPI_SEC = 125,
+	RDC_PDAP_GPU_EXSC = 126,
+	RDC_PDAP_PCIE = 127,
+};
+
+enum csu_csl_idx {
+	CSU_CSL_GPIO1 = 0,
+	CSU_CSL_GPIO2 = 1,
+	CSU_CSL_GPIO3 = 2,
+	CSU_CSL_GPIO4 = 3,
+	CSU_CSL_GPIO5 = 4,
+	CSU_CSL_ANA_TSENSOR = 6,
+	CSU_CSL_ANA_OSC = 7,
+	CSU_CSL_WDOG1 = 8,
+	CSU_CSL_WDOG2 = 9,
+	CSU_CSL_WDOG3 = 10,
+	CSU_CSL_SDMA2 = 12,
+	CSU_CSL_GPT1 = 13,
+	CSU_CSL_GPT2 = 14,
+	CSU_CSL_GPT3 = 15,
+	CSU_CSL_ROMCP = 17,
+	CSU_CSL_LCDIF = 18,
+	CSU_CSL_IOMUXC = 19,
+	CSU_CSL_IOMUXC_GPR = 20,
+	CSU_CSL_OCOTP_CTRL = 21,
+	CSU_CSL_ANATOP_PLL = 22,
+	CSU_CSL_SNVS_HP = 23,
+	CSU_CSL_CCM = 24,
+	CSU_CSL_SRC = 25,
+	CSU_CSL_GPC = 26,
+	CSU_CSL_SEMAPHORE1 = 27,
+	CSU_CSL_SEMAPHORE2 = 28,
+	CSU_CSL_RDC = 29,
+	CSU_CSL_CSU = 30,
+	CSU_CSL_MST0 = 32,
+	CSU_CSL_MST1 = 33,
+	CSU_CSL_MST2 = 34,
+	CSU_CSL_MST3 = 35,
+	CSU_CSL_HDMI_SEC = 36,
+	CSU_CSL_PWM1 = 38,
+	CSU_CSL_PWM2 = 39,
+	CSU_CSL_PWM3 = 40,
+	CSU_CSL_PWM4 = 41,
+	CSU_CSL_SysCounter_RD = 42,
+	CSU_CSL_SysCounter_CMP = 43,
+	CSU_CSL_SysCounter_CTRL = 44,
+	CSU_CSL_HDMI_CTRL = 45,
+	CSU_CSL_GPT6 = 46,
+	CSU_CSL_GPT5 = 47,
+	CSU_CSL_GPT4 = 48,
+	CSU_CSL_TZASC = 56,
+	CSU_CSL_MTR = 59,
+	CSU_CSL_PERFMON1 = 60,
+	CSU_CSL_PERFMON2 = 61,
+	CSU_CSL_PLATFORM_CTRL = 62,
+	CSU_CSL_QoSC = 63,
+	CSU_CSL_MIPI_PHY = 64,
+	CSU_CSL_MIPI_DSI = 65,
+	CSU_CSL_I2C1 = 66,
+	CSU_CSL_I2C2 = 67,
+	CSU_CSL_I2C3 = 68,
+	CSU_CSL_I2C4 = 69,
+	CSU_CSL_UART4 = 70,
+	CSU_CSL_MIPI_CSI1 = 71,
+	CSU_CSL_MIPI_CSI_PHY1 = 72,
+	CSU_CSL_CSI1 = 73,
+	CSU_CSL_MU_A = 74,
+	CSU_CSL_MU_B = 75,
+	CSU_CSL_SEMAPHORE_HS = 76,
+	CSU_CSL_SAI1 = 78,
+	CSU_CSL_SAI6 = 80,
+	CSU_CSL_SAI5 = 81,
+	CSU_CSL_SAI4 = 82,
+	CSU_CSL_USDHC1 = 84,
+	CSU_CSL_USDHC2 = 85,
+	CSU_CSL_MIPI_CSI2 = 86,
+	CSU_CSL_MIPI_CSI_PHY2 = 87,
+	CSU_CSL_CSI2 = 88,
+	CSU_CSL_QSPI = 91,
+	CSU_CSL_SDMA1 = 93,
+	CSU_CSL_ENET1 = 94,
+	CSU_CSL_SPDIF1 = 97,
+	CSU_CSL_ECSPI1 = 98,
+	CSU_CSL_ECSPI2 = 99,
+	CSU_CSL_ECSPI3 = 100,
+	CSU_CSL_UART1 = 102,
+	CSU_CSL_UART3 = 104,
+	CSU_CSL_UART2 = 105,
+	CSU_CSL_SPDIF2 = 106,
+	CSU_CSL_SAI2 = 107,
+	CSU_CSL_SAI3 = 108,
+	CSU_CSL_SPBA1 = 111,
+	CSU_CSL_MOD_EN3 = 112,
+	CSU_CSL_MOD_EN0 = 113,
+	CSU_CSL_CAAM = 114,
+	CSU_CSL_DDRC_SEC = 115,
+	CSU_CSL_GIC_EXSC = 116,
+	CSU_CSL_USB_EXSC = 117,
+	CSU_CSL_OCRAM_TZ = 118,
+	CSU_CSL_OCRAM_S_TZ = 119,
+	CSU_CSL_VPU_SEC = 120,
+	CSU_CSL_DAP_EXSC = 121,
+	CSU_CSL_ROMCP_SEC = 122,
+	CSU_CSL_APBHDMA_SEC = 123,
+	CSU_CSL_M4_SEC = 124,
+	CSU_CSL_QSPI_SEC = 125,
+	CSU_CSL_GPU_EXSC = 126,
+	CSU_CSL_PCIE = 127,
+};
+
+#endif /* IMX_SEC_DEF_H */
diff --git a/plat/imx/imx8m/imx8mq/include/platform_def.h b/plat/imx/imx8m/imx8mq/include/platform_def.h
index 6d6a8650e..a76e89529 100644
--- a/plat/imx/imx8m/imx8mq/include/platform_def.h
+++ b/plat/imx/imx8m/imx8mq/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -35,6 +35,7 @@
 
 /* non-secure uboot base */
 #define PLAT_NS_IMAGE_OFFSET		U(0x40200000)
+#define BL32_FDT_OVERLAY_ADDR		(PLAT_NS_IMAGE_OFFSET + 0x3000000)
 
 /* GICv3 base address */
 #define PLAT_GICD_BASE			U(0x38800000)
@@ -43,8 +44,13 @@
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
 
+#ifdef SPD_trusty
+#define MAX_XLAT_TABLES			5
+#define MAX_MMAP_REGIONS		15
+#else
 #define MAX_XLAT_TABLES			4
 #define MAX_MMAP_REGIONS		14
+#endif
 
 #define HAB_RVT_BASE			U(0x00000880) /* HAB_RVT for i.MX8MQ */
 
diff --git a/plat/imx/imx8m/imx8mq/platform.mk b/plat/imx/imx8m/imx8mq/platform.mk
index 546101043..7b6df920c 100644
--- a/plat/imx/imx8m/imx8mq/platform.mk
+++ b/plat/imx/imx8m/imx8mq/platform.mk
@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/common/include		\
 				-Iplat/imx/imx8m/include		\
 				-Iplat/imx/imx8m/imx8mq/include
@@ -28,12 +31,11 @@ BL31_SOURCES		+=	plat/imx/common/imx8_helpers.S			\
 				plat/imx/common/imx_sip_handler.c		\
 				plat/imx/common/imx_sip_svc.c			\
 				plat/imx/common/imx_uart_console.S		\
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				drivers/arm/tzc/tzc380.c			\
 				drivers/delay_timer/delay_timer.c		\
 				drivers/delay_timer/generic_delay_timer.c	\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}
 
 USE_COHERENT_MEM	:=	1
@@ -49,3 +51,7 @@ $(eval $(call add_define,BL32_BASE))
 
 BL32_SIZE		?=	0x2000000
 $(eval $(call add_define,BL32_SIZE))
+
+ifeq (${SPD},trusty)
+	BL31_CFLAGS    +=      -DPLAT_XLAT_TABLES_DYNAMIC=1
+endif
diff --git a/plat/imx/imx8m/include/imx8m_csu.h b/plat/imx/imx8m/include/imx8m_csu.h
new file mode 100644
index 000000000..dc634ed91
--- /dev/null
+++ b/plat/imx/imx8m/include/imx8m_csu.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2020-2022 NXP
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IMX_CSU_H
+#define IMX_CSU_H
+
+#include <lib/utils_def.h>
+
+#include <platform_def.h>
+
+#define CSU_SEC_LEVEL_0		0xff
+#define CSU_SEC_LEVEL_1		0xbb
+#define CSU_SEC_LEVEL_2		0x3f
+#define CSU_SEC_LEVEL_3		0x3b
+#define CSU_SEC_LEVEL_4		0x33
+#define CSU_SEC_LEVEL_5		0x22
+#define CSU_SEC_LEVEL_6		0x03
+#define CSU_SEC_LEVEL_7		0x0
+
+#define LOCKED			0x1
+#define UNLOCKED		0x0
+
+#define CSLx_REG(x)		(IMX_CSU_BASE + ((x) / 2) * 4)
+#define CSLx_LOCK(x)		((0x1 << (((x) % 2) * 16 + 8)))
+#define CSLx_CFG(x, n)		((x) << (((n) % 2) * 16))
+
+#define CSU_HP_REG(x)		(IMX_CSU_BASE + ((x) / 16) * 4 + 0x200)
+#define CSU_HP_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_HP_CFG(x, n)	((x) << (((n) % 16) * 2))
+
+#define CSU_SA_REG(x)		(IMX_CSU_BASE + 0x218)
+#define CSU_SA_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_SA_CFG(x, n)	((x) << (((n) % 16) * 2))
+
+#define CSU_HPCONTROL_REG(x)		(IMX_CSU_BASE + (((x) / 16) * 4) + 0x358)
+#define CSU_HPCONTROL_LOCK(x)		((0x1 << (((x) % 16) * 2 + 1)))
+#define CSU_HPCONTROL_CFG(x, n)		((x) << (((n) % 16) * 2))
+
+enum csu_cfg_type {
+	CSU_INVALID,
+	CSU_CSL,
+	CSU_HP,
+	CSU_SA,
+	CSU_HPCONTROL,
+};
+
+struct imx_csu_cfg {
+	enum csu_cfg_type type;
+	uint16_t idx;
+	uint16_t lock : 1;
+	uint16_t csl_level : 8;
+	uint16_t hp : 1;
+	uint16_t sa : 1;
+	uint16_t hpctrl : 1;
+};
+
+#define CSU_CSLx(i, level, lk)	\
+	{CSU_CSL, .idx = (i), .csl_level = (level), .lock = (lk),}
+
+#define CSU_HPx(i, val, lk)	\
+	{CSU_HP, .idx = (i), .hp = (val), .lock = (lk), }
+
+#define CSU_SA(i, val, lk)	\
+	{CSU_SA, .idx = (i), .sa = (val), .lock = (lk), }
+
+#define CSU_HPCTRL(i, val, lk)	\
+	{CSU_HPCONTROL, .idx = (i), .hpctrl = (val), .lock = (lk), }
+
+void imx_csu_init(const struct imx_csu_cfg *csu_cfg);
+
+#endif /* IMX_CSU_H */
diff --git a/plat/imx/imx8m/include/imx_rdc.h b/plat/imx/imx8m/include/imx_rdc.h
index e25b0e6d4..a6e10a7b7 100644
--- a/plat/imx/imx8m/include/imx_rdc.h
+++ b/plat/imx/imx8m/include/imx_rdc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, NXP. All rights reserved.
+ * Copyright (c) 2019-2022 NXP. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 
 #include <lib/utils_def.h>
 
+#include <imx_sec_def.h>
 #include <platform_def.h>
 
 #define MDAn(x)		(IMX_RDC_BASE + 0x200 + (x) * 4)
diff --git a/plat/imx/imx8qm/imx8qm_bl31_setup.c b/plat/imx/imx8qm/imx8qm_bl31_setup.c
index d9c91107c..68eb53422 100644
--- a/plat/imx/imx8qm/imx8qm_bl31_setup.c
+++ b/plat/imx/imx8qm/imx8qm_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 #include <drivers/console.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <imx8qm_pads.h>
diff --git a/plat/imx/imx8qm/platform.mk b/plat/imx/imx8qm/platform.mk
index f35fa0020..c57edbe2a 100644
--- a/plat/imx/imx8qm/platform.mk
+++ b/plat/imx/imx8qm/platform.mk
@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/imx8qm/include		\
 				-Iplat/imx/common/include		\
 
@@ -23,11 +26,10 @@ BL31_SOURCES		+=	plat/imx/common/lpuart_console.S	\
 				plat/imx/common/imx8_psci.c		\
 				plat/imx/common/imx_sip_svc.c		\
 				plat/imx/common/imx_sip_handler.c	\
-				lib/xlat_tables/aarch64/xlat_tables.c		\
-				lib/xlat_tables/xlat_tables_common.c		\
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a72.S			\
 				drivers/arm/cci/cci.c				\
+				${XLAT_TABLES_LIB_SRCS}				\
 				${IMX_GIC_SOURCES}				\
 
 include plat/imx/common/sci/sci_api.mk
diff --git a/plat/imx/imx8qx/imx8qx_bl31_setup.c b/plat/imx/imx8qx/imx8qx_bl31_setup.c
index 3739cd681..1da8d296c 100644
--- a/plat/imx/imx8qx/imx8qx_bl31_setup.c
+++ b/plat/imx/imx8qx/imx8qx_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,7 +19,7 @@
 #include <drivers/console.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/mmio.h>
-#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
 #include <imx8qx_pads.h>
diff --git a/plat/imx/imx8qx/platform.mk b/plat/imx/imx8qx/platform.mk
index b25be0742..85b5f3dc4 100644
--- a/plat/imx/imx8qx/platform.mk
+++ b/plat/imx/imx8qx/platform.mk
@@ -1,9 +1,12 @@
 #
-# Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+# Translation tables library
+include lib/xlat_tables_v2/xlat_tables.mk
+
 PLAT_INCLUDES		:=	-Iplat/imx/imx8qx/include		\
 				-Iplat/imx/common/include		\
 
@@ -23,9 +26,8 @@ BL31_SOURCES		+=	plat/imx/common/lpuart_console.S	\
 				plat/imx/common/imx_sip_svc.c		\
 				plat/imx/common/imx_sip_handler.c	\
 				plat/common/plat_psci_common.c		\
-				lib/xlat_tables/xlat_tables_common.c	\
-				lib/xlat_tables/aarch64/xlat_tables.c	\
 				lib/cpus/aarch64/cortex_a35.S		\
+				${XLAT_TABLES_LIB_SRCS}			\
 				${IMX_GIC_SOURCES}			\
 
 include plat/imx/common/sci/sci_api.mk