!555 Add cpu_ops and secondary cpu boot for qemu-virt64-aarch64

Merge pull request !555 from PolarLush/aarch64-smp-cpu

bsp/qemu-virt64-aarch64/drivers/board.c

@@ -20,6 +20,12 @@
 #endif
 #include "board.h"
 
+#ifdef RT_USING_FDT
+#include "interrupt.h"
+#include "dtb_node.h"
+#include <cpu.h>
+#endif
+
 #ifdef RT_USING_USERSPACE
 struct mem_desc platform_mem_desc[] = {
     {KERNEL_VADDR_START, KERNEL_VADDR_START + 0x0fffffff, KERNEL_VADDR_START + PV_OFFSET, NORMAL_MEM}
@@ -74,9 +80,19 @@ void rt_hw_board_init(void)
     /* initialize system heap */
     rt_system_heap_init((void *)HEAP_BEGIN, (void *)HEAP_END);
 
-    rt_components_board_init();
+    /* support debug feature before components init */
+    rt_hw_uart_init();
     rt_console_set_device(RT_CONSOLE_DEVICE_NAME);
 
+#if defined(RT_USING_FDT) && defined(RT_USING_SMP)
+    // TODO 0x44000000 should be replace by a variable
+    void * fdt_start = (void *)0x44000000 - PV_OFFSET;
+    device_tree_setup(fdt_start);
+    rt_hw_cpu_init();
+#endif
+
+    rt_components_board_init();
+
     rt_thread_idle_sethook(idle_wfi);
 
 #ifdef RT_USING_SMP

bsp/qemu-virt64-aarch64/drivers/board.h

@@ -21,7 +21,7 @@ extern unsigned char __bss_end;
 
 #ifdef RT_USING_USERSPACE
 #define HEAP_END    (rt_size_t)(KERNEL_VADDR_START + 64 * 1024 * 1024)
-#define PAGE_START  HEAP_END
+#define PAGE_START  HEAP_END + 1 * 1024 * 1024
 #define PAGE_END    ((rt_size_t)KERNEL_VADDR_START + 128 * 1024 * 1024)
 #else
 #define HEAP_END    ((void *)HEAP_BEGIN + 64 * 1024 * 1024)
@@ -29,4 +29,6 @@ extern unsigned char __bss_end;
 
 void rt_hw_board_init(void);
 
+int rt_hw_uart_init(void);
+
 #endif

bsp/qemu-virt64-aarch64/drivers/drv_uart.c

@@ -141,4 +141,3 @@ int rt_hw_uart_init(void)
 
     return 0;
 }
-INIT_BOARD_EXPORT(rt_hw_uart_init);

bsp/qemu-virt64-aarch64/drivers/secondary_cpu.c

@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#include <rthw.h>
+#include <rtthread.h>
+#include <cpu.h>
+#include "gic.h"
+#include "interrupt.h"
+#include "mmu.h"
+
+#ifdef RT_USING_SMP
+
+extern unsigned long MMUTable[];
+
+void rt_hw_secondary_cpu_bsp_start(void)
+{
+    rt_hw_spin_lock(&_cpus_lock);
+
+    kernel_mmu_switch((unsigned long)MMUTable);
+
+    // interrupt init
+    rt_hw_vector_init();
+
+    arm_gic_cpu_init(0, 0);
+
+    // local timer init
+
+    rt_system_scheduler_start();
+}
+
+#endif // SMP

bsp/qemu-virt64-aarch64/qemu.sh

@@ -1,7 +1,7 @@
 if [ ! -f "sd.bin" ]; then
 dd if=/dev/zero of=sd.bin bs=1024 count=65536
 fi
-qemu-system-aarch64 -M virt,gic-version=2 -cpu cortex-a53 -smp 4 -kernel rtthread.bin -nographic \
+qemu-system-aarch64 -M virt,gic-version=2,virtualization=on,secure=on -cpu cortex-a53 -smp 4 -kernel rtthread.bin -nographic
 -drive if=none,file=sd.bin,format=raw,id=blk0 -device virtio-blk-device,drive=blk0,bus=virtio-mmio-bus.0 \
 -netdev user,id=net0 -device virtio-net-device,netdev=net0,bus=virtio-mmio-bus.1 \
 -device virtio-serial-device -chardev socket,host=127.0.0.1,port=4321,server=on,wait=off,telnet=on,id=console0 -device virtserialport,chardev=console0

bsp/qemu-virt64-aarch64/rtconfig.h

@@ -8,6 +8,8 @@
 
 #define RT_NAME_MAX 16
 #define RT_USING_SMART
+#define RT_USING_SMP
+#define RT_CPUS_NR 4
 #define RT_ALIGN_SIZE 4
 #define RT_THREAD_PRIORITY_32
 #define RT_THREAD_PRIORITY_MAX 32
@@ -17,6 +19,7 @@
 #define RT_USING_IDLE_HOOK
 #define RT_IDLE_HOOK_LIST_SIZE 4
 #define IDLE_THREAD_STACK_SIZE 8192
+#define SYSTEM_THREAD_STACK_SIZE 8192
 #define RT_USING_TIMER_SOFT
 #define RT_TIMER_THREAD_PRIO 4
 #define RT_TIMER_THREAD_STACK_SIZE 8192
