[mirror_ubuntu-focal-kernel.git] / arch / mips / kernel / cps-vec.S

/*
 * Copyright (C) 2013 Imagination Technologies
 * Author: Paul Burton <paul.burton@imgtec.com>
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation;  either version 2 of the  License, or (at your
 * option) any later version.
 */

#include <asm/addrspace.h>
#include <asm/asm.h>
#include <asm/asm-offsets.h>
#include <asm/asmmacro.h>
#include <asm/cacheops.h>
#include <asm/mipsregs.h>
#include <asm/mipsmtregs.h>

#define GCR_CL_COHERENCE_OFS	0x2008
#define GCR_CL_ID_OFS		0x2028

.extern mips_cm_base

.set noreorder

	/*
	 * Set dest to non-zero if the core supports the MT ASE, else zero. If
	 * MT is not supported then branch to nomt.
	 */
	.macro	has_mt	dest, nomt
	mfc0	\dest, CP0_CONFIG
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 1
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 2
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 3
	andi	\dest, \dest, MIPS_CONF3_MT
	beqz	\dest, \nomt
	.endm

.section .text.cps-vec
.balign 0x1000

LEAF(mips_cps_core_entry)
	/*
	 * These first 8 bytes will be patched by cps_smp_setup to load the
	 * base address of the CM GCRs into register v1.
	 */
	.quad	0

	/* Check whether we're here due to an NMI */
	mfc0	k0, CP0_STATUS
	and	k0, k0, ST0_NMI
	beqz	k0, not_nmi
	 nop

	/* This is an NMI */
	la	k0, nmi_handler
	jr	k0
	 nop

not_nmi:
	/* Setup Cause */
	li	t0, CAUSEF_IV
	mtc0	t0, CP0_CAUSE

	/* Setup Status */
	li	t0, ST0_CU1 | ST0_CU0
	mtc0	t0, CP0_STATUS

	/*
	 * Clear the bits used to index the caches. Note that the architecture
	 * dictates that writing to any of TagLo or TagHi selects 0 or 2 should
	 * be valid for all MIPS32 CPUs, even those for which said writes are
	 * unnecessary.
	 */
	mtc0	zero, CP0_TAGLO, 0
	mtc0	zero, CP0_TAGHI, 0
	mtc0	zero, CP0_TAGLO, 2
	mtc0	zero, CP0_TAGHI, 2
	ehb

	/* Primary cache configuration is indicated by Config1 */
	mfc0	v0, CP0_CONFIG, 1

	/* Detect I-cache line size */
	_EXT	t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
	beqz	t0, icache_done
	 li	t1, 2
	sllv	t0, t1, t0

	/* Detect I-cache size */
	_EXT	t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
	xori	t2, t1, 0x7
	beqz	t2, 1f
	 li	t3, 32
	addi	t1, t1, 1
	sllv	t1, t3, t1
1:	/* At this point t1 == I-cache sets per way */
	_EXT	t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
	addi	t2, t2, 1
	mul	t1, t1, t0
	mul	t1, t1, t2

	li	a0, KSEG0
	add	a1, a0, t1
1:	cache	Index_Store_Tag_I, 0(a0)
	add	a0, a0, t0
	bne	a0, a1, 1b
	 nop
icache_done:

	/* Detect D-cache line size */
	_EXT	t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
	beqz	t0, dcache_done
	 li	t1, 2
	sllv	t0, t1, t0

	/* Detect D-cache size */
	_EXT	t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
	xori	t2, t1, 0x7
	beqz	t2, 1f
	 li	t3, 32
	addi	t1, t1, 1
	sllv	t1, t3, t1
1:	/* At this point t1 == D-cache sets per way */
	_EXT	t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
	addi	t2, t2, 1
	mul	t1, t1, t0
	mul	t1, t1, t2

	li	a0, KSEG0
	addu	a1, a0, t1
	subu	a1, a1, t0
1:	cache	Index_Store_Tag_D, 0(a0)
	bne	a0, a1, 1b
	 add	a0, a0, t0
