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

/*
 * Copyright (C) 2013 Imagination Technologies
 * Author: Paul Burton <paul.burton@mips.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_CPC_BASE_OFS	0x0088
#define GCR_CL_COHERENCE_OFS	0x2008
#define GCR_CL_ID_OFS		0x2028

#define CPC_CL_VC_STOP_OFS	0x2020
#define CPC_CL_VC_RUN_OFS	0x2028

.extern mips_cm_base

.set noreorder

#ifdef CONFIG_64BIT
# define STATUS_BITDEPS		ST0_KX
#else
# define STATUS_BITDEPS		0
#endif

#ifdef CONFIG_MIPS_CPS_NS16550

#define DUMP_EXCEP(name)		\
	PTR_LA	a0, 8f;			\
	jal	mips_cps_bev_dump;	\
	 nop;				\
	TEXT(name)

#else /* !CONFIG_MIPS_CPS_NS16550 */

#define DUMP_EXCEP(name)

#endif /* !CONFIG_MIPS_CPS_NS16550 */

	/*
	 * 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, 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

	/*
	 * Set dest to non-zero if the core supports MIPSr6 multithreading
	 * (ie. VPs), else zero. If MIPSr6 multithreading is not supported then
	 * branch to nomt.
	 */
	.macro	has_vp	dest, nomt
	mfc0	\dest, CP0_CONFIG, 1
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 2
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 3
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 4
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 5
	andi	\dest, \dest, MIPS_CONF5_VP
	beqz	\dest, \nomt
	 nop
	.endm

	/* Calculate an uncached address for the CM GCRs */
	.macro	cmgcrb	dest
	.set	push
	.set	noat
	MFC0	$1, CP0_CMGCRBASE
	PTR_SLL	$1, $1, 4
	PTR_LI	\dest, UNCAC_BASE
	PTR_ADDU \dest, \dest, $1
	.set	pop
	.endm

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

LEAF(mips_cps_core_entry)
	/*
	 * These first 4 bytes will be patched by cps_smp_setup to load the
	 * CCA to use into register s0.
	 */
	.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 | ST0_BEV | STATUS_BITDEPS
	mtc0	t0, CP0_STATUS

	/* Skip cache & coherence setup if we're already coherent */
	cmgcrb	v1
	lw	s7, GCR_CL_COHERENCE_OFS(v1)
	bnez	s7, 1f
	 nop

	/* Initialize the L1 caches */
	jal	mips_cps_cache_init
	 nop

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

	/* Set Kseg0 CCA to that in s0 */
1:	mfc0	t0, CP0_CONFIG
	ori	t0, 0x7
	xori	t0, 0x7
	or	t0, t0, s0
	mtc0	t0, CP0_CONFIG
	ehb

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

	/*
	 * We're up, cached & coherent. Perform any EVA initialization necessary
	 * before we access memory.
	 */
1:	eva_init

	/* Retrieve boot configuration pointers */
	jal	mips_cps_get_bootcfg
	 nop

	/* Skip core-level init if we started up coherent */
	bnez	s7, 1f
	 nop

	/* Perform any further required core-level initialisation */
	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.
	 */
	move	a1, t9
	jal	mips_cps_boot_vpes
	 move	a0, v0

	/* Off we go! */
1:	PTR_L	t1, VPEBOOTCFG_PC(v1)
	PTR_L	gp, VPEBOOTCFG_GP(v1)
	PTR_L	sp, VPEBOOTCFG_SP(v1)
	jr	t1
	 nop
	END(mips_cps_core_entry)

.org 0x200
LEAF(excep_tlbfill)
	DUMP_EXCEP("TLB Fill")
	b	.
	 nop
	END(excep_tlbfill)

.org 0x280
LEAF(excep_xtlbfill)
	DUMP_EXCEP("XTLB Fill")
	b	.
	 nop
	END(excep_xtlbfill)

.org 0x300
LEAF(excep_cache)
	DUMP_EXCEP("Cache")
	b	.
	 nop
	END(excep_cache)

.org 0x380
LEAF(excep_genex)
	DUMP_EXCEP("General")
	b	.
	 nop
	END(excep_genex)

.org 0x400
LEAF(excep_intex)
	DUMP_EXCEP("Interrupt")
	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_SMP
	/* Check that the core implements the MT ASE */
	has_mt	t0, 3f

	.set	push
	.set	MIPS_ISA_LEVEL_RAW
	.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)

/**
 * mips_cps_get_bootcfg() - retrieve boot configuration pointers
 *
 * Returns: pointer to struct core_boot_config in v0, pointer to
 *          struct vpe_boot_config in v1, VPE ID in t9
 */
LEAF(mips_cps_get_bootcfg)
	/* Calculate a pointer to this cores struct core_boot_config */
	cmgcrb	t0
	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 v0, t0, t1

	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
	li	t9, 0
#if defined(CONFIG_CPU_MIPSR6)
	has_vp	ta2, 1f

	/*
	 * Assume non-contiguous numbering. Perhaps some day we'll need
	 * to handle contiguous VP numbering, but no such systems yet
	 * exist.
	 */
	mfc0	t9, CP0_GLOBALNUMBER
	andi	t9, t9, MIPS_GLOBALNUMBER_VP
#elif defined(CONFIG_MIPS_MT_SMP)
	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	v1, t9, t1
	PTR_L	ta3, COREBOOTCFG_VPECONFIG(v0)
	PTR_ADDU v1, v1, ta3

	jr	ra
	 nop
	END(mips_cps_get_bootcfg)

LEAF(mips_cps_boot_vpes)
	lw	ta2, COREBOOTCFG_VPEMASK(a0)
	PTR_L	ta3, COREBOOTCFG_VPECONFIG(a0)

#if defined(CONFIG_CPU_MIPSR6)

	has_vp	t0, 5f

	/* Find base address of CPC */
	cmgcrb	t3
	PTR_L	t1, GCR_CPC_BASE_OFS(t3)
	PTR_LI	t2, ~0x7fff
	and	t1, t1, t2
	PTR_LI	t2, UNCAC_BASE
	PTR_ADD	t1, t1, t2

	/* Start any other VPs that ought to be running */
	PTR_S	ta2, CPC_CL_VC_RUN_OFS(t1)

	/* Ensure this VP stops running if it shouldn't be */
	not	ta2
	PTR_S	ta2, CPC_CL_VC_STOP_OFS(t1)
	ehb

#elif defined(CONFIG_MIPS_MT)

	.set	push
	.set	MIPS_ISA_LEVEL_RAW
	.set	mt

	/* If the core doesn't support MT then return */
	has_mt	t0, 5f

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

	/*
	 * Copy the EVA config from this VPE if the CPU supports it.
	 * CONFIG3 must exist to be running MT startup - just read it.
	 */
	mfc0	t0, CP0_CONFIG, 3
	and	t0, t0, MIPS_CONF3_SC
	beqz	t0, 3f
	 nop
	mfc0    t0, CP0_SEGCTL0
	mttc0	t0, CP0_SEGCTL0
	mfc0    t0, CP0_SEGCTL1
	mttc0	t0, CP0_SEGCTL1
	mfc0    t0, CP0_SEGCTL2
	mttc0	t0, CP0_SEGCTL2
3:
	/* 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, a1
	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_SMP */

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