@@ -119,6 +122,8 @@
 #define RT_SERIAL_RB_BUFSZ 256
 #define RT_USING_TTY
 #define RT_USING_PIN
+#define RT_USING_FDT
+#define RT_USING_FDTLIB
 #define RT_USING_RTC
 #define RT_USING_VIRTIO
 #define RT_USING_VIRTIO10

components/drivers/rtc/rtc.c

@@ -17,6 +17,7 @@
 
 #include <time.h>
 #include <string.h>
+#include <stdlib.h>
 
 /* Using NTP auto sync RTC time */
 #ifdef RTC_SYNC_USING_NTP

libcpu/aarch64/common/cpu.c

@@ -14,13 +14,19 @@
 #include <board.h>
 #include "cp15.h"
 
+#define DBG_TAG "libcpu.aarch64.cpu"
+#define DBG_LVL DBG_INFO
+#include <rtdbg.h>
+#include <string.h>
+#include "cpu.h"
+
 #ifdef RT_USING_SMP
 void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock)
 {
     lock->slock = 0;
 }
 
-#define TICKET_SHIFT    16
+#define TICKET_SHIFT 16
 void rt_hw_spin_lock(rt_hw_spinlock_t *lock)
 {
     unsigned int tmp;
@@ -62,6 +68,256 @@ void rt_hw_spin_unlock(rt_hw_spinlock_t *lock)
         : "r"(lock->tickets.owner + 1)
         : "memory");
 }
