powerpc/44x: break out cpu init code into stand-alone function

The 47x platform supports multiple cores and shares code with 44x.
Break out code that is common for initializing the primary and secondary
cpus into a function which can be called for both.

Signed-off-by: Dave Kleikamp <shaggy@linux.vnet.ibm.com>
Signed-off-by: Josh Boyer <jwboyer@linux.vnet.ibm.com>

1 files changed, 171 insertions, 159 deletions

diff --git a/arch/powerpc/kernel/head_44x.S b/arch/powerpc/kernel/head_44x.S
index 711368b993f2..39be049a7850 100644
--- a/arch/powerpc/kernel/head_44x.S
+++ b/arch/powerpc/kernel/head_44x.S
@@ -69,165 +69,7 @@ _ENTRY(_start);
         mr      r27,r7
         li      r24,0           /* CPU number */
 
-/*
- * In case the firmware didn't do it, we apply some workarounds
- * that are good for all 440 core variants here
- */
-        mfspr   r3,SPRN_CCR0
-        rlwinm  r3,r3,0,0,27    /* disable icache prefetch */
-        isync
-        mtspr   SPRN_CCR0,r3
-        isync
-        sync
-
-/*
- * Set up the initial MMU state
- *
- * We are still executing code at the virtual address
- * mappings set by the firmware for the base of RAM.
- *
- * We first invalidate all TLB entries but the one
- * we are running from.  We then load the KERNELBASE
- * mappings so we can begin to use kernel addresses
- * natively and so the interrupt vector locations are
- * permanently pinned (necessary since Book E
- * implementations always have translation enabled).
- *
- * TODO: Use the known TLB entry we are running from to
- *       determine which physical region we are located
- *       in.  This can be used to determine where in RAM
- *       (on a shared CPU system) or PCI memory space
- *       (on a DRAMless system) we are located.
- *       For now, we assume a perfect world which means
- *       we are located at the base of DRAM (physical 0).
- */
-
-/*
- * Search TLB for entry that we are currently using.
- * Invalidate all entries but the one we are using.
- */
-        /* Load our current PID->MMUCR TID and MSR IS->MMUCR STS */
-        mfspr   r3,SPRN_PID                     /* Get PID */
-        mfmsr   r4                              /* Get MSR */
-        andi.   r4,r4,MSR_IS@l                  /* TS=1? */
-        beq     wmmucr                          /* If not, leave STS=0 */
-        oris    r3,r3,PPC44x_MMUCR_STS@h        /* Set STS=1 */
-wmmucr: mtspr   SPRN_MMUCR,r3                   /* Put MMUCR */
-        sync
-
-        bl      invstr                          /* Find our address */
-invstr: mflr    r5                              /* Make it accessible */
-        tlbsx   r23,0,r5                        /* Find entry we are in */
-        li      r4,0                            /* Start at TLB entry 0 */
-        li      r3,0                            /* Set PAGEID inval value */
-1:      cmpw    r23,r4                          /* Is this our entry? */
-        beq     skpinv                          /* If so, skip the inval */
-        tlbwe   r3,r4,PPC44x_TLB_PAGEID         /* If not, inval the entry */
-skpinv: addi    r4,r4,1                         /* Increment */
-        cmpwi   r4,64                           /* Are we done? */
-        bne     1b                              /* If not, repeat */
-        isync                                   /* If so, context change */
-
-/*
- * Configure and load pinned entry into TLB slot 63.
- */
-
-        lis     r3,PAGE_OFFSET@h
-        ori     r3,r3,PAGE_OFFSET@l
-
-        /* Kernel is at the base of RAM */
-        li r4, 0                        /* Load the kernel physical address */
-
-        /* Load the kernel PID = 0 */
-        li      r0,0
-        mtspr   SPRN_PID,r0
-        sync
-
-        /* Initialize MMUCR */
-        li      r5,0
-        mtspr   SPRN_MMUCR,r5
-        sync
-
-        /* pageid fields */
-        clrrwi  r3,r3,10                /* Mask off the effective page number */
-        ori     r3,r3,PPC44x_TLB_VALID | PPC44x_TLB_256M
-
-        /* xlat fields */
-        clrrwi  r4,r4,10                /* Mask off the real page number */
-                                        /* ERPN is 0 for first 4GB page */
-
-        /* attrib fields */
-        /* Added guarded bit to protect against speculative loads/stores */
-        li      r5,0
-        ori     r5,r5,(PPC44x_TLB_SW | PPC44x_TLB_SR | PPC44x_TLB_SX | PPC44x_TLB_G)
-
-        li      r0,63                    /* TLB slot 63 */
-
-        tlbwe   r3,r0,PPC44x_TLB_PAGEID /* Load the pageid fields */
-        tlbwe   r4,r0,PPC44x_TLB_XLAT   /* Load the translation fields */
-        tlbwe   r5,r0,PPC44x_TLB_ATTRIB /* Load the attrib/access fields */
-
-        /* Force context change */
-        mfmsr   r0
-        mtspr   SPRN_SRR1, r0
-        lis     r0,3f@h
-        ori     r0,r0,3f@l
-        mtspr   SPRN_SRR0,r0
-        sync
-        rfi
-
-        /* If necessary, invalidate original entry we used */
-3:      cmpwi   r23,63
-        beq     4f
-        li      r6,0
-        tlbwe   r6,r23,PPC44x_TLB_PAGEID
-        isync
-
-4:
-#ifdef CONFIG_PPC_EARLY_DEBUG_44x
-        /* Add UART mapping for early debug. */
-
-        /* pageid fields */
-        lis     r3,PPC44x_EARLY_DEBUG_VIRTADDR@h
-        ori     r3,r3,PPC44x_TLB_VALID|PPC44x_TLB_TS|PPC44x_TLB_64K
-
-        /* xlat fields */
-        lis     r4,CONFIG_PPC_EARLY_DEBUG_44x_PHYSLOW@h
-        ori     r4,r4,CONFIG_PPC_EARLY_DEBUG_44x_PHYSHIGH
-
-        /* attrib fields */
-        li      r5,(PPC44x_TLB_SW|PPC44x_TLB_SR|PPC44x_TLB_I|PPC44x_TLB_G)
-        li      r0,62                    /* TLB slot 0 */
-
-        tlbwe   r3,r0,PPC44x_TLB_PAGEID
-        tlbwe   r4,r0,PPC44x_TLB_XLAT
-        tlbwe   r5,r0,PPC44x_TLB_ATTRIB
-
-        /* Force context change */
-        isync
-#endif /* CONFIG_PPC_EARLY_DEBUG_44x */
-
-        /* Establish the interrupt vector offsets */
-        SET_IVOR(0,  CriticalInput);
-        SET_IVOR(1,  MachineCheck);
-        SET_IVOR(2,  DataStorage);
-        SET_IVOR(3,  InstructionStorage);
-        SET_IVOR(4,  ExternalInput);
-        SET_IVOR(5,  Alignment);
-        SET_IVOR(6,  Program);
-        SET_IVOR(7,  FloatingPointUnavailable);
-        SET_IVOR(8,  SystemCall);
-        SET_IVOR(9,  AuxillaryProcessorUnavailable);
-        SET_IVOR(10, Decrementer);
-        SET_IVOR(11, FixedIntervalTimer);
-        SET_IVOR(12, WatchdogTimer);
-        SET_IVOR(13, DataTLBError);
-        SET_IVOR(14, InstructionTLBError);
-        SET_IVOR(15, DebugCrit);
-
-        /* Establish the interrupt vector base */
-        lis     r4,interrupt_base@h     /* IVPR only uses the high 16-bits */
-        mtspr   SPRN_IVPR,r4
+        bl      init_cpu_state
 
         /*
          * This is where the main kernel code starts.
@@ -647,6 +489,176 @@ _GLOBAL(set_context)
         blr
 
 /*
+ * Init CPU state. This is called at boot time or for secondary CPUs
+ * to setup initial TLB entries, setup IVORs, etc...
+ */
+_GLOBAL(init_cpu_state)
+        mflr    r22
+/*
+ * In case the firmware didn't do it, we apply some workarounds
+ * that are good for all 440 core variants here
+ */
+        mfspr   r3,SPRN_CCR0
+        rlwinm  r3,r3,0,0,27    /* disable icache prefetch */
+        isync
+        mtspr   SPRN_CCR0,r3
+        isync
+        sync
+
+/*
+ * Set up the initial MMU state
+ *
+ * We are still executing code at the virtual address
+ * mappings set by the firmware for the base of RAM.
+ *
+ * We first invalidate all TLB entries but the one
+ * we are running from.  We then load the KERNELBASE
+ * mappings so we can begin to use kernel addresses
+ * natively and so the interrupt vector locations are
+ * permanently pinned (necessary since Book E
+ * implementations always have translation enabled).
+ *
+ * TODO: Use the known TLB entry we are running from to
+ *       determine which physical region we are located
+ *       in.  This can be used to determine where in RAM
+ *       (on a shared CPU system) or PCI memory space
+ *       (on a DRAMless system) we are located.
+ *       For now, we assume a perfect world which means
+ *       we are located at the base of DRAM (physical 0).
+ */
+
+/*
+ * Search TLB for entry that we are currently using.
+ * Invalidate all entries but the one we are using.
+ */
+        /* Load our current PID->MMUCR TID and MSR IS->MMUCR STS */
+        mfspr   r3,SPRN_PID                     /* Get PID */
+        mfmsr   r4                              /* Get MSR */
+        andi.   r4,r4,MSR_IS@l                  /* TS=1? */
+        beq     wmmucr                          /* If not, leave STS=0 */
+        oris    r3,r3,PPC44x_MMUCR_STS@h        /* Set STS=1 */
+wmmucr: mtspr   SPRN_MMUCR,r3                   /* Put MMUCR */
+        sync
+
+        bl      invstr                          /* Find our address */
+invstr: mflr    r5                              /* Make it accessible */
+        tlbsx   r23,0,r5                        /* Find entry we are in */
+        li      r4,0                            /* Start at TLB entry 0 */
+        li      r3,0                            /* Set PAGEID inval value */
+1:      cmpw    r23,r4                          /* Is this our entry? */
+        beq     skpinv                          /* If so, skip the inval */
+        tlbwe   r3,r4,PPC44x_TLB_PAGEID         /* If not, inval the entry */
+skpinv: addi    r4,r4,1                         /* Increment */
+        cmpwi   r4,64                           /* Are we done? */
+        bne     1b                              /* If not, repeat */
+        isync                                   /* If so, context change */
+
+/*
+ * Configure and load pinned entry into TLB slot 63.
+ */
+
+        lis     r3,PAGE_OFFSET@h
+        ori     r3,r3,PAGE_OFFSET@l
+
+        /* Kernel is at the base of RAM */
+        li r4, 0                        /* Load the kernel physical address */
+
+        /* Load the kernel PID = 0 */
+        li      r0,0
+        mtspr   SPRN_PID,r0
+        sync
+
+        /* Initialize MMUCR */
+        li      r5,0
+        mtspr   SPRN_MMUCR,r5
+        sync
+
+        /* pageid fields */
+        clrrwi  r3,r3,10                /* Mask off the effective page number */
+        ori     r3,r3,PPC44x_TLB_VALID | PPC44x_TLB_256M
+
+        /* xlat fields */
+        clrrwi  r4,r4,10                /* Mask off the real page number */
+                                        /* ERPN is 0 for first 4GB page */
+
+        /* attrib fields */
+        /* Added guarded bit to protect against speculative loads/stores */
+        li      r5,0
+        ori     r5,r5,(PPC44x_TLB_SW | PPC44x_TLB_SR | PPC44x_TLB_SX | PPC44x_TLB_G)
+
+        li      r0,63                    /* TLB slot 63 */
+
+        tlbwe   r3,r0,PPC44x_TLB_PAGEID /* Load the pageid fields */
+        tlbwe   r4,r0,PPC44x_TLB_XLAT   /* Load the translation fields */
+        tlbwe   r5,r0,PPC44x_TLB_ATTRIB /* Load the attrib/access fields */
+
+        /* Force context change */
+        mfmsr   r0
+        mtspr   SPRN_SRR1, r0
+        lis     r0,3f@h
+        ori     r0,r0,3f@l
+        mtspr   SPRN_SRR0,r0
+        sync
+        rfi
+
+        /* If necessary, invalidate original entry we used */
+3:      cmpwi   r23,63
+        beq     4f
+        li      r6,0
+        tlbwe   r6,r23,PPC44x_TLB_PAGEID
+        isync
+
+4:
+#ifdef CONFIG_PPC_EARLY_DEBUG_44x
+        /* Add UART mapping for early debug. */
+
+        /* pageid fields */
+        lis     r3,PPC44x_EARLY_DEBUG_VIRTADDR@h
+        ori     r3,r3,PPC44x_TLB_VALID|PPC44x_TLB_TS|PPC44x_TLB_64K
+
+        /* xlat fields */
+        lis     r4,CONFIG_PPC_EARLY_DEBUG_44x_PHYSLOW@h
+        ori     r4,r4,CONFIG_PPC_EARLY_DEBUG_44x_PHYSHIGH
+
+        /* attrib fields */
+        li      r5,(PPC44x_TLB_SW|PPC44x_TLB_SR|PPC44x_TLB_I|PPC44x_TLB_G)
+        li      r0,62                    /* TLB slot 0 */
+
+        tlbwe   r3,r0,PPC44x_TLB_PAGEID
+        tlbwe   r4,r0,PPC44x_TLB_XLAT
+        tlbwe   r5,r0,PPC44x_TLB_ATTRIB
+
+        /* Force context change */
+        isync
+#endif /* CONFIG_PPC_EARLY_DEBUG_44x */
+
+        /* Establish the interrupt vector offsets */
+        SET_IVOR(0,  CriticalInput);
+        SET_IVOR(1,  MachineCheck);
+        SET_IVOR(2,  DataStorage);
+        SET_IVOR(3,  InstructionStorage);
+        SET_IVOR(4,  ExternalInput);
+        SET_IVOR(5,  Alignment);
+        SET_IVOR(6,  Program);
+        SET_IVOR(7,  FloatingPointUnavailable);
+        SET_IVOR(8,  SystemCall);
+        SET_IVOR(9,  AuxillaryProcessorUnavailable);
+        SET_IVOR(10, Decrementer);
+        SET_IVOR(11, FixedIntervalTimer);
+        SET_IVOR(12, WatchdogTimer);
+        SET_IVOR(13, DataTLBError);
+        SET_IVOR(14, InstructionTLBError);
+        SET_IVOR(15, DebugCrit);
+
+        /* Establish the interrupt vector base */
+        lis     r4,interrupt_base@h     /* IVPR only uses the high 16-bits */
+        mtspr   SPRN_IVPR,r4
+
+        addis   r22,r22,KERNELBASE@h
+        mtlr    r22
+        blr
+
+/*
  * We put a few things here that have to be page-aligned. This stuff
  * goes at the beginning of the data segment, which is page-aligned.
  */