LEAF(mips_cps_cache_init)
	/*
	 * 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:

	jr	ra
	 nop
	END(mips_cps_cache_init)

#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	/*
0ee958e1 PB	2	* Copyright (C) 2013 Imagination Technologies
fb615d61	3	* Author: Paul Burton <paul.burton@mips.com>
0ee958e1 PB	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
5a3e7c02	21	#define GCR_CPC_BASE_OFS 0x0088
245a7868 PB	22	#define GCR_CL_COHERENCE_OFS 0x2008
	23	#define GCR_CL_ID_OFS 0x2028
	24
fa7a3b4a	25	#define CPC_CL_VC_STOP_OFS 0x2020
5a3e7c02 PB	26	#define CPC_CL_VC_RUN_OFS 0x2028
5a3e7c02 PB	27
245a7868 PB	28	.extern mips_cm_base
	29
	30	.set noreorder
	31
8fe2c547 PB	32	#ifdef CONFIG_64BIT
	33	# define STATUS_BITDEPS ST0_KX
	34	#else
	35	# define STATUS_BITDEPS 0
	36	#endif
	37
609cf6f2 PB	38	#ifdef CONFIG_MIPS_CPS_NS16550
	39
	40	#define DUMP_EXCEP(name) \
	41	PTR_LA a0, 8f; \
	42	jal mips_cps_bev_dump; \
	43	nop; \
	44	TEXT(name)
	45
	46	#else /* !CONFIG_MIPS_CPS_NS16550 */
	47
	48	#define DUMP_EXCEP(name)
	49
	50	#endif /* !CONFIG_MIPS_CPS_NS16550 */
	51
245a7868 PB	52	/*
	53	* Set dest to non-zero if the core supports the MT ASE, else zero. If
	54	* MT is not supported then branch to nomt.
	55	*/
	56	.macro has_mt dest, nomt
192fbc20	57	mfc0 \dest, CP0_CONFIG, 1
245a7868 PB	58	bgez \dest, \nomt
	59	mfc0 \dest, CP0_CONFIG, 2
	60	bgez \dest, \nomt
	61	mfc0 \dest, CP0_CONFIG, 3
	62	andi \dest, \dest, MIPS_CONF3_MT
	63	beqz \dest, \nomt
1e5fb282	64	nop
245a7868	65	.endm
0ee958e1	66
5a3e7c02 PB	67	/*
	68	* Set dest to non-zero if the core supports MIPSr6 multithreading
	69	* (ie. VPs), else zero. If MIPSr6 multithreading is not supported then
	70	* branch to nomt.
	71	*/
	72	.macro has_vp dest, nomt
	73	mfc0 \dest, CP0_CONFIG, 1
	74	bgez \dest, \nomt
	75	mfc0 \dest, CP0_CONFIG, 2
	76	bgez \dest, \nomt
	77	mfc0 \dest, CP0_CONFIG, 3
	78	bgez \dest, \nomt
	79	mfc0 \dest, CP0_CONFIG, 4
	80	bgez \dest, \nomt
	81	mfc0 \dest, CP0_CONFIG, 5
	82	andi \dest, \dest, MIPS_CONF5_VP
	83	beqz \dest, \nomt
	84	nop
	85	.endm
	86
f12401d7 PB	87	/* Calculate an uncached address for the CM GCRs */
	88	.macro cmgcrb dest
	89	.set push
	90	.set noat
	91	MFC0 $1, CP0_CMGCRBASE
	92	PTR_SLL $1, $1, 4
	93	PTR_LI \dest, UNCAC_BASE
	94	PTR_ADDU \dest, \dest, $1
	95	.set pop
	96	.endm
	97
0ee958e1 PB	98	.section .text.cps-vec
0ee958e1 PB	99	.balign 0x1000
0ee958e1 PB	100
	101	LEAF(mips_cps_core_entry)
	102	/*
946db173 PB	103	* These first 4 bytes will be patched by cps_smp_setup to load the
946db173 PB	104	* CCA to use into register s0.
0ee958e1	105	*/
0155a065	106	.word 0
0ee958e1 PB	107
	108	/* Check whether we're here due to an NMI */
	109	mfc0 k0, CP0_STATUS
	110	and k0, k0, ST0_NMI
	111	beqz k0, not_nmi
	112	nop
	113
	114	/* This is an NMI */
81a02e34	115	PTR_LA k0, nmi_handler
0ee958e1 PB	116	jr k0
	117	nop
	118
	119	not_nmi:
	120	/* Setup Cause */
	121	li t0, CAUSEF_IV
	122	mtc0 t0, CP0_CAUSE
	123
	124	/* Setup Status */
