From b77953a3955b958b6592d07f55144ef3b42eca83 Mon Sep 17 00:00:00 2001
From: Denis Drakhnia <numas13@gmail.com>
Date: Fri, 9 Feb 2024 09:30:30 +0200
Subject: [PATCH] e2k: stmqp v6 unaligned access

---
 linux-user/e2k/cpu_loop.c |  3 ++
 target/e2k/helper-tcg.h   |  3 ++
 target/e2k/helper.h       |  5 +--
 target/e2k/helper_sm.c    | 11 ++++---
 target/e2k/helper_vec.c   | 66 +++++++++++++++++++++++++++++++++++++--
 target/e2k/translate.c    | 56 +++++++--------------------------
 6 files changed, 90 insertions(+), 54 deletions(-)

diff --git a/linux-user/e2k/cpu_loop.c b/linux-user/e2k/cpu_loop.c
index a18b5206f2..5fa7bfbcdc 100644
--- a/linux-user/e2k/cpu_loop.c
+++ b/linux-user/e2k/cpu_loop.c
@@ -164,6 +164,9 @@ void cpu_loop(CPUE2KState *env)
         case E2K_EXCP_DATA_PAGE:
             gen_signal(env, TARGET_SIGSEGV, TARGET_SEGV_MAPERR, env->ip);
             break;
+        case E2K_EXCP_ADDR_NOT_ALIGNED:
+            gen_signal(env, TARGET_SIGBUS, TARGET_BUS_ADRALN, env->ip);
+            break;
         case E2K_EXCP_DIV:
             gen_signal(env, TARGET_SIGFPE, 0, env->ip);
             break;
diff --git a/target/e2k/helper-tcg.h b/target/e2k/helper-tcg.h
index afe83520bf..b3774fa853 100644
--- a/target/e2k/helper-tcg.h
+++ b/target/e2k/helper-tcg.h
@@ -39,6 +39,9 @@ void G_NORETURN raise_exception(CPUE2KState *desc, int exception_index);
 void G_NORETURN raise_exception_ra(CPUE2KState *desc, int exception_index,
     uintptr_t retaddr);
 
+/* helper_sm.c */
+bool e2k_probe_rw_access(CPUE2KState *env, target_ulong addr, int size);
+
 static inline uint64_t env_wd_get(CPUE2KState *env)
 {
     E2KWdState *wd = &env->wd;
diff --git a/target/e2k/helper.h b/target/e2k/helper.h
index 2dd6cec3f0..4c81ec005b 100644
--- a/target/e2k/helper.h
+++ b/target/e2k/helper.h
@@ -11,7 +11,6 @@ DEF_HELPER_4(setwd, void, env, int, int, int)
 
 DEF_HELPER_FLAGS_3(probe_read_access,   TCG_CALL_NO_RWG_SE, int, env, tl, int)
 DEF_HELPER_FLAGS_3(probe_write_access,  TCG_CALL_NO_RWG_SE, int, env, tl, int)
-DEF_HELPER_FLAGS_3(probe_rw_access,     TCG_CALL_NO_RWG_SE, int, env, tl, int)
 
 DEF_HELPER_FLAGS_2(aau_load_program,    TCG_CALL_NO_RWG, void, env, i64)
 DEF_HELPER_FLAGS_6(mova_ptr,            TCG_CALL_NO_RWG, tl, env, int, int, int, int, int)
@@ -346,7 +345,9 @@ DEF_HELPER_FLAGS_2(fxtoidtr, TCG_CALL_NO_RWG, i64, env, f80)
 DEF_HELPER_FLAGS_3(fstoifs,  TCG_CALL_NO_RWG, i32, env, i32, i32)
 DEF_HELPER_FLAGS_3(fdtoifd,  TCG_CALL_NO_RWG, i64, env, i64, i64)
 
-DEF_HELPER_FLAGS_1(stmqp_mask, TCG_CALL_NO_RWG_SE, vec, i32)
+DEF_HELPER_FLAGS_5(stmqp5,      TCG_CALL_NO_RWG, void, env, tl, vec, i32, i32)
+DEF_HELPER_FLAGS_5(stmqp6,      TCG_CALL_NO_RWG, void, env, tl, vec, i32, i32)
+DEF_HELPER_FLAGS_6(stmqp_mlock, TCG_CALL_NO_RWG, i32, env, tl, vec, i32, i32, vec)
 
 #undef f80
 #undef vec
diff --git a/target/e2k/helper_sm.c b/target/e2k/helper_sm.c
index 1a6a8ccf00..f722dcad16 100644
--- a/target/e2k/helper_sm.c
+++ b/target/e2k/helper_sm.c
@@ -1,6 +1,7 @@
 #include "qemu/osdep.h"
 #include "qemu/log.h"
 #include "cpu.h"
+#include "helper-tcg.h"
 #include "exec/exec-all.h"
 #include "qemu/host-utils.h"
 #include "exec/helper-proto.h"
@@ -39,6 +40,11 @@ static bool e2k_probe_access(CPUE2KState *env, target_ulong addr, int size, int
     }
 }
 
