ARM: milbeaut: Add basic support for Milbeaut m10v SoC
[muen/linux.git] / arch / arm / mach-milbeaut / platsmp.c
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright:   (C) 2018 Socionext Inc.
4  * Copyright:   (C) 2015 Linaro Ltd.
5  */
6
7 #include <linux/cpu_pm.h>
8 #include <linux/irqchip/arm-gic.h>
9 #include <linux/of_address.h>
10 #include <linux/suspend.h>
11
12 #include <asm/cacheflush.h>
13 #include <asm/cp15.h>
14 #include <asm/idmap.h>
15 #include <asm/smp_plat.h>
16 #include <asm/suspend.h>
17
18 #define M10V_MAX_CPU    4
19 #define KERNEL_UNBOOT_FLAG      0x12345678
20
21 static void __iomem *m10v_smp_base;
22
23 static int m10v_boot_secondary(unsigned int l_cpu, struct task_struct *idle)
24 {
25         unsigned int mpidr, cpu, cluster;
26
27         if (!m10v_smp_base)
28                 return -ENXIO;
29
30         mpidr = cpu_logical_map(l_cpu);
31         cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
32         cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
33
34         if (cpu >= M10V_MAX_CPU)
35                 return -EINVAL;
36
37         pr_info("%s: cpu %u l_cpu %u cluster %u\n",
38                         __func__, cpu, l_cpu, cluster);
39
40         writel(__pa_symbol(secondary_startup), m10v_smp_base + cpu * 4);
41         arch_send_wakeup_ipi_mask(cpumask_of(l_cpu));
42
43         return 0;
44 }
45
46 static void m10v_smp_init(unsigned int max_cpus)
47 {
48         unsigned int mpidr, cpu, cluster;
49         struct device_node *np;
50
51         np = of_find_compatible_node(NULL, NULL, "socionext,milbeaut-smp-sram");
52         if (!np)
53                 return;
54
55         m10v_smp_base = of_iomap(np, 0);
56         if (!m10v_smp_base)
57                 return;
58
59         mpidr = read_cpuid_mpidr();
60         cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
61         cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
62         pr_info("MCPM boot on cpu_%u cluster_%u\n", cpu, cluster);
63
64         for (cpu = 0; cpu < M10V_MAX_CPU; cpu++)
65                 writel(KERNEL_UNBOOT_FLAG, m10v_smp_base + cpu * 4);
66 }
67
68 static void m10v_cpu_die(unsigned int l_cpu)
69 {
70         gic_cpu_if_down(0);
71         v7_exit_coherency_flush(louis);
72         wfi();
73 }
74
75 static int m10v_cpu_kill(unsigned int l_cpu)
76 {
77         unsigned int mpidr, cpu;
78
79         mpidr = cpu_logical_map(l_cpu);
80         cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
81
82         writel(KERNEL_UNBOOT_FLAG, m10v_smp_base + cpu * 4);
83
84         return 1;
85 }
86
87 static struct smp_operations m10v_smp_ops __initdata = {
88         .smp_prepare_cpus       = m10v_smp_init,
89         .smp_boot_secondary     = m10v_boot_secondary,
90         .cpu_die                = m10v_cpu_die,
91         .cpu_kill               = m10v_cpu_kill,
92 };
93 CPU_METHOD_OF_DECLARE(m10v_smp, "socionext,milbeaut-m10v-smp", &m10v_smp_ops);
94
95 static int m10v_pm_valid(suspend_state_t state)
96 {
97         return (state == PM_SUSPEND_STANDBY) || (state == PM_SUSPEND_MEM);
98 }
99
100 typedef void (*phys_reset_t)(unsigned long);
101 static phys_reset_t phys_reset;
102
103 static int m10v_die(unsigned long arg)
104 {
105         setup_mm_for_reboot();
106         asm("wfi");
107         /* Boot just like a secondary */
108         phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset);
109         phys_reset(virt_to_phys(cpu_resume));
110
111         return 0;
112 }
113
114 static int m10v_pm_enter(suspend_state_t state)
115 {
116         switch (state) {
117         case PM_SUSPEND_STANDBY:
118                 asm("wfi");
119                 break;
120         case PM_SUSPEND_MEM:
121                 cpu_pm_enter();
122                 cpu_suspend(0, m10v_die);
123                 cpu_pm_exit();
124                 break;
125         }
126         return 0;
127 }
128
129 static const struct platform_suspend_ops m10v_pm_ops = {
130         .valid          = m10v_pm_valid,
131         .enter          = m10v_pm_enter,
132 };
133
134 struct clk *m10v_clclk_register(struct device *cpu_dev);
135
136 static int __init m10v_pm_init(void)
137 {
138         if (of_machine_is_compatible("socionext,milbeaut-evb"))
139                 suspend_set_ops(&m10v_pm_ops);
140
141         return 0;
142 }
143 late_initcall(m10v_pm_init);