Impressum cps-vec.S   Sprache: Sparc

/* SPDX-License-Identifier: GPL-2.0-or-later */
/*
 * Copyright (C) 2013 Imagination Technologies
 * Author: Paul Burton <paul.burton@mips.com>
 */

#include <linux/init.h>
#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>
#include <asm/smp-cps.h>

#define GCR_CPC_BASE_OFS 0x0088
#define GCR_CL_COHERENCE_OFS 0x2008
#define GCR_CL_ID_OFS  0x2028
#define  CM3_GCR_Cx_ID_CLUSTER_SHF 8
#define  CM3_GCR_Cx_ID_CLUSTER_MSK (0xff << 8)
#define  CM3_GCR_Cx_ID_CORENUM_SHF 0
#define  CM3_GCR_Cx_ID_CORENUM_MSK (0xff << 0)

#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


LEAF(mips_cps_core_boot)
 /* Save  CCA and GCR base */
 move   s0, a0
 move   s1, a1

 /* We don't know how to do coherence setup on earlier ISA */
#if MIPS_ISA_REV > 0
 /* Skip cache & coherence setup if we're already coherent */
 lw s7, GCR_CL_COHERENCE_OFS(s1)
 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(s1)
 ehb
#endif /* MIPS_ISA_REV > 0 */

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

 __INIT
LEAF(excep_tlbfill)
 DUMP_EXCEP("TLB Fill")
 b .
  nop
 END(excep_tlbfill)

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

LEAF(excep_cache)
 DUMP_EXCEP("Cache")
 b .
  nop
 END(excep_cache)

LEAF(excep_genex)
 DUMP_EXCEP("General")
 b .
  nop
 END(excep_genex)

LEAF(excep_intex)
 DUMP_EXCEP("Interrupt")
 b .
  nop
 END(excep_intex)

LEAF(excep_ejtag)
 PTR_LA k0, ejtag_debug_handler
 jr k0
  nop
 END(excep_ejtag)
 __FINIT

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 */
 PTR_LA v0, mips_cps_cluster_bootcfg
 PTR_L v0, 0(v0)
 lw t0, GCR_CL_ID_OFS(s1)
#ifdef CONFIG_CPU_MIPSR6
 ext t1, t0, CM3_GCR_Cx_ID_CLUSTER_SHF, 8
 li t2, CLUSTERBOOTCFG_SIZE
 mul t1, t1, t2
 PTR_ADDU \
  v0, v0, t1
#endif
 PTR_L v0, CLUSTERBOOTCFG_CORECONFIG(v0)
 andi t0, t0, CM3_GCR_Cx_ID_CORENUM_MSK
 li t1, COREBOOTCFG_SIZE
 mul t0, t0, t1
 PTR_ADDU v0, v0, t0

 /* 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 */
 PTR_LA t1, mips_gcr_base
 PTR_L t1, 0(t1)
 PTR_L t1, GCR_CPC_BASE_OFS(t1)
 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)

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

 /* Enter VPE configuration state */
 .set push
 .set MIPS_ISA_LEVEL_RAW
 .set mt
 dvpe
 .set pop

 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

 .set push
 .set MIPS_ISA_LEVEL_RAW
 .set mt

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

 .set pop

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

#endif /* CONFIG_MIPS_MT_SMP */

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

#if MIPS_ISA_REV > 0
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)
#endif /* MIPS_ISA_REV > 0 */

#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
 PTR_ADDU $1, $1, \dest
 lw $1, 0($1)
 PTR_LA \dest, cps_cpu_state
 PTR_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 */