diff --git a/plat/freescale/imx8mq/imx8m_bl31_setup.c b/plat/freescale/imx8mq/imx8m_bl31_setup.c
index 45ef95e..a91f307 100644
--- a/plat/freescale/imx8mq/imx8m_bl31_setup.c
+++ b/plat/freescale/imx8mq/imx8m_bl31_setup.c
@@ -156,12 +156,14 @@
 void bl31_early_platform_setup(bl31_params_t *from_bl2,
 				void *plat_params_from_bl2)
 {
-	int i;
 	uint32_t sm_cmd;
+#if !defined (CSU_RDC_TEST)
+	int i;
 	/* enable CSU NS access permission */
 	for (i = 0; i < 64; i++) {
 		mmio_write_32(0x303e0000 + i * 4, 0xffffffff);
 	}
+#endif
 
 	/* Dealloc part 0 and 2 with current DID */
 	sm_cmd = (0 << SMC_PART_SHIFT | SMC_CMD_DEALLOC_PART);
@@ -253,6 +255,11 @@
 	bl33_image_ep_info.args.arg2 = 0x2000000;
 #endif
 	bl31_tzc380_setup();
+
+#if defined (CSU_RDC_TEST)
+	csu_test();
+	rdc_test();
+#endif
 }
 
 void bl31_plat_arch_setup(void)
diff --git a/plat/freescale/imx8mq/imx_csu.c b/plat/freescale/imx8mq/imx_csu.c
index 3362908..85c1caf 100644
--- a/plat/freescale/imx8mq/imx_csu.c
+++ b/plat/freescale/imx8mq/imx_csu.c
@@ -25,7 +25,12 @@
 	{CSU_CSLn_GPIO2, CSU_RW, 0},
 	{CSU_CSLn_GPIO3, CSU_RW, 0},
 	{CSU_CSLn_GPIO4, CSU_RW, 0},
+#if defined (CSU_RDC_TEST)
+	{CSU_CSLn_GPIO5, CSU_SSRW, 0},
+#else
 	{CSU_CSLn_GPIO5, CSU_RW, 0},
+#endif
+	{CSU_CSLn_Reserved1, CSU_RW, 0},
 	{CSU_CSLn_ANA_TSENSOR, CSU_RW, 0},
 	{CSU_CSLn_ANA_OSC, CSU_RW, 0},
 	{CSU_CSLn_WDOG1, CSU_RW, 0},
@@ -275,3 +280,10 @@
 {
 	csu_set_sa_configs(sa_def_configs, (uint32_t)ARRAY_SIZE(sa_def_configs));
 }
+
+#if defined (CSU_RDC_TEST)
+void csu_test(void)
+{
+	csu_set_default_slaves_modes();
+}
+#endif
diff --git a/plat/freescale/imx8mq/imx_rdc.c b/plat/freescale/imx8mq/imx_rdc.c
index 3f1d259..1929c99 100644
--- a/plat/freescale/imx8mq/imx_rdc.c
+++ b/plat/freescale/imx8mq/imx_rdc.c
@@ -142,6 +142,18 @@
 	return ret;
 }
 
+#if defined (CSU_RDC_TEST)
+/* Default peripherals settings as an example */
+static struct rdc_pdap_conf periph_config[] = {
+	{RDC_PDAP_GPIO4, 0x30, 0},
+};
+
+
+/* Default masters settings as an example */
+static struct rdc_mda_conf masters_config[] = {
+	{RDC_MDA_A53, 0, 0},
+};
+#else
 /* Default peripherals settings as an example */
 static struct rdc_pdap_conf periph_config[] = {
 	{RDC_PDAP_GPIO1, 0x3, 0},
@@ -156,7 +168,7 @@
 	{RDC_MDA_A53, 0, 0},
 	{RDC_MDA_CAAM, 0, 0},
 };
-
+#endif
 void imx_rdc_set_peripherals_default(void)
 {
 	imx_rdc_set_peripherals(periph_config, ARRAY_SIZE(periph_config));
@@ -166,3 +178,10 @@
 {
 	imx_rdc_set_masters(masters_config, ARRAY_SIZE(masters_config));
 }
+#if defined (CSU_RDC_TEST)
+void rdc_test(void)
+{
+	imx_rdc_set_peripherals_default();
+	imx_rdc_set_masters_default();
+}
+#endif
diff --git a/plat/freescale/imx8mq/include/imx_csu.h b/plat/freescale/imx8mq/include/imx_csu.h
index c5a198c..a4ae9ab 100644
--- a/plat/freescale/imx8mq/include/imx_csu.h
+++ b/plat/freescale/imx8mq/include/imx_csu.h
@@ -211,4 +211,7 @@
 void csu_set_sa_configs(struct csu_sa_conf *sa_configs,  uint32_t count);
 void csu_set_default_secure_configs(void);
 
+#if defined (CSU_RDC_TEST)
+void csu_test(void);
+#endif
 #endif /* __IMX_CSU_H__ */
diff --git a/plat/freescale/imx8mq/include/imx_rdc.h b/plat/freescale/imx8mq/include/imx_rdc.h
index cdc7719..1b7179c 100644
--- a/plat/freescale/imx8mq/include/imx_rdc.h
+++ b/plat/freescale/imx8mq/include/imx_rdc.h
@@ -218,4 +218,7 @@
 int imx_rdc_set_masters(struct rdc_mda_conf *masters_list, uint32_t count);
 void imx_rdc_set_masters_default(void);
 
+#if defined (CSU_RDC_TEST)
+void rdc_test(void);
+#endif
 #endif	/* __IMX_RDC_H__*/