+
+/**
+ * cpu_ops_tbl contains cpu_ops_t for each cpu kernel observed,
+ * given cpu logical id 'i', its cpu_ops_t is 'cpu_ops_tbl[i]'
+ */
+struct cpu_ops_t *cpu_ops_tbl[RT_CPUS_NR];
+
+// _id_to_mpidr is a table translate logical id to mpid, which is a 64-bit value
+rt_uint64_t rt_cpu_mpidr_early[RT_CPUS_NR] RT_WEAK = {[0 ... RT_CPUS_NR - 1] = ID_ERROR};
+
+#ifdef RT_USING_FDT
+#include "dtb_node.h"
+struct dtb_node *_cpu_node[RT_CPUS_NR];
+#endif /* RT_USING_FDT */
+
+#define MPIDR_AFF_MASK 0x000000FF00FFFFFFul
+#define REPORT_ERR(retval) LOG_E("got error code %d in %s(), %s:%d", (retval), __func__, __FILE__, __LINE__)
+#define CHECK_RETVAL(retval) if (retval) {REPORT_ERR(retval);}
+
+static int _cpus_init_data_hardcoded(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[])
+{
+    // load in cpu_hw_ids in cpuid_to_hwid,
+    // cpu_ops to cpu_ops_tbl
+    if (num_cpus > RT_CPUS_NR)
+    {
+        LOG_W("num_cpus (%d) greater than RT_CPUS_NR (%d)\n", num_cpus, RT_CPUS_NR);
+        num_cpus = RT_CPUS_NR;
+    }
+
+    for (int i = 0; i < num_cpus; i++)
+    {
+        set_hwid(i, cpu_hw_ids[i]);
+        cpu_ops_tbl[i] = cpu_ops[i];
+    }
+    return 0;
+}
+
+#ifdef RT_USING_FDT
+
+/** read ('size' * 4) bytes number from start, big-endian format */
+static rt_uint64_t _read_be_number(void *start, int size)
+{
+    rt_uint64_t buf = 0;
+    for (; size > 0; size--)
+        buf = (buf << 32) | fdt32_to_cpu(*(uint32_t *)start++);
+    return buf;
+}
+
+/** check device-type of the node, */
+static bool _node_is_cpu(struct dtb_node *node)
+{
+    char *device_type = dtb_node_get_dtb_node_property_value(node, "device_type", NULL);
+    if (device_type)
+    {
+        return !strcmp(device_type, "cpu");
+    }
+    return false;
+}
+
+static int _read_and_set_hwid(struct dtb_node *cpu, int *id_pool, int *pcpuid)
+{
+    // size/address_cells is number of elements in reg array
+    int size;
+    static int address_cells, size_cells;
+    if (!address_cells && !size_cells)
+        dtb_node_get_dtb_node_cells(cpu, &address_cells, &size_cells);
+
+    void *id_start = dtb_node_get_dtb_node_property_value(cpu, "reg", &size);
+    rt_uint64_t mpid = _read_be_number(id_start, address_cells);
+
+    *pcpuid = *id_pool;
+    *id_pool = *id_pool + 1;
+    set_hwid(*pcpuid, mpid);
+
+    LOG_I("Using MPID 0x%lx as cpu %d", mpid, *pcpuid);
+
+    // setting _cpu_node for cpu_init use
+    _cpu_node[*pcpuid] = cpu;
+
+    return 0;
+}
+
+static int _read_and_set_cpuops(struct dtb_node *cpu, int cpuid)
+{
+    char *method = dtb_node_get_dtb_node_property_value(cpu, "enable-method", NULL);
+    if (!method)
+    {
+        LOG_E("Cannot read method from cpu node");
+        return -1;
+    }
+
+    struct cpu_ops_t *cpu_ops;
+    if (!strcmp(method, cpu_ops_psci.method))
+    {
+        cpu_ops = &cpu_ops_psci;
+    }
+    else if (!strcmp(method, cpu_ops_spin_tbl.method))
+    {
+        cpu_ops = &cpu_ops_spin_tbl;
+    }
+    else
+    {
+        cpu_ops = RT_NULL;
+        LOG_E("Not supported cpu_ops: %s", method);
+    }
+    cpu_ops_tbl[cpuid] = cpu_ops;
+
+    LOG_D("Using boot method [%s] for cpu %d", cpu_ops->method, cpuid);
+    return 0;
+}
+
+static int _cpus_init_data_fdt()
+{
+    // cpuid_to_hwid and cpu_ops_tbl with fdt
+    void *root = get_dtb_node_head();
+    int id_pool = 0;
+    int cpuid;
+    struct dtb_node *cpus = dtb_node_get_dtb_node_by_path(root, "/cpus");
+
+    // for each cpu node (device-type is cpu), read its mpid and set its cpuid_to_hwid
+    for_each_node_child(cpus)
+    {
+        if (!_node_is_cpu(cpus))
+        {
+            continue;
+        }
+
+        if (id_pool > RT_CPUS_NR)
+        {
+            LOG_W("Reading more cpus from FDT than RT_CPUS_NR"
+                "\n  Parsing will not continue and only %d cpus will be used.", RT_CPUS_NR);
+            break;
+        }
+
+        _read_and_set_hwid(cpus, &id_pool, &cpuid);
+
+        _read_and_set_cpuops(cpus, cpuid);
+    }
+    return 0;
+}
+
+#endif /* RT_USING_FDT */
+
+/** init cpu with hardcoded infomation or parsing from FDT */
+static int _cpus_init(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[])
+{
+    int retval;
+
+    // first setup cpu_ops_tbl and cpuid_to_hwid
+    if (num_cpus > 0)
+        retval = _cpus_init_data_hardcoded(num_cpus, cpu_hw_ids, cpu_ops);
+    else
+    {
+        retval = -1;
+#ifdef RT_USING_FDT
+        retval = _cpus_init_data_fdt();
+#endif
+    }
+
+    if (retval)
+        return retval;
+
+    // using cpuid_to_hwid and cpu_ops_tbl to call method_init and cpu_init
+    // assuming that cpuid 0 has already init
+    for (int i = 1; i < RT_CPUS_NR; i++)
+    {
+        if (cpuid_to_hwid(i) == ID_ERROR)
+        {
+            LOG_E("Failed to find hardware id of CPU %d", i);
+            continue;
+        }
+
+        if (cpu_ops_tbl[i] && cpu_ops_tbl[i]->cpu_init)
+        {
+            retval = cpu_ops_tbl[i]->cpu_init(i);
+            CHECK_RETVAL(retval);
+        }
+        else
+        {
+            LOG_E("Failed to find cpu_init for cpu %d with cpu_ops[%p], cpu_ops->cpu_init[%p]"
+                , cpuid_to_hwid(i), cpu_ops_tbl[i], cpu_ops_tbl[i] ? cpu_ops_tbl[i]->cpu_init : NULL);
+        }
+    }
+    return 0;
+}
+
+static void _boot_secondary(void)
+{
+    for (int i = 1; i < RT_CPUS_NR; i++)
+    {
+        int retval = -0xbad0; // mark no support operation
+        if (cpu_ops_tbl[i] && cpu_ops_tbl[i]->cpu_boot)
+            retval = cpu_ops_tbl[i]->cpu_boot(i);
+        if (retval)
+        {
+            LOG_E("Failed to boot secondary CPU %d, error code %d", i, retval);
+        } else {
+            LOG_I("Secondary CPU %d booted", i);
+        }
+    }
+}
+
+RT_WEAK void rt_hw_secondary_cpu_up(void)
+{
+    _boot_secondary();
+}
+
+/**
+ * @brief boot cpu with hardcoded data
+ *
+ * @param num_cpus number of cpus
+ * @param cpu_hw_ids each element represents a hwid of cpu[i]
+ * @param cpu_ops each element represents a pointer to cpu_ops of cpu[i]
+ * @return int 0 on success,
+ */
+int rt_hw_cpu_boot_secondary(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[])
+{
+    int retval = 0;
+    if (num_cpus < 1 || !cpu_hw_ids || !cpu_ops)
+        return -1;
+
+    retval = _cpus_init(num_cpus, cpu_hw_ids, cpu_ops);
+    CHECK_RETVAL(retval);
+
+    return retval;
+}
+
+#define CPU_INIT_USING_FDT 0,0,0
+
+/**
+ * @brief Initialize cpu infomation from fdt
+ *
+ * @return int
+ */
+int rt_hw_cpu_init()
+{
+#ifdef RT_USING_FDT
+    return _cpus_init(CPU_INIT_USING_FDT);
+#else
+    LOG_E("CPU init failed since RT_USING_FDT was not defined");
+    return -0xa; /* no fdt support */
+#endif /* RT_USING_FDT */
+}
+
+RT_WEAK void rt_hw_secondary_cpu_idle_exec(void)
+{
+    asm volatile("wfe" ::
+                     : "memory", "cc");
+}
+
 #endif /*RT_USING_SMP*/
 
 /**

libcpu/aarch64/common/cpu.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2006-2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#ifndef __RT_HW_CPU_H__
+#define __RT_HW_CPU_H__
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <stdbool.h>
+
+struct cpu_ops_t
+{
+    const char *method;
+    int     (*cpu_init)(rt_uint32_t id);
+    int     (*cpu_boot)(rt_uint32_t id);
+    void    (*cpu_shutdown)(void);
+};
+
+/**
+ * Identifier to mark a wrong CPU MPID.
+ * All elements in rt_cpu_mpidr_early[] should be initialized with this value
+ */
+#define ID_ERROR __INT64_MAX__
+
+extern rt_uint64_t rt_cpu_mpidr_early[];
+extern struct dtb_node *_cpu_node[];
+
+#define cpuid_to_hwid(cpuid) \
+    ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? rt_cpu_mpidr_early[cpuid] : ID_ERROR)
+#define set_hwid(cpuid, hwid) \
+    ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? (rt_cpu_mpidr_early[cpuid] = (hwid)) : ID_ERROR)
+#define get_cpu_node(cpuid) \
+    ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? _cpu_node[cpuid] : NULL)
+#define set_cpu_node(cpuid, node) \
+    ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? (_cpu_node[cpuid] = node) : NULL)
+
+extern void rt_hw_cpu_shutdown(void);
+
+extern int rt_hw_cpu_init();
+
+extern int rt_hw_cpu_boot_secondary(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[]);
+
+extern void rt_hw_secondary_cpu_idle_exec(void);
+
+extern struct cpu_ops_t cpu_ops_psci;
+
+extern struct cpu_ops_t cpu_ops_spin_tbl;
+
+#endif /* __RT_HW_CPU_H__ */

