target-mips: Add TLB Refill exception interpreter

Exception interpreter supersedes the TLB optimisation which was
initially written to optimize the kernel refill handler
and it was pretty hard to maintain it because the table walker
depends on the exact layout of the refill handler code.
The idea behind this approach is not to make any assumptions
about the refill handler and let the interpreter execute it
as it is.

Change-Id: I7acaa77215aae721937c3c5f10ee77da7f6d4cca
Signed-off-by: Miodrag Dinic <miodrag.dinic@imgtec.com>

3 files changed, 916 insertions, 487 deletions

diff --git a/target-mips/cpu.h b/target-mips/cpu.h
index b6494518f4..21c8cfc50e 100644
--- a/target-mips/cpu.h
+++ b/target-mips/cpu.h
@@ -598,6 +598,29 @@ struct CPUMIPSState {
 #include "cpu-qom.h"
 
 #if !defined(CONFIG_USER_ONLY)
+
+typedef struct {
+    target_long gpr_reg[32];
+    uint32_t pc;
+    uint32_t branch_addr;
+    target_ulong CP0_EntryLo0;
+    target_ulong CP0_EntryLo1;
+    target_ulong CP0_BadVAddr;
+    target_ulong CP0_EntryHi;
+    target_ulong CP0_Context;
+    target_ulong CP0_XContext;
+    target_ulong CP0_PageMask;
+    target_ulong CP0_PageGrain;
+    target_ulong CP0_PageGrain_rw_bitmask;
+    target_ulong CP0_Cause;
+    int32_t CP0_Index;
+    target_ulong CP0_KScratch[MIPS_KSCRATCH_NUM];
+    int do_tlbwr;
+} CPUInterpreterContext;
+
+extern CPUInterpreterContext CPU;
+
+int tlb_exception_interpreter(CPUMIPSState *env, uint32_t *handler, uint32_t size);
 int no_mmu_map_address (CPUMIPSState *env, hwaddr *physical, int *prot,
                         target_ulong address, int rw, int access_type);
 int fixed_mmu_map_address (CPUMIPSState *env, hwaddr *physical, int *prot,
@@ -921,5 +944,4 @@ static inline void compute_hflags(CPUMIPSState *env)
         }
     }
 }
-
 #endif /* !defined (__MIPS_CPU_H__) */
