From 5a26dd5ccb934a1b03c02761c47eaebdf13df9dc Mon Sep 17 00:00:00 2001
From: Johannes Stoelp <johannes.stoelp@gmail.com>
Date: Sat, 28 Oct 2023 00:07:44 +0200
Subject: lt_bus: fix addr fixup in dmi invalidation and get dmi ptr

---
 src/models/lt_bus.h | 98 +++++++++++++++++++++++++++++++++++++----------------
 1 file changed, 69 insertions(+), 29 deletions(-)

(limited to 'src/models')

diff --git a/src/models/lt_bus.h b/src/models/lt_bus.h
index f52f8b0..37cfe84 100644
--- a/src/models/lt_bus.h
+++ b/src/models/lt_bus.h
@@ -96,14 +96,29 @@ class lt_bus : public sc_core::sc_module {
   void invalidate_direct_mem_ptr(usize id,
                                  sc_dt::uint64 start,
                                  sc_dt::uint64 end) {
+    assert(start <= end);
+
     for (const auto& map : m_mappings) {
       if (map.idx != id) {
         continue;
       }
-      const sc_dt::uint64 s = map.addr.start + start;
-      const sc_dt::uint64 e = map.addr.start + end;
+
+      // Compute absolute start address.
+      const sc_dt::uint64 abs_start = map.addr.start + start;
+      assert(abs_start <= map.addr.end);
+
+      // Compute absolute end address. Limit if mapping range exceeded.
+      const auto comp_abs_end = [&map](sc_dt::uint64 abs_end) -> sc_dt::uint64 {
+        if (abs_end > map.addr.end /* exceeds mapping */ ||
+            abs_end < map.addr.start /* wraps around */) {
+          return map.addr.end;
+        }
+        return abs_end;
+      };
+      const sc_dt::uint64 abs_end = comp_abs_end(map.addr.start + end);
+
       for (auto& sock : m_initiators) {
-        (*sock)->invalidate_direct_mem_ptr(s, e);
+        (*sock)->invalidate_direct_mem_ptr(abs_start, abs_end);
       }
     }
   }
@@ -113,14 +128,12 @@ class lt_bus : public sc_core::sc_module {
   void b_transport(usize id,
                    tlm::tlm_generic_payload& tx,
                    sc_core::sc_time& t) {
-    u64 start = tx.get_address();
-    u64 end = start + tx.get_data_length() - 1;
+    if (const auto r = decode(tx)) {
+      const auto start = tx.get_address();
+      assert(r.start <= start);
 
-    if (auto res = decode(range{start, end})) {
-      assert(res.base <= start);
-
-      tx.set_address(start - res.base);
-      (*res.sock)->b_transport(tx, t);
+      tx.set_address(start - r.start);
+      (*r.sock)->b_transport(tx, t);
       tx.set_address(start);
     } else {
       tx.set_response_status(tlm::TLM_ADDRESS_ERROR_RESPONSE);
@@ -130,30 +143,50 @@ class lt_bus : public sc_core::sc_module {
   bool get_direct_mem_ptr(usize,
                           tlm::tlm_generic_payload& tx,
                           tlm::tlm_dmi& dmi) {
-    u64 start = tx.get_address();
-    u64 end = start + tx.get_data_length() - 1;
-
-    bool ret = false;
-    if (auto res = decode(range{start, end})) {
-      assert(res.base <= start);
+    if (const auto r = decode(tx)) {
+      const auto start = tx.get_address();
+      assert(r.start <= start);
 
-      tx.set_address(start - res.base);
-      ret = (*res.sock)->get_direct_mem_ptr(tx, dmi);
+      tx.set_address(start - r.start);
+      const bool ok = (*r.sock)->get_direct_mem_ptr(tx, dmi);
       tx.set_address(start);
+
+      // Early return, dmi request failed, no need to fixup dmi addresses.
+      if (!ok) {
+        return false;
+      }
+
+      // Compute absolute start address.
+      const sc_dt::uint64 abs_start = r.start + dmi.get_start_address();
+      assert(abs_start < r.end);
+
+      // Compute absolute end address. Limit if mapping range exceeded.
+      const auto comp_abs_end = [&r](sc_dt::uint64 abs_end) -> sc_dt::uint64 {
+        if (abs_end > r.end /* exceeds mapping */ ||
+            abs_end < r.start /* wraps around */) {
+          return r.end;
+        }
+        return abs_end;
+      };
+      const sc_dt::uint64 abs_end =
+          comp_abs_end(r.start + dmi.get_end_address());
+
+      dmi.set_start_address(abs_start);
+      dmi.set_end_address(abs_end);
+
+      return true;
     }
-    return ret;
+    return false;
   }
 
   unsigned int transport_dbg(usize, tlm::tlm_generic_payload& tx) {
-    u64 start = tx.get_address();
-    u64 end = start + tx.get_data_length() - 1;
-
     unsigned int ret = 0;
-    if (auto res = decode(range{start, end})) {
-      assert(res.base <= start);
+    if (const auto r = decode(tx)) {
+      const auto start = tx.get_address();
+      assert(r.start <= start);
 
-      tx.set_address(start - res.base);
-      ret = (*res.sock)->transport_dbg(tx);
+      tx.set_address(start - r.start);
+      ret = (*r.sock)->transport_dbg(tx);
       tx.set_address(start);
     }
     return ret;
@@ -163,7 +196,8 @@ class lt_bus : public sc_core::sc_module {
 
   struct decode_result {
     initiator_socket* sock{nullptr};
-    u64 base;
+    u64 start{0ull};
+    u64 end{0ull};
 
     constexpr explicit operator bool() const {
       return sock != nullptr;
@@ -173,10 +207,16 @@ class lt_bus : public sc_core::sc_module {
   decode_result decode(range addr) const {
     for (const auto& map : m_mappings) {
       if (map.addr.contains(addr)) {
-        return {m_targets[map.idx].get(), map.addr.start};
+        return {m_targets[map.idx].get(), map.addr.start, map.addr.end};
       }
     }
-    return {nullptr, 0ull};
+    return {};
+  }
+
+  decode_result decode(const tlm::tlm_generic_payload& tx) const {
+    const u64 start = tx.get_address();
+    const u64 end = start + tx.get_data_length() - 1;
+    return decode(range{start, end});
   }
 
   // -- SC_MODULE CALLBACKS ----------------------------------------------------
-- 
cgit v1.2.3