diff --git a/arch/m32r/mm/fault.c b/arch/m32r/mm/fault.c
index 9b9feb0..fc7ccdf 100644
--- a/arch/m32r/mm/fault.c
+++ b/arch/m32r/mm/fault.c
@@ -362,8 +362,10 @@
 		if (!pte_present(*pte_k))
 			goto no_context;
 
-		addr = (address & PAGE_MASK) | (error_code & ACE_INSTRUCTION);
+		addr = (address & PAGE_MASK);
+		set_thread_fault_code(error_code);
 		update_mmu_cache(NULL, addr, *pte_k);
+		set_thread_fault_code(0);
 		return;
 	}
 }
@@ -377,7 +379,7 @@
 void update_mmu_cache(struct vm_area_struct *vma, unsigned long vaddr,
 	pte_t pte)
 {
-	unsigned long *entry1, *entry2;
+	volatile unsigned long *entry1, *entry2;
 	unsigned long pte_data, flags;
 	unsigned int *entry_dat;
 	int inst = get_thread_fault_code() & ACE_INSTRUCTION;
@@ -391,30 +393,26 @@
 
 	vaddr = (vaddr & PAGE_MASK) | get_asid();
 
-#ifdef CONFIG_CHIP_OPSP
-	entry1 = (unsigned long *)ITLB_BASE;
-	for(i = 0 ; i < NR_TLB_ENTRIES; i++) {
-	        if(*entry1++ == vaddr) {
-	                pte_data = pte_val(pte);
-	                set_tlb_data(entry1, pte_data);
-	                break;
-	        }
-	        entry1++;
-	}
-	entry2 = (unsigned long *)DTLB_BASE;
-	for(i = 0 ; i < NR_TLB_ENTRIES ; i++) {
-	        if(*entry2++ == vaddr) {
-	                pte_data = pte_val(pte);
-	                set_tlb_data(entry2, pte_data);
-	                break;
-	        }
-	        entry2++;
-	}
-	local_irq_restore(flags);
-	return;
-#else
 	pte_data = pte_val(pte);
 
+#ifdef CONFIG_CHIP_OPSP
+	entry1 = (unsigned long *)ITLB_BASE;
+	for (i = 0; i < NR_TLB_ENTRIES; i++) {
+		if (*entry1++ == vaddr) {
+			set_tlb_data(entry1, pte_data);
+			break;
+		}
+		entry1++;
+	}
+	entry2 = (unsigned long *)DTLB_BASE;
+	for (i = 0; i < NR_TLB_ENTRIES; i++) {
+		if (*entry2++ == vaddr) {
+			set_tlb_data(entry2, pte_data);
+			break;
+		}
+		entry2++;
+	}
+#else
 	/*
 	 * Update TLB entries
 	 *  entry1: ITLB entry address
@@ -439,6 +437,7 @@
 		"i" (MSVA_offset), "i" (MTOP_offset), "i" (MIDXI_offset)
 		: "r4", "memory"
 	);
+#endif
 
 	if ((!inst && entry2 >= DTLB_END) || (inst && entry1 >= ITLB_END))
 		goto notfound;
@@ -482,7 +481,6 @@
 	set_tlb_data(entry1, pte_data);
 
 	goto found;
-#endif
 }
 
 /*======================================================================*