8fe2c547	125	li t0, ST0_CU1 \| ST0_CU0 \| ST0_BEV \| STATUS_BITDEPS
0ee958e1 PB	126	mtc0 t0, CP0_STATUS
0ee958e1 PB	127
87a70bcd PB	128	/* Skip cache & coherence setup if we're already coherent */
	129	cmgcrb v1
	130	lw s7, GCR_CL_COHERENCE_OFS(v1)
	131	bnez s7, 1f
	132	nop
	133
3dbc9971 PB	134	/* Initialize the L1 caches */
3dbc9971 PB	135	jal mips_cps_cache_init
0ee958e1	136	nop
0ee958e1	137
87a70bcd PB	138	/* Enter the coherent domain */
	139	li t0, 0xff
	140	sw t0, GCR_CL_COHERENCE_OFS(v1)
	141	ehb
	142
0155a065	143	/* Set Kseg0 CCA to that in s0 */
87a70bcd	144	1: mfc0 t0, CP0_CONFIG
0ee958e1	145	ori t0, 0x7
0155a065 PB	146	xori t0, 0x7
0155a065 PB	147	or t0, t0, s0
0ee958e1 PB	148	mtc0 t0, CP0_CONFIG
	149	ehb
	150
0ee958e1	151	/* Jump to kseg0 */
81a02e34	152	PTR_LA t0, 1f
0ee958e1 PB	153	jr t0
	154	nop
	155
245a7868	156	/*
87a70bcd PB	157	* We're up, cached & coherent. Perform any EVA initialization necessary
87a70bcd PB	158	* before we access memory.
245a7868	159	*/
87a70bcd	160	1: eva_init
6521d9a4	161
f12401d7 PB	162	/* Retrieve boot configuration pointers */
	163	jal mips_cps_get_bootcfg
	164	nop
	165
87a70bcd PB	166	/* Skip core-level init if we started up coherent */
	167	bnez s7, 1f
	168	nop
	169
	170	/* Perform any further required core-level initialisation */
	171	jal mips_cps_core_init
	172	nop
	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	*/
f12401d7	178	move a1, t9
245a7868	179	jal mips_cps_boot_vpes
f12401d7	180	move a0, v0
0ee958e1 PB	181
0ee958e1 PB	182	/* Off we go! */
87a70bcd	183	1: PTR_L t1, VPEBOOTCFG_PC(v1)
f12401d7 PB	184	PTR_L gp, VPEBOOTCFG_GP(v1)
f12401d7 PB	185	PTR_L sp, VPEBOOTCFG_SP(v1)
0ee958e1 PB	186	jr t1
	187	nop
	188	END(mips_cps_core_entry)
	189
	190	.org 0x200
	191	LEAF(excep_tlbfill)
609cf6f2	192	DUMP_EXCEP("TLB Fill")
0ee958e1 PB	193	b .
	194	nop
	195	END(excep_tlbfill)
	196
	197	.org 0x280
	198	LEAF(excep_xtlbfill)
609cf6f2	199	DUMP_EXCEP("XTLB Fill")
0ee958e1 PB	200	b .
	201	nop
	202	END(excep_xtlbfill)
	203
	204	.org 0x300
	205	LEAF(excep_cache)
609cf6f2	206	DUMP_EXCEP("Cache")
0ee958e1 PB	207	b .
	208	nop
	209	END(excep_cache)
	210
	211	.org 0x380
	212	LEAF(excep_genex)
609cf6f2	213	DUMP_EXCEP("General")
0ee958e1 PB	214	b .
	215	nop
	216	END(excep_genex)
	217
	218	.org 0x400
	219	LEAF(excep_intex)
609cf6f2	220	DUMP_EXCEP("Interrupt")
0ee958e1 PB	221	b .
	222	nop
	223	END(excep_intex)
	224
	225	.org 0x480
	226	LEAF(excep_ejtag)
81a02e34	227	PTR_LA k0, ejtag_debug_handler
0ee958e1 PB	228	jr k0
	229	nop
	230	END(excep_ejtag)
