libs/rt-thread-official
libs
/
rt-thread-official
mirror of https://github.com/RT-Thread/rt-thread.git
4
0
Fork 0
Code Issues Releases Wiki Activity
rt-thread-official/libcpu/aarch64/common/psci.c

419 lines
10 KiB
C
Raw Blame History

/*
* Copyright (c) 2006-2022, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* 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 <rtthread.h>
#define DBG_TAG "osi.psci"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>
/* 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
{
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;
};
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);
/* [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_FNC_ID(version_major, version_min, name) PSCI_##version_major##_##version_min##_FN_##name
#define MPIDR_MASK 0x00ffffff
#endif
static struct psci_ops _psci_ops = {};
static struct psci_0_1_func_ids psci_0_1_func_ids = {};
static psci_call_handle psci_call;
/* 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;
arm_smccc_smc(fn, arg0, arg1, arg2, 0, 0, 0, 0, &res, RT_NULL);
return res.a0;
}
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;
arm_smccc_hvc(fn, arg0, arg1, arg2, 0, 0, 0, 0, &res, RT_NULL);
return res.a0;
}
/* PSCI VERSION */
static rt_uint32_t psci_0_1_get_version(void)
{
return PSCI_VERSION(0, 1);
}
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);
}
/* 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)
{
rt_ubase_t mpid = rt_cpu_mpidr_table[cpuid] & MPIDR_MASK;
ret = (rt_uint32_t)psci_call(func_id, mpid, entry_point, 0);
}
return ret;
}
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);
}
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);
}
/* 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);
}
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);
}
static rt_uint32_t psci_0_2_cpu_off(rt_uint32_t state)
{
return psci_cpu_off(PSCI_0_2_FN_CPU_OFF, state);
}
/* 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)
{
return (rt_uint32_t)psci_call(func_id, power_state, entry_point, 0);
}
static rt_uint32_t psci_0_1_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point)
{
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);
}
/* PSCI CPU_MIGRATE */
static rt_uint32_t psci_migrate(rt_uint32_t func_id, int cpuid)
{
rt_uint32_t ret = -PSCI_RET_INVALID_PARAMETERS;
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_migrate(psci_0_1_func_ids.migrate, cpuid);
}
static rt_uint32_t psci_0_2_migrate(int cpuid)
{
return psci_migrate(PSCI_FNC_ID(0, 2, MIGRATE), cpuid);
}
/* 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);
}
/* 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);
}
/* PSCI SYSTEM_RESET */
void psci_system_reboot(void)
{
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);
}
}
#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)
{
return PSCI_CALL_FN_RET(get_version);
}
rt_uint32_t rt_psci_cpu_on(int cpuid, rt_ubase_t entry_point)
{
return PSCI_CALL_FN_RET(cpu_on, cpuid, entry_point);
}
rt_uint32_t rt_psci_cpu_off(rt_uint32_t state)
{
return PSCI_CALL_FN_RET(cpu_off, state);
}
rt_uint32_t rt_psci_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point)
{
return PSCI_CALL_FN_RET(cpu_suspend, power_state, entry_point);
}
rt_uint32_t rt_psci_migrate(int cpuid)
{
return PSCI_CALL_FN_RET(migrate, cpuid);
}
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);
}
rt_uint32_t rt_psci_migrate_info_type(void)
{
return PSCI_CALL_FN_RET(migrate_info_type);
}
#undef PSCI_CALL_FN_RET
#undef PSCI_CALL_FN
/* PSCI INIT */
static rt_err_t psci_0_1_init(struct rt_ofw_node *np)
{
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))
{
psci_0_1_func_ids.cpu_on = func_id;
_psci_ops.cpu_on = psci_0_1_cpu_on;
}
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))
{
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("PSCI version detected");
err = -RT_EINVAL;
}
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;
}
static rt_err_t psci_ofw_init(struct rt_platform_device *pdev)
{
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))
{
if (!rt_strcmp(method, "smc"))
{
psci_call = psci_smc_call;
}
else if (!rt_strcmp(method, "hvc"))
{
psci_call = psci_hvc_call;
}
else
{
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
{
err = -RT_ENOSYS;
}
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);
View Git Blame Copy Permalink