libcpu/aarch64/common/cpu_ops_common.h

@@ -0,0 +1,21 @@
+#ifndef __CPU_OPS_COMMON_H__
+#define __CPU_OPS_COMMON_H__
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <mmu.h>
+#include "entry_point.h"
+
+static inline rt_uint64_t get_secondary_entry_pa(void)
+{
+    rt_uint64_t secondary_entry_pa = (rt_uint64_t)rt_hw_mmu_v2p(&mmu_info, _secondary_cpu_entry);
+
+    if (!secondary_entry_pa)
+    {
+        LOG_E("Failed to translate 'secondary_entry_pa' to physical address");
+        return 0;
+    }
+    return secondary_entry_pa;
+}
+
+#endif /* __CPU_OPS_COMMON_H__ */

libcpu/aarch64/common/cpu_psci.c

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2006-2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#include <rthw.h>
+#include <rtthread.h>
+#include <stdint.h>
+
+#define DBG_TAG "libcpu.aarch64.cpu_psci"
+#define DBG_LVL DBG_INFO
+#include <rtdbg.h>
+#include "cpu_ops_common.h"
+
+#include "cpu.h"
+#include "errno.h"
+#include "psci.h"
+#include "psci_api.h"
+
+static int (*_psci_init)(void) = psci_init;
+
+static int __call_method_init()
+{
+    int (*init)(void) = _psci_init;
+    _psci_init = RT_NULL;
+
+    return init();
+}
+
+/** return 0 on success, otherwise failed */
+#define _call_method_init() ((_psci_init) ? __call_method_init() : 0);
+
+static int cpu_psci_cpu_init(rt_uint32_t cpuid)
+{
+    // init psci only once
+    return _call_method_init();
+}
+
+static int cpu_psci_cpu_boot(rt_uint32_t cpuid)
+{
+    rt_uint64_t secondary_entry_pa = get_secondary_entry_pa();
+
+    if (!secondary_entry_pa)
+        return -1;
+
+    if (!psci_ops.cpu_on) {
+        LOG_E("Uninitialized psci operation");
+        return -1;
+    }
+    return psci_ops.cpu_on(cpuid_to_hwid(cpuid), secondary_entry_pa);
+}
+
+struct cpu_ops_t cpu_ops_psci = {
+    .method = "psci",
+    .cpu_boot = cpu_psci_cpu_boot,
+    .cpu_init = cpu_psci_cpu_init,
+    .cpu_shutdown = RT_NULL
+};

libcpu/aarch64/common/cpu_spin_table.c

