sparc64 name mmu registers and general cleanup

- add names to mmu registers, this helps understanding the code which
uses/modifies them.
- fold i/d mmu tlb entries tag and tte arrays into arrays of tlb entries
- extract demap_tlb routine (code duplication)
- extract replace_tlb routine (code duplication)

- flush qemu tlb translations when replacing sparc64 mmu tlb entries

I have no test case which demands flushing qemu translations,
and this patch should have no other visible changes to runtime.

Signed-off-by: igor.v.kovalenko@gmail.com

--
Kind regards,
Igor V. Kovalenko

4 files changed, 225 insertions, 111 deletions

diff --git a/target-sparc/cpu.h b/target-sparc/cpu.h
index 76e1e796ba..98c185f001 100644
--- a/target-sparc/cpu.h
+++ b/target-sparc/cpu.h
@@ -273,6 +273,11 @@ enum {
 };
 #endif
 
+typedef struct SparcTLBEntry {
+    uint64_t tag;
+    uint64_t tte;
+} SparcTLBEntry;
+
 typedef struct CPUSPARCState {
     target_ulong gregs[8]; /* general registers */
     target_ulong *regwptr; /* pointer to current register window */
@@ -319,12 +324,33 @@ typedef struct CPUSPARCState {
     uint64_t lsu;
 #define DMMU_E 0x8
 #define IMMU_E 0x4
-    uint64_t immuregs[16];
-    uint64_t dmmuregs[16];
-    uint64_t itlb_tag[64];
-    uint64_t itlb_tte[64];
-    uint64_t dtlb_tag[64];
-    uint64_t dtlb_tte[64];
+    //typedef struct SparcMMU
+    union {
+        uint64_t immuregs[16];
+        struct {
+            uint64_t tsb_tag_target;
+            uint64_t unused_mmu_primary_context;   // use DMMU
+            uint64_t unused_mmu_secondary_context; // use DMMU
+            uint64_t sfsr;
+            uint64_t sfar;
+            uint64_t tsb;
+            uint64_t tag_access;
+        } immu;
+    };
+    union {
+        uint64_t dmmuregs[16];
+        struct {
+            uint64_t tsb_tag_target;
+            uint64_t mmu_primary_context;
+            uint64_t mmu_secondary_context;
+            uint64_t sfsr;
+            uint64_t sfar;
+            uint64_t tsb;
+            uint64_t tag_access;
+        } dmmu;
+    };
+    SparcTLBEntry itlb[64];
+    SparcTLBEntry dtlb[64];
     uint32_t mmu_version;
 #else
     uint32_t mmuregs[32];
diff --git a/target-sparc/helper.c b/target-sparc/helper.c
index 5951e6b5c3..429be3768c 100644
--- a/target-sparc/helper.c
+++ b/target-sparc/helper.c
@@ -386,13 +386,13 @@ static inline int compare_masked(uint64_t x, uint64_t y, uint64_t mask)
 
 // Returns true if TTE tag is valid and matches virtual address value in context
 // requires virtual address mask value calculated from TTE entry size
-static inline int ultrasparc_tag_match(uint64_t tlb_tag, uint64_t tlb_tte,
+static inline int ultrasparc_tag_match(SparcTLBEntry *tlb,
                                        uint64_t address, uint64_t context,
                                        target_phys_addr_t *physical)
 {
     uint64_t mask;
 
-    switch ((tlb_tte >> 61) & 3) {
+    switch ((tlb->tte >> 61) & 3) {
     default:
     case 0x0: // 8k
         mask = 0xffffffffffffe000ULL;
@@ -409,12 +409,12 @@ static inline int ultrasparc_tag_match(uint64_t tlb_tag, uint64_t tlb_tte,
     }
 
     // valid, context match, virtual address match?
-    if ((tlb_tte & 0x8000000000000000ULL) &&
-            compare_masked(context, tlb_tag, 0x1fff) &&
-            compare_masked(address, tlb_tag, mask))
+    if ((tlb->tte & 0x8000000000000000ULL) &&
+            compare_masked(context, tlb->tag, 0x1fff) &&
+            compare_masked(address, tlb->tag, mask))
     {
         // decode physical address
-        *physical = ((tlb_tte & mask) | (address & ~mask)) & 0x1ffffffe000ULL;
+        *physical = ((tlb->tte & mask) | (address & ~mask)) & 0x1ffffffe000ULL;
         return 1;
     }
 
@@ -434,21 +434,31 @@ static int get_physical_address_data(CPUState *env,
         return 0;
     }
 
-    context = env->dmmuregs[1] & 0x1fff;
+    context = env->dmmu.mmu_primary_context & 0x1fff;
 
     for (i = 0; i < 64; i++) {
         // ctx match, vaddr match, valid?
-        if (ultrasparc_tag_match(env->dtlb_tag[i], env->dtlb_tte[i],
+        if (ultrasparc_tag_match(&env->dtlb[i],
                                  address, context, physical)
         ) {
             // access ok?
-            if (((env->dtlb_tte[i] & 0x4) && is_user) ||
-                (!(env->dtlb_tte[i] & 0x2) && (rw == 1))) {
-                if (env->dmmuregs[3]) /* Fault status register */
-                    env->dmmuregs[3] = 2; /* overflow (not read before
+            if (((env->dtlb[i].tte & 0x4) && is_user) ||
+                (!(env->dtlb[i].tte & 0x2) && (rw == 1))) {
+                uint8_t fault_type = 0;
+
+                if ((env->dtlb[i].tte & 0x4) && is_user) {
+                    fault_type |= 1; /* privilege violation */
+                }
+
+                if (env->dmmu.sfsr & 1) /* Fault status register */
+                    env->dmmu.sfsr = 2; /* overflow (not read before
                                              another fault) */
-                env->dmmuregs[3] |= (is_user << 3) | ((rw == 1) << 2) | 1;
-                env->dmmuregs[4] = address; /* Fault address register */
+
+                env->dmmu.sfsr |= (is_user << 3) | ((rw == 1) << 2) | 1;
+
+                env->dmmu.sfsr |= (fault_type << 7);
+
+                env->dmmu.sfar = address; /* Fault address register */
                 env->exception_index = TT_DFAULT;
 #ifdef DEBUG_MMU
                 printf("DFAULT at 0x%" PRIx64 "\n", address);
@@ -456,7 +466,7 @@ static int get_physical_address_data(CPUState *env,
                 return 1;
             }
             *prot = PAGE_READ;
-            if (env->dtlb_tte[i] & 0x2)
+            if (env->dtlb[i].tte & 0x2)
                 *prot |= PAGE_WRITE;
             return 0;
         }
@@ -464,7 +474,7 @@ static int get_physical_address_data(CPUState *env,
 #ifdef DEBUG_MMU
     printf("DMISS at 0x%" PRIx64 "\n", address);
 #endif
-    env->dmmuregs[6] = (address & ~0x1fffULL) | context;
+    env->dmmu.tag_access = (address & ~0x1fffULL) | context;
     env->exception_index = TT_DMISS;
     return 1;
 }
@@ -483,19 +493,19 @@ static int get_physical_address_code(CPUState *env,
         return 0;
     }
 
-    context = env->dmmuregs[1] & 0x1fff;
+    context = env->dmmu.mmu_primary_context & 0x1fff;
 
     for (i = 0; i < 64; i++) {
         // ctx match, vaddr match, valid?
-        if (ultrasparc_tag_match(env->itlb_tag[i], env->itlb_tte[i],
+        if (ultrasparc_tag_match(&env->itlb[i],
                                  address, context, physical)
         ) {
             // access ok?
-            if ((env->itlb_tte[i] & 0x4) && is_user) {
-                if (env->immuregs[3]) /* Fault status register */
-                    env->immuregs[3] = 2; /* overflow (not read before
+            if ((env->itlb[i].tte & 0x4) && is_user) {
+                if (env->immu.sfsr) /* Fault status register */
+                    env->immu.sfsr = 2; /* overflow (not read before
                                              another fault) */
-                env->immuregs[3] |= (is_user << 3) | 1;
+                env->immu.sfsr |= (is_user << 3) | 1;
                 env->exception_index = TT_TFAULT;
 #ifdef DEBUG_MMU
                 printf("TFAULT at 0x%" PRIx64 "\n", address);
@@ -510,7 +520,7 @@ static int get_physical_address_code(CPUState *env,
     printf("TMISS at 0x%" PRIx64 "\n", address);
 #endif
     /* Context is stored in DMMU (dmmuregs[1]) also for IMMU */
-    env->immuregs[6] = (address & ~0x1fffULL) | context;
+    env->immu.tag_access = (address & ~0x1fffULL) | context;
     env->exception_index = TT_TMISS;
     return 1;
 }
@@ -561,7 +571,7 @@ void dump_mmu(CPUState *env)
     const char *mask;
 
     printf("MMU contexts: Primary: %" PRId64 ", Secondary: %" PRId64 "\n",
-           env->dmmuregs[1], env->dmmuregs[2]);
+           env->dmmu.mmu_primary_context, env->dmmu.mmu_secondary_context);
     if ((env->lsu & DMMU_E) == 0) {
         printf("DMMU disabled\n");
     } else {
@@ -583,8 +593,9 @@ void dump_mmu(CPUState *env)
                 break;
             }
             if ((env->dtlb_tte[i] & 0x8000000000000000ULL) != 0) {
-                printf("VA: %" PRIx64 ", PA: %" PRIx64
+                printf("[%02u] VA: " PRIx64 ", PA: " PRIx64
                        ", %s, %s, %s, %s, ctx %" PRId64 "\n",
+                       i,
                        env->dtlb_tag[i] & (uint64_t)~0x1fffULL,
                        env->dtlb_tte[i] & (uint64_t)0x1ffffffe000ULL,
                        mask,
@@ -616,14 +627,15 @@ void dump_mmu(CPUState *env)
                 break;
             }
             if ((env->itlb_tte[i] & 0x8000000000000000ULL) != 0) {
-                printf("VA: %" PRIx64 ", PA: %" PRIx64
+                printf("[%02u] VA: " PRIx64 ", PA: " PRIx64
                        ", %s, %s, %s, ctx %" PRId64 "\n",
-                       env->itlb_tag[i] & (uint64_t)~0x1fffULL,
+                       i,
+                       env->itlb[i].tag & (uint64_t)~0x1fffULL,
                        env->itlb_tte[i] & (uint64_t)0x1ffffffe000ULL,
                        mask,
                        env->itlb_tte[i] & 0x4? "priv": "user",
                        env->itlb_tte[i] & 0x40? "locked": "unlocked",
-                       env->itlb_tag[i] & (uint64_t)0x1fffULL);
+                       env->itlb[i].tag & (uint64_t)0x1fffULL);
             }
         }
     }
diff --git a/target-sparc/machine.c b/target-sparc/machine.c
index 4984e8d886..3d196eedc9 100644
--- a/target-sparc/machine.c
+++ b/target-sparc/machine.c
@@ -52,10 +52,10 @@ void cpu_save(QEMUFile *f, void *opaque)
         qemu_put_be64s(f, &env->dmmuregs[i]);
     }
     for (i = 0; i < 64; i++) {
-        qemu_put_be64s(f, &env->itlb_tag[i]);
-        qemu_put_be64s(f, &env->itlb_tte[i]);
-        qemu_put_be64s(f, &env->dtlb_tag[i]);
-        qemu_put_be64s(f, &env->dtlb_tte[i]);
+        qemu_put_be64s(f, &env->itlb[i].tag);
+        qemu_put_be64s(f, &env->itlb[i].tte);
+        qemu_put_be64s(f, &env->dtlb[i].tag);
+        qemu_put_be64s(f, &env->dtlb[i].tte);
     }
     qemu_put_be32s(f, &env->mmu_version);
     for (i = 0; i < MAXTL_MAX; i++) {
@@ -148,10 +148,10 @@ int cpu_load(QEMUFile *f, void *opaque, int version_id)
         qemu_get_be64s(f, &env->dmmuregs[i]);
     }
     for (i = 0; i < 64; i++) {
-        qemu_get_be64s(f, &env->itlb_tag[i]);
-        qemu_get_be64s(f, &env->itlb_tte[i]);
-        qemu_get_be64s(f, &env->dtlb_tag[i]);
-        qemu_get_be64s(f, &env->dtlb_tte[i]);
+        qemu_get_be64s(f, &env->itlb[i].tag);
+        qemu_get_be64s(f, &env->itlb[i].tte);
+        qemu_get_be64s(f, &env->dtlb[i].tag);
+        qemu_get_be64s(f, &env->dtlb[i].tte);
     }
     qemu_get_be32s(f, &env->mmu_version);
     for (i = 0; i < MAXTL_MAX; i++) {
diff --git a/target-sparc/op_helper.c b/target-sparc/op_helper.c
index 739ed9abd2..5acaa0d268 100644
--- a/target-sparc/op_helper.c
+++ b/target-sparc/op_helper.c
@@ -46,8 +46,8 @@ static uint64_t ultrasparc_tsb_pointer(uint64_t tsb_register,
                                        int page_size)
 {
     uint64_t tsb_base = tsb_register & ~0x1fffULL;
-    int tsb_split = (env->dmmuregs[5] & 0x1000ULL) ? 1 : 0;
-    int tsb_size  = env->dmmuregs[5] & 0xf;
+    int tsb_split = (tsb_register & 0x1000ULL) ? 1 : 0;
+    int tsb_size  = tsb_register & 0xf;
 
     // discard lower 13 bits which hold tag access context
     uint64_t tag_access_va = tag_access_register & ~0x1fffULL;
@@ -87,6 +87,55 @@ static uint64_t ultrasparc_tag_target(uint64_t tag_access_register)
     return ((tag_access_register & 0x1fff) << 48) | (tag_access_register >> 22);
 }
 
+static void replace_tlb_entry(SparcTLBEntry *tlb, CPUState *env1,
+                              uint64_t tlb_tag, uint64_t tlb_tte)
+{
+    target_ulong mask, size, va, offset;
+
+    // flush page range if translation is valid
+    if (tlb->tte & 0x8000000000000000ULL) {
+
+        mask = 0xffffffffffffe000ULL;
+        mask <<= 3 * ((tlb->tte >> 61) & 3);
+        size = ~mask + 1;
+
+        va = tlb->tag & mask;
+
+        for (offset = 0; offset < size; offset += TARGET_PAGE_SIZE) {
+            tlb_flush_page(env1, va + offset);
+        }
+    }
+
+    tlb->tag = tlb_tag;
+    tlb->tte = tlb_tte;
+}
+
+static void demap_tlb(SparcTLBEntry *tlb, target_ulong demap_addr,
+                      CPUState *env1)
+{
+    unsigned int i;
+    target_ulong mask;
+
+    for (i = 0; i < 64; i++) {
+        if (tlb[i].tte & 0x8000000000000000ULL) {
+
+            mask = 0xffffffffffffe000ULL;
+            mask <<= 3 * ((tlb[i].tte >> 61) & 3);
+
+            if ((demap_addr & mask) == (tlb[i].tag & mask)) {
+                replace_tlb_entry(&tlb[i], env1, 0, 0);
+#ifdef DEBUG_MMU
+                DPRINTF_MMU("mmu demap invalidated entry [%02u]\n",
+                            i);
+                dump_mmu(env);
+#endif
+            }
+            //return;
+        }
+    }
+
+}
+
 #endif
 
 static inline void address_mask(CPUState *env1, target_ulong *addr)
@@ -2143,7 +2192,7 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
 
             if (reg == 0) {
                 // I-TSB Tag Target register
-                ret = ultrasparc_tag_target(env->immuregs[6]);
+                ret = ultrasparc_tag_target(env->immu.tag_access);
             } else {
                 ret = env->immuregs[reg];
             }
@@ -2154,7 +2203,7 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
         {
             // env->immuregs[5] holds I-MMU TSB register value
             // env->immuregs[6] holds I-MMU Tag Access register value
-            ret = ultrasparc_tsb_pointer(env->immuregs[5], env->immuregs[6],
+            ret = ultrasparc_tsb_pointer(env->immu.tsb, env->immu.tag_access,
                                          8*1024);
             break;
         }
@@ -2162,7 +2211,7 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
         {
             // env->immuregs[5] holds I-MMU TSB register value
             // env->immuregs[6] holds I-MMU Tag Access register value
-            ret = ultrasparc_tsb_pointer(env->immuregs[5], env->immuregs[6],
+            ret = ultrasparc_tsb_pointer(env->immu.tsb, env->immu.tag_access,
                                          64*1024);
             break;
         }
@@ -2170,14 +2219,14 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
         {
             int reg = (addr >> 3) & 0x3f;
 
-            ret = env->itlb_tte[reg];
+            ret = env->itlb[reg].tte;
             break;
         }
     case 0x56: // I-MMU tag read
         {
             int reg = (addr >> 3) & 0x3f;
 
-            ret = env->itlb_tag[reg];
+            ret = env->itlb[reg].tag;
             break;
         }
     case 0x58: // D-MMU regs
@@ -2186,7 +2235,7 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
 
             if (reg == 0) {
                 // D-TSB Tag Target register
-                ret = ultrasparc_tag_target(env->dmmuregs[6]);
+                ret = ultrasparc_tag_target(env->dmmu.tag_access);
             } else {
                 ret = env->dmmuregs[reg];
             }
@@ -2196,7 +2245,7 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
         {
             // env->dmmuregs[5] holds D-MMU TSB register value
             // env->dmmuregs[6] holds D-MMU Tag Access register value
-            ret = ultrasparc_tsb_pointer(env->dmmuregs[5], env->dmmuregs[6],
+            ret = ultrasparc_tsb_pointer(env->dmmu.tsb, env->dmmu.tag_access,
                                          8*1024);
             break;
         }
@@ -2204,7 +2253,7 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
         {
             // env->dmmuregs[5] holds D-MMU TSB register value
             // env->dmmuregs[6] holds D-MMU Tag Access register value
-            ret = ultrasparc_tsb_pointer(env->dmmuregs[5], env->dmmuregs[6],
+            ret = ultrasparc_tsb_pointer(env->dmmu.tsb, env->dmmu.tag_access,
                                          64*1024);
             break;
         }
@@ -2212,14 +2261,14 @@ uint64_t helper_ld_asi(target_ulong addr, int asi, int size, int sign)
         {
             int reg = (addr >> 3) & 0x3f;
 
-            ret = env->dtlb_tte[reg];
+            ret = env->dtlb[reg].tte;
             break;
         }
     case 0x5e: // D-MMU tag read
         {
             int reg = (addr >> 3) & 0x3f;
 
-            ret = env->dtlb_tag[reg];
+            ret = env->dtlb[reg].tag;
             break;
         }
     case 0x46: // D-cache data
@@ -2462,25 +2511,34 @@ void helper_st_asi(target_ulong addr, target_ulong val, int asi, int size)
             oldreg = env->immuregs[reg];
             switch(reg) {
             case 0: // RO
-            case 4:
                 return;
             case 1: // Not in I-MMU
             case 2:
-            case 7:
-            case 8:
                 return;
             case 3: // SFSR
                 if ((val & 1) == 0)
                     val = 0; // Clear SFSR
+                env->immu.sfsr = val;
                 break;
+            case 4: // RO
+                return;
             case 5: // TSB access
+                DPRINTF_MMU("immu TSB write: 0x%016" PRIx64 " -> 0x%016"
+                            PRIx64 "\n", env->immu.tsb, val);
+                env->immu.tsb = val;
+                break;
             case 6: // Tag access
+                env->immu.tag_access = val;
+                break;
+            case 7:
+            case 8:
+                return;
             default:
                 break;
             }
-            env->immuregs[reg] = val;
+
             if (oldreg != env->immuregs[reg]) {
-                DPRINTF_MMU("mmu change reg[%d]: 0x%08" PRIx64 " -> 0x%08"
+                DPRINTF_MMU("immu change reg[%d]: 0x%016" PRIx64 " -> 0x%016"
                             PRIx64 "\n", reg, oldreg, env->immuregs[reg]);
             }
 #ifdef DEBUG_MMU
@@ -2494,20 +2552,33 @@ void helper_st_asi(target_ulong addr, target_ulong val, int asi, int size)
 
             // Try finding an invalid entry
             for (i = 0; i < 64; i++) {
-                if ((env->itlb_tte[i] & 0x8000000000000000ULL) == 0) {
-                    env->itlb_tag[i] = env->immuregs[6];
-                    env->itlb_tte[i] = val;
+                if ((env->itlb[i].tte & 0x8000000000000000ULL) == 0) {
+                    replace_tlb_entry(&env->itlb[i], env,
+                                      env->immu.tag_access, val);
+#ifdef DEBUG_MMU
+                    DPRINTF_MMU("immu data map replaced invalid entry [%i]\n",
+                                i);
+                    dump_mmu(env);
+#endif
                     return;
                 }
             }
             // Try finding an unlocked entry
             for (i = 0; i < 64; i++) {
-                if ((env->itlb_tte[i] & 0x40) == 0) {
-                    env->itlb_tag[i] = env->immuregs[6];
-                    env->itlb_tte[i] = val;
+                if ((env->itlb[i].tte & 0x40) == 0) {
+                    replace_tlb_entry(&env->itlb[i], env,
+                                      env->immu.tag_access, val);
+#ifdef DEBUG_MMU
+                    DPRINTF_MMU("immu data map replaced unlocked entry [%i]\n",
+                                i);
+                    dump_mmu(env);
+#endif
                     return;
                 }
             }
+#ifdef DEBUG_MMU
+            DPRINTF_MMU("immu data map failed: no entries available\n");
+#endif
             // error state?
             return;
         }
@@ -2517,27 +2588,18 @@ void helper_st_asi(target_ulong addr, target_ulong val, int asi, int size)
 
             unsigned int i = (addr >> 3) & 0x3f;
 
-            env->itlb_tag[i] = env->immuregs[6];
-            env->itlb_tte[i] = val;
+            replace_tlb_entry(&env->itlb[i], env,
+                              env->immu.tag_access, val);
+
+#ifdef DEBUG_MMU
+            DPRINTF_MMU("immu data access replaced entry [%i]\n",
+                        i);
+            dump_mmu(env);
+#endif
             return;
         }
     case 0x57: // I-MMU demap
-        {
-            unsigned int i;
-
-            for (i = 0; i < 64; i++) {
-                if ((env->itlb_tte[i] & 0x8000000000000000ULL) != 0) {
-                    target_ulong mask = 0xffffffffffffe000ULL;
-
-                    mask <<= 3 * ((env->itlb_tte[i] >> 61) & 3);
-                    if ((val & mask) == (env->itlb_tag[i] & mask)) {
-                        env->itlb_tag[i] = 0;
-                        env->itlb_tte[i] = 0;
-                    }
-                    return;
-                }
-            }
-        }
+        demap_tlb(env->itlb, val, env);
         return;
     case 0x58: // D-MMU regs
         {
@@ -2552,22 +2614,33 @@ void helper_st_asi(target_ulong addr, target_ulong val, int asi, int size)
             case 3: // SFSR
                 if ((val & 1) == 0) {
                     val = 0; // Clear SFSR, Fault address
-                    env->dmmuregs[4] = 0;
+                    env->dmmu.sfar = 0;
                 }
-                env->dmmuregs[reg] = val;
+                env->dmmu.sfsr = val;
                 break;
             case 1: // Primary context
+                env->dmmu.mmu_primary_context = val;
+                break;
             case 2: // Secondary context
+                env->dmmu.mmu_secondary_context = val;
+                break;
             case 5: // TSB access
+                DPRINTF_MMU("dmmu TSB write: 0x%016" PRIx64 " -> 0x%016"
+                            PRIx64 "\n", env->dmmu.tsb, val);
+                env->dmmu.tsb = val;
+                break;
             case 6: // Tag access
+                env->dmmu.tag_access = val;
+                break;
             case 7: // Virtual Watchpoint
             case 8: // Physical Watchpoint
             default:
+                env->dmmuregs[reg] = val;
                 break;
             }
-            env->dmmuregs[reg] = val;
+
             if (oldreg != env->dmmuregs[reg]) {
-                DPRINTF_MMU("mmu change reg[%d]: 0x%08" PRIx64 " -> 0x%08"
+                DPRINTF_MMU("dmmu change reg[%d]: 0x%016" PRIx64 " -> 0x%016"
                             PRIx64 "\n", reg, oldreg, env->dmmuregs[reg]);
             }
 #ifdef DEBUG_MMU
@@ -2581,20 +2654,33 @@ void helper_st_asi(target_ulong addr, target_ulong val, int asi, int size)
 
             // Try finding an invalid entry
             for (i = 0; i < 64; i++) {
-                if ((env->dtlb_tte[i] & 0x8000000000000000ULL) == 0) {
-                    env->dtlb_tag[i] = env->dmmuregs[6];
-                    env->dtlb_tte[i] = val;
+                if ((env->dtlb[i].tte & 0x8000000000000000ULL) == 0) {
+                    replace_tlb_entry(&env->dtlb[i], env,
+                                      env->dmmu.tag_access, val);
+#ifdef DEBUG_MMU
+                    DPRINTF_MMU("dmmu data map replaced invalid entry [%i]\n",
+                                i);
+                    dump_mmu(env);
+#endif
                     return;
                 }
             }
             // Try finding an unlocked entry
             for (i = 0; i < 64; i++) {
-                if ((env->dtlb_tte[i] & 0x40) == 0) {
-                    env->dtlb_tag[i] = env->dmmuregs[6];
-                    env->dtlb_tte[i] = val;
+                if ((env->dtlb[i].tte & 0x40) == 0) {
+                    replace_tlb_entry(&env->dtlb[i], env,
+                                      env->dmmu.tag_access, val);
+#ifdef DEBUG_MMU
+                    DPRINTF_MMU("dmmu data map replaced unlocked entry [%i]\n",
+                                i);
+                    dump_mmu(env);
+#endif
                     return;
                 }
             }
+#ifdef DEBUG_MMU
+            DPRINTF_MMU("dmmu data map failed: no entries available\n");
+#endif
             // error state?
             return;
         }
@@ -2602,27 +2688,17 @@ void helper_st_asi(target_ulong addr, target_ulong val, int asi, int size)
         {
             unsigned int i = (addr >> 3) & 0x3f;
 
-            env->dtlb_tag[i] = env->dmmuregs[6];
-            env->dtlb_tte[i] = val;
+            replace_tlb_entry(&env->dtlb[i], env,
+                              env->dmmu.tag_access, val);
+#ifdef DEBUG_MMU
+            DPRINTF_MMU("dmmu data access replaced entry [%i]\n",
+                        i);
+            dump_mmu(env);
+#endif
             return;
         }
     case 0x5f: // D-MMU demap
-        {
-            unsigned int i;
-
-            for (i = 0; i < 64; i++) {
-                if ((env->dtlb_tte[i] & 0x8000000000000000ULL) != 0) {
-                    target_ulong mask = 0xffffffffffffe000ULL;
-
-                    mask <<= 3 * ((env->dtlb_tte[i] >> 61) & 3);
-                    if ((val & mask) == (env->dtlb_tag[i] & mask)) {
-                        env->dtlb_tag[i] = 0;
-                        env->dtlb_tte[i] = 0;
-                    }
-                    return;
-                }
-            }
-        }
+        demap_tlb(env->dtlb, val, env);
         return;
     case 0x49: // Interrupt data receive
         // XXX