diff options
Diffstat (limited to 'libcpu/aarch64/common')
| -rw-r--r-- | libcpu/aarch64/common/SConscript | 3 | ||||
| -rw-r--r-- | libcpu/aarch64/common/cpu.c | 211 | ||||
| -rw-r--r-- | libcpu/aarch64/common/cpu.h | 54 | ||||
| -rw-r--r-- | libcpu/aarch64/common/cpu_psci.c | 58 | ||||
| -rw-r--r-- | libcpu/aarch64/common/cpu_spin_table.c | 71 | ||||
| -rw-r--r-- | libcpu/aarch64/common/psci.c | 528 | ||||
| -rw-r--r-- | libcpu/aarch64/common/psci.h | 156 | ||||
| -rw-r--r-- | libcpu/aarch64/common/psci_api.h | 34 | ||||
| -rw-r--r-- | libcpu/aarch64/common/setup.c | 219 | ||||
| -rw-r--r-- | libcpu/aarch64/common/setup.h | 22 |
10 files changed, 740 insertions, 616 deletions
diff --git a/libcpu/aarch64/common/SConscript b/libcpu/aarch64/common/SConscript index 72fb8e26c3..38fd4874c5 100644 --- a/libcpu/aarch64/common/SConscript +++ b/libcpu/aarch64/common/SConscript @@ -8,6 +8,9 @@ cwd = GetCurrentDir() src = Glob('*.c') + Glob('*.cpp') + Glob('*.S') CPPPATH = [cwd] +if GetDepend('RT_USING_OFW') == False: + SrcRemove(src, ['setup.c', 'cpu_psci.c', 'psci.c']) + group = DefineGroup('common', src, depend = [''], CPPPATH = CPPPATH) # build for sub-directory diff --git a/libcpu/aarch64/common/cpu.c b/libcpu/aarch64/common/cpu.c index 5dbb45af71..15b17629a3 100644 --- a/libcpu/aarch64/common/cpu.c +++ b/libcpu/aarch64/common/cpu.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2019, RT-Thread Development Team + * Copyright (c) 2006-2023, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -7,37 +7,33 @@ * Date Author Notes * 2011-09-15 Bernard first version * 2019-07-28 zdzn add smp support + * 2023-02-21 GuEe-GUI mov cpu ofw init to setup */ #include <rthw.h> #include <rtthread.h> -#include <board.h> -#include "cp15.h" +#include <rtdevice.h> +#include <cpu.h> #define DBG_TAG "libcpu.aarch64.cpu" #define DBG_LVL DBG_INFO #include <rtdbg.h> -#include <string.h> -#include "cpu.h" -#include "psci_api.h" -void (*system_off)(void); #ifdef RT_USING_SMP -#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);} +#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) -/** - * 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]' - */ +typedef rt_hw_spinlock_t arch_spinlock_t; struct cpu_ops_t *cpu_ops_tbl[RT_CPUS_NR]; #ifdef RT_USING_SMART @@ -59,8 +55,6 @@ rt_weak rt_uint64_t rt_cpu_mpidr_early[] = }; #endif /* RT_USING_SMART */ -typedef rt_hw_spinlock_t arch_spinlock_t; - static inline void arch_spin_lock(arch_spinlock_t *lock) { unsigned int tmp; @@ -138,115 +132,6 @@ static int _cpus_init_data_hardcoded(int num_cpus, rt_uint64_t *cpu_hw_ids, stru 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); - start = (uint32_t *)start + 1; - } - 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[]) { @@ -258,9 +143,6 @@ static int _cpus_init(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *c else { retval = -1; -#ifdef RT_USING_FDT - retval = _cpus_init_data_fdt(); -#endif } if (retval) @@ -278,7 +160,7 @@ static int _cpus_init(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *c if (cpu_ops_tbl[i] && cpu_ops_tbl[i]->cpu_init) { - retval = cpu_ops_tbl[i]->cpu_init(i); + retval = cpu_ops_tbl[i]->cpu_init(i, RT_NULL); CHECK_RETVAL(retval); } else @@ -290,30 +172,6 @@ static int _cpus_init(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *c 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) - { - if (retval == -0xbad0) - LOG_E("No cpu_ops was probed for CPU %d. Try to configure it or use fdt", i); - else - 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 * @@ -334,29 +192,6 @@ int rt_hw_cpu_boot_secondary(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_o 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*/ /** @@ -369,24 +204,6 @@ const char *rt_hw_cpu_arch(void) return "aarch64"; } -/** shutdown CPU */ -void rt_hw_cpu_shutdown(void) -{ - rt_uint32_t level; - rt_kprintf("shutdown...\n"); - - if (system_off) - system_off(); - LOG_E("system shutdown failed"); - - level = rt_hw_interrupt_disable(); - while (level) - { - RT_ASSERT(0); - } -} -MSH_CMD_EXPORT_ALIAS(rt_hw_cpu_shutdown, shutdown, shutdown machine); - #ifdef RT_USING_CPU_FFS /** * This function finds the first bit set (beginning with the least significant bit) diff --git a/libcpu/aarch64/common/cpu.h b/libcpu/aarch64/common/cpu.h index 873207d91b..02a4357a9f 100644 --- a/libcpu/aarch64/common/cpu.h +++ b/libcpu/aarch64/common/cpu.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2019, RT-Thread Development Team + * Copyright (c) 2006-2023, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -9,49 +9,25 @@ #ifndef __RT_HW_CPU_H__ #define __RT_HW_CPU_H__ -#include <rthw.h> -#include <rtthread.h> -#include <stdbool.h> +#include <rtdef.h> +#include <cpuport.h> +#include <mm_aspace.h> + +#ifdef RT_USING_OFW +#include <drivers/ofw.h> +#endif + +#define ID_ERROR __INT64_MAX__ +#define MPIDR_AFFINITY_MASK 0x000000ff00ffffffUL -#ifdef RT_USING_SMP struct cpu_ops_t { const char *method; - int (*cpu_init)(rt_uint32_t id); - int (*cpu_boot)(rt_uint32_t id); + int (*cpu_init)(rt_uint32_t id, void *param); + int (*cpu_boot)(rt_uint32_t id, rt_uint64_t entry); 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 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_USING_SMP */ - -extern void (*system_off)(void); +extern rt_uint64_t rt_cpu_mpidr_table[]; -#endif /* __RT_HW_CPU_H__ */
\ No newline at end of file +#endif /* __RT_HW_CPU_H__ */ diff --git a/libcpu/aarch64/common/cpu_psci.c b/libcpu/aarch64/common/cpu_psci.c index ddb8ae4a47..6242710ca0 100644 --- a/libcpu/aarch64/common/cpu_psci.c +++ b/libcpu/aarch64/common/cpu_psci.c @@ -5,66 +5,36 @@ * * Change Logs: * Date Author Notes + * 2023-02-21 GuEe-GUI replace with drivers/psci */ #include <rthw.h> #include <rtthread.h> -#include <stdint.h> -#ifdef RT_USING_SMP - -#define DBG_TAG "libcpu.aarch64.cpu_psci" +#define DBG_TAG "cpu.aa64" #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); +#include <cpu.h> +#include <cpuport.h> +#include <psci.h> -static int cpu_psci_cpu_init(rt_uint32_t cpuid) +static int psci_cpu_boot(rt_uint32_t cpuid, rt_uint64_t entry) { - // init psci only once - return _call_method_init(); + return rt_psci_cpu_on(cpuid, entry); } -static int cpu_psci_cpu_boot(rt_uint32_t cpuid) +static void psci_cpu_shutdown(void) { - rt_uint64_t secondary_entry_pa = get_secondary_entry_pa(); + rt_uint32_t state, state_id = PSCI_POWER_STATE_ID(0, 0, 0, PSCI_POWER_STATE_ID_POWERDOWN); - if (!secondary_entry_pa) - return -1; + state = PSCI_POWER_STATE(PSCI_POWER_STATE_LEVEL_CORES, PSCI_POWER_STATE_TYPE_STANDBY, state_id); - if (!psci_ops.cpu_on) { - LOG_E("Uninitialized psci operation"); - return -1; - } - return psci_ops.cpu_on(cpuid_to_hwid(cpuid), secondary_entry_pa); + rt_psci_cpu_off(state); } -static void cpu_psci_cpu_shutdown() +struct cpu_ops_t cpu_psci_ops = { - psci_ops.cpu_off(cpuid_to_hwid(rt_hw_cpu_id())); -} - -struct cpu_ops_t cpu_ops_psci = { .method = "psci", - .cpu_boot = cpu_psci_cpu_boot, - .cpu_init = cpu_psci_cpu_init, - .cpu_shutdown = cpu_psci_cpu_shutdown, + .cpu_boot = psci_cpu_boot, + .cpu_shutdown = psci_cpu_shutdown, }; - -#endif /* RT_USING_SMP */
\ No newline at end of file diff --git a/libcpu/aarch64/common/cpu_spin_table.c b/libcpu/aarch64/common/cpu_spin_table.c index 67e788225f..2cae749a48 100644 --- a/libcpu/aarch64/common/cpu_spin_table.c +++ b/libcpu/aarch64/common/cpu_spin_table.c @@ -5,76 +5,61 @@ * * Change Logs: * Date Author Notes + * 2023-02-21 GuEe-GUI replace with ofw */ #include <rthw.h> #include <rtthread.h> -#include "cpu.h" - -#ifdef RT_USING_SMART -#include <ioremap.h> -#else -#define rt_ioremap(x, ...) (x) -#define rt_iounmap(x) -#endif - -#define DBG_TAG "libcpu.aarch64.cpu_spin_table" +#define DBG_TAG "cpu.aa64" #define DBG_LVL DBG_INFO #include <rtdbg.h> -#include "cpu_ops_common.h" -#ifdef RT_USING_SMP -#ifdef RT_USING_FDT -#include <dtb_node.h> +#include <cpu.h> +#include <cpuport.h> + +#include <ioremap.h> +#include <drivers/core/dm.h> + +#ifdef RT_USING_OFW static rt_uint64_t cpu_release_addr[RT_CPUS_NR]; -static int spin_table_cpu_init(rt_uint32_t cpuid) +static int spin_table_cpu_init(rt_uint32_t cpuid, void *param) { - struct dtb_node *cpu = get_cpu_node(cpuid); - if (!cpu) - return -1; /* uninitialized cpu node in fdt */ + struct rt_ofw_node *cpu_np = param; - 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); + rt_ofw_prop_read_u64(cpu_np, "cpu-release-addr", &cpu_release_addr[cpuid]); - LOG_D("Using release address 0x%p for CPU %d", cpu_release_addr[cpuid], cpuid); - return 0; + 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) +static int spin_table_cpu_boot(rt_uint32_t cpuid, rt_uint64_t entry) { - rt_uint64_t secondary_entry_pa = get_secondary_entry_pa(); - if (!secondary_entry_pa) - return -1; + void *cpu_release_vaddr; - // map release_addr to addressable place - void *rel_va = (void *)cpu_release_addr[cpuid]; + cpu_release_vaddr = rt_ioremap((void *)cpu_release_addr[cpuid], sizeof(cpu_release_addr[0])); -#ifdef RT_USING_SMART - rel_va = rt_ioremap(rel_va, sizeof(cpu_release_addr[0])); -#endif - if (!rel_va) + if (!cpu_release_vaddr) { 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); + __asm__ volatile ("str %0, [%1]" ::"rZ"(entry), "r"(cpu_release_vaddr)); + rt_hw_barrier(dsb, sy); + rt_hw_sev(); + + rt_iounmap(cpu_release_vaddr); + return 0; } -#endif /* RT_USING_FDT */ -struct cpu_ops_t cpu_ops_spin_tbl = { +struct cpu_ops_t cpu_spin_table_ops = +{ .method = "spin-table", -#ifdef RT_USING_FDT .cpu_init = spin_table_cpu_init, .cpu_boot = spin_table_cpu_boot, -#endif }; - -#endif /* RT_USING_SMP */
\ No newline at end of file +#endif diff --git a/libcpu/aarch64/common/psci.c b/libcpu/aarch64/common/psci.c index b3bf1c50af..4b64ad1512 100644 --- a/libcpu/aarch64/common/psci.c +++ b/libcpu/aarch64/common/psci.c @@ -5,296 +5,414 @@ * * Change Logs: * Date Author Notes + * 2021-09-09 GuEe-GUI The first version + * 2022-09-24 GuEe-GUI Add operations and fdt init support */ -#include <rthw.h> + #include <rtthread.h> -#include <stdint.h> -#include <stdio.h> -#include <string.h> -#include "cpu.h" -#include "psci.h" -#include "psci_api.h" -#include "smccc.h" -#define DBG_TAG "libcpu.aarch64.psci" +#define DBG_TAG "osi.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); \ - } +/* support cpu mpidr and smccc from libcpu */ +#include <cpu.h> +#include <smccc.h> +#include <psci.h> +#include <drivers/ofw.h> +#include <drivers/platform.h> +#include <drivers/core/dm.h> -struct psci_ops_t psci_ops; +struct psci_ops +{ + rt_uint32_t (*get_version)(void); + rt_uint32_t (*cpu_on)(int cpuid, rt_ubase_t entry_point); + rt_uint32_t (*cpu_off)(rt_uint32_t state); + rt_uint32_t (*cpu_suspend)(rt_uint32_t power_state, rt_ubase_t entry_point); + rt_uint32_t (*migrate)(int cpuid); + rt_uint32_t (*get_affinity_info)(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level); + rt_uint32_t (*migrate_info_type)(void); +}; + +struct psci_0_1_func_ids +{ + rt_uint32_t cpu_on; + rt_uint32_t cpu_off; + rt_uint32_t cpu_suspend; + rt_uint32_t migrate; +}; -#ifdef RT_USING_FDT -#include "dtb_node.h" -#endif /* RT_USING_FDT */ +typedef rt_err_t (*psci_init_ofw_handle)(struct rt_ofw_node *np); +typedef rt_ubase_t (*psci_call_handle)(rt_uint32_t fn, rt_ubase_t arg0, rt_ubase_t arg1, rt_ubase_t arg2); -#if __SIZE_WIDTH__ == 64 -#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN64_##name +/* [40:63] and [24:31] must be zero, other is aff3 (64bit), aff2, aff1, aff0 */ +#ifdef ARCH_CPU_64BIT +#define PSCI_FNC_ID(version_major, version_min, name) PSCI_##version_major##_##version_min##_FN64_##name +#define MPIDR_MASK 0xff00ffffff #else -#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN_##name +#define PSCI_FNC_ID(version_major, version_min, name) PSCI_##version_major##_##version_min##_FN_##name +#define MPIDR_MASK 0x00ffffff #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 struct psci_ops _psci_ops = {}; + +static struct psci_0_1_func_ids psci_0_1_func_ids = {}; +static psci_call_handle psci_call; -static rt_uint32_t psci_call(unsigned long a0, unsigned long a1, unsigned long a2, unsigned long a3) +/* PSCI SMCCC */ +static rt_ubase_t psci_smc_call(rt_uint32_t fn, rt_ubase_t arg0, rt_ubase_t arg1, rt_ubase_t arg2) { struct arm_smccc_res_t res; - smccc_call(a0, a1, a2, a3, 0, 0, 0, 0, &res, (void *)0); + + arm_smccc_smc(fn, arg0, arg1, arg2, 0, 0, 0, 0, &res, RT_NULL); + return res.a0; } -static int _psci_init_with_version(int major, int minor); +static rt_ubase_t psci_hvc_call(rt_uint32_t fn, rt_ubase_t arg0, rt_ubase_t arg1, rt_ubase_t arg2) +{ + struct arm_smccc_res_t res; -#ifdef RT_USING_FDT -static int _psci_probe_version(char *version, int *major, int *minor); + arm_smccc_hvc(fn, arg0, arg1, arg2, 0, 0, 0, 0, &res, RT_NULL); -static int psci_ver_major; -static int psci_ver_minor; -static struct dtb_node *psci_node; + return res.a0; +} -/** - * @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) +/* PSCI VERSION */ +static rt_uint32_t psci_0_1_get_version(void) { - void *root = get_dtb_node_head(); - psci_node = dtb_node_get_dtb_node_by_path(root, "/psci"); - if (!psci_node) - { - LOG_E("No PSCI node found"); - 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); + return PSCI_VERSION(0, 1); +} - int retval = 0; +static rt_uint32_t psci_0_2_get_version(void) +{ + return (rt_uint32_t)psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); +} - // setup psci-method - if (!strcmp("hvc", method)) - { - smccc_call = arm_smccc_hvc; - } - else if (!strcmp("smc", method)) - { - smccc_call = arm_smccc_smc; - } - else +/* PSCI FEATURES */ +static rt_uint32_t psci_get_features(rt_uint32_t psci_func_id) +{ + return (rt_uint32_t)psci_call(PSCI_1_0_FN_PSCI_FEATURES, psci_func_id, 0, 0); +} + +/* PSCI CPU_ON */ +static rt_uint32_t psci_cpu_on(rt_uint32_t func_id, int cpuid, rt_ubase_t entry_point) +{ + rt_uint32_t ret = -PSCI_RET_INVALID_PARAMETERS; + + if (cpuid < RT_CPUS_NR) { - LOG_E("Unknown PSCI method: %s", method); - return -1; + rt_ubase_t mpid = rt_cpu_mpidr_table[cpuid] & MPIDR_MASK; + + ret = (rt_uint32_t)psci_call(func_id, mpid, entry_point, 0); } - LOG_D("Using psci method %s", method); - retval = _psci_probe_version(compatible, &psci_ver_major, &psci_ver_minor); - if (retval != 0) - return retval; + return ret; +} - // init psci_ops with specified psci version - retval = _psci_init_with_version(psci_ver_major, psci_ver_minor); +static rt_uint32_t psci_0_1_cpu_on(int cpuid, rt_ubase_t entry_point) +{ + return psci_cpu_on(psci_0_1_func_ids.cpu_on, cpuid, entry_point); +} - return retval; +static rt_uint32_t psci_0_2_cpu_on(int cpuid, rt_ubase_t entry_point) +{ + return psci_cpu_on(PSCI_FNC_ID(0, 2, CPU_ON), cpuid, entry_point); } -/* 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; +/* PSCI CPU_OFF */ +static rt_uint32_t psci_cpu_off(rt_uint32_t func_id, rt_uint32_t state) +{ + return (rt_uint32_t)psci_call(func_id, state, 0, 0); +} -/* 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); +static rt_uint32_t psci_0_1_cpu_off(rt_uint32_t state) +{ + return psci_cpu_off(psci_0_1_func_ids.cpu_off, state); +} -/* 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 rt_uint32_t psci_0_2_cpu_off(rt_uint32_t state) +{ + return psci_cpu_off(PSCI_0_2_FN_CPU_OFF, state); +} -static int psci_0_1_init() +/* PSCI CPU_SUSPEND */ +static rt_uint32_t psci_cpu_suspend(rt_uint32_t func_id, rt_uint32_t power_state, rt_ubase_t entry_point) { - // 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; + return (rt_uint32_t)psci_call(func_id, power_state, entry_point, 0); } -#else -int psci_init(void) + +static rt_uint32_t psci_0_1_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point) { - smccc_call = arm_smccc_smc; - _psci_init_with_version(0, 2); - return 0; + return psci_cpu_suspend(psci_0_1_func_ids.cpu_suspend, power_state, entry_point); +} + +static rt_uint32_t psci_0_2_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point) +{ + return psci_cpu_suspend(PSCI_FNC_ID(0, 2, CPU_SUSPEND), power_state, entry_point); } -#endif /* RT_USING_FDT */ -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)); +/* PSCI CPU_MIGRATE */ +static rt_uint32_t psci_migrate(rt_uint32_t func_id, int cpuid) +{ + rt_uint32_t ret = -PSCI_RET_INVALID_PARAMETERS; -static rt_uint32_t psci_0_2_get_version(void) + if (cpuid < RT_CPUS_NR) + { + rt_ubase_t mpid = rt_cpu_mpidr_table[cpuid] & MPIDR_MASK; + + ret = (rt_uint32_t)psci_call(func_id, mpid, 0, 0); + } + + return ret; +} + +static rt_uint32_t psci_0_1_migrate(int cpuid) { - return psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); + return psci_migrate(psci_0_1_func_ids.migrate, cpuid); } -static void psci_0_2_set_basic_ops() +static rt_uint32_t psci_0_2_migrate(int cpuid) { - psci_ops = (struct psci_ops_t){ - .get_version = psci_0_2_get_version, + return psci_migrate(PSCI_FNC_ID(0, 2, MIGRATE), cpuid); +} - // 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, - }; +/* PSCI AFFINITY_INFO */ +static rt_uint32_t psci_affinity_info(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level) +{ + return (rt_uint32_t)psci_call(PSCI_FNC_ID(0, 2, AFFINITY_INFO), target_affinity, lowest_affinity_level, 0); } -static void psci_0_2_system_off(void) +/* PSCI MIGRATE_INFO_TYPE */ +static rt_uint32_t psci_migrate_info_type(void) +{ + return (rt_uint32_t)psci_call(PSCI_0_2_FN_MIGRATE_INFO_TYPE, 0, 0, 0); +} + +/* PSCI SYSTEM_OFF */ +void psci_system_off(void) { psci_call(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0); } -static void psci_0_2_system_reset(void) +/* PSCI SYSTEM_RESET */ +void psci_system_reboot(void) { - psci_call(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0); + if (psci_get_features(PSCI_FNC_ID(1, 1, SYSTEM_RESET2)) != PSCI_RET_NOT_SUPPORTED) + { + /* + * reset_type[31] = 0 (architectural) + * reset_type[30:0] = 0 (SYSTEM_WARM_RESET) + * cookie = 0 (ignored by the implementation) + */ + psci_call(PSCI_FNC_ID(1, 1, SYSTEM_RESET2), 0, 0, 0); + } + else + { + psci_call(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0); + } + } -static int psci_0_2_init() +#define PSCI_CALL_FN_RET(fn, ...) \ +({ \ + rt_uint32_t rc; \ + rc = PSCI_RET_NOT_SUPPORTED; \ + if (_psci_ops.fn) \ + rc = _psci_ops.fn(__VA_ARGS__); \ + rc; \ +}) + +#define PSCI_CALL_FN(fn, ...) \ +({ \ + if (_psci_ops.fn) \ + _psci_ops.fn(__VA_ARGS__); \ +}) + +rt_uint32_t rt_psci_get_version(void) { - psci_0_2_set_basic_ops(); + return PSCI_CALL_FN_RET(get_version); +} - // TODO init other version 0.2 features... - // psci system off and reset which controlling machine - psci_ops.system_off = psci_0_2_system_off; - psci_ops.system_reset = psci_0_2_system_reset; +rt_uint32_t rt_psci_cpu_on(int cpuid, rt_ubase_t entry_point) +{ + return PSCI_CALL_FN_RET(cpu_on, cpuid, entry_point); +} - system_off = psci_0_2_system_off; - return 0; +rt_uint32_t rt_psci_cpu_off(rt_uint32_t state) +{ + return PSCI_CALL_FN_RET(cpu_off, state); } -/* PSCI v1.0 & after */ -static int psci_1_0_features(uint32_t psci_func_id) +rt_uint32_t rt_psci_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point) { - return psci_call(PSCI_1_0_FN_PSCI_FEATURES, - psci_func_id, 0, 0); + return PSCI_CALL_FN_RET(cpu_suspend, power_state, entry_point); } -static int psci_1_0_init() +rt_uint32_t rt_psci_migrate(int cpuid) { - psci_0_2_init(); + return PSCI_CALL_FN_RET(migrate, cpuid); +} - // TODO init other version 1.0 features... - // remove unsupported features - if (psci_1_0_features(PSCI_0_2_FN_SYSTEM_OFF) == PSCI_RET_NOT_SUPPORTED) - { - psci_ops.system_off = RT_NULL; - system_off = RT_NULL; - } - else - LOG_D("Using SYSTEM OFF feature"); - if (psci_1_0_features(PSCI_0_2_FN_SYSTEM_RESET) == PSCI_RET_NOT_SUPPORTED) - psci_ops.system_reset = RT_NULL; - else - LOG_D("Using SYSTEM RESET feature"); +rt_uint32_t rt_psci_get_affinity_info(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level) +{ + return PSCI_CALL_FN_RET(get_affinity_info, target_affinity, lowest_affinity_level); +} - return 0; +rt_uint32_t rt_psci_migrate_info_type(void) +{ + return PSCI_CALL_FN_RET(migrate_info_type); } -#ifdef RT_USING_FDT -/* probe psci version from fdt or SMC call */ -static int _psci_probe_version(char *version, int *major, int *minor) +#undef PSCI_CALL_FN_RET +#undef PSCI_CALL_FN + +/* PSCI INIT */ +static rt_err_t psci_0_1_init(struct rt_ofw_node *np) { - int retval = 0; - // if strcmp compatible 'arm,psci-0.1' - if (!strcmp(version, "arm,psci")) + rt_err_t err = RT_EOK; + rt_uint32_t func_id; + + _psci_ops.get_version = psci_0_1_get_version; + + if (!rt_ofw_prop_read_u32(np, "cpu_on", &func_id)) { - *major = 0; - *minor = 1; + psci_0_1_func_ids.cpu_on = func_id; + _psci_ops.cpu_on = psci_0_1_cpu_on; } - else if (!strncmp(version, "arm,psci-", 8)) + + if (!rt_ofw_prop_read_u32(np, "cpu_off", &func_id)) + { + psci_0_1_func_ids.cpu_off = func_id; + _psci_ops.cpu_off = psci_0_1_cpu_off; + } + + if (!rt_ofw_prop_read_u32(np, "cpu_suspend", &func_id)) { - // 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); + psci_0_1_func_ids.cpu_suspend = func_id; + _psci_ops.cpu_suspend = psci_0_1_cpu_suspend; + } + + if (!rt_ofw_prop_read_u32(np, "migrate", &func_id)) + { + psci_0_1_func_ids.migrate = func_id; + _psci_ops.migrate = psci_0_1_migrate; + } + + return err; +} + +static rt_err_t psci_0_2_init(struct rt_ofw_node *np) +{ + rt_err_t err = RT_EOK; + rt_uint32_t version = psci_0_2_get_version(); + + if (version >= PSCI_VERSION(0, 2)) + { + _psci_ops.get_version = psci_0_2_get_version; + _psci_ops.cpu_on = psci_0_2_cpu_on; + _psci_ops.cpu_off = psci_0_2_cpu_off; + _psci_ops.cpu_suspend = psci_0_2_cpu_suspend; + _psci_ops.migrate = psci_0_2_migrate; + _psci_ops.get_affinity_info = psci_affinity_info; + _psci_ops.migrate_info_type = psci_migrate_info_type; } else { - LOG_E("[%s] was not a proper PSCI version", version); - retval = -1; + LOG_E("PSCI version detected"); + err = -RT_EINVAL; } - LOG_D("Using PSCI v%d.%d", *major, *minor); - return retval; + + return err; +} + +static rt_err_t psci_1_0_init(struct rt_ofw_node *np) +{ + rt_err_t err; + + err = psci_0_2_init(np); + + return err; } -#endif /* RT_USING_FDT */ -/* init psci ops with version info */ -static int _psci_init_with_version(int major, int minor) +static rt_err_t psci_ofw_init(struct rt_platform_device *pdev) { - int retval = -0xbeef; // mark unsupported - if (major == 0) + rt_err_t err = RT_EOK; + const char *method; + const struct rt_ofw_node_id *id = pdev->id; + struct rt_ofw_node *np = pdev->parent.ofw_node; + + if (!rt_ofw_prop_read_string(np, "method", &method)) { - // for v0.1, psci function id was provided fdt - if (minor == 1) + if (!rt_strcmp(method, "smc")) { -#ifdef RT_USING_FDT - retval = psci_0_1_init(); -#endif + psci_call = psci_smc_call; + } + else if (!rt_strcmp(method, "hvc")) + { + psci_call = psci_hvc_call; } - else if (minor == 2) + else { - retval = psci_0_2_init(); + LOG_E("Invalid \"method\" property: %s", method); + err = -RT_EINVAL; + } + + if (!err) + { + psci_init_ofw_handle psci_init = (psci_init_ofw_handle)id->data; + + err = psci_init(np); + + if (!err) + { + rt_uint32_t version = rt_psci_get_version(); + + rt_ofw_data(np) = &_psci_ops; + + RT_UNUSED(version); + + LOG_I("Using PSCI v%d.%d Function IDs", PSCI_VERSION_MAJOR(version), PSCI_VERSION_MINOR(version)); + } } } - else if (major == 1) + else { - // psci_1_0_init is a base setup for version after v1.0 - retval = psci_1_0_init(); + err = -RT_ENOSYS; } - if (retval == -0xbeef) - { - LOG_E("PSCI init with incompatible version %d.%d", major, minor); - } - return retval; + return err; +} + +static rt_err_t psci_probe(struct rt_platform_device *pdev) +{ + rt_err_t err; + + err = psci_ofw_init(pdev); + + return err; +} + +static const struct rt_ofw_node_id psci_ofw_ids[] = +{ + { .compatible = "arm,psci", .data = psci_0_1_init }, + { .compatible = "arm,psci-0.2", .data = psci_0_2_init }, + { .compatible = "arm,psci-1.0", .data = psci_1_0_init }, + { /* sentinel */ } +}; + +static struct rt_platform_driver psci_driver = +{ + .name = "arm-psci", + .ids = psci_ofw_ids, + + .probe = psci_probe, +}; + +static int psci_drv_register(void) +{ + rt_platform_driver_register(&psci_driver); + + return 0; } +INIT_FRAMEWORK_EXPORT(psci_drv_register); diff --git a/libcpu/aarch64/common/psci.h b/libcpu/aarch64/common/psci.h index 3cce66211f..5557d9192b 100644 --- a/libcpu/aarch64/common/psci.h +++ b/libcpu/aarch64/common/psci.h @@ -5,21 +5,26 @@ * * Change Logs: * Date Author Notes + * 2021-09-09 GuEe-GUI The first version */ + #ifndef __PSCI_H__ #define __PSCI_H__ -/** - * PSCI protocol content - * For PSCI v0.1, only return values below are protocol defined +#include <rtdef.h> + +/* + * Non-Confidential PSCI 1.0 release (30 January 2015), and errata fix for PSCI 0.2, unsupport PSCI 0.1 */ -/* PSCI v0.2 interface */ +/* PSCI 0.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_FN_END 0x8400001F + +#define PSCI_0_2_FN64_BASE 0xC4000000 #define PSCI_0_2_FN64(n) (PSCI_0_2_FN64_BASE + (n)) +#define PSCI_0_2_FN64_END 0xC400001F #define PSCI_0_2_FN_PSCI_VERSION PSCI_0_2_FN(0) #define PSCI_0_2_FN_CPU_SUSPEND PSCI_0_2_FN(1) @@ -35,69 +40,112 @@ #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 PSCI_0_2_FN64(5) #define PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU PSCI_0_2_FN64(7) +/* PSCI 1.0 interface */ #define PSCI_1_0_FN_PSCI_FEATURES PSCI_0_2_FN(10) +#define PSCI_1_0_FN_CPU_FREEZE PSCI_0_2_FN(11) +#define PSCI_1_0_FN_CPU_DEFAULT_SUSPEND PSCI_0_2_FN(12) +#define PSCI_1_0_FN_NODE_HW_STATE PSCI_0_2_FN(13) #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_0_FN_STAT_RESIDENCY PSCI_0_2_FN(16) +#define PSCI_1_0_FN_STAT_COUNT PSCI_0_2_FN(17) #define PSCI_1_1_FN_SYSTEM_RESET2 PSCI_0_2_FN(18) +#define PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND PSCI_0_2_FN64(12) +#define PSCI_1_0_FN64_NODE_HW_STATE PSCI_0_2_FN64(13) #define PSCI_1_0_FN64_SYSTEM_SUSPEND PSCI_0_2_FN64(14) +#define PSCI_1_0_FN64_STAT_RESIDENCY PSCI_0_2_FN64(16) +#define PSCI_1_0_FN64_STAT_COUNT PSCI_0_2_FN64(17) #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)) +#define PSCI_VERSION_MAJOR(version) (((version) & PSCI_VERSION_MAJOR_MASK) >> PSCI_VERSION_MAJOR_SHIFT) +#define PSCI_VERSION_MINOR(version) ((version) & PSCI_VERSION_MINOR_MASK) +#define PSCI_VERSION(major, min) ((((major) << PSCI_VERSION_MAJOR_SHIFT) & PSCI_VERSION_MAJOR_MASK) | \ + ((min) & PSCI_VERSION_MINOR_MASK)) + +/* PSCI affinity level state returned by AFFINITY_INFO */ +#define PSCI_AFFINITY_LEVEL_ON 0 +#define PSCI_AFFINITY_LEVEL_OFF 1 +#define PSCI_AFFINITY_LEVEL_ON_PENDING 2 + +/* + * PSCI power state + * power_level: + * Level 0: cores + * Level 1: clusters + * Level 2: system + * state_type: + * value 0: standby or retention state + * value 1: powerdown state(entry and context_id is valid) + * state_id: + * StateID + */ +#define PSCI_POWER_STATE_LEVEL_CORES 0 +#define PSCI_POWER_STATE_LEVEL_CLUSTERS 1 +#define PSCI_POWER_STATE_LEVEL_SYSTEM 2 + +#define PSCI_POWER_STATE_TYPE_STANDBY 0 +#define PSCI_POWER_STATE_TYPE_POWER_DOWN 1 + +#define PSCI_POWER_LEVEL_SHIFT 24 +#define PSCI_POWER_STATE_TYPE_SHIFT 16 +#define PSCI_POWER_STATE_ID_SHIFT 0 +#define PSCI_POWER_STATE(power_level, state_type, state_id) \ +( \ + ((power_level) << PSCI_POWER_LEVEL_SHIFT) | \ + ((state_type) << PSCI_POWER_STATE_TYPE_SHIFT) | \ + ((state_id) << PSCI_POWER_STATE_ID_SHIFT) \ +) +#define PSCI_POWER_LEVEL_VAL(state) (((state) >> PSCI_POWER_LEVEL_SHIFT) & 0x3) +#define PSCI_POWER_STATE_TYPE_VAL(state) (((state) >> PSCI_POWER_STATE_TYPE_SHIFT) & 0x1) +#define PSCI_POWER_STATE_ID_VAL(state) (((state) >> PSCI_POWER_STATE_ID_SHIFT) & 0xffff) + +/* + * For system, cluster, core + * 0: run + * 1: standby(only core) + * 2: retention + * 3: powerdown + */ +#define PSCI_POWER_STATE_ID_RUN 0 +#define PSCI_POWER_STATE_ID_STANDBY 1 +#define PSCI_POWER_STATE_ID_RETENTION 2 +#define PSCI_POWER_STATE_ID_POWERDOWN 3 -/* 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_POWER_STATE_ID(state_id_power_level, system, cluster, core) \ +( \ + ((state_id_power_level) << 12) | \ + ((system) << 8) | \ + ((cluster) << 4) | \ + (core) \ +) -#define PSCI_1_0_OS_INITIATED BIT(0) -#define PSCI_1_0_SUSPEND_MODE_PC 0 -#define PSCI_1_0_SUSPEND_MODE_OSI 1 +#define PSCI_RET_SUCCESS 0 +#define PSCI_RET_NOT_SUPPORTED (-1) +#define PSCI_RET_INVALID_PARAMETERS (-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) -/* 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 +void psci_system_off(void); +void psci_system_reboot(void); +rt_uint32_t rt_psci_get_version(void); +rt_uint32_t rt_psci_cpu_on(int cpuid, rt_ubase_t entry_point); +rt_uint32_t rt_psci_cpu_off(rt_uint32_t state); +rt_uint32_t rt_psci_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point); +rt_uint32_t rt_psci_migrate(int cpuid); +rt_uint32_t rt_psci_get_affinity_info(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level); +rt_uint32_t rt_psci_migrate_info_type(void); -#endif /*__PSCI_H__*/ +#endif /* __PSCI_H__ */ diff --git a/libcpu/aarch64/common/psci_api.h b/libcpu/aarch64/common/psci_api.h deleted file mode 100644 index 36566c3e6f..0000000000 --- a/libcpu/aarch64/common/psci_api.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * 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); - - void (*system_off)(void); - void (*system_reset)(void); -}; - -extern struct psci_ops_t psci_ops; - -extern int psci_init(void); - -#endif // __PSCI_API_H__ diff --git a/libcpu/aarch64/common/setup.c b/libcpu/aarch64/common/setup.c new file mode 100644 index 0000000000..9f9709db47 --- /dev/null +++ b/libcpu/aarch64/common/setup.c @@ -0,0 +1,219 @@ +/* + * Copyright (c) 2006-2023, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2023-02-21 GuEe-GUI first version + */ + +#include <rtthread.h> + +#define DBG_TAG "cpu.aa64" +#define DBG_LVL DBG_INFO +#include <rtdbg.h> + +#include <cpu.h> +#include <mmu.h> +#include <cpuport.h> +#include <interrupt.h> + +#include <stdlib.h> +#include <ioremap.h> +#include <drivers/ofw.h> +#include <drivers/ofw_fdt.h> +#include <drivers/ofw_raw.h> +#include <drivers/core/dm.h> + +#define rt_sysreg_write(sysreg, val) \ + __asm__ volatile ("msr "RT_STRINGIFY(sysreg)", %0"::"r"((rt_uint64_t)(val))) + +#define rt_sysreg_read(sysreg, val) \ + __asm__ volatile ("mrs %0, "RT_STRINGIFY(sysreg)"":"=r"((val))) + +extern void _secondary_cpu_entry(void); +extern size_t MMUTable[]; +extern void *system_vectors; + +static void *fdt_ptr = RT_NULL; +static rt_size_t fdt_size = 0; + +#ifdef RT_USING_SMP +extern struct cpu_ops_t cpu_psci_ops; +extern struct cpu_ops_t cpu_spin_table_ops; +#else +extern int rt_hw_cpu_id(void); +#endif + +rt_uint64_t rt_cpu_mpidr_table[] = +{ + [RT_CPUS_NR] = 0, +}; + +static struct cpu_ops_t *cpu_ops[] = +{ +#ifdef RT_USING_SMP + &cpu_psci_ops, + &cpu_spin_table_ops, +#endif +}; + +static struct rt_ofw_node *cpu_np[RT_CPUS_NR] = { }; + +void rt_hw_fdt_install_early(void *fdt) +{ + void *fdt_vaddr = fdt - PV_OFFSET; + + if (fdt != RT_NULL && !fdt_check_header(fdt_vaddr)) + { + fdt_ptr = fdt_vaddr; + fdt_size = fdt_totalsize(fdt_vaddr); + } +} + +static void system_vectors_init(void) +{ + rt_hw_set_current_vbar((rt_ubase_t)&system_vectors); +} + +rt_inline void cpu_info_init(void) +{ + int i = 0; + rt_uint64_t mpidr; + struct rt_ofw_node *np; + + /* get boot cpu info */ + rt_sysreg_read(mpidr_el1, mpidr); + + rt_ofw_foreach_cpu_node(np) + { + rt_uint64_t hwid = rt_ofw_get_cpu_hwid(np, 0); + + if ((mpidr & MPIDR_AFFINITY_MASK) != hwid) + { + /* Only save affinity and res make smp boot can check */ + hwid |= 1ULL << 31; + } + else + { + hwid = mpidr; + } + + cpu_np[i] = np; + rt_cpu_mpidr_table[i] = hwid; + + rt_ofw_data(np) = (void *)hwid; + + for (int idx = 0; idx < RT_ARRAY_SIZE(cpu_ops); ++idx) + { + struct cpu_ops_t *ops = cpu_ops[idx]; + + if (ops->cpu_init) + { + ops->cpu_init(i, np); + } + } + + if (++i >= RT_CPUS_NR) + { + break; + } + } + + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, rt_cpu_mpidr_table, sizeof(rt_cpu_mpidr_table)); +} + +void rt_hw_common_setup(void) +{ + if (rt_fdt_prefetch(fdt_ptr)) + { + /* Platform cannot be initialized */ + RT_ASSERT(0); + } + + rt_fdt_unflatten(); + + cpu_info_init(); +} + +#ifdef RT_USING_SMP +rt_weak void rt_hw_secondary_cpu_up(void) +{ + int cpu_id = rt_hw_cpu_id(); + rt_uint64_t entry = (rt_uint64_t)rt_kmem_v2p(_secondary_cpu_entry); + + if (!entry) + { + LOG_E("Failed to translate '_secondary_cpu_entry' to physical address"); + RT_ASSERT(0); + } + + /* Maybe we are no in the first cpu */ + for (int i = 0; i < RT_ARRAY_SIZE(cpu_np); ++i) + { + int err; + const char *enable_method; + + if (!cpu_np[i] || i == cpu_id) + { + continue; + } + + err = rt_ofw_prop_read_string(cpu_np[i], "enable-method", &enable_method); + + for (int idx = 0; !err && idx < RT_ARRAY_SIZE(cpu_ops); ++idx) + { + struct cpu_ops_t *ops = cpu_ops[idx]; + + if (ops->method && !rt_strcmp(ops->method, enable_method) && ops->cpu_boot) + { + err = ops->cpu_boot(i, entry); + + break; + } + } + + if (err) + { + LOG_W("Call cpu %d on %s", i, "failed"); + } + } +} + +rt_weak void rt_hw_secondary_cpu_bsp_start(void) +{ + int cpu_id = rt_hw_cpu_id(); + + system_vectors_init(); + + rt_hw_spin_lock(&_cpus_lock); + + /* Save all mpidr */ + rt_sysreg_read(mpidr_el1, rt_cpu_mpidr_table[cpu_id]); + + rt_hw_mmu_ktbl_set((unsigned long)MMUTable); + + rt_hw_interrupt_init(); + + rt_dm_secondary_cpu_init(); + rt_hw_interrupt_umask(RT_SCHEDULE_IPI); + rt_hw_interrupt_umask(RT_STOP_IPI); + + LOG_I("Call cpu %d on %s", cpu_id, "success"); + +#ifdef RT_USING_HWTIMER + if (rt_device_hwtimer_us_delay == &cpu_us_delay) + { + cpu_loops_per_tick_init(); + } +#endif + + rt_system_scheduler_start(); +} + +rt_weak void rt_hw_secondary_cpu_idle_exec(void) +{ + rt_hw_wfe(); +} +#endif diff --git a/libcpu/aarch64/common/setup.h b/libcpu/aarch64/common/setup.h new file mode 100644 index 0000000000..f571dba8f0 --- /dev/null +++ b/libcpu/aarch64/common/setup.h @@ -0,0 +1,22 @@ +/* + * Copyright (c) 2006-2023, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2023-02-21 GuEe-GUI first version + */ + +#ifndef __SETUP_H__ +#define __SETUP_H__ + +#include <rtdef.h> +#include <mm_aspace.h> +#ifdef RT_USING_OFW +#include <drivers/ofw_fdt.h> +#endif + +void rt_hw_common_setup(void); + +#endif /* __SETUP_H__ */ |