dcache_done:

	/* Set Kseg0 cacheable, coherent, write-back, write-allocate */
	mfc0	t0, CP0_CONFIG
	ori	t0, 0x7
	xori	t0, 0x2
	mtc0	t0, CP0_CONFIG
	ehb

	/* Enter the coherent domain */
	li	t0, 0xff
	sw	t0, GCR_CL_COHERENCE_OFS(v1)
	ehb

	/* Jump to kseg0 */
	la	t0, 1f
	jr	t0
	 nop

	/*
	 * We're up, cached & coherent. Perform any further required core-level
	 * initialisation.
	 */
1:	jal	mips_cps_core_init
	 nop

	/*
	 * Boot any other VPEs within this core that should be online, and
	 * deactivate this VPE if it should be offline.
	 */
	jal	mips_cps_boot_vpes
	 nop

	/* Off we go! */
	lw	t1, VPEBOOTCFG_PC(v0)
	lw	gp, VPEBOOTCFG_GP(v0)
	lw	sp, VPEBOOTCFG_SP(v0)
	jr	t1
	 nop
	END(mips_cps_core_entry)

.org 0x200
LEAF(excep_tlbfill)
	b	.
	 nop
	END(excep_tlbfill)

.org 0x280
LEAF(excep_xtlbfill)
	b	.
	 nop
	END(excep_xtlbfill)

.org 0x300
LEAF(excep_cache)
	b	.
	 nop
	END(excep_cache)

.org 0x380
LEAF(excep_genex)
	b	.
	 nop
	END(excep_genex)

.org 0x400
LEAF(excep_intex)
	b	.
	 nop
	END(excep_intex)

.org 0x480
LEAF(excep_ejtag)
	la	k0, ejtag_debug_handler
	jr	k0
	 nop
	END(excep_ejtag)

LEAF(mips_cps_core_init)
#ifdef CONFIG_MIPS_MT
	/* Check that the core implements the MT ASE */
	has_mt	t0, 3f
	 nop

	.set	push
	.set	mt

	/* Only allow 1 TC per VPE to execute... */
	dmt

	/* ...and for the moment only 1 VPE */
	dvpe
	la	t1, 1f
	jr.hb	t1
	 nop

	/* Enter VPE configuration state */
1:	mfc0	t0, CP0_MVPCONTROL
	ori	t0, t0, MVPCONTROL_VPC
	mtc0	t0, CP0_MVPCONTROL

	/* Retrieve the number of VPEs within the core */
	mfc0	t0, CP0_MVPCONF0
	srl	t0, t0, MVPCONF0_PVPE_SHIFT
	andi	t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
	addi	t7, t0, 1

	/* If there's only 1, we're done */
	beqz	t0, 2f
	 nop

	/* Loop through each VPE within this core */
	li	t5, 1

1:	/* Operate on the appropriate TC */
	mtc0	t5, CP0_VPECONTROL
	ehb

	/* Bind TC to VPE (1:1 TC:VPE mapping) */
	mttc0	t5, CP0_TCBIND

	/* Set exclusive TC, non-active, master */
	li	t0, VPECONF0_MVP
	sll	t1, t5, VPECONF0_XTC_SHIFT
	or	t0, t0, t1
	mttc0	t0, CP0_VPECONF0

	/* Set TC non-active, non-allocatable */
	mttc0	zero, CP0_TCSTATUS

	/* Set TC halted */
	li	t0, TCHALT_H
	mttc0	t0, CP0_TCHALT

	/* Next VPE */
	addi	t5, t5, 1
	slt	t0, t5, t7
	bnez	t0, 1b
	 nop

	/* Leave VPE configuration state */
2:	mfc0	t0, CP0_MVPCONTROL
	xori	t0, t0, MVPCONTROL_VPC
	mtc0	t0, CP0_MVPCONTROL

3:	.set	pop
#endif
	jr	ra
	 nop
	END(mips_cps_core_init)

LEAF(mips_cps_boot_vpes)
	/* Retrieve CM base address */
	la	t0, mips_cm_base
	lw	t0, 0(t0)

	/* Calculate a pointer to this cores struct core_boot_config */
	lw	t0, GCR_CL_ID_OFS(t0)
	li	t1, COREBOOTCFG_SIZE
	mul	t0, t0, t1
	la	t1, mips_cps_core_bootcfg
	lw	t1, 0(t1)
	addu	t0, t0, t1

	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
	has_mt	t6, 1f
	 li	t9, 0

	/* Find the number of VPEs present in the core */
	mfc0	t1, CP0_MVPCONF0
	srl	t1, t1, MVPCONF0_PVPE_SHIFT
	andi	t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
	addi	t1, t1, 1

	/* Calculate a mask for the VPE ID from EBase.CPUNum */
	clz	t1, t1
	li	t2, 31
	subu	t1, t2, t1
	li	t2, 1
	sll	t1, t2, t1
	addiu	t1, t1, -1

	/* Retrieve the VPE ID from EBase.CPUNum */
	mfc0	t9, $15, 1
	and	t9, t9, t1

