[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

#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

	/* 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)
	DUMP_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	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
#ifdef 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)
	PTR_L	ta2, COREBOOTCFG_VPEMASK(a0)
	PTR_L	ta3, COREBOOTCFG_VPECONFIG(a0)

#ifdef CONFIG_MIPS_MT

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

	/* 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	/*
	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
8fe2c547 PB	28	#ifdef CONFIG_64BIT
	29	# define STATUS_BITDEPS ST0_KX
	30	#else
	31	# define STATUS_BITDEPS 0
	32	#endif
	33
609cf6f2 PB	34	#ifdef CONFIG_MIPS_CPS_NS16550
	35
	36	#define DUMP_EXCEP(name) \
	37	PTR_LA a0, 8f; \
	38	jal mips_cps_bev_dump; \
	39	nop; \
	40	TEXT(name)
	41
	42	#else /* !CONFIG_MIPS_CPS_NS16550 */
	43
	44	#define DUMP_EXCEP(name)
	45
	46	#endif /* !CONFIG_MIPS_CPS_NS16550 */
	47
245a7868 PB	48	/*
	49	* Set dest to non-zero if the core supports the MT ASE, else zero. If
	50	* MT is not supported then branch to nomt.
	51	*/
	52	.macro has_mt dest, nomt
192fbc20	53	mfc0 \dest, CP0_CONFIG, 1
245a7868 PB	54	bgez \dest, \nomt
	55	mfc0 \dest, CP0_CONFIG, 2
	56	bgez \dest, \nomt
	57	mfc0 \dest, CP0_CONFIG, 3
	58	andi \dest, \dest, MIPS_CONF3_MT
	59	beqz \dest, \nomt
1e5fb282	60	nop
245a7868	61	.endm
0ee958e1	62
f12401d7 PB	63	/* Calculate an uncached address for the CM GCRs */
	64	.macro cmgcrb dest
	65	.set push
	66	.set noat
	67	MFC0 $1, CP0_CMGCRBASE
	68	PTR_SLL $1, $1, 4
	69	PTR_LI \dest, UNCAC_BASE
	70	PTR_ADDU \dest, \dest, $1
	71	.set pop
	72	.endm
	73
0ee958e1 PB	74	.section .text.cps-vec
0ee958e1 PB	75	.balign 0x1000
0ee958e1 PB	76
	77	LEAF(mips_cps_core_entry)
	78	/*
946db173 PB	79	* These first 4 bytes will be patched by cps_smp_setup to load the
946db173 PB	80	* CCA to use into register s0.
0ee958e1	81	*/
0155a065	82	.word 0
0ee958e1 PB	83
	84	/* Check whether we're here due to an NMI */
	85	mfc0 k0, CP0_STATUS
	86	and k0, k0, ST0_NMI
	87	beqz k0, not_nmi
	88	nop
	89
	90	/* This is an NMI */
81a02e34	91	PTR_LA k0, nmi_handler
0ee958e1 PB	92	jr k0
	93	nop
	94
	95	not_nmi:
	96	/* Setup Cause */
	97	li t0, CAUSEF_IV
	98	mtc0 t0, CP0_CAUSE
	99
	100	/* Setup Status */
8fe2c547	101	li t0, ST0_CU1 \| ST0_CU0 \| ST0_BEV \| STATUS_BITDEPS
0ee958e1 PB	102	mtc0 t0, CP0_STATUS
0ee958e1 PB	103
87a70bcd PB	104	/* Skip cache & coherence setup if we're already coherent */
	105	cmgcrb v1
	106	lw s7, GCR_CL_COHERENCE_OFS(v1)
	107	bnez s7, 1f
	108	nop
	109
3dbc9971 PB	110	/* Initialize the L1 caches */
3dbc9971 PB	111	jal mips_cps_cache_init
0ee958e1	112	nop
0ee958e1	113
87a70bcd PB	114	/* Enter the coherent domain */
	115	li t0, 0xff
	116	sw t0, GCR_CL_COHERENCE_OFS(v1)
	117	ehb
	118
0155a065	119	/* Set Kseg0 CCA to that in s0 */
87a70bcd	120	1: mfc0 t0, CP0_CONFIG
0ee958e1	121	ori t0, 0x7
0155a065 PB	122	xori t0, 0x7
0155a065 PB	123	or t0, t0, s0
0ee958e1 PB	124	mtc0 t0, CP0_CONFIG
	125	ehb
	126
0ee958e1	127	/* Jump to kseg0 */
81a02e34	128	PTR_LA t0, 1f
0ee958e1 PB	129	jr t0
	130	nop
	131
245a7868	132	/*
87a70bcd PB	133	* We're up, cached & coherent. Perform any EVA initialization necessary
87a70bcd PB	134	* before we access memory.
245a7868	135	*/
87a70bcd	136	1: eva_init
6521d9a4	137
f12401d7 PB	138	/* Retrieve boot configuration pointers */
	139	jal mips_cps_get_bootcfg
	140	nop
	141
87a70bcd PB	142	/* Skip core-level init if we started up coherent */
	143	bnez s7, 1f
	144	nop
	145
	146	/* Perform any further required core-level initialisation */
	147	jal mips_cps_core_init
	148	nop
	149
0ee958e1	150	/*
245a7868 PB	151	* Boot any other VPEs within this core that should be online, and
245a7868 PB	152	* deactivate this VPE if it should be offline.
0ee958e1	153	*/
f12401d7	154	move a1, t9
245a7868	155	jal mips_cps_boot_vpes
f12401d7	156	move a0, v0
0ee958e1 PB	157
0ee958e1 PB	158	/* Off we go! */
87a70bcd	159	1: PTR_L t1, VPEBOOTCFG_PC(v1)
f12401d7 PB	160	PTR_L gp, VPEBOOTCFG_GP(v1)
f12401d7 PB	161	PTR_L sp, VPEBOOTCFG_SP(v1)
0ee958e1 PB	162	jr t1
	163	nop
	164	END(mips_cps_core_entry)
	165
	166	.org 0x200
	167	LEAF(excep_tlbfill)
609cf6f2	168	DUMP_EXCEP("TLB Fill")
0ee958e1 PB	169	b .
	170	nop
	171	END(excep_tlbfill)
	172
	173	.org 0x280
	174	LEAF(excep_xtlbfill)
609cf6f2	175	DUMP_EXCEP("XTLB Fill")
0ee958e1 PB	176	b .
	177	nop
	178	END(excep_xtlbfill)
	179
	180	.org 0x300
	181	LEAF(excep_cache)
609cf6f2	182	DUMP_EXCEP("Cache")
0ee958e1 PB	183	b .
	184	nop
	185	END(excep_cache)
	186
	187	.org 0x380
	188	LEAF(excep_genex)
609cf6f2	189	DUMP_EXCEP("General")
0ee958e1 PB	190	b .
	191	nop
	192	END(excep_genex)
	193
	194	.org 0x400
	195	LEAF(excep_intex)
609cf6f2	196	DUMP_EXCEP("Interrupt")
0ee958e1 PB	197	b .
	198	nop
	199	END(excep_intex)
	200
	201	.org 0x480
	202	LEAF(excep_ejtag)
609cf6f2	203	DUMP_EXCEP("EJTAG")
81a02e34	204	PTR_LA k0, ejtag_debug_handler
0ee958e1 PB	205	jr k0
	206	nop
	207	END(excep_ejtag)
245a7868 PB	208
245a7868 PB	209	LEAF(mips_cps_core_init)
7a63076d	210	#ifdef CONFIG_MIPS_MT_SMP
245a7868 PB	211	/* Check that the core implements the MT ASE */
245a7868 PB	212	has_mt t0, 3f
245a7868 PB	213
	214	.set push
	215	.set mt
	216
	217	/* Only allow 1 TC per VPE to execute... */
	218	dmt
	219
	220	/* ...and for the moment only 1 VPE */
	221	dvpe
81a02e34	222	PTR_LA t1, 1f
245a7868 PB	223	jr.hb t1
	224	nop
	225
	226	/* Enter VPE configuration state */
	227	1: mfc0 t0, CP0_MVPCONTROL
	228	ori t0, t0, MVPCONTROL_VPC
	229	mtc0 t0, CP0_MVPCONTROL
	230
	231	/* Retrieve the number of VPEs within the core */
	232	mfc0 t0, CP0_MVPCONF0
	233	srl t0, t0, MVPCONF0_PVPE_SHIFT
	234	andi t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
0586ac75	235	addiu ta3, t0, 1
245a7868 PB	236
	237	/* If there's only 1, we're done */
	238	beqz t0, 2f
	239	nop
	240
	241	/* Loop through each VPE within this core */
0586ac75	242	li ta1, 1
245a7868 PB	243
245a7868 PB	244	1: /* Operate on the appropriate TC */
0586ac75	245	mtc0 ta1, CP0_VPECONTROL
245a7868 PB	246	ehb
	247
	248	/* Bind TC to VPE (1:1 TC:VPE mapping) */
0586ac75	249	mttc0 ta1, CP0_TCBIND
245a7868 PB	250
	251	/* Set exclusive TC, non-active, master */
	252	li t0, VPECONF0_MVP
0586ac75	253	sll t1, ta1, VPECONF0_XTC_SHIFT
245a7868 PB	254	or t0, t0, t1
	255	mttc0 t0, CP0_VPECONF0
	256
	257	/* Set TC non-active, non-allocatable */
	258	mttc0 zero, CP0_TCSTATUS
	259
	260	/* Set TC halted */
	261	li t0, TCHALT_H
	262	mttc0 t0, CP0_TCHALT
	263
	264	/* Next VPE */
0586ac75 MC	265	addiu ta1, ta1, 1
0586ac75 MC	266	slt t0, ta1, ta3
245a7868 PB	267	bnez t0, 1b
	268	nop
	269
	270	/* Leave VPE configuration state */
	271	2: mfc0 t0, CP0_MVPCONTROL
	272	xori t0, t0, MVPCONTROL_VPC
	273	mtc0 t0, CP0_MVPCONTROL
	274
	275	3: .set pop
	276	#endif
	277	jr ra
	278	nop
	279	END(mips_cps_core_init)
	280
f12401d7 PB	281	/**
	282	* mips_cps_get_bootcfg() - retrieve boot configuration pointers
	283	*
	284	* Returns: pointer to struct core_boot_config in v0, pointer to
	285	* struct vpe_boot_config in v1, VPE ID in t9
	286	*/
	287	LEAF(mips_cps_get_bootcfg)
245a7868	288	/* Calculate a pointer to this cores struct core_boot_config */
f12401d7	289	cmgcrb t0
90996511	290	lw t0, GCR_CL_ID_OFS(t0)
245a7868 PB	291	li t1, COREBOOTCFG_SIZE
245a7868 PB	292	mul t0, t0, t1
81a02e34	293	PTR_LA t1, mips_cps_core_bootcfg
b677bc03	294	PTR_L t1, 0(t1)
f12401d7	295	PTR_ADDU v0, t0, t1
245a7868 PB	296
245a7868 PB	297	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
1e5fb282	298	li t9, 0
7a63076d	299	#ifdef CONFIG_MIPS_MT_SMP
0586ac75	300	has_mt ta2, 1f
245a7868 PB	301
	302	/* Find the number of VPEs present in the core */
	303	mfc0 t1, CP0_MVPCONF0
	304	srl t1, t1, MVPCONF0_PVPE_SHIFT
	305	andi t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
acac4108	306	addiu t1, t1, 1
245a7868 PB	307
	308	/* Calculate a mask for the VPE ID from EBase.CPUNum */
	309	clz t1, t1
	310	li t2, 31
	311	subu t1, t2, t1
	312	li t2, 1
	313	sll t1, t2, t1
	314	addiu t1, t1, -1
	315
	316	/* Retrieve the VPE ID from EBase.CPUNum */
	317	mfc0 t9, $15, 1
	318	and t9, t9, t1
a5b0f6db	319	#endif
245a7868 PB	320
	321	1: /* Calculate a pointer to this VPEs struct vpe_boot_config */
	322	li t1, VPEBOOTCFG_SIZE
f12401d7 PB	323	mul v1, t9, t1
	324	PTR_L ta3, COREBOOTCFG_VPECONFIG(v0)
	325	PTR_ADDU v1, v1, ta3
245a7868	326
245a7868 PB	327	jr ra
245a7868 PB	328	nop
f12401d7 PB	329	END(mips_cps_get_bootcfg)
	330
	331	LEAF(mips_cps_boot_vpes)
	332	PTR_L ta2, COREBOOTCFG_VPEMASK(a0)
	333	PTR_L ta3, COREBOOTCFG_VPECONFIG(a0)
	334
	335	#ifdef CONFIG_MIPS_MT
245a7868 PB	336
	337	.set push
	338	.set mt
	339
f12401d7 PB	340	/* If the core doesn't support MT then return */
	341	has_mt t0, 5f
	342
	343	/* Enter VPE configuration state */
245a7868	344	dvpe
81a02e34	345	PTR_LA t1, 1f
245a7868 PB	346	jr.hb t1
	347	nop
	348	1: mfc0 t1, CP0_MVPCONTROL
	349	ori t1, t1, MVPCONTROL_VPC
	350	mtc0 t1, CP0_MVPCONTROL
	351	ehb
	352
	353	/* Loop through each VPE */
0586ac75 MC	354	move t8, ta2
0586ac75 MC	355	li ta1, 0
245a7868 PB	356
245a7868 PB	357	/* Check whether the VPE should be running. If not, skip it */
0586ac75	358	1: andi t0, ta2, 1
245a7868 PB	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
0586ac75	366	or t0, t0, ta1
245a7868 PB	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
0586ac75 MC	377	mul t0, t0, ta1
0586ac75 MC	378	addu t0, t0, ta3
245a7868 PB	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 */
0586ac75 MC	416	2: srl ta2, ta2, 1
	417	addiu ta1, ta1, 1
	418	bnez ta2, 1b
245a7868 PB	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
f12401d7	430	sll t0, t0, a1
245a7868 PB	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
81a02e34	438	PTR_LA t0, 1f
245a7868 PB	439	1: jr.hb t0
	440	nop
	441
	442	2: .set pop
	443
7a63076d	444	#endif /* CONFIG_MIPS_MT_SMP */
245a7868 PB	445
245a7868 PB	446	/* Return */
f12401d7	447	5: jr ra
245a7868 PB	448	nop
245a7868 PB	449	END(mips_cps_boot_vpes)
3179d37e	450
3dbc9971 PB	451	LEAF(mips_cps_cache_init)
	452	/*
	453	* Clear the bits used to index the caches. Note that the architecture
	454	* dictates that writing to any of TagLo or TagHi selects 0 or 2 should
	455	* be valid for all MIPS32 CPUs, even those for which said writes are
	456	* unnecessary.
	457	*/
	458	mtc0 zero, CP0_TAGLO, 0
	459	mtc0 zero, CP0_TAGHI, 0
	460	mtc0 zero, CP0_TAGLO, 2
	461	mtc0 zero, CP0_TAGHI, 2
	462	ehb
	463
	464	/* Primary cache configuration is indicated by Config1 */
	465	mfc0 v0, CP0_CONFIG, 1
	466
	467	/* Detect I-cache line size */
	468	_EXT t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
	469	beqz t0, icache_done
	470	li t1, 2
	471	sllv t0, t1, t0
	472
	473	/* Detect I-cache size */
	474	_EXT t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
	475	xori t2, t1, 0x7
	476	beqz t2, 1f
	477	li t3, 32
	478	addiu t1, t1, 1
	479	sllv t1, t3, t1
	480	1: /* At this point t1 == I-cache sets per way */
	481	_EXT t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
	482	addiu t2, t2, 1
	483	mul t1, t1, t0
	484	mul t1, t1, t2
	485
	486	li a0, CKSEG0
	487	PTR_ADD a1, a0, t1
	488	1: cache Index_Store_Tag_I, 0(a0)
	489	PTR_ADD a0, a0, t0
	490	bne a0, a1, 1b
	491	nop
	492	icache_done:
	493
	494	/* Detect D-cache line size */
	495	_EXT t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
	496	beqz t0, dcache_done
	497	li t1, 2
	498	sllv t0, t1, t0
	499
	500	/* Detect D-cache size */
	501	_EXT t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
	502	xori t2, t1, 0x7
	503	beqz t2, 1f
	504	li t3, 32
	505	addiu t1, t1, 1
	506	sllv t1, t3, t1
	507	1: /* At this point t1 == D-cache sets per way */
	508	_EXT t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
	509	addiu t2, t2, 1
	510	mul t1, t1, t0
	511	mul t1, t1, t2
	512
	513	li a0, CKSEG0
	514	PTR_ADDU a1, a0, t1
515	PTR_SUBU a1, a1, t0
516	1: cache Index_Store_Tag_D, 0(a0)
517	bne a0, a1, 1b
518	PTR_ADD a0, a0, t0
519	dcache_done:
520
521	jr ra
522	nop
523	END(mips_cps_cache_init)
524
3179d37e PB	525	#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)
	526
	527	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
	528	.macro psstate dest
	529	.set push
	530	.set noat
	531	lw $1, TI_CPU(gp)
	532	sll $1, $1, LONGLOG
81a02e34	533	PTR_LA \dest, __per_cpu_offset
3179d37e PB	534	addu $1, $1, \dest
3179d37e PB	535	lw $1, 0($1)
81a02e34	536	PTR_LA \dest, cps_cpu_state
3179d37e PB	537	addu \dest, \dest, $1
	538	.set pop
	539	.endm
	540
	541	LEAF(mips_cps_pm_save)
	542	/* Save CPU state */
	543	SUSPEND_SAVE_REGS
	544	psstate t1
	545	SUSPEND_SAVE_STATIC
	546	jr v0
	547	nop
	548	END(mips_cps_pm_save)
	549
	550	LEAF(mips_cps_pm_restore)
	551	/* Restore CPU state */
	552	psstate t1
	553	RESUME_RESTORE_STATIC
	554	RESUME_RESTORE_REGS_RETURN
	555	END(mips_cps_pm_restore)
	556
	557	#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */