[deliverable/linux.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/eva.h>
#include <asm/mipsregs.h>
#include <asm/mipsmtregs.h>
#include <asm/pm.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 12 bytes will be patched by cps_smp_setup to load the
	 * base address of the CM GCRs into register v1 and the CCA to use into
	 * register s0.
	 */
	.quad	0
	.word	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 */
	PTR_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
	addiu	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
	addiu	t2, t2, 1
	mul	t1, t1, t0
	mul	t1, t1, t2

	li	a0, CKSEG0
	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
	addiu	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
	addiu	t2, t2, 1
	mul	t1, t1, t0
	mul	t1, t1, t2

	li	a0, CKSEG0
	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 CCA to that in s0 */
	mfc0	t0, CP0_CONFIG
	ori	t0, 0x7
	xori	t0, 0x7
	or	t0, t0, s0
	mtc0	t0, CP0_CONFIG
	ehb

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

	/* Jump to kseg0 */
	PTR_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

	/* Do any EVA initialization if necessary */
	eva_init

	/*
	 * 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)
	PTR_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	mips64r2
	.set	mt

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

	/* ...and for the moment only 1 VPE */
	dvpe
	PTR_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)
	addiu	ta3, t0, 1

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

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

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

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

	/* Set exclusive TC, non-active, master */
	li	t0, VPECONF0_MVP
	sll	t1, ta1, 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 */
	addiu	ta1, ta1, 1
	slt	t0, ta1, ta3
	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 */
	PTR_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
	PTR_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	ta2, 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
	addiu	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	ta3, COREBOOTCFG_VPECONFIG(t0)
	addu	v0, v0, ta3

#ifdef CONFIG_MIPS_MT

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

	.set	push
	.set	mips64r2
	.set	mt

1:	/* Enter VPE configuration state */
	dvpe
	PTR_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	ta2, COREBOOTCFG_VPEMASK(t0)
	move	t8, ta2
	li	ta1, 0

	/* Check whether the VPE should be running. If not, skip it */
1:	andi	t0, ta2, 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, ta1
	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, ta1
	addu	t0, t0, ta3

	/* 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	ta2, ta2, 1
	addiu	ta1, ta1, 1
	bnez	ta2, 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
	PTR_LA	t0, 1f
1:	jr.hb	t0
	 nop

2:	.set	pop

#endif /* CONFIG_MIPS_MT */

	/* Return */
	jr	ra
	 nop
	END(mips_cps_boot_vpes)

#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)

	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
	.macro	psstate	dest
	.set	push
	.set	noat
	lw	$1, TI_CPU(gp)
	sll	$1, $1, LONGLOG
	PTR_LA	\dest, __per_cpu_offset
	addu	$1, $1, \dest
	lw	$1, 0($1)
	PTR_LA	\dest, cps_cpu_state
	addu	\dest, \dest, $1
	.set	pop
	.endm

LEAF(mips_cps_pm_save)
	/* Save CPU state */
	SUSPEND_SAVE_REGS
	psstate	t1
	SUSPEND_SAVE_STATIC
	jr	v0
	 nop
	END(mips_cps_pm_save)

LEAF(mips_cps_pm_restore)
	/* Restore CPU state */
	psstate	t1
	RESUME_RESTORE_STATIC
	RESUME_RESTORE_REGS_RETURN
	END(mips_cps_pm_restore)

#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */
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>
6521d9a4	16	#include <asm/eva.h>
0ee958e1	17	#include <asm/mipsregs.h>
245a7868	18	#include <asm/mipsmtregs.h>
3179d37e	19	#include <asm/pm.h>
0ee958e1	20
245a7868 PB	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
0ee958e1 PB	43
	44	.section .text.cps-vec
	45	.balign 0x1000
0ee958e1 PB	46
	47	LEAF(mips_cps_core_entry)
	48	/*
0155a065 PB	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.
0ee958e1 PB	52	*/
0ee958e1 PB	53	.quad 0
0155a065	54	.word 0
0ee958e1 PB	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 */
81a02e34	63	PTR_LA k0, nmi_handler
0ee958e1 PB	64	jr k0
	65	nop
	66
	67	not_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