245a7868 PB	231
245a7868 PB	232	LEAF(mips_cps_core_init)
7a63076d	233	#ifdef CONFIG_MIPS_MT_SMP
245a7868 PB	234	/* Check that the core implements the MT ASE */
245a7868 PB	235	has_mt t0, 3f
245a7868 PB	236
245a7868 PB	237	.set push
17278a91	238	.set MIPS_ISA_LEVEL_RAW
245a7868 PB	239	.set mt
	240
	241	/* Only allow 1 TC per VPE to execute... */
	242	dmt
	243
	244	/* ...and for the moment only 1 VPE */
	245	dvpe
81a02e34	246	PTR_LA t1, 1f
245a7868 PB	247	jr.hb t1
	248	nop
	249
	250	/* Enter VPE configuration state */
	251	1: mfc0 t0, CP0_MVPCONTROL
	252	ori t0, t0, MVPCONTROL_VPC
	253	mtc0 t0, CP0_MVPCONTROL
	254
	255	/* Retrieve the number of VPEs within the core */
	256	mfc0 t0, CP0_MVPCONF0
	257	srl t0, t0, MVPCONF0_PVPE_SHIFT
	258	andi t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
0586ac75	259	addiu ta3, t0, 1
245a7868 PB	260
	261	/* If there's only 1, we're done */
	262	beqz t0, 2f
	263	nop
	264
	265	/* Loop through each VPE within this core */
0586ac75	266	li ta1, 1
245a7868 PB	267
245a7868 PB	268	1: /* Operate on the appropriate TC */
0586ac75	269	mtc0 ta1, CP0_VPECONTROL
245a7868 PB	270	ehb
	271
	272	/* Bind TC to VPE (1:1 TC:VPE mapping) */
0586ac75	273	mttc0 ta1, CP0_TCBIND
245a7868 PB	274
	275	/* Set exclusive TC, non-active, master */
	276	li t0, VPECONF0_MVP
0586ac75	277	sll t1, ta1, VPECONF0_XTC_SHIFT
245a7868 PB	278	or t0, t0, t1
	279	mttc0 t0, CP0_VPECONF0
	280
	281	/* Set TC non-active, non-allocatable */
	282	mttc0 zero, CP0_TCSTATUS
	283
	284	/* Set TC halted */
	285	li t0, TCHALT_H
	286	mttc0 t0, CP0_TCHALT
	287
	288	/* Next VPE */
0586ac75 MC	289	addiu ta1, ta1, 1
0586ac75 MC	290	slt t0, ta1, ta3
245a7868 PB	291	bnez t0, 1b
	292	nop
	293
	294	/* Leave VPE configuration state */
	295	2: mfc0 t0, CP0_MVPCONTROL
	296	xori t0, t0, MVPCONTROL_VPC
	297	mtc0 t0, CP0_MVPCONTROL
	298
	299	3: .set pop
	300	#endif
	301	jr ra
	302	nop
	303	END(mips_cps_core_init)
	304
f12401d7 PB	305	/**
	306	* mips_cps_get_bootcfg() - retrieve boot configuration pointers
	307	*
	308	* Returns: pointer to struct core_boot_config in v0, pointer to
	309	* struct vpe_boot_config in v1, VPE ID in t9
	310	*/
	311	LEAF(mips_cps_get_bootcfg)
245a7868	312	/* Calculate a pointer to this cores struct core_boot_config */
f12401d7	313	cmgcrb t0
90996511	314	lw t0, GCR_CL_ID_OFS(t0)
245a7868 PB	315	li t1, COREBOOTCFG_SIZE
245a7868 PB	316	mul t0, t0, t1
81a02e34	317	PTR_LA t1, mips_cps_core_bootcfg
b677bc03	318	PTR_L t1, 0(t1)
f12401d7	319	PTR_ADDU v0, t0, t1
245a7868 PB	320
245a7868 PB	321	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
1e5fb282	322	li t9, 0
5a3e7c02 PB	323	#if defined(CONFIG_CPU_MIPSR6)
	324	has_vp ta2, 1f
	325
	326	/*
	327	* Assume non-contiguous numbering. Perhaps some day we'll need
	328	* to handle contiguous VP numbering, but no such systems yet
	329	* exist.
	330	*/
