MA-11015 Support Trusty OS on imx8qm/qxp

Mapped the BL32 code into MMU due the Trusty
SPD need to check the code status and decide
the CPU executing mode.

To reserve and protect the memory for secure
world, modify the partition code to keep
BL32 spaces in secure_part.

Signed-off-by: Haoran.Wang <elven.wang@nxp.com>
Signed-off-by: Ji Luo  <ji.luo@nxp.com>
Reviewed-by: Ye Li <ye.li@nxp.com>
(cherry picked from commit 36ad60e46fd94eac7646218ae2b3e760bcfc33d6)

4 files changed, 269 insertions, 63 deletions

diff --git a/plat/imx/imx8qm/imx8qm_bl31_setup.c b/plat/imx/imx8qm/imx8qm_bl31_setup.c
index ae0879b7..c9cdf406 100644
--- a/plat/imx/imx8qm/imx8qm_bl31_setup.c
+++ b/plat/imx/imx8qm/imx8qm_bl31_setup.c
@@ -27,6 +27,9 @@
 #include <sci/sci.h>
 #include <sec_rsrc.h>
 #include <imx_sip_svc.h>
+#include <string.h>
+
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
 
 IMPORT_SYM(unsigned long, __COHERENT_RAM_START__, BL31_COHERENT_RAM_START);
 IMPORT_SYM(unsigned long, __COHERENT_RAM_END__, BL31_COHERENT_RAM_END);
@@ -166,6 +169,11 @@ void mx8_partition_resources(void)
 	bool owned, owned2;
 	sc_err_t err;
 	int i;
+#ifdef SPD_trusty
+	sc_rm_mr_t mr_tee = 64;
+	bool mr_tee_atf_same = false;
+	sc_faddr_t reg_start;
+#endif
 	uint32_t cpu_id, cpu_rev = 0x1; /* Set Rev B as default */
 
 	if (imx_get_cpu_rev(&cpu_id, &cpu_rev) != 0)
@@ -197,48 +205,113 @@ void mx8_partition_resources(void)
 		owned = sc_rm_is_memreg_owned(ipc_handle, mr);
 		if (owned) {
 			err = sc_rm_get_memreg_info(ipc_handle, mr, &start, &end);
-			if (err)
+			if (err) {
 				ERROR("Memreg get info failed, %u\n", mr);
-			NOTICE("Memreg %u 0x%llx -- 0x%llx\n", mr, start, end);
-			if (BL31_BASE >= start && (BL31_LIMIT - 1) <= end) {
-				mr_record = mr; /* Record the mr for ATF running */
+			} else {
+				NOTICE("Memreg %u 0x%llx -- 0x%llx\n", mr, start, end);
+
+				if (BL31_BASE >= start && (BL31_LIMIT - 1) <= end) {
+					mr_record = mr; /* Record the mr for ATF running */
+				}
+#ifdef SPD_trusty
+				else if (BL32_BASE >= start && (BL32_LIMIT -1) <= end) {
+					mr_tee = mr;
+				}
+#endif
+				else if (cpu_rev >= 1 && 0 >= start && (OCRAM_BASE + OCRAM_ALIAS_SIZE - 1) <= end) {
+					mr_ocram = mr;
+				}
+				else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx, err %d\n", start, end, err);
+				}
 			}
-			else if (cpu_rev >= 1 && 0 >= start && (OCRAM_BASE + OCRAM_ALIAS_SIZE - 1) <= end) {
-				mr_ocram = mr;
+		}
+	}
+
+#ifdef SPD_trusty
+	if (mr_tee != 64) {
+		err = sc_rm_get_memreg_info(ipc_handle, mr_tee, &start, &end);
+		if (err) {
+			ERROR("Memreg get info failed, %u\n", mr_tee);
+		} else {
+			if ((BL32_LIMIT - 1) < end) {
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, BL32_LIMIT , end);
+				if (err) {
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL32_LIMIT, end);
+				} else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL32_LIMIT, end);
+				}
 			}
-			else {
-				err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
-				if (err)
-					ERROR("Memreg assign failed, 0x%llx -- 0x%llx, \
-						err %d\n", start, end, err);
+
+			if (start < (BL32_BASE - 1)) {
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, start, BL32_BASE - 1);
+				if (err) {
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", start, (sc_faddr_t)BL32_BASE - 1);
+				} else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", start, (sc_faddr_t)BL32_BASE - 1);
+				}
 			}
 		}
 	}