1:	/* Calculate a pointer to this VPEs struct vpe_boot_config */
	li	t1, VPEBOOTCFG_SIZE
	mul	v0, t9, t1
	lw	t7, COREBOOTCFG_VPECONFIG(t0)
	addu	v0, v0, t7

#ifdef CONFIG_MIPS_MT

	/* If the core doesn't support MT then return */
	bnez	t6, 1f
	 nop
	jr	ra
	 nop

	.set	push
	.set	mt

1:	/* Enter VPE configuration state */
	dvpe
	la	t1, 1f
	jr.hb	t1
	 nop
1:	mfc0	t1, CP0_MVPCONTROL
	ori	t1, t1, MVPCONTROL_VPC
	mtc0	t1, CP0_MVPCONTROL
	ehb

	/* Loop through each VPE */
	lw	t6, COREBOOTCFG_VPEMASK(t0)
	move	t8, t6
	li	t5, 0

	/* Check whether the VPE should be running. If not, skip it */
1:	andi	t0, t6, 1
	beqz	t0, 2f
	 nop

	/* Operate on the appropriate TC */
	mfc0	t0, CP0_VPECONTROL
	ori	t0, t0, VPECONTROL_TARGTC
	xori	t0, t0, VPECONTROL_TARGTC
	or	t0, t0, t5
	mtc0	t0, CP0_VPECONTROL
	ehb

	/* Skip the VPE if its TC is not halted */
	mftc0	t0, CP0_TCHALT
	beqz	t0, 2f
	 nop

	/* Calculate a pointer to the VPEs struct vpe_boot_config */
	li	t0, VPEBOOTCFG_SIZE
	mul	t0, t0, t5
	addu	t0, t0, t7

	/* Set the TC restart PC */
	lw	t1, VPEBOOTCFG_PC(t0)
	mttc0	t1, CP0_TCRESTART

	/* Set the TC stack pointer */
	lw	t1, VPEBOOTCFG_SP(t0)
	mttgpr	t1, sp

	/* Set the TC global pointer */
	lw	t1, VPEBOOTCFG_GP(t0)
	mttgpr	t1, gp

	/* Copy config from this VPE */
	mfc0	t0, CP0_CONFIG
	mttc0	t0, CP0_CONFIG

	/* Ensure no software interrupts are pending */
	mttc0	zero, CP0_CAUSE
	mttc0	zero, CP0_STATUS

	/* Set TC active, not interrupt exempt */
	mftc0	t0, CP0_TCSTATUS
	li	t1, ~TCSTATUS_IXMT
	and	t0, t0, t1
	ori	t0, t0, TCSTATUS_A
	mttc0	t0, CP0_TCSTATUS

	/* Clear the TC halt bit */
	mttc0	zero, CP0_TCHALT

	/* Set VPE active */
	mftc0	t0, CP0_VPECONF0
	ori	t0, t0, VPECONF0_VPA
	mttc0	t0, CP0_VPECONF0

	/* Next VPE */
2:	srl	t6, t6, 1
	addi	t5, t5, 1
	bnez	t6, 1b
	 nop

	/* Leave VPE configuration state */
	mfc0	t1, CP0_MVPCONTROL
	xori	t1, t1, MVPCONTROL_VPC
	mtc0	t1, CP0_MVPCONTROL
	ehb
	evpe

	/* Check whether this VPE is meant to be running */
	li	t0, 1
	sll	t0, t0, t9
	and	t0, t0, t8
	bnez	t0, 2f
	 nop

	/* This VPE should be offline, halt the TC */
	li	t0, TCHALT_H
	mtc0	t0, CP0_TCHALT
	la	t0, 1f
1:	jr.hb	t0
	 nop

2:	.set	pop

#endif /* CONFIG_MIPS_MT */

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