From c83e31af7ec419323150fdee8dbcb3c0dd12c99c Mon Sep 17 00:00:00 2001
From: morgan <mc@m-labs.hk>
Date: Thu, 19 Sep 2024 14:39:09 +0800
Subject: [PATCH] pipeline GW: add trig ack checker

---
 src/gateware/cxp_pipeline.py | 69 +++++++++++++++++++++++++++++++++++-
 1 file changed, 68 insertions(+), 1 deletion(-)

diff --git a/src/gateware/cxp_pipeline.py b/src/gateware/cxp_pipeline.py
index 09cc05f..d8f7af5 100644
--- a/src/gateware/cxp_pipeline.py
+++ b/src/gateware/cxp_pipeline.py
@@ -13,6 +13,13 @@ downconn_layout = [("data", downconn_dw), ("k", downconn_dw//8)]
 def K(x, y):
     return ((y << 5) | x)
 
+def bytes2word(arr):
+    assert len(arr) == 4
+    sum = 0
+    for i, val in enumerate(arr):
+        sum += (val & 0xFF) << i*8
+    return sum 
+
 class Code_Source(Module):
     def __init__(self, layout, counts=4):
 
@@ -334,7 +341,7 @@ class TX_Test_Packet(Module, AutoCSR):
         self.submodules.pak_wrp = pak_wrp = Packet_Wrapper(upconn_layout)
         self.comb += [
             pak_type_inserter.data.eq(0x04),
-            pak_type_inserter.k.eq(0x04),
+            pak_type_inserter.k.eq(0),
 
             testdata_src.connect(pak_type_inserter.sink),
             pak_type_inserter.source.connect(pak_wrp.sink),
@@ -366,3 +373,63 @@ class RX_Debug_Buffer(Module,AutoCSR):
             self.kout_pak.status.eq(buf_out.source.k),
             self.dout_valid.status.eq(buf_out.source.stb),
         ]
+
+class Receiver_Path(Module, AutoCSR):
+    def __init__(self):
+        self.trig_ack = Signal()
+        self.trig_clr = Signal()
+
+        # TODO:
+        self.packet_type = Signal(8)
+
+    
+class CXP_Data_Packet_Decode(Module):
+    def __init__(self):
+        self.sink = stream.Endpoint(downconn_layout)
+        # This is where data stream comes out
+        self.source = stream.Endpoint(downconn_layout)
+
+        # # #
+
+        self.comb += self.sink.connect(self.source)
+
+
+class CXP_Trig_Ack_Checker(Module, AutoCSR):
+    def __init__(self):
+        self.sink = stream.Endpoint(downconn_layout)
+        self.source = stream.Endpoint(downconn_layout)
+
+        self.ack = Signal()
+
+        # # #
+
+        self.submodules.fsm = fsm = FSM(reset_state="IDLE")
+
+        fsm.act("IDLE",
+            self.sink.ack.eq(1),
+            If(self.sink.stb,
+                self.sink.ack.eq(0),
+                NextState("COPY"),
+            )
+        )
+
+        fsm.act("COPY",
+            If((self.sink.stb & (self.sink.data == bytes2word([K(28, 6)]*4)) & (self.sink.k == 0b1111)),
+                # discard K28,6
+                self.sink.ack.eq(1),
+                NextState("CHECK_ACK")
+            ).Else(
+                self.sink.connect(self.source),
+            )
+        )
+
+        fsm.act("CHECK_ACK",
+            If(self.sink.stb,
+                NextState("IDLE"),
+                # discard the word after K28,6
+                self.sink.ack.eq(1),
+                If(self.sink.data == bytes2word([0x01]*4),
+                    self.ack.eq(1),
+                )
+            )
+        )