3 files changed, 41 insertions, 21 deletions

diff --git a/arch/arm/mm/Kconfig b/arch/arm/mm/Kconfig
index ed15f876c725..330814d1ee25 100644
--- a/arch/arm/mm/Kconfig
+++ b/arch/arm/mm/Kconfig
@@ -735,6 +735,14 @@ config CACHE_FEROCEON_L2
         help
           This option enables the Feroceon L2 cache controller.
 
+config CACHE_FEROCEON_L2_WRITETHROUGH
+        bool "Force Feroceon L2 cache write through"
+        depends on CACHE_FEROCEON_L2
+        default n
+        help
+          Say Y here to use the Feroceon L2 cache in writethrough mode.
+          Unless you specifically require this, say N for writeback mode.
+
 config CACHE_L2X0
         bool "Enable the L2x0 outer cache controller"
         depends on REALVIEW_EB_ARM11MP || MACH_REALVIEW_PB11MP || MACH_REALVIEW_PB1176
diff --git a/arch/arm/mm/cache-feroceon-l2.c b/arch/arm/mm/cache-feroceon-l2.c
index 7b5a25d81576..13cdae8b0d44 100644
--- a/arch/arm/mm/cache-feroceon-l2.c
+++ b/arch/arm/mm/cache-feroceon-l2.c
@@ -48,11 +48,12 @@ static inline void l2_clean_mva_range(unsigned long start, unsigned long end)
          * L2 is PIPT and range operations only do a TLB lookup on
          * the start address.
          */
-        BUG_ON((start ^ end) & ~(PAGE_SIZE - 1));
+        BUG_ON((start ^ end) >> PAGE_SHIFT);
 
         raw_local_irq_save(flags);
-        __asm__("mcr p15, 1, %0, c15, c9, 4" : : "r" (start));
-        __asm__("mcr p15, 1, %0, c15, c9, 5" : : "r" (end));
+        __asm__("mcr p15, 1, %0, c15, c9, 4\n\t"
+                "mcr p15, 1, %1, c15, c9, 5"
+                : : "r" (start), "r" (end));
         raw_local_irq_restore(flags);
 }
 
@@ -80,11 +81,12 @@ static inline void l2_inv_mva_range(unsigned long start, unsigned long end)
          * L2 is PIPT and range operations only do a TLB lookup on
          * the start address.
          */
-        BUG_ON((start ^ end) & ~(PAGE_SIZE - 1));
+        BUG_ON((start ^ end) >> PAGE_SHIFT);
 
         raw_local_irq_save(flags);
-        __asm__("mcr p15, 1, %0, c15, c11, 4" : : "r" (start));
-        __asm__("mcr p15, 1, %0, c15, c11, 5" : : "r" (end));
+        __asm__("mcr p15, 1, %0, c15, c11, 4\n\t"
+                "mcr p15, 1, %1, c15, c11, 5"
+                : : "r" (start), "r" (end));
         raw_local_irq_restore(flags);
 }
 
@@ -205,7 +207,7 @@ static void feroceon_l2_flush_range(unsigned long start, unsigned long end)
  * time.  These are necessary because the L2 cache can only be enabled
  * or disabled while the L1 Dcache and Icache are both disabled.
  */
-static void __init invalidate_and_disable_dcache(void)
+static int __init flush_and_disable_dcache(void)
 {
         u32 cr;
 
@@ -217,7 +219,9 @@ static void __init invalidate_and_disable_dcache(void)
                 flush_cache_all();
                 set_cr(cr & ~CR_C);
                 raw_local_irq_restore(flags);
+                return 1;
         }
+        return 0;
 }
 
 static void __init enable_dcache(void)
@@ -225,18 +229,17 @@ static void __init enable_dcache(void)
         u32 cr;
 
         cr = get_cr();
-        if (!(cr & CR_C))
-                set_cr(cr | CR_C);
+        set_cr(cr | CR_C);
 }
 
 static void __init __invalidate_icache(void)
 {
         int dummy;
 
-        __asm__ __volatile__("mcr p15, 0, %0, c7, c5, 0\n" : "=r" (dummy));
+        __asm__ __volatile__("mcr p15, 0, %0, c7, c5, 0" : "=r" (dummy));
 }
 
-static void __init invalidate_and_disable_icache(void)
+static int __init invalidate_and_disable_icache(void)
 {
         u32 cr;
 
@@ -244,7 +247,9 @@ static void __init invalidate_and_disable_icache(void)
         if (cr & CR_I) {
                 set_cr(cr & ~CR_I);
                 __invalidate_icache();
+                return 1;
         }
+        return 0;
 }
 
 static void __init enable_icache(void)
@@ -252,8 +257,7 @@ static void __init enable_icache(void)
         u32 cr;
 
         cr = get_cr();
-        if (!(cr & CR_I))
-                set_cr(cr | CR_I);
+        set_cr(cr | CR_I);
 }
 
 static inline u32 read_extra_features(void)
@@ -291,13 +295,17 @@ static void __init enable_l2(void)
 
         u = read_extra_features();
         if (!(u & 0x00400000)) {
+                int i, d;
+
                 printk(KERN_INFO "Feroceon L2: Enabling L2\n");
 
-                invalidate_and_disable_dcache();
-                invalidate_and_disable_icache();
+                d = flush_and_disable_dcache();
+                i = invalidate_and_disable_icache();
                 write_extra_features(u | 0x00400000);
-                enable_icache();
-                enable_dcache();
+                if (i)
+                        enable_icache();
+                if (d)
+                        enable_dcache();
         }
 }
 
diff --git a/arch/arm/mm/proc-feroceon.S b/arch/arm/mm/proc-feroceon.S
index 2b8bb383755e..0fe1f8fc3488 100644
--- a/arch/arm/mm/proc-feroceon.S
+++ b/arch/arm/mm/proc-feroceon.S
@@ -80,7 +80,8 @@ ENTRY(cpu_feroceon_proc_fin)
         msr     cpsr_c, ip
         bl      feroceon_flush_kern_cache_all
 
-#if defined(CONFIG_CACHE_FEROCEON_L2) && !defined(CONFIG_L2_CACHE_WRITETHROUGH)
+#if defined(CONFIG_CACHE_FEROCEON_L2) && \
+        !defined(CONFIG_CACHE_FEROCEON_L2_WRITETHROUGH)
         mov     r0, #0
         mcr     p15, 1, r0, c15, c9, 0          @ clean L2
         mcr     p15, 0, r0, c7, c10, 4          @ drain WB
@@ -389,7 +390,8 @@ ENTRY(feroceon_range_cache_fns)
 
         .align  5
 ENTRY(cpu_feroceon_dcache_clean_area)
-#if defined(CONFIG_CACHE_FEROCEON_L2) && !defined(CONFIG_L2_CACHE_WRITETHROUGH)
+#if defined(CONFIG_CACHE_FEROCEON_L2) && \
+        !defined(CONFIG_CACHE_FEROCEON_L2_WRITETHROUGH)
         mov     r2, r0
         mov     r3, r1
 #endif
@@ -397,7 +399,8 @@ ENTRY(cpu_feroceon_dcache_clean_area)
         add     r0, r0, #CACHE_DLINESIZE
         subs    r1, r1, #CACHE_DLINESIZE
         bhi     1b
-#if defined(CONFIG_CACHE_FEROCEON_L2) && !defined(CONFIG_L2_CACHE_WRITETHROUGH)
+#if defined(CONFIG_CACHE_FEROCEON_L2) && \
+        !defined(CONFIG_CACHE_FEROCEON_L2_WRITETHROUGH)
 1:      mcr     p15, 1, r2, c15, c9, 1          @ clean L2 entry
         add     r2, r2, #CACHE_DLINESIZE
         subs    r3, r3, #CACHE_DLINESIZE
@@ -449,7 +452,8 @@ ENTRY(cpu_feroceon_set_pte_ext)
         armv3_set_pte_ext wc_disable=0
         mov     r0, r0
         mcr     p15, 0, r0, c7, c10, 1          @ clean D entry
-#if defined(CONFIG_CACHE_FEROCEON_L2) && !defined(CONFIG_L2_CACHE_WRITETHROUGH)
+#if defined(CONFIG_CACHE_FEROCEON_L2) && \
+        !defined(CONFIG_CACHE_FEROCEON_L2_WRITETHROUGH)
         mcr     p15, 1, r0, c15, c9, 1          @ clean L2 entry
 #endif
         mcr     p15, 0, r0, c7, c10, 4          @ drain WB