X-Git-Url: https://git.sur5r.net/?a=blobdiff_plain;f=arch%2Farm%2Fcpu%2Farmv8%2Fzynqmp%2Fmp.c;h=2a71870ae7bccd81dbbe3b3a1db5db063a3379db;hb=20b016a33665f7b3ff875b4b7063180eb955f092;hp=17e32a7b7ce8426e9895938b6772de5388e55c72;hpb=b939689c7b87773c44275a578ffc8674a867e39d;p=u-boot

diff --git a/arch/arm/cpu/armv8/zynqmp/mp.c b/arch/arm/cpu/armv8/zynqmp/mp.c
index 17e32a7b7c..2a71870ae7 100644
--- a/arch/arm/cpu/armv8/zynqmp/mp.c
+++ b/arch/arm/cpu/armv8/zynqmp/mp.c
@@ -1,8 +1,7 @@
+// SPDX-License-Identifier: GPL-2.0+
 /*
  * (C) Copyright 2014 - 2015 Xilinx, Inc.
  * Michal Simek <michal.simek@xilinx.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
  */
 
 #include <common.h>
@@ -46,7 +45,7 @@ int is_core_valid(unsigned int core)
 	return 0;
 }
 
-int cpu_reset(int nr)
+int cpu_reset(u32 nr)
 {
 	puts("Feature is not implemented.\n");
 	return 0;
@@ -128,11 +127,11 @@ static void enable_clock_r5(void)
 	writel(tmp, &crlapb_base->cpu_r5_ctrl);
 
 	/* Give some delay for clock
-	 * to propogate */
+	 * to propagate */
 	udelay(0x500);
 }
 
-int cpu_disable(int nr)
+int cpu_disable(u32 nr)
 {
 	if (nr >= ZYNQMP_CORE_APU0 && nr <= ZYNQMP_CORE_APU3) {
 		u32 val = readl(&crfapb_base->rst_fpd_apu);
@@ -145,7 +144,7 @@ int cpu_disable(int nr)
 	return 0;
 }
 
-int cpu_status(int nr)
+int cpu_status(u32 nr)
 {
 	if (nr >= ZYNQMP_CORE_APU0 && nr <= ZYNQMP_CORE_APU3) {
 		u32 addr_low = readl(((u8 *)&apu_base->rvbar_addr0_l) + nr * 8);
@@ -183,7 +182,45 @@ static void set_r5_start(u8 high)
 	writel(tmp, &rpu_base->rpu1_cfg);
 }
 
-int cpu_release(int nr, int argc, char * const argv[])
+static void write_tcm_boot_trampoline(u32 boot_addr)
+{
+	if (boot_addr) {
+		/*
+		 * Boot trampoline is simple ASM code below.
+		 *
+		 *		b over;
+		 *	label:
+		 *	.word	0
+		 *	over:	ldr	r0, =label
+		 *		ldr	r1, [r0]
+		 *		bx	r1
+		 */
+		debug("Write boot trampoline for %x\n", boot_addr);
+		writel(0xea000000, ZYNQMP_TCM_START_ADDRESS);
+		writel(boot_addr, ZYNQMP_TCM_START_ADDRESS + 0x4);
+		writel(0xe59f0004, ZYNQMP_TCM_START_ADDRESS + 0x8);
+		writel(0xe5901000, ZYNQMP_TCM_START_ADDRESS + 0xc);
+		writel(0xe12fff11, ZYNQMP_TCM_START_ADDRESS + 0x10);
+		writel(0x00000004, ZYNQMP_TCM_START_ADDRESS + 0x14); // address for
+	}
+}
+
+void initialize_tcm(bool mode)
+{
+	if (!mode) {
+		set_r5_tcm_mode(LOCK);
+		set_r5_halt_mode(HALT, LOCK);
+		enable_clock_r5();
+		release_r5_reset(LOCK);
+	} else {
+		set_r5_tcm_mode(SPLIT);
+		set_r5_halt_mode(HALT, SPLIT);
+		enable_clock_r5();
+		release_r5_reset(SPLIT);
+	}
+}
+
+int cpu_release(u32 nr, int argc, char * const argv[])
 {
 	if (nr >= ZYNQMP_CORE_APU0 && nr <= ZYNQMP_CORE_APU3) {
 		u64 boot_addr = simple_strtoull(argv[0], NULL, 16);
@@ -205,32 +242,50 @@ int cpu_release(int nr, int argc, char * const argv[])
 		}
 
 		u32 boot_addr = simple_strtoul(argv[0], NULL, 16);
+		u32 boot_addr_uniq = 0;
 		if (!(boot_addr == ZYNQMP_R5_LOVEC_ADDR ||
 		      boot_addr == ZYNQMP_R5_HIVEC_ADDR)) {
-			printf("Invalid starting address 0x%x\n", boot_addr);
-			printf("0 or 0xffff0000 are permitted\n");
-			return 1;
+			printf("Using TCM jump trampoline for address 0x%x\n",
+			       boot_addr);
+			/* Save boot address for later usage */
+			boot_addr_uniq = boot_addr;
+			/*
+			 * R5 needs to start from LOVEC at TCM
+			 * OCM will be probably occupied by ATF
+			 */
+			boot_addr = ZYNQMP_R5_LOVEC_ADDR;
 		}
 
+		/*
+		 * Since we don't know where the user may have loaded the image
+		 * for an R5 we have to flush all the data cache to ensure
+		 * the R5 sees it.
+		 */
+		flush_dcache_all();
+
 		if (!strncmp(argv[1], "lockstep", 8)) {
 			printf("R5 lockstep mode\n");
+			set_r5_reset(LOCK);
 			set_r5_tcm_mode(LOCK);
 			set_r5_halt_mode(HALT, LOCK);
-
-			if (boot_addr == 0)
-				set_r5_start(0);
-			else
-				set_r5_start(1);
-
+			set_r5_start(boot_addr);
 			enable_clock_r5();
 			release_r5_reset(LOCK);
+			dcache_disable();
+			write_tcm_boot_trampoline(boot_addr_uniq);
+			dcache_enable();
 			set_r5_halt_mode(RELEASE, LOCK);
 		} else if (!strncmp(argv[1], "split", 5)) {
 			printf("R5 split mode\n");
+			set_r5_reset(SPLIT);
 			set_r5_tcm_mode(SPLIT);
 			set_r5_halt_mode(HALT, SPLIT);
+			set_r5_start(boot_addr);
 			enable_clock_r5();
 			release_r5_reset(SPLIT);
+			dcache_disable();
+			write_tcm_boot_trampoline(boot_addr_uniq);
+			dcache_enable();
 			set_r5_halt_mode(RELEASE, SPLIT);
 		} else {
 			printf("Unsupported mode\n");