[libcpu/aarch64] 增加基本psci协议支持

libcpu/aarch64/common/psci.c

@@ -0,0 +1,252 @@
+/*
+ * 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_operations 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;
+
+/**
+ * init psci operations.
+ * using device tree to probe version and psci-method,
+ * setup psci ops for future use
+ */
+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 ver_major, ver_minor;
+    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, &ver_major, &ver_minor);
+    if (retval != 0)
+        return retval;
+
+    // init psci_ops with specified psci version
+    retval = _psci_init_with_version(ver_major, 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;
+
+#ifdef RT_USING_FDT
+/* 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()
+{
+#ifdef RT_USING_FDT
+    // 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;
+#else
+    return -1;
+#endif
+}
+
+#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));
+
+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_operations){
+        .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_I("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_operations
+{
+    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_operations psci_ops;
+
+extern int psci_init(void);
+
+#endif // __PSCI_API_H__

libcpu/aarch64/common/smccc.S

@@ -0,0 +1,25 @@
+
+/**
+ * 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__ */