From 32d33a7fde62fc6be5cd3cb9c72bd2a5b54c00c5 Mon Sep 17 00:00:00 2001
From: Luke Kenneth Casson Leighton <lkcl@lkcl.net>
Date: Fri, 26 Jun 2020 22:06:32 +0100
Subject: [PATCH] add in LenExpand shift/mask

---
 src/soc/experiment/pi2ls.py | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/soc/experiment/pi2ls.py b/src/soc/experiment/pi2ls.py
index 548ac5e5..8e6b2ccc 100644
--- a/src/soc/experiment/pi2ls.py
+++ b/src/soc/experiment/pi2ls.py
@@ -25,6 +25,7 @@
 from soc.minerva.units.loadstore import LoadStoreUnitInterface
 from soc.experiment.pimem import PortInterface
 from soc.scoreboard.addr_match import LenExpand
+from nmigen.utils import log2_int
 
 from nmigen import Elaboratable, Module, Signal
 
@@ -49,7 +50,7 @@ class Pi2LSUI(Elaboratable):
     def elaborate(self, platform):
         m = Module()
         pi, lsui, addrbits = self.pi, self.lsui, self.addrbits
-        m.submodules.lenexp = lenexp = LenExpand(self.addrbits, 8)
+        m.submodules.lenexp = lenexp = LenExpand(log2_int(self.addrbits), 8)
 
         ld_in_progress = Signal(reset=0)
 
@@ -59,17 +60,20 @@ class Pi2LSUI(Elaboratable):
 
         with m.If(pi.addr.ok):
             # expand the LSBs of address plus LD/ST len into 16-bit mask
+            lsbaddr, msbaddr = self.splitaddr(pi.addr.data)
             m.d.comb += lenexp.len_i.eq(pi.data_len)
-            m.d.comb += lenexp.addr_i.eq(pi.addr.data[addrbits]) # LSBs of addr
+            m.d.comb += lenexp.addr_i.eq(lsbaddr) # LSBs of addr
             m.d.comb += lsui.x_mask_i.eq(lenexp.lexp_o)
             # pass through the address, indicate "valid"
-            m.d.comb += lsui.x_addr_i.eq(pi.addr.data) # full address
+            m.d.comb += lsui.x_addr_i.eq(msbaddr) # XXX hmmm...
             m.d.comb += lsui.x_valid_i.eq(1)
             # indicate "OK" - XXX should be checking address valid
             m.d.comb += pi.addr_ok_o.eq(1)
 
         with m.If(pi.is_ld_i):
-            m.d.comb += pi.ld.data.eq(lsui.m_ld_data_o)
+            # shift/mask out the loaded data
+            m.d.comb += pi.ld.data.eq((lsui.m_ld_data_o & lenexp.rexp_o) >>
+                                      (lenexp.addr_i*8))
             # remember we're in the process of loading
             m.d.sync += ld_in_progress.eq(1)
 
@@ -84,6 +88,6 @@ class Pi2LSUI(Elaboratable):
             m.d.comb += pi.ld.ok.eq(0)
 
         with m.If(pi.is_st_i & pi.st.ok):
-            m.d.comb += lsui.x_st_data_i.eq(pi.st.data)
+            m.d.comb += lsui.x_st_data_i.eq(pi.st.data << (lenexp.addr_i*8))
 
         return m
-- 
2.30.2