microblaze: Correct ffs regression for Microblaze

[u-boot] / cpu / mpc85xx / cpu_init.c

diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 07856c2a24cee575d162ee3ff6bc86ec6ac29cbb..0041a60df967c0c51ee97c004a038a50e6ad9238 100644 (file)
--- a/cpu/mpc85xx/cpu_init.c
+++ b/cpu/mpc85xx/cpu_init.c
@@ -129,57 +129,6 @@ void config_8560_ioports (volatile ccsr_cpm_t * cpm)
 }
 #endif
 
-/* We run cpu_init_early_f in AS = 1 */
-void cpu_init_early_f(void)
-{
-       u32 mas0, mas1, mas2, mas3, mas7;
-       int i;
-
-       /* Pointer is writable since we allocated a register for it */
-       gd = (gd_t *) (CONFIG_SYS_INIT_RAM_ADDR + CONFIG_SYS_GBL_DATA_OFFSET);
-
-       /*
-        * Clear initial global data
-        *   we don't use memset so we can share this code with NAND_SPL
-        */
-       for (i = 0; i < sizeof(gd_t); i++)
-               ((char *)gd)[i] = 0;
-
-       mas0 = MAS0_TLBSEL(0) | MAS0_ESEL(0);
-       mas1 = MAS1_VALID | MAS1_TID(0) | MAS1_TS | MAS1_TSIZE(BOOKE_PAGESZ_4K);
-       mas2 = FSL_BOOKE_MAS2(CONFIG_SYS_CCSRBAR, MAS2_I|MAS2_G);
-       mas3 = FSL_BOOKE_MAS3(CONFIG_SYS_CCSRBAR_PHYS, 0, MAS3_SW|MAS3_SR);
-       mas7 = FSL_BOOKE_MAS7(CONFIG_SYS_CCSRBAR_PHYS);
-
-       write_tlb(mas0, mas1, mas2, mas3, mas7);
-
-       /* set up CCSR if we want it moved */
-#if (CONFIG_SYS_CCSRBAR_DEFAULT != CONFIG_SYS_CCSRBAR_PHYS)
-       {
-               u32 temp;
-               volatile u32 *ccsr_virt =
-                       (volatile u32 *)(CONFIG_SYS_CCSRBAR + 0x1000);
-
-               mas0 = MAS0_TLBSEL(0) | MAS0_ESEL(1);
-               /* mas1 is the same as above */
-               mas2 = FSL_BOOKE_MAS2((u32)ccsr_virt, MAS2_I|MAS2_G);
-               mas3 = FSL_BOOKE_MAS3(CONFIG_SYS_CCSRBAR_DEFAULT, 0,
-                                               MAS3_SW|MAS3_SR);
-               mas7 = FSL_BOOKE_MAS7(CONFIG_SYS_CCSRBAR_DEFAULT);
-
-               write_tlb(mas0, mas1, mas2, mas3, mas7);
-
-               temp = in_be32(ccsr_virt);
-               out_be32(ccsr_virt, CONFIG_SYS_CCSRBAR_PHYS >> 12);
-               temp = in_be32((volatile u32 *)CONFIG_SYS_CCSRBAR);
-       }
-#endif
-
-       init_laws();
-       invalidate_tlb(0);
-       init_tlbs();
-}
-
 /*
  * Breathe some life into the CPU...
  *
@@ -187,6 +136,20 @@ void cpu_init_early_f(void)
  * initialize a bunch of registers
  */
 
+#ifdef CONFIG_FSL_CORENET
+static void corenet_tb_init(void)
+{
+       volatile ccsr_rcpm_t *rcpm =
+               (void *)(CONFIG_SYS_FSL_CORENET_RCPM_ADDR);
+       volatile ccsr_pic_t *pic =
+               (void *)(CONFIG_SYS_MPC85xx_PIC_ADDR);
+       u32 whoami = in_be32(&pic->whoami);
+
+       /* Enable the timebase register for this core */
+       out_be32(&rcpm->ctbenrl, (1 << whoami));
+}
+#endif
+
 void cpu_init_f (void)
 {
        volatile ccsr_lbc_t *memctl = (void *)(CONFIG_SYS_MPC85xx_LBC_ADDR);
@@ -280,6 +243,9 @@ void cpu_init_f (void)
 #if defined(CONFIG_FSL_DMA)
        dma_init();
 #endif
+#ifdef CONFIG_FSL_CORENET
+       corenet_tb_init();
+#endif
 }
 
 
@@ -387,15 +353,18 @@ int cpu_init_r(void)
        u32 l2cfg0 = mfspr(SPRN_L2CFG0);
 
        /* invalidate the L2 cache */
-       mtspr(SPRN_L2CSR0, L2CSR0_L2FI);
-       while (mfspr(SPRN_L2CSR0) & L2CSR0_L2FI)
+       mtspr(SPRN_L2CSR0, (L2CSR0_L2FI|L2CSR0_L2LFC));
+       while (mfspr(SPRN_L2CSR0) & (L2CSR0_L2FI|L2CSR0_L2LFC))
                ;
 
        /* enable the cache */
        mtspr(SPRN_L2CSR0, CONFIG_SYS_INIT_L2CSR0);
 
-       if (CONFIG_SYS_INIT_L2CSR0 & L2CSR0_L2E)
+       if (CONFIG_SYS_INIT_L2CSR0 & L2CSR0_L2E) {
+               while (!(mfspr(SPRN_L2CSR0) & L2CSR0_L2E))
+                       ;
                printf("%d KB enabled\n", (l2cfg0 & 0x3fff) * 64);
+       }
 #else
        puts("disabled\n");
 #endif
@@ -415,5 +384,16 @@ extern void setup_ivors(void);
 
 void arch_preboot_os(void)
 {
+       u32 msr;
+
+       /*
+        * We are changing interrupt offsets and are about to boot the OS so
+        * we need to make sure we disable all async interrupts. EE is already
+        * disabled by the time we get called.
+        */
+       msr = mfmsr();
+       msr &= ~(MSR_ME|MSR_CE|MSR_DE);
+       mtmsr(msr);
+
        setup_ivors();
 }