rpi: Implement PSCI CPU_OFF

We simulate the PSCI CPU_OFF operation by reseting the core via RMR.
For secondaries, that already puts them in the holding pen waiting for a
"warm boot" request as part of PSCI CPU_ON. For the BSP, we have to add
logic to distinguish a regular boot from a CPU_OFF state, where, like the
secondaries, the BSP needs to wait foor a "warm boot" request as part
of CPU_ON.

Testing done:

- ACS suite now passes more tests (since it repeatedly
calls code on secondaries via CPU_ON).

- Linux testing including offlining/onlineing CPU0, e.g.
"echo 0 > /sys/devices/system/cpu/cpu0/online".

Change-Id: Id0ae11a0ee0721b20fa2578b54dadc72dcbd69e0
Link: https://developer.trustedfirmware.org/T686

Signed-off-by: Andrei Warkentin <andrey.warkentin@gmail.com>
[Andre: adapt to unified plat_helpers.S, smaller fixes]
Signed-off-by: Andre Przywara <andre.przywara@arm.com>

plat/rpi/common/aarch64/plat_helpers.S

@@ -63,21 +63,23 @@ func plat_is_my_cpu_primary
 endfunc plat_is_my_cpu_primary
 
 	/* -----------------------------------------------------
-	 * void plat_secondary_cold_boot_setup (void);
+	 * void plat_wait_for_warm_boot (void);
 	 *
 	 * This function performs any platform specific actions
-	 * needed for a secondary cpu after a cold reset e.g
-	 * mark the cpu's presence, mechanism to place it in a
-	 * holding pen etc.
+	 * needed for a CPU to be put into holding pen to wait
+	 * for a warm boot request.
+	 * The function will never return.
 	 * -----------------------------------------------------
 	 */
-func plat_secondary_cold_boot_setup
-	/* Calculate address of our hold entry */
+func plat_wait_for_warm_boot
+	/*
+	 * Calculate address of our hold entry.
+	 * As the function will never return, there is no need to save LR.
+	 */
 	bl	plat_my_core_pos
 	lsl	x0, x0, #3
 	mov_imm	x2, PLAT_RPI3_TM_HOLD_BASE
 	add	x0, x0, x2
-
 	/*
 	 * This code runs way before requesting the warmboot of this core,
 	 * so it is possible to clear the mailbox before getting a request
@@ -97,6 +99,19 @@ poll_mailbox:
 	mov_imm	x0, PLAT_RPI3_TM_ENTRYPOINT
 	ldr	x1, [x0]
 	br	x1
+endfunc plat_wait_for_warm_boot
+
+	/* -----------------------------------------------------
+	 * void plat_secondary_cold_boot_setup (void);
+	 *
+	 * This function performs any platform specific actions
+	 * needed for a secondary cpu after a cold reset e.g
+	 * mark the cpu's presence, mechanism to place it in a
+	 * holding pen etc.
+	 * -----------------------------------------------------
+	 */
+func plat_secondary_cold_boot_setup
+	b	plat_wait_for_warm_boot
 endfunc plat_secondary_cold_boot_setup
 
 	/* ---------------------------------------------------------------------
@@ -111,9 +126,24 @@ endfunc plat_secondary_cold_boot_setup
 	 * ---------------------------------------------------------------------
 	 */
 func plat_get_my_entrypoint
-	/* TODO: support warm boot */
-	mov	x0, #0
-	ret
+	mov	x1, x30
+	bl	plat_is_my_cpu_primary
+	/*
+	 * Secondaries always cold boot.
+	*/
+	cbz	w0, 1f
+	/*
+	 * Primaries warm boot if they are requested
+	 * to power off.
+	 */
+	mov_imm	x0, PLAT_RPI3_TM_HOLD_BASE
+	ldr	x0, [x0]
+	cmp	x0, PLAT_RPI3_TM_HOLD_STATE_BSP_OFF
+	adr	x0, plat_wait_for_warm_boot
+	csel	x0, x0, xzr, eq
+	ret	x1
+1:	mov	x0, #0
+	ret	x1
 endfunc plat_get_my_entrypoint
 
 	/* ---------------------------------------------

plat/rpi/common/rpi3_pm.c

@@ -174,6 +174,32 @@ static void rpi3_pwr_domain_on_finish(const psci_power_state_t *target_state)
 #endif
 }
 
+static void __dead2 rpi3_pwr_down_wfi(
+		const psci_power_state_t *target_state)
+{
+	uintptr_t hold_base = PLAT_RPI3_TM_HOLD_BASE;
+	unsigned int pos = plat_my_core_pos();
+
+	if (pos == 0) {
+		/*
+		 * The secondaries will always be in a wait
+		 * for warm boot on reset, but the BSP needs
+		 * to be able to distinguish between waiting
+		 * for warm boot (e.g. after psci_off, waiting
+		 * for psci_on) and a cold boot.
+		 */
+		mmio_write_64(hold_base, PLAT_RPI3_TM_HOLD_STATE_BSP_OFF);
+		/* No cache maintenance here, we run with caches off already. */
+		dsb();
+		isb();
+	}
+
+	write_rmr_el3(RMR_EL3_RR_BIT | RMR_EL3_AA64_BIT);
+
+	while (1)
+		;
+}
+
 /*******************************************************************************
  * Platform handlers for system reset and system off.
  ******************************************************************************/
@@ -239,6 +265,7 @@ static const plat_psci_ops_t plat_rpi3_psci_pm_ops = {
 	.pwr_domain_pwr_down_wfi = rpi3_pwr_domain_pwr_down_wfi,
 	.pwr_domain_on = rpi3_pwr_domain_on,
 	.pwr_domain_on_finish = rpi3_pwr_domain_on_finish,
+	.pwr_domain_pwr_down_wfi = rpi3_pwr_down_wfi,
 	.system_off = rpi3_system_off,
 	.system_reset = rpi3_system_reset,
 	.validate_power_state = rpi3_validate_power_state,

plat/rpi/rpi3/include/platform_def.h

@@ -153,6 +153,7 @@
 
 #define PLAT_RPI3_TM_HOLD_STATE_WAIT	ULL(0)
 #define PLAT_RPI3_TM_HOLD_STATE_GO	ULL(1)
+#define PLAT_RPI3_TM_HOLD_STATE_BSP_OFF	ULL(2)
 
 /*
  * BL1 specific defines.

plat/rpi/rpi4/include/platform_def.h

@@ -93,6 +93,7 @@
 
 #define PLAT_RPI3_TM_HOLD_STATE_WAIT	ULL(0)
 #define PLAT_RPI3_TM_HOLD_STATE_GO	ULL(1)
+#define PLAT_RPI3_TM_HOLD_STATE_BSP_OFF	ULL(2)
 
 /*
  * BL31 specific defines.