arm64: zynqmp: Fix application loading on R5 core1

From U-Boot, loading application on RPU core 0 is fine but loading on
core 1 is not handled properly. Lock-step mode needs both the R5 cores
to be initialized and it is working fine. Whereas in SPLIT mode individual
R5 cores needs to be initialized as they need to execute differenet
applications. Handle both these lock-step and split modes by propagating
mode and RPU core number(4 for RPU0 and 5 for RPU1) for various functions
and by adding conditions in those functions.

Signed-off-by: Ashok Reddy Soma <ashok.reddy.soma@xilinx.com>
Signed-off-by: Michal Simek <michal.simek@xilinx.com>

1 files changed, 39 insertions, 31 deletions

diff --git a/arch/arm/mach-zynqmp/mp.c b/arch/arm/mach-zynqmp/mp.c
index 656678a155..74783ae5a7 100644
--- a/arch/arm/mach-zynqmp/mp.c
+++ b/arch/arm/mach-zynqmp/mp.c
@@ -37,6 +37,8 @@
 
 #define ZYNQMP_CORE_APU0	0
 #define ZYNQMP_CORE_APU3	3
+#define ZYNQMP_CORE_RPU0	4
+#define ZYNQMP_CORE_RPU1	5
 
 #define ZYNQMP_MAX_CORES	6
 
@@ -54,18 +56,20 @@ int cpu_reset(u32 nr)
 	return 0;
 }
 
-static void set_r5_halt_mode(u8 halt, u8 mode)
+static void set_r5_halt_mode(u32 nr, u8 halt, u8 mode)
 {
 	u32 tmp;
 
-	tmp = readl(&rpu_base->rpu0_cfg);
-	if (halt == HALT)
-		tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
-	else
-		tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
-	writel(tmp, &rpu_base->rpu0_cfg);
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0) {
+		tmp = readl(&rpu_base->rpu0_cfg);
+		if (halt == HALT)
+			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+		else
+			tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+		writel(tmp, &rpu_base->rpu0_cfg);
+	}
 
-	if (mode == LOCK) {
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1) {
 		tmp = readl(&rpu_base->rpu1_cfg);
 		if (halt == HALT)
 			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
@@ -93,30 +97,34 @@ static void set_r5_tcm_mode(u8 mode)
 	writel(tmp, &rpu_base->rpu_glbl_ctrl);
 }
 
-static void set_r5_reset(u8 mode)
+static void set_r5_reset(u32 nr, u8 mode)
 {
 	u32 tmp;
 
 	tmp = readl(&crlapb_base->rst_lpd_top);
-	tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
-	       ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0)
+		tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
 
-	if (mode == LOCK)
-		tmp |= ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1)
+		tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK);
 
 	writel(tmp, &crlapb_base->rst_lpd_top);
 }
 
-static void release_r5_reset(u8 mode)
+static void release_r5_reset(u32 nr, u8 mode)
 {
 	u32 tmp;
 
 	tmp = readl(&crlapb_base->rst_lpd_top);
-	tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
-	       ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0)
+		tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			 ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
 
-	if (mode == LOCK)
-		tmp &= ~ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1)
+		tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			 ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK);
 
 	writel(tmp, &crlapb_base->rst_lpd_top);
 }
@@ -141,7 +149,7 @@ int cpu_disable(u32 nr)
 		val |= 1 << nr;
 		writel(val, &crfapb_base->rst_fpd_apu);
 	} else {
-		set_r5_reset(LOCK);
+		set_r5_reset(nr, SPLIT);
 	}
 
 	return 0;
@@ -212,14 +220,14 @@ void initialize_tcm(bool mode)
 {
 	if (!mode) {
 		set_r5_tcm_mode(LOCK);
-		set_r5_halt_mode(HALT, LOCK);
+		set_r5_halt_mode(ZYNQMP_CORE_RPU0, HALT, LOCK);
 		enable_clock_r5();
-		release_r5_reset(LOCK);
+		release_r5_reset(ZYNQMP_CORE_RPU0, LOCK);
 	} else {
 		set_r5_tcm_mode(SPLIT);
-		set_r5_halt_mode(HALT, SPLIT);
+		set_r5_halt_mode(ZYNQMP_CORE_RPU1, HALT, SPLIT);
 		enable_clock_r5();
-		release_r5_reset(SPLIT);
+		release_r5_reset(ZYNQMP_CORE_RPU1, SPLIT);
 	}
 }
 
@@ -268,28 +276,28 @@ int cpu_release(u32 nr, int argc, char *const argv[])
 
 		if (!strncmp(argv[1], "lockstep", 8)) {
 			printf("R5 lockstep mode\n");
-			set_r5_reset(LOCK);
+			set_r5_reset(nr, LOCK);
 			set_r5_tcm_mode(LOCK);
-			set_r5_halt_mode(HALT, LOCK);
+			set_r5_halt_mode(nr, HALT, LOCK);
 			set_r5_start(boot_addr);
 			enable_clock_r5();
-			release_r5_reset(LOCK);
+			release_r5_reset(nr, LOCK);
 			dcache_disable();
 			write_tcm_boot_trampoline(boot_addr_uniq);
 			dcache_enable();
-			set_r5_halt_mode(RELEASE, LOCK);
+			set_r5_halt_mode(nr, RELEASE, LOCK);
 		} else if (!strncmp(argv[1], "split", 5)) {
 			printf("R5 split mode\n");
-			set_r5_reset(SPLIT);
+			set_r5_reset(nr, SPLIT);
 			set_r5_tcm_mode(SPLIT);
-			set_r5_halt_mode(HALT, SPLIT);
+			set_r5_halt_mode(nr, HALT, SPLIT);
 			set_r5_start(boot_addr);
 			enable_clock_r5();
-			release_r5_reset(SPLIT);
+			release_r5_reset(nr, SPLIT);
 			dcache_disable();
 			write_tcm_boot_trampoline(boot_addr_uniq);
 			dcache_enable();
-			set_r5_halt_mode(RELEASE, SPLIT);
+			set_r5_halt_mode(nr, RELEASE, SPLIT);
 		} else {
 			printf("Unsupported mode\n");
 			return 1;