From b1282f61c10f120e0a6d338e9aad1c1fd69ef4a6 Mon Sep 17 00:00:00 2001
From: Michael Nolan <mtnolan2640@gmail.com>
Date: Sat, 16 May 2020 14:36:39 -0400
Subject: [PATCH] Implement mfcr and mfocrf

---
 src/soc/cr/main_stage.py            | 31 ++++++++++++++++++++---------
 src/soc/cr/test/test_pipe_caller.py | 22 ++++++++++++++++++++
 src/soc/decoder/isa/sprset.patch    | 13 ++++++++++--
 3 files changed, 55 insertions(+), 11 deletions(-)

diff --git a/src/soc/cr/main_stage.py b/src/soc/cr/main_stage.py
index 97120123..5ba25be6 100644
--- a/src/soc/cr/main_stage.py
+++ b/src/soc/cr/main_stage.py
@@ -70,6 +70,16 @@ class CRMainStage(PipeModBase):
         move_one = Signal(reset_less=True)
         comb += move_one.eq(self.i.ctx.op.insn[20])
 
+        # Generate the mask for mtcrf, mtocrf, and mfocrf
+        fxm = Signal(xfx_fields['FXM'][0:-1].shape())
+        comb += fxm.eq(xfx_fields['FXM'][0:-1])
+
+        mask = Signal(32, reset_less=True)
+
+        # replicate every fxm field in the insn to 4-bit, as a mask
+        for i in range(8):
+            comb += mask[i*4:(i+1)*4].eq(Repl(fxm[i], 4))
+
         with m.Switch(op.insn_type):
             ##### mcrf #####
             with m.Case(InternalOp.OP_MCRF):
@@ -123,17 +133,20 @@ class CRMainStage(PipeModBase):
 
             ##### mtcrf #####
             with m.Case(InternalOp.OP_MTCRF):
-                fxm = Signal(xfx_fields['FXM'][0:-1].shape())
-                comb += fxm.eq(xfx_fields['FXM'][0:-1])
-
-                # replicate every fxm field in the insn to 4-bit, as a mask
-                fxl = [Repl(fxm[i], 4) for i in range(8)]
-                mask = Signal(32, reset_less=True)
-                comb += mask.eq(Cat(*fxl))
-
+                # mtocrf and mtcrf are essentially identical
                 # put input (RA) - mask-selected - into output CR, leave
                 # rest of CR alone.
-                comb += cr_o.eq((self.i.a[0:32] & mask) | (self.i.cr & ~mask))
+                comb += cr_o.eq((self.i.a[0:32] & mask) |
+                                     (self.i.cr & ~mask))
+            with m.Case(InternalOp.OP_MFCR):
+                # mfocrf
+                with m.If(move_one):
+                    comb += self.o.o.eq(self.i.cr & mask)
+                # mfcrf
+                with m.Else():
+                    comb += self.o.o.eq(self.i.cr)
+                    
+                    
 
         comb += self.o.cr.eq(cr_o)
         comb += self.o.ctx.eq(self.i.ctx)
diff --git a/src/soc/cr/test/test_pipe_caller.py b/src/soc/cr/test/test_pipe_caller.py
index ef90ddca..fa08fb66 100644
--- a/src/soc/cr/test/test_pipe_caller.py
+++ b/src/soc/cr/test/test_pipe_caller.py
@@ -102,6 +102,20 @@ class CRTestCase(FHDLTestCase):
             self.run_tst_program(Program(lst), initial_regs=initial_regs,
                                  initial_cr=cr)
 
+    def test_mfcr(self):
+        for i in range(5):
+            lst = ["mfcr 2"]
+            cr = random.randint(0, (1<<32)-1)
+            self.run_tst_program(Program(lst), initial_cr=cr)
+
+    def test_mfocrf(self):
+        for i in range(20):
+            mask = 1<<random.randint(0, 7)
+            lst = [f"mfocrf 2, {mask}"]
+            cr = random.randint(0, (1<<32)-1)
+            self.run_tst_program(Program(lst), initial_cr=cr)
+        
+
     def test_ilang(self):
         rec = CompALUOpSubset()
 
@@ -189,6 +203,14 @@ class TestRunner(FHDLTestCase):
                         msg += " code: %s" % code
                         self.assertEqual(cr_expected, cr_real, msg)
 
+                    reg_out = yield pdecode2.e.write_reg.ok
+                    if reg_out:
+                        reg_sel = yield pdecode2.e.write_reg.data
+                        reg_data = simulator.gpr(reg_sel).value
+                        output = yield alu.n.data_o.o
+                        msg = f"real: {reg_data:x}, actual: {output:x}"
+                        self.assertEqual(reg_data, output)
+
         sim.add_sync_process(process)
         with sim.write_vcd("simulator.vcd", "simulator.gtkw",
                             traces=[]):
diff --git a/src/soc/decoder/isa/sprset.patch b/src/soc/decoder/isa/sprset.patch
index 4f1abe71..afc0d7f2 100644
--- a/src/soc/decoder/isa/sprset.patch
+++ b/src/soc/decoder/isa/sprset.patch
@@ -1,5 +1,5 @@
---- sprset.py.orig	2020-05-16 14:04:00.414533305 -0400
-+++ sprset.py	2020-05-16 14:04:05.611261221 -0400
+--- sprset.py.orig	2020-05-16 14:04:43.548374778 -0400
++++ sprset.py	2020-05-16 14:34:05.369303775 -0400
 @@ -54,7 +54,7 @@
                  n = i
                  count = count + 1
@@ -9,3 +9,12 @@
          else:
              CR = undefined
          return (CR,)
+@@ -78,7 +78,7 @@
+                 count = count + 1
+         if eq(count, 1):
+             RT = concat(0, repeat=64)
+-        RT[4 * n + 32:4 * n + 35 + 1] = CR[4 * n + 32:4 * n + 35 + 1]
++        RT[4 * n + 32:4 * n + 35 + 1] = CR.si[4 * n + 32:4 * n + 35 + 1]
+         return (RT, CR,)
+ 
+     @inject()
-- 
2.30.2