You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
151 lines
2.9 KiB
151 lines
2.9 KiB
/* |
|
* Copyright 2020 Carlo Caione <ccaione@baylibre.com> |
|
* |
|
* SPDX-License-Identifier: Apache-2.0 |
|
*/ |
|
|
|
#define DT_DRV_COMPAT arm_psci_0_2 |
|
|
|
#define LOG_LEVEL CONFIG_PM_CPU_OPS_LOG_LEVEL |
|
#include <logging/log.h> |
|
LOG_MODULE_REGISTER(psci); |
|
|
|
#include <kernel.h> |
|
#include <arch/cpu.h> |
|
|
|
#include <soc.h> |
|
#include <device.h> |
|
#include <init.h> |
|
|
|
#include <drivers/pm_cpu_ops.h> |
|
#include "pm_cpu_ops_psci.h" |
|
|
|
static struct psci psci_data; |
|
|
|
static int psci_to_dev_err(int ret) |
|
{ |
|
switch (ret) { |
|
case PSCI_RET_SUCCESS: |
|
return 0; |
|
case PSCI_RET_NOT_SUPPORTED: |
|
return -ENOTSUP; |
|
case PSCI_RET_INVALID_PARAMS: |
|
case PSCI_RET_INVALID_ADDRESS: |
|
return -EINVAL; |
|
case PSCI_RET_DENIED: |
|
return -EPERM; |
|
} |
|
|
|
return -EINVAL; |
|
} |
|
|
|
int pm_cpu_off(void) |
|
{ |
|
int ret; |
|
|
|
if (psci_data.conduit == SMCCC_CONDUIT_NONE) |
|
return -EINVAL; |
|
|
|
ret = psci_data.invoke_psci_fn(PSCI_0_2_FN_CPU_OFF, 0, 0, 0); |
|
|
|
return psci_to_dev_err(ret); |
|
} |
|
|
|
int pm_cpu_on(unsigned long cpuid, |
|
uintptr_t entry_point) |
|
{ |
|
int ret; |
|
|
|
if (psci_data.conduit == SMCCC_CONDUIT_NONE) |
|
return -EINVAL; |
|
|
|
ret = psci_data.invoke_psci_fn(PSCI_FN_NATIVE(0_2, CPU_ON), cpuid, |
|
(unsigned long) entry_point, 0); |
|
|
|
return psci_to_dev_err(ret); |
|
} |
|
|
|
static unsigned long __invoke_psci_fn_hvc(unsigned long function_id, |
|
unsigned long arg0, |
|
unsigned long arg1, |
|
unsigned long arg2) |
|
{ |
|
struct arm_smccc_res res; |
|
|
|
arm_smccc_hvc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res); |
|
return res.a0; |
|
} |
|
|
|
static unsigned long __invoke_psci_fn_smc(unsigned long function_id, |
|
unsigned long arg0, |
|
unsigned long arg1, |
|
unsigned long arg2) |
|
{ |
|
struct arm_smccc_res res; |
|
|
|
arm_smccc_smc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res); |
|
return res.a0; |
|
} |
|
|
|
static uint32_t psci_get_version(void) |
|
{ |
|
return psci_data.invoke_psci_fn(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); |
|
} |
|
|
|
static int set_conduit_method(void) |
|
{ |
|
const char *method; |
|
|
|
method = DT_PROP(DT_INST(0, DT_DRV_COMPAT), method); |
|
|
|
if (!strcmp("hvc", method)) { |
|
psci_data.conduit = SMCCC_CONDUIT_HVC; |
|
psci_data.invoke_psci_fn = __invoke_psci_fn_hvc; |
|
} else if (!strcmp("smc", method)) { |
|
psci_data.conduit = SMCCC_CONDUIT_SMC; |
|
psci_data.invoke_psci_fn = __invoke_psci_fn_smc; |
|
} else { |
|
LOG_ERR("Invalid conduit method"); |
|
return -EINVAL; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static int psci_detect(void) |
|
{ |
|
uint32_t ver = psci_get_version(); |
|
|
|
LOG_DBG("Detected PSCIv%d.%d", |
|
PSCI_VERSION_MAJOR(ver), |
|
PSCI_VERSION_MINOR(ver)); |
|
|
|
if (PSCI_VERSION_MAJOR(ver) == 0 && PSCI_VERSION_MINOR(ver) < 2) { |
|
LOG_ERR("PSCI unsupported version"); |
|
return -ENOTSUP; |
|
} |
|
|
|
psci_data.ver = ver; |
|
|
|
return 0; |
|
} |
|
|
|
uint32_t psci_version(void) |
|
{ |
|
return psci_data.ver; |
|
} |
|
|
|
static int psci_init(const struct device *dev) |
|
{ |
|
psci_data.conduit = SMCCC_CONDUIT_NONE; |
|
|
|
if (set_conduit_method()) { |
|
return -ENOTSUP; |
|
} |
|
|
|
return psci_detect(); |
|
} |
|
|
|
DEVICE_DT_INST_DEFINE(0, psci_init, NULL, |
|
&psci_data, NULL, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_DEVICE, |
|
NULL);
|
|
|