15e6529f PB	331	mfc0 t9, CP0_GLOBALNUMBER
15e6529f PB	332	andi t9, t9, MIPS_GLOBALNUMBER_VP
5a3e7c02	333	#elif defined(CONFIG_MIPS_MT_SMP)
0586ac75	334	has_mt ta2, 1f
245a7868 PB	335
	336	/* Find the number of VPEs present in the core */
	337	mfc0 t1, CP0_MVPCONF0
	338	srl t1, t1, MVPCONF0_PVPE_SHIFT
	339	andi t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
acac4108	340	addiu t1, t1, 1
245a7868 PB	341
	342	/* Calculate a mask for the VPE ID from EBase.CPUNum */
	343	clz t1, t1
	344	li t2, 31
	345	subu t1, t2, t1
	346	li t2, 1
	347	sll t1, t2, t1
	348	addiu t1, t1, -1
	349
	350	/* Retrieve the VPE ID from EBase.CPUNum */
	351	mfc0 t9, $15, 1
	352	and t9, t9, t1
a5b0f6db	353	#endif
245a7868 PB	354
	355	1: /* Calculate a pointer to this VPEs struct vpe_boot_config */
	356	li t1, VPEBOOTCFG_SIZE
f12401d7 PB	357	mul v1, t9, t1
	358	PTR_L ta3, COREBOOTCFG_VPECONFIG(v0)
	359	PTR_ADDU v1, v1, ta3
245a7868	360
245a7868 PB	361	jr ra
245a7868 PB	362	nop
f12401d7 PB	363	END(mips_cps_get_bootcfg)
	364
	365	LEAF(mips_cps_boot_vpes)
fb2155e3	366	lw ta2, COREBOOTCFG_VPEMASK(a0)
f12401d7 PB	367	PTR_L ta3, COREBOOTCFG_VPECONFIG(a0)
f12401d7 PB	368
5a3e7c02 PB	369	#if defined(CONFIG_CPU_MIPSR6)
	370
	371	has_vp t0, 5f
	372
	373	/* Find base address of CPC */
	374	cmgcrb t3
	375	PTR_L t1, GCR_CPC_BASE_OFS(t3)
	376	PTR_LI t2, ~0x7fff
	377	and t1, t1, t2
	378	PTR_LI t2, UNCAC_BASE
	379	PTR_ADD t1, t1, t2
	380
fa7a3b4a	381	/* Start any other VPs that ought to be running */
5a3e7c02	382	PTR_S ta2, CPC_CL_VC_RUN_OFS(t1)
fa7a3b4a PB	383
	384	/* Ensure this VP stops running if it shouldn't be */
	385	not ta2
	386	PTR_S ta2, CPC_CL_VC_STOP_OFS(t1)
5a3e7c02 PB	387	ehb
	388
	389	#elif defined(CONFIG_MIPS_MT)
245a7868 PB	390
245a7868 PB	391	.set push
17278a91	392	.set MIPS_ISA_LEVEL_RAW
245a7868 PB	393	.set mt
245a7868 PB	394
f12401d7 PB	395	/* If the core doesn't support MT then return */
	396	has_mt t0, 5f
	397
	398	/* Enter VPE configuration state */
245a7868	399	dvpe
81a02e34	400	PTR_LA t1, 1f
245a7868 PB	401	jr.hb t1
	402	nop
	403	1: mfc0 t1, CP0_MVPCONTROL
	404	ori t1, t1, MVPCONTROL_VPC
	405	mtc0 t1, CP0_MVPCONTROL
	406	ehb
	407
	408	/* Loop through each VPE */
0586ac75 MC	409	move t8, ta2
0586ac75 MC	410	li ta1, 0
245a7868 PB	411
245a7868 PB	412	/* Check whether the VPE should be running. If not, skip it */
0586ac75	413	1: andi t0, ta2, 1
245a7868 PB	414	beqz t0, 2f
	415	nop
	416
	417	/* Operate on the appropriate TC */
	418	mfc0 t0, CP0_VPECONTROL
	419	ori t0, t0, VPECONTROL_TARGTC
	420	xori t0, t0, VPECONTROL_TARGTC
