xref: /openbmc/linux/arch/mips/kernel/cps-vec.S (revision d7a3d85e) 
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 <asm/addrspace.h>
12#include <asm/asm.h>
13#include <asm/asm-offsets.h>
14#include <asm/asmmacro.h>
15#include <asm/cacheops.h>
16#include <asm/eva.h>
17#include <asm/mipsregs.h>
18#include <asm/mipsmtregs.h>
19#include <asm/pm.h>
20
21#define GCR_CL_COHERENCE_OFS	0x2008
22#define GCR_CL_ID_OFS		0x2028
23
24.extern mips_cm_base
25
26.set noreorder
27
28	/*
29	 * Set dest to non-zero if the core supports the MT ASE, else zero. If
30	 * MT is not supported then branch to nomt.
31	 */
32	.macro	has_mt	dest, nomt
33	mfc0	\dest, CP0_CONFIG
34	bgez	\dest, \nomt
35	 mfc0	\dest, CP0_CONFIG, 1
36	bgez	\dest, \nomt
37	 mfc0	\dest, CP0_CONFIG, 2
38	bgez	\dest, \nomt
39	 mfc0	\dest, CP0_CONFIG, 3
40	andi	\dest, \dest, MIPS_CONF3_MT
41	beqz	\dest, \nomt
42	.endm
43
44.section .text.cps-vec
45.balign 0x1000
46
47LEAF(mips_cps_core_entry)
48	/*
49	 * These first 12 bytes will be patched by cps_smp_setup to load the
50	 * base address of the CM GCRs into register v1 and the CCA to use into
51	 * register s0.
52	 */
53	.quad	0
54	.word	0
55
56	/* Check whether we're here due to an NMI */
57	mfc0	k0, CP0_STATUS
58	and	k0, k0, ST0_NMI
59	beqz	k0, not_nmi
60	 nop
61
62	/* This is an NMI */
63	la	k0, nmi_handler
64	jr	k0
65	 nop
66
67not_nmi:
68	/* Setup Cause */
69	li	t0, CAUSEF_IV
70	mtc0	t0, CP0_CAUSE
71
72	/* Setup Status */
73	li	t0, ST0_CU1 | ST0_CU0
74	mtc0	t0, CP0_STATUS
75
76	/*
77	 * Clear the bits used to index the caches. Note that the architecture
78	 * dictates that writing to any of TagLo or TagHi selects 0 or 2 should
79	 * be valid for all MIPS32 CPUs, even those for which said writes are
80	 * unnecessary.
81	 */
82	mtc0	zero, CP0_TAGLO, 0
83	mtc0	zero, CP0_TAGHI, 0
84	mtc0	zero, CP0_TAGLO, 2
85	mtc0	zero, CP0_TAGHI, 2
86	ehb
87
88	/* Primary cache configuration is indicated by Config1 */
89	mfc0	v0, CP0_CONFIG, 1
90
91	/* Detect I-cache line size */
92	_EXT	t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
93	beqz	t0, icache_done
94	 li	t1, 2
95	sllv	t0, t1, t0
96
97	/* Detect I-cache size */
98	_EXT	t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
99	xori	t2, t1, 0x7
100	beqz	t2, 1f
101	 li	t3, 32
102	addiu	t1, t1, 1
103	sllv	t1, t3, t1
1041:	/* At this point t1 == I-cache sets per way */
105	_EXT	t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
106	addiu	t2, t2, 1
107	mul	t1, t1, t0
108	mul	t1, t1, t2
109
110	li	a0, KSEG0
111	add	a1, a0, t1
1121:	cache	Index_Store_Tag_I, 0(a0)
113	add	a0, a0, t0
114	bne	a0, a1, 1b
115	 nop
116icache_done:
117
118	/* Detect D-cache line size */
119	_EXT	t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
120	beqz	t0, dcache_done
121	 li	t1, 2
122	sllv	t0, t1, t0
123
124	/* Detect D-cache size */
125	_EXT	t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
126	xori	t2, t1, 0x7
127	beqz	t2, 1f
128	 li	t3, 32
129	addiu	t1, t1, 1
130	sllv	t1, t3, t1
1311:	/* At this point t1 == D-cache sets per way */
132	_EXT	t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
133	addiu	t2, t2, 1
134	mul	t1, t1, t0
135	mul	t1, t1, t2
136
137	li	a0, KSEG0
138	addu	a1, a0, t1
139	subu	a1, a1, t0
1401:	cache	Index_Store_Tag_D, 0(a0)
141	bne	a0, a1, 1b
142	 add	a0, a0, t0
143dcache_done:
144
145	/* Set Kseg0 CCA to that in s0 */
146	mfc0	t0, CP0_CONFIG
147	ori	t0, 0x7
148	xori	t0, 0x7
149	or	t0, t0, s0
150	mtc0	t0, CP0_CONFIG
151	ehb
152
153	/* Enter the coherent domain */
154	li	t0, 0xff
155	sw	t0, GCR_CL_COHERENCE_OFS(v1)
156	ehb
157
158	/* Jump to kseg0 */
159	la	t0, 1f
160	jr	t0
161	 nop
162
163	/*
164	 * We're up, cached & coherent. Perform any further required core-level
165	 * initialisation.
166	 */
1671:	jal	mips_cps_core_init
168	 nop
169
170	/* Do any EVA initialization if necessary */
171	eva_init
172
173	/*
174	 * Boot any other VPEs within this core that should be online, and
175	 * deactivate this VPE if it should be offline.
176	 */
177	jal	mips_cps_boot_vpes
178	 nop
179
180	/* Off we go! */
181	lw	t1, VPEBOOTCFG_PC(v0)
182	lw	gp, VPEBOOTCFG_GP(v0)
183	lw	sp, VPEBOOTCFG_SP(v0)
184	jr	t1
185	 nop
186	END(mips_cps_core_entry)
187
188.org 0x200
189LEAF(excep_tlbfill)
190	b	.
191	 nop
192	END(excep_tlbfill)
193
194.org 0x280
195LEAF(excep_xtlbfill)
196	b	.
197	 nop
198	END(excep_xtlbfill)
199
200.org 0x300
201LEAF(excep_cache)
202	b	.
203	 nop
204	END(excep_cache)
205
206.org 0x380
207LEAF(excep_genex)
208	b	.
209	 nop
210	END(excep_genex)
211
212.org 0x400
213LEAF(excep_intex)
214	b	.
215	 nop
216	END(excep_intex)
217
218.org 0x480
219LEAF(excep_ejtag)
220	la	k0, ejtag_debug_handler
221	jr	k0
222	 nop
223	END(excep_ejtag)
224
225LEAF(mips_cps_core_init)
226#ifdef CONFIG_MIPS_MT
227	/* Check that the core implements the MT ASE */
228	has_mt	t0, 3f
229	 nop
230
231	.set	push
232	.set	mips32r2
233	.set	mt
234
235	/* Only allow 1 TC per VPE to execute... */
236	dmt
237
238	/* ...and for the moment only 1 VPE */
239	dvpe
240	la	t1, 1f
241	jr.hb	t1
242	 nop
243
244	/* Enter VPE configuration state */
2451:	mfc0	t0, CP0_MVPCONTROL
246	ori	t0, t0, MVPCONTROL_VPC
247	mtc0	t0, CP0_MVPCONTROL
248
249	/* Retrieve the number of VPEs within the core */
250	mfc0	t0, CP0_MVPCONF0
251	srl	t0, t0, MVPCONF0_PVPE_SHIFT
252	andi	t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
253	addiu	t7, t0, 1
254
255	/* If there's only 1, we're done */
256	beqz	t0, 2f
257	 nop
258
259	/* Loop through each VPE within this core */
260	li	t5, 1
261
2621:	/* Operate on the appropriate TC */
263	mtc0	t5, CP0_VPECONTROL
264	ehb
265
266	/* Bind TC to VPE (1:1 TC:VPE mapping) */
267	mttc0	t5, CP0_TCBIND
268
269	/* Set exclusive TC, non-active, master */
270	li	t0, VPECONF0_MVP
271	sll	t1, t5, VPECONF0_XTC_SHIFT
272	or	t0, t0, t1
273	mttc0	t0, CP0_VPECONF0
274
275	/* Set TC non-active, non-allocatable */
276	mttc0	zero, CP0_TCSTATUS
277
278	/* Set TC halted */
279	li	t0, TCHALT_H
280	mttc0	t0, CP0_TCHALT
281
282	/* Next VPE */
283	addiu	t5, t5, 1
284	slt	t0, t5, t7
285	bnez	t0, 1b
286	 nop
287
288	/* Leave VPE configuration state */
2892:	mfc0	t0, CP0_MVPCONTROL
290	xori	t0, t0, MVPCONTROL_VPC
291	mtc0	t0, CP0_MVPCONTROL
292
2933:	.set	pop
294#endif
295	jr	ra
296	 nop
297	END(mips_cps_core_init)
298
299LEAF(mips_cps_boot_vpes)
300	/* Retrieve CM base address */
301	la	t0, mips_cm_base
302	lw	t0, 0(t0)
303
304	/* Calculate a pointer to this cores struct core_boot_config */
305	lw	t0, GCR_CL_ID_OFS(t0)
306	li	t1, COREBOOTCFG_SIZE
307	mul	t0, t0, t1
308	la	t1, mips_cps_core_bootcfg
309	lw	t1, 0(t1)
310	addu	t0, t0, t1
311
312	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
313	has_mt	t6, 1f
314	 li	t9, 0
315
316	/* Find the number of VPEs present in the core */
317	mfc0	t1, CP0_MVPCONF0
318	srl	t1, t1, MVPCONF0_PVPE_SHIFT
319	andi	t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
320	addiu	t1, t1, 1
321
322	/* Calculate a mask for the VPE ID from EBase.CPUNum */
323	clz	t1, t1
324	li	t2, 31
325	subu	t1, t2, t1
326	li	t2, 1
327	sll	t1, t2, t1
328	addiu	t1, t1, -1
329
330	/* Retrieve the VPE ID from EBase.CPUNum */
331	mfc0	t9, $15, 1
332	and	t9, t9, t1
333
3341:	/* Calculate a pointer to this VPEs struct vpe_boot_config */
335	li	t1, VPEBOOTCFG_SIZE
336	mul	v0, t9, t1
337	lw	t7, COREBOOTCFG_VPECONFIG(t0)
338	addu	v0, v0, t7
339
340#ifdef CONFIG_MIPS_MT
341
342	/* If the core doesn't support MT then return */
343	bnez	t6, 1f
344	 nop
345	jr	ra
346	 nop
347
348	.set	push
349	.set	mips32r2
350	.set	mt
351
3521:	/* Enter VPE configuration state */
353	dvpe
354	la	t1, 1f
355	jr.hb	t1
356	 nop
3571:	mfc0	t1, CP0_MVPCONTROL
358	ori	t1, t1, MVPCONTROL_VPC
359	mtc0	t1, CP0_MVPCONTROL
360	ehb
361
362	/* Loop through each VPE */
363	lw	t6, COREBOOTCFG_VPEMASK(t0)
364	move	t8, t6
365	li	t5, 0
366
367	/* Check whether the VPE should be running. If not, skip it */
3681:	andi	t0, t6, 1
369	beqz	t0, 2f
370	 nop
371
372	/* Operate on the appropriate TC */
373	mfc0	t0, CP0_VPECONTROL
374	ori	t0, t0, VPECONTROL_TARGTC
375	xori	t0, t0, VPECONTROL_TARGTC
376	or	t0, t0, t5
377	mtc0	t0, CP0_VPECONTROL
378	ehb
379
380	/* Skip the VPE if its TC is not halted */
381	mftc0	t0, CP0_TCHALT
382	beqz	t0, 2f
383	 nop
384
385	/* Calculate a pointer to the VPEs struct vpe_boot_config */
386	li	t0, VPEBOOTCFG_SIZE
387	mul	t0, t0, t5
388	addu	t0, t0, t7
389
390	/* Set the TC restart PC */
391	lw	t1, VPEBOOTCFG_PC(t0)
392	mttc0	t1, CP0_TCRESTART
393
394	/* Set the TC stack pointer */
395	lw	t1, VPEBOOTCFG_SP(t0)
396	mttgpr	t1, sp
397
398	/* Set the TC global pointer */
399	lw	t1, VPEBOOTCFG_GP(t0)
400	mttgpr	t1, gp
401
402	/* Copy config from this VPE */
403	mfc0	t0, CP0_CONFIG
404	mttc0	t0, CP0_CONFIG
405
406	/* Ensure no software interrupts are pending */
407	mttc0	zero, CP0_CAUSE
408	mttc0	zero, CP0_STATUS
409
410	/* Set TC active, not interrupt exempt */
411	mftc0	t0, CP0_TCSTATUS
412	li	t1, ~TCSTATUS_IXMT
413	and	t0, t0, t1
414	ori	t0, t0, TCSTATUS_A
415	mttc0	t0, CP0_TCSTATUS
416
417	/* Clear the TC halt bit */
418	mttc0	zero, CP0_TCHALT
419
420	/* Set VPE active */
421	mftc0	t0, CP0_VPECONF0
422	ori	t0, t0, VPECONF0_VPA
423	mttc0	t0, CP0_VPECONF0
424
425	/* Next VPE */
4262:	srl	t6, t6, 1
427	addiu	t5, t5, 1
428	bnez	t6, 1b
429	 nop
430
431	/* Leave VPE configuration state */
432	mfc0	t1, CP0_MVPCONTROL
433	xori	t1, t1, MVPCONTROL_VPC
434	mtc0	t1, CP0_MVPCONTROL
435	ehb
436	evpe
437
438	/* Check whether this VPE is meant to be running */
439	li	t0, 1
440	sll	t0, t0, t9
441	and	t0, t0, t8
442	bnez	t0, 2f
443	 nop
444
445	/* This VPE should be offline, halt the TC */
446	li	t0, TCHALT_H
447	mtc0	t0, CP0_TCHALT
448	la	t0, 1f
4491:	jr.hb	t0
450	 nop
451
4522:	.set	pop
453
454#endif /* CONFIG_MIPS_MT */
455
456	/* Return */
457	jr	ra
458	 nop
459	END(mips_cps_boot_vpes)
460
461#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)
462
463	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
464	.macro	psstate	dest
465	.set	push
466	.set	noat
467	lw	$1, TI_CPU(gp)
468	sll	$1, $1, LONGLOG
469	la	\dest, __per_cpu_offset
470	addu	$1, $1, \dest
471	lw	$1, 0($1)
472	la	\dest, cps_cpu_state
473	addu	\dest, \dest, $1
474	.set	pop
475	.endm
476
477LEAF(mips_cps_pm_save)
478	/* Save CPU state */
479	SUSPEND_SAVE_REGS
480	psstate	t1
481	SUSPEND_SAVE_STATIC
482	jr	v0
483	 nop
484	END(mips_cps_pm_save)
485
486LEAF(mips_cps_pm_restore)
487	/* Restore CPU state */
488	psstate	t1
489	RESUME_RESTORE_STATIC
490	RESUME_RESTORE_REGS_RETURN
491	END(mips_cps_pm_restore)
492
493#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */
494