xref: /openbmc/linux/arch/mips/loongson64/smp.c (revision b7ec62d7)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Copyright (C) 2010, 2011, 2012, Lemote, Inc.
4  * Author: Chen Huacai, chenhc@lemote.com
5  */
6 
7 #include <irq.h>
8 #include <linux/init.h>
9 #include <linux/cpu.h>
10 #include <linux/sched.h>
11 #include <linux/sched/hotplug.h>
12 #include <linux/sched/task_stack.h>
13 #include <linux/smp.h>
14 #include <linux/cpufreq.h>
15 #include <linux/kexec.h>
16 #include <asm/processor.h>
17 #include <asm/time.h>
18 #include <asm/tlbflush.h>
19 #include <asm/cacheflush.h>
20 #include <loongson.h>
21 #include <loongson_regs.h>
22 #include <workarounds.h>
23 
24 #include "smp.h"
25 
26 DEFINE_PER_CPU(int, cpu_state);
27 
28 #define LS_IPI_IRQ (MIPS_CPU_IRQ_BASE + 6)
29 
30 static void *ipi_set0_regs[16];
31 static void *ipi_clear0_regs[16];
32 static void *ipi_status0_regs[16];
33 static void *ipi_en0_regs[16];
34 static void *ipi_mailbox_buf[16];
35 static uint32_t core0_c0count[NR_CPUS];
36 
37 /* read a 32bit value from ipi register */
38 #define loongson3_ipi_read32(addr) readl(addr)
39 /* read a 64bit value from ipi register */
40 #define loongson3_ipi_read64(addr) readq(addr)
41 /* write a 32bit value to ipi register */
42 #define loongson3_ipi_write32(action, addr)	\
43 	do {					\
44 		writel(action, addr);		\
45 		__wbflush();			\
46 	} while (0)
47 /* write a 64bit value to ipi register */
48 #define loongson3_ipi_write64(action, addr)	\
49 	do {					\
50 		writeq(action, addr);		\
51 		__wbflush();			\
52 	} while (0)
53 
54 static u32 (*ipi_read_clear)(int cpu);
55 static void (*ipi_write_action)(int cpu, u32 action);
56 static void (*ipi_write_enable)(int cpu);
57 static void (*ipi_clear_buf)(int cpu);
58 static void (*ipi_write_buf)(int cpu, struct task_struct *idle);
59 
60 /* send mail via Mail_Send register for 3A4000+ CPU */
61 static void csr_mail_send(uint64_t data, int cpu, int mailbox)
62 {
63 	uint64_t val;
64 
65 	/* send high 32 bits */
66 	val = CSR_MAIL_SEND_BLOCK;
67 	val |= (CSR_MAIL_SEND_BOX_HIGH(mailbox) << CSR_MAIL_SEND_BOX_SHIFT);
68 	val |= (cpu << CSR_MAIL_SEND_CPU_SHIFT);
69 	val |= (data & CSR_MAIL_SEND_H32_MASK);
70 	csr_writeq(val, LOONGSON_CSR_MAIL_SEND);
71 
72 	/* send low 32 bits */
73 	val = CSR_MAIL_SEND_BLOCK;
74 	val |= (CSR_MAIL_SEND_BOX_LOW(mailbox) << CSR_MAIL_SEND_BOX_SHIFT);
75 	val |= (cpu << CSR_MAIL_SEND_CPU_SHIFT);
76 	val |= (data << CSR_MAIL_SEND_BUF_SHIFT);
77 	csr_writeq(val, LOONGSON_CSR_MAIL_SEND);
78 };
79 
80 static u32 csr_ipi_read_clear(int cpu)
81 {
82 	u32 action;
83 
84 	/* Load the ipi register to figure out what we're supposed to do */
85 	action = csr_readl(LOONGSON_CSR_IPI_STATUS);
86 	/* Clear the ipi register to clear the interrupt */
87 	csr_writel(action, LOONGSON_CSR_IPI_CLEAR);
88 
89 	return action;
90 }
91 
92 static void csr_ipi_write_action(int cpu, u32 action)
93 {
94 	unsigned int irq = 0;
95 
96 	while ((irq = ffs(action))) {
97 		uint32_t val = CSR_IPI_SEND_BLOCK;
98 		val |= (irq - 1);
99 		val |= (cpu << CSR_IPI_SEND_CPU_SHIFT);
100 		csr_writel(val, LOONGSON_CSR_IPI_SEND);
101 		action &= ~BIT(irq - 1);
102 	}
103 }
104 
105 static void csr_ipi_write_enable(int cpu)
106 {
107 	csr_writel(0xffffffff, LOONGSON_CSR_IPI_EN);
108 }
109 
110 static void csr_ipi_clear_buf(int cpu)
111 {
112 	csr_writeq(0, LOONGSON_CSR_MAIL_BUF0);
113 }
114 
115 static void csr_ipi_write_buf(int cpu, struct task_struct *idle)
116 {
117 	unsigned long startargs[4];
118 
119 	/* startargs[] are initial PC, SP and GP for secondary CPU */
120 	startargs[0] = (unsigned long)&smp_bootstrap;
121 	startargs[1] = (unsigned long)__KSTK_TOS(idle);
122 	startargs[2] = (unsigned long)task_thread_info(idle);
123 	startargs[3] = 0;
124 
125 	pr_debug("CPU#%d, func_pc=%lx, sp=%lx, gp=%lx\n",
126 		cpu, startargs[0], startargs[1], startargs[2]);
127 
128 	csr_mail_send(startargs[3], cpu_logical_map(cpu), 3);
129 	csr_mail_send(startargs[2], cpu_logical_map(cpu), 2);
130 	csr_mail_send(startargs[1], cpu_logical_map(cpu), 1);
131 	csr_mail_send(startargs[0], cpu_logical_map(cpu), 0);
132 }
133 
134 static u32 legacy_ipi_read_clear(int cpu)
135 {
136 	u32 action;
137 
138 	/* Load the ipi register to figure out what we're supposed to do */
139 	action = loongson3_ipi_read32(ipi_status0_regs[cpu_logical_map(cpu)]);
140 	/* Clear the ipi register to clear the interrupt */
141 	loongson3_ipi_write32(action, ipi_clear0_regs[cpu_logical_map(cpu)]);
142 
143 	return action;
144 }
145 
146 static void legacy_ipi_write_action(int cpu, u32 action)
147 {
148 	loongson3_ipi_write32((u32)action, ipi_set0_regs[cpu]);
149 }
150 
151 static void legacy_ipi_write_enable(int cpu)
152 {
153 	loongson3_ipi_write32(0xffffffff, ipi_en0_regs[cpu_logical_map(cpu)]);
154 }
155 
156 static void legacy_ipi_clear_buf(int cpu)
157 {
158 	loongson3_ipi_write64(0, ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x0);
159 }
160 
161 static void legacy_ipi_write_buf(int cpu, struct task_struct *idle)
162 {
163 	unsigned long startargs[4];
164 
165 	/* startargs[] are initial PC, SP and GP for secondary CPU */
166 	startargs[0] = (unsigned long)&smp_bootstrap;
167 	startargs[1] = (unsigned long)__KSTK_TOS(idle);
168 	startargs[2] = (unsigned long)task_thread_info(idle);
169 	startargs[3] = 0;
170 
171 	pr_debug("CPU#%d, func_pc=%lx, sp=%lx, gp=%lx\n",
172 			cpu, startargs[0], startargs[1], startargs[2]);
173 
174 	loongson3_ipi_write64(startargs[3],
175 			ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x18);
176 	loongson3_ipi_write64(startargs[2],
177 			ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x10);
178 	loongson3_ipi_write64(startargs[1],
179 			ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x8);
180 	loongson3_ipi_write64(startargs[0],
181 			ipi_mailbox_buf[cpu_logical_map(cpu)] + 0x0);
182 }
183 
184 static void csr_ipi_probe(void)
185 {
186 	if (cpu_has_csr() && csr_readl(LOONGSON_CSR_FEATURES) & LOONGSON_CSRF_IPI) {
187 		ipi_read_clear = csr_ipi_read_clear;
188 		ipi_write_action = csr_ipi_write_action;
189 		ipi_write_enable = csr_ipi_write_enable;
190 		ipi_clear_buf = csr_ipi_clear_buf;
191 		ipi_write_buf = csr_ipi_write_buf;
192 	} else {
193 		ipi_read_clear = legacy_ipi_read_clear;
194 		ipi_write_action = legacy_ipi_write_action;
195 		ipi_write_enable = legacy_ipi_write_enable;
196 		ipi_clear_buf = legacy_ipi_clear_buf;
197 		ipi_write_buf = legacy_ipi_write_buf;
198 	}
199 }
200 
201 static void ipi_set0_regs_init(void)
202 {
203 	ipi_set0_regs[0] = (void *)
204 		(SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + SET0);
205 	ipi_set0_regs[1] = (void *)
206 		(SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + SET0);
207 	ipi_set0_regs[2] = (void *)
208 		(SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + SET0);
209 	ipi_set0_regs[3] = (void *)
210 		(SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + SET0);
211 	ipi_set0_regs[4] = (void *)
212 		(SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + SET0);
213 	ipi_set0_regs[5] = (void *)
214 		(SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + SET0);
215 	ipi_set0_regs[6] = (void *)
216 		(SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + SET0);
217 	ipi_set0_regs[7] = (void *)
218 		(SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + SET0);
219 	ipi_set0_regs[8] = (void *)
220 		(SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + SET0);
221 	ipi_set0_regs[9] = (void *)
222 		(SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + SET0);
223 	ipi_set0_regs[10] = (void *)
224 		(SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + SET0);
225 	ipi_set0_regs[11] = (void *)
226 		(SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + SET0);
227 	ipi_set0_regs[12] = (void *)
228 		(SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + SET0);
229 	ipi_set0_regs[13] = (void *)
230 		(SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + SET0);
231 	ipi_set0_regs[14] = (void *)
232 		(SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + SET0);
233 	ipi_set0_regs[15] = (void *)
234 		(SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + SET0);
235 }
236 
237 static void ipi_clear0_regs_init(void)
238 {
239 	ipi_clear0_regs[0] = (void *)
240 		(SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + CLEAR0);
241 	ipi_clear0_regs[1] = (void *)
242 		(SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + CLEAR0);
243 	ipi_clear0_regs[2] = (void *)
244 		(SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + CLEAR0);
245 	ipi_clear0_regs[3] = (void *)
246 		(SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + CLEAR0);
247 	ipi_clear0_regs[4] = (void *)
248 		(SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + CLEAR0);
249 	ipi_clear0_regs[5] = (void *)
250 		(SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + CLEAR0);
251 	ipi_clear0_regs[6] = (void *)
252 		(SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + CLEAR0);
253 	ipi_clear0_regs[7] = (void *)
254 		(SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + CLEAR0);
255 	ipi_clear0_regs[8] = (void *)
256 		(SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + CLEAR0);
257 	ipi_clear0_regs[9] = (void *)
258 		(SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + CLEAR0);
259 	ipi_clear0_regs[10] = (void *)
260 		(SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + CLEAR0);
261 	ipi_clear0_regs[11] = (void *)
262 		(SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + CLEAR0);
263 	ipi_clear0_regs[12] = (void *)
264 		(SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + CLEAR0);
265 	ipi_clear0_regs[13] = (void *)
266 		(SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + CLEAR0);
267 	ipi_clear0_regs[14] = (void *)
268 		(SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + CLEAR0);
269 	ipi_clear0_regs[15] = (void *)
270 		(SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + CLEAR0);
271 }
272 
273 static void ipi_status0_regs_init(void)
274 {
275 	ipi_status0_regs[0] = (void *)
276 		(SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + STATUS0);
277 	ipi_status0_regs[1] = (void *)
278 		(SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + STATUS0);
279 	ipi_status0_regs[2] = (void *)
280 		(SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + STATUS0);
281 	ipi_status0_regs[3] = (void *)
282 		(SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + STATUS0);
283 	ipi_status0_regs[4] = (void *)
284 		(SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + STATUS0);
285 	ipi_status0_regs[5] = (void *)
286 		(SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + STATUS0);
287 	ipi_status0_regs[6] = (void *)
288 		(SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + STATUS0);
289 	ipi_status0_regs[7] = (void *)
290 		(SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + STATUS0);
291 	ipi_status0_regs[8] = (void *)
292 		(SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + STATUS0);
293 	ipi_status0_regs[9] = (void *)
294 		(SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + STATUS0);
295 	ipi_status0_regs[10] = (void *)
296 		(SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + STATUS0);
297 	ipi_status0_regs[11] = (void *)
298 		(SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + STATUS0);
299 	ipi_status0_regs[12] = (void *)
300 		(SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + STATUS0);
301 	ipi_status0_regs[13] = (void *)
302 		(SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + STATUS0);
303 	ipi_status0_regs[14] = (void *)
304 		(SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + STATUS0);
305 	ipi_status0_regs[15] = (void *)
306 		(SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + STATUS0);
307 }
308 
309 static void ipi_en0_regs_init(void)
310 {
311 	ipi_en0_regs[0] = (void *)
312 		(SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + EN0);
313 	ipi_en0_regs[1] = (void *)
314 		(SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + EN0);
315 	ipi_en0_regs[2] = (void *)
316 		(SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + EN0);
317 	ipi_en0_regs[3] = (void *)
318 		(SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + EN0);
319 	ipi_en0_regs[4] = (void *)
320 		(SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + EN0);
321 	ipi_en0_regs[5] = (void *)
322 		(SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + EN0);
323 	ipi_en0_regs[6] = (void *)
324 		(SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + EN0);
325 	ipi_en0_regs[7] = (void *)
326 		(SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + EN0);
327 	ipi_en0_regs[8] = (void *)
328 		(SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + EN0);
329 	ipi_en0_regs[9] = (void *)
330 		(SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + EN0);
331 	ipi_en0_regs[10] = (void *)
332 		(SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + EN0);
333 	ipi_en0_regs[11] = (void *)
334 		(SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + EN0);
335 	ipi_en0_regs[12] = (void *)
336 		(SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + EN0);
337 	ipi_en0_regs[13] = (void *)
338 		(SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + EN0);
339 	ipi_en0_regs[14] = (void *)
340 		(SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + EN0);
341 	ipi_en0_regs[15] = (void *)
342 		(SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + EN0);
343 }
344 
345 static void ipi_mailbox_buf_init(void)
346 {
347 	ipi_mailbox_buf[0] = (void *)
348 		(SMP_CORE_GROUP0_BASE + SMP_CORE0_OFFSET + BUF);
349 	ipi_mailbox_buf[1] = (void *)
350 		(SMP_CORE_GROUP0_BASE + SMP_CORE1_OFFSET + BUF);
351 	ipi_mailbox_buf[2] = (void *)
352 		(SMP_CORE_GROUP0_BASE + SMP_CORE2_OFFSET + BUF);
353 	ipi_mailbox_buf[3] = (void *)
354 		(SMP_CORE_GROUP0_BASE + SMP_CORE3_OFFSET + BUF);
355 	ipi_mailbox_buf[4] = (void *)
356 		(SMP_CORE_GROUP1_BASE + SMP_CORE0_OFFSET + BUF);
357 	ipi_mailbox_buf[5] = (void *)
358 		(SMP_CORE_GROUP1_BASE + SMP_CORE1_OFFSET + BUF);
359 	ipi_mailbox_buf[6] = (void *)
360 		(SMP_CORE_GROUP1_BASE + SMP_CORE2_OFFSET + BUF);
361 	ipi_mailbox_buf[7] = (void *)
362 		(SMP_CORE_GROUP1_BASE + SMP_CORE3_OFFSET + BUF);
363 	ipi_mailbox_buf[8] = (void *)
364 		(SMP_CORE_GROUP2_BASE + SMP_CORE0_OFFSET + BUF);
365 	ipi_mailbox_buf[9] = (void *)
366 		(SMP_CORE_GROUP2_BASE + SMP_CORE1_OFFSET + BUF);
367 	ipi_mailbox_buf[10] = (void *)
368 		(SMP_CORE_GROUP2_BASE + SMP_CORE2_OFFSET + BUF);
369 	ipi_mailbox_buf[11] = (void *)
370 		(SMP_CORE_GROUP2_BASE + SMP_CORE3_OFFSET + BUF);
371 	ipi_mailbox_buf[12] = (void *)
372 		(SMP_CORE_GROUP3_BASE + SMP_CORE0_OFFSET + BUF);
373 	ipi_mailbox_buf[13] = (void *)
374 		(SMP_CORE_GROUP3_BASE + SMP_CORE1_OFFSET + BUF);
375 	ipi_mailbox_buf[14] = (void *)
376 		(SMP_CORE_GROUP3_BASE + SMP_CORE2_OFFSET + BUF);
377 	ipi_mailbox_buf[15] = (void *)
378 		(SMP_CORE_GROUP3_BASE + SMP_CORE3_OFFSET + BUF);
379 }
380 
381 /*
382  * Simple enough, just poke the appropriate ipi register
383  */
384 static void loongson3_send_ipi_single(int cpu, unsigned int action)
385 {
386 	ipi_write_action(cpu_logical_map(cpu), (u32)action);
387 }
388 
389 static void
390 loongson3_send_ipi_mask(const struct cpumask *mask, unsigned int action)
391 {
392 	unsigned int i;
393 
394 	for_each_cpu(i, mask)
395 		ipi_write_action(cpu_logical_map(i), (u32)action);
396 }
397 
398 
399 static irqreturn_t loongson3_ipi_interrupt(int irq, void *dev_id)
400 {
401 	int i, cpu = smp_processor_id();
402 	unsigned int action, c0count;
403 
404 	action = ipi_read_clear(cpu);
405 
406 	if (action & SMP_RESCHEDULE_YOURSELF)
407 		scheduler_ipi();
408 
409 	if (action & SMP_CALL_FUNCTION) {
410 		irq_enter();
411 		generic_smp_call_function_interrupt();
412 		irq_exit();
413 	}
414 
415 	if (action & SMP_ASK_C0COUNT) {
416 		BUG_ON(cpu != 0);
417 		c0count = read_c0_count();
418 		c0count = c0count ? c0count : 1;
419 		for (i = 1; i < nr_cpu_ids; i++)
420 			core0_c0count[i] = c0count;
421 		__wbflush(); /* Let others see the result ASAP */
422 	}
423 
424 	return IRQ_HANDLED;
425 }
426 
427 #define MAX_LOOPS 800
428 /*
429  * SMP init and finish on secondary CPUs
430  */
431 static void loongson3_init_secondary(void)
432 {
433 	int i;
434 	uint32_t initcount;
435 	unsigned int cpu = smp_processor_id();
436 	unsigned int imask = STATUSF_IP7 | STATUSF_IP6 |
437 			     STATUSF_IP3 | STATUSF_IP2;
438 
439 	/* Set interrupt mask, but don't enable */
440 	change_c0_status(ST0_IM, imask);
441 	ipi_write_enable(cpu);
442 
443 	per_cpu(cpu_state, cpu) = CPU_ONLINE;
444 	cpu_set_core(&cpu_data[cpu],
445 		     cpu_logical_map(cpu) % loongson_sysconf.cores_per_package);
446 	cpu_data[cpu].package =
447 		cpu_logical_map(cpu) / loongson_sysconf.cores_per_package;
448 
449 	i = 0;
450 	core0_c0count[cpu] = 0;
451 	loongson3_send_ipi_single(0, SMP_ASK_C0COUNT);
452 	while (!core0_c0count[cpu]) {
453 		i++;
454 		cpu_relax();
455 	}
456 
457 	if (i > MAX_LOOPS)
458 		i = MAX_LOOPS;
459 	if (cpu_data[cpu].package)
460 		initcount = core0_c0count[cpu] + i;
461 	else /* Local access is faster for loops */
462 		initcount = core0_c0count[cpu] + i/2;
463 
464 	write_c0_count(initcount);
465 }
466 
467 static void loongson3_smp_finish(void)
468 {
469 	int cpu = smp_processor_id();
470 
471 	write_c0_compare(read_c0_count() + mips_hpt_frequency/HZ);
472 	local_irq_enable();
473 	ipi_clear_buf(cpu);
474 
475 	pr_info("CPU#%d finished, CP0_ST=%x\n",
476 			smp_processor_id(), read_c0_status());
477 }
478 
479 static void __init loongson3_smp_setup(void)
480 {
481 	int i = 0, num = 0; /* i: physical id, num: logical id */
482 
483 	init_cpu_possible(cpu_none_mask);
484 
485 	/* For unified kernel, NR_CPUS is the maximum possible value,
486 	 * loongson_sysconf.nr_cpus is the really present value
487 	 */
488 	while (i < loongson_sysconf.nr_cpus) {
489 		if (loongson_sysconf.reserved_cpus_mask & (1<<i)) {
490 			/* Reserved physical CPU cores */
491 			__cpu_number_map[i] = -1;
492 		} else {
493 			__cpu_number_map[i] = num;
494 			__cpu_logical_map[num] = i;
495 			set_cpu_possible(num, true);
496 			/* Loongson processors are always grouped by 4 */
497 			cpu_set_cluster(&cpu_data[num], i / 4);
498 			num++;
499 		}
500 		i++;
501 	}
502 	pr_info("Detected %i available CPU(s)\n", num);
503 
504 	while (num < loongson_sysconf.nr_cpus) {
505 		__cpu_logical_map[num] = -1;
506 		num++;
507 	}
508 
509 	csr_ipi_probe();
510 	ipi_set0_regs_init();
511 	ipi_clear0_regs_init();
512 	ipi_status0_regs_init();
513 	ipi_en0_regs_init();
514 	ipi_mailbox_buf_init();
515 	ipi_write_enable(0);
516 
517 	cpu_set_core(&cpu_data[0],
518 		     cpu_logical_map(0) % loongson_sysconf.cores_per_package);
519 	cpu_data[0].package = cpu_logical_map(0) / loongson_sysconf.cores_per_package;
520 }
521 
522 static void __init loongson3_prepare_cpus(unsigned int max_cpus)
523 {
524 	if (request_irq(LS_IPI_IRQ, loongson3_ipi_interrupt,
525 			IRQF_PERCPU | IRQF_NO_SUSPEND, "SMP_IPI", NULL))
526 		pr_err("Failed to request IPI IRQ\n");
527 	init_cpu_present(cpu_possible_mask);
528 	per_cpu(cpu_state, smp_processor_id()) = CPU_ONLINE;
529 }
530 
531 /*
532  * Setup the PC, SP, and GP of a secondary processor and start it runing!
533  */
534 static int loongson3_boot_secondary(int cpu, struct task_struct *idle)
535 {
536 	pr_info("Booting CPU#%d...\n", cpu);
537 
538 	ipi_write_buf(cpu, idle);
539 
540 	return 0;
541 }
542 
543 #ifdef CONFIG_HOTPLUG_CPU
544 
545 static int loongson3_cpu_disable(void)
546 {
547 	unsigned long flags;
548 	unsigned int cpu = smp_processor_id();
549 
550 	set_cpu_online(cpu, false);
551 	calculate_cpu_foreign_map();
552 	local_irq_save(flags);
553 	clear_c0_status(ST0_IM);
554 	local_irq_restore(flags);
555 	local_flush_tlb_all();
556 
557 	return 0;
558 }
559 
560 
561 static void loongson3_cpu_die(unsigned int cpu)
562 {
563 	while (per_cpu(cpu_state, cpu) != CPU_DEAD)
564 		cpu_relax();
565 
566 	mb();
567 }
568 
569 /* To shutdown a core in Loongson 3, the target core should go to CKSEG1 and
570  * flush all L1 entries at first. Then, another core (usually Core 0) can
571  * safely disable the clock of the target core. loongson3_play_dead() is
572  * called via CKSEG1 (uncached and unmmaped)
573  */
574 static void loongson3_type1_play_dead(int *state_addr)
575 {
576 	register int val;
577 	register long cpuid, core, node, count;
578 	register void *addr, *base, *initfunc;
579 
580 	__asm__ __volatile__(
581 		"   .set push                     \n"
582 		"   .set noreorder                \n"
583 		"   li %[addr], 0x80000000        \n" /* KSEG0 */
584 		"1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
585 		"   cache 0, 1(%[addr])           \n"
586 		"   cache 0, 2(%[addr])           \n"
587 		"   cache 0, 3(%[addr])           \n"
588 		"   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
589 		"   cache 1, 1(%[addr])           \n"
590 		"   cache 1, 2(%[addr])           \n"
591 		"   cache 1, 3(%[addr])           \n"
592 		"   addiu %[sets], %[sets], -1    \n"
593 		"   bnez  %[sets], 1b             \n"
594 		"   addiu %[addr], %[addr], 0x20  \n"
595 		"   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
596 		"   sw    %[val], (%[state_addr]) \n"
597 		"   sync                          \n"
598 		"   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
599 		"   .set pop                      \n"
600 		: [addr] "=&r" (addr), [val] "=&r" (val)
601 		: [state_addr] "r" (state_addr),
602 		  [sets] "r" (cpu_data[smp_processor_id()].dcache.sets));
603 
604 	__asm__ __volatile__(
605 		"   .set push                         \n"
606 		"   .set noreorder                    \n"
607 		"   .set mips64                       \n"
608 		"   mfc0  %[cpuid], $15, 1            \n"
609 		"   andi  %[cpuid], 0x3ff             \n"
610 		"   dli   %[base], 0x900000003ff01000 \n"
611 		"   andi  %[core], %[cpuid], 0x3      \n"
612 		"   sll   %[core], 8                  \n" /* get core id */
613 		"   or    %[base], %[base], %[core]   \n"
614 		"   andi  %[node], %[cpuid], 0xc      \n"
615 		"   dsll  %[node], 42                 \n" /* get node id */
616 		"   or    %[base], %[base], %[node]   \n"
617 		"1: li    %[count], 0x100             \n" /* wait for init loop */
618 		"2: bnez  %[count], 2b                \n" /* limit mailbox access */
619 		"   addiu %[count], -1                \n"
620 		"   ld    %[initfunc], 0x20(%[base])  \n" /* get PC via mailbox */
621 		"   beqz  %[initfunc], 1b             \n"
622 		"   nop                               \n"
623 		"   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
624 		"   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
625 		"   ld    $a1, 0x38(%[base])          \n"
626 		"   jr    %[initfunc]                 \n" /* jump to initial PC */
627 		"   nop                               \n"
628 		"   .set pop                          \n"
629 		: [core] "=&r" (core), [node] "=&r" (node),
630 		  [base] "=&r" (base), [cpuid] "=&r" (cpuid),
631 		  [count] "=&r" (count), [initfunc] "=&r" (initfunc)
632 		: /* No Input */
633 		: "a1");
634 }
635 
636 static void loongson3_type2_play_dead(int *state_addr)
637 {
638 	register int val;
639 	register long cpuid, core, node, count;
640 	register void *addr, *base, *initfunc;
641 
642 	__asm__ __volatile__(
643 		"   .set push                     \n"
644 		"   .set noreorder                \n"
645 		"   li %[addr], 0x80000000        \n" /* KSEG0 */
646 		"1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
647 		"   cache 0, 1(%[addr])           \n"
648 		"   cache 0, 2(%[addr])           \n"
649 		"   cache 0, 3(%[addr])           \n"
650 		"   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
651 		"   cache 1, 1(%[addr])           \n"
652 		"   cache 1, 2(%[addr])           \n"
653 		"   cache 1, 3(%[addr])           \n"
654 		"   addiu %[sets], %[sets], -1    \n"
655 		"   bnez  %[sets], 1b             \n"
656 		"   addiu %[addr], %[addr], 0x20  \n"
657 		"   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
658 		"   sw    %[val], (%[state_addr]) \n"
659 		"   sync                          \n"
660 		"   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
661 		"   .set pop                      \n"
662 		: [addr] "=&r" (addr), [val] "=&r" (val)
663 		: [state_addr] "r" (state_addr),
664 		  [sets] "r" (cpu_data[smp_processor_id()].dcache.sets));
665 
666 	__asm__ __volatile__(
667 		"   .set push                         \n"
668 		"   .set noreorder                    \n"
669 		"   .set mips64                       \n"
670 		"   mfc0  %[cpuid], $15, 1            \n"
671 		"   andi  %[cpuid], 0x3ff             \n"
672 		"   dli   %[base], 0x900000003ff01000 \n"
673 		"   andi  %[core], %[cpuid], 0x3      \n"
674 		"   sll   %[core], 8                  \n" /* get core id */
675 		"   or    %[base], %[base], %[core]   \n"
676 		"   andi  %[node], %[cpuid], 0xc      \n"
677 		"   dsll  %[node], 42                 \n" /* get node id */
678 		"   or    %[base], %[base], %[node]   \n"
679 		"   dsrl  %[node], 30                 \n" /* 15:14 */
680 		"   or    %[base], %[base], %[node]   \n"
681 		"1: li    %[count], 0x100             \n" /* wait for init loop */
682 		"2: bnez  %[count], 2b                \n" /* limit mailbox access */
683 		"   addiu %[count], -1                \n"
684 		"   ld    %[initfunc], 0x20(%[base])  \n" /* get PC via mailbox */
685 		"   beqz  %[initfunc], 1b             \n"
686 		"   nop                               \n"
687 		"   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
688 		"   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
689 		"   ld    $a1, 0x38(%[base])          \n"
690 		"   jr    %[initfunc]                 \n" /* jump to initial PC */
691 		"   nop                               \n"
692 		"   .set pop                          \n"
693 		: [core] "=&r" (core), [node] "=&r" (node),
694 		  [base] "=&r" (base), [cpuid] "=&r" (cpuid),
695 		  [count] "=&r" (count), [initfunc] "=&r" (initfunc)
696 		: /* No Input */
697 		: "a1");
698 }
699 
700 static void loongson3_type3_play_dead(int *state_addr)
701 {
702 	register int val;
703 	register long cpuid, core, node, count;
704 	register void *addr, *base, *initfunc;
705 
706 	__asm__ __volatile__(
707 		"   .set push                     \n"
708 		"   .set noreorder                \n"
709 		"   li %[addr], 0x80000000        \n" /* KSEG0 */
710 		"1: cache 0, 0(%[addr])           \n" /* flush L1 ICache */
711 		"   cache 0, 1(%[addr])           \n"
712 		"   cache 0, 2(%[addr])           \n"
713 		"   cache 0, 3(%[addr])           \n"
714 		"   cache 1, 0(%[addr])           \n" /* flush L1 DCache */
715 		"   cache 1, 1(%[addr])           \n"
716 		"   cache 1, 2(%[addr])           \n"
717 		"   cache 1, 3(%[addr])           \n"
718 		"   addiu %[sets], %[sets], -1    \n"
719 		"   bnez  %[sets], 1b             \n"
720 		"   addiu %[addr], %[addr], 0x40  \n"
721 		"   li %[addr], 0x80000000        \n" /* KSEG0 */
722 		"2: cache 2, 0(%[addr])           \n" /* flush L1 VCache */
723 		"   cache 2, 1(%[addr])           \n"
724 		"   cache 2, 2(%[addr])           \n"
725 		"   cache 2, 3(%[addr])           \n"
726 		"   cache 2, 4(%[addr])           \n"
727 		"   cache 2, 5(%[addr])           \n"
728 		"   cache 2, 6(%[addr])           \n"
729 		"   cache 2, 7(%[addr])           \n"
730 		"   cache 2, 8(%[addr])           \n"
731 		"   cache 2, 9(%[addr])           \n"
732 		"   cache 2, 10(%[addr])          \n"
733 		"   cache 2, 11(%[addr])          \n"
734 		"   cache 2, 12(%[addr])          \n"
735 		"   cache 2, 13(%[addr])          \n"
736 		"   cache 2, 14(%[addr])          \n"
737 		"   cache 2, 15(%[addr])          \n"
738 		"   addiu %[vsets], %[vsets], -1  \n"
739 		"   bnez  %[vsets], 2b            \n"
740 		"   addiu %[addr], %[addr], 0x40  \n"
741 		"   li    %[val], 0x7             \n" /* *state_addr = CPU_DEAD; */
742 		"   sw    %[val], (%[state_addr]) \n"
743 		"   sync                          \n"
744 		"   cache 21, (%[state_addr])     \n" /* flush entry of *state_addr */
745 		"   .set pop                      \n"
746 		: [addr] "=&r" (addr), [val] "=&r" (val)
747 		: [state_addr] "r" (state_addr),
748 		  [sets] "r" (cpu_data[smp_processor_id()].dcache.sets),
749 		  [vsets] "r" (cpu_data[smp_processor_id()].vcache.sets));
750 
751 	__asm__ __volatile__(
752 		"   .set push                         \n"
753 		"   .set noreorder                    \n"
754 		"   .set mips64                       \n"
755 		"   mfc0  %[cpuid], $15, 1            \n"
756 		"   andi  %[cpuid], 0x3ff             \n"
757 		"   dli   %[base], 0x900000003ff01000 \n"
758 		"   andi  %[core], %[cpuid], 0x3      \n"
759 		"   sll   %[core], 8                  \n" /* get core id */
760 		"   or    %[base], %[base], %[core]   \n"
761 		"   andi  %[node], %[cpuid], 0xc      \n"
762 		"   dsll  %[node], 42                 \n" /* get node id */
763 		"   or    %[base], %[base], %[node]   \n"
764 		"1: li    %[count], 0x100             \n" /* wait for init loop */
765 		"2: bnez  %[count], 2b                \n" /* limit mailbox access */
766 		"   addiu %[count], -1                \n"
767 		"   lw    %[initfunc], 0x20(%[base])  \n" /* check lower 32-bit as jump indicator */
768 		"   beqz  %[initfunc], 1b             \n"
769 		"   nop                               \n"
770 		"   ld    %[initfunc], 0x20(%[base])  \n" /* get PC (whole 64-bit) via mailbox */
771 		"   ld    $sp, 0x28(%[base])          \n" /* get SP via mailbox */
772 		"   ld    $gp, 0x30(%[base])          \n" /* get GP via mailbox */
773 		"   ld    $a1, 0x38(%[base])          \n"
774 		"   jr    %[initfunc]                 \n" /* jump to initial PC */
775 		"   nop                               \n"
776 		"   .set pop                          \n"
777 		: [core] "=&r" (core), [node] "=&r" (node),
778 		  [base] "=&r" (base), [cpuid] "=&r" (cpuid),
779 		  [count] "=&r" (count), [initfunc] "=&r" (initfunc)
780 		: /* No Input */
781 		: "a1");
782 }
783 
784 void play_dead(void)
785 {
786 	int prid_imp, prid_rev, *state_addr;
787 	unsigned int cpu = smp_processor_id();
788 	void (*play_dead_at_ckseg1)(int *);
789 
790 	idle_task_exit();
791 
792 	prid_imp = read_c0_prid() & PRID_IMP_MASK;
793 	prid_rev = read_c0_prid() & PRID_REV_MASK;
794 
795 	if (prid_imp == PRID_IMP_LOONGSON_64G) {
796 		play_dead_at_ckseg1 =
797 			(void *)CKSEG1ADDR((unsigned long)loongson3_type3_play_dead);
798 		goto out;
799 	}
800 
801 	switch (prid_rev) {
802 	case PRID_REV_LOONGSON3A_R1:
803 	default:
804 		play_dead_at_ckseg1 =
805 			(void *)CKSEG1ADDR((unsigned long)loongson3_type1_play_dead);
806 		break;
807 	case PRID_REV_LOONGSON3B_R1:
808 	case PRID_REV_LOONGSON3B_R2:
809 		play_dead_at_ckseg1 =
810 			(void *)CKSEG1ADDR((unsigned long)loongson3_type2_play_dead);
811 		break;
812 	case PRID_REV_LOONGSON3A_R2_0:
813 	case PRID_REV_LOONGSON3A_R2_1:
814 	case PRID_REV_LOONGSON3A_R3_0:
815 	case PRID_REV_LOONGSON3A_R3_1:
816 		play_dead_at_ckseg1 =
817 			(void *)CKSEG1ADDR((unsigned long)loongson3_type3_play_dead);
818 		break;
819 	}
820 
821 out:
822 	state_addr = &per_cpu(cpu_state, cpu);
823 	mb();
824 	play_dead_at_ckseg1(state_addr);
825 }
826 
827 static int loongson3_disable_clock(unsigned int cpu)
828 {
829 	uint64_t core_id = cpu_core(&cpu_data[cpu]);
830 	uint64_t package_id = cpu_data[cpu].package;
831 
832 	if ((read_c0_prid() & PRID_REV_MASK) == PRID_REV_LOONGSON3A_R1) {
833 		LOONGSON_CHIPCFG(package_id) &= ~(1 << (12 + core_id));
834 	} else {
835 		if (!(loongson_sysconf.workarounds & WORKAROUND_CPUHOTPLUG))
836 			LOONGSON_FREQCTRL(package_id) &= ~(1 << (core_id * 4 + 3));
837 	}
838 	return 0;
839 }
840 
841 static int loongson3_enable_clock(unsigned int cpu)
842 {
843 	uint64_t core_id = cpu_core(&cpu_data[cpu]);
844 	uint64_t package_id = cpu_data[cpu].package;
845 
846 	if ((read_c0_prid() & PRID_REV_MASK) == PRID_REV_LOONGSON3A_R1) {
847 		LOONGSON_CHIPCFG(package_id) |= 1 << (12 + core_id);
848 	} else {
849 		if (!(loongson_sysconf.workarounds & WORKAROUND_CPUHOTPLUG))
850 			LOONGSON_FREQCTRL(package_id) |= 1 << (core_id * 4 + 3);
851 	}
852 	return 0;
853 }
854 
855 static int register_loongson3_notifier(void)
856 {
857 	return cpuhp_setup_state_nocalls(CPUHP_MIPS_SOC_PREPARE,
858 					 "mips/loongson:prepare",
859 					 loongson3_enable_clock,
860 					 loongson3_disable_clock);
861 }
862 early_initcall(register_loongson3_notifier);
863 
864 #endif
865 
866 const struct plat_smp_ops loongson3_smp_ops = {
867 	.send_ipi_single = loongson3_send_ipi_single,
868 	.send_ipi_mask = loongson3_send_ipi_mask,
869 	.init_secondary = loongson3_init_secondary,
870 	.smp_finish = loongson3_smp_finish,
871 	.boot_secondary = loongson3_boot_secondary,
872 	.smp_setup = loongson3_smp_setup,
873 	.prepare_cpus = loongson3_prepare_cpus,
874 #ifdef CONFIG_HOTPLUG_CPU
875 	.cpu_disable = loongson3_cpu_disable,
876 	.cpu_die = loongson3_cpu_die,
877 #endif
878 #ifdef CONFIG_KEXEC
879 	.kexec_nonboot_cpu = kexec_nonboot_cpu_jump,
880 #endif
881 };
882