[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/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
	 nop
	.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
	PTR_ADD	a1, a0, t1
1:	cache	Index_Store_Tag_I, 0(a0)
	PTR_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
	PTR_ADDU a1, a0, t1
	PTR_SUBU a1, a1, t0
1:	cache	Index_Store_Tag_D, 0(a0)
	bne	a0, a1, 1b
	 PTR_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! */
	PTR_L	t1, VPEBOOTCFG_PC(v0)
	PTR_L	gp, VPEBOOTCFG_GP(v0)
	PTR_L	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

	.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
	PTR_L	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
	PTR_L	t1, 0(t1)
	PTR_ADDU t0, t0, t1

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

	/* 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
#endif

1:	/* Calculate a pointer to this VPEs struct vpe_boot_config */
	li	t1, VPEBOOTCFG_SIZE
	mul	v0, t9, t1
	PTR_L	ta3, COREBOOTCFG_VPECONFIG(t0)
	PTR_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 */
	PTR_L	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
1e5fb282	42	nop
245a7868	43	.endm
0ee958e1 PB	44
	45	.section .text.cps-vec
	46	.balign 0x1000
0ee958e1 PB	47
	48	LEAF(mips_cps_core_entry)
	49	/*
0155a065 PB	50	* These first 12 bytes will be patched by cps_smp_setup to load the
	51	* base address of the CM GCRs into register v1 and the CCA to use into
	52	* register s0.
0ee958e1 PB	53	*/
0ee958e1 PB	54	.quad 0
0155a065	55	.word 0
0ee958e1 PB	56
	57	/* Check whether we're here due to an NMI */
	58	mfc0 k0, CP0_STATUS
	59	and k0, k0, ST0_NMI
	60	beqz k0, not_nmi
	61	nop
	62
	63	/* This is an NMI */
81a02e34	64	PTR_LA k0, nmi_handler
0ee958e1 PB	65	jr k0
	66	nop
	67
	68	not_nmi:
	69	/* Setup Cause */
	70	li t0, CAUSEF_IV
	71	mtc0 t0, CP0_CAUSE
	72
	73	/* Setup Status */
	74	li t0, ST0_CU1 \| ST0_CU0
	75	mtc0 t0, CP0_STATUS
	76
	77	/*
	78	* Clear the bits used to index the caches. Note that the architecture
	79	* dictates that writing to any of TagLo or TagHi selects 0 or 2 should
	80	* be valid for all MIPS32 CPUs, even those for which said writes are
	81	* unnecessary.
	82	*/
	83	mtc0 zero, CP0_TAGLO, 0
	84	mtc0 zero, CP0_TAGHI, 0
	85	mtc0 zero, CP0_TAGLO, 2
	86	mtc0 zero, CP0_TAGHI, 2
	87	ehb
	88
	89	/* Primary cache configuration is indicated by Config1 */
	90	mfc0 v0, CP0_CONFIG, 1
	91
	92	/* Detect I-cache line size */
	93	_EXT t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
	94	beqz t0, icache_done
	95	li t1, 2
	96	sllv t0, t1, t0
	97
	98	/* Detect I-cache size */
	99	_EXT t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
	100	xori t2, t1, 0x7
	101	beqz t2, 1f
	102	li t3, 32
acac4108	103	addiu t1, t1, 1
0ee958e1 PB	104	sllv t1, t3, t1
	105	1: /* At this point t1 == I-cache sets per way */
	106	_EXT t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
acac4108	107	addiu t2, t2, 1
0ee958e1 PB	108	mul t1, t1, t0
	109	mul t1, t1, t2
	110
717f1425	111	li a0, CKSEG0
b677bc03	112	PTR_ADD a1, a0, t1
0ee958e1	113	1: cache Index_Store_Tag_I, 0(a0)
b677bc03	114	PTR_ADD a0, a0, t0
0ee958e1 PB	115	bne a0, a1, 1b
	116	nop
	117	icache_done:
	118
	119	/* Detect D-cache line size */
	120	_EXT t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
	121	beqz t0, dcache_done
	122	li t1, 2
	123	sllv t0, t1, t0
	124
	125	/* Detect D-cache size */
	126	_EXT t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
	127	xori t2, t1, 0x7
	128	beqz t2, 1f
	129	li t3, 32
acac4108	130	addiu t1, t1, 1
0ee958e1 PB	131	sllv t1, t3, t1
	132	1: /* At this point t1 == D-cache sets per way */
	133	_EXT t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
acac4108	134	addiu t2, t2, 1
0ee958e1 PB	135	mul t1, t1, t0
	136	mul t1, t1, t2
	137
717f1425	138	li a0, CKSEG0
b677bc03 MC	139	PTR_ADDU a1, a0, t1
b677bc03 MC	140	PTR_SUBU a1, a1, t0
0ee958e1 PB	141	1: cache Index_Store_Tag_D, 0(a0)
0ee958e1 PB	142	bne a0, a1, 1b
b677bc03	143	PTR_ADD a0, a0, t0
0ee958e1 PB	144	dcache_done:
0ee958e1 PB	145
0155a065	146	/* Set Kseg0 CCA to that in s0 */
0ee958e1 PB	147	mfc0 t0, CP0_CONFIG
0ee958e1 PB	148	ori t0, 0x7
0155a065 PB	149	xori t0, 0x7
0155a065 PB	150	or t0, t0, s0
0ee958e1 PB	151	mtc0 t0, CP0_CONFIG
	152	ehb
	153
	154	/* Enter the coherent domain */
	155	li t0, 0xff
90996511	156	sw t0, GCR_CL_COHERENCE_OFS(v1)
0ee958e1 PB	157	ehb
	158
	159	/* Jump to kseg0 */
81a02e34	160	PTR_LA t0, 1f
0ee958e1 PB	161	jr t0
	162	nop
	163
245a7868 PB	164	/*
	165	* We're up, cached & coherent. Perform any further required core-level
	166	* initialisation.
	167	*/
	168	1: jal mips_cps_core_init
	169	nop
0ee958e1	170
6521d9a4 MC	171	/* Do any EVA initialization if necessary */
	172	eva_init
	173
0ee958e1	174	/*
245a7868 PB	175	* Boot any other VPEs within this core that should be online, and
245a7868 PB	176	* deactivate this VPE if it should be offline.
0ee958e1	177	*/
245a7868 PB	178	jal mips_cps_boot_vpes
245a7868 PB	179	nop
0ee958e1 PB	180
0ee958e1 PB	181	/* Off we go! */
b677bc03 MC	182	PTR_L t1, VPEBOOTCFG_PC(v0)
	183	PTR_L gp, VPEBOOTCFG_GP(v0)
	184	PTR_L sp, VPEBOOTCFG_SP(v0)
0ee958e1 PB	185	jr t1
	186	nop
	187	END(mips_cps_core_entry)
	188
	189	.org 0x200
	190	LEAF(excep_tlbfill)
	191	b .
	192	nop
	193	END(excep_tlbfill)
	194
	195	.org 0x280
	196	LEAF(excep_xtlbfill)
	197	b .
	198	nop
	199	END(excep_xtlbfill)
	200
	201	.org 0x300
	202	LEAF(excep_cache)
	203	b .
	204	nop
	205	END(excep_cache)
	206
	207	.org 0x380
	208	LEAF(excep_genex)
	209	b .
	210	nop
	211	END(excep_genex)
	212
	213	.org 0x400
	214	LEAF(excep_intex)
	215	b .
	216	nop
	217	END(excep_intex)
	218
	219	.org 0x480
	220	LEAF(excep_ejtag)
81a02e34	221	PTR_LA k0, ejtag_debug_handler
0ee958e1 PB	222	jr k0
	223	nop
	224	END(excep_ejtag)
245a7868 PB	225
	226	LEAF(mips_cps_core_init)
	227	#ifdef CONFIG_MIPS_MT
	228	/* Check that the core implements the MT ASE */
	229	has_mt t0, 3f
245a7868 PB	230
245a7868 PB	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
b677bc03	302	PTR_L t0, 0(t0)
245a7868 PB	303
245a7868 PB	304	/* Calculate a pointer to this cores struct core_boot_config */
90996511	305	lw t0, GCR_CL_ID_OFS(t0)
245a7868 PB	306	li t1, COREBOOTCFG_SIZE
245a7868 PB	307	mul t0, t0, t1
81a02e34	308	PTR_LA t1, mips_cps_core_bootcfg
b677bc03 MC	309	PTR_L t1, 0(t1)
b677bc03 MC	310	PTR_ADDU t0, t0, t1
245a7868 PB	311
245a7868 PB	312	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
1e5fb282	313	li t9, 0
a5b0f6db	314	#ifdef CONFIG_MIPS_MT
0586ac75	315	has_mt ta2, 1f
245a7868 PB	316
	317	/* Find the number of VPEs present in the core */
	318	mfc0 t1, CP0_MVPCONF0
	319	srl t1, t1, MVPCONF0_PVPE_SHIFT
	320	andi t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
acac4108	321	addiu t1, t1, 1
245a7868 PB	322
	323	/* Calculate a mask for the VPE ID from EBase.CPUNum */
	324	clz t1, t1
	325	li t2, 31
	326	subu t1, t2, t1
	327	li t2, 1
	328	sll t1, t2, t1
	329	addiu t1, t1, -1
	330
	331	/* Retrieve the VPE ID from EBase.CPUNum */
	332	mfc0 t9, $15, 1
	333	and t9, t9, t1
a5b0f6db	334	#endif
245a7868 PB	335
	336	1: /* Calculate a pointer to this VPEs struct vpe_boot_config */
	337	li t1, VPEBOOTCFG_SIZE
	338	mul v0, t9, t1
b677bc03 MC	339	PTR_L ta3, COREBOOTCFG_VPECONFIG(t0)
b677bc03 MC	340	PTR_ADDU v0, v0, ta3
245a7868 PB	341
	342	#ifdef CONFIG_MIPS_MT
	343
	344	/* If the core doesn't support MT then return */
0586ac75	345	bnez ta2, 1f
245a7868 PB	346	nop
	347	jr ra
	348	nop
	349
	350	.set push
977e043d	351	.set mips64r2
245a7868 PB	352	.set mt
	353
	354	1: /* Enter VPE configuration state */
	355	dvpe
81a02e34	356	PTR_LA t1, 1f
245a7868 PB	357	jr.hb t1
	358	nop
	359	1: mfc0 t1, CP0_MVPCONTROL
	360	ori t1, t1, MVPCONTROL_VPC
	361	mtc0 t1, CP0_MVPCONTROL
	362	ehb
	363
	364	/* Loop through each VPE */
b677bc03	365	PTR_L ta2, COREBOOTCFG_VPEMASK(t0)
0586ac75 MC	366	move t8, ta2
0586ac75 MC	367	li ta1, 0
245a7868 PB	368
245a7868 PB	369	/* Check whether the VPE should be running. If not, skip it */
0586ac75	370	1: andi t0, ta2, 1
245a7868 PB	371	beqz t0, 2f
	372	nop
	373
	374	/* Operate on the appropriate TC */
	375	mfc0 t0, CP0_VPECONTROL
	376	ori t0, t0, VPECONTROL_TARGTC
	377	xori t0, t0, VPECONTROL_TARGTC
0586ac75	378	or t0, t0, ta1
245a7868 PB	379	mtc0 t0, CP0_VPECONTROL
	380	ehb
	381
	382	/* Skip the VPE if its TC is not halted */
	383	mftc0 t0, CP0_TCHALT
	384	beqz t0, 2f
	385	nop
	386
	387	/* Calculate a pointer to the VPEs struct vpe_boot_config */
	388	li t0, VPEBOOTCFG_SIZE
0586ac75 MC	389	mul t0, t0, ta1
0586ac75 MC	390	addu t0, t0, ta3
245a7868 PB	391
	392	/* Set the TC restart PC */
	393	lw t1, VPEBOOTCFG_PC(t0)
	394	mttc0 t1, CP0_TCRESTART
	395
	396	/* Set the TC stack pointer */
	397	lw t1, VPEBOOTCFG_SP(t0)
	398	mttgpr t1, sp
	399
	400	/* Set the TC global pointer */
	401	lw t1, VPEBOOTCFG_GP(t0)
	402	mttgpr t1, gp
	403
	404	/* Copy config from this VPE */
	405	mfc0 t0, CP0_CONFIG
	406	mttc0 t0, CP0_CONFIG
	407
	408	/* Ensure no software interrupts are pending */
	409	mttc0 zero, CP0_CAUSE
	410	mttc0 zero, CP0_STATUS
	411
	412	/* Set TC active, not interrupt exempt */
	413	mftc0 t0, CP0_TCSTATUS
	414	li t1, ~TCSTATUS_IXMT
	415	and t0, t0, t1
	416	ori t0, t0, TCSTATUS_A
	417	mttc0 t0, CP0_TCSTATUS
	418
	419	/* Clear the TC halt bit */
	420	mttc0 zero, CP0_TCHALT
	421
	422	/* Set VPE active */
	423	mftc0 t0, CP0_VPECONF0
	424	ori t0, t0, VPECONF0_VPA
	425	mttc0 t0, CP0_VPECONF0
	426
	427	/* Next VPE */
0586ac75 MC	428	2: srl ta2, ta2, 1
	429	addiu ta1, ta1, 1
	430	bnez ta2, 1b
245a7868 PB	431	nop
	432
	433	/* Leave VPE configuration state */
	434	mfc0 t1, CP0_MVPCONTROL
	435	xori t1, t1, MVPCONTROL_VPC
	436	mtc0 t1, CP0_MVPCONTROL
	437	ehb
	438	evpe
	439
	440	/* Check whether this VPE is meant to be running */
	441	li t0, 1
	442	sll t0, t0, t9
	443	and t0, t0, t8
	444	bnez t0, 2f
	445	nop
	446
	447	/* This VPE should be offline, halt the TC */
	448	li t0, TCHALT_H
	449	mtc0 t0, CP0_TCHALT
81a02e34	450	PTR_LA t0, 1f
245a7868 PB	451	1: jr.hb t0
	452	nop
	453
	454	2: .set pop
	455
	456	#endif /* CONFIG_MIPS_MT */
	457
	458	/* Return */
	459	jr ra
	460	nop
	461	END(mips_cps_boot_vpes)
3179d37e PB	462
	463	#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)
	464
	465	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
	466	.macro psstate dest
	467	.set push
	468	.set noat
	469	lw $1, TI_CPU(gp)
	470	sll $1, $1, LONGLOG
81a02e34	471	PTR_LA \dest, __per_cpu_offset
3179d37e PB	472	addu $1, $1, \dest
3179d37e PB	473	lw $1, 0($1)
81a02e34	474	PTR_LA \dest, cps_cpu_state
3179d37e PB	475	addu \dest, \dest, $1
	476	.set pop
	477	.endm
	478
	479	LEAF(mips_cps_pm_save)
	480	/* Save CPU state */
	481	SUSPEND_SAVE_REGS
	482	psstate t1
	483	SUSPEND_SAVE_STATIC
	484	jr v0
	485	nop
	486	END(mips_cps_pm_save)
	487
	488	LEAF(mips_cps_pm_restore)
	489	/* Restore CPU state */
	490	psstate t1
	491	RESUME_RESTORE_STATIC
	492	RESUME_RESTORE_REGS_RETURN
	493	END(mips_cps_pm_restore)
	494
	495	#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */