xref: /linux/arch/mips/kernel/pm-cps.c (revision 79790b6818e96c58fe2bffee1b418c16e64e7b80)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Copyright (C) 2014 Imagination Technologies
4  * Author: Paul Burton <paul.burton@mips.com>
5  */
6 
7 #include <linux/cpuhotplug.h>
8 #include <linux/init.h>
9 #include <linux/percpu.h>
10 #include <linux/slab.h>
11 #include <linux/suspend.h>
12 
13 #include <asm/asm-offsets.h>
14 #include <asm/cacheflush.h>
15 #include <asm/cacheops.h>
16 #include <asm/idle.h>
17 #include <asm/mips-cps.h>
18 #include <asm/mipsmtregs.h>
19 #include <asm/pm.h>
20 #include <asm/pm-cps.h>
21 #include <asm/regdef.h>
22 #include <asm/smp-cps.h>
23 #include <asm/uasm.h>
24 
25 /*
26  * cps_nc_entry_fn - type of a generated non-coherent state entry function
27  * @online: the count of online coupled VPEs
28  * @nc_ready_count: pointer to a non-coherent mapping of the core ready_count
29  *
30  * The code entering & exiting non-coherent states is generated at runtime
31  * using uasm, in order to ensure that the compiler cannot insert a stray
32  * memory access at an unfortunate time and to allow the generation of optimal
33  * core-specific code particularly for cache routines. If coupled_coherence
34  * is non-zero and this is the entry function for the CPS_PM_NC_WAIT state,
35  * returns the number of VPEs that were in the wait state at the point this
36  * VPE left it. Returns garbage if coupled_coherence is zero or this is not
37  * the entry function for CPS_PM_NC_WAIT.
38  */
39 typedef unsigned (*cps_nc_entry_fn)(unsigned online, u32 *nc_ready_count);
40 
41 /*
42  * The entry point of the generated non-coherent idle state entry/exit
43  * functions. Actually per-core rather than per-CPU.
44  */
45 static DEFINE_PER_CPU_READ_MOSTLY(cps_nc_entry_fn[CPS_PM_STATE_COUNT],
46 				  nc_asm_enter);
47 
48 /* Bitmap indicating which states are supported by the system */
49 static DECLARE_BITMAP(state_support, CPS_PM_STATE_COUNT);
50 
51 /*
52  * Indicates the number of coupled VPEs ready to operate in a non-coherent
53  * state. Actually per-core rather than per-CPU.
54  */
55 static DEFINE_PER_CPU_ALIGNED(u32*, ready_count);
56 
57 /* Indicates online CPUs coupled with the current CPU */
58 static DEFINE_PER_CPU_ALIGNED(cpumask_t, online_coupled);
59 
60 /*
61  * Used to synchronize entry to deep idle states. Actually per-core rather
62  * than per-CPU.
63  */
64 static DEFINE_PER_CPU_ALIGNED(atomic_t, pm_barrier);
65 
66 /* Saved CPU state across the CPS_PM_POWER_GATED state */
67 DEFINE_PER_CPU_ALIGNED(struct mips_static_suspend_state, cps_cpu_state);
68 
69 /* A somewhat arbitrary number of labels & relocs for uasm */
70 static struct uasm_label labels[32];
71 static struct uasm_reloc relocs[32];
72 
cps_pm_support_state(enum cps_pm_state state)73 bool cps_pm_support_state(enum cps_pm_state state)
74 {
75 	return test_bit(state, state_support);
76 }
77 
coupled_barrier(atomic_t * a,unsigned online)78 static void coupled_barrier(atomic_t *a, unsigned online)
79 {
80 	/*
81 	 * This function is effectively the same as
82 	 * cpuidle_coupled_parallel_barrier, which can't be used here since
83 	 * there's no cpuidle device.
84 	 */
85 
86 	if (!coupled_coherence)
87 		return;
88 
89 	smp_mb__before_atomic();
90 	atomic_inc(a);
91 
92 	while (atomic_read(a) < online)
93 		cpu_relax();
94 
95 	if (atomic_inc_return(a) == online * 2) {
96 		atomic_set(a, 0);
97 		return;
98 	}
99 
100 	while (atomic_read(a) > online)
101 		cpu_relax();
102 }
103 
cps_pm_enter_state(enum cps_pm_state state)104 int cps_pm_enter_state(enum cps_pm_state state)
105 {
106 	unsigned cpu = smp_processor_id();
107 	unsigned core = cpu_core(&current_cpu_data);
108 	unsigned online, left;
109 	cpumask_t *coupled_mask = this_cpu_ptr(&online_coupled);
110 	u32 *core_ready_count, *nc_core_ready_count;
111 	void *nc_addr;
112 	cps_nc_entry_fn entry;
113 	struct core_boot_config *core_cfg;
114 	struct vpe_boot_config *vpe_cfg;
115 
116 	/* Check that there is an entry function for this state */
117 	entry = per_cpu(nc_asm_enter, core)[state];
118 	if (!entry)
119 		return -EINVAL;
120 
121 	/* Calculate which coupled CPUs (VPEs) are online */
122 #if defined(CONFIG_MIPS_MT) || defined(CONFIG_CPU_MIPSR6)
123 	if (cpu_online(cpu)) {
124 		cpumask_and(coupled_mask, cpu_online_mask,
125 			    &cpu_sibling_map[cpu]);
126 		online = cpumask_weight(coupled_mask);
127 		cpumask_clear_cpu(cpu, coupled_mask);
128 	} else
129 #endif
130 	{
131 		cpumask_clear(coupled_mask);
132 		online = 1;
133 	}
134 
135 	/* Setup the VPE to run mips_cps_pm_restore when started again */
136 	if (IS_ENABLED(CONFIG_CPU_PM) && state == CPS_PM_POWER_GATED) {
137 		/* Power gating relies upon CPS SMP */
138 		if (!mips_cps_smp_in_use())
139 			return -EINVAL;
140 
141 		core_cfg = &mips_cps_core_bootcfg[core];
142 		vpe_cfg = &core_cfg->vpe_config[cpu_vpe_id(&current_cpu_data)];
143 		vpe_cfg->pc = (unsigned long)mips_cps_pm_restore;
144 		vpe_cfg->gp = (unsigned long)current_thread_info();
145 		vpe_cfg->sp = 0;
146 	}
147 
148 	/* Indicate that this CPU might not be coherent */
149 	cpumask_clear_cpu(cpu, &cpu_coherent_mask);
150 	smp_mb__after_atomic();
151 
152 	/* Create a non-coherent mapping of the core ready_count */
153 	core_ready_count = per_cpu(ready_count, core);
154 	nc_addr = kmap_noncoherent(virt_to_page(core_ready_count),
155 				   (unsigned long)core_ready_count);
156 	nc_addr += ((unsigned long)core_ready_count & ~PAGE_MASK);
157 	nc_core_ready_count = nc_addr;
158 
159 	/* Ensure ready_count is zero-initialised before the assembly runs */
160 	WRITE_ONCE(*nc_core_ready_count, 0);
161 	coupled_barrier(&per_cpu(pm_barrier, core), online);
162 
163 	/* Run the generated entry code */
164 	left = entry(online, nc_core_ready_count);
165 
166 	/* Remove the non-coherent mapping of ready_count */
167 	kunmap_noncoherent();
168 
169 	/* Indicate that this CPU is definitely coherent */
170 	cpumask_set_cpu(cpu, &cpu_coherent_mask);
171 
172 	/*
173 	 * If this VPE is the first to leave the non-coherent wait state then
174 	 * it needs to wake up any coupled VPEs still running their wait
175 	 * instruction so that they return to cpuidle, which can then complete
176 	 * coordination between the coupled VPEs & provide the governor with
177 	 * a chance to reflect on the length of time the VPEs were in the
178 	 * idle state.
179 	 */
180 	if (coupled_coherence && (state == CPS_PM_NC_WAIT) && (left == online))
181 		arch_send_call_function_ipi_mask(coupled_mask);
182 
183 	return 0;
184 }
185 
cps_gen_cache_routine(u32 ** pp,struct uasm_label ** pl,struct uasm_reloc ** pr,const struct cache_desc * cache,unsigned op,int lbl)186 static void cps_gen_cache_routine(u32 **pp, struct uasm_label **pl,
187 				  struct uasm_reloc **pr,
188 				  const struct cache_desc *cache,
189 				  unsigned op, int lbl)
190 {
191 	unsigned cache_size = cache->ways << cache->waybit;
192 	unsigned i;
193 	const unsigned unroll_lines = 32;
194 
195 	/* If the cache isn't present this function has it easy */
196 	if (cache->flags & MIPS_CACHE_NOT_PRESENT)
197 		return;
198 
199 	/* Load base address */
200 	UASM_i_LA(pp, GPR_T0, (long)CKSEG0);
201 
202 	/* Calculate end address */
203 	if (cache_size < 0x8000)
204 		uasm_i_addiu(pp, GPR_T1, GPR_T0, cache_size);
205 	else
206 		UASM_i_LA(pp, GPR_T1, (long)(CKSEG0 + cache_size));
207 
208 	/* Start of cache op loop */
209 	uasm_build_label(pl, *pp, lbl);
210 
211 	/* Generate the cache ops */
212 	for (i = 0; i < unroll_lines; i++) {
213 		if (cpu_has_mips_r6) {
214 			uasm_i_cache(pp, op, 0, GPR_T0);
215 			uasm_i_addiu(pp, GPR_T0, GPR_T0, cache->linesz);
216 		} else {
217 			uasm_i_cache(pp, op, i * cache->linesz, GPR_T0);
218 		}
219 	}
220 
221 	if (!cpu_has_mips_r6)
222 		/* Update the base address */
223 		uasm_i_addiu(pp, GPR_T0, GPR_T0, unroll_lines * cache->linesz);
224 
225 	/* Loop if we haven't reached the end address yet */
226 	uasm_il_bne(pp, pr, GPR_T0, GPR_T1, lbl);
227 	uasm_i_nop(pp);
228 }
229 
cps_gen_flush_fsb(u32 ** pp,struct uasm_label ** pl,struct uasm_reloc ** pr,const struct cpuinfo_mips * cpu_info,int lbl)230 static int cps_gen_flush_fsb(u32 **pp, struct uasm_label **pl,
231 			     struct uasm_reloc **pr,
232 			     const struct cpuinfo_mips *cpu_info,
233 			     int lbl)
234 {
235 	unsigned i, fsb_size = 8;
236 	unsigned num_loads = (fsb_size * 3) / 2;
237 	unsigned line_stride = 2;
238 	unsigned line_size = cpu_info->dcache.linesz;
239 	unsigned perf_counter, perf_event;
240 	unsigned revision = cpu_info->processor_id & PRID_REV_MASK;
241 
242 	/*
243 	 * Determine whether this CPU requires an FSB flush, and if so which
244 	 * performance counter/event reflect stalls due to a full FSB.
245 	 */
246 	switch (__get_cpu_type(cpu_info->cputype)) {
247 	case CPU_INTERAPTIV:
248 		perf_counter = 1;
249 		perf_event = 51;
250 		break;
251 
252 	case CPU_PROAPTIV:
253 		/* Newer proAptiv cores don't require this workaround */
254 		if (revision >= PRID_REV_ENCODE_332(1, 1, 0))
255 			return 0;
256 
257 		/* On older ones it's unavailable */
258 		return -1;
259 
260 	default:
261 		/* Assume that the CPU does not need this workaround */
262 		return 0;
263 	}
264 
265 	/*
266 	 * Ensure that the fill/store buffer (FSB) is not holding the results
267 	 * of a prefetch, since if it is then the CPC sequencer may become
268 	 * stuck in the D3 (ClrBus) state whilst entering a low power state.
269 	 */
270 
271 	/* Preserve perf counter setup */
272 	uasm_i_mfc0(pp, GPR_T2, 25, (perf_counter * 2) + 0); /* PerfCtlN */
273 	uasm_i_mfc0(pp, GPR_T3, 25, (perf_counter * 2) + 1); /* PerfCntN */
274 
275 	/* Setup perf counter to count FSB full pipeline stalls */
276 	uasm_i_addiu(pp, GPR_T0, GPR_ZERO, (perf_event << 5) | 0xf);
277 	uasm_i_mtc0(pp, GPR_T0, 25, (perf_counter * 2) + 0); /* PerfCtlN */
278 	uasm_i_ehb(pp);
279 	uasm_i_mtc0(pp, GPR_ZERO, 25, (perf_counter * 2) + 1); /* PerfCntN */
280 	uasm_i_ehb(pp);
281 
282 	/* Base address for loads */
283 	UASM_i_LA(pp, GPR_T0, (long)CKSEG0);
284 
285 	/* Start of clear loop */
286 	uasm_build_label(pl, *pp, lbl);
287 
288 	/* Perform some loads to fill the FSB */
289 	for (i = 0; i < num_loads; i++)
290 		uasm_i_lw(pp, GPR_ZERO, i * line_size * line_stride, GPR_T0);
291 
292 	/*
293 	 * Invalidate the new D-cache entries so that the cache will need
294 	 * refilling (via the FSB) if the loop is executed again.
295 	 */
296 	for (i = 0; i < num_loads; i++) {
297 		uasm_i_cache(pp, Hit_Invalidate_D,
298 			     i * line_size * line_stride, GPR_T0);
299 		uasm_i_cache(pp, Hit_Writeback_Inv_SD,
300 			     i * line_size * line_stride, GPR_T0);
301 	}
302 
303 	/* Barrier ensuring previous cache invalidates are complete */
304 	uasm_i_sync(pp, __SYNC_full);
305 	uasm_i_ehb(pp);
306 
307 	/* Check whether the pipeline stalled due to the FSB being full */
308 	uasm_i_mfc0(pp, GPR_T1, 25, (perf_counter * 2) + 1); /* PerfCntN */
309 
310 	/* Loop if it didn't */
311 	uasm_il_beqz(pp, pr, GPR_T1, lbl);
312 	uasm_i_nop(pp);
313 
314 	/* Restore perf counter 1. The count may well now be wrong... */
315 	uasm_i_mtc0(pp, GPR_T2, 25, (perf_counter * 2) + 0); /* PerfCtlN */
316 	uasm_i_ehb(pp);
317 	uasm_i_mtc0(pp, GPR_T3, 25, (perf_counter * 2) + 1); /* PerfCntN */
318 	uasm_i_ehb(pp);
319 
320 	return 0;
321 }
322 
cps_gen_set_top_bit(u32 ** pp,struct uasm_label ** pl,struct uasm_reloc ** pr,unsigned r_addr,int lbl)323 static void cps_gen_set_top_bit(u32 **pp, struct uasm_label **pl,
324 				struct uasm_reloc **pr,
325 				unsigned r_addr, int lbl)
326 {
327 	uasm_i_lui(pp, GPR_T0, uasm_rel_hi(0x80000000));
328 	uasm_build_label(pl, *pp, lbl);
329 	uasm_i_ll(pp, GPR_T1, 0, r_addr);
330 	uasm_i_or(pp, GPR_T1, GPR_T1, GPR_T0);
331 	uasm_i_sc(pp, GPR_T1, 0, r_addr);
332 	uasm_il_beqz(pp, pr, GPR_T1, lbl);
333 	uasm_i_nop(pp);
334 }
335 
cps_gen_entry_code(unsigned cpu,enum cps_pm_state state)336 static void *cps_gen_entry_code(unsigned cpu, enum cps_pm_state state)
337 {
338 	struct uasm_label *l = labels;
339 	struct uasm_reloc *r = relocs;
340 	u32 *buf, *p;
341 	const unsigned r_online = GPR_A0;
342 	const unsigned r_nc_count = GPR_A1;
343 	const unsigned r_pcohctl = GPR_T8;
344 	const unsigned max_instrs = 256;
345 	unsigned cpc_cmd;
346 	int err;
347 	enum {
348 		lbl_incready = 1,
349 		lbl_poll_cont,
350 		lbl_secondary_hang,
351 		lbl_disable_coherence,
352 		lbl_flush_fsb,
353 		lbl_invicache,
354 		lbl_flushdcache,
355 		lbl_hang,
356 		lbl_set_cont,
357 		lbl_secondary_cont,
358 		lbl_decready,
359 	};
360 
361 	/* Allocate a buffer to hold the generated code */
362 	p = buf = kcalloc(max_instrs, sizeof(u32), GFP_KERNEL);
363 	if (!buf)
364 		return NULL;
365 
366 	/* Clear labels & relocs ready for (re)use */
367 	memset(labels, 0, sizeof(labels));
368 	memset(relocs, 0, sizeof(relocs));
369 
370 	if (IS_ENABLED(CONFIG_CPU_PM) && state == CPS_PM_POWER_GATED) {
371 		/* Power gating relies upon CPS SMP */
372 		if (!mips_cps_smp_in_use())
373 			goto out_err;
374 
375 		/*
376 		 * Save CPU state. Note the non-standard calling convention
377 		 * with the return address placed in v0 to avoid clobbering
378 		 * the ra register before it is saved.
379 		 */
380 		UASM_i_LA(&p, GPR_T0, (long)mips_cps_pm_save);
381 		uasm_i_jalr(&p, GPR_V0, GPR_T0);
382 		uasm_i_nop(&p);
383 	}
384 
385 	/*
386 	 * Load addresses of required CM & CPC registers. This is done early
387 	 * because they're needed in both the enable & disable coherence steps
388 	 * but in the coupled case the enable step will only run on one VPE.
389 	 */
390 	UASM_i_LA(&p, r_pcohctl, (long)addr_gcr_cl_coherence());
391 
392 	if (coupled_coherence) {
393 		/* Increment ready_count */
394 		uasm_i_sync(&p, __SYNC_mb);
395 		uasm_build_label(&l, p, lbl_incready);
396 		uasm_i_ll(&p, GPR_T1, 0, r_nc_count);
397 		uasm_i_addiu(&p, GPR_T2, GPR_T1, 1);
398 		uasm_i_sc(&p, GPR_T2, 0, r_nc_count);
399 		uasm_il_beqz(&p, &r, GPR_T2, lbl_incready);
400 		uasm_i_addiu(&p, GPR_T1, GPR_T1, 1);
401 
402 		/* Barrier ensuring all CPUs see the updated r_nc_count value */
403 		uasm_i_sync(&p, __SYNC_mb);
404 
405 		/*
406 		 * If this is the last VPE to become ready for non-coherence
407 		 * then it should branch below.
408 		 */
409 		uasm_il_beq(&p, &r, GPR_T1, r_online, lbl_disable_coherence);
410 		uasm_i_nop(&p);
411 
412 		if (state < CPS_PM_POWER_GATED) {
413 			/*
414 			 * Otherwise this is not the last VPE to become ready
415 			 * for non-coherence. It needs to wait until coherence
416 			 * has been disabled before proceeding, which it will do
417 			 * by polling for the top bit of ready_count being set.
418 			 */
419 			uasm_i_addiu(&p, GPR_T1, GPR_ZERO, -1);
420 			uasm_build_label(&l, p, lbl_poll_cont);
421 			uasm_i_lw(&p, GPR_T0, 0, r_nc_count);
422 			uasm_il_bltz(&p, &r, GPR_T0, lbl_secondary_cont);
423 			uasm_i_ehb(&p);
424 			if (cpu_has_mipsmt)
425 				uasm_i_yield(&p, GPR_ZERO, GPR_T1);
426 			uasm_il_b(&p, &r, lbl_poll_cont);
427 			uasm_i_nop(&p);
428 		} else {
429 			/*
430 			 * The core will lose power & this VPE will not continue
431 			 * so it can simply halt here.
432 			 */
433 			if (cpu_has_mipsmt) {
434 				/* Halt the VPE via C0 tchalt register */
435 				uasm_i_addiu(&p, GPR_T0, GPR_ZERO, TCHALT_H);
436 				uasm_i_mtc0(&p, GPR_T0, 2, 4);
437 			} else if (cpu_has_vp) {
438 				/* Halt the VP via the CPC VP_STOP register */
439 				unsigned int vpe_id;
440 
441 				vpe_id = cpu_vpe_id(&cpu_data[cpu]);
442 				uasm_i_addiu(&p, GPR_T0, GPR_ZERO, 1 << vpe_id);
443 				UASM_i_LA(&p, GPR_T1, (long)addr_cpc_cl_vp_stop());
444 				uasm_i_sw(&p, GPR_T0, 0, GPR_T1);
445 			} else {
446 				BUG();
447 			}
448 			uasm_build_label(&l, p, lbl_secondary_hang);
449 			uasm_il_b(&p, &r, lbl_secondary_hang);
450 			uasm_i_nop(&p);
451 		}
452 	}
453 
454 	/*
455 	 * This is the point of no return - this VPE will now proceed to
456 	 * disable coherence. At this point we *must* be sure that no other
457 	 * VPE within the core will interfere with the L1 dcache.
458 	 */
459 	uasm_build_label(&l, p, lbl_disable_coherence);
460 
461 	/* Invalidate the L1 icache */
462 	cps_gen_cache_routine(&p, &l, &r, &cpu_data[cpu].icache,
463 			      Index_Invalidate_I, lbl_invicache);
464 
465 	/* Writeback & invalidate the L1 dcache */
466 	cps_gen_cache_routine(&p, &l, &r, &cpu_data[cpu].dcache,
467 			      Index_Writeback_Inv_D, lbl_flushdcache);
468 
469 	/* Barrier ensuring previous cache invalidates are complete */
470 	uasm_i_sync(&p, __SYNC_full);
471 	uasm_i_ehb(&p);
472 
473 	if (mips_cm_revision() < CM_REV_CM3) {
474 		/*
475 		* Disable all but self interventions. The load from COHCTL is
476 		* defined by the interAptiv & proAptiv SUMs as ensuring that the
477 		*  operation resulting from the preceding store is complete.
478 		*/
479 		uasm_i_addiu(&p, GPR_T0, GPR_ZERO, 1 << cpu_core(&cpu_data[cpu]));
480 		uasm_i_sw(&p, GPR_T0, 0, r_pcohctl);
481 		uasm_i_lw(&p, GPR_T0, 0, r_pcohctl);
482 
483 		/* Barrier to ensure write to coherence control is complete */
484 		uasm_i_sync(&p, __SYNC_full);
485 		uasm_i_ehb(&p);
486 	}
487 
488 	/* Disable coherence */
489 	uasm_i_sw(&p, GPR_ZERO, 0, r_pcohctl);
490 	uasm_i_lw(&p, GPR_T0, 0, r_pcohctl);
491 
492 	if (state >= CPS_PM_CLOCK_GATED) {
493 		err = cps_gen_flush_fsb(&p, &l, &r, &cpu_data[cpu],
494 					lbl_flush_fsb);
495 		if (err)
496 			goto out_err;
497 
498 		/* Determine the CPC command to issue */
499 		switch (state) {
500 		case CPS_PM_CLOCK_GATED:
501 			cpc_cmd = CPC_Cx_CMD_CLOCKOFF;
502 			break;
503 		case CPS_PM_POWER_GATED:
504 			cpc_cmd = CPC_Cx_CMD_PWRDOWN;
505 			break;
506 		default:
507 			BUG();
508 			goto out_err;
509 		}
510 
511 		/* Issue the CPC command */
512 		UASM_i_LA(&p, GPR_T0, (long)addr_cpc_cl_cmd());
513 		uasm_i_addiu(&p, GPR_T1, GPR_ZERO, cpc_cmd);
514 		uasm_i_sw(&p, GPR_T1, 0, GPR_T0);
515 
516 		if (state == CPS_PM_POWER_GATED) {
517 			/* If anything goes wrong just hang */
518 			uasm_build_label(&l, p, lbl_hang);
519 			uasm_il_b(&p, &r, lbl_hang);
520 			uasm_i_nop(&p);
521 
522 			/*
523 			 * There's no point generating more code, the core is
524 			 * powered down & if powered back up will run from the
525 			 * reset vector not from here.
526 			 */
527 			goto gen_done;
528 		}
529 
530 		/* Barrier to ensure write to CPC command is complete */
531 		uasm_i_sync(&p, __SYNC_full);
532 		uasm_i_ehb(&p);
533 	}
534 
535 	if (state == CPS_PM_NC_WAIT) {
536 		/*
537 		 * At this point it is safe for all VPEs to proceed with
538 		 * execution. This VPE will set the top bit of ready_count
539 		 * to indicate to the other VPEs that they may continue.
540 		 */
541 		if (coupled_coherence)
542 			cps_gen_set_top_bit(&p, &l, &r, r_nc_count,
543 					    lbl_set_cont);
544 
545 		/*
546 		 * VPEs which did not disable coherence will continue
547 		 * executing, after coherence has been disabled, from this
548 		 * point.
549 		 */
550 		uasm_build_label(&l, p, lbl_secondary_cont);
551 
552 		/* Now perform our wait */
553 		uasm_i_wait(&p, 0);
554 	}
555 
556 	/*
557 	 * Re-enable coherence. Note that for CPS_PM_NC_WAIT all coupled VPEs
558 	 * will run this. The first will actually re-enable coherence & the
559 	 * rest will just be performing a rather unusual nop.
560 	 */
561 	uasm_i_addiu(&p, GPR_T0, GPR_ZERO, mips_cm_revision() < CM_REV_CM3
562 				? CM_GCR_Cx_COHERENCE_COHDOMAINEN
563 				: CM3_GCR_Cx_COHERENCE_COHEN);
564 
565 	uasm_i_sw(&p, GPR_T0, 0, r_pcohctl);
566 	uasm_i_lw(&p, GPR_T0, 0, r_pcohctl);
567 
568 	/* Barrier to ensure write to coherence control is complete */
569 	uasm_i_sync(&p, __SYNC_full);
570 	uasm_i_ehb(&p);
571 
572 	if (coupled_coherence && (state == CPS_PM_NC_WAIT)) {
573 		/* Decrement ready_count */
574 		uasm_build_label(&l, p, lbl_decready);
575 		uasm_i_sync(&p, __SYNC_mb);
576 		uasm_i_ll(&p, GPR_T1, 0, r_nc_count);
577 		uasm_i_addiu(&p, GPR_T2, GPR_T1, -1);
578 		uasm_i_sc(&p, GPR_T2, 0, r_nc_count);
579 		uasm_il_beqz(&p, &r, GPR_T2, lbl_decready);
580 		uasm_i_andi(&p, GPR_V0, GPR_T1, (1 << fls(smp_num_siblings)) - 1);
581 
582 		/* Barrier ensuring all CPUs see the updated r_nc_count value */
583 		uasm_i_sync(&p, __SYNC_mb);
584 	}
585 
586 	if (coupled_coherence && (state == CPS_PM_CLOCK_GATED)) {
587 		/*
588 		 * At this point it is safe for all VPEs to proceed with
589 		 * execution. This VPE will set the top bit of ready_count
590 		 * to indicate to the other VPEs that they may continue.
591 		 */
592 		cps_gen_set_top_bit(&p, &l, &r, r_nc_count, lbl_set_cont);
593 
594 		/*
595 		 * This core will be reliant upon another core sending a
596 		 * power-up command to the CPC in order to resume operation.
597 		 * Thus an arbitrary VPE can't trigger the core leaving the
598 		 * idle state and the one that disables coherence might as well
599 		 * be the one to re-enable it. The rest will continue from here
600 		 * after that has been done.
601 		 */
602 		uasm_build_label(&l, p, lbl_secondary_cont);
603 
604 		/* Barrier ensuring all CPUs see the updated r_nc_count value */
605 		uasm_i_sync(&p, __SYNC_mb);
606 	}
607 
608 	/* The core is coherent, time to return to C code */
609 	uasm_i_jr(&p, GPR_RA);
610 	uasm_i_nop(&p);
611 
612 gen_done:
613 	/* Ensure the code didn't exceed the resources allocated for it */
614 	BUG_ON((p - buf) > max_instrs);
615 	BUG_ON((l - labels) > ARRAY_SIZE(labels));
616 	BUG_ON((r - relocs) > ARRAY_SIZE(relocs));
617 
618 	/* Patch branch offsets */
619 	uasm_resolve_relocs(relocs, labels);
620 
621 	/* Flush the icache */
622 	local_flush_icache_range((unsigned long)buf, (unsigned long)p);
623 
624 	return buf;
625 out_err:
626 	kfree(buf);
627 	return NULL;
628 }
629 
cps_pm_online_cpu(unsigned int cpu)630 static int cps_pm_online_cpu(unsigned int cpu)
631 {
632 	enum cps_pm_state state;
633 	unsigned core = cpu_core(&cpu_data[cpu]);
634 	void *entry_fn, *core_rc;
635 
636 	for (state = CPS_PM_NC_WAIT; state < CPS_PM_STATE_COUNT; state++) {
637 		if (per_cpu(nc_asm_enter, core)[state])
638 			continue;
639 		if (!test_bit(state, state_support))
640 			continue;
641 
642 		entry_fn = cps_gen_entry_code(cpu, state);
643 		if (!entry_fn) {
644 			pr_err("Failed to generate core %u state %u entry\n",
645 			       core, state);
646 			clear_bit(state, state_support);
647 		}
648 
649 		per_cpu(nc_asm_enter, core)[state] = entry_fn;
650 	}
651 
652 	if (!per_cpu(ready_count, core)) {
653 		core_rc = kmalloc(sizeof(u32), GFP_KERNEL);
654 		if (!core_rc) {
655 			pr_err("Failed allocate core %u ready_count\n", core);
656 			return -ENOMEM;
657 		}
658 		per_cpu(ready_count, core) = core_rc;
659 	}
660 
661 	return 0;
662 }
663 
cps_pm_power_notifier(struct notifier_block * this,unsigned long event,void * ptr)664 static int cps_pm_power_notifier(struct notifier_block *this,
665 				 unsigned long event, void *ptr)
666 {
667 	unsigned int stat;
668 
669 	switch (event) {
670 	case PM_SUSPEND_PREPARE:
671 		stat = read_cpc_cl_stat_conf();
672 		/*
673 		 * If we're attempting to suspend the system and power down all
674 		 * of the cores, the JTAG detect bit indicates that the CPC will
675 		 * instead put the cores into clock-off state. In this state
676 		 * a connected debugger can cause the CPU to attempt
677 		 * interactions with the powered down system. At best this will
678 		 * fail. At worst, it can hang the NoC, requiring a hard reset.
679 		 * To avoid this, just block system suspend if a JTAG probe
680 		 * is detected.
681 		 */
682 		if (stat & CPC_Cx_STAT_CONF_EJTAG_PROBE) {
683 			pr_warn("JTAG probe is connected - abort suspend\n");
684 			return NOTIFY_BAD;
685 		}
686 		return NOTIFY_DONE;
687 	default:
688 		return NOTIFY_DONE;
689 	}
690 }
691 
cps_pm_init(void)692 static int __init cps_pm_init(void)
693 {
694 	/* A CM is required for all non-coherent states */
695 	if (!mips_cm_present()) {
696 		pr_warn("pm-cps: no CM, non-coherent states unavailable\n");
697 		return 0;
698 	}
699 
700 	/*
701 	 * If interrupts were enabled whilst running a wait instruction on a
702 	 * non-coherent core then the VPE may end up processing interrupts
703 	 * whilst non-coherent. That would be bad.
704 	 */
705 	if (cpu_wait == r4k_wait_irqoff)
706 		set_bit(CPS_PM_NC_WAIT, state_support);
707 	else
708 		pr_warn("pm-cps: non-coherent wait unavailable\n");
709 
710 	/* Detect whether a CPC is present */
711 	if (mips_cpc_present()) {
712 		/* Detect whether clock gating is implemented */
713 		if (read_cpc_cl_stat_conf() & CPC_Cx_STAT_CONF_CLKGAT_IMPL)
714 			set_bit(CPS_PM_CLOCK_GATED, state_support);
715 		else
716 			pr_warn("pm-cps: CPC does not support clock gating\n");
717 
718 		/* Power gating is available with CPS SMP & any CPC */
719 		if (mips_cps_smp_in_use())
720 			set_bit(CPS_PM_POWER_GATED, state_support);
721 		else
722 			pr_warn("pm-cps: CPS SMP not in use, power gating unavailable\n");
723 	} else {
724 		pr_warn("pm-cps: no CPC, clock & power gating unavailable\n");
725 	}
726 
727 	pm_notifier(cps_pm_power_notifier, 0);
728 
729 	return cpuhp_setup_state(CPUHP_AP_ONLINE_DYN, "mips/cps_pm:online",
730 				 cps_pm_online_cpu, NULL);
731 }
732 arch_initcall(cps_pm_init);
733