MIPS: BMIPS: Add Whirlwind (BMIPS5200) initialization code
authorFlorian Fainelli <f.fainelli@gmail.com>
Thu, 4 Feb 2016 02:14:50 +0000 (18:14 -0800)
committerRalf Baechle <ralf@linux-mips.org>
Mon, 9 May 2016 10:00:01 +0000 (12:00 +0200)
Import bmips_5xxx_init.S from the stblinux-3.3 tree, and to make sure that this
would work nicely with a BMIPS multiplatform kernel (with BMIPS330, BMIPS43XX
and BMIPS5000 enabled), update soft_reset to check for the BMIPS5200 processor
id (PRID_IMP_BMIPS5200) and execute bmips_5xxx_init for these processors to
bring them online.

Tested on 7425, 7429 and 7435 with CPU hotplug. 7435 SMP still needs some
additional changes in the L1 interrupt area to work properly with interrupt
affinity.

Signed-off-by: Florian Fainelli <f.fainelli@gmail.com>
Cc: linux-mips@linux-mips.org
Cc: john@phrozen.org
Cc: cernekee@gmail.com
Cc: jon.fraser@broadcom.com
Cc: jaedon.shin@gmail.com
Cc: dragan.stancevic@gmail.com
Cc: jogo@openwrt.org
Patchwork: https://patchwork.linux-mips.org/patch/12377/
Signed-off-by: Ralf Baechle <ralf@linux-mips.org>
arch/mips/kernel/Makefile
arch/mips/kernel/bmips_5xxx_init.S [new file with mode: 0644]
arch/mips/kernel/bmips_vec.S

index b0988fd62fcc3e6b6b296d3647cd9cdea39043f1..8a8b775fa7c84f412e21d9a63e14dc8bc5e1757f 100644 (file)
@@ -44,7 +44,7 @@ obj-$(CONFIG_CPU_CAVIUM_OCTEON)       += r4k_fpu.o octeon_switch.o
 
 obj-$(CONFIG_SMP)              += smp.o
 obj-$(CONFIG_SMP_UP)           += smp-up.o
-obj-$(CONFIG_CPU_BMIPS)                += smp-bmips.o bmips_vec.o
+obj-$(CONFIG_CPU_BMIPS)                += smp-bmips.o bmips_vec.o bmips_5xxx_init.o
 
 obj-$(CONFIG_MIPS_MT)          += mips-mt.o
 obj-$(CONFIG_MIPS_MT_FPAFF)    += mips-mt-fpaff.o
