From e95b4372c5163bb6f753c92d8445d4af7f117457 Mon Sep 17 00:00:00 2001
From: Michael Nolan <mtnolan2640@gmail.com>
Date: Mon, 23 Mar 2020 16:01:10 -0400
Subject: [PATCH] Implement load and store of bytes, halfwords, and words

---
 src/soc/simulator/internalop_sim.py | 40 ++++++++++++++++++++++++-----
 src/soc/simulator/test_sim.py       | 20 +++++++++++++++
 2 files changed, 54 insertions(+), 6 deletions(-)

diff --git a/src/soc/simulator/internalop_sim.py b/src/soc/simulator/internalop_sim.py
index cf71670d..813dd19c 100644
--- a/src/soc/simulator/internalop_sim.py
+++ b/src/soc/simulator/internalop_sim.py
@@ -11,20 +11,45 @@ class MemorySim:
         self.bytes_per_word = bytes_per_word
         self.word_log2 = math.ceil(math.log2(bytes_per_word))
 
+    def _get_shifter_mask(self, width, remainder):
+        shifter = ((self.bytes_per_word - width) - remainder) * \
+            8  # bits per byte
+        mask = (1 << (width * 8)) - 1
+        return shifter, mask
+
     # TODO: Implement ld/st of lesser width
-    def ld(self, address):
+    def ld(self, address, width=8):
+        remainder = address & (self.bytes_per_word - 1)
         address = address >> self.word_log2
+        assert remainder & (width - 1) == 0, "Unaligned access unsupported!"
         if address in self.mem:
             val = self.mem[address]
         else:
             val = 0
+
+        if width != self.bytes_per_word:
+            shifter, mask = self._get_shifter_mask(width, remainder)
+            val = val & (mask << shifter)
+            val >>= shifter
         print("Read {:x} from addr {:x}".format(val, address))
         return val
 
-    def st(self, address, value):
+    def st(self, address, value, width=8):
+        remainder = address & (self.bytes_per_word - 1)
         address = address >> self.word_log2
+        assert remainder & (width - 1) == 0, "Unaligned access unsupported!"
         print("Writing {:x} to addr {:x}".format(value, address))
-        self.mem[address] = value
+        if width != self.bytes_per_word:
+            if address in self.mem:
+                val = self.mem[address]
+            else:
+                val = 0
+            shifter, mask = self._get_shifter_mask(width, remainder)
+            val &= ~(mask << shifter)
+            val |= value << shifter
+            self.mem[address] = val
+        else:
+            self.mem[address] = value
 
 
 class RegFile:
@@ -60,8 +85,10 @@ class InternalOpSimulator:
             return op1 + op2
         elif internal_op == InternalOp.OP_AND.value:
             return op1 & op2
+        elif internal_op == InternalOp.OP_OR.value:
+            return op1 | op2
         else:
-            assert(False, "Not implemented")
+            assert False, "Not implemented" 
 
     def alu_op(self, pdecode2):
         internal_op = yield pdecode2.dec.op.internal_op
@@ -97,6 +124,7 @@ class InternalOpSimulator:
         
         imm_ok = yield pdecode2.e.imm_data.ok
         r2_ok = yield pdecode2.e.read_reg2.ok
+        width = yield pdecode2.e.data_len
         if imm_ok:
             imm = yield pdecode2.e.imm_data.data
             addr += imm
@@ -106,10 +134,10 @@ class InternalOpSimulator:
         if internal_op == InternalOp.OP_STORE.value:
             val_reg = yield pdecode2.e.read_reg3.data
             val = self.regfile.read_reg(val_reg)
-            self.mem_sim.st(addr, val)
+            self.mem_sim.st(addr, val, width)
         elif internal_op == InternalOp.OP_LOAD.value:
             dest_reg = yield pdecode2.e.write_reg.data
-            val = self.mem_sim.ld(addr)
+            val = self.mem_sim.ld(addr, width)
             self.regfile.write_reg(dest_reg, val)
 
 
diff --git a/src/soc/simulator/test_sim.py b/src/soc/simulator/test_sim.py
index 94a9f39a..210141fd 100644
--- a/src/soc/simulator/test_sim.py
+++ b/src/soc/simulator/test_sim.py
@@ -104,6 +104,26 @@ class DecoderTestCase(FHDLTestCase):
             {1: 0x1234,
              2: 0x5678,
              3: 0x1234})
+    def test_ldst_widths(self):
+        lst = [" lis 1, 0xdead",
+               "ori 1, 1, 0xbeef",
+               "addi 2, 0, 0x1000",
+               "std 1, 0(2)",
+               "lbz 1, 5(2)",
+               "lhz 3, 4(2)",
+               "lwz 4, 4(2)",
+               "ori 5, 0, 0x12",
+               "stb 5, 5(2)",
+               "ld  5, 0(2)"]
+        gen = InstrList(lst)
+        simulator = InternalOpSimulator()
+        self.run_tst(gen, simulator)
+        simulator.regfile.assert_gprs({
+            1: 0xad,
+            3: 0xdead,
+            4: 0xdeadbeef,
+            5: 0xffffffffde12beef})  # checked with qemu
+
 
 if __name__ == "__main__":
     unittest.main()
-- 
2.30.2