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, compat: "socionext,milbeaut-smp-sram" ); |
52 | if (!np) |
53 | return; |
54 | |
55 | m10v_smp_base = of_iomap(node: np, index: 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, addr: m10v_smp_base + cpu * 4); |
66 | } |
67 | |
68 | #ifdef CONFIG_HOTPLUG_CPU |
69 | static void m10v_cpu_die(unsigned int l_cpu) |
70 | { |
71 | gic_cpu_if_down(gic_nr: 0); |
72 | v7_exit_coherency_flush(louis); |
73 | wfi(); |
74 | } |
75 | |
76 | static int m10v_cpu_kill(unsigned int l_cpu) |
77 | { |
78 | unsigned int mpidr, cpu; |
79 | |
80 | mpidr = cpu_logical_map(l_cpu); |
81 | cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0); |
82 | |
83 | writel(KERNEL_UNBOOT_FLAG, addr: m10v_smp_base + cpu * 4); |
84 | |
85 | return 1; |
86 | } |
87 | #endif |
88 | |
89 | static struct smp_operations m10v_smp_ops __initdata = { |
90 | .smp_prepare_cpus = m10v_smp_init, |
91 | .smp_boot_secondary = m10v_boot_secondary, |
92 | #ifdef CONFIG_HOTPLUG_CPU |
93 | .cpu_die = m10v_cpu_die, |
94 | .cpu_kill = m10v_cpu_kill, |
95 | #endif |
96 | }; |
97 | CPU_METHOD_OF_DECLARE(m10v_smp, "socionext,milbeaut-m10v-smp" , &m10v_smp_ops); |
98 | |
99 | static int m10v_pm_valid(suspend_state_t state) |
100 | { |
101 | return (state == PM_SUSPEND_STANDBY) || (state == PM_SUSPEND_MEM); |
102 | } |
103 | |
104 | typedef void (*phys_reset_t)(unsigned long); |
105 | static phys_reset_t phys_reset; |
106 | |
107 | static int m10v_die(unsigned long arg) |
108 | { |
109 | setup_mm_for_reboot(); |
110 | asm("wfi" ); |
111 | /* Boot just like a secondary */ |
112 | phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(address: cpu_reset); |
113 | phys_reset(virt_to_phys(address: cpu_resume)); |
114 | |
115 | return 0; |
116 | } |
117 | |
118 | static int m10v_pm_enter(suspend_state_t state) |
119 | { |
120 | switch (state) { |
121 | case PM_SUSPEND_STANDBY: |
122 | asm("wfi" ); |
123 | break; |
124 | case PM_SUSPEND_MEM: |
125 | cpu_pm_enter(); |
126 | cpu_suspend(0, m10v_die); |
127 | cpu_pm_exit(); |
128 | break; |
129 | } |
130 | return 0; |
131 | } |
132 | |
133 | static const struct platform_suspend_ops m10v_pm_ops = { |
134 | .valid = m10v_pm_valid, |
135 | .enter = m10v_pm_enter, |
136 | }; |
137 | |
138 | struct clk *m10v_clclk_register(struct device *cpu_dev); |
139 | |
140 | static int __init m10v_pm_init(void) |
141 | { |
142 | if (of_machine_is_compatible(compat: "socionext,milbeaut-evb" )) |
143 | suspend_set_ops(ops: &m10v_pm_ops); |
144 | |
145 | return 0; |
146 | } |
147 | late_initcall(m10v_pm_init); |
148 | |