From 4315a63c124c6459774354420c0909828d68e84a Mon Sep 17 00:00:00 2001
From: Michael Nolan <mtnolan2640@gmail.com>
Date: Sat, 16 May 2020 13:34:16 -0400
Subject: [PATCH] Implement mtcrf

---
 src/soc/cr/main_stage.py            | 13 +++++++++++++
 src/soc/cr/test/test_pipe_caller.py | 23 +++++++++++++++++++++--
 2 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/src/soc/cr/main_stage.py b/src/soc/cr/main_stage.py
index e954dd8a..21f19b65 100644
--- a/src/soc/cr/main_stage.py
+++ b/src/soc/cr/main_stage.py
@@ -40,6 +40,7 @@ class CRMainStage(PipeModBase):
         comb = m.d.comb
         op = self.i.ctx.op
         xl_fields = self.fields.instrs['XL']
+        xfx_fields = self.fields.instrs['XFX']
 
         cr_output = Signal.like(self.i.cr)
         comb += cr_output.eq(self.i.cr)
@@ -107,6 +108,18 @@ class CRMainStage(PipeModBase):
                 # Set the output to the result above
                 comb += cr_out_arr[bt].eq(bit_out)
 
+            with m.Case(InternalOp.OP_MTCRF):
+                fxm = Signal(xfx_fields['FXM'][0:-1].shape())
+                comb += fxm.eq(xfx_fields['FXM'][0:-1])
+
+                mask = Signal(32, reset_less=True)
+
+                for i in range(8):
+                    comb += mask[i*4:(i+1)*4].eq(Repl(fxm[i], 4))
+                
+                comb += cr_output.eq((self.i.a[0:32] & mask) |
+                                      (self.i.cr & ~mask))
+
 
 
         comb += self.o.cr.eq(cr_output)
diff --git a/src/soc/cr/test/test_pipe_caller.py b/src/soc/cr/test/test_pipe_caller.py
index 10ede467..42133771 100644
--- a/src/soc/cr/test/test_pipe_caller.py
+++ b/src/soc/cr/test/test_pipe_caller.py
@@ -78,12 +78,22 @@ class CRTestCase(FHDLTestCase):
             cr = random.randint(0, 7)
             self.run_tst_program(Program(lst), initial_cr=cr)
 
-
     def test_mcrf(self):
         lst = ["mcrf 0, 5"]
         cr = 0xffff0000
         self.run_tst_program(Program(lst), initial_cr=cr)
 
+    def test_mtcrf(self):
+        for i in range(20):
+            mask = random.randint(0, 255)
+            lst = [f"mtcrf {mask}, 2"]
+            cr = random.randint(0, (1<<32)-1)
+            initial_regs = [0] * 32
+            initial_regs[2] = random.randint(0, (1<<32)-1)
+            self.run_tst_program(Program(lst), initial_regs=initial_regs,
+                                 initial_cr=cr)
+        
+
     def test_ilang(self):
         rec = CompALUOpSubset()
 
@@ -100,6 +110,15 @@ class TestRunner(FHDLTestCase):
         super().__init__("run_all")
         self.test_data = test_data
 
+    def set_inputs(self, alu, dec2, simulator):
+        yield alu.p.data_i.cr.eq(simulator.cr.get_range().value)
+
+        reg3_ok = yield dec2.e.read_reg3.ok
+        if reg3_ok:
+            reg3_sel = yield dec2.e.read_reg3.data
+            reg3 = simulator.gpr(reg3_sel).value
+            yield alu.p.data_i.a.eq(reg3)
+
     def run_all(self):
         m = Module()
         comb = m.d.comb
@@ -140,8 +159,8 @@ class TestRunner(FHDLTestCase):
                     # ask the decoder to decode this binary data (endian'd)
                     yield pdecode2.dec.bigendian.eq(0)  # little / big?
                     yield instruction.eq(ins)          # raw binary instr.
-                    yield alu.p.data_i.cr.eq(simulator.cr.get_range().value)
                     yield Settle()
+                    yield from self.set_inputs(alu, pdecode2, simulator)
                     fn_unit = yield pdecode2.e.fn_unit
                     self.assertEqual(fn_unit, Function.CR.value, code)
                     yield 
-- 
2.30.2