1 files changed, 16 insertions, 6 deletions

diff --git a/arch/mips/mm/tlbex.c b/arch/mips/mm/tlbex.c
index 0de0e4127d66..d1f68aadbc4c 100644
--- a/arch/mips/mm/tlbex.c
+++ b/arch/mips/mm/tlbex.c
@@ -788,10 +788,15 @@ static void __cpuinit build_r4000_tlb_refill_handler(void)
          * create the plain linear handler
          */
         if (bcm1250_m3_war()) {
-                UASM_i_MFC0(&p, K0, C0_BADVADDR);
-                UASM_i_MFC0(&p, K1, C0_ENTRYHI);
+                unsigned int segbits = 44;
+
+                uasm_i_dmfc0(&p, K0, C0_BADVADDR);
+                uasm_i_dmfc0(&p, K1, C0_ENTRYHI);
                 uasm_i_xor(&p, K0, K0, K1);
-                UASM_i_SRL(&p, K0, K0, PAGE_SHIFT + 1);
+                uasm_i_dsrl32(&p, K1, K0, 62 - 32);
+                uasm_i_dsrl(&p, K0, K0, 12 + 1);
+                uasm_i_dsll32(&p, K0, K0, 64 + 12 + 1 - segbits - 32);
+                uasm_i_or(&p, K0, K0, K1);
                 uasm_il_bnez(&p, &r, K0, label_leave);
                 /* No need for uasm_i_nop */
         }
@@ -1312,10 +1317,15 @@ static void __cpuinit build_r4000_tlb_load_handler(void)
         memset(relocs, 0, sizeof(relocs));
 
         if (bcm1250_m3_war()) {
-                UASM_i_MFC0(&p, K0, C0_BADVADDR);
-                UASM_i_MFC0(&p, K1, C0_ENTRYHI);
+                unsigned int segbits = 44;
+
+                uasm_i_dmfc0(&p, K0, C0_BADVADDR);
+                uasm_i_dmfc0(&p, K1, C0_ENTRYHI);
                 uasm_i_xor(&p, K0, K0, K1);
-                UASM_i_SRL(&p, K0, K0, PAGE_SHIFT + 1);
+                uasm_i_dsrl32(&p, K1, K0, 62 - 32);
+                uasm_i_dsrl(&p, K0, K0, 12 + 1);
+                uasm_i_dsll32(&p, K0, K0, 64 + 12 + 1 - segbits - 32);
+                uasm_i_or(&p, K0, K0, K1);
                 uasm_il_bnez(&p, &r, K0, label_leave);
                 /* No need for uasm_i_nop */
         }