From d193092e16aad09f48e8849c3bfd4fae915cea0f Mon Sep 17 00:00:00 2001
From: Florent Kermarrec <florent@enjoy-digital.fr>
Date: Mon, 30 Nov 2020 12:18:59 +0100
Subject: [PATCH] cores/cpu/cv32e40p/core: rewrite OBI2Wishbone to reduce
 write/read latency by 1 cycle.

---
 litex/soc/cores/cpu/cv32e40p/core.py | 64 +++++++++++++++++++---------
 1 file changed, 45 insertions(+), 19 deletions(-)

diff --git a/litex/soc/cores/cpu/cv32e40p/core.py b/litex/soc/cores/cpu/cv32e40p/core.py
index 79908b76f..2073abd93 100644
--- a/litex/soc/cores/cpu/cv32e40p/core.py
+++ b/litex/soc/cores/cpu/cv32e40p/core.py
@@ -78,27 +78,53 @@ def add_manifest_sources(platform, manifest):
 
 class OBI2Wishbone(Module):
     def __init__(self, obi, wb):
-        dat_r_d = Signal().like(wb.dat_r)
-        addr_d  = Signal().like(obi.addr)
-        ack_d   = Signal()
+        addr  = Signal.like(obi.addr)
+        be    = Signal.like(obi.be)
+        we    = Signal.like(obi.we)
+        wdata = Signal.like(obi.wdata)
 
-        self.sync += [
-            dat_r_d.eq(wb.dat_r),
-            ack_d.eq(wb.ack),
-            addr_d.eq(obi.addr),
-        ]
+        self.submodules.fsm = fsm = FSM(reset_state="IDLE")
+        fsm.act("IDLE",
+            # On OBI request:
+            If(obi.req,
+                # Drive Wishbone bus from OBI bus.
+                wb.adr.eq(obi.addr[2:32]),
+                wb.stb.eq(            1),
+                wb.dat_w.eq(  obi.wdata),
+                wb.cyc.eq(            1),
+                wb.sel.eq(       obi.be),
+                wb.we.eq(        obi.we),
 
-        self.comb += [
-            wb.adr.eq(obi.addr[2:32]),
-            wb.stb.eq(obi.req & (~ack_d)),
-            wb.dat_w.eq(obi.wdata),
-            wb.cyc.eq(obi.req),
-            wb.sel.eq(obi.be),
-            wb.we.eq(obi.we),
-            obi.gnt.eq(wb.ack & (addr_d == obi.addr)),
-            obi.rvalid.eq(ack_d),
-            obi.rdata.eq(dat_r_d),
-        ]
+                # Store OBI bus values.
+                NextValue(addr,  obi.addr),
+                NextValue(be,    obi.be),
+                NextValue(we,    obi.we),
+                NextValue(wdata, obi.wdata),
+
+                # Now we need to wait Wishbone Ack.
+                NextState("ACK")
+            ),
+            obi.gnt.eq(1), # Always ack OBI request in Idle.
+        )
+        fsm.act("ACK",
+            # Drive Wishbone bus from stored OBI bus values.
+            wb.adr.eq(addr[2:32]),
+            wb.stb.eq(         1),
+            wb.dat_w.eq(   wdata),
+            wb.cyc.eq(         1),
+            wb.sel.eq(        be),
+            wb.we.eq(         we),
+
+            # On Wishbone Ack:
+            If(wb.ack,
+                # Generate OBI response.
+                obi.rvalid.eq(1),
+                obi.rdata.eq(wb.dat_r),
+
+                # Return to Idle.
+                NextState("IDLE")
+            )
+        )
 
 class Wishbone2OBI(Module):
     def __init__(self, wb, obi):