powepc/booke: Separate out E.HV check and ivor setup code.

Move the E.HV check and CPU_FTR_EMB_HV flag manipulation to the cpu setup
code.  Create a separate routine for E.HV ivors setup.

Signed-off-by: Varun Sethi <Varun.Sethi@freescale.com>
Signed-off-by: Kumar Gala <galak@kernel.crashing.org>

2 files changed, 30 insertions, 17 deletions

diff --git a/arch/powerpc/kernel/cpu_setup_fsl_booke.S b/arch/powerpc/kernel/cpu_setup_fsl_booke.S
index 69fdd2322a66..a55d028aa0da 100644
--- a/arch/powerpc/kernel/cpu_setup_fsl_booke.S
+++ b/arch/powerpc/kernel/cpu_setup_fsl_booke.S
@@ -16,6 +16,8 @@
 #include <asm/processor.h>
 #include <asm/cputable.h>
 #include <asm/ppc_asm.h>
+#include <asm/mmu-book3e.h>
+#include <asm/asm-offsets.h>
 
 _GLOBAL(__e500_icache_setup)
         mfspr   r0, SPRN_L1CSR1
@@ -73,12 +75,33 @@ _GLOBAL(__setup_cpu_e500v2)
         mtlr    r4
         blr
 _GLOBAL(__setup_cpu_e500mc)
-        mr      r5, r4
-        mflr    r4
+        mflr    r5
         bl      __e500_icache_setup
         bl      __e500_dcache_setup
         bl      __setup_e500mc_ivors
-        mtlr    r4
+        /*
+         * We only want to touch IVOR38-41 if we're running on hardware
+         * that supports category E.HV.  The architectural way to determine
+         * this is MMUCFG[LPIDSIZE].
+         */
+        mfspr   r3, SPRN_MMUCFG
+        rlwinm. r3, r3, 0, MMUCFG_LPIDSIZE
+        beq     1f
+        bl      __setup_ehv_ivors
+        b       2f
+1:
+        lwz     r3, CPU_SPEC_FEATURES(r4)
+        /* We need this check as cpu_setup is also called for
+         * the secondary cores. So, if we have already cleared
+         * the feature on the primary core, avoid doing it on the
+         * secondary core.
+         */
+        andis.  r6, r3, CPU_FTR_EMB_HV@h
+        beq     2f
+        rlwinm  r3, r3, 0, ~CPU_FTR_EMB_HV
+        stw     r3, CPU_SPEC_FEATURES(r4)
+2:
+        mtlr    r5
         blr
 #endif
 /* Right now, restore and setup are the same thing */
diff --git a/arch/powerpc/kernel/head_fsl_booke.S b/arch/powerpc/kernel/head_fsl_booke.S
index b221541d9861..6f62a737f607 100644
--- a/arch/powerpc/kernel/head_fsl_booke.S
+++ b/arch/powerpc/kernel/head_fsl_booke.S
@@ -895,15 +895,11 @@ _GLOBAL(__setup_e500mc_ivors)
         mtspr   SPRN_IVOR36,r3
         li      r3,CriticalDoorbell@l
         mtspr   SPRN_IVOR37,r3
+        sync
+        blr
 
-        /*
-         * We only want to touch IVOR38-41 if we're running on hardware
-         * that supports category E.HV.  The architectural way to determine
-         * this is MMUCFG[LPIDSIZE].
-         */
-        mfspr   r3, SPRN_MMUCFG
-        andis.  r3, r3, MMUCFG_LPIDSIZE@h
-        beq     no_hv
+/* setup ehv ivors for */
+_GLOBAL(__setup_ehv_ivors)
         li      r3,GuestDoorbell@l
         mtspr   SPRN_IVOR38,r3
         li      r3,CriticalGuestDoorbell@l
@@ -912,14 +908,8 @@ _GLOBAL(__setup_e500mc_ivors)
         mtspr   SPRN_IVOR40,r3
         li      r3,Ehvpriv@l
         mtspr   SPRN_IVOR41,r3
-skip_hv_ivors:
         sync
         blr
-no_hv:
-        lwz     r3, CPU_SPEC_FEATURES(r5)
-        rlwinm  r3, r3, 0, ~CPU_FTR_EMB_HV
-        stw     r3, CPU_SPEC_FEATURES(r5)
-        b       skip_hv_ivors
 
 #ifdef CONFIG_SPE
 /*