target-m68k: Implement bitfield ops for memory

Signed-off-by: Richard Henderson <rth@twiddle.net>
Message-Id: <1478699171-10637-6-git-send-email-rth@twiddle.net>
Signed-off-by: Laurent Vivier <laurent@vivier.eu>

1 files changed, 185 insertions, 0 deletions

diff --git a/target/m68k/op_helper.c b/target/m68k/op_helper.c
index e56b815d73..51b9e00f5e 100644
--- a/target/m68k/op_helper.c
+++ b/target/m68k/op_helper.c
@@ -469,3 +469,188 @@ void HELPER(cas2l)(CPUM68KState *env, uint32_t regs, uint32_t a1, uint32_t a2)
     env->dregs[Dc1] = l1;
     env->dregs[Dc2] = l2;
 }
+
+struct bf_data {
+    uint32_t addr;
+    uint32_t bofs;
+    uint32_t blen;
+    uint32_t len;
+};
+
+static struct bf_data bf_prep(uint32_t addr, int32_t ofs, uint32_t len)
+{
+    int bofs, blen;
+
+    /* Bound length; map 0 to 32.  */
+    len = ((len - 1) & 31) + 1;
+
+    /* Note that ofs is signed.  */
+    addr += ofs / 8;
+    bofs = ofs % 8;
+    if (bofs < 0) {
+        bofs += 8;
+        addr -= 1;
+    }
+
+    /* Compute the number of bytes required (minus one) to
+       satisfy the bitfield.  */
+    blen = (bofs + len - 1) / 8;
+
+    /* Canonicalize the bit offset for data loaded into a 64-bit big-endian
+       word.  For the cases where BLEN is not a power of 2, adjust ADDR so
+       that we can use the next power of two sized load without crossing a
+       page boundary, unless the field itself crosses the boundary.  */
+    switch (blen) {
+    case 0:
+        bofs += 56;
+        break;
+    case 1:
+        bofs += 48;
+        break;
+    case 2:
+        if (addr & 1) {
+            bofs += 8;
+            addr -= 1;
+        }
+        /* fallthru */
+    case 3:
+        bofs += 32;
+        break;
+    case 4:
+        if (addr & 3) {
+            bofs += 8 * (addr & 3);
+            addr &= -4;
+        }
+        break;
+    default:
+        g_assert_not_reached();
+    }
+
+    return (struct bf_data){
+        .addr = addr,
+        .bofs = bofs,
+        .blen = blen,
+        .len = len,
+    };
+}
+
+static uint64_t bf_load(CPUM68KState *env, uint32_t addr, int blen,
+                        uintptr_t ra)
+{
+    switch (blen) {
+    case 0:
+        return cpu_ldub_data_ra(env, addr, ra);
+    case 1:
+        return cpu_lduw_data_ra(env, addr, ra);
+    case 2:
+    case 3:
+        return cpu_ldl_data_ra(env, addr, ra);
+    case 4:
+        return cpu_ldq_data_ra(env, addr, ra);
+    default:
+        g_assert_not_reached();
+    }
+}
+
+static void bf_store(CPUM68KState *env, uint32_t addr, int blen,
+                     uint64_t data, uintptr_t ra)
+{
+    switch (blen) {
+    case 0:
+        cpu_stb_data_ra(env, addr, data, ra);
+        break;
+    case 1:
+        cpu_stw_data_ra(env, addr, data, ra);
+        break;
+    case 2:
+    case 3:
+        cpu_stl_data_ra(env, addr, data, ra);
+        break;
+    case 4:
+        cpu_stq_data_ra(env, addr, data, ra);
+        break;
+    default:
+        g_assert_not_reached();
+    }
+}
+
+uint32_t HELPER(bfexts_mem)(CPUM68KState *env, uint32_t addr,
+                            int32_t ofs, uint32_t len)
+{
+    uintptr_t ra = GETPC();
+    struct bf_data d = bf_prep(addr, ofs, len);
+    uint64_t data = bf_load(env, d.addr, d.blen, ra);
+
+    return (int64_t)(data << d.bofs) >> (64 - d.len);
+}
+
+uint64_t HELPER(bfextu_mem)(CPUM68KState *env, uint32_t addr,
+                            int32_t ofs, uint32_t len)
+{
+    uintptr_t ra = GETPC();
+    struct bf_data d = bf_prep(addr, ofs, len);
+    uint64_t data = bf_load(env, d.addr, d.blen, ra);
+
+    /* Put CC_N at the top of the high word; put the zero-extended value
+       at the bottom of the low word.  */
+    data <<= d.bofs;
+    data >>= 64 - d.len;
+    data |= data << (64 - d.len);
+
+    return data;
+}
+
+uint32_t HELPER(bfins_mem)(CPUM68KState *env, uint32_t addr, uint32_t val,
+                           int32_t ofs, uint32_t len)
+{
+    uintptr_t ra = GETPC();
+    struct bf_data d = bf_prep(addr, ofs, len);
+    uint64_t data = bf_load(env, d.addr, d.blen, ra);
+    uint64_t mask = -1ull << (64 - d.len) >> d.bofs;
+
+    data = (data & ~mask) | (((uint64_t)val << (64 - d.len)) >> d.bofs);
+
+    bf_store(env, d.addr, d.blen, data, ra);
+
+    /* The field at the top of the word is also CC_N for CC_OP_LOGIC.  */
+    return val << (32 - d.len);
+}
+
+uint32_t HELPER(bfchg_mem)(CPUM68KState *env, uint32_t addr,
+                           int32_t ofs, uint32_t len)
+{
+    uintptr_t ra = GETPC();
+    struct bf_data d = bf_prep(addr, ofs, len);
+    uint64_t data = bf_load(env, d.addr, d.blen, ra);
+    uint64_t mask = -1ull << (64 - d.len) >> d.bofs;
+
+    bf_store(env, d.addr, d.blen, data ^ mask, ra);
+
+    return ((data & mask) << d.bofs) >> 32;
+}
+
+uint32_t HELPER(bfclr_mem)(CPUM68KState *env, uint32_t addr,
+                           int32_t ofs, uint32_t len)
+{
+    uintptr_t ra = GETPC();
+    struct bf_data d = bf_prep(addr, ofs, len);
+    uint64_t data = bf_load(env, d.addr, d.blen, ra);
+    uint64_t mask = -1ull << (64 - d.len) >> d.bofs;
+
+    bf_store(env, d.addr, d.blen, data & ~mask, ra);
+
+    return ((data & mask) << d.bofs) >> 32;
+}
+
+uint32_t HELPER(bfset_mem)(CPUM68KState *env, uint32_t addr,
+                           int32_t ofs, uint32_t len)
+{
+    uintptr_t ra = GETPC();
+    struct bf_data d = bf_prep(addr, ofs, len);
+    uint64_t data = bf_load(env, d.addr, d.blen, ra);
+    uint64_t mask = -1ull << (64 - d.len) >> d.bofs;
+
+    bf_store(env, d.addr, d.blen, data | mask, ra);
+
+    return ((data & mask) << d.bofs) >> 32;
+}