![Markos Chandras](/assets/img/avatar_default.png)
The CPS code is doing several memory loads when configuring the VPEs from secondary cores, so the segmentation control registers must be initialized in time otherwise the kernel will crash with strange TLB exceptions. Reviewed-by: Paul Burton <paul.burton@imgtec.com> Signed-off-by: Markos Chandras <markos.chandras@imgtec.com> Patchwork: http://patchwork.linux-mips.org/patch/7424/ Signed-off-by: James Hogan <james.hogan@imgtec.com>
492 lines
9.1 KiB
ArmAsm
492 lines
9.1 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/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 */
|
|
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
|
|
|
|
/* 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)
|
|
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 */
|