+bool e2k_probe_rw_access(CPUE2KState *env, target_ulong addr, int size)
+{
+    return e2k_probe_access(env, addr, size, PAGE_READ | PAGE_WRITE_ORG);
+}
+
 int HELPER(probe_read_access)(CPUE2KState *env, target_ulong addr, int size)
 {
     return e2k_probe_access(env, addr, size, PAGE_READ);
@@ -48,9 +54,4 @@ int HELPER(probe_write_access)(CPUE2KState *env, target_ulong addr, int size)
 {
     return e2k_probe_access(env, addr, size, PAGE_WRITE_ORG);
 }
-
-int HELPER(probe_rw_access)(CPUE2KState *env, target_ulong addr, int size)
-{
-    return e2k_probe_access(env, addr, size, PAGE_READ | PAGE_WRITE_ORG);
-}
 #endif /* CONFIG_USER_ONLY */
diff --git a/target/e2k/helper_vec.c b/target/e2k/helper_vec.c
index fb54e2666a..cca5707ef1 100644
--- a/target/e2k/helper_vec.c
+++ b/target/e2k/helper_vec.c
@@ -658,7 +658,69 @@ static inline Int128 mask16(uint16_t bitmask)
     return int128_make128(mask8(bitmask), mask8(bitmask >> 8));
 }
 
-Int128 HELPER(stmqp_mask)(uint32_t bitmask)
+static Int128 qmerge(Int128 a, Int128 b, uint32_t mask)
 {
-    return mask16(bitmask);
+    Int128 m;
+
+    m = mask16(mask);
+    a = int128_and(a, m);
+    b = int128_and(b, int128_not(m));
+    return int128_or(a,b );
+}
+
+static void stmqp(CPUE2KState *env, target_ulong addr, Int128 value,
+    uint32_t mask, MemOp memop, bool sm)
+{
+    Int128 t;
+    MemOpIdx oi = make_memop_idx(memop | MO_LE | MO_UO, e2k_env_mmu_index(env, false));
+
+    if (mask & 0xffff && (!sm || e2k_probe_rw_access(env, addr, 16))) {
+        t = cpu_ld16_mmu(env, addr, oi, GETPC());
+        t = qmerge(value, t, mask);
+        cpu_st16_mmu(env, addr, t, oi, GETPC());
+    }
+}
+
+void HELPER(stmqp5)(CPUE2KState *env, target_ulong addr, Int128 value,
+    uint32_t mask, uint32_t sm)
+{
+    stmqp(env, addr, value, mask, MO_ALIGN, sm);
+}
+
+void HELPER(stmqp6)(CPUE2KState *env, target_ulong addr, Int128 value,
+    uint32_t mask, uint32_t sm)
+{
+    target_ulong start, last;
+
+    start = addr & TARGET_PAGE_MASK;
+    last = (addr + 15) & TARGET_PAGE_MASK;
+
+    if (start != last) {
+        int align = addr & 15;
+
+        stmqp(env, addr - align, int128_lshift(value, align * 8), mask << align, MO_UNALN, sm);
+
+        align = 16 - align;
+        value = int128_rshift(value, align * 8);
+        addr += align;
+        mask = (mask & 0xffff) >> align;
+    }
+
+    stmqp(env, addr, value, mask, MO_UNALN, sm);
+}
+
+uint32_t HELPER(stmqp_mlock)(CPUE2KState *env, target_ulong addr, Int128 value,
+    uint32_t mask, uint32_t sm, Int128 last_val)
+{
+    Int128 t;
+    MemOpIdx oi = make_memop_idx(MO_ALIGN | MO_LE | MO_UO, e2k_env_mmu_index(env, false));
+
+    if (mask & 0xffff && (!sm || e2k_probe_rw_access(env, addr, 16))) {
+        t = cpu_ld16_mmu(env, addr, oi, GETPC());
+        t = qmerge(value, t, mask);
+        t = cpu_atomic_cmpxchgo_le_mmu(env, addr, last_val, t, oi, GETPC());
+        return int128_ne(t, last_val);
+    } else {
+        return 0;
+    }
 }
