arm/mach-milbeaut/platsmp.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright:   (C) 2018 Socionext Inc.
0004  * Copyright:   (C) 2015 Linaro Ltd.
0005  */
0006
0007 #include <linux/cpu_pm.h>
0008 #include <linux/irqchip/arm-gic.h>
0009 #include <linux/of_address.h>
0010 #include <linux/suspend.h>
0011
0012 #include <asm/cacheflush.h>
0013 #include <asm/cp15.h>
0014 #include <asm/idmap.h>
0015 #include <asm/smp_plat.h>
0016 #include <asm/suspend.h>
0017
0018 #define M10V_MAX_CPU    4
0019 #define KERNEL_UNBOOT_FLAG  0x12345678
0020
0021 static void __iomem *m10v_smp_base;
0022
0023 static int m10v_boot_secondary(unsigned int l_cpu, struct task_struct *idle)
0024 {
0025     unsigned int mpidr, cpu, cluster;
0026
0027     if (!m10v_smp_base)
0028         return -ENXIO;
0029
0030     mpidr = cpu_logical_map(l_cpu);
0031     cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
0032     cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
0033
0034     if (cpu >= M10V_MAX_CPU)
0035         return -EINVAL;
0036
0037     pr_info("%s: cpu %u l_cpu %u cluster %u\n",
0038             __func__, cpu, l_cpu, cluster);
0039
0040     writel(__pa_symbol(secondary_startup), m10v_smp_base + cpu * 4);
0041     arch_send_wakeup_ipi_mask(cpumask_of(l_cpu));
0042
0043     return 0;
0044 }
0045
0046 static void m10v_smp_init(unsigned int max_cpus)
0047 {
0048     unsigned int mpidr, cpu, cluster;
0049     struct device_node *np;
0050
0051     np = of_find_compatible_node(NULL, NULL, "socionext,milbeaut-smp-sram");
0052     if (!np)
0053         return;
0054
0055     m10v_smp_base = of_iomap(np, 0);
0056     if (!m10v_smp_base)
0057         return;
0058
0059     mpidr = read_cpuid_mpidr();
0060     cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
0061     cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
0062     pr_info("MCPM boot on cpu_%u cluster_%u\n", cpu, cluster);
0063
0064     for (cpu = 0; cpu < M10V_MAX_CPU; cpu++)
0065         writel(KERNEL_UNBOOT_FLAG, m10v_smp_base + cpu * 4);
0066 }
0067
0068 #ifdef CONFIG_HOTPLUG_CPU
0069 static void m10v_cpu_die(unsigned int l_cpu)
0070 {
0071     gic_cpu_if_down(0);
0072     v7_exit_coherency_flush(louis);
0073     wfi();
0074 }
0075
0076 static int m10v_cpu_kill(unsigned int l_cpu)
0077 {
0078     unsigned int mpidr, cpu;
0079
0080     mpidr = cpu_logical_map(l_cpu);
0081     cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
0082
0083     writel(KERNEL_UNBOOT_FLAG, m10v_smp_base + cpu * 4);
0084
0085     return 1;
0086 }
0087 #endif
0088
0089 static struct smp_operations m10v_smp_ops __initdata = {
0090     .smp_prepare_cpus   = m10v_smp_init,
0091     .smp_boot_secondary = m10v_boot_secondary,
0092 #ifdef CONFIG_HOTPLUG_CPU
0093     .cpu_die        = m10v_cpu_die,
0094     .cpu_kill       = m10v_cpu_kill,
0095 #endif
0096 };
0097 CPU_METHOD_OF_DECLARE(m10v_smp, "socionext,milbeaut-m10v-smp", &m10v_smp_ops);
0098
0099 static int m10v_pm_valid(suspend_state_t state)
0100 {
0101     return (state == PM_SUSPEND_STANDBY) || (state == PM_SUSPEND_MEM);
0102 }
0103
0104 typedef void (*phys_reset_t)(unsigned long);
0105 static phys_reset_t phys_reset;
0106
0107 static int m10v_die(unsigned long arg)
0108 {
0109     setup_mm_for_reboot();
0110     asm("wfi");
0111     /* Boot just like a secondary */
0112     phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset);
0113     phys_reset(virt_to_phys(cpu_resume));
0114
0115     return 0;
0116 }
0117
0118 static int m10v_pm_enter(suspend_state_t state)
0119 {
0120     switch (state) {
0121     case PM_SUSPEND_STANDBY:
0122         asm("wfi");
0123         break;
0124     case PM_SUSPEND_MEM:
0125         cpu_pm_enter();
0126         cpu_suspend(0, m10v_die);
0127         cpu_pm_exit();
0128         break;
0129     }
0130     return 0;
0131 }
0132
0133 static const struct platform_suspend_ops m10v_pm_ops = {
0134     .valid      = m10v_pm_valid,
0135     .enter      = m10v_pm_enter,
0136 };
0137
0138 struct clk *m10v_clclk_register(struct device *cpu_dev);
0139
0140 static int __init m10v_pm_init(void)
0141 {
0142     if (of_machine_is_compatible("socionext,milbeaut-evb"))
0143         suspend_set_ops(&m10v_pm_ops);
0144
0145     return 0;
0146 }
0147 late_initcall(m10v_pm_init);