xref: /openbmc/linux/arch/mips/kernel/cps-vec.S (revision e1f7c9ee) 
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	addi	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	addi	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	addi	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	addi	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	mt
233
234	/* Only allow 1 TC per VPE to execute... */
235	dmt
236
237	/* ...and for the moment only 1 VPE */
238	dvpe
239	la	t1, 1f
240	jr.hb	t1
241	 nop
242
243	/* Enter VPE configuration state */
2441:	mfc0	t0, CP0_MVPCONTROL
245	ori	t0, t0, MVPCONTROL_VPC
246	mtc0	t0, CP0_MVPCONTROL
247
248	/* Retrieve the number of VPEs within the core */
249	mfc0	t0, CP0_MVPCONF0
250	srl	t0, t0, MVPCONF0_PVPE_SHIFT
251	andi	t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
252	addi	t7, t0, 1
253
254	/* If there's only 1, we're done */
255	beqz	t0, 2f
256	 nop
257
258	/* Loop through each VPE within this core */
259	li	t5, 1
260
2611:	/* Operate on the appropriate TC */
262	mtc0	t5, CP0_VPECONTROL
263	ehb
264
265	/* Bind TC to VPE (1:1 TC:VPE mapping) */
266	mttc0	t5, CP0_TCBIND
267
268	/* Set exclusive TC, non-active, master */
269	li	t0, VPECONF0_MVP
270	sll	t1, t5, VPECONF0_XTC_SHIFT
271	or	t0, t0, t1
272	mttc0	t0, CP0_VPECONF0
273
274	/* Set TC non-active, non-allocatable */
275	mttc0	zero, CP0_TCSTATUS
276
277	/* Set TC halted */
278	li	t0, TCHALT_H
279	mttc0	t0, CP0_TCHALT
280
281	/* Next VPE */
282	addi	t5, t5, 1
283	slt	t0, t5, t7
284	bnez	t0, 1b
285	 nop
286
287	/* Leave VPE configuration state */
2882:	mfc0	t0, CP0_MVPCONTROL
289	xori	t0, t0, MVPCONTROL_VPC
290	mtc0	t0, CP0_MVPCONTROL
291
2923:	.set	pop
293#endif
294	jr	ra
295	 nop
296	END(mips_cps_core_init)
297
298LEAF(mips_cps_boot_vpes)
299	/* Retrieve CM base address */
300	la	t0, mips_cm_base
301	lw	t0, 0(t0)
302
303	/* Calculate a pointer to this cores struct core_boot_config */
304	lw	t0, GCR_CL_ID_OFS(t0)
305	li	t1, COREBOOTCFG_SIZE
306	mul	t0, t0, t1
307	la	t1, mips_cps_core_bootcfg
308	lw	t1, 0(t1)
309	addu	t0, t0, t1
310
311	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
312	has_mt	t6, 1f
313	 li	t9, 0
314
315	/* Find the number of VPEs present in the core */
316	mfc0	t1, CP0_MVPCONF0
317	srl	t1, t1, MVPCONF0_PVPE_SHIFT
318	andi	t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
319	addi	t1, t1, 1
320
321	/* Calculate a mask for the VPE ID from EBase.CPUNum */
322	clz	t1, t1
323	li	t2, 31
324	subu	t1, t2, t1
325	li	t2, 1
326	sll	t1, t2, t1
327	addiu	t1, t1, -1
328
329	/* Retrieve the VPE ID from EBase.CPUNum */
330	mfc0	t9, $15, 1
331	and	t9, t9, t1
332
3331:	/* Calculate a pointer to this VPEs struct vpe_boot_config */
334	li	t1, VPEBOOTCFG_SIZE
335	mul	v0, t9, t1
336	lw	t7, COREBOOTCFG_VPECONFIG(t0)
337	addu	v0, v0, t7
338
339#ifdef CONFIG_MIPS_MT
340
341	/* If the core doesn't support MT then return */
342	bnez	t6, 1f
343	 nop
344	jr	ra
345	 nop
346
347	.set	push
348	.set	mt
349
3501:	/* Enter VPE configuration state */
351	dvpe
352	la	t1, 1f
353	jr.hb	t1
354	 nop
3551:	mfc0	t1, CP0_MVPCONTROL
356	ori	t1, t1, MVPCONTROL_VPC
357	mtc0	t1, CP0_MVPCONTROL
358	ehb
359
360	/* Loop through each VPE */
361	lw	t6, COREBOOTCFG_VPEMASK(t0)
362	move	t8, t6
363	li	t5, 0
364
365	/* Check whether the VPE should be running. If not, skip it */
3661:	andi	t0, t6, 1
367	beqz	t0, 2f
368	 nop
369
370	/* Operate on the appropriate TC */
371	mfc0	t0, CP0_VPECONTROL
372	ori	t0, t0, VPECONTROL_TARGTC
373	xori	t0, t0, VPECONTROL_TARGTC
374	or	t0, t0, t5
375	mtc0	t0, CP0_VPECONTROL
376	ehb
377
378	/* Skip the VPE if its TC is not halted */
379	mftc0	t0, CP0_TCHALT
380	beqz	t0, 2f
381	 nop
382
383	/* Calculate a pointer to the VPEs struct vpe_boot_config */
384	li	t0, VPEBOOTCFG_SIZE
385	mul	t0, t0, t5
386	addu	t0, t0, t7
387
388	/* Set the TC restart PC */
389	lw	t1, VPEBOOTCFG_PC(t0)
390	mttc0	t1, CP0_TCRESTART
391
392	/* Set the TC stack pointer */
393	lw	t1, VPEBOOTCFG_SP(t0)
394	mttgpr	t1, sp
395
396	/* Set the TC global pointer */
397	lw	t1, VPEBOOTCFG_GP(t0)
398	mttgpr	t1, gp
399
400	/* Copy config from this VPE */
401	mfc0	t0, CP0_CONFIG
402	mttc0	t0, CP0_CONFIG
403
404	/* Ensure no software interrupts are pending */
405	mttc0	zero, CP0_CAUSE
406	mttc0	zero, CP0_STATUS
407
408	/* Set TC active, not interrupt exempt */
409	mftc0	t0, CP0_TCSTATUS
410	li	t1, ~TCSTATUS_IXMT
411	and	t0, t0, t1
412	ori	t0, t0, TCSTATUS_A
413	mttc0	t0, CP0_TCSTATUS
414
415	/* Clear the TC halt bit */
416	mttc0	zero, CP0_TCHALT
417
418	/* Set VPE active */
419	mftc0	t0, CP0_VPECONF0
420	ori	t0, t0, VPECONF0_VPA
421	mttc0	t0, CP0_VPECONF0
422
423	/* Next VPE */
4242:	srl	t6, t6, 1
425	addi	t5, t5, 1
426	bnez	t6, 1b
427	 nop
428
429	/* Leave VPE configuration state */
430	mfc0	t1, CP0_MVPCONTROL
431	xori	t1, t1, MVPCONTROL_VPC
432	mtc0	t1, CP0_MVPCONTROL
433	ehb
434	evpe
435
436	/* Check whether this VPE is meant to be running */
437	li	t0, 1
438	sll	t0, t0, t9
439	and	t0, t0, t8
440	bnez	t0, 2f
441	 nop
442
443	/* This VPE should be offline, halt the TC */
444	li	t0, TCHALT_H
445	mtc0	t0, CP0_TCHALT
446	la	t0, 1f
4471:	jr.hb	t0
448	 nop
449
4502:	.set	pop
451
452#endif /* CONFIG_MIPS_MT */
453
454	/* Return */
455	jr	ra
456	 nop
457	END(mips_cps_boot_vpes)
458
459#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)
460
461	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
462	.macro	psstate	dest
463	.set	push
464	.set	noat
465	lw	$1, TI_CPU(gp)
466	sll	$1, $1, LONGLOG
467	la	\dest, __per_cpu_offset
468	addu	$1, $1, \dest
469	lw	$1, 0($1)
470	la	\dest, cps_cpu_state
471	addu	\dest, \dest, $1
472	.set	pop
473	.endm
474
475LEAF(mips_cps_pm_save)
476	/* Save CPU state */
477	SUSPEND_SAVE_REGS
478	psstate	t1
479	SUSPEND_SAVE_STATIC
480	jr	v0
481	 nop
482	END(mips_cps_pm_save)
483
484LEAF(mips_cps_pm_restore)
485	/* Restore CPU state */
486	psstate	t1
487	RESUME_RESTORE_STATIC
488	RESUME_RESTORE_REGS_RETURN
489	END(mips_cps_pm_restore)
490
491#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */
492