Add Cortex-A Core remoteproc support, it use PSCI and SIP SMC call to manage Cortex-A Core to be on or off.
Signed-off-by: Jiafei Pan <[email protected]> --- drivers/remoteproc/imx_rproc.c | 134 +++++++++++++++++++++++++++++++++ drivers/remoteproc/imx_rproc.h | 2 + 2 files changed, 136 insertions(+) diff --git a/drivers/remoteproc/imx_rproc.c b/drivers/remoteproc/imx_rproc.c index 0dd80e688b0e..8a3de27c96b7 100644 --- a/drivers/remoteproc/imx_rproc.c +++ b/drivers/remoteproc/imx_rproc.c @@ -6,6 +6,7 @@ #include <dt-bindings/firmware/imx/rsrc.h> #include <linux/arm-smccc.h> #include <linux/clk.h> +#include <linux/cpu.h> #include <linux/err.h> #include <linux/firmware/imx/sci.h> #include <linux/firmware/imx/sm.h> @@ -20,12 +21,17 @@ #include <linux/platform_device.h> #include <linux/pm_domain.h> #include <linux/pm_runtime.h> +#include <linux/psci.h> #include <linux/reboot.h> #include <linux/regmap.h> #include <linux/remoteproc.h> #include <linux/scmi_imx_protocol.h> #include <linux/workqueue.h> +#include <uapi/linux/psci.h> + +#include <asm/smp_plat.h> + #include "imx_rproc.h" #include "remoteproc_internal.h" @@ -72,6 +78,8 @@ #define IMX_SIP_RPROC_STARTED 0x01 #define IMX_SIP_RPROC_STOP 0x02 +#define IMX_SIP_CPU_OFF 0xC2000012 + #define IMX_SC_IRQ_GROUP_REBOOTED 5 /** @@ -131,6 +139,9 @@ struct imx_rproc { * BIT 0: IMX_RPROC_FLAGS_SM_LMM_CTRL(RPROC LM is under Linux control ) */ u32 flags; + /* used by Cortex-A Core remoteproc to manage all CPU Cores */ + cpumask_t cpus; + cpumask_t offlined_cpus; }; static const struct imx_rproc_att imx_rproc_att_imx95_m7[] = { @@ -381,6 +392,45 @@ static int imx_rproc_sm_lmm_start(struct rproc *rproc) return 0; } +static int imx_rproc_psci_start(struct rproc *rproc) +{ + struct imx_rproc *priv = rproc->priv; + struct device *dev = priv->dev; + unsigned int cpu; + int ret; + + if (cpumask_empty(&priv->cpus)) { + dev_err(dev, "No CPU Core assigned!\n"); + return -ENODEV; + } + + for_each_cpu(cpu, &priv->cpus) { + if (cpu_online(cpu)) { + ret = remove_cpu(cpu); + if (ret) + goto err; + cpumask_set_cpu(cpu, &priv->offlined_cpus); + } + } + + cpu = cpumask_first(&priv->cpus); + ret = psci_ops.cpu_on(cpu_logical_map(cpu), rproc->bootaddr); + if (ret) { + dev_err(dev, "Boot failed on CPU Core %d\n", cpu); + goto err; + } + + return 0; + +err: + for_each_cpu(cpu, &priv->cpus) { + if (!cpu_online(cpu) && add_cpu(cpu) == 0) + cpumask_clear_cpu(cpu, &priv->offlined_cpus); + } + + return ret; +} + static int imx_rproc_start(struct rproc *rproc) { struct imx_rproc *priv = rproc->priv; @@ -456,6 +506,50 @@ static int imx_rproc_sm_lmm_stop(struct rproc *rproc) return scmi_imx_lmm_operation(dcfg->lmid, SCMI_IMX_LMM_SHUTDOWN, 0); } +static int imx_rproc_psci_stop(struct rproc *rproc) +{ + struct imx_rproc *priv = rproc->priv; + struct device *dev = priv->dev; + struct arm_smccc_res res; + unsigned int cpu; + unsigned long start, end; + int err; + + for_each_cpu(cpu, &priv->cpus) { + /* Check CPU status */ + err = psci_ops.affinity_info(cpu_logical_map(cpu), 0); + if (err == PSCI_0_2_AFFINITY_LEVEL_OFF) + continue; + + /* Bring CPU to be off */ + arm_smccc_smc(IMX_SIP_CPU_OFF, cpu, 0, + 0, 0, 0, 0, 0, &res); + start = jiffies; + end = start + msecs_to_jiffies(100); + do { + err = psci_ops.affinity_info(cpu_logical_map(cpu), 0); + if (err == PSCI_0_2_AFFINITY_LEVEL_OFF) { + pr_info("CPU%d is killed (polled %d ms)\n", cpu, + jiffies_to_msecs(jiffies - start)); + break; + } + + usleep_range(100, 1000); + } while (time_before(jiffies, end)); + } + + /* Return back freed CPU Core to Linux kernel */ + for_each_cpu(cpu, &priv->cpus) { + if (cpumask_test_cpu(cpu, &priv->offlined_cpus)) { + if (add_cpu(cpu) != 0) + dev_err(dev, "Failed to bring CPU %d back to be online", cpu); + cpumask_clear_cpu(cpu, &priv->offlined_cpus); + } + } + + return 0; +} + static int imx_rproc_stop(struct rproc *rproc) { struct imx_rproc *priv = rproc->priv; @@ -480,6 +574,12 @@ static int imx_rproc_da_to_sys(struct imx_rproc *priv, u64 da, const struct imx_rproc_dcfg *dcfg = priv->dcfg; int i; + /* No need to translate for Cortex-A Core */ + if (dcfg->flags & IMX_RPROC_NO_ADDR_TRANS) { + *sys = da; + return 0; + } + /* parse address translation table */ for (i = 0; i < dcfg->att_size; i++) { const struct imx_rproc_att *att = &dcfg->att[i]; @@ -1184,6 +1284,13 @@ static int imx_rproc_sm_detect_mode(struct rproc *rproc) return imx_rproc_sm_lmm_check(rproc, started); } +static int imx_rproc_psci_detect_mode(struct rproc *rproc) +{ + rproc->state = RPROC_OFFLINE; + + return 0; +} + static int imx_rproc_detect_mode(struct imx_rproc *priv) { /* @@ -1228,6 +1335,8 @@ static int imx_rproc_probe(struct platform_device *pdev) struct imx_rproc *priv; struct rproc *rproc; const struct imx_rproc_dcfg *dcfg; + unsigned int cpus; + unsigned long cpus_bits; int ret; /* set some other name then imx */ @@ -1274,6 +1383,17 @@ static int imx_rproc_probe(struct platform_device *pdev) if (ret) return dev_err_probe(dev, ret, "failed on imx_rproc_addr_init\n"); + ret = of_property_read_u32(dev->of_node, "fsl,cpus-bits", &cpus); + if (ret) { + cpumask_clear(&priv->cpus); + } else { + cpus_bits = cpus; + bitmap_copy(cpumask_bits(&priv->cpus), &cpus_bits, + min((unsigned int)nr_cpumask_bits, + (unsigned int)sizeof(unsigned long))); + rproc->auto_boot = false; + } + ret = imx_rproc_detect_mode(priv); if (ret) return dev_err_probe(dev, ret, "failed on detect mode\n"); @@ -1372,6 +1492,12 @@ static const struct imx_rproc_plat_ops imx_rproc_ops_sm_cpu = { .stop = imx_rproc_sm_cpu_stop, }; +static const struct imx_rproc_plat_ops imx_rproc_ops_psci = { + .start = imx_rproc_psci_start, + .stop = imx_rproc_psci_stop, + .detect_mode = imx_rproc_psci_detect_mode, +}; + static const struct imx_rproc_dcfg imx_rproc_cfg_imx8mn_mmio = { .src_reg = IMX7D_SRC_SCR, .src_mask = IMX7D_M4_RST_MASK, @@ -1464,6 +1590,13 @@ static const struct imx_rproc_dcfg imx_rproc_cfg_imx95_m7 = { .lmid = 1, /* Use 1 as Logical Machine ID where M7 resides */ }; +static const struct imx_rproc_dcfg imx_rproc_cfg_psci = { + .att = NULL, + .att_size = 0, + .ops = &imx_rproc_ops_psci, + .flags = IMX_RPROC_NO_ADDR_TRANS, +}; + static const struct of_device_id imx_rproc_of_match[] = { { .compatible = "fsl,imx7ulp-cm4", .data = &imx_rproc_cfg_imx7ulp }, { .compatible = "fsl,imx7d-cm4", .data = &imx_rproc_cfg_imx7d }, @@ -1479,6 +1612,7 @@ static const struct of_device_id imx_rproc_of_match[] = { { .compatible = "fsl,imx8ulp-cm33", .data = &imx_rproc_cfg_imx8ulp }, { .compatible = "fsl,imx93-cm33", .data = &imx_rproc_cfg_imx93 }, { .compatible = "fsl,imx95-cm7", .data = &imx_rproc_cfg_imx95_m7 }, + { .compatible = "fsl,imx-rproc-psci", .data = &imx_rproc_cfg_psci }, {}, }; MODULE_DEVICE_TABLE(of, imx_rproc_of_match); diff --git a/drivers/remoteproc/imx_rproc.h b/drivers/remoteproc/imx_rproc.h index d37e6f90548c..be6709971042 100644 --- a/drivers/remoteproc/imx_rproc.h +++ b/drivers/remoteproc/imx_rproc.h @@ -18,6 +18,8 @@ struct imx_rproc_att { /* dcfg flags */ #define IMX_RPROC_NEED_SYSTEM_OFF BIT(0) #define IMX_RPROC_NEED_CLKS BIT(1) +/* No need address translation */ +#define IMX_RPROC_NO_ADDR_TRANS BIT(2) struct imx_rproc_plat_ops { int (*start)(struct rproc *rproc); -- 2.43.0
