[PATCH v1 1/1] arm64: zynqmp: dynamically mark r5 cores as used

[Lukas Funke]

From: Lukas Funke <lukas.funke at weidmueller.com>

When Linux boot takes over control of the pmu
(by signaling PM_INIT_FINALIZE via ipi), pmu will switch off 'unused'
rpu cores. The Xilinx zynqmp fsbl prevents switching off those cores by
marking rpu cores as 'used' when loading code partitions to those cores.
The current u-boot SPL is missing this behaviour, which results in
halting rpu cores during Linux boot.

This commit mimics the xilinx zynqmp fsbl behavior by marking r5 cores as
used when they are released during boot.

Signed-off-by: Lukas Funke <lukas.funke at weidmueller.com>
Signed-off-by: Lukas Funke <lukas.funke-oss at weidmueller.com>
---

 arch/arm/mach-zynqmp/include/mach/hardware.h |  4 ++-
 arch/arm/mach-zynqmp/mp.c                    | 26 ++++++++++++++++++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/arch/arm/mach-zynqmp/include/mach/hardware.h b/arch/arm/mach-zynqmp/include/mach/hardware.h
index a70d6d611b..70221e0305 100644
--- a/arch/arm/mach-zynqmp/include/mach/hardware.h
+++ b/arch/arm/mach-zynqmp/include/mach/hardware.h
@@ -175,7 +175,9 @@ struct csu_regs {
 #define ZYNQMP_PMU_BASEADDR	0xFFD80000
 
 struct pmu_regs {
-	u32 reserved[18];
+	u32 reserved0[16];
+	u32 gen_storage4; /* 0x40 */
+	u32 reserved1[1];
 	u32 gen_storage6; /* 0x48 */
 };
 
diff --git a/arch/arm/mach-zynqmp/mp.c b/arch/arm/mach-zynqmp/mp.c
index 949456d530..2891878973 100644
--- a/arch/arm/mach-zynqmp/mp.c
+++ b/arch/arm/mach-zynqmp/mp.c
@@ -42,6 +42,9 @@
 
 #define ZYNQMP_MAX_CORES	6
 
+#define ZYNQMP_RPU0_USE_MASK BIT(1)
+#define ZYNQMP_RPU1_USE_MASK BIT(2)
+
 int is_core_valid(unsigned int core)
 {
 	if (core < ZYNQMP_MAX_CORES)
@@ -250,6 +253,27 @@ void initialize_tcm(bool mode)
 	}
 }
 
+static void mark_r5_used(u32 nr, u8 mode)
+{
+	u32 mask = 0;
+
+	if (mode == LOCK) {
+		mask = ZYNQMP_RPU0_USE_MASK | ZYNQMP_RPU1_USE_MASK;
+	} else {
+		switch (nr) {
+		case ZYNQMP_CORE_RPU0:
+			mask = ZYNQMP_RPU0_USE_MASK;
+			break;
+		case ZYNQMP_CORE_RPU1:
+			mask = ZYNQMP_RPU1_USE_MASK;
+			break;
+		default:
+			return;
+		}
+	}
+	zynqmp_mmio_write((ulong)&pmu_base->gen_storage4, mask, mask);
+}
+
 int cpu_release(u32 nr, int argc, char *const argv[])
 {
 	if (nr <= ZYNQMP_CORE_APU3) {
@@ -305,6 +329,7 @@ int cpu_release(u32 nr, int argc, char *const argv[])
 			write_tcm_boot_trampoline(boot_addr_uniq);
 			dcache_enable();
 			set_r5_halt_mode(nr, RELEASE, LOCK);
+			mark_r5_used(nr, LOCK);
 		} else if (!strncmp(argv[1], "split", 5)) {
 			printf("R5 split mode\n");
 			set_r5_reset(nr, SPLIT);
@@ -317,6 +342,7 @@ int cpu_release(u32 nr, int argc, char *const argv[])
 			write_tcm_boot_trampoline(boot_addr_uniq);
 			dcache_enable();
 			set_r5_halt_mode(nr, RELEASE, SPLIT);
+			mark_r5_used(nr, SPLIT);
 		} else {
 			printf("Unsupported mode\n");
 			return 1;
-- 
2.30.2