arm: psci: Add API to check for support of specific PSCI function

Add an API to check if a specific PSCI function is supported or not.
This is based on the psci_features() function present in Linux kernel
(drivers/firmware/psci/psci.c).

Reviewed-by: Casey Connolly <[email protected]>
Reviewed-by: Sumit Garg <[email protected]>
Signed-off-by: Varadarajan Narayanan <[email protected]>
Link: https://patch.msgid.link/[email protected]
Signed-off-by: Casey Connolly <[email protected]>

2 files changed, 16 insertions, 0 deletions

diff --git a/arch/arm/cpu/armv8/fwcall.c b/arch/arm/cpu/armv8/fwcall.c
index 87de09979b1..f834d770dd6 100644
--- a/arch/arm/cpu/armv8/fwcall.c
+++ b/arch/arm/cpu/armv8/fwcall.c
@@ -129,3 +129,18 @@ void __noreturn psci_system_off(void)
 	while (1)
 		;
 }
+
+int psci_features(u32 psci_func_id)
+{
+	struct pt_regs regs;
+
+	regs.regs[0] = ARM_PSCI_1_0_FN_PSCI_FEATURES;
+	regs.regs[1] = psci_func_id;
+
+	if (use_smc_for_psci)
+		smc_call(&regs);
+	else
+		hvc_call(&regs);
+
+	return regs.regs[0];
+}
diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
index 9e3ad57073d..5ed6833c155 100644
--- a/arch/arm/include/asm/system.h
+++ b/arch/arm/include/asm/system.h
@@ -342,6 +342,7 @@ void smc_call(struct pt_regs *args);
 void __noreturn psci_system_reset(void);
 void __noreturn psci_system_reset2(u32 reset_level, u32 cookie);
 void __noreturn psci_system_off(void);
+int psci_features(u32 psci_func_id);
 
 #ifdef CONFIG_ARMV8_PSCI
 extern char __secure_start[];