diff --git a/arch/mips/kernel/bmips_5xxx_init.S b/arch/mips/kernel/bmips_5xxx_init.S
new file mode 100644 (file)
index 0000000..788ebf9
--- /dev/null
@@ -0,0 +1,753 @@
+
+/*
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 2011-2012 by Broadcom Corporation
+ *
+ * Init for bmips 5000.
+ * Used to init second core in dual core 5000's.
+ */
+
+#include <linux/init.h>
+
+#include <asm/asm.h>
+#include <asm/asmmacro.h>
+#include <asm/cacheops.h>
+#include <asm/regdef.h>
+#include <asm/mipsregs.h>
+#include <asm/stackframe.h>
+#include <asm/addrspace.h>
+#include <asm/hazards.h>
+#include <asm/bmips.h>
+
+#ifdef CONFIG_CPU_BMIPS5000
+
+
+#define cacheop(kva, size, linesize, op)       \
+       .set noreorder                  ;       \
+       addu            t1, kva, size   ;       \
+        subu           t2, linesize, 1 ;       \
+        not            t2              ;       \
+        and            t0, kva, t2     ;       \
+        addiu          t1, t1, -1      ;       \
+        and            t1, t2          ;       \
+9:     cache           op, 0(t0)       ;       \
+        bne            t0, t1, 9b      ;       \
+        addu           t0, linesize    ;       \
+        .set reorder                   ;
+
+
+
+#define        IS_SHIFT        22
+#define        IL_SHIFT        19
+#define        IA_SHIFT        16
+#define        DS_SHIFT        13
+#define        DL_SHIFT        10
+#define        DA_SHIFT         7
+#define        IS_MASK          7
+#define        IL_MASK          7
+#define        IA_MASK          7
+#define        DS_MASK          7
+#define        DL_MASK          7
+#define        DA_MASK          7
+#define        ICE_MASK        0x80000000
+#define        DCE_MASK        0x40000000
+
+#define CP0_BRCM_CONFIG0       $22, 0
+#define CP0_BRCM_MODE          $22, 1
+#define        CP0_CONFIG_K0_MASK      7
+
+#define CP0_ICACHE_TAG_LO       $28
+#define CP0_ICACHE_DATA_LO      $28, 1
+#define CP0_DCACHE_TAG_LO       $28, 2
+#define CP0_D_SEC_CACHE_DATA_LO        $28, 3
+#define CP0_ICACHE_TAG_HI       $29
+#define CP0_ICACHE_DATA_HI      $29, 1
+#define CP0_DCACHE_TAG_HI       $29, 2
+
+#define CP0_BRCM_MODE_Luc_MASK         (1 << 11)
+#define        CP0_BRCM_CONFIG0_CWF_MASK       (1 << 20)
+#define        CP0_BRCM_CONFIG0_TSE_MASK       (1 << 19)
+#define CP0_BRCM_MODE_SET_MASK         (1 << 7)
+#define CP0_BRCM_MODE_ClkRATIO_MASK    (7 << 4)
+#define CP0_BRCM_MODE_BrPRED_MASK      (3 << 24)
+#define CP0_BRCM_MODE_BrPRED_SHIFT     24
+#define CP0_BRCM_MODE_BrHIST_MASK      (0x1f << 20)
+#define CP0_BRCM_MODE_BrHIST_SHIFT     20
+
+/* ZSC L2 Cache Register Access Register Definitions */
+#define BRCM_ZSC_ALL_REGS_SELECT                0x7 << 24
+
+#define BRCM_ZSC_CONFIG_REG                    0 << 3
+#define BRCM_ZSC_REQ_BUFFER_REG                        2 << 3
+#define BRCM_ZSC_RBUS_ADDR_MAPPING_REG0                4 << 3
+#define BRCM_ZSC_RBUS_ADDR_MAPPING_REG1                6 << 3
+#define BRCM_ZSC_RBUS_ADDR_MAPPING_REG2                8 << 3
+
+#define BRCM_ZSC_SCB0_ADDR_MAPPING_REG0                0xa << 3
+#define BRCM_ZSC_SCB0_ADDR_MAPPING_REG1                0xc << 3
+
+#define BRCM_ZSC_SCB1_ADDR_MAPPING_REG0                0xe << 3
+#define BRCM_ZSC_SCB1_ADDR_MAPPING_REG1                0x10 << 3
+
+#define BRCM_ZSC_CONFIG_LMB1En                 1 << (15)
+#define BRCM_ZSC_CONFIG_LMB0En                 1 << (14)
+
+/* branch predition values */
+
+#define BRCM_BrPRED_ALL_TAKEN          (0x0)
+#define BRCM_BrPRED_ALL_NOT_TAKEN      (0x1)
+#define BRCM_BrPRED_BHT_ENABLE         (0x2)
+#define BRCM_BrPRED_PREDICT_BACKWARD   (0x3)
+
+
+
+.align 2
+/*
+ * Function:   size_i_cache
+ * Arguments:  None
+ * Returns:    v0 = i cache size, v1 = I cache line size
+ * Description: compute the I-cache size and I-cache line size
+ * Trashes:    v0, v1, a0, t0
+ *
+ *     pseudo code:
+ *
+ */
+
+LEAF(size_i_cache)
+       .set    noreorder
+
+       mfc0    a0, CP0_CONFIG, 1
+       move    t0, a0
+
+       /*
+        * Determine sets per way: IS
+        *
+        * This field contains the number of sets (i.e., indices) per way of
+        * the instruction cache:
+        * i) 0x0: 64, ii) 0x1: 128, iii) 0x2: 256, iv) 0x3: 512, v) 0x4: 1k
+        * vi) 0x5 - 0x7: Reserved.
+        */
+
+       srl     a0, a0, IS_SHIFT
+       and     a0, a0, IS_MASK
+
+       /* sets per way = (64<<IS) */
+
+       li      v0, 0x40
+       sllv    v0, v0, a0
+
+       /*
+        * Determine line size
+        *
+        * This field contains the line size of the instruction cache:
+        * i) 0x0: No I-cache present, i) 0x3: 16 bytes, ii) 0x4: 32 bytes, iii)
+        * 0x5: 64 bytes, iv) the rest: Reserved.
+        */
+
+       move    a0, t0
+
+       srl     a0, a0, IL_SHIFT
+       and     a0, a0, IL_MASK
+
+       beqz    a0, no_i_cache
+       nop
+
+       /* line size = 2 ^ (IL+1) */
+
+       addi    a0, a0, 1
+       li      v1, 1
+       sll     v1, v1, a0
+
+       /* v0 now have sets per way, multiply it by line size now
+        * that will give the set size
+        */
+
+       sll     v0, v0, a0
+
+       /*
+        * Determine set associativity
+        *
+        * This field contains the set associativity of the instruction cache.
+        * i) 0x0: Direct mapped, ii) 0x1: 2-way, iii) 0x2: 3-way, iv) 0x3:
+        * 4-way, v) 0x4 - 0x7: Reserved.
+        */
+
+       move    a0, t0
+
+       srl     a0, a0, IA_SHIFT
+       and     a0, a0, IA_MASK
+       addi    a0, a0, 0x1
+
+       /* v0 has the set size, multiply it by
+        * set associativiy, to get the cache size
+        */
+
+       multu   v0, a0  /*multu is interlocked, so no need to insert nops */
+       mflo    v0
+       b       1f
+       nop
+
+no_i_cache:
+       move    v0, zero
+       move    v1, zero
+1:
+       jr       ra
+       nop
+       .set    reorder
+
+END(size_i_cache)
+
+/*
+ * Function:   size_d_cache
+ * Arguments:  None
+ * Returns:    v0 = d cache size, v1 = d cache line size
+ * Description: compute the D-cache size and D-cache line size.
+ * Trashes:    v0, v1, a0, t0
+ *
+ */
+
+LEAF(size_d_cache)
+       .set    noreorder
+
+       mfc0    a0, CP0_CONFIG, 1
+       move    t0, a0
+
+       /*
+        * Determine sets per way: IS
+        *
+        * This field contains the number of sets (i.e., indices) per way of
+        * the instruction cache:
+        * i) 0x0: 64, ii) 0x1: 128, iii) 0x2: 256, iv) 0x3: 512, v) 0x4: 1k
+        * vi) 0x5 - 0x7: Reserved.
+        */
+
+       srl     a0, a0, DS_SHIFT
+       and     a0, a0, DS_MASK
+
+       /* sets per way = (64<<IS) */
+
+       li      v0, 0x40
+       sllv    v0, v0, a0
+
+       /*
+        * Determine line size
+        *
+        * This field contains the line size of the instruction cache:
+        * i) 0x0: No I-cache present, i) 0x3: 16 bytes, ii) 0x4: 32 bytes, iii)
+        * 0x5: 64 bytes, iv) the rest: Reserved.
+        */
+       move    a0, t0
+
+       srl     a0, a0, DL_SHIFT
+       and     a0, a0, DL_MASK
+
+       beqz    a0, no_d_cache
+       nop
+
+       /* line size = 2 ^ (IL+1) */
+
+       addi    a0, a0, 1
+       li      v1, 1
+       sll     v1, v1, a0
+
+       /* v0 now have sets per way, multiply it by line size now
+        * that will give the set size
+        */
+
+       sll     v0, v0, a0
+
+       /* determine set associativity
+        *
+        * This field contains the set associativity of the instruction cache.
+        * i) 0x0: Direct mapped, ii) 0x1: 2-way, iii) 0x2: 3-way, iv) 0x3:
+        * 4-way, v) 0x4 - 0x7: Reserved.
+        */
+
+       move    a0, t0
+
+       srl     a0, a0, DA_SHIFT
+       and     a0, a0, DA_MASK
+       addi    a0, a0, 0x1
+
+       /* v0 has the set size, multiply it by
+        * set associativiy, to get the cache size
+        */
+
+       multu   v0, a0  /*multu is interlocked, so no need to insert nops */
+       mflo    v0
+
+       b       1f
+       nop
+
+no_d_cache:
+       move    v0, zero
+       move    v1, zero
+1:
+       jr      ra
+       nop
+       .set    reorder
+
+END(size_d_cache)
+
+
+/*
+ * Function: enable_ID
+ * Arguments:  None
+ * Returns:    None
+ * Description: Enable I and D caches, initialize I and D-caches, also set
+ *                 hardware delay for d-cache (TP0).
+ * Trashes:    t0
+ *
+ */
+       .global enable_ID
+       .ent    enable_ID
+       .set    noreorder
+enable_ID:
+       mfc0    t0, CP0_BRCM_CONFIG0
+       or      t0, t0, (ICE_MASK | DCE_MASK)
+       mtc0    t0, CP0_BRCM_CONFIG0
+       jr      ra
+       nop
+
+       .end    enable_ID
+       .set    reorder
+
+
+/*
+ * Function: l1_init
+ * Arguments:  None
+ * Returns:    None
+ * Description: Enable I and D caches, and initialize I and D-caches
+ * Trashes:    a0, v0, v1, t0, t1, t2, t8
+ *
+ */
+       .globl  l1_init
+       .ent    l1_init
+       .set    noreorder
+l1_init:
+
+       /* save return address */
+       move    t8, ra
+
+
+       /* initialize I and D cache Data and Tag registers.  */
+       mtc0    zero, CP0_ICACHE_TAG_LO
+       mtc0    zero, CP0_ICACHE_TAG_HI
+       mtc0    zero, CP0_ICACHE_DATA_LO
+       mtc0    zero, CP0_ICACHE_DATA_HI
+       mtc0    zero, CP0_DCACHE_TAG_LO
+       mtc0    zero, CP0_DCACHE_TAG_HI
+
+       /* Enable Caches before Clearing. If the caches are disabled
+        * then the cache operations to clear the cache will be ignored
+        */
+
+       jal     enable_ID
+       nop
+
+       jal     size_i_cache    /* v0 = i-cache size, v1 = i-cache line size */
+       nop
+
+       /* run uncached in kseg 1 */
+       la      k0, 1f
+       lui     k1, 0x2000
+       or      k0, k1, k0
+       jr      k0
+       nop
+1:
+
+       /*
+        * set K0 cache mode
+        */
+
+       mfc0    t0, CP0_CONFIG
+       and     t0, t0, ~CP0_CONFIG_K0_MASK
+       or      t0, t0, 3       /* Write Back mode */
+       mtc0    t0, CP0_CONFIG
+
+       /*
+        * Initialize  instruction cache.
+        */
+
+       li      a0, KSEG0
+       cacheop(a0, v0, v1, Index_Store_Tag_I)
+
+       /*
+        * Now we can run from I-$, kseg 0
+        */
+       la      k0, 1f
+       lui     k1, 0x2000
+       or      k0, k1, k0
+       xor     k0, k1, k0
+       jr      k0
+       nop
+1:
+       /*
+        * Initialize  data cache.
+        */
+
+       jal     size_d_cache    /* v0 = d-cache size, v1 = d-cache line size */
+       nop
+
+
+       li      a0, KSEG0
+       cacheop(a0, v0, v1, Index_Store_Tag_D)
+
+       jr      t8
+       nop
+
+       .end    l1_init
+       .set    reorder
+
+
+/*
+ * Function:   set_other_config
+ * Arguments:  none
+ * Returns:    None
+ * Description: initialize other remainder configuration to defaults.
+ * Trashes:    t0, t1
+ *
+ *     pseudo code:
+ *
+ */
+LEAF(set_other_config)
+       .set noreorder
+
+        /* enable Bus error for I-fetch */
+        mfc0   t0, CP0_CACHEERR, 0
+        li     t1, 0x4
+        or     t0, t1
+       mtc0    t0, CP0_CACHEERR, 0
+
+        /* enable Bus error for Load */
+        mfc0   t0, CP0_CACHEERR, 1
+        li     t1, 0x4
+        or     t0, t1
+       mtc0    t0, CP0_CACHEERR, 1
+
+       /* enable Bus Error for Store */
+        mfc0   t0, CP0_CACHEERR, 2
+       li      t1, 0x4
+       or      t0, t1
+        mtc0   t0, CP0_CACHEERR, 2
+
+       jr      ra
+       nop
+       .set reorder
+END(set_other_config)
+
+/*
+ * Function:   set_branch_pred
+ * Arguments:  none
+ * Returns:    None
+ * Description:
+ * Trashes:    t0, t1
+ *
+ *     pseudo code:
+ *
+ */
+
+LEAF(set_branch_pred)
+       .set noreorder
+       mfc0    t0, CP0_BRCM_MODE
+       li      t1, ~(CP0_BRCM_MODE_BrPRED_MASK | CP0_BRCM_MODE_BrHIST_MASK )
+       and     t0, t0, t1
+
+       /* enable Branch prediction */
+       li      t1, BRCM_BrPRED_BHT_ENABLE
+       sll     t1, CP0_BRCM_MODE_BrPRED_SHIFT
+       or      t0, t0, t1
+
+       /* set history count to 8 */
+       li      t1, 8
+       sll     t1, CP0_BRCM_MODE_BrHIST_SHIFT
+       or      t0, t0, t1
+
+       mtc0    t0, CP0_BRCM_MODE
+       jr      ra
+       nop
+       .set    reorder
+END(set_branch_pred)
+
+
+/*
+ * Function:   set_luc
+ * Arguments:  set link uncached.
+ * Returns:    None
+ * Description:
+ * Trashes:    t0, t1
+ *
+ */
+LEAF(set_luc)
+       .set noreorder
+       mfc0    t0, CP0_BRCM_MODE
+       li      t1, ~(CP0_BRCM_MODE_Luc_MASK)
+       and     t0, t0, t1
+
+       /* set Luc */
+        ori    t0, t0, CP0_BRCM_MODE_Luc_MASK
+
+       mtc0    t0, CP0_BRCM_MODE
+       jr      ra
+       nop
+       .set    reorder
+END(set_luc)
+
+/*
+ * Function:   set_cwf_tse
+ * Arguments:  set CWF and TSE bits
+ * Returns:    None
+ * Description:
+ * Trashes:    t0, t1
+ *
+ */
+LEAF(set_cwf_tse)
+       .set noreorder
+       mfc0    t0, CP0_BRCM_CONFIG0
+       li      t1, (CP0_BRCM_CONFIG0_CWF_MASK | CP0_BRCM_CONFIG0_TSE_MASK)
+       or      t0, t0, t1
+
+       mtc0    t0, CP0_BRCM_CONFIG0
+       jr      ra
+       nop
+       .set    reorder
+END(set_cwf_tse)
+
+/*
+ * Function:   set_clock_ratio
+ * Arguments:   set clock ratio specified by a0
+ * Returns:    None
+ * Description:
+ * Trashes:    v0, v1, a0, a1
+ *
+ *     pseudo code:
+ *
+ */
+LEAF(set_clock_ratio)
+       .set noreorder
+
+       mfc0    t0, CP0_BRCM_MODE
+       li      t1, ~(CP0_BRCM_MODE_SET_MASK | CP0_BRCM_MODE_ClkRATIO_MASK)
+       and     t0, t0, t1
+       li      t1, CP0_BRCM_MODE_SET_MASK
+       or      t0, t0, t1
+       or      t0, t0, a0
+       mtc0    t0, CP0_BRCM_MODE
+       jr      ra
+       nop
+       .set    reorder
+END(set_clock_ratio)
+/*
+ * Function: set_zephyr
+ * Arguments:   None
+ * Returns:     None
+ * Description: Set any zephyr bits
+ * Trashes:     t0 & t1
+ *
+ */
+LEAF(set_zephyr)
+        .set    noreorder
+
+        /* enable read/write of CP0 #22 sel. 8 */
+        li      t0, 0x5a455048
+       .word   0x4088b00f      /* mtc0    t0, $22, 15 */
+
+       .word   0x4008b008      /* mfc0    t0, $22, 8 */
+        li      t1, 0x09008000 /* turn off pref, jtb */
+        or      t0, t0, t1
+       .word   0x4088b008      /* mtc0    t0, $22, 8 */
+        sync
+
+       /* disable read/write of CP0 #22 sel 8 */
+        li      t0, 0x0
+       .word   0x4088b00f      /* mtc0    t0, $22, 15 */
+
+
+        jr      ra
+        nop
+       .set reorder
+
+END(set_zephyr)
+
+
+/*
+ * Function:    set_llmb
+ * Arguments:   a0=0 disable llmb, a0=1 enables llmb
+ * Returns:     None
+ * Description:
+ * Trashes:     t0, t1, t2
+ *
+ *      pseudo code:
+ *
+ */
+LEAF(set_llmb)
+       .set noreorder
+
+       li      t2, 0x90000000 | BRCM_ZSC_ALL_REGS_SELECT | BRCM_ZSC_CONFIG_REG
+       sync
+       cache   0x7, 0x0(t2)
+       sync
+       mfc0    t0, CP0_D_SEC_CACHE_DATA_LO
+       li      t1, ~(BRCM_ZSC_CONFIG_LMB1En | BRCM_ZSC_CONFIG_LMB0En)
+       and     t0, t0, t1
+
+       beqz    a0, svlmb
+       nop
+
+enable_lmb:
+       li      t1, (BRCM_ZSC_CONFIG_LMB1En | BRCM_ZSC_CONFIG_LMB0En)
+       or      t0, t0, t1
+
+svlmb:
+       mtc0    t0, CP0_D_SEC_CACHE_DATA_LO
+       sync
+       cache   0xb, 0x0(t2)
+       sync
+
+       jr      ra
+       nop
+       .set reorder
+
+END(set_llmb)
+/*
+ * Function:   core_init
+ * Arguments:  none
+ * Returns:    None
+ * Description: initialize core related configuration
+ * Trashes:    v0,v1,a0,a1,t8
+ *
+ *     pseudo code:
+ *
+ */
+       .globl  core_init
+       .ent    core_init
+       .set    noreorder
+core_init:
+       move    t8, ra
+
+       /* set Zephyr bits. */
+       bal     set_zephyr
+       nop
+
+#if ENABLE_FPU==1
+       /* initialize the Floating point unit (both TPs) */
+       bal     init_fpu
+       nop
+#endif
+
+       /* set low latency memory bus */
+       li      a0, 1
+       bal     set_llmb
+       nop
+
+       /* set branch prediction (TP0 only) */
+       bal     set_branch_pred
+       nop
+
+       /* set link uncached */
+       bal     set_luc
+       nop
+
+       /* set CWF and TSE */
+       bal     set_cwf_tse
+       nop
+
+       /*
+        *set clock ratio by setting 1 to 'set'
+        * and 0 to ClkRatio, (TP0 only)
+        */
+       li      a0, 0
+       bal     set_clock_ratio
+       nop
+
+       /* set other configuration to defaults */
+       bal     set_other_config
+       nop
+
+       move    ra, t8
+       jr      ra
+       nop
+
+       .set reorder
+       .end    core_init
+
+/*
+ * Function:   clear_jump_target_buffer
+ * Arguments:   None
+ * Returns:     None
+ * Description:
+ * Trashes:     t0, t1, t2
+ *
+ */
+#define RESET_CALL_RETURN_STACK_THIS_THREAD             (0x06<<16)
+#define RESET_JUMP_TARGET_BUFFER_THIS_THREAD           (0x04<<16)
+#define JTB_CS_CNTL_MASK                               (0xFF<<16)
+
+       .globl  clear_jump_target_buffer
+       .ent    clear_jump_target_buffer
+       .set    noreorder
+clear_jump_target_buffer:
+
+        mfc0    t0, $22, 2
+        nop
+        nop
+
+        li     t1, ~JTB_CS_CNTL_MASK
+        and    t0, t0, t1
+        li     t2, RESET_CALL_RETURN_STACK_THIS_THREAD
+        or     t0, t0, t2
+        mtc0    t0, $22, 2
+        nop
+        nop
+
+        and    t0, t0, t1
+        li     t2, RESET_JUMP_TARGET_BUFFER_THIS_THREAD
+        or     t0, t0, t2
+        mtc0   t0, $22, 2
+        nop
+        nop
+        jr      ra
+        nop
+
+       .end    clear_jump_target_buffer
+       .set    reorder
+/*
+ * Function:   bmips_cache_init
+ * Arguments:  None
+ * Returns:    None
+ * Description: Enable I and D caches, and initialize I and D-caches
+ * Trashes:    v0, v1, t0, t1, t2, t5, t7, t8
+ *
+ */
+       .globl  bmips_5xxx_init
+       .ent    bmips_5xxx_init
+       .set    noreorder
+bmips_5xxx_init:
+
+       /* save return address  and A0 */
+       move    t7, ra
+       move    t5, a0
+
+       jal     l1_init
+       nop
+
+       jal     core_init
+       nop
+
+       jal     clear_jump_target_buffer
+       nop
+
+        mtc0    zero, CP0_CAUSE
+
+       move    a0, t5
+       jr      t7
+       nop
+
+       .end    bmips_5xxx_init
+       .set    reorder
+
+
+#endif
index d9495f3f3fad345521aa5968731cd1f51e65020c..921a5fa55da6eae3ea704d5e0eeb6101da0217ef 100644 (file)
@@ -88,7 +88,7 @@ NESTED(bmips_reset_nmi_vec, PT_SIZE, sp)
        li      k1, (1 << 19)
        mfc0    k0, CP0_STATUS
        and     k0, k1