-
+#endif
 	if (mr_record != 64) {
 		err = sc_rm_get_memreg_info(ipc_handle, mr_record, &start, &end);
-		if (err)
+
+#ifdef SPD_trusty
+		if (BL32_BASE >= start && (BL32_LIMIT - 1) <= end)
+			mr_tee_atf_same = true;
+#endif
+		reg_end = end;
+
+		if (err) {
 			ERROR("Memreg get info failed, %u\n", mr_record);
-		if ((BL31_LIMIT - 1) < end) {
-			err = sc_rm_memreg_alloc(ipc_handle, &mr, BL31_LIMIT, end);
-			if (err)
-				ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n",
-					(sc_faddr_t)BL31_LIMIT, end);
-			err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
-			if (err)
-				ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n",
-					(sc_faddr_t)BL31_LIMIT, end);
-		}
+		} else {
+			if ((BL31_LIMIT - 1) < end) {
+#ifdef SPD_trusty
+				if ((end > BL32_BASE) && mr_tee_atf_same)
+					reg_end = BL32_BASE - 1;
+#endif
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, BL31_LIMIT, reg_end);
+				if (err) {
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL31_LIMIT, reg_end);
+				} else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL31_LIMIT, reg_end);
+				}
+			}
+#ifdef SPD_trusty
+			if (mr_tee_atf_same) {
+				if ((BL32_LIMIT - 1) < end) {
+					reg_start = BL32_LIMIT;
+					err = sc_rm_memreg_alloc(ipc_handle, &mr, reg_start, end);
+					if (err) {
+						ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", reg_start, reg_end);
+					} else {
+						err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+						if (err)
+							ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", reg_start, reg_end);
+					}
+				}
+			}
+#endif
 
-		if (start < (BL31_BASE - 1)) {
-			err = sc_rm_memreg_alloc(ipc_handle, &mr, start, BL31_BASE - 1);
-			if (err)
-				ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n",
-					start, (sc_faddr_t)BL31_BASE - 1);
-			err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+			if (start < (BL31_BASE - 1)) {
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, start, BL31_BASE - 1);
 				if (err)
-					ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n",
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n",
 						start, (sc_faddr_t)BL31_BASE - 1);
+				err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n",
+							start, (sc_faddr_t)BL31_BASE - 1);
+			}
 		}
 	}
 
@@ -366,6 +439,14 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 
 	bl33_image_ep_info.pc = PLAT_NS_IMAGE_OFFSET;
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
+#ifdef SPD_trusty
+	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
+	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
+	bl32_image_ep_info.pc = BL32_BASE;
+	bl32_image_ep_info.spsr = 0;
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#endif
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
 	/* init the first cluster's cci slave interface */
@@ -390,6 +471,10 @@ void bl31_plat_arch_setup(void)
 		MT_RW | MT_MEMORY | MT_SECURE);
 	mmap_add(imx_mmap);
 
+#ifdef SPD_trusty
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+#endif
+
 #if USE_COHERENT_MEM
 	mmap_add_region(coh_start, coh_start, coh_size,
 			MT_DEVICE | MT_RW | MT_SECURE);
@@ -426,3 +511,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/imx8qm/include/platform_def.h b/plat/imx/imx8qm/include/platform_def.h
index 4b02a3f8..ff838ed2 100644
--- a/plat/imx/imx8qm/include/platform_def.h
+++ b/plat/imx/imx8qm/include/platform_def.h
@@ -43,6 +43,12 @@
 #define OCRAM_BASE		0x100000
 #define OCRAM_ALIAS_SIZE 0x18000 /* The lower 96KB is in OCRAM alias from 0x0 */
 
+#ifdef SPD_trusty
+#define BL32_BASE			0xfe000000
+#define BL32_SIZE			0x02000000
+#define BL32_LIMIT			0x100000000
+#endif
+
 #define PLAT_GICD_BASE			0x51a00000
 #define PLAT_GICR_BASE			0x51b00000
 #define PLAT_CCI_BASE			0x52090000
@@ -76,6 +82,10 @@
 #define SC_CONSOLE			0
 
 #define DEBUG_CONSOLE			0
+#ifdef SPD_trusty
+#define DEBUG_CONSOLE_A53		1
+#else
 #define DEBUG_CONSOLE_A53		0
+#endif
 
 #endif /* PLATFORM_DEF_H */
