Commit | Line | Data |
---|---|---|
0ee958e1 PB |
1 | /* |
2 | * Copyright (C) 2013 Imagination Technologies | |
3 | * Author: Paul Burton <paul.burton@imgtec.com> | |
4 | * | |
5 | * This program is free software; you can redistribute it and/or modify it | |
6 | * under the terms of the GNU General Public License as published by the | |
7 | * Free Software Foundation; either version 2 of the License, or (at your | |
8 | * option) any later version. | |
9 | */ | |
10 | ||
11 | #include <linux/io.h> | |
12 | #include <linux/sched.h> | |
13 | #include <linux/slab.h> | |
14 | #include <linux/smp.h> | |
15 | #include <linux/types.h> | |
16 | ||
17 | #include <asm/cacheflush.h> | |
18 | #include <asm/gic.h> | |
19 | #include <asm/mips-cm.h> | |
20 | #include <asm/mips-cpc.h> | |
21 | #include <asm/mips_mt.h> | |
22 | #include <asm/mipsregs.h> | |
23 | #include <asm/smp-cps.h> | |
24 | #include <asm/time.h> | |
25 | #include <asm/uasm.h> | |
26 | ||
27 | static DECLARE_BITMAP(core_power, NR_CPUS); | |
28 | ||
245a7868 | 29 | struct core_boot_config *mips_cps_core_bootcfg; |
0ee958e1 | 30 | |
245a7868 | 31 | static unsigned core_vpe_count(unsigned core) |
0ee958e1 | 32 | { |
245a7868 | 33 | unsigned cfg; |
0ee958e1 | 34 | |
245a7868 PB |
35 | if (!config_enabled(CONFIG_MIPS_MT_SMP) || !cpu_has_mipsmt) |
36 | return 1; | |
0ee958e1 | 37 | |
245a7868 PB |
38 | write_gcr_cl_other(core << CM_GCR_Cx_OTHER_CORENUM_SHF); |
39 | cfg = read_gcr_co_config() & CM_GCR_Cx_CONFIG_PVPE_MSK; | |
40 | return (cfg >> CM_GCR_Cx_CONFIG_PVPE_SHF) + 1; | |
0ee958e1 PB |
41 | } |
42 | ||
43 | static void __init cps_smp_setup(void) | |
44 | { | |
45 | unsigned int ncores, nvpes, core_vpes; | |
46 | int c, v; | |
245a7868 | 47 | u32 *entry_code; |
0ee958e1 PB |
48 | |
49 | /* Detect & record VPE topology */ | |
50 | ncores = mips_cm_numcores(); | |
51 | pr_info("VPE topology "); | |
52 | for (c = nvpes = 0; c < ncores; c++) { | |
245a7868 | 53 | core_vpes = core_vpe_count(c); |
0ee958e1 PB |
54 | pr_cont("%c%u", c ? ',' : '{', core_vpes); |
55 | ||
245a7868 PB |
56 | /* Use the number of VPEs in core 0 for smp_num_siblings */ |
57 | if (!c) | |
58 | smp_num_siblings = core_vpes; | |
59 | ||
0ee958e1 PB |
60 | for (v = 0; v < min_t(int, core_vpes, NR_CPUS - nvpes); v++) { |
61 | cpu_data[nvpes + v].core = c; | |
62 | #ifdef CONFIG_MIPS_MT_SMP | |
63 | cpu_data[nvpes + v].vpe_id = v; | |
64 | #endif | |
65 | } | |
66 | ||
67 | nvpes += core_vpes; | |
68 | } | |
69 | pr_cont("} total %u\n", nvpes); | |
70 | ||
71 | /* Indicate present CPUs (CPU being synonymous with VPE) */ | |
72 | for (v = 0; v < min_t(unsigned, nvpes, NR_CPUS); v++) { | |
73 | set_cpu_possible(v, true); | |
74 | set_cpu_present(v, true); | |
75 | __cpu_number_map[v] = v; | |
76 | __cpu_logical_map[v] = v; | |
77 | } | |
78 | ||
79 | /* Core 0 is powered up (we're running on it) */ | |
80 | bitmap_set(core_power, 0, 1); | |
81 | ||
0ee958e1 | 82 | /* Initialise core 0 */ |
245a7868 | 83 | mips_cps_core_init(); |
0ee958e1 PB |
84 | |
85 | /* Patch the start of mips_cps_core_entry to provide the CM base */ | |
86 | entry_code = (u32 *)&mips_cps_core_entry; | |
87 | UASM_i_LA(&entry_code, 3, (long)mips_cm_base); | |
88 | ||
89 | /* Make core 0 coherent with everything */ | |
90 | write_gcr_cl_coherence(0xff); | |
91 | } | |
92 | ||
93 | static void __init cps_prepare_cpus(unsigned int max_cpus) | |
94 | { | |
245a7868 PB |
95 | unsigned ncores, core_vpes, c; |
96 | ||
0ee958e1 | 97 | mips_mt_set_cpuoptions(); |
245a7868 PB |
98 | |
99 | /* Allocate core boot configuration structs */ | |
100 | ncores = mips_cm_numcores(); | |
101 | mips_cps_core_bootcfg = kcalloc(ncores, sizeof(*mips_cps_core_bootcfg), | |
102 | GFP_KERNEL); | |
103 | if (!mips_cps_core_bootcfg) { | |
104 | pr_err("Failed to allocate boot config for %u cores\n", ncores); | |
105 | goto err_out; | |
106 | } | |
107 | ||
108 | /* Allocate VPE boot configuration structs */ | |
109 | for (c = 0; c < ncores; c++) { | |
110 | core_vpes = core_vpe_count(c); | |
111 | mips_cps_core_bootcfg[c].vpe_config = kcalloc(core_vpes, | |
112 | sizeof(*mips_cps_core_bootcfg[c].vpe_config), | |
113 | GFP_KERNEL); | |
114 | if (!mips_cps_core_bootcfg[c].vpe_config) { | |
115 | pr_err("Failed to allocate %u VPE boot configs\n", | |
116 | core_vpes); | |
117 | goto err_out; | |
118 | } | |
119 | } | |
120 | ||
121 | /* Mark this CPU as booted */ | |
122 | atomic_set(&mips_cps_core_bootcfg[current_cpu_data.core].vpe_mask, | |
123 | 1 << cpu_vpe_id(¤t_cpu_data)); | |
124 | ||
125 | return; | |
126 | err_out: | |
127 | /* Clean up allocations */ | |
128 | if (mips_cps_core_bootcfg) { | |
129 | for (c = 0; c < ncores; c++) | |
130 | kfree(mips_cps_core_bootcfg[c].vpe_config); | |
131 | kfree(mips_cps_core_bootcfg); | |
132 | mips_cps_core_bootcfg = NULL; | |
133 | } | |
134 | ||
135 | /* Effectively disable SMP by declaring CPUs not present */ | |
136 | for_each_possible_cpu(c) { | |
137 | if (c == 0) | |
138 | continue; | |
139 | set_cpu_present(c, false); | |
140 | } | |
0ee958e1 PB |
141 | } |
142 | ||
245a7868 | 143 | static void boot_core(unsigned core) |
0ee958e1 PB |
144 | { |
145 | u32 access; | |
146 | ||
147 | /* Select the appropriate core */ | |
245a7868 | 148 | write_gcr_cl_other(core << CM_GCR_Cx_OTHER_CORENUM_SHF); |
0ee958e1 PB |
149 | |
150 | /* Set its reset vector */ | |
151 | write_gcr_co_reset_base(CKSEG1ADDR((unsigned long)mips_cps_core_entry)); | |
152 | ||
153 | /* Ensure its coherency is disabled */ | |
154 | write_gcr_co_coherence(0); | |
155 | ||
156 | /* Ensure the core can access the GCRs */ | |
157 | access = read_gcr_access(); | |
245a7868 | 158 | access |= 1 << (CM_GCR_ACCESS_ACCESSEN_SHF + core); |
0ee958e1 PB |
159 | write_gcr_access(access); |
160 | ||
0ee958e1 PB |
161 | if (mips_cpc_present()) { |
162 | /* Select the appropriate core */ | |
245a7868 | 163 | write_cpc_cl_other(core << CPC_Cx_OTHER_CORENUM_SHF); |
0ee958e1 PB |
164 | |
165 | /* Reset the core */ | |
166 | write_cpc_co_cmd(CPC_Cx_CMD_RESET); | |
167 | } else { | |
168 | /* Take the core out of reset */ | |
169 | write_gcr_co_reset_release(0); | |
170 | } | |
171 | ||
172 | /* The core is now powered up */ | |
245a7868 | 173 | bitmap_set(core_power, core, 1); |
0ee958e1 PB |
174 | } |
175 | ||
245a7868 | 176 | static void remote_vpe_boot(void *dummy) |
0ee958e1 | 177 | { |
245a7868 | 178 | mips_cps_boot_vpes(); |
0ee958e1 PB |
179 | } |
180 | ||
181 | static void cps_boot_secondary(int cpu, struct task_struct *idle) | |
182 | { | |
245a7868 PB |
183 | unsigned core = cpu_data[cpu].core; |
184 | unsigned vpe_id = cpu_vpe_id(&cpu_data[cpu]); | |
185 | struct core_boot_config *core_cfg = &mips_cps_core_bootcfg[core]; | |
186 | struct vpe_boot_config *vpe_cfg = &core_cfg->vpe_config[vpe_id]; | |
0ee958e1 PB |
187 | unsigned int remote; |
188 | int err; | |
189 | ||
245a7868 PB |
190 | vpe_cfg->pc = (unsigned long)&smp_bootstrap; |
191 | vpe_cfg->sp = __KSTK_TOS(idle); | |
192 | vpe_cfg->gp = (unsigned long)task_thread_info(idle); | |
0ee958e1 | 193 | |
245a7868 PB |
194 | atomic_or(1 << cpu_vpe_id(&cpu_data[cpu]), &core_cfg->vpe_mask); |
195 | ||
196 | if (!test_bit(core, core_power)) { | |
0ee958e1 | 197 | /* Boot a VPE on a powered down core */ |
245a7868 | 198 | boot_core(core); |
0ee958e1 PB |
199 | return; |
200 | } | |
201 | ||
245a7868 | 202 | if (core != current_cpu_data.core) { |
0ee958e1 PB |
203 | /* Boot a VPE on another powered up core */ |
204 | for (remote = 0; remote < NR_CPUS; remote++) { | |
245a7868 | 205 | if (cpu_data[remote].core != core) |
0ee958e1 PB |
206 | continue; |
207 | if (cpu_online(remote)) | |
208 | break; | |
209 | } | |
210 | BUG_ON(remote >= NR_CPUS); | |
211 | ||
245a7868 PB |
212 | err = smp_call_function_single(remote, remote_vpe_boot, |
213 | NULL, 1); | |
0ee958e1 PB |
214 | if (err) |
215 | panic("Failed to call remote CPU\n"); | |
216 | return; | |
217 | } | |
218 | ||
219 | BUG_ON(!cpu_has_mipsmt); | |
220 | ||
221 | /* Boot a VPE on this core */ | |
245a7868 | 222 | mips_cps_boot_vpes(); |
0ee958e1 PB |
223 | } |
224 | ||
225 | static void cps_init_secondary(void) | |
226 | { | |
227 | /* Disable MT - we only want to run 1 TC per VPE */ | |
228 | if (cpu_has_mipsmt) | |
229 | dmt(); | |
230 | ||
0ee958e1 PB |
231 | change_c0_status(ST0_IM, STATUSF_IP3 | STATUSF_IP4 | |
232 | STATUSF_IP6 | STATUSF_IP7); | |
233 | } | |
234 | ||
235 | static void cps_smp_finish(void) | |
236 | { | |
237 | write_c0_compare(read_c0_count() + (8 * mips_hpt_frequency / HZ)); | |
238 | ||
239 | #ifdef CONFIG_MIPS_MT_FPAFF | |
240 | /* If we have an FPU, enroll ourselves in the FPU-full mask */ | |
241 | if (cpu_has_fpu) | |
242 | cpu_set(smp_processor_id(), mt_fpu_cpumask); | |
243 | #endif /* CONFIG_MIPS_MT_FPAFF */ | |
244 | ||
245 | local_irq_enable(); | |
246 | } | |
247 | ||
248 | static void cps_cpus_done(void) | |
249 | { | |
250 | } | |
251 | ||
252 | static struct plat_smp_ops cps_smp_ops = { | |
253 | .smp_setup = cps_smp_setup, | |
254 | .prepare_cpus = cps_prepare_cpus, | |
255 | .boot_secondary = cps_boot_secondary, | |
256 | .init_secondary = cps_init_secondary, | |
257 | .smp_finish = cps_smp_finish, | |
258 | .send_ipi_single = gic_send_ipi_single, | |
259 | .send_ipi_mask = gic_send_ipi_mask, | |
260 | .cpus_done = cps_cpus_done, | |
261 | }; | |
262 | ||
68c1232f PB |
263 | bool mips_cps_smp_in_use(void) |
264 | { | |
265 | extern struct plat_smp_ops *mp_ops; | |
266 | return mp_ops == &cps_smp_ops; | |
267 | } | |
268 | ||
0ee958e1 PB |
269 | int register_cps_smp_ops(void) |
270 | { | |
271 | if (!mips_cm_present()) { | |
272 | pr_warn("MIPS CPS SMP unable to proceed without a CM\n"); | |
273 | return -ENODEV; | |
274 | } | |
275 | ||
276 | /* check we have a GIC - we need one for IPIs */ | |
277 | if (!(read_gcr_gic_status() & CM_GCR_GIC_STATUS_EX_MSK)) { | |
278 | pr_warn("MIPS CPS SMP unable to proceed without a GIC\n"); | |
279 | return -ENODEV; | |
280 | } | |
281 | ||
282 | register_smp_ops(&cps_smp_ops); | |
283 | return 0; | |
284 | } |