-       beqz    k0, bmips_smp_entry
+       beqz    k0, soft_reset
 
 #if defined(CONFIG_CPU_BMIPS5000)
        mfc0    k0, CP0_PRID
@@ -126,13 +126,48 @@ NESTED(bmips_reset_nmi_vec, PT_SIZE, sp)
        .set    arch=r4000
        eret
 
+#ifdef CONFIG_SMP
+soft_reset:
+
+#if defined(CONFIG_CPU_BMIPS5000)
+       mfc0    k0, CP0_PRID
+       andi    k0, 0xff00
+       li      k1, PRID_IMP_BMIPS5200
+       bne     k0, k1, bmips_smp_entry
+
+        /* if running on TP 1, jump  to  bmips_smp_entry */
+        mfc0    k0, $22
+        li      k1, (1 << 24)
+        and     k1, k0
+        bnez    k1, bmips_smp_entry
+        nop
+
+        /*
+         * running on TP0, can not be core 0 (the boot core).
+         * Check for soft reset.  Indicates a warm boot
+         */
+        mfc0    k0, $12
+        li      k1, (1 << 20)
+        and     k0, k1
+        beqz    k0, bmips_smp_entry
+
+        /*
+         * Warm boot.
+         * Cache init is only done on TP0
+         */
+        la      k0, bmips_5xxx_init
+        jalr    k0
+        nop
+
+        b       bmips_smp_entry
+        nop
+#endif
+
 /***********************************************************************
  * CPU1 reset vector (used for the initial boot only)
  * This is still part of bmips_reset_nmi_vec().
  ***********************************************************************/
 
-#ifdef CONFIG_SMP
-
 bmips_smp_entry:
 
        /* set up CP0 STATUS; enable FPU */