drivers/pm_cpu_ops/pm_cpu_ops_psci.c

/*
 * 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 <zephyr/logging/log.h>
LOG_MODULE_REGISTER(psci);

#include <zephyr/kernel.h>
#include <zephyr/arch/cpu.h>

#include <zephyr/device.h>
#include <zephyr/init.h>

#include <zephyr/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);