@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#include <rthw.h>
+#include <rtthread.h>
+#include <ioremap.h>
+#include "cpu.h"
+
+#define DBG_TAG "libcpu.aarch64.cpu_spin_table"
+#define DBG_LVL DBG_INFO
+#include <rtdbg.h>
+#include "cpu_ops_common.h"
+
+#ifdef RT_USING_FDT
+#include <dtb_node.h>
+
+static rt_uint64_t cpu_release_addr[RT_CPUS_NR];
+
+static int spin_table_cpu_init(rt_uint32_t cpuid)
+{
+    struct dtb_node *cpu = get_cpu_node(cpuid);
+    if (!cpu)
+        return -1; /* uninitialized cpu node in fdt */
+
+    int size;
+    rt_uint64_t *phead = (rt_uint64_t*)dtb_node_get_dtb_node_property_value(cpu, "cpu-release-addr", &size);
+    cpu_release_addr[cpuid] = fdt64_to_cpu(*phead);
+
+    LOG_D("Using release address 0x%p for CPU %d", cpu_release_addr[cpuid], cpuid);
+    return 0;
+}
+
+static int spin_table_cpu_boot(rt_uint32_t cpuid)
+{
+    rt_uint64_t secondary_entry_pa = get_secondary_entry_pa();
+    if (!secondary_entry_pa)
+        return -1;
+
+    // map release_addr to addressable place
+    void *rel_va = rt_ioremap((void *)cpu_release_addr[cpuid], sizeof(cpu_release_addr[0]));
+
+    if (!rel_va)
+    {
+        LOG_E("IO remap failing");
+        return -1;
+    }
+
+    __asm__ volatile("str %0, [%1]" ::"rZ"(secondary_entry_pa), "r"(rel_va));
+    __asm__ volatile("dsb sy");
+    __asm__ volatile("sev");
+    rt_iounmap(rel_va);
+    return 0;
+}
+#endif /* RT_USING_FDT */
+
+struct cpu_ops_t cpu_ops_spin_tbl = {
+    .method = "spin-table",
+#ifdef RT_USING_FDT
+    .cpu_init = spin_table_cpu_init,
+    .cpu_boot = spin_table_cpu_boot,
+#endif
+};

libcpu/aarch64/common/entry_point.h

@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#ifndef __ENTRY_POINT_H__
+#define __ENTRY_POINT_H__
+
+extern void _secondary_cpu_entry(void);
+#endif /* __ENTRY_POINT_H__ */

libcpu/aarch64/common/mmu.h

@@ -135,4 +135,8 @@ void *rt_hw_mmu_v2p(rt_mmu_info *mmu_info, void* v_addr);
 void rt_mm_lock(void);
 void rt_mm_unlock(void);
 
+void kernel_mmu_switch(unsigned long tbl);
+
+extern rt_mmu_info mmu_info;
+
 #endif

libcpu/aarch64/common/psci.c

