488 lines
9.0 KiB
ArmAsm
488 lines
9.0 KiB
ArmAsm
/*
|
|
* Copyright (C) 2013 Imagination Technologies
|
|
* Author: Paul Burton <paul.burton@imgtec.com>
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify it
|
|
* under the terms of the GNU General Public License as published by the
|
|
* Free Software Foundation; either version 2 of the License, or (at your
|
|
* option) any later version.
|
|
*/
|
|
|
|
#include <asm/addrspace.h>
|
|
#include <asm/asm.h>
|
|
#include <asm/asm-offsets.h>
|
|
#include <asm/asmmacro.h>
|
|
#include <asm/cacheops.h>
|
|
#include <asm/mipsregs.h>
|
|
#include <asm/mipsmtregs.h>
|
|
#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 */
|
|
la k0, nmi_handler
|
|
jr k0
|
|
nop
|
|
|
|
not_nmi:
|
|
/* Setup Cause */
|
|
li t0, CAUSEF_IV
|
|
mtc0 t0, CP0_CAUSE
|
|
|
|
/* Setup Status */
|
|
li t0, ST0_CU1 | ST0_CU0
|
|
mtc0 t0, CP0_STATUS
|
|
|
|
/*
|
|
* Clear the bits used to index the caches. Note that the architecture
|
|
* dictates that writing to any of TagLo or TagHi selects 0 or 2 should
|
|
* be valid for all MIPS32 CPUs, even those for which said writes are
|
|
* unnecessary.
|
|
*/
|
|
mtc0 zero, CP0_TAGLO, 0
|
|
mtc0 zero, CP0_TAGHI, 0
|
|
mtc0 zero, CP0_TAGLO, 2
|
|
mtc0 zero, CP0_TAGHI, 2
|
|
ehb
|
|
|
|
/* Primary cache configuration is indicated by Config1 */
|
|
mfc0 v0, CP0_CONFIG, 1
|
|
|
|
/* Detect I-cache line size */
|
|
_EXT t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
|
|
beqz t0, icache_done
|
|
li t1, 2
|
|
sllv t0, t1, t0
|
|
|
|
/* Detect I-cache size */
|
|
_EXT t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
|
|
xori t2, t1, 0x7
|
|
beqz t2, 1f
|
|
li t3, 32
|
|
addi t1, t1, 1
|
|
sllv t1, t3, t1
|
|
1: /* At this point t1 == I-cache sets per way */
|
|
_EXT t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
|
|
addi t2, t2, 1
|
|
mul t1, t1, t0
|
|
mul t1, t1, t2
|
|
|
|
li a0, KSEG0
|
|
add a1, a0, t1
|
|
1: cache Index_Store_Tag_I, 0(a0)
|
|
add a0, a0, t0
|
|
bne a0, a1, 1b
|
|
nop
|
|
icache_done:
|
|
|
|
/* Detect D-cache line size */
|
|
_EXT t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
|
|
beqz t0, dcache_done
|
|
li t1, 2
|
|
sllv t0, t1, t0
|
|
|
|
/* Detect D-cache size */
|
|
_EXT t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
|
|
xori t2, t1, 0x7
|
|
beqz t2, 1f
|
|
li t3, 32
|
|
addi t1, t1, 1
|
|
sllv t1, t3, t1
|
|
1: /* At this point t1 == D-cache sets per way */
|
|
_EXT t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
|
|
addi t2, t2, 1
|
|
mul t1, t1, t0
|
|
mul t1, t1, t2
|
|
|
|
li a0, KSEG0
|
|
addu a1, a0, t1
|
|
subu a1, a1, t0
|
|
1: cache Index_Store_Tag_D, 0(a0)
|
|
bne a0, a1, 1b
|
|
add a0, a0, t0
|
|
dcache_done:
|
|
|
|
/* Set Kseg0 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 */
|
|
la t0, 1f
|
|
jr t0
|
|
nop
|
|
|
|
/*
|
|
* We're up, cached & coherent. Perform any further required core-level
|
|
* initialisation.
|
|
*/
|
|
1: jal mips_cps_core_init
|
|
nop
|
|
|
|
/*
|
|
* Boot any other VPEs within this core that should be online, and
|
|
* deactivate this VPE if it should be offline.
|
|
*/
|
|
jal mips_cps_boot_vpes
|
|
nop
|
|
|
|
/* Off we go! */
|
|
lw t1, VPEBOOTCFG_PC(v0)
|
|
lw gp, VPEBOOTCFG_GP(v0)
|
|
lw sp, VPEBOOTCFG_SP(v0)
|
|
jr t1
|
|
nop
|
|
END(mips_cps_core_entry)
|
|
|
|
.org 0x200
|
|
LEAF(excep_tlbfill)
|
|
b .
|
|
nop
|
|
END(excep_tlbfill)
|
|
|
|
.org 0x280
|
|
LEAF(excep_xtlbfill)
|
|
b .
|
|
nop
|
|
END(excep_xtlbfill)
|
|
|
|
.org 0x300
|
|
LEAF(excep_cache)
|
|
b .
|
|
nop
|
|
END(excep_cache)
|
|
|
|
.org 0x380
|
|
LEAF(excep_genex)
|
|
b .
|
|
nop
|
|
END(excep_genex)
|
|
|
|
.org 0x400
|
|
LEAF(excep_intex)
|
|
b .
|
|
nop
|
|
END(excep_intex)
|
|
|
|
.org 0x480
|
|
LEAF(excep_ejtag)
|
|
la k0, ejtag_debug_handler
|
|
jr k0
|
|
nop
|
|
END(excep_ejtag)
|
|
|
|
LEAF(mips_cps_core_init)
|
|
#ifdef CONFIG_MIPS_MT
|
|
/* Check that the core implements the MT ASE */
|
|
has_mt t0, 3f
|
|
nop
|
|
|
|
.set push
|
|
.set mt
|
|
|
|
/* Only allow 1 TC per VPE to execute... */
|
|
dmt
|
|
|
|
/* ...and for the moment only 1 VPE */
|
|
dvpe
|
|
la t1, 1f
|
|
jr.hb t1
|
|
nop
|
|
|
|
/* Enter VPE configuration state */
|
|
1: mfc0 t0, CP0_MVPCONTROL
|
|
ori t0, t0, MVPCONTROL_VPC
|
|
mtc0 t0, CP0_MVPCONTROL
|
|
|
|
/* Retrieve the number of VPEs within the core */
|
|
mfc0 t0, CP0_MVPCONF0
|
|
srl t0, t0, MVPCONF0_PVPE_SHIFT
|
|
andi t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
|
|
addi t7, t0, 1
|
|
|
|
/* If there's only 1, we're done */
|
|
beqz t0, 2f
|
|
nop
|
|
|
|
/* Loop through each VPE within this core */
|
|
li t5, 1
|
|
|
|
1: /* Operate on the appropriate TC */
|
|
mtc0 t5, CP0_VPECONTROL
|
|
ehb
|
|
|
|
/* Bind TC to VPE (1:1 TC:VPE mapping) */
|
|
mttc0 t5, CP0_TCBIND
|
|
|
|
/* Set exclusive TC, non-active, master */
|
|
li t0, VPECONF0_MVP
|
|
sll t1, t5, VPECONF0_XTC_SHIFT
|
|
or t0, t0, t1
|
|
mttc0 t0, CP0_VPECONF0
|
|
|
|
/* Set TC non-active, non-allocatable */
|
|
mttc0 zero, CP0_TCSTATUS
|
|
|
|
/* Set TC halted */
|
|
li t0, TCHALT_H
|
|
mttc0 t0, CP0_TCHALT
|
|
|
|
/* Next VPE */
|
|
addi t5, t5, 1
|
|
slt t0, t5, t7
|
|
bnez t0, 1b
|
|
nop
|
|
|
|
/* Leave VPE configuration state */
|
|
2: mfc0 t0, CP0_MVPCONTROL
|
|
xori t0, t0, MVPCONTROL_VPC
|
|
mtc0 t0, CP0_MVPCONTROL
|
|
|
|
3: .set pop
|
|
#endif
|
|
jr ra
|
|
nop
|
|
END(mips_cps_core_init)
|
|
|
|
LEAF(mips_cps_boot_vpes)
|
|
/* Retrieve CM base address */
|
|
la t0, mips_cm_base
|
|
lw t0, 0(t0)
|
|
|
|
/* Calculate a pointer to this cores struct core_boot_config */
|
|
lw t0, GCR_CL_ID_OFS(t0)
|
|
li t1, COREBOOTCFG_SIZE
|
|
mul t0, t0, t1
|
|
la t1, mips_cps_core_bootcfg
|
|
lw t1, 0(t1)
|
|
addu t0, t0, t1
|
|
|
|
/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
|
|
has_mt t6, 1f
|
|
li t9, 0
|
|
|
|
/* Find the number of VPEs present in the core */
|
|
mfc0 t1, CP0_MVPCONF0
|
|
srl t1, t1, MVPCONF0_PVPE_SHIFT
|
|
andi t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
|
|
addi t1, t1, 1
|
|
|
|
/* Calculate a mask for the VPE ID from EBase.CPUNum */
|
|
clz t1, t1
|
|
li t2, 31
|
|
subu t1, t2, t1
|
|
li t2, 1
|
|
sll t1, t2, t1
|
|
addiu t1, t1, -1
|
|
|
|
/* Retrieve the VPE ID from EBase.CPUNum */
|
|
mfc0 t9, $15, 1
|
|
and t9, t9, t1
|
|
|
|
1: /* Calculate a pointer to this VPEs struct vpe_boot_config */
|
|
li t1, VPEBOOTCFG_SIZE
|
|
mul v0, t9, t1
|
|
lw t7, COREBOOTCFG_VPECONFIG(t0)
|
|
addu v0, v0, t7
|
|
|
|
#ifdef CONFIG_MIPS_MT
|
|
|
|
/* If the core doesn't support MT then return */
|
|
bnez t6, 1f
|
|
nop
|
|
jr ra
|
|
nop
|
|
|
|
.set push
|
|
.set mt
|
|
|
|
1: /* Enter VPE configuration state */
|
|
dvpe
|
|
la t1, 1f
|
|
jr.hb t1
|
|
nop
|
|
1: mfc0 t1, CP0_MVPCONTROL
|
|
ori t1, t1, MVPCONTROL_VPC
|
|
mtc0 t1, CP0_MVPCONTROL
|
|
ehb
|
|
|
|
/* Loop through each VPE */
|
|
lw t6, COREBOOTCFG_VPEMASK(t0)
|
|
move t8, t6
|
|
li t5, 0
|
|
|
|
/* Check whether the VPE should be running. If not, skip it */
|
|
1: andi t0, t6, 1
|
|
beqz t0, 2f
|
|
nop
|
|
|
|
/* Operate on the appropriate TC */
|
|
mfc0 t0, CP0_VPECONTROL
|
|
ori t0, t0, VPECONTROL_TARGTC
|
|
xori t0, t0, VPECONTROL_TARGTC
|
|
or t0, t0, t5
|
|
mtc0 t0, CP0_VPECONTROL
|
|
ehb
|
|
|
|
/* Skip the VPE if its TC is not halted */
|
|
mftc0 t0, CP0_TCHALT
|
|
beqz t0, 2f
|
|
nop
|
|
|
|
/* Calculate a pointer to the VPEs struct vpe_boot_config */
|
|
li t0, VPEBOOTCFG_SIZE
|
|
mul t0, t0, t5
|
|
addu t0, t0, t7
|
|
|
|
/* Set the TC restart PC */
|
|
lw t1, VPEBOOTCFG_PC(t0)
|
|
mttc0 t1, CP0_TCRESTART
|
|
|
|
/* Set the TC stack pointer */
|
|
lw t1, VPEBOOTCFG_SP(t0)
|
|
mttgpr t1, sp
|
|
|
|
/* Set the TC global pointer */
|
|
lw t1, VPEBOOTCFG_GP(t0)
|
|
mttgpr t1, gp
|
|
|
|
/* Copy config from this VPE */
|
|
mfc0 t0, CP0_CONFIG
|
|
mttc0 t0, CP0_CONFIG
|
|
|
|
/* Ensure no software interrupts are pending */
|
|
mttc0 zero, CP0_CAUSE
|
|
mttc0 zero, CP0_STATUS
|
|
|
|
/* Set TC active, not interrupt exempt */
|
|
mftc0 t0, CP0_TCSTATUS
|
|
li t1, ~TCSTATUS_IXMT
|
|
and t0, t0, t1
|
|
ori t0, t0, TCSTATUS_A
|
|
mttc0 t0, CP0_TCSTATUS
|
|
|
|
/* Clear the TC halt bit */
|
|
mttc0 zero, CP0_TCHALT
|
|
|
|
/* Set VPE active */
|
|
mftc0 t0, CP0_VPECONF0
|
|
ori t0, t0, VPECONF0_VPA
|
|
mttc0 t0, CP0_VPECONF0
|
|
|
|
/* Next VPE */
|
|
2: srl t6, t6, 1
|
|
addi t5, t5, 1
|
|
bnez t6, 1b
|
|
nop
|
|
|
|
/* Leave VPE configuration state */
|
|
mfc0 t1, CP0_MVPCONTROL
|
|
xori t1, t1, MVPCONTROL_VPC
|
|
mtc0 t1, CP0_MVPCONTROL
|
|
ehb
|
|
evpe
|
|
|
|
/* Check whether this VPE is meant to be running */
|
|
li t0, 1
|
|
sll t0, t0, t9
|
|
and t0, t0, t8
|
|
bnez t0, 2f
|
|
nop
|
|
|
|
/* This VPE should be offline, halt the TC */
|
|
li t0, TCHALT_H
|
|
mtc0 t0, CP0_TCHALT
|
|
la t0, 1f
|
|
1: jr.hb t0
|
|
nop
|
|
|
|
2: .set pop
|
|
|
|
#endif /* CONFIG_MIPS_MT */
|
|
|
|
/* Return */
|
|
jr ra
|
|
nop
|
|
END(mips_cps_boot_vpes)
|
|
|
|
#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
|
|
la \dest, __per_cpu_offset
|
|
addu $1, $1, \dest
|
|
lw $1, 0($1)
|
|
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 */
|