0586ac75	421	or t0, t0, ta1
245a7868 PB	422	mtc0 t0, CP0_VPECONTROL
	423	ehb
	424
	425	/* Skip the VPE if its TC is not halted */
	426	mftc0 t0, CP0_TCHALT
	427	beqz t0, 2f
	428	nop
	429
	430	/* Calculate a pointer to the VPEs struct vpe_boot_config */
	431	li t0, VPEBOOTCFG_SIZE
0586ac75 MC	432	mul t0, t0, ta1
0586ac75 MC	433	addu t0, t0, ta3
245a7868 PB	434
	435	/* Set the TC restart PC */
	436	lw t1, VPEBOOTCFG_PC(t0)
	437	mttc0 t1, CP0_TCRESTART
	438
	439	/* Set the TC stack pointer */
	440	lw t1, VPEBOOTCFG_SP(t0)
	441	mttgpr t1, sp
	442
	443	/* Set the TC global pointer */
	444	lw t1, VPEBOOTCFG_GP(t0)
	445	mttgpr t1, gp
	446
	447	/* Copy config from this VPE */
	448	mfc0 t0, CP0_CONFIG
	449	mttc0 t0, CP0_CONFIG
	450
63a8802f MR	451	/*
	452	* Copy the EVA config from this VPE if the CPU supports it.
	453	* CONFIG3 must exist to be running MT startup - just read it.
	454	*/
	455	mfc0 t0, CP0_CONFIG, 3
	456	and t0, t0, MIPS_CONF3_SC
	457	beqz t0, 3f
	458	nop
	459	mfc0 t0, CP0_SEGCTL0
	460	mttc0 t0, CP0_SEGCTL0
	461	mfc0 t0, CP0_SEGCTL1
	462	mttc0 t0, CP0_SEGCTL1
	463	mfc0 t0, CP0_SEGCTL2
	464	mttc0 t0, CP0_SEGCTL2
	465	3:
245a7868 PB	466	/* Ensure no software interrupts are pending */
	467	mttc0 zero, CP0_CAUSE
	468	mttc0 zero, CP0_STATUS
	469
	470	/* Set TC active, not interrupt exempt */
	471	mftc0 t0, CP0_TCSTATUS
	472	li t1, ~TCSTATUS_IXMT
	473	and t0, t0, t1
	474	ori t0, t0, TCSTATUS_A
	475	mttc0 t0, CP0_TCSTATUS
	476
	477	/* Clear the TC halt bit */
	478	mttc0 zero, CP0_TCHALT
	479
	480	/* Set VPE active */
	481	mftc0 t0, CP0_VPECONF0
	482	ori t0, t0, VPECONF0_VPA
	483	mttc0 t0, CP0_VPECONF0
	484
	485	/* Next VPE */
0586ac75 MC	486	2: srl ta2, ta2, 1
	487	addiu ta1, ta1, 1
	488	bnez ta2, 1b
245a7868 PB	489	nop
	490
	491	/* Leave VPE configuration state */
	492	mfc0 t1, CP0_MVPCONTROL
	493	xori t1, t1, MVPCONTROL_VPC
	494	mtc0 t1, CP0_MVPCONTROL
	495	ehb
	496	evpe
	497
	498	/* Check whether this VPE is meant to be running */
	499	li t0, 1
f12401d7	500	sll t0, t0, a1
245a7868 PB	501	and t0, t0, t8
	502	bnez t0, 2f
	503	nop
	504
	505	/* This VPE should be offline, halt the TC */
	506	li t0, TCHALT_H
	507	mtc0 t0, CP0_TCHALT
81a02e34	508	PTR_LA t0, 1f
245a7868 PB	509	1: jr.hb t0
	510	nop
	511
	512	2: .set pop
	513
