[libcpu/aarch64] 增加cpu抽象及PSCI和Spin-table的CPU操作实现

libcpu/aarch64/common/cpu.c

@@ -14,6 +14,12 @@
 #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)
 {
@@ -62,8 +68,245 @@ void rt_hw_spin_unlock(rt_hw_spinlock_t *lock)
         : "r"(lock->tickets.owner + 1)
         : "memory");
 }
+
+#ifdef RT_CPUS_NR
+#define ID_ERROR __INT64_MAX__
+
+/**
+ * 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 */
+
+#else // RT_CPUS_NR not define
+#error "RT_CPUS_NR not define"
+#endif /* RT_CPUS_NR */
+
+#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++)
+    {
+        cpuid_to_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 = *pcpuid + 1;
+    rt_cpu_mpidr_early[*pcpuid] = mpid;
+
+    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;
+    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;
+        }
+
+        _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
+    for (int i = 0; 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_init)
+        {
+            retval = cpu_ops_tbl[i]->cpu_init(i);
+            CHECK_RETVAL(retval);
+        }
+        else
+        {
+            LOG_E("No cpu_init() supported in cpu %d", cpuid_to_hwid(i));
+        }
+    }
+    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);
+
+    if (!retval)
+        _boot_secondary();
+    return retval;
+}
+
 #endif /*RT_USING_SMP*/
 
+#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 */
+}
+
 /**
  * @addtogroup ARM CPU
  */
@@ -82,4 +325,10 @@ void rt_hw_cpu_shutdown()
     }
 }
 
+RT_WEAK void rt_hw_secondary_cpu_idle_exec(void)
+{
+    asm volatile("wfe" ::
+                     : "memory", "cc");
+}
+
 /*@}*/

libcpu/aarch64/common/cpu.h

@@ -0,0 +1,37 @@
+/*
+ * 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);
+};
+
+extern rt_uint64_t rt_cpu_mpidr_early[];
+
+#define cpuid_to_hwid(cpuid) rt_cpu_mpidr_early[cpuid]
+
+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 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_psci.c

@@ -0,0 +1,57 @@
+/*
+ * 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.h"
+#include "errno.h"
+#include "psci.h"
+#include "psci_api.h"
+#include "entry_point.h"
+
+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 = (rt_uint64_t)_secondary_cpu_entry + PV_OFFSET;
+    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,60 @@
+/*
+ * 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"
+
+#ifdef RT_USING_FDT
+#include <dtb_node.h>
+#include "entry_point.h"
+
+#define get_cpu_node(cpuid) _cpu_node[cpuid]
+extern struct dtb_node *_cpu_node[];
+
+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;
+    
+    int size;
+    rt_uint64_t head = (rt_uint64_t)dtb_node_get_dtb_node_property_value(cpu, "cpu-release-addr", &size);
+
+    cpu_release_addr[cpuid] = fdt64_to_cpu(head);
+
+    return 0;
+}
+
+static int spin_table_cpu_boot(rt_uint32_t cpuid)
+{
+    rt_uint64_t secondary_entry_pa = (rt_uint64_t)_secondary_cpu_entry + PV_OFFSET;
+    // map release_addr to addressable place
+    void *rel_va = rt_ioremap((void *)cpu_release_addr[cpuid], sizeof(cpu_release_addr[0]));
+
+    if (!rel_va)
+        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/psci.c

@@ -70,10 +70,15 @@ 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;
+
 /**
- * init psci operations.
+ * @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()
 {
@@ -85,7 +90,7 @@ int psci_init()
     }
     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 ver_major, ver_minor;
+    
     int retval = 0;
 
     // setup psci-method
@@ -103,12 +108,12 @@ int psci_init()
         return -1;
     }
 
-    retval = _psci_probe_version(compatible, &ver_major, &ver_minor);
+    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(ver_major, ver_minor);
+    retval = _psci_init_with_version(psci_ver_major, psci_ver_minor);
 
     return retval;
 }
@@ -168,7 +173,7 @@ static rt_uint32_t psci_0_2_get_version(void)
 static void psci_0_2_set_basic_ops()
 {
     psci_ops = (struct psci_operations){
-        .get_version = psci_0_2_get_version,
+        .get_version = psci_0_2_get_version, 
 
         // followings API are v0.1 compatible
         .cpu_suspend = psci_0_2_cpu_suspend,