acac4108	102	addiu t1, t1, 1
0ee958e1 PB	103	sllv t1, t3, t1
	104	1: /* At this point t1 == I-cache sets per way */
	105	_EXT t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
acac4108	106	addiu t2, t2, 1
0ee958e1 PB	107	mul t1, t1, t0
	108	mul t1, t1, t2
	109
717f1425	110	li a0, CKSEG0
0ee958e1 PB	111	add a1, a0, t1
	112	1: cache Index_Store_Tag_I, 0(a0)
	113	add a0, a0, t0
	114	bne a0, a1, 1b
	115	nop
	116	icache_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
acac4108	129	addiu t1, t1, 1
0ee958e1 PB	130	sllv t1, t3, t1
	131	1: /* At this point t1 == D-cache sets per way */
	132	_EXT t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
acac4108	133	addiu t2, t2, 1
0ee958e1 PB	134	mul t1, t1, t0
	135	mul t1, t1, t2
	136
717f1425	137	li a0, CKSEG0
0ee958e1 PB	138	addu a1, a0, t1
	139	subu a1, a1, t0
	140	1: cache Index_Store_Tag_D, 0(a0)
	141	bne a0, a1, 1b
	142	add a0, a0, t0
	143	dcache_done:
	144
0155a065	145	/* Set Kseg0 CCA to that in s0 */
0ee958e1 PB	146	mfc0 t0, CP0_CONFIG
0ee958e1 PB	147	ori t0, 0x7
0155a065 PB	148	xori t0, 0x7
0155a065 PB	149	or t0, t0, s0
0ee958e1 PB	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 */
81a02e34	159	PTR_LA t0, 1f
0ee958e1 PB	160	jr t0
	161	nop
	162
245a7868 PB	163	/*
	164	* We're up, cached & coherent. Perform any further required core-level
	165	* initialisation.
	166	*/
	167	1: jal mips_cps_core_init
	168	nop
0ee958e1	169
6521d9a4 MC	170	/* Do any EVA initialization if necessary */
	171	eva_init
	172
0ee958e1	173	/*
245a7868 PB	174	* Boot any other VPEs within this core that should be online, and
245a7868 PB	175	* deactivate this VPE if it should be offline.
0ee958e1	176	*/
245a7868 PB	177	jal mips_cps_boot_vpes
245a7868 PB	178	nop
0ee958e1 PB	179
0ee958e1 PB	180	/* Off we go! */
245a7868 PB	181	lw t1, VPEBOOTCFG_PC(v0)
	182	lw gp, VPEBOOTCFG_GP(v0)
	183	lw sp, VPEBOOTCFG_SP(v0)
0ee958e1 PB	184	jr t1
	185	nop
	186	END(mips_cps_core_entry)
	187
	188	.org 0x200
	189	LEAF(excep_tlbfill)
	190	b .
	191	nop
	192	END(excep_tlbfill)
	193
	194	.org 0x280
	195	LEAF(excep_xtlbfill)
	196	b .
	197	nop
	198	END(excep_xtlbfill)
	199
	200	.org 0x300
	201	LEAF(excep_cache)
	202	b .
	203	nop
	204	END(excep_cache)
	205
	206	.org 0x380
	207	LEAF(excep_genex)
	208	b .
	209	nop
	210	END(excep_genex)
	211
	212	.org 0x400
	213	LEAF(excep_intex)
	214	b .
	215	nop
	216	END(excep_intex)
	217
	218	.org 0x480
	219	LEAF(excep_ejtag)
81a02e34	220	PTR_LA k0, ejtag_debug_handler
0ee958e1 PB	221	jr k0
	222	nop
	223	END(excep_ejtag)
245a7868 PB	224
	225	LEAF(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
977e043d	232	.set mips64r2
245a7868 PB	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
81a02e34	240	PTR_LA t1, 1f
245a7868 PB	241	jr.hb t1
	242	nop
	243
	244	/* Enter VPE configuration state */
	245	1: 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)
