4 files changed, 32 insertions, 8 deletions

diff --git a/arch/mips/include/asm/cpu-info.h b/arch/mips/include/asm/cpu-info.h
index a6c9ccb33c5c..c3f4f2d2e108 100644
--- a/arch/mips/include/asm/cpu-info.h
+++ b/arch/mips/include/asm/cpu-info.h
@@ -84,6 +84,11 @@ struct cpuinfo_mips {
          * (shifted by _CACHE_SHIFT)
          */
         unsigned int            writecombine;
+        /*
+         * Simple counter to prevent enabling HTW in nested
+         * htw_start/htw_stop calls
+         */
+        unsigned int            htw_seq;
 } __attribute__((aligned(SMP_CACHE_BYTES)));
 
 extern struct cpuinfo_mips cpu_data[];
diff --git a/arch/mips/include/asm/mmu_context.h b/arch/mips/include/asm/mmu_context.h
index 87f11072f557..45914b59824c 100644
--- a/arch/mips/include/asm/mmu_context.h
+++ b/arch/mips/include/asm/mmu_context.h
@@ -25,7 +25,6 @@ do {									\
         if (cpu_has_htw) {                                              \
                 write_c0_pwbase(pgd);                                   \
                 back_to_back_c0_hazard();                               \
-                htw_reset();                                            \
         }                                                               \
 } while (0)
 
@@ -144,6 +143,7 @@ static inline void switch_mm(struct mm_struct *prev, struct mm_struct *next,
         unsigned long flags;
         local_irq_save(flags);
 
+        htw_stop();
         /* Check if our ASID is of an older version and thus invalid */
         if ((cpu_context(cpu, next) ^ asid_cache(cpu)) & ASID_VERSION_MASK)
                 get_new_mmu_context(next, cpu);
@@ -156,6 +156,7 @@ static inline void switch_mm(struct mm_struct *prev, struct mm_struct *next,
          */
         cpumask_clear_cpu(cpu, mm_cpumask(prev));
         cpumask_set_cpu(cpu, mm_cpumask(next));
+        htw_start();
 
         local_irq_restore(flags);
 }
@@ -182,6 +183,7 @@ activate_mm(struct mm_struct *prev, struct mm_struct *next)
 
         local_irq_save(flags);
 
+        htw_stop();
         /* Unconditionally get a new ASID.  */
         get_new_mmu_context(next, cpu);
 
@@ -191,6 +193,7 @@ activate_mm(struct mm_struct *prev, struct mm_struct *next)
         /* mark mmu ownership change */
         cpumask_clear_cpu(cpu, mm_cpumask(prev));
         cpumask_set_cpu(cpu, mm_cpumask(next));
+        htw_start();
 
         local_irq_restore(flags);
 }
@@ -205,6 +208,7 @@ drop_mmu_context(struct mm_struct *mm, unsigned cpu)
         unsigned long flags;
 
         local_irq_save(flags);
+        htw_stop();
 
         if (cpumask_test_cpu(cpu, mm_cpumask(mm)))  {
                 get_new_mmu_context(mm, cpu);
@@ -213,6 +217,7 @@ drop_mmu_context(struct mm_struct *mm, unsigned cpu)
                 /* will get a new context next time */
                 cpu_context(cpu, mm) = 0;
         }
+        htw_start();
         local_irq_restore(flags);
 }
 
diff --git a/arch/mips/include/asm/pgtable.h b/arch/mips/include/asm/pgtable.h
index 3aa982b50a10..845016d1cdbd 100644
--- a/arch/mips/include/asm/pgtable.h
+++ b/arch/mips/include/asm/pgtable.h
@@ -99,19 +99,31 @@ extern void paging_init(void);
 
 #define htw_stop()                                                      \
 do {                                                                    \
+        unsigned long flags;                                            \
+                                                                        \
         if (cpu_has_htw) {                                              \
-                write_c0_pwctl(read_c0_pwctl() &                        \
-                               ~(1 << MIPS_PWCTL_PWEN_SHIFT));          \
-                back_to_back_c0_hazard();                               \
+                local_irq_save(flags);                                  \
+                if(!raw_current_cpu_data.htw_seq++) {                   \
+                        write_c0_pwctl(read_c0_pwctl() &                \
+                                       ~(1 << MIPS_PWCTL_PWEN_SHIFT));  \
+                        back_to_back_c0_hazard();                       \
+                }                                                       \
+                local_irq_restore(flags);                               \
         }                                                               \
 } while(0)
 
 #define htw_start()                                                     \
 do {                                                                    \
+        unsigned long flags;                                            \
+                                                                        \
         if (cpu_has_htw) {                                              \
-                write_c0_pwctl(read_c0_pwctl() |                        \
-                               (1 << MIPS_PWCTL_PWEN_SHIFT));           \
-                back_to_back_c0_hazard();                               \
+                local_irq_save(flags);                                  \
+                if (!--raw_current_cpu_data.htw_seq) {                  \
+                        write_c0_pwctl(read_c0_pwctl() |                \
+                                       (1 << MIPS_PWCTL_PWEN_SHIFT));   \
+                        back_to_back_c0_hazard();                       \
+                }                                                       \
+                local_irq_restore(flags);                               \
         }                                                               \
 } while(0)
 
diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index 5342674842f5..228ae864c92e 100644
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -424,8 +424,10 @@ static inline unsigned int decode_config3(struct cpuinfo_mips *c)
         if (config3 & MIPS_CONF3_MSA)
                 c->ases |= MIPS_ASE_MSA;
         /* Only tested on 32-bit cores */
-        if ((config3 & MIPS_CONF3_PW) && config_enabled(CONFIG_32BIT))
+        if ((config3 & MIPS_CONF3_PW) && config_enabled(CONFIG_32BIT)) {
+                c->htw_seq = 0;
                 c->options |= MIPS_CPU_HTW;
+        }
 
         return config3 & MIPS_CONF_M;
 }