diff --git a/plat/imx/imx8qx/imx8qx_bl31_setup.c b/plat/imx/imx8qx/imx8qx_bl31_setup.c
index 24b773c1..0b10191c 100644
--- a/plat/imx/imx8qx/imx8qx_bl31_setup.c
+++ b/plat/imx/imx8qx/imx8qx_bl31_setup.c
@@ -27,6 +27,9 @@
 #include <sci/sci.h>
 #include <sec_rsrc.h>
 #include <imx_sip_svc.h>
+#include <string.h>
+
+#define TRUSTY_PARAMS_LEN_BYTES      (4096*2)
 
 IMPORT_SYM(unsigned long, __COHERENT_RAM_START__, BL31_COHERENT_RAM_START);
 IMPORT_SYM(unsigned long, __COHERENT_RAM_END__, BL31_COHERENT_RAM_END);
@@ -161,6 +164,11 @@ void imx8_partition_resources(void)
 	sc_err_t err;
 	bool owned;
 	int i;
+#ifdef SPD_trusty
+	sc_rm_mr_t mr_tee = 64;
+	bool mr_tee_atf_same = false;
+	sc_faddr_t reg_start;
+#endif
 	uint32_t cpu_id, cpu_rev = 0x1; /* Set Rev B as default */
 
 	if (imx_get_cpu_rev(&cpu_id, &cpu_rev) != 0)
@@ -197,49 +205,112 @@ void imx8_partition_resources(void)
 		owned = sc_rm_is_memreg_owned(ipc_handle, mr);
 		if (owned) {
 			err = sc_rm_get_memreg_info(ipc_handle, mr, &start, &end);
-			if (err)
+			if (err) {
 				ERROR("Memreg get info failed, %u\n", mr);
+			} else {
+				NOTICE("Memreg %u 0x%llx -- 0x%llx\n", mr, start, end);
 
-			NOTICE("Memreg %u 0x%llx -- 0x%llx\n", mr, start, end);
-			if (BL31_BASE >= start && (BL31_LIMIT - 1) <= end) {
-				mr_record = mr; /* Record the mr for ATF running */
+				if (BL31_BASE >= start && (BL31_LIMIT - 1) <= end) {
+					mr_record = mr; /* Record the mr for ATF running */
+				}
+#ifdef SPD_trusty
+				else if (BL32_BASE >= start && (BL32_LIMIT -1) <= end) {
+					mr_tee = mr;
+				}
+#endif
+				else if (cpu_rev >= 1 && 0 >= start && (OCRAM_BASE + OCRAM_ALIAS_SIZE - 1) <= end) {
+					mr_ocram = mr;
+				}
+				else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx, err %d\n", start, end, err);
+				}
 			}
-			else if (cpu_rev >= 1 && 0 >= start && (OCRAM_BASE + OCRAM_ALIAS_SIZE - 1) <= end) {
-				mr_ocram = mr;
+		}
+	}
+
+#ifdef SPD_trusty
+	if (mr_tee != 64) {
+		err = sc_rm_get_memreg_info(ipc_handle, mr_tee, &start, &end);
+		if (err) {
+			ERROR("Memreg get info failed, %u\n", mr_tee);
+		} else {
+			if ((BL32_LIMIT - 1) < end) {
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, BL32_LIMIT , end);
+				if (err) {
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL32_LIMIT, end);
+				} else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL32_LIMIT, end);
+				}
 			}
-			else {
-				err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
-				if (err)
-					ERROR("Memreg assign failed, 0x%llx -- 0x%llx, \
-						err %d\n", start, end, err);
+
+			if (start < (BL32_BASE - 1)) {
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, start, BL32_BASE - 1);
+				if (err) {
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", start, (sc_faddr_t)BL32_BASE - 1);
+				} else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", start, (sc_faddr_t)BL32_BASE - 1);
+				}
 			}
 		}
 	}
