target-m68k: Implement bfffo

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

1 files changed, 21 insertions, 0 deletions

diff --git a/target/m68k/op_helper.c b/target/m68k/op_helper.c
index 51b9e00f5e..7b5126c88d 100644
--- a/target/m68k/op_helper.c
+++ b/target/m68k/op_helper.c
@@ -654,3 +654,24 @@ uint32_t HELPER(bfset_mem)(CPUM68KState *env, uint32_t addr,
 
     return ((data & mask) << d.bofs) >> 32;
 }
+
+uint32_t HELPER(bfffo_reg)(uint32_t n, uint32_t ofs, uint32_t len)
+{
+    return (n ? clz32(n) : len) + ofs;
+}
+
+uint64_t HELPER(bfffo_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;
+    uint64_t n = (data & mask) << d.bofs;
+    uint32_t ffo = helper_bfffo_reg(n >> 32, ofs, d.len);
+
+    /* Return FFO in the low word and N in the high word.
+       Note that because of MASK and the shift, the low word
+       is already zero.  */
+    return n | ffo;
+}