[dev][psci] clean up the psci driver a bit, add arg to cpu_on

- General cleanup of the driver a bit
- Aadd a boot time message that prints the version
- Add the argument field to CPU_ON
- Pass the cpu number through from fdtwalk library

dev/power/psci/include/dev/power/psci.h

@@ -13,7 +13,7 @@
 __BEGIN_CDECLS
 
 uint32_t psci_version(void);
-int psci_cpu_on(int corenr, ulong entrypoint);
+int psci_cpu_on(ulong mpidr, ulong entrypoint, uint32_t cpu_num);
 void psci_system_off(void);
 void psci_system_reset(void);
 

dev/power/psci/psci.c

@@ -5,50 +5,92 @@
  * license that can be found in the LICENSE file or at
  * https://opensource.org/licenses/MIT
  */
-#include <dev/power/psci.h>
+#include "dev/power/psci.h"
+
+#include <lk/debug.h>
+#include <lk/init.h>
+#include <stdbool.h>
 
 #if WITH_LIB_CONSOLE
 #include <lk/console_cmd.h>
-#include <stdio.h>
 #endif
 
-#define PSCI_VERSION  0x84000000
-#define SYSTEM_OFF    0x84000008
-#define SYSTEM_RESET  0x84000009
+// PSCI function numbers as of PSCI 1.3 (DEN 0022 F.b)
+enum psci_function {
+    PSCI_VERSION = 0x84000000,
+    PSCI_CPU_SUSPEND = 0x84000001,
+    PSCI_CPU_OFF = 0x84000002,
+    PSCI_CPU_ON = 0x84000003,
+    PSCI_AFFINITY_INFO = 0x84000004,
+    PSCI_MIGRATE = 0x84000005,
+    PSCI_MIGRATE_INFO_TYPE = 0x84000006,
+    PSCI_MIGRATE_INFO_UP_CPU = 0x84000007,
+    PSCI_SYSTEM_OFF = 0x84000008,
+    PSCI_SYSTEM_RESET = 0x84000009,
+    PSCI_SYSTEM_RESET2 = 0x84000012,
+    PSCI_FEATURES = 0x8400000A,
+    PSCI_CPU_FREEZE = 0x8400000B,
+    PSCI_CPU_DEFAULT_SUSPEND = 0x8400000C,
+    PSCI_NODE_HW_STATE = 0x8400000D,
+    PSCI_SYSTEM_SUSPEND = 0x8400000E,
+    PSCI_SET_SUSPEND_MODE = 0x8400000F,
+    PSCI_STAT_RESIDENCY = 0x84000010,
+    PSCI_STAT_COUNT = 0x84000011,
+    PSCI_MEM_PROTECT = 0x84000013,
+    PSCI_MEM_PROTECT_RANGE = 0xC4000014,
+    PSCI_SYSTEM_OFF2 = 0xC4000015,
 
-#if ARCH_ARM
-#define CPU_ON        0x84000003
-#endif
+    PSCI_SMC64_FLAG = 0x40000000,
+};
 
-#if ARCH_ARM64
-#define CPU_ON        0xC4000003
-#endif
+static bool psci_present = false;
 
 /* low level ASM routine to make the raw PSCI call */
 int psci_call(ulong arg0, ulong arg1, ulong arg2, ulong arg3);
 
-uint32_t psci_version(void) {
-  return psci_call(PSCI_VERSION, 0, 0, 0);
+// wrap the function number in the SMC64 flag if we're on aarch64
+static inline uint32_t psci_function_64(uint32_t func) {
+#if __aarch64__
+    return func | PSCI_SMC64_FLAG;
+#else
+    return func;
+#endif
 }
 
-int psci_cpu_on(int corenr, ulong entrypoint) {
-  return psci_call(CPU_ON, corenr, entrypoint, corenr);
+uint32_t psci_version(void) {
+    return psci_call(PSCI_VERSION, 0, 0, 0);
+}
+
+int psci_cpu_on(ulong mpidr, ulong entrypoint, uint32_t cpu_num) {
+    return psci_call(psci_function_64(PSCI_CPU_ON), mpidr, entrypoint, cpu_num);
 }
 
 void psci_system_off(void) {
-  psci_call(SYSTEM_OFF, 0, 0, 0);
+    psci_call(PSCI_SYSTEM_OFF, 0, 0, 0);
 }
 
 void psci_system_reset(void) {
-  psci_call(SYSTEM_RESET, 0, 0, 0);
+    psci_call(PSCI_SYSTEM_RESET, 0, 0, 0);
 }
 
+void psci_detect(uint level) {
+    uint32_t ver = psci_version();
+    if (ver == 0) {
+        // no psci support
+        return;
+    }
+    psci_present = true;
+    dprintf(INFO, "PSCI: detected version %x:%x\n", ver >> 16, ver & 0xffff);
+}
+
+LK_INIT_HOOK(psci, psci_detect, LK_INIT_LEVEL_PLATFORM_EARLY);
+
 #if WITH_LIB_CONSOLE
 
 static int cmd_psci_version(int argc, const console_cmd_args *argv) {
-  int ret = psci_version();
-  printf("PSCI VERSION: 0x%x\n", ret);
-  return 0;
+    uint32_t ret = psci_version();
+    printf("PSCI VERSION: 0x%x\n", ret);
+    return 0;
 }
 
 STATIC_COMMAND_START

lib/fdtwalk/helpers.cpp

@@ -221,7 +221,7 @@ status_t fdtwalk_setup_cpus_arm(const void *fdt) {
             for (size_t i = 1; i < cpu_count; i++) {
                 /* note: assumes cpuids are numbered like MPIDR 0:0:0:N */
                 dprintf(INFO, "ARM: starting cpu %#x\n", cpus[i].id);
-                int ret = psci_cpu_on(cpus[i].id, MEMBASE + KERNEL_LOAD_OFFSET);
+                int ret = psci_cpu_on(cpus[i].id, MEMBASE + KERNEL_LOAD_OFFSET, i);
                 if (ret != 0) {
                     printf("ERROR: psci CPU_ON returns %d\n", ret);
                 }