-
+#endif
 	if (mr_record != 64) {
 		err = sc_rm_get_memreg_info(ipc_handle, mr_record, &start, &end);
-		if (err)
+
+#ifdef SPD_trusty
+		if (BL32_BASE >= start && (BL32_LIMIT - 1) <= end)
+			mr_tee_atf_same = true;
+#endif
+		reg_end = end;
+		if (err) {
 			ERROR("Memreg get info failed, %u\n", mr_record);
-		if ((BL31_LIMIT - 1) < end) {
-			err = sc_rm_memreg_alloc(ipc_handle, &mr, BL31_LIMIT, end);
-			if (err)
-				ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n",
-					(sc_faddr_t)BL31_LIMIT, end);
-			err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
-			if (err)
-				ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n",
-					(sc_faddr_t)BL31_LIMIT, end);
-		}
+		} else {
+			if ((BL31_LIMIT - 1) < end) {
+#ifdef SPD_trusty
+				if ((end > BL32_BASE) && mr_tee_atf_same)
+					reg_end = BL32_BASE - 1;
+#endif
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, BL31_LIMIT, reg_end);
+				if (err) {
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL31_LIMIT, reg_end);
+				} else {
+					err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", (sc_faddr_t)BL31_LIMIT, reg_end);
+				}
+			}
+#ifdef SPD_trusty
+			if (mr_tee_atf_same) {
+				if ((BL32_LIMIT - 1) < end) {
+					reg_start = BL32_LIMIT;
+					err = sc_rm_memreg_alloc(ipc_handle, &mr, reg_start, end);
+					if (err) {
+						ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n", reg_start, reg_end);
+					} else {
+						err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+					if (err)
+						ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n", reg_start, reg_end);
+					}
+				}
+			}
+#endif
 
-		if (start < (BL31_BASE - 1)) {
-			err = sc_rm_memreg_alloc(ipc_handle, &mr, start, BL31_BASE - 1);
-			if (err)
-				ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n",
-					start, (sc_faddr_t)BL31_BASE - 1);
-			err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
-			if (err)
-				ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n",
-					start, (sc_faddr_t)BL31_BASE - 1);
+			if (start < (BL31_BASE - 1)) {
+				err = sc_rm_memreg_alloc(ipc_handle, &mr, start, BL31_BASE - 1);
+				if (err)
+					ERROR("sc_rm_memreg_alloc failed, 0x%llx -- 0x%llx\n",
+						start, (sc_faddr_t)BL31_BASE - 1);
+				err = sc_rm_assign_memreg(ipc_handle, os_part, mr);
+				if (err)
+					ERROR("Memreg assign failed, 0x%llx -- 0x%llx\n",
+						start, (sc_faddr_t)BL31_BASE - 1);
+			}
 		}
 	}
 
@@ -345,6 +416,14 @@ void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 
 	bl33_image_ep_info.pc = PLAT_NS_IMAGE_OFFSET;
 	bl33_image_ep_info.spsr = get_spsr_for_bl33_entry();
+#ifdef SPD_trusty
+	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
+	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
+	bl32_image_ep_info.pc = BL32_BASE;
+	bl32_image_ep_info.spsr = 0;
+	bl32_image_ep_info.args.arg0 = BL32_SIZE;
+	bl32_image_ep_info.args.arg1 = BL32_BASE;
+#endif
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 }
 
@@ -365,6 +444,10 @@ void bl31_plat_arch_setup(void)
 		MT_RW | MT_MEMORY | MT_SECURE);
 	mmap_add(imx_mmap);
 
+#ifdef SPD_trusty
+	mmap_add_region(BL32_BASE, BL32_BASE, BL32_SIZE, MT_MEMORY | MT_RW);
+#endif
+
 #if USE_COHERENT_MEM
 	mmap_add_region(coh_start, coh_start, coh_size,
 			MT_DEVICE | MT_RW | MT_SECURE);
@@ -399,3 +482,11 @@ 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/imx8qx/include/platform_def.h b/plat/imx/imx8qx/include/platform_def.h
index 6e9d13c5..785e3bf8 100644
--- a/plat/imx/imx8qx/include/platform_def.h
+++ b/plat/imx/imx8qx/include/platform_def.h
@@ -38,11 +38,17 @@
 #define OCRAM_BASE		0x100000
 #define OCRAM_ALIAS_SIZE 0x18000 /* The lower 96KB is in OCRAM alias from 0x0 */
 
+#ifdef SPD_trusty
+#define BL32_BASE			0xfe000000
+#define BL32_SIZE			0x02000000
+#define BL32_LIMIT			0x100000000
+#endif
+
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ull << 32)
 #define PLAT_PHY_ADDR_SPACE_SIZE	(1ull << 32)
 
 #define MAX_XLAT_TABLES			8
-#define MAX_MMAP_REGIONS		8
+#define MAX_MMAP_REGIONS		9
 
 #define PLAT_GICD_BASE			0x51a00000
 #define PLAT_GICR_BASE			0x51b00000
@@ -68,6 +74,11 @@
 #define SC_CONSOLE			0
 
 #define DEBUG_CONSOLE			0
+
+#ifdef SPD_trusty
+#define DEBUG_CONSOLE_A35		1
+#else
 #define DEBUG_CONSOLE_A35		0
+#endif
 
 #endif /* PLATFORM_DEF_H */