@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#include <rthw.h>
+#include <rtthread.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include "psci.h"
+#include "psci_api.h"
+#include "smccc.h"
+
+#define DBG_TAG "libcpu.aarch64.psci"
+#define DBG_LVL DBG_INFO
+#include <rtdbg.h>
+
+/** template for creating 4 PSCI ops: SUSPEND, OFF, ON, MIGRATE */
+#define COMMON_PSCI_OPS_TEMPLATE(VER, SUSPEND, OFF, ON, MIGRATE)                   \
+    static int psci_##VER##_cpu_suspend(uint32_t state, unsigned long entry_point) \
+    {                                                                              \
+        return psci_call((SUSPEND), state, entry_point, 0);                        \
+    }                                                                              \
+    static int psci_##VER##_cpu_off(uint32_t state)                                \
+    {                                                                              \
+        return psci_call((OFF), state, 0, 0);                                      \
+    }                                                                              \
+    static int psci_##VER##_cpu_on(unsigned long cpuid, unsigned long entry_point) \
+    {                                                                              \
+        return psci_call((ON), cpuid, entry_point, 0);                             \
+    }                                                                              \
+    static int psci_##VER##_migrate(unsigned long cpuid)                           \
+    {                                                                              \
+        return psci_call((MIGRATE), cpuid, 0, 0);                                  \
+    }
+
+#ifdef RT_USING_FDT
+#include "dtb_node.h"
+
+struct psci_ops_t psci_ops;
+
+#if __SIZE_WIDTH__ == 64
+#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN64_##name
+#else
+#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN_##name
+#endif
+
+/**
+ * SMCCC can use either smc or hvc method
+ * smccc_call will be init to proper interface when psci_init() was executed
+ */
+static void (*smccc_call)(unsigned long a0, unsigned long a1, unsigned long a2,
+                          unsigned long a3, unsigned long a4, unsigned long a5,
+                          unsigned long a6, unsigned long a7, struct arm_smccc_res_t *res,
+                          struct arm_smccc_quirk_t *quirk);
+
+static rt_uint32_t psci_call(unsigned long a0, unsigned long a1, unsigned long a2, unsigned long a3)
+{
+    struct arm_smccc_res_t res;
+    smccc_call(a0, a1, a2, a3, 0, 0, 0, 0, &res, (void *)0);
+    return res.a0;
+}
+
+static int _psci_probe_version(char *version, int *major, int *minor);
+static int _psci_init_with_version(int major, int minor);
+
+static struct dtb_node *psci_node;
+
+static int psci_ver_major;
+static int psci_ver_minor;
+
+/**
+ * @brief init psci operations.
+ * using device tree to probe version and psci-method,
+ * setup psci ops for future use
+ *
+ * @return int 0 on success
+ */
+int psci_init()
+{
+    void *root = get_dtb_node_head();
+    psci_node = dtb_node_get_dtb_node_by_path(root, "/psci");
+    if (!psci_node)
+    {
+        return -1;
+    }
+    char *compatible = dtb_node_get_dtb_node_property_value(psci_node, "compatible", NULL);
+    char *method = dtb_node_get_dtb_node_property_value(psci_node, "method", NULL);
+    
+    int retval = 0;
+
+    // setup psci-method
+    if (!strcmp("hvc", method))
+    {
+        smccc_call = arm_smccc_hvc;
+    }
+    else if (!strcmp("smc", method))
+    {
+        smccc_call = arm_smccc_smc;
+    }
+    else
+    {
+        LOG_E("Unknown PSCI method: %s", method);
+        return -1;
+    }
+
+    retval = _psci_probe_version(compatible, &psci_ver_major, &psci_ver_minor);
+    if (retval != 0)
+        return retval;
+
+    // init psci_ops with specified psci version
+    retval = _psci_init_with_version(psci_ver_major, psci_ver_minor);
+
+    return retval;
+}
+
+/* function id of PSCI v0.1 should be probed in FDT, they are implementation defined value */
+static rt_uint32_t cpu_suspend_0_1;
+static rt_uint32_t cpu_off_0_1;
+static rt_uint32_t cpu_on_0_1;
+static rt_uint32_t migrate_0_1;
+
+/* basic operations TEMPLATE for API since 0.1 version */
+COMMON_PSCI_OPS_TEMPLATE(0_1, cpu_suspend_0_1, cpu_off_0_1, cpu_on_0_1, migrate_0_1);
+
+/* used for v0.1 only, rely on FDT to probe function id */
+#define PROBE_AND_SET(FUNC_NAME)                                                       \
+    do                                                                                 \
+    {                                                                                  \
+        int num_of_elem;                                                               \
+        funcid =                                                                       \
+            dtb_node_get_dtb_node_property_value(psci_node, #FUNC_NAME, &num_of_elem); \
+        if (num_of_elem != 4 || funcid == 0 || *funcid == 0)                           \
+        {                                                                              \
+            LOG_E("Failed to probe " #FUNC_NAME " in FDT");                            \
+        }                                                                              \
+        else                                                                           \
+        {                                                                              \
+            FUNC_NAME##_0_1 = (rt_uint32_t)fdt32_to_cpu(*funcid);                      \
+            psci_ops.FUNC_NAME = psci_0_1_##FUNC_NAME;                                 \
+        }                                                                              \
+    } while (0)
+
+static int psci_0_1_init()
+{
+    // reading function id from fdt
+    rt_uint32_t *funcid;
+    PROBE_AND_SET(cpu_suspend);
+    PROBE_AND_SET(cpu_off);
+    PROBE_AND_SET(cpu_on);
+    PROBE_AND_SET(migrate);
+    return 0;
+}
+
+COMMON_PSCI_OPS_TEMPLATE(0_2, PSCI_FN_NATIVE(0_2, CPU_SUSPEND), PSCI_0_2_FN_CPU_OFF, PSCI_FN_NATIVE(0_2, CPU_ON), PSCI_FN_NATIVE(0_2, MIGRATE));
+
+static rt_uint32_t psci_0_2_get_version(void)
+{
+    return psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
+}
+
+static void psci_0_2_set_basic_ops()
+{
+    psci_ops = (struct psci_ops_t){
+        .get_version = psci_0_2_get_version, 
+
+        // followings API are v0.1 compatible
+        .cpu_suspend = psci_0_2_cpu_suspend,
+        .cpu_off = psci_0_2_cpu_off,
+        .cpu_on = psci_0_2_cpu_on,
+        .migrate = psci_0_2_migrate,
+    };
+}
+
+static int psci_0_2_init()
+{
+    psci_0_2_set_basic_ops();
+
+    // TODO init other version 0.2 features...
+    return 0;
+}
+
+static int psci_1_0_init()
+{
+    psci_0_2_init();
+
+    // TODO init other version 1.0 features...
+    return 0;
+}
+
+/* probe psci version from fdt or SMC call */
+static int _psci_probe_version(char *version, int *major, int *minor)
+{
+    int retval = 0;
+    // if strcmp compatible 'arm,psci-0.1'
+    if (!strcmp(version, "arm,psci"))
+    {
+        *major = 0;
+        *minor = 1;
+    }
+    else if (!strncmp(version, "arm,psci-", 8))
+    {
+        // since psci-0.2, using psci call to probe version
+        rt_uint32_t ret = psci_0_2_get_version();
+        *major = PSCI_VERSION_MAJOR(ret);
+        *minor = PSCI_VERSION_MINOR(ret);
+    }
+    else
+    {
+        LOG_E("[%s] was not a proper PSCI version", version);
+        retval = -1;
+    }
+    LOG_D("Using PSCI v%d.%d", *major, *minor);
+    return retval;
+}
+
+/* init psci ops with version info */
+static int _psci_init_with_version(int major, int minor)
+{
+    int retval = -0xbeef; // mark unsupported
+    if (major == 0)
+    {
+        // for v0.1, psci function id was provided fdt
+        if (minor == 1)
+        {
+            retval = psci_0_1_init();
+        }
+        else if (minor == 2)
+        {
+            retval = psci_0_2_init();
+        }
+    }
+    else if (major == 1)
+    {
+        // psci_1_0_init is a base setup for version after v1.0
+        retval = psci_1_0_init();
+    }
+
+    if (retval == -0xbeef)
+    {
+        LOG_E("PSCI init with incompatible version %d.%d", major, minor);
+    }
+    return retval;
+}
+
+#endif /* RT_USING_FDT */

libcpu/aarch64/common/psci.h

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#ifndef __PSCI_H__
+#define __PSCI_H__
+
+/**
+ * PSCI protocol content
+ * For PSCI v0.1, only return values below are protocol defined
+ */
+
+/* PSCI v0.2 interface */
+#define PSCI_0_2_FN_BASE                    0x84000000
+#define PSCI_0_2_FN(n)                      (PSCI_0_2_FN_BASE + (n))
+#define PSCI_0_2_64BIT                      0x40000000
+#define PSCI_0_2_FN64_BASE                  (PSCI_0_2_FN_BASE + PSCI_0_2_64BIT)
+#define PSCI_0_2_FN64(n)                    (PSCI_0_2_FN64_BASE + (n))
+
+#define PSCI_0_2_FN_PSCI_VERSION            PSCI_0_2_FN(0)
+#define PSCI_0_2_FN_CPU_SUSPEND             PSCI_0_2_FN(1)
+#define PSCI_0_2_FN_CPU_OFF                 PSCI_0_2_FN(2)
+#define PSCI_0_2_FN_CPU_ON                  PSCI_0_2_FN(3)
+#define PSCI_0_2_FN_AFFINITY_INFO           PSCI_0_2_FN(4)
+#define PSCI_0_2_FN_MIGRATE                 PSCI_0_2_FN(5)
+#define PSCI_0_2_FN_MIGRATE_INFO_TYPE       PSCI_0_2_FN(6)
+#define PSCI_0_2_FN_MIGRATE_INFO_UP_CPU     PSCI_0_2_FN(7)
+#define PSCI_0_2_FN_SYSTEM_OFF              PSCI_0_2_FN(8)
+#define PSCI_0_2_FN_SYSTEM_RESET            PSCI_0_2_FN(9)
+
+#define PSCI_0_2_FN64_CPU_SUSPEND           PSCI_0_2_FN64(1)
+#define PSCI_0_2_FN64_CPU_ON                PSCI_0_2_FN64(3)
+#define PSCI_0_2_FN64_AFFINITY_INFO         PSCI_0_2_FN64(4)
+#define PSCI_0_2_FN64_MIGRATE               (5)
+#define PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU   PSCI_0_2_FN64(7)
+
+#define PSCI_1_0_FN_PSCI_FEATURES           PSCI_0_2_FN(10)
+#define PSCI_1_0_FN_SYSTEM_SUSPEND          PSCI_0_2_FN(14)
+#define PSCI_1_0_FN_SET_SUSPEND_MODE        PSCI_0_2_FN(15)
+#define PSCI_1_1_FN_SYSTEM_RESET2           PSCI_0_2_FN(18)
+
+#define PSCI_1_0_FN64_SYSTEM_SUSPEND        PSCI_0_2_FN64(14)
+#define PSCI_1_1_FN64_SYSTEM_RESET2         PSCI_0_2_FN64(18)
+
+/* PSCI v0.2 power state encoding for CPU_SUSPEND function */
+#define PSCI_0_2_POWER_STATE_ID_MASK        0xffff
+#define PSCI_0_2_POWER_STATE_ID_SHIFT       0
+#define PSCI_0_2_POWER_STATE_TYPE_SHIFT     16
+#define PSCI_0_2_POWER_STATE_TYPE_MASK      (0x1 << PSCI_0_2_POWER_STATE_TYPE_SHIFT)
+#define PSCI_0_2_POWER_STATE_AFFL_SHIFT     24
+#define PSCI_0_2_POWER_STATE_AFFL_MASK      (0x3 << PSCI_0_2_POWER_STATE_AFFL_SHIFT)
+
+/* PSCI extended power state encoding for CPU_SUSPEND function */
+#define PSCI_1_0_EXT_POWER_STATE_ID_MASK    0xfffffff
+#define PSCI_1_0_EXT_POWER_STATE_ID_SHIFT   0
+#define PSCI_1_0_EXT_POWER_STATE_TYPE_SHIFT 30
+#define PSCI_1_0_EXT_POWER_STATE_TYPE_MASK  (0x1 << PSCI_1_0_EXT_POWER_STATE_TYPE_SHIFT)
+
+/* PSCI v0.2 affinity level state returned by AFFINITY_INFO */
+#define PSCI_0_2_AFFINITY_LEVEL_ON          0
+#define PSCI_0_2_AFFINITY_LEVEL_OFF         1
+#define PSCI_0_2_AFFINITY_LEVEL_ON_PENDING  2
+
+/* PSCI v0.2 multicore support in Trusted OS returned by MIGRATE_INFO_TYPE */
+#define PSCI_0_2_TOS_UP_MIGRATE             0
+#define PSCI_0_2_TOS_UP_NO_MIGRATE          1
+#define PSCI_0_2_TOS_MP                     2
+
+/* PSCI version decoding (independent of PSCI version) */
+#define PSCI_VERSION_MAJOR_SHIFT            16
+#define PSCI_VERSION_MINOR_MASK             ((1U << PSCI_VERSION_MAJOR_SHIFT) - 1)
+#define PSCI_VERSION_MAJOR_MASK             ~PSCI_VERSION_MINOR_MASK
+#define PSCI_VERSION_MAJOR(ver)             (((ver) & PSCI_VERSION_MAJOR_MASK) >> PSCI_VERSION_MAJOR_SHIFT)
+#define PSCI_VERSION_MINOR(ver)             ((ver) & PSCI_VERSION_MINOR_MASK)
+#define PSCI_VERSION(maj, min)              \
+            ((((maj) << PSCI_VERSION_MAJOR_SHIFT) & PSCI_VERSION_MAJOR_MASK) | \
+            ((min) & PSCI_VERSION_MINOR_MASK))
+
+/* PSCI features decoding (>=1.0) */
+#define PSCI_1_0_FEATURES_CPU_SUSPEND_PF_SHIFT      1
+#define PSCI_1_0_FEATURES_CPU_SUSPEND_PF_MASK       (0x1 << PSCI_1_0_FEATURES_CPU_SUSPEND_PF_SHIFT)
+
+#define PSCI_1_0_OS_INITIATED           BIT(0)
+#define PSCI_1_0_SUSPEND_MODE_PC        0
+#define PSCI_1_0_SUSPEND_MODE_OSI       1
+
+/* PSCI return values (inclusive of all PSCI versions) */
+#define PSCI_RET_SUCCESS                0
+#define PSCI_RET_NOT_SUPPORTED          -1
+#define PSCI_RET_INVALID_PARAMS         -2
+#define PSCI_RET_DENIED                 -3
+#define PSCI_RET_ALREADY_ON             -4
+#define PSCI_RET_ON_PENDING             -5
+#define PSCI_RET_INTERNAL_FAILURE       -6
+#define PSCI_RET_NOT_PRESENT            -7
+#define PSCI_RET_DISABLED               -8
+#define PSCI_RET_INVALID_ADDRESS        -9
+
+#endif  /*__PSCI_H__*/

libcpu/aarch64/common/psci_api.h

@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#ifndef __PSCI_API_H__
+#define __PSCI_API_H__
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <stdint.h>
+#include "psci_api.h"
+
+/** generic psci ops supported v0.1 v0.2 v1.0 v1.1 */
+struct psci_ops_t
+{
+    uint32_t (*get_version)(void);
+    int32_t (*cpu_suspend)(uint32_t state, unsigned long entry_point);
+    int32_t (*cpu_off)(uint32_t state);
+    int32_t (*cpu_on)(unsigned long cpuid, unsigned long entry_point);
+    int32_t (*migrate)(unsigned long cpuid);
+};
+
+extern struct psci_ops_t psci_ops;
+
+extern int psci_init(void);
+
+#endif // __PSCI_API_H__

libcpu/aarch64/common/smccc.S

@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+/**
+ * SMCCC v0.2
+ * ARM DEN0028E chapter 2.6
+ */
+    .macro SMCCC instr
+    stp    x29, x30, [sp, #-16]!
+    mov    x29, sp
+    \instr    #0
+    // store in arm_smccc_res
+    ldr    x4, [sp, #16]
+    stp    x0, x1, [x4, #0]
+    stp    x2, x3, [x4, #16]
+1:  
+    ldp    x29, x30, [sp], #16
+    ret
+    .endm
+
+.global arm_smccc_smc
+arm_smccc_smc:
+    SMCCC    smc
+
+.global arm_smccc_hvc
+arm_smccc_hvc:
+    SMCCC    hvc

libcpu/aarch64/common/smccc.h

@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2006-2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#ifndef __SMCCC_H__
+#define __SMCCC_H__
+
+/**
+ * result from SMC/HVC call
+ * ARM DEN0028E chapter 5,
+ */
+typedef struct arm_smccc_res_t
+{
+    unsigned long a0;
+    // reserved for ARM SMC and HVC Fast Call services
+    unsigned long a1;
+    unsigned long a2;
+    unsigned long a3;
+} arm_smccc_res_t;
+
+/**
+ * quirk is a structure contains vendor specified information,
+ * it just a placeholder currently
+ */
+struct arm_smccc_quirk_t
+{
+};
+
+/* smccc version 0.2 */
+
+void arm_smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2,
+                   unsigned long a3, unsigned long a4, unsigned long a5,
+                   unsigned long a6, unsigned long a7, struct arm_smccc_res_t *res,
+                   struct arm_smccc_quirk_t *quirk);
+
+void arm_smccc_hvc(unsigned long a0, unsigned long a1, unsigned long a2,
+                   unsigned long a3, unsigned long a4, unsigned long a5,
+                   unsigned long a6, unsigned long a7, struct arm_smccc_res_t *res,
+                   struct arm_smccc_quirk_t *quirk);
+
+#endif /* __SMCCC_H__ */