diff --git a/target/e2k/translate.c b/target/e2k/translate.c
index 5bc0a291f8..d5df52719c 100644
--- a/target/e2k/translate.c
+++ b/target/e2k/translate.c
@@ -3970,13 +3970,6 @@ static void gen_probe_write_access(TCGv_i32 ret, TCGv addr, int size,
     gen_helper_probe_write_access(ret, tcg_env, addr, t0);
 }
 
-static void gen_probe_rw_access(TCGv_i32 ret, TCGv addr, int size,
-    int mmu_idx)
-{
-    TCGv_i32 t0 = tcg_constant_i32(size);
-    gen_helper_probe_rw_access(ret, tcg_env, addr, t0);
-}
-
 static AlopResult gen_ld_raw_i64(Alop *alop, TCGv_i32 tag, TCGv addr,
     MemOp memop, bool skip, bool save)
 {
@@ -4129,52 +4122,25 @@ IMPL_GEN_ST(gen_st_raw_i32,  s, gen_atomic_cmpxchg_mlock_i32,  tcg_gen_qemu_st_i
 IMPL_GEN_ST(gen_st_raw_i64,  d, gen_atomic_cmpxchg_mlock_i64,  tcg_gen_qemu_st_i64)
 IMPL_GEN_ST(gen_st_raw_i128, q, gen_atomic_cmpxchg_mlock_i128, tcg_gen_qemu_st_i128)
 
-static void gen_stmqp_merge_i128(TCGv_i128 ret, TCGv_i128 a, TCGv_i128 b, TCGv_i32 mask)
-{
-    TCGv_i128 m = tcg_temp_new_i128();
-    TCGv_i128 t0 = tcg_temp_new_i128();
-    TCGv_i128 t1 = tcg_temp_new_i128();
-
-    gen_helper_stmqp_mask(m, mask);
-    gen_qpand(t0, a, m);
-    gen_qpandn(t1, b, m);
-    gen_qpor(ret, t0, t1);
-}
-
 static void gen_stm_raw_i128(Alop *alop, TCGv addr,
     MemOp memop, bool check)
 {
-    TCGLabel *l0 = gen_new_label();
     Tagged_i32 s2 = gen_tagged_src2_s(alop);
     Tagged_i128 s4 = gen_tagged_src4_q(alop);
-    TCGv_i32 mask = tcg_temp_new_i32();
-    TCGv_i128 t0 = tcg_temp_new_i128();
-
-    // force alignment for v5 only
-    if (alop->ctx->version == 5) {
-        memop |= MO_ALIGN_16;
-    }
-
-    tcg_gen_andi_i32(mask, s2.val, 0xffff);
-    tcg_gen_brcondi_i32(TCG_COND_EQ, mask, 0, l0);
-
-    if (alop->als.sm) {
-        TCGv_i32 t5 = tcg_temp_new_i32();
-
-        gen_probe_rw_access(t5, addr, 16, alop->ctx->mmuidx);
-        tcg_gen_brcondi_i32(TCG_COND_EQ, t5, 0, l0);
-    }
-
-    tcg_gen_qemu_ld_i128(t0, addr, alop->ctx->mmuidx, memop);
-    gen_stmqp_merge_i128(t0, s4.val, t0, s2.val);
+    TCGv_i32 sm = tcg_constant_i32(alop->als.sm);
 
     if (check && alop->ctx->mlock) {
-        gen_atomic_cmpxchg_mlock_i128(alop, t0, addr, memop);
-    } else {
-        tcg_gen_qemu_st_i128(t0, addr, alop->ctx->mmuidx, memop);
-    }
+        TCGv_i128 t0 = tcg_temp_new_i128();
 
-    gen_set_label(l0);
+        tcg_gen_concat_i64_i128(t0, cpu_last_val0, cpu_last_val1);
+        gen_helper_stmqp_mlock(alop->ctx->mlock, tcg_env, addr, s4.val, s2.val, sm, t0);
+    } else {
+        if (alop->ctx->version <= 5) {
+            gen_helper_stmqp5(tcg_env, addr, s4.val, s2.val, sm);
+        } else {
+            gen_helper_stmqp6(tcg_env, addr, s4.val, s2.val, sm);
+        }
+    }
 }
 
 typedef enum {