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