diff --git a/target-mips/helper.c b/target-mips/helper.c
index e3634d5347..48181f2296 100644
--- a/target-mips/helper.c
+++ b/target-mips/helper.c
@@ -313,447 +313,70 @@ static void raise_mmu_exception(CPUMIPSState *env, target_ulong address,
 
 #if !defined(CONFIG_USER_ONLY)
 
-#define MUST_HAVE_FASTTLB 1
+#define TLB_HANDLER_SIZE (0x40)
 
-#define KERNEL_64_BIT           (1 << 0)
-#define KERNEL_PGD_C0_CONTEXT   (1 << 1)
-#define KERNEL_HUGE_TLB         (1 << 2)
-#define KERNEL_RIXI             (1 << 3)
-#define KERNEL_HIGHMEM          (1 << 4)
+typedef enum { PROBE = 0, USEFASTTLB, USESLOWTLB } interpreter_state;
 
-/* TLB maintenance PTE software flags.
- *
- * Low bits are: CCC D V G RI XI [S H] A W R M(=F) P
- * TLB refill will do a ROTR 7/9 (in case of cpu_has_rixi),
- * or SRL/DSRL 7/9 to strip low bits.
- * PFN size in high bits is 49 or 51 bit --> 512TB or 4*512TB for 4KB pages
- *
- * take a look at <KERNEL>/arch/mips/include/asm/pgtable-bits.h
- */
-#define PTE_PAGE_PRESENT_SHIFT      (0)
-#define PTE_PAGE_PRESENT            (1 << PTE_PAGE_PRESENT_SHIFT)
-
-#define PTE_PAGE_MODIFIED_SHIFT     (PTE_PAGE_PRESENT_SHIFT + 1)
-#define PTE_PAGE_MODIFIED           (1 << PTE_PAGE_MODIFIED_SHIFT)
-
-#define PTE_PAGE_FILE               (PTE_PAGE_MODIFIED)
-
-#define PTE_PAGE_READ_SHIFT         (PTE_PAGE_MODIFIED_SHIFT + 1)
-#define PTE_PAGE_READ               (1 << PTE_PAGE_READ_SHIFT)
+typedef struct {
+    interpreter_state state;
+    char name[32];
+    int32_t ebase;
+    uint32_t handler[TLB_HANDLER_SIZE];
+} TLBHandlerInfo;
 
-#define PTE_PAGE_WRITE_SHIFT        (PTE_PAGE_READ_SHIFT + 1)
-#define PTE_PAGE_WRITE              (1 << PTE_PAGE_WRITE_SHIFT)
-
-#define PTE_PAGE_ACCESSED_SHIFT     (PTE_PAGE_WRITE_SHIFT + 1)
-#define PTE_PAGE_ACCESSED           (1 << PTE_PAGE_ACCESSED_SHIFT)
-
-/* Huge TLB support maintenance bits */
-#define PTE_PAGE_HUGE_SHIFT         (PTE_PAGE_ACCESSED_SHIFT + 1)
-#define PTE_PAGE_HUGE               (1 << PTE_PAGE_HUGE_SHIFT)
-
-#define PTE_PAGE_SPLITTING_SHIFT    (PTE_PAGE_HUGE_SHIFT + 1)
-#define PTE_PAGE_SPLITTING          (1 << PTE_PAGE_SPLITTING_SHIFT)
+// #define DUMP_HANDLER 1
 
+#if defined(DUMP_HANDLER)
 /*
- * Get the pgd_current from TLB refill handler
- * The kernel refill handler is generated by
- * function build_r4000_tlb_refill_handler.
+ * Print the contents of TLBHandlerInfo buffer holding
+ * holding the TLB exception handler
  */
-typedef void (*pagetable_walk_t)(CPUState *cs,
-                                  target_ulong pgd_addr, target_ulong vaddr,
-                                  target_ulong *entrylo0, target_ulong *entrylo1,
-                                  target_ulong *sw_pte_lo0, target_ulong *sw_pte_lo1);
-static struct {
-    enum {PROBE, USEFASTTLB, USESLOWTLB} state;
-    uint32_t config;
-    pagetable_walk_t pagetable_walk;
-    target_ulong pgd_current_p;
-    target_ulong swapper_pg_dir;
-    int softshift;
-} linux_pte_info = {0};
-
-static inline target_ulong cpu_debug_translate_address(CPUMIPSState *env, target_ulong address) {
-
-#if defined(TARGET_MIPS64)
-    if (!(linux_pte_info.config & KERNEL_64_BIT))
-        address = (int32_t)address;
-#endif
-
-    if (address <= USEG_LIMIT) {
-        /* useg */
-        if (env->CP0_Status & (1 << CP0St_ERL)) {
-            return address & 0xFFFFFFFF;
-        } else {
-            return address;
-        }
-#if defined(TARGET_MIPS64)
-    } else if (address < 0x4000000000000000ULL) {
-        return address;
-    } else if (address < 0x8000000000000000ULL) {
-        return address;
-    } else if (address < 0xC000000000000000ULL) {
-        /* xkphys */
-        if ((address & 0x07FFFFFFFFFFFFFFULL) <= env->PAMask) {
-            return address & env->PAMask;
-        } else {
-            return address;
-        }
-    } else if (address < 0xFFFFFFFF80000000ULL) {
-        /* xkseg */
-        return address;
-#endif
-    } else if (address < (int32_t)KSEG1_BASE) {
-        /* kseg0 */
-        return address - (int32_t)KSEG0_BASE;
-    } else if (address < (int32_t)KSEG2_BASE) {
-        /* kseg1 */
-        return address - (int32_t)KSEG1_BASE;
-    } else
-        return address;
-}
-
-static inline target_ulong get_mtc0_entrylo_mask(const CPUMIPSState *env)
-{
-#if defined(TARGET_MIPS64)
-    return env->PAMask >> 6;
-#else
-    return (env->PAMask >> 6) & 0x3FFFFFFF;
-#endif
-}
-
-static inline void pagetable_walk32(CPUState *cs,
-                                  target_ulong pgd_addr, target_ulong vaddr,
-                                  target_ulong *entrylo0, target_ulong *entrylo1,
-                                  target_ulong *sw_pte_lo0, target_ulong *sw_pte_lo1)
+static inline void dump_handler(TLBHandlerInfo *handler_info)
 {
-    target_ulong ptw_phys, pt_addr, index;
-    MIPSCPU *cpu = MIPS_CPU(cs);
-    CPUMIPSState *env = &cpu->env;
-
-#if defined(TARGET_MIPS64)
-    /* workaround when running a 32bit
-     * emulation with the 64bit target emulator
-     */
-    vaddr = (uint32_t)vaddr;
-#endif
-
-    /* 32bit PTE lookup */
-    ptw_phys = cpu_debug_translate_address(env, pgd_addr);
-    index = (vaddr >> 22) << 2; /* Use bits 31..22 to index pgd */
-    ptw_phys += index;
-
-    pt_addr = ldl_phys(cs->as, ptw_phys);
-
-    ptw_phys = cpu_debug_translate_address(env, pt_addr);
-    index = ((vaddr >> 13) & 0x1ff) << 3; /* Use bits 21..13 to index pgt */
-    ptw_phys += index;
-
-    /* Get the entrylo values from pgt */
-    if (linux_pte_info.config & KERNEL_RIXI) {
-        target_ulong mask = ~(-1 << linux_pte_info.softshift);
-
-        *entrylo0 = ldl_phys(cs->as, ptw_phys);
-        if (sw_pte_lo0) {
-            if (linux_pte_info.config & KERNEL_HIGHMEM)
-                *sw_pte_lo0 = *entrylo0 & ~(-1 << (linux_pte_info.softshift + 4));
-            else
-                *sw_pte_lo0 = *entrylo0 & ~(-1 << (linux_pte_info.softshift));
-        }
-        if (linux_pte_info.config & KERNEL_HIGHMEM)
-            *entrylo0 = (*entrylo0) >> 4;
-        *entrylo0 = ((*entrylo0 & mask) << (32 - linux_pte_info.softshift)) |
-                    ((uint32_t)*entrylo0 >> linux_pte_info.softshift);
-        *entrylo0 = (*entrylo0 & get_mtc0_entrylo_mask(env)) |
-                    ((*entrylo0 & (env->CP0_PageGrain & (3u << CP0PG_XIE))) <<
-                    (CP0EnLo_XI - 30));
-
-        *entrylo1 = ldl_phys(cs->as, ptw_phys + 4);
-        if (sw_pte_lo1) {
-            if (linux_pte_info.config & KERNEL_HIGHMEM)
-                *sw_pte_lo1 = *entrylo1 & ~(-1 << (linux_pte_info.softshift + 4));
-            else
-                *sw_pte_lo1 = *entrylo1 & ~(-1 << (linux_pte_info.softshift));
-        }
-        if (linux_pte_info.config & KERNEL_HIGHMEM)
-            *entrylo1 = (*entrylo1) >> 4;
-        *entrylo1 = ((*entrylo1 & mask) << (32 - linux_pte_info.softshift)) |
-                    ((uint32_t)*entrylo1 >> linux_pte_info.softshift);
-        *entrylo1 = (*entrylo1 & get_mtc0_entrylo_mask(env)) |
-                    ((*entrylo1 & (env->CP0_PageGrain & (3u << CP0PG_XIE))) <<
-                    (CP0EnLo_XI - 30));
-    } else {
-        *entrylo0 = ldl_phys(cs->as, ptw_phys);
-        if (sw_pte_lo0) {
-            *sw_pte_lo0 = *entrylo0 & ((target_ulong)(1 << linux_pte_info.softshift) - 1);
-        }
-        *entrylo0 >>= linux_pte_info.softshift;
-
-        *entrylo1 = ldl_phys(cs->as, ptw_phys + 4);
-        if (sw_pte_lo1) {
-            *sw_pte_lo1 = *entrylo1 & ((target_ulong)(1 << linux_pte_info.softshift) - 1);
-        }
-        *entrylo1 >>= linux_pte_info.softshift;
+    int i;
+    fprintf(stderr, "\n===========================================\n");
+    fprintf(stderr, "%s exception handler\n", handler_info->name);
+    fprintf(stderr, "===========================================\n");
+    for (i = 0; i < TLB_HANDLER_SIZE; i++) {
+        fprintf(stderr, "0x" TARGET_FMT_lx " %08x\n",
+                (target_ulong)(handler_info->ebase + (i << 2)),
+                handler_info->handler[i]);
     }
+    fprintf(stderr, "===========================================\n\n");
 }
-
-static inline void pagetable_walk64(CPUState *cs,
-                                  target_ulong pgd_addr, target_ulong vaddr,
-                                  target_ulong *entrylo0, target_ulong *entrylo1,
-                                  target_ulong *sw_pte_lo0, target_ulong *sw_pte_lo1)
+#else
+static inline void dump_handler(TLBHandlerInfo *handler_info)
 {
-    MIPSCPU *cpu = MIPS_CPU(cs);
-    CPUMIPSState *env = &cpu->env;
-    target_ulong ptw_phys, pt_addr, index;
-
-    pgd_addr = cpu_debug_translate_address(env, pgd_addr);
-    index = ((uint64_t)vaddr >> 0x1b) & 0x1ff8;
-    pgd_addr += index;
-
-    pgd_addr = ldq_phys(cs->as, pgd_addr);
-
-    ptw_phys = cpu_debug_translate_address(env, pgd_addr);
-    index = ((uint64_t)vaddr >> 0x12) & 0xff8;
-    ptw_phys += index;
-
-    pt_addr = ldq_phys(cs->as, ptw_phys);
-
-    ptw_phys = cpu_debug_translate_address(env, pt_addr);
-    index = (((vaddr & 0xC00000000000ULL) >> (55 - env->SEGBITS)) |
-             ((vaddr & ((1ULL << env->SEGBITS) - 1) & 0xFFFFFFFFFFFFE000ULL) >> 9)) & 0xff0;
-    ptw_phys += index;
-
-    if (linux_pte_info.config & KERNEL_RIXI) {
-        target_ulong mask = ~(-1 << linux_pte_info.softshift);
-
-        *entrylo0 = ldq_phys(cs->as, ptw_phys);
-        if (sw_pte_lo0) {
-            *sw_pte_lo0 = *entrylo0 & ((target_ulong)(1 << linux_pte_info.softshift) - 1);
-        }
-        *entrylo0 = ((*entrylo0 & mask) << (64 - linux_pte_info.softshift)) |
-                     ((uint64_t)*entrylo0 >> linux_pte_info.softshift);
-        *entrylo0 = (*entrylo0 & get_mtc0_entrylo_mask(env)) |
-                    (*entrylo0 & ((env->CP0_PageGrain & (3ull << CP0PG_XIE)) << 32));
-
-        *entrylo1 = ldq_phys(cs->as, ptw_phys + 8);
-        if (sw_pte_lo1) {
-            *sw_pte_lo1 = *entrylo1 & ((target_ulong)(1 << linux_pte_info.softshift) - 1);
-        }
-        *entrylo1 = ((*entrylo1 & mask) << (64 - linux_pte_info.softshift)) |
-                     ((uint64_t)*entrylo1 >> linux_pte_info.softshift);
-        *entrylo1 = (*entrylo1 & get_mtc0_entrylo_mask(env)) |
-                    (*entrylo1 & ((env->CP0_PageGrain & (3ull << CP0PG_XIE)) << 32));
-    } else {
-        /* Get the entrylo values from pgt */
-        *entrylo0 = ldq_phys(cs->as, ptw_phys);
-        if (sw_pte_lo0) {
-            *sw_pte_lo0 = *entrylo0 & ((target_ulong)(1 << linux_pte_info.softshift) - 1);
-        }
-        *entrylo0 >>= linux_pte_info.softshift;
-
-        *entrylo1 = ldq_phys(cs->as, ptw_phys + 8);
-        if (sw_pte_lo1) {
-            *sw_pte_lo1 = *entrylo1 & ((target_ulong)(1 << linux_pte_info.softshift) - 1);
-        }
-        *entrylo1 >>= linux_pte_info.softshift;
-    }
+    ;
 }
-
-static inline target_ulong cpu_mips_get_pgd(CPUState *cs, target_long bad_vaddr)
-{
-    MIPSCPU *cpu = MIPS_CPU(cs);
-    CPUMIPSState *env = &cpu->env;
-
-    if (unlikely(linux_pte_info.state == PROBE)) {
-        int i;
-        uint32_t lui_ins, lw_ins, srl_ins, config;
-        target_ulong address;
-        uint32_t ebase;
-
-        /*
-         * The exact TLB refill code varies depeing on the kernel version
-         * and configuration. Examins the TLB handler to extract
-         * pgd_current_p and the shift required to convert in memory PTE
-         * to TLB format
-         */
-        static struct {
-            uint32_t config;
-            struct {
-                uint32_t off;
-                uint32_t op;
-                uint32_t mask;
-            } lui, lw, srl;
-        } handlers[] = {
-            /* 3.10+ 32-bit R6 Kernel */
-            {
-                KERNEL_RIXI,
-                {0x00, 0x3c1b0000, 0xffff0000}, /* 0x3c1b803f : lui k1,%hi(pgd_current_p) */
-                {0x08, 0x8f7b0000, 0xffff0000}, /* 0x8f7b3000 : lw  k1,%lo(k1) */
-                {0x2c, 0x003ad002, 0xfffff83f}  /* 0x003ad082 : ror k0,k0,#shift */
-            },
-            /* 3.10+ 32-bit R6 Kernel */
-            {
-                KERNEL_RIXI | KERNEL_HIGHMEM,
-                {0x00, 0x3c1b0000, 0xffff0000}, /* 0x3c1b803f : lui k1,%hi(pgd_current_p) */
-                {0x08, 0x8f7b0000, 0xffff0000}, /* 0x8f7b3000 : lw  k1,%lo(k1) */
-                {0x34, 0x003ad002, 0xfffff83f}  /* 0x003ad082 : ror k0,k0,#shift */
-            },
-            /* 3.10+ 32-bit R6 Kernel */
-            {
-                KERNEL_RIXI | KERNEL_HIGHMEM,
-                {0x00, 0x3c1b0000, 0xffff0000}, /* 0x3c1b803f : lui k1,%hi(pgd_current_p) */
-                {0x08, 0x8f7b0000, 0xffff0000}, /* 0x8f7b3000 : lw  k1,%lo(k1) */
-                {0x38, 0x003ad002, 0xfffff83f}  /* 0x003ad082 : ror k0,k0,#shift */
-            },
-            /* 3.10+ 32-bit R2 Kernel */
-            {
-                0,
-                {0x00, 0x3c1b0000, 0xffff0000}, /* 0x3c1b803f : lui k1,%hi(pgd_current_p) */
-                {0x08, 0x8f7b0000, 0xffff0000}, /* 0x8f7b3000 : lw  k1,%lo(k1) */
-                {0x2c, 0x001ad002, 0xfffff83f}  /* 0x001ad002 : srl k0,k0,#shift */
-            },
-            /* 3.10+ 64-bit R2 Kernel */
-            {
-                KERNEL_64_BIT | KERNEL_PGD_C0_CONTEXT,
-                {0x04, 0x3c1b0000, 0xffff0000}, /* 0x3c1b0000 : lui  k1,%hi(swapper_pg_dir) */
-                {0xac, 0xdf7b0000, 0xffff0000}, /* 0xdf7b0000 : ld   k1,0(k1) */
-                {0xd4, 0x001ad03a, 0xfffff83f}  /* 0x001ad03a : dsrl k0,k0,#shift */
-            },
-            /* 3.10+ 64-bit R6 Kernel */
-            {
-                KERNEL_64_BIT | KERNEL_RIXI,
-                {0x8c, 0x3c1b0000, 0xffff0000}, /* 0x3c1b0000 : lui  k1,%hi(pgd_current_p) */
-                {0x90, 0xdf7b0000, 0xffff0000}, /* 0xdf7b0000 : ld   k1,%lo(k1) */
-                {0xcc, 0x003ad03a, 0xfffff83f}  /* 0x003ad03a : dror k0,k0,#shift */
-            }
-        };
-
-        ebase = env->CP0_EBase - 0x80000000;
-        linux_pte_info.config = 0;
-
-        /* Match the kernel TLB refill exception handler against known code */
-        for (i = 0; i < sizeof(handlers)/sizeof(handlers[0]); i++) {
-            config = handlers[i].config;
-            lui_ins = ldl_phys(cs->as, ebase + handlers[i].lui.off);
-            lw_ins = ldl_phys(cs->as, ebase + handlers[i].lw.off);
-            srl_ins = ldl_phys(cs->as, ebase + handlers[i].srl.off);
-            if (((lui_ins & handlers[i].lui.mask) == handlers[i].lui.op) &&
-                ((lw_ins & handlers[i].lw.mask) == handlers[i].lw.op) &&
-                ((srl_ins & handlers[i].srl.mask) == handlers[i].srl.op))
-                break;
-        }
-        if (i >= sizeof(handlers)/sizeof(handlers[0])) {
-#if defined(MUST_HAVE_FASTTLB)
-                printf("TLBMiss handler dump:\n");
-            for (i = 0; i < 0x100; i+= 4)
-                printf("0x%08x: 0x%08x\n", ebase + i, ldl_phys(cs->as, ebase + i));
-            fprintf(stderr, "TLBMiss handler signature not recognised\n");
-            exit(1);
 #endif
-            fprintf(stderr, "TLBMiss handler signature not recognised, using slowpath\n");
-            linux_pte_info.state = USESLOWTLB;
-            linux_pte_info.pagetable_walk = NULL;
-            goto done;
-        }
 
-        linux_pte_info.config = config;
+static TLBHandlerInfo tlb_refill_info = { PROBE, "Refill" };
 
-        if (config & KERNEL_64_BIT) {
-            linux_pte_info.pagetable_walk = &pagetable_walk64;
-        } else {
-            linux_pte_info.pagetable_walk = &pagetable_walk32;
-        }
-
-        /* swapper_pg_dir address */
-        address = (int32_t)((lui_ins & 0xffff) << 16);
-        linux_pte_info.swapper_pg_dir = cpu_debug_translate_address(env, address);
-
-        if (!(config & KERNEL_PGD_C0_CONTEXT)) {
-            address += (((int32_t)(lw_ins & 0xffff)) << 16) >> 16;
-            address = cpu_debug_translate_address(env, address);
-        }
-
-        linux_pte_info.state = USEFASTTLB;
-        linux_pte_info.pgd_current_p = address;
-        linux_pte_info.softshift = (srl_ins >> 6) & 0x1f;
-    }
+/*
+ * This function should save the TLB Refill handler to a
+ * tlb_refill_info.handler buffer which will later
+ * be used for the Interpreter
+ */
+static inline void tlb_refill_exception_prepare(CPUMIPSState *env)
+{
+    MIPSCPU *cpu = mips_env_get_cpu(env);
+    CPUState *cs = CPU(cpu);
+    int i;
 
-done:
-    /* Get pgd_current */
-    if (linux_pte_info.state == USEFASTTLB) {
-        if (linux_pte_info.config & KERNEL_64_BIT) {
-            target_ulong address = 0;
-            /*
-             * The kernel currently implicitely assumes that the
-             * MIPS SEGBITS parameter for the processor is
-             * (PGDIR_SHIFT+PGDIR_BITS) or less, and will never
-             * allocate virtual addresses outside the maximum
-             * range for SEGBITS = (PGDIR_SHIFT+PGDIR_BITS). But
-             * that doesn't prevent user code from accessing the
-             * higher xuseg addresses.  Here, we make sure that
-             * everything but the lower xuseg addresses goes down
-             * the module_alloc/vmalloc path.
-             */
-            address = ((uint64_t)bad_vaddr) >> 40;
-            if (likely(!address)) {
-                if (linux_pte_info.config & KERNEL_PGD_C0_CONTEXT) {
-                    /*
-                     * &pgd << 11 stored in CONTEXT [23..63].
-                     */
-                    address = env->CP0_Context;
-                    address = ((uint64_t)address >> 23) << 23;
-                    /* 1 0  1 0 1  << 6  xkphys cached */
-                    address |= 0x540;
-                    /* dror k1,k1,0xb */
-                    address = ((uint64_t)address >> 11) |
-                              (((uint64_t)address & 0x7ff) << 53);
-                    return address;
-                } else {
-                    return ldl_phys(cs->as, linux_pte_info.pgd_current_p);
-                }
-            } else if (bad_vaddr < 0) {
-                /* swapper_pg_dir address */
-                return linux_pte_info.swapper_pg_dir;
-            } else {
-                /*
-                 * We get here if we are an xsseg address, or if we are
-                 * an xuseg address above (PGDIR_SHIFT+PGDIR_BITS) boundary.
-                 *
-                 * Ignoring xsseg (assume disabled so would generate
-                 * (address errors?), the only remaining possibility
-                 * is the upper xuseg addresses.  On processors with
-                 * TLB_SEGBITS <= PGDIR_SHIFT+PGDIR_BITS, these
-                 * addresses would have taken an address error. We try
-                 * to mimic that here by taking a load/istream page
-                 * fault.
-                 */
-                return 0; /* fallback to software handler and do page fault */
-            }
-        } else {
-            return ldl_phys(cs->as, linux_pte_info.pgd_current_p);
+    if (unlikely(tlb_refill_info.state == PROBE)) {
+        tlb_refill_info.ebase = env->CP0_EBase;
+        for (i = 0; i < TLB_HANDLER_SIZE; i++) {
+            tlb_refill_info.handler[i] = ldl_phys(cs->as, tlb_refill_info.ebase - 0x80000000 + (i << 2));
+            CPU.do_tlbwr = 1;
         }
+        tlb_refill_info.state = USEFASTTLB;
+        dump_handler(&tlb_refill_info);
     }
-    return 0;
 }
 
-static inline int cpu_mips_tlb_refill(CPUState *cs, target_ulong address, int rw,
-                                      int mmu_idx, int is_softmmu)
+static inline void tlb_exception_interpreter_prepare(CPUMIPSState *env, target_ulong address, int exception)
 {
-    MIPSCPU *cpu = MIPS_CPU(cs);
-    CPUMIPSState *env = &cpu->env;
-
-    int32_t saved_hflags;
-    target_ulong saved_badvaddr,saved_entryhi,saved_context,saved_xcontext;
-    target_ulong pgd_addr;
-    target_ulong fault_addr;
-    target_ulong entrylo0, entrylo1;
-    int ret;
-
-    saved_badvaddr = env->CP0_BadVAddr;
-    saved_context = env->CP0_Context;
-    saved_xcontext = env->CP0_XContext;
-    saved_entryhi = env->CP0_EntryHi;
-    saved_hflags = env->hflags;
-
     env->CP0_BadVAddr = address;
     env->CP0_Context = (env->CP0_Context & ~0x007fffff) |
                     ((address >> 9) &   0x007ffff0);
@@ -766,50 +389,58 @@ static inline int cpu_mips_tlb_refill(CPUState *cs, target_ulong address, int rw
                         ((address & ((1ULL << env->SEGBITS) - 1) & 0xFFFFFFFFFFFFE000ULL) >> 9);
 #endif
 
-    pgd_addr = 0;
-    pgd_addr = cpu_mips_get_pgd(cs, address);
-
-    /* if pgd_addr is unknown return TLBRET_NOMATCH
-     * to allow software handler to run
-     */
-    if (unlikely(pgd_addr == 0)) {
-        ret = TLBRET_NOMATCH;
-        goto out;
-    }
+    CPU.pc = 0;
+#if defined(TARGET_MIPS64)
+    int R = env->CP0_BadVAddr >> 62;
+    int UX = (env->CP0_Status & (1 << CP0St_UX)) != 0;
+    int SX = (env->CP0_Status & (1 << CP0St_SX)) != 0;
+    int KX = (env->CP0_Status & (1 << CP0St_KX)) != 0;
 
-    env->hflags = MIPS_HFLAG_KM;
-    fault_addr = env->CP0_BadVAddr;
+    if (((R == 0 && UX) || (R == 1 && SX) || (R == 3 && KX)) &&
+        (!(env->insn_flags & (INSN_LOONGSON2E | INSN_LOONGSON2F))))
+        CPU.pc = 0x080 / 4;
+#endif
 
-    linux_pte_info.pagetable_walk(cs, pgd_addr, fault_addr, &entrylo0, &entrylo1, NULL, NULL);
+    if (likely(tlb_refill_info.state != PROBE))
+        return;
 
-    /* Refill the TLB */
-    env->CP0_EntryLo0 = entrylo0;
-    env->CP0_EntryLo1 = entrylo1;
-    r4k_helper_tlbwr(env);
+    switch (exception) {
+        case EXCP_TLBF:
+            tlb_refill_exception_prepare(env);
+            break;
+        default:
+            fprintf(stderr, "%s : Unexpected TLB exception %d\n", __func__, exception);
+            exit(1);
+    }
+}
 
-    /* Since we know the TLB contents, we can
-     * return the TLB lookup value here.
-     */
+/*
+ * Once the page walk is done by the Interpreter,
+ * this function is responsible for verifying
+ * the TLB Entry. The performance boost comes from this
+ * function actually, because it prevents raising new
+ * exception if it goes through all check and returns TLBRET_MATCH.
+ */
+static inline int tlb_exception_return(CPUState *cs, target_ulong address, int rw,
+                                      int mmu_idx)
+{
+    MIPSCPU *cpu = MIPS_CPU(cs);
+    CPUMIPSState *env = &cpu->env;
 
     target_ulong mask = env->CP0_PageMask | ~(TARGET_PAGE_MASK << 1);
-    target_ulong lo = (address & mask & ~(mask >> 1)) ? entrylo1 : entrylo0;
+    target_ulong lo = (address & mask & ~(mask >> 1)) ? env->CP0_EntryLo1 : env->CP0_EntryLo0;
     uint16_t RI = (lo >> CP0EnLo_RI) & 1;
     uint16_t XI = (lo >> CP0EnLo_XI) & 1;
 
-    if (rw == MMU_INST_FETCH && (XI)) {
-        ret = TLBRET_XI;
-        goto out;
-    }
-    if (rw == MMU_DATA_LOAD && (RI)) {
-        ret = TLBRET_RI;
-        goto out;
-    }
+    if (rw == MMU_INST_FETCH && (XI))
+        return TLBRET_XI;
+
+    if (rw == MMU_DATA_LOAD && (RI))
+        return TLBRET_RI;
 
     /* Is the TLB entry valid? */
-    if ((lo & (1 << CP0EnLo_V)) == 0) {
-        ret = TLBRET_INVALID;
-        goto out;
-    }
+    if ((lo & (1 << CP0EnLo_V)) == 0)
+        return TLBRET_INVALID;
 
     /* Is this a read access or a write to a modifiable page? */
     if (rw != MMU_DATA_STORE || (lo & (1 << CP0EnLo_D))) {
@@ -822,18 +453,54 @@ static inline int cpu_mips_tlb_refill(CPUState *cs, target_ulong address, int rw
         tlb_set_page(cs, address & TARGET_PAGE_MASK,
                         physical & TARGET_PAGE_MASK, prot | PAGE_EXEC,
                         mmu_idx, TARGET_PAGE_SIZE);
-        ret = TLBRET_MATCH;
-        goto out;
+        return TLBRET_MATCH;
     }
-    ret = TLBRET_DIRTY;
+    return TLBRET_DIRTY;
+}
+
+/*
+ * This function routes Refill exceptions through the TLB exception Interpreter
+ * and all others are handled by the kernel.
+ */
+static inline int do_tlb_refill(CPUState *cs, vaddr address, int rw, int mmu_idx)
+{
+    MIPSCPU *cpu = MIPS_CPU(cs);
+    CPUMIPSState *env = &cpu->env;
+    int tlbret = TLBRET_NOMATCH;
+
+    target_ulong saved_badvaddr;
+    target_ulong saved_entryhi;
+    target_ulong saved_context;
+    target_ulong saved_xcontext;
+    target_ulong saved_pagemask;
+    target_ulong saved_pagegrain;
+
+    saved_badvaddr = env->CP0_BadVAddr;
+    saved_context = env->CP0_Context;
+    saved_xcontext = env->CP0_XContext;
+    saved_entryhi = env->CP0_EntryHi;
+    saved_pagemask = env->CP0_PageMask;
+    saved_pagegrain = env->CP0_PageGrain;
+
+    if (unlikely(tlb_refill_info.state == USESLOWTLB))
+        goto out;
+
+    tlb_exception_interpreter_prepare(env, address, EXCP_TLBF);
+
+    if (tlb_exception_interpreter(env, tlb_refill_info.handler, TLB_HANDLER_SIZE))
+        goto out;
+
+    tlbret = tlb_exception_return(cs, address, rw, mmu_idx);
 
 out:
     env->CP0_BadVAddr = saved_badvaddr;
     env->CP0_Context = saved_context;
     env->CP0_XContext = saved_xcontext;
     env->CP0_EntryHi = saved_entryhi;
-    env->hflags = saved_hflags;
-    return ret;
+    env->CP0_PageGrain = saved_pagegrain;
+    env->CP0_PageMask = saved_pagemask;
+
+    return tlbret;
 }
 
 hwaddr mips_cpu_get_phys_page_debug(CPUState *cs, vaddr addr)
@@ -843,37 +510,45 @@ hwaddr mips_cpu_get_phys_page_debug(CPUState *cs, vaddr addr)
     hwaddr phys_addr;
     int prot, ret;
 
-#if defined(TARGET_MIPS64)
-    if (!(linux_pte_info.config & KERNEL_64_BIT))
-        addr = (int32_t)addr;
-#endif
-
     ret = get_physical_address(env, &phys_addr, &prot, addr, 0, ACCESS_INT);
     if (ret != TLBRET_MATCH) {
-        target_ulong pgd_addr = cpu_mips_get_pgd(cs, addr);
-
-        phys_addr = -1;
-        if (likely(pgd_addr)) {
-            target_ulong entrylo0, entrylo1;
-            target_ulong sw_pte_lo0, sw_pte_lo1;
-
-            linux_pte_info.pagetable_walk(cs, pgd_addr, addr,
-                                          &entrylo0, &entrylo1,
-                                          &sw_pte_lo0, &sw_pte_lo1);
-
-            target_ulong mask = env->CP0_PageMask | ~(TARGET_PAGE_MASK << 1);
-            target_ulong lo = (addr & mask & ~(mask >> 1)) ? entrylo1 : entrylo0;
-            target_ulong sw_pte = (addr & mask & ~(mask >> 1)) ? sw_pte_lo1 : sw_pte_lo0;
-
-            if (sw_pte & PTE_PAGE_PRESENT) {
-                phys_addr = ((lo >> CP0EnLo_PFN) << 12) | (addr & (mask >> 1));
+        target_ulong saved_badvaddr;
+        target_ulong saved_entryhi;
+        target_ulong saved_context;
+        target_ulong saved_xcontext;
+        target_ulong saved_pagemask;
+        target_ulong saved_pagegrain;
+
+        saved_pagemask = env->CP0_PageMask;
+        saved_pagegrain = env->CP0_PageGrain;
+        saved_badvaddr = env->CP0_BadVAddr;
+        saved_context = env->CP0_Context;
+        saved_xcontext = env->CP0_XContext;
+        saved_entryhi = env->CP0_EntryHi;
+
+        tlb_exception_interpreter_prepare(env, addr, EXCP_TLBF);
+
+        if (likely(tlb_refill_info.state == USEFASTTLB)) {
+            CPU.do_tlbwr = 0;
+            if (tlb_exception_interpreter(env, tlb_refill_info.handler, TLB_HANDLER_SIZE) != 0) {
+                phys_addr = -1;
+                fprintf(stderr, "cpu_get_phys_page_debug() fails for vaddr: 0x" TARGET_FMT_plx "\n", addr);
             } else {
-                qemu_log("cpu_get_phys_page_debug() invalid mapping for vaddr: 0x" TARGET_FMT_plx "\n", addr);
+                target_ulong mask = env->CP0_PageMask | ~(TARGET_PAGE_MASK << 1);
+                target_ulong lo = (addr & mask & ~(mask >> 1)) ? CPU.CP0_EntryLo1 : CPU.CP0_EntryLo0;
+                phys_addr = ((lo >> CP0EnLo_PFN) << 12) | (addr & (mask >> 1));
             }
-        } else {
-            qemu_log("cpu_get_phys_page_debug() fails for vaddr: 0x" TARGET_FMT_plx "\n", addr);
+            CPU.do_tlbwr = 1;
         }
+
+        env->CP0_BadVAddr = saved_badvaddr;
+        env->CP0_Context = saved_context;
+        env->CP0_XContext = saved_xcontext;
+        env->CP0_EntryHi = saved_entryhi;
+        env->CP0_PageGrain = saved_pagegrain;
+        env->CP0_PageMask = saved_pagemask;
     }
+
     return phys_addr;
 }
 #endif
@@ -912,15 +587,15 @@ int mips_cpu_handle_mmu_fault(CPUState *cs, vaddr address, int rw,
                      mmu_idx, TARGET_PAGE_SIZE);
         ret = 0;
     } else if (ret == TLBRET_NOMATCH)
-        ret = cpu_mips_tlb_refill(cs, address, rw, mmu_idx, 1);
+        ret = do_tlb_refill(cs, address, rw, mmu_idx);
 
     if (ret < 0)
 #endif
+
     {
         raise_mmu_exception(env, address, rw, ret);
         ret = 1;
     }
-
     return ret;
 }
 
diff --git a/target-mips/translate.c b/target-mips/translate.c
index 84bdaba1fb..b4e49de2db 100644
--- a/target-mips/translate.c
+++ b/target-mips/translate.c
@@ -1348,6 +1348,738 @@ enum {
     OPC_BINSRI_df   = (0x7 << 23) | OPC_MSA_BIT_09,
 };
 
+#if !defined(CONFIG_USER_ONLY)
+
+// #define DEBUG_INTERPRETER 1
+
+#if defined(DEBUG_INTERPRETER)
+#define DEBUG_DISSAS(opc) do {                                                          \
+    int i;                                                                              \
+    fprintf(stderr, "%s:%d\t%s pc %u op 0x%x rs %d rt %d rd %d sa %d imm 0x%x\n",       \
+                    __func__, __LINE__, opc, CPU.pc, op, rs, rt, rd, sa, (uint32_t)imm);\
+    for (i = 0; i < 32; i++) {                                                          \
+        fprintf(stderr, "$%d = 0x" TARGET_FMT_lx " ", i, CPU.gpr_reg[i]);               \
+        if ((i % 4) == 3) fprintf(stderr, "\n");                                        \
+    }                                                                                   \
+    fprintf(stderr, "\n");                                                              \
+} while (0)
+#else
+#define DEBUG_DISSAS(opc)
+#endif
+
+#define DEBUG_ERROR(msg) do {                                                           \
+    fprintf(stderr, "%s:%d\t%s pc %u op 0x%x rs %d rt %d rd %d sa %d imm 0x%x\n",       \
+                    __func__, __LINE__, msg, CPU.pc, op, rs, rt, rd, sa, (uint32_t)imm);\
+    exit(1);                                                                            \
+} while(0)
+
+CPUInterpreterContext CPU;
+
+static inline hwaddr translate_address(CPUMIPSState *env, target_ulong address) {
+
+#define USEG_LIMIT      0x7FFFFFFFUL
+#define KSEG0_BASE      0x80000000UL
+#define KSEG1_BASE      0xA0000000UL
+#define KSEG2_BASE      0xC0000000UL
+#define KSEG3_BASE      0xE0000000UL
+
+#define KVM_KSEG0_BASE  0x40000000UL
+#define KVM_KSEG2_BASE  0x60000000UL
+
+    if (address <= USEG_LIMIT) {
+        /* useg */
+        if (env->CP0_Status & (1 << CP0St_ERL)) {
+            return address & 0xFFFFFFFF;
+        } else {
+            return address;
+        }
+#if defined(TARGET_MIPS64)
+    } else if (address < 0x4000000000000000ULL) {
+        return address;
+    } else if (address < 0x8000000000000000ULL) {
+        return address;
+    } else if (address < 0xC000000000000000ULL) {
+        /* xkphys */
+        if ((address & 0x07FFFFFFFFFFFFFFULL) <= env->PAMask) {
+            return address & env->PAMask;
+        } else {
+            return address;
+        }
+    } else if (address < 0xFFFFFFFF80000000ULL) {
+        /* xkseg */
+        return address;
+#endif
+    } else if (address < (int32_t)KSEG1_BASE) {
+        /* kseg0 */
+        return address - (int32_t)KSEG0_BASE;
+    } else if (address < (int32_t)KSEG2_BASE) {
+        /* kseg1 */
+        return address - (int32_t)KSEG1_BASE;
+    } else
+        return address;
+}
+
+int tlb_exception_interpreter(CPUMIPSState *env, uint32_t *code, uint32_t size)
+{
+    uint32_t opcode;
+    int rs, rt, rd, sa;
+    uint32_t op, op1;
+    int16_t imm;
+
+    MIPSCPU *cpu = mips_env_get_cpu(env);
+    CPUState *cs = CPU(cpu);
+
+    CPU.branch_addr = 0;
+
+    CPU.CP0_EntryLo0 = env->CP0_EntryLo0;
+    CPU.CP0_EntryLo1 = env->CP0_EntryLo1;
+    CPU.CP0_BadVAddr = env->CP0_BadVAddr;
+    CPU.CP0_EntryHi = env->CP0_EntryHi;
+    CPU.CP0_Context = env->CP0_Context;
+    CPU.CP0_XContext = env->CP0_XContext;
+    CPU.CP0_PageMask = env->CP0_PageMask;
+    CPU.CP0_PageGrain = env->CP0_PageGrain;
+    CPU.CP0_PageGrain_rw_bitmask = env->CP0_PageGrain_rw_bitmask;
+    CPU.CP0_Index = env->CP0_Index;
+
+    while (1) {
+
+        op = MASK_OP_MAJOR(code[CPU.pc]);
+        rs = (code[CPU.pc] >> 21) & 0x1f;
+        rt = (code[CPU.pc] >> 16) & 0x1f;
+        rd = (code[CPU.pc] >> 11) & 0x1f;
+        sa = (code[CPU.pc] >> 6) & 0x1f;
+        imm = (int16_t)code[CPU.pc];
+        opcode = code[CPU.pc];
+
+        CPU.pc++;
+
+        switch (op) {
+        case OPC_SPECIAL:
+            op1 = MASK_SPECIAL(opcode);
+            switch (op1) {
+            case OPC_JR:
+            case OPC_JALR:
+                DEBUG_DISSAS("Jump");
+                return -2;
+            case OPC_SLL:
+                CPU.gpr_reg[rt] = (int32_t)(CPU.gpr_reg[rt] << sa);
+                DEBUG_DISSAS("sll");
+                break;
+            case OPC_SRA:
+                CPU.gpr_reg[rt] = (int32_t)((int32_t)CPU.gpr_reg[rt] >> sa);
+                DEBUG_DISSAS("sra");
+                break;
+            case OPC_SRL:
+                switch ((opcode >> 21) & 0x1f) {
+                    case 1:
+                    {
+                        uint32_t mask = ~(-1 << sa);
+                        CPU.gpr_reg[rd] = (int32_t)((((uint32_t)CPU.gpr_reg[rt] & mask) << (32 - sa)) |
+                                                   (((uint32_t)CPU.gpr_reg[rt]) >> sa));
+                        DEBUG_DISSAS("ror");
+                        break;
+                    }
+                    case 0:
+                        CPU.gpr_reg[rt] = (int32_t)((uint32_t)CPU.gpr_reg[rt] >> sa);
+                        DEBUG_DISSAS("srl");
+                        break;
+                    default:
+                        DEBUG_ERROR("srl Unknown");
+                        break;
+                }
+                break;
+            case OPC_ROTR:
+            {
+                target_ulong mask = ~(-1 << sa);
+                CPU.gpr_reg[rd] = (int32_t)(((CPU.gpr_reg[rt] & mask) << (32 - sa)) |
+                                           (((target_ulong)CPU.gpr_reg[rt]) >> sa));
+                DEBUG_DISSAS("rotr");
+                break;
+            }
+            case OPC_ADDU:
+                CPU.gpr_reg[rd] = (int32_t)(CPU.gpr_reg[rs] + CPU.gpr_reg[rt]);
+                DEBUG_DISSAS("addu");
+                break;
+            case OPC_SUBU:
+                CPU.gpr_reg[rd] = (int32_t)(CPU.gpr_reg[rs] - CPU.gpr_reg[rt]);
+                DEBUG_DISSAS("subu");
+                break;
+            case OPC_AND:          /* Logic*/
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rs] & CPU.gpr_reg[rt];
+                DEBUG_DISSAS("and");
+                break;
+            case OPC_OR:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rs] | CPU.gpr_reg[rt];
+                DEBUG_DISSAS("or");
+                break;
+            case OPC_XOR:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rs] ^ CPU.gpr_reg[rt];
+                DEBUG_DISSAS("xor");
+                break;
+#if defined(TARGET_MIPS64)
+                /* MIPS64 specific opcodes */
+            case OPC_DSLL:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rt] << sa;
+                DEBUG_DISSAS("dsll");
+                break;
+            case OPC_DSRA:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rt] >> sa;
+                DEBUG_DISSAS("dsra");
+                break;
+            case OPC_DSLL32:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rt] << (sa + 32);
+                DEBUG_DISSAS("dsll32");
+                break;
+            case OPC_DSRA32:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rt] >> (sa + 32);
+                DEBUG_DISSAS("dsra32");
+                break;
+            case OPC_DSRL:
+                switch ((opcode >> 21) & 0x1f) {
+                    case 1:
+                    {
+                        target_ulong mask = ~(-1 << sa);
+                        CPU.gpr_reg[rd] = (((target_ulong)CPU.gpr_reg[rt] & mask) << (64 - sa)) |
+                                          (((target_ulong)CPU.gpr_reg[rt]) >> sa);
+                        DEBUG_DISSAS("dror");
+                        break;
+                    }
+                    case 0:
+                        CPU.gpr_reg[rt] = ((target_ulong)CPU.gpr_reg[rt] >> sa);
+                        DEBUG_DISSAS("dsrl");
+                        break;
+                    default:
+                        DEBUG_DISSAS("dsrl Unknown");
+                        break;
+                }
+                break;
+            case OPC_DROTR:
+            {
+                target_ulong mask = ~(-1 << sa);
+                CPU.gpr_reg[rd] = (((target_ulong)CPU.gpr_reg[rt] & mask) << (64 - sa)) |
+                                  (((target_ulong)CPU.gpr_reg[rt]) >> sa);
+                DEBUG_DISSAS("drotr");
+                break;
+            }
+            case OPC_DROTR32:
+            {
+                target_ulong mask = ~(-1 << (sa + 32));
+                CPU.gpr_reg[rd] = (((target_ulong)CPU.gpr_reg[rt] & mask) << (32 - sa)) |
+                                  (((target_ulong)CPU.gpr_reg[rt]) >> (sa + 32));
+                DEBUG_DISSAS("drotr32");
+                break;
+            }
+            case OPC_DSRL32:
+                CPU.gpr_reg[rd] = ((target_ulong)CPU.gpr_reg[rt]) >> (sa + 32);
+                DEBUG_DISSAS("dsrl32");
+                break;
+            case OPC_DADDU:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rs] + CPU.gpr_reg[rt];
+                DEBUG_DISSAS("daddu");
+                break;
+            case OPC_DSUBU:
+                CPU.gpr_reg[rd] = CPU.gpr_reg[rs] - CPU.gpr_reg[rt];
+                DEBUG_DISSAS("dsubu");
+                break;
+#endif
+            default:
+                DEBUG_ERROR("Unknown");
+                break;
+            }
+            break;
+        case OPC_SPECIAL3:
+        {
+            target_ulong mask;
+
+            op1 = MASK_SPECIAL3(opcode);
+            switch (op1) {
+            case OPC_EXT:
+                mask = (~(-1 << (rd + 1))) << sa;
+                CPU.gpr_reg[rt] = (int32_t)(((target_ulong)(CPU.gpr_reg[rs] & mask)) >> sa);
+                DEBUG_DISSAS("ext");
+                break;
+            case OPC_INS:
+                mask = ~(-1 << (rd - sa + 1));
+                CPU.gpr_reg[rt] = (int32_t)((CPU.gpr_reg[rt] & ~(mask << sa)) |
+                                            ((CPU.gpr_reg[rs] & mask) << sa));
+                DEBUG_DISSAS("ins");
+                break;
+#if defined(TARGET_MIPS64)
+            case OPC_DEXT:
+                mask = (~(-1 << (rd + 1))) << sa;
+                CPU.gpr_reg[rt] = ((target_ulong)(CPU.gpr_reg[rs] & mask)) >> sa |
+                                  (CPU.gpr_reg[rt] & ~((target_ulong)mask >> sa));
+                DEBUG_DISSAS("dext");
+                break;
+            case OPC_DINSM:
+                mask = ~(-1 << ((rd + 32) - sa + 1));
+                CPU.gpr_reg[rt] = (CPU.gpr_reg[rt] & ~(mask << sa)) |
+                                  ((CPU.gpr_reg[rs] & mask) << sa);
+                DEBUG_DISSAS("dinsm");
+                break;
+            case OPC_DINS:
+                mask = ~(-1 << (rd - sa + 1));
+                CPU.gpr_reg[rt] = (CPU.gpr_reg[rt] & ~(mask << sa)) |
+                                  ((CPU.gpr_reg[rs] & mask) << sa);
+                DEBUG_DISSAS("dins");
+                break;
+#endif
+            default:
+                DEBUG_ERROR("Unknown");
+                break;
+            }
+            break;
+        }
+        case OPC_REGIMM:
+            op1 = MASK_REGIMM(opcode);
+            switch (op1) {
+            case OPC_BLTZL: /* REGIMM branches */
+                if (CPU.gpr_reg[rs] < 0) {
+                    CPU.branch_addr = CPU.pc + (int32_t)imm;
+                    DEBUG_DISSAS("bltzl");
+                    continue;
+                } else {
+                    CPU.pc++;
+                }
+                DEBUG_DISSAS("bltzl");
+                break;
+            case OPC_BGEZL:
+                if (CPU.gpr_reg[rs] >= 0) {
+                    CPU.branch_addr = CPU.pc + (int32_t)imm;
+                    DEBUG_DISSAS("bgezl");
+                    continue;
+                } else {
+                    CPU.pc++;
+                }
+                DEBUG_DISSAS("bgezl");
+                break;
+            case OPC_BLTZ:
+                if (CPU.gpr_reg[rs] < 0) {
+                    CPU.branch_addr = CPU.pc + (int32_t)imm;
+                    DEBUG_DISSAS("bltz");
+                    continue;
+                 }
+                DEBUG_DISSAS("bltz");
+                break;
+            case OPC_BGEZ:
+                if (CPU.gpr_reg[rs] >= 0) {
+                    CPU.branch_addr = CPU.pc + (int32_t)imm;
+                    DEBUG_DISSAS("bgez");
+                    continue;
+                }
+                DEBUG_DISSAS("bgez");
+                break;
+            default:            /* Invalid */
+                DEBUG_ERROR("Unknown");
+                break;
+            }
+            break;
+        case OPC_CP0:
+            op1 = MASK_CP0(opcode);
+            switch (op1) {
+            case OPC_MFC0:
+                switch (rd) {
+                case 4:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        // Context
+                        CPU.gpr_reg[rt] = (int32_t)CPU.CP0_Context;
+                        DEBUG_DISSAS("mfc0 Context");
+                        break;
+                    default:
+                        DEBUG_DISSAS("mfc0 reg 4: Unknown select");
+                        break;
+                     }
+                     break;
+                case 5:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        CPU.gpr_reg[rt] = CPU.CP0_PageMask;
+                        DEBUG_DISSAS("mfc0 PageMask");
+                        break;
+                    case 1:
+                        CPU.gpr_reg[rt] = CPU.CP0_PageGrain;
+                        DEBUG_DISSAS("mfc0 PageGrain");
+                        break;
+                    default:
+                        DEBUG_ERROR("mfc0 reg 5 Unknown select");
+                    }
+                    break;
+                case 8:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        // BadVAddr
+                        CPU.gpr_reg[rt] = CPU.CP0_BadVAddr;
+                        DEBUG_DISSAS("mfc0 BadVAddr");
+                        break;
+                    default:
+                        DEBUG_ERROR("mfc0 reg 8 Unknown select");
+                        break;
+                    }
+                    break;
+#if defined(TARGET_MIPS64)
+                case 20:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        // XContext
+                        CPU.gpr_reg[rt] = CPU.CP0_XContext;
+                        DEBUG_DISSAS("mfc0 XContext");
+                        break;
+                    default:
+                        DEBUG_DISSAS("mfc0 reg 20: Unknown select");
+                        break;
+                     }
+                     break;
+#endif
+                case 13:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        CPU.gpr_reg[rt] = CPU.CP0_Cause;
+                        DEBUG_DISSAS("mfc0 CP0_Cause");
+                        break;
+                    default:
+                        DEBUG_ERROR("mfc0 Unknown select");
+                        break;
+                    }
+                    break;
+                default:
+                    DEBUG_ERROR("mfc0 Unknown");
+                    break;
+                }
+                break;
+            case OPC_MTC0:
+                switch (rd) {
+                case 2:
+                    switch (opcode & 0x7) {
+                    case 0:
+                    {
+                        // EntryLo0
+                        target_ulong rxi = CPU.gpr_reg[rt] & (CPU.CP0_PageGrain & (3u << CP0PG_XIE));
+                        CPU.CP0_EntryLo0 = (CPU.gpr_reg[rt] & 0x3FFFFFFF) | (rxi << (CP0EnLo_XI));
+                        DEBUG_DISSAS("mtc0 EntryLo0");
+                        break;
+                    }
+                    default:
+                        DEBUG_ERROR("mtc0 reg 2 Unknown select");
+                        break;
+                    }
+                    break;
+                case 3:
+                    switch (opcode & 0x7) {
+                    case 0:
+                    {
+                        // EntryLo1
+                        target_ulong rxi = CPU.gpr_reg[rt] & (CPU.CP0_PageGrain & (3u << CP0PG_XIE));
+                        CPU.CP0_EntryLo1 = (CPU.gpr_reg[rt] & 0x3FFFFFFF) | (rxi << (CP0EnLo_XI));
+                        DEBUG_DISSAS("mtc0 EntryLo1");
+                        break;
+                    }
+                    default:
+                        DEBUG_ERROR("mtc0 reg 3 Unknown select");
+                        break;
+                    }
+                    break;
+                case 5:
+                    switch (opcode & 0x7) {
+                    case 0:
+                    {
+                        uint64_t mask = CPU.gpr_reg[rt] >> (TARGET_PAGE_BITS + 1);
+                        if (!(env->insn_flags & ISA_MIPS32R6) || (CPU.gpr_reg[rt] == ~0) ||
+                            (mask == 0x0000 || mask == 0x0003 || mask == 0x000F ||
+                             mask == 0x003F || mask == 0x00FF || mask == 0x03FF ||
+                             mask == 0x0FFF || mask == 0x3FFF || mask == 0xFFFF)) {
+                            CPU.CP0_PageMask = CPU.gpr_reg[rt] & (0x1FFFFFFF & (TARGET_PAGE_MASK << 1));
+                        }
+                        DEBUG_DISSAS("mtc0 PageMask");
+                        break;
+                    }
+                    case 1:
+                        CPU.CP0_PageGrain = (CPU.gpr_reg[rt] & CPU.CP0_PageGrain_rw_bitmask) |
+                                             (CPU.CP0_PageGrain & ~CPU.CP0_PageGrain_rw_bitmask);
+                        DEBUG_DISSAS("mtc0 PageGrain");
+                        break;
+                    default:
+                        DEBUG_ERROR("mtc0 reg 5 Unknown select");
+                    }
+                    break;
+                default:
+                    DEBUG_ERROR("mtc0 Unknown");
+                    break;
+                }
+                break;
+#if defined(TARGET_MIPS64)
+            case OPC_DMFC0:
+                switch (rd) {
+                case 4:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        // Context
+                        CPU.gpr_reg[rt] = CPU.CP0_Context;
+                        DEBUG_DISSAS("dmfc0 Context");
+                        break;
+                    default:
+                        DEBUG_DISSAS("dmfc0 reg 4: Unknown select");
+                        break;
+                     }
+                     break;
+                case 5:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        CPU.gpr_reg[rt] = CPU.CP0_PageMask;
+                        DEBUG_DISSAS("mfc0 PageMask");
+                        break;
+                    case 1:
+                        CPU.gpr_reg[rt] = CPU.CP0_PageGrain;
+                        DEBUG_DISSAS("mfc0 PageGrain");
+                        break;
+                    default:
+                        DEBUG_ERROR("mfc0 reg 5 Unknown select");
+                    }
+                    break;
+                case 8:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        // BadVAddr
+                        CPU.gpr_reg[rt] = CPU.CP0_BadVAddr;
+                        DEBUG_DISSAS("dmfc0 BadVAddr");
+                        break;
+                    default:
+                        DEBUG_DISSAS("dmfc0 reg 8: Unknown select");
+                        break;
+                    }
+                    break;
+                case 20:
+                    switch (opcode & 0x7) {
+                    case 0:
+                        // XContext
+                        CPU.gpr_reg[rt] = CPU.CP0_XContext;
+                        DEBUG_DISSAS("dmfc0 XContext");
+                        break;
+                    default:
+                        DEBUG_DISSAS("dmfc0 reg 20: Unknown select");
+                        break;
+                     }
+                     break;
+                case 31:
+                    switch(opcode & 0x7) {
+                        case 2 ... 7:
+                            CPU.gpr_reg[rt] = CPU.CP0_KScratch[(opcode & 0x7) - 2];
+                            DEBUG_DISSAS("dmfc0 KScratch");
+                            break;
+                        default:
+                            DEBUG_ERROR("dmfc0 Unknown sel");
+                    }
+                    break;
+                default:
+                    DEBUG_ERROR("dmfc0 Unknown");
+                    break;
+                }
+                break;
+            case OPC_DMTC0:
+                switch (rd) {
+                case 2:
+                    switch (opcode & 0x7) {
+                    case 0:
+                    {
+                        // EntryLo0
+                        target_ulong rxi = CPU.gpr_reg[rt] & ((CPU.CP0_PageGrain & (3ull << CP0PG_XIE)) << 32);
+                        CPU.CP0_EntryLo0 = (CPU.gpr_reg[rt] & 0x3FFFFFFF) | rxi;
+                        DEBUG_DISSAS("dmtc0 EntryLo0");
+                        break;
+
+                    }
+                    default:
+                        DEBUG_ERROR("dmtc0 Unknown sel");
+                        break;
+                    }
+                    break;
+                case 3:
+                    switch (opcode & 0x7) {
+                    case 0:
+                    {
+                        // EntryLo1
+                        target_ulong rxi = CPU.gpr_reg[rt] & ((CPU.CP0_PageGrain & (3ull << CP0PG_XIE)) << 32);
+                        CPU.CP0_EntryLo1 = (CPU.gpr_reg[rt] & 0x3FFFFFFF) | rxi;
+                        DEBUG_DISSAS("dmtc0 EntryLo1");
+                        break;
+                    }
+                    default:
+                        DEBUG_ERROR("dmtc0 Unknown select");
+                        break;
+                    }
+                    break;
+                case 5:
+                    switch (opcode & 0x7) {
+                    case 0:
+                    {
+                        uint64_t mask = CPU.gpr_reg[rt] >> (TARGET_PAGE_BITS + 1);
+                        if (!(env->insn_flags & ISA_MIPS32R6) || (CPU.gpr_reg[rt] == ~0) ||
+                            (mask == 0x0000 || mask == 0x0003 || mask == 0x000F ||
+                             mask == 0x003F || mask == 0x00FF || mask == 0x03FF ||
+                             mask == 0x0FFF || mask == 0x3FFF || mask == 0xFFFF)) {
+                            CPU.CP0_PageMask = CPU.gpr_reg[rt] & (0x1FFFFFFF & (TARGET_PAGE_MASK << 1));
+                        }
+                        DEBUG_DISSAS("mtc0 PageMask");
+                        break;
+                    }
+                    case 1:
+                        CPU.CP0_PageGrain = (CPU.gpr_reg[rt] & CPU.CP0_PageGrain_rw_bitmask) |
+                                             (CPU.CP0_PageGrain & ~CPU.CP0_PageGrain_rw_bitmask);
+                        DEBUG_DISSAS("mtc0 PageGrain");
+                        break;
+                    default:
+                        DEBUG_ERROR("mtc0 reg 5 Unknown select");
+                    }
+                    break;
+                case 31:
+                    switch(opcode & 0x7) {
+                        case 2 ... 7:
+                            CPU.CP0_KScratch[(opcode & 0x7) - 2] = CPU.gpr_reg[rt];
+                            DEBUG_DISSAS("dmtc0 KScratch");
+                            break;
+                        default:
+                            DEBUG_ERROR("dmtc0 Unknown sel");
+                    }
+                    break;
+                default:
+                    DEBUG_ERROR("dmtc0 Unknown");
+                    break;
+                }
+                break;
+#endif
+
+            case OPC_C0_FIRST ... OPC_C0_LAST:
+                switch (MASK_C0(opcode)) {
+                    case OPC_ERET:
+                        // Exception return
+                        DEBUG_DISSAS("eret");
+                        return 0;
+                    case OPC_TLBWR:
+                        if (likely(CPU.do_tlbwr)) {
+                            env->CP0_EntryLo0 = CPU.CP0_EntryLo0;
+                            env->CP0_EntryLo1 = CPU.CP0_EntryLo1;
+                            r4k_helper_tlbwr(env);
+                        }
+                        DEBUG_DISSAS("tlbwr");
+                        break;
+                    case OPC_TLBWI:
+                        env->CP0_EntryLo0 = CPU.CP0_EntryLo0;
+                        env->CP0_EntryLo1 = CPU.CP0_EntryLo1;
+                        r4k_helper_tlbwi(env);
+                        DEBUG_DISSAS("tlbwi");
+                        break;
+                    case OPC_TLBP:
+                        r4k_helper_tlbp(env);
+                        CPU.CP0_Index = env->CP0_Index;
+                        DEBUG_DISSAS("tlbp");
+                        break;
+                    default:
+                        DEBUG_ERROR("opc_c0 Unknown");
+                        break;
+                }
+                break;
+            default:
+                DEBUG_ERROR("Unknown");
+                break;
+            }
+            break;
+        case OPC_ADDIU:
+            CPU.gpr_reg[rt] = CPU.gpr_reg[rs] + imm;
+            DEBUG_DISSAS("addiu");
+            break;
+        case OPC_ANDI: /* Arithmetic with immediate opcode */
+            CPU.gpr_reg[rt] = CPU.gpr_reg[rs] & (uint32_t)imm;
+            DEBUG_DISSAS("andi");
+            break;
+        case OPC_LUI: /* OPC_AUI */
+            CPU.gpr_reg[rt] = ((int32_t)imm) << 16;
+            DEBUG_DISSAS("lui");
+            break;
+        case OPC_ORI:
+            CPU.gpr_reg[rt] = CPU.gpr_reg[rs] | ((uint32_t)imm);
+            DEBUG_DISSAS("ori");
+            break;
+        case OPC_XORI:
+            CPU.gpr_reg[rt] = CPU.gpr_reg[rs] ^ ((uint32_t)imm);
+            DEBUG_DISSAS("xori");
+            break;
+        case OPC_J:
+        case OPC_JAL: /* Jump */
+        case OPC_JALX:
+            DEBUG_DISSAS("Jump");
+            return -1;
+        /* Branch */
+        case OPC_BEQ:
+            if (CPU.gpr_reg[rs] == CPU.gpr_reg[rt]) {
+                CPU.branch_addr = CPU.pc + (int32_t)imm;
+                DEBUG_DISSAS("beq");
+                continue;
+            }
+            DEBUG_DISSAS("beq");
+            break;
+        case OPC_BEQL:
+            if (CPU.gpr_reg[rs] == CPU.gpr_reg[rt]) {
+                CPU.branch_addr = CPU.pc + (int32_t)imm;
+                DEBUG_DISSAS("beql");
+                continue;
+            } else {
+                CPU.pc++;
+            }
+            DEBUG_DISSAS("beql");
+            break;
+        case OPC_BNE:
+            if (CPU.gpr_reg[rs] != CPU.gpr_reg[rt]) {
+                CPU.branch_addr = CPU.pc + (int32_t)imm;
+                DEBUG_DISSAS("bne");
+                continue;
+            }
+            DEBUG_DISSAS("bne");
+            break;
+        case OPC_LW:
+        {
+            CPU.gpr_reg[rt] = (int32_t)ldl_phys(cs->as, translate_address(env, CPU.gpr_reg[rs] + imm));
+            DEBUG_DISSAS("lw");
+            break;
+        }
+        case OPC_SW:
+            stl_phys(cs->as, translate_address(env, CPU.gpr_reg[rs] + imm), CPU.gpr_reg[rt]);
+            DEBUG_DISSAS("sw");
+            break;
+    #if defined(TARGET_MIPS64)
+        /* MIPS64 opcodes */
+        case OPC_LD:
+            CPU.gpr_reg[rt] = ldq_phys(cs->as, translate_address(env, CPU.gpr_reg[rs] + imm));
+            DEBUG_DISSAS("ld");
+            break;
+        case OPC_SD:
+            stq_phys(cs->as, translate_address(env, CPU.gpr_reg[rs] + imm), CPU.gpr_reg[rt]);
+            DEBUG_DISSAS("sd");
+            break;
+        case OPC_DADDIU:
+            CPU.gpr_reg[rt] = CPU.gpr_reg[rs] + imm;
+            DEBUG_DISSAS("daddiu");
+            break;
+    #endif
+        default:            /* Invalid */
+            DEBUG_ERROR("Unknown");
+            break;
+        }
+
+        if (CPU.branch_addr) {
+            CPU.pc = CPU.branch_addr;
+            CPU.branch_addr = 0;
+        }
+
+        if (CPU.pc >= size) {
+            DEBUG_ERROR("Code buffer Index out of bounds");
+            return -3;
+        }
+
+    } //end of while
+}
+#endif
+
 /* global register indices */
 static TCGv_ptr cpu_env;
 static TCGv cpu_gpr[32], cpu_PC;