7a63076d	514	#endif /* CONFIG_MIPS_MT_SMP */
245a7868 PB	515
245a7868 PB	516	/* Return */
f12401d7	517	5: jr ra
245a7868 PB	518	nop
245a7868 PB	519	END(mips_cps_boot_vpes)
3179d37e	520
3dbc9971 PB	521	LEAF(mips_cps_cache_init)
	522	/*
	523	* Clear the bits used to index the caches. Note that the architecture
	524	* dictates that writing to any of TagLo or TagHi selects 0 or 2 should
	525	* be valid for all MIPS32 CPUs, even those for which said writes are
	526	* unnecessary.
	527	*/
	528	mtc0 zero, CP0_TAGLO, 0
	529	mtc0 zero, CP0_TAGHI, 0
	530	mtc0 zero, CP0_TAGLO, 2
	531	mtc0 zero, CP0_TAGHI, 2
	532	ehb
	533
	534	/* Primary cache configuration is indicated by Config1 */
	535	mfc0 v0, CP0_CONFIG, 1
	536
	537	/* Detect I-cache line size */
	538	_EXT t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
	539	beqz t0, icache_done
	540	li t1, 2
	541	sllv t0, t1, t0
	542
	543	/* Detect I-cache size */
	544	_EXT t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
	545	xori t2, t1, 0x7
	546	beqz t2, 1f
	547	li t3, 32
	548	addiu t1, t1, 1
	549	sllv t1, t3, t1
	550	1: /* At this point t1 == I-cache sets per way */
	551	_EXT t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
	552	addiu t2, t2, 1
	553	mul t1, t1, t0
	554	mul t1, t1, t2
	555
	556	li a0, CKSEG0
	557	PTR_ADD a1, a0, t1
	558	1: cache Index_Store_Tag_I, 0(a0)
	559	PTR_ADD a0, a0, t0
	560	bne a0, a1, 1b
	561	nop
	562	icache_done:
	563
	564	/* Detect D-cache line size */
	565	_EXT t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
	566	beqz t0, dcache_done
	567	li t1, 2
	568	sllv t0, t1, t0
	569
	570	/* Detect D-cache size */
	571	_EXT t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
	572	xori t2, t1, 0x7
	573	beqz t2, 1f
	574	li t3, 32
	575	addiu t1, t1, 1
	576	sllv t1, t3, t1
	577	1: /* At this point t1 == D-cache sets per way */
	578	_EXT t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
	579	addiu t2, t2, 1
	580	mul t1, t1, t0
	581	mul t1, t1, t2
	582
	583	li a0, CKSEG0
	584	PTR_ADDU a1, a0, t1
585	PTR_SUBU a1, a1, t0
586	1: cache Index_Store_Tag_D, 0(a0)
587	bne a0, a1, 1b
588	PTR_ADD a0, a0, t0
589	dcache_done:
590
591	jr ra
592	nop
593	END(mips_cps_cache_init)
594
3179d37e PB	595	#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)
	596
	597	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
	598	.macro psstate dest
	599	.set push
	600	.set noat
	601	lw $1, TI_CPU(gp)
	602	sll $1, $1, LONGLOG
81a02e34	603	PTR_LA \dest, __per_cpu_offset
3179d37e PB	604	addu $1, $1, \dest
3179d37e PB	605	lw $1, 0($1)
81a02e34	606	PTR_LA \dest, cps_cpu_state
3179d37e PB	607	addu \dest, \dest, $1
	608	.set pop
	609	.endm
	610
	611	LEAF(mips_cps_pm_save)
	612	/* Save CPU state */
	613	SUSPEND_SAVE_REGS
	614	psstate t1
	615	SUSPEND_SAVE_STATIC
	616	jr v0
	617	nop
	618	END(mips_cps_pm_save)
	619
	620	LEAF(mips_cps_pm_restore)
	621	/* Restore CPU state */
	622	psstate t1
	623	RESUME_RESTORE_STATIC
	624	RESUME_RESTORE_REGS_RETURN
	625	END(mips_cps_pm_restore)
	626
	627	#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */