1 files changed, 100 insertions, 0 deletions

diff --git a/arch/mips/mm/tlbex.c b/arch/mips/mm/tlbex.c
index 3b3822afb059..4d46d3787576 100644
--- a/arch/mips/mm/tlbex.c
+++ b/arch/mips/mm/tlbex.c
@@ -29,6 +29,7 @@
 #include <linux/init.h>
 #include <linux/cache.h>
 
+#include <asm/mmu_context.h>
 #include <asm/cacheflush.h>
 #include <asm/pgtable.h>
 #include <asm/war.h>
@@ -305,6 +306,78 @@ static struct uasm_reloc relocs[128] __cpuinitdata;
 static int check_for_high_segbits __cpuinitdata;
 #endif
 
+static void __cpuinit insn_fixup(unsigned int **start, unsigned int **stop,
+                                        unsigned int i_const)
+{
+        unsigned int **p;
+
+        for (p = start; p < stop; p++) {
+#ifndef CONFIG_CPU_MICROMIPS
+                unsigned int *ip;
+
+                ip = *p;
+                *ip = (*ip & 0xffff0000) | i_const;
+#else
+                unsigned short *ip;
+
+                ip = ((unsigned short *)((unsigned int)*p - 1));
+                if ((*ip & 0xf000) == 0x4000) {
+                        *ip &= 0xfff1;
+                        *ip |= (i_const << 1);
+                } else if ((*ip & 0xf000) == 0x6000) {
+                        *ip &= 0xfff1;
+                        *ip |= ((i_const >> 2) << 1);
+                } else {
+                        ip++;
+                        *ip = i_const;
+                }
+#endif
+                local_flush_icache_range((unsigned long)ip,
+                                         (unsigned long)ip + sizeof(*ip));
+        }
+}
+
+#define asid_insn_fixup(section, const)                                 \
+do {                                                                    \
+        extern unsigned int *__start_ ## section;                       \
+        extern unsigned int *__stop_ ## section;                        \
+        insn_fixup(&__start_ ## section, &__stop_ ## section, const);   \
+} while(0)
+
+/*
+ * Caller is assumed to flush the caches before the first context switch.
+ */
+static void __cpuinit setup_asid(unsigned int inc, unsigned int mask,
+                                 unsigned int version_mask,
+                                 unsigned int first_version)
+{
+        extern asmlinkage void handle_ri_rdhwr_vivt(void);
+        unsigned long *vivt_exc;
+
+#ifdef CONFIG_CPU_MICROMIPS
+        /*
+         * Worst case optimised microMIPS addiu instructions support
+         * only a 3-bit immediate value.
+         */
+        if(inc > 7)
+                panic("Invalid ASID increment value!");
+#endif
+        asid_insn_fixup(__asid_inc, inc);
+        asid_insn_fixup(__asid_mask, mask);
+        asid_insn_fixup(__asid_version_mask, version_mask);
+        asid_insn_fixup(__asid_first_version, first_version);
+
+        /* Patch up the 'handle_ri_rdhwr_vivt' handler. */
+        vivt_exc = (unsigned long *) &handle_ri_rdhwr_vivt;
+#ifdef CONFIG_CPU_MICROMIPS
+        vivt_exc = (unsigned long *)((unsigned long) vivt_exc - 1);
+#endif
+        vivt_exc++;
+        *vivt_exc = (*vivt_exc & ~mask) | mask;
+
+        current_cpu_data.asid_cache = first_version;
+}
+
 static int check_for_high_segbits __cpuinitdata;
 
 static unsigned int kscratch_used_mask __cpuinitdata;
@@ -2030,6 +2103,13 @@ static void __cpuinit build_r4000_tlb_load_handler(void)
 
         uasm_l_nopage_tlbl(&l, p);
         build_restore_work_registers(&p);
+#ifdef CONFIG_CPU_MICROMIPS
+        if ((unsigned long)tlb_do_page_fault_0 & 1) {
+                uasm_i_lui(&p, K0, uasm_rel_hi((long)tlb_do_page_fault_0));
+                uasm_i_addiu(&p, K0, K0, uasm_rel_lo((long)tlb_do_page_fault_0));
+                uasm_i_jr(&p, K0);
+        } else
+#endif
         uasm_i_j(&p, (unsigned long)tlb_do_page_fault_0 & 0x0fffffff);
         uasm_i_nop(&p);
 
@@ -2077,6 +2157,13 @@ static void __cpuinit build_r4000_tlb_store_handler(void)
 
         uasm_l_nopage_tlbs(&l, p);
         build_restore_work_registers(&p);
+#ifdef CONFIG_CPU_MICROMIPS
+        if ((unsigned long)tlb_do_page_fault_1 & 1) {
+                uasm_i_lui(&p, K0, uasm_rel_hi((long)tlb_do_page_fault_1));
+                uasm_i_addiu(&p, K0, K0, uasm_rel_lo((long)tlb_do_page_fault_1));
+                uasm_i_jr(&p, K0);
+        } else
+#endif
         uasm_i_j(&p, (unsigned long)tlb_do_page_fault_1 & 0x0fffffff);
         uasm_i_nop(&p);
 
@@ -2125,6 +2212,13 @@ static void __cpuinit build_r4000_tlb_modify_handler(void)
 
         uasm_l_nopage_tlbm(&l, p);
         build_restore_work_registers(&p);
+#ifdef CONFIG_CPU_MICROMIPS
+        if ((unsigned long)tlb_do_page_fault_1 & 1) {
+                uasm_i_lui(&p, K0, uasm_rel_hi((long)tlb_do_page_fault_1));
+                uasm_i_addiu(&p, K0, K0, uasm_rel_lo((long)tlb_do_page_fault_1));
+                uasm_i_jr(&p, K0);
+        } else
+#endif
         uasm_i_j(&p, (unsigned long)tlb_do_page_fault_1 & 0x0fffffff);
         uasm_i_nop(&p);
 
@@ -2162,6 +2256,7 @@ void __cpuinit build_tlb_refill_handler(void)
         case CPU_TX3922:
         case CPU_TX3927:
 #ifndef CONFIG_MIPS_PGD_C0_CONTEXT
+                setup_asid(0x40, 0xfc0, 0xf000, ASID_FIRST_VERSION_R3000);
                 if (cpu_has_local_ebase)
                         build_r3000_tlb_refill_handler();
                 if (!run_once) {
@@ -2187,6 +2282,11 @@ void __cpuinit build_tlb_refill_handler(void)
                 break;
 
         default:
+#ifndef CONFIG_MIPS_MT_SMTC
+                setup_asid(0x1, 0xff, 0xff00, ASID_FIRST_VERSION_R4000);
+#else
+                setup_asid(0x1, smtc_asid_mask, 0xff00, ASID_FIRST_VERSION_R4000);
+#endif
                 if (!run_once) {
                         scratch_reg = allocate_kscratch();
 #ifdef CONFIG_MIPS_PGD_C0_CONTEXT