firmware/psci: Pass given partition number through

Pi 5 uses BL31 as its armstub file, so the reset goes via PSCI. Parse
any "reboot" parameter as a partition number to reboot into.
N.B. This code path is only used if reboot mode has been set to warm
or soft.

Signed-off-by: Phil Elwell <phil@raspberrypi.com>

diff --git a/drivers/firmware/psci/psci.c b/drivers/firmware/psci/psci.c
index d9629ff..e2ffd73 100644 (file)
--- a/drivers/firmware/psci/psci.c
+++ b/drivers/firmware/psci/psci.c
@@ -315,7 +315,14 @@ static int psci_sys_reset(struct notifier_block *nb, unsigned long action,
                 * reset_type[30:0] = 0 (SYSTEM_WARM_RESET)
                 * cookie = 0 (ignored by the implementation)
                 */
-               invoke_psci_fn(PSCI_FN_NATIVE(1_1, SYSTEM_RESET2), 0, 0, 0);
+               // Allow extra arguments separated by spaces after
+               // the partition number.
+               unsigned long val;
+               u8 partition = 0;
+
+               if (data && sscanf(data, "%lu", &val) == 1 && val < 63)
+                       partition = val;
+               invoke_psci_fn(PSCI_FN_NATIVE(1_1, SYSTEM_RESET2), 0, partition, 0);
        } else {
                invoke_psci_fn(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0);
        }