From 35de6b3b61cc4dc3ec70420c21f0a55c46471252 Mon Sep 17 00:00:00 2001
From: Phil Elwell <phil@raspberrypi.com>
Date: Mon, 11 Dec 2023 16:58:07 +0000
Subject: [PATCH] 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>
---
 drivers/firmware/psci/psci.c | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

--- a/drivers/firmware/psci/psci.c
+++ b/drivers/firmware/psci/psci.c
@@ -315,7 +315,14 @@ static int psci_sys_reset(struct notifie
 		 * 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);
 	}