0586ac75	253	addiu ta3, t0, 1
245a7868 PB	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 */
0586ac75	260	li ta1, 1
245a7868 PB	261
245a7868 PB	262	1: /* Operate on the appropriate TC */
0586ac75	263	mtc0 ta1, CP0_VPECONTROL
245a7868 PB	264	ehb
	265
	266	/* Bind TC to VPE (1:1 TC:VPE mapping) */
0586ac75	267	mttc0 ta1, CP0_TCBIND
245a7868 PB	268
	269	/* Set exclusive TC, non-active, master */
	270	li t0, VPECONF0_MVP
0586ac75	271	sll t1, ta1, VPECONF0_XTC_SHIFT
245a7868 PB	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 */
0586ac75 MC	283	addiu ta1, ta1, 1
0586ac75 MC	284	slt t0, ta1, ta3
245a7868 PB	285	bnez t0, 1b
	286	nop
	287
	288	/* Leave VPE configuration state */
	289	2: mfc0 t0, CP0_MVPCONTROL
	290	xori t0, t0, MVPCONTROL_VPC
	291	mtc0 t0, CP0_MVPCONTROL
	292
	293	3: .set pop
	294	#endif
	295	jr ra
	296	nop
	297	END(mips_cps_core_init)
	298
	299	LEAF(mips_cps_boot_vpes)
	300	/* Retrieve CM base address */
81a02e34	301	PTR_LA t0, mips_cm_base
245a7868 PB	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
81a02e34	308	PTR_LA t1, mips_cps_core_bootcfg
245a7868 PB	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 */
0586ac75	313	has_mt ta2, 1f
245a7868 PB	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
acac4108	320	addiu t1, t1, 1
245a7868 PB	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
	334	1: /* Calculate a pointer to this VPEs struct vpe_boot_config */
	335	li t1, VPEBOOTCFG_SIZE
	336	mul v0, t9, t1
0586ac75 MC	337	lw ta3, COREBOOTCFG_VPECONFIG(t0)
0586ac75 MC	338	addu v0, v0, ta3
245a7868 PB	339
	340	#ifdef CONFIG_MIPS_MT
	341
	342	/* If the core doesn't support MT then return */
0586ac75	343	bnez ta2, 1f
245a7868 PB	344	nop
	345	jr ra
	346	nop
	347
	348	.set push
977e043d	349	.set mips64r2
245a7868 PB	350	.set mt
	351
	352	1: /* Enter VPE configuration state */
	353	dvpe
81a02e34	354	PTR_LA t1, 1f
245a7868 PB	355	jr.hb t1
	356	nop
	357	1: mfc0 t1, CP0_MVPCONTROL
	358	ori t1, t1, MVPCONTROL_VPC
	359	mtc0 t1, CP0_MVPCONTROL
	360	ehb
	361
	362	/* Loop through each VPE */
0586ac75 MC	363	lw ta2, COREBOOTCFG_VPEMASK(t0)
	364	move t8, ta2
	365	li ta1, 0
245a7868 PB	366
245a7868 PB	367	/* Check whether the VPE should be running. If not, skip it */
0586ac75	368	1: andi t0, ta2, 1
245a7868 PB	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
0586ac75	376	or t0, t0, ta1
245a7868 PB	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
0586ac75 MC	387	mul t0, t0, ta1
0586ac75 MC	388	addu t0, t0, ta3
245a7868 PB	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 */
0586ac75 MC	426	2: srl ta2, ta2, 1
	427	addiu ta1, ta1, 1
	428	bnez ta2, 1b
245a7868 PB	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
81a02e34	448	PTR_LA t0, 1f
245a7868 PB	449	1: jr.hb t0
	450	nop
	451
	452	2: .set pop
	453
	454	#endif /* CONFIG_MIPS_MT */
	455
	456	/* Return */
	457	jr ra
	458	nop
	459	END(mips_cps_boot_vpes)
3179d37e PB	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
81a02e34	469	PTR_LA \dest, __per_cpu_offset
3179d37e PB	470	addu $1, $1, \dest
3179d37e PB	471	lw $1, 0($1)
81a02e34	472	PTR_LA \dest, cps_cpu_state
3179d37e PB	473	addu \dest, \dest, $1
	474	.set pop
	475	.endm
	476
	477	LEAF(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
	486	LEAF(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 */