From bfd901ddc452a25f1307ed48c189b738b0661d61 Mon Sep 17 00:00:00 2001
From: morgan <mc@m-labs.hk>
Date: Fri, 8 Nov 2024 17:36:53 +0800
Subject: [PATCH] sim: add proto frame extractor

---
 sim_pipeline.py | 105 +++++++++++++++++++++++++++++++++++++++++++++---
 sim_stream.py   |   4 +-
 2 files changed, 102 insertions(+), 7 deletions(-)

diff --git a/sim_pipeline.py b/sim_pipeline.py
index 931dd3b..ad9a7ed 100644
--- a/sim_pipeline.py
+++ b/sim_pipeline.py
@@ -263,14 +263,108 @@ class Stream_Parser(Module):
             ),
         )
 
-class Frame_Decoder(Module):
-    def __init__(self):
-        self.sink = stream.Endpoint(word_layout_dchar)
-        self.source = stream.Endpoint(word_layout_dchar)
+class Frame_Extractor(Module):
+    def __init__(self, pixel_format="mono16"):
+        assert pixel_format in ["mono16"]
+        pixel_format = {
+            "mono16": C(0x0105, 2*char_width)
+        }
+        self.format_error = Signal()
+        self.decode_err = Signal()
+
+        self.new_frame = Signal()
+        self.new_line = Signal()
+
+        n_metadata_chars = 23
+        self.metadata = Record([
+            ("stream_id", char_width),
+            ("source_tag", 2*char_width), 
+            ("x_size", 3*char_width), 
+            ("x_offset", 3*char_width), 
+            ("y_size", 3*char_width), 
+            ("y_offset", 3*char_width), 
+            ("l_size", 3*char_width), # number of data words per image line 
+            ("pixel_format", 2*char_width), 
+            ("tap_geo", 2*char_width), 
+            ("flag", char_width), 
+        ])
+        assert len(self.metadata.raw_bits()) == n_metadata_chars*char_width  
 
         # # #
 
         # TODO: decode Image header, line break
+        self.sink = stream.Endpoint(word_layout_dchar)
+        self.source = stream.Endpoint(word_layout_dchar)
+
+        self.submodules.fsm = fsm = FSM(reset_state="IDLE")
+
+        # DEBUG: remove this
+        self.fsm_state = Signal()
+        self.comb += self.fsm_state.eq(fsm.ongoing("IDLE"))
+
+        fsm.act("IDLE",
+            self.sink.ack.eq(1),
+            If((self.sink.stb & (self.sink.dchar == KCode["stream_marker"]) & (self.sink.dchar_k == 1)),
+                NextState("DECODE"),
+            )
+        )
+
+        fsm.act("COPY",
+            # until for new line or new frame
+            If((self.sink.stb & (self.sink.dchar == KCode["stream_marker"]) & (self.sink.dchar_k == 1)),
+                self.sink.ack.eq(1),
+                NextState("DECODE"),
+            ).Else(
+                self.sink.connect(self.source),
+            )
+        )
+
+        type = {
+            "new_frame": 0x01,
+            "line_break": 0x02,
+        }
+
+        cnt = Signal(max=n_metadata_chars)
+        fsm.act("DECODE",
+            self.sink.ack.eq(1),
+            If(self.sink.stb,
+                Case(self.sink.dchar, {
+                    type["new_frame"]: [
+                        self.new_frame.eq(1),
+                        NextValue(cnt, cnt.reset),
+                        NextState("GET_STREAM_ID"),
+                    ],
+                    type["line_break"]: [
+                        self.new_line.eq(1),
+                        NextState("COPY"),
+                    ],
+                    "default": [
+                         self.decode_err.eq(1),
+                         # discard all data until valid frame
+                         NextState("IDLE"),
+                    ],
+                }),
+            )
+        )
+
+        case = dict(
+            (i, NextValue(self.metadata.raw_bits()[8*i:8*(i+1)], self.sink.dchar))
+            for i in range(n_metadata_chars)
+        )
+        fsm.act("GET_STREAM_ID",
+            self.sink.ack.eq(1),
+            If(self.sink.stb,
+                Case(cnt, case),
+                If(cnt == n_metadata_chars - 1,
+                    NextState("COPY"),
+                    NextValue(cnt, cnt.reset),
+                ).Else(
+                    NextValue(cnt, cnt + 1),
+                ),
+            ),
+        )
+
+
 
 class Pixel_Decoder(Module):
     def __init__(self, pixel_format="mono16"):
@@ -289,8 +383,9 @@ class Stream_Pipeline(Module):
 
         self.submodules.double_buffer = double_buffer = Double_Stream_Buffer(size)
         self.submodules.parser = parser = Stream_Parser()
+        self.submodules.frame_extractor = frame_extractor = Frame_Extractor()
 
-        pipeline = [double_buffer, parser]
+        pipeline = [double_buffer, parser, frame_extractor]
         for s, d in zip(pipeline, pipeline[1:]):
             self.comb += s.source.connect(d.sink)
         self.sink = pipeline[0].sink
diff --git a/sim_stream.py b/sim_stream.py
index 5da2229..326d9cd 100644
--- a/sim_stream.py
+++ b/sim_stream.py
@@ -84,7 +84,7 @@ def check_case(packet=[]):
         print(
             f"\nCYCLE#{i} : source char = {yield source.data:#X} k = {yield source.k:#X} stb = {yield source.stb} ack = {yield source.ack} eop = {yield source.eop}"
             # f" source dchar = {yield source.dchar:#X} dchar_k = {yield source.dchar_k:#X}"
-            f"\nCYCLE#{i} : read mask = {yield dut.crossbar .mux.sel}"
+            f"\nCYCLE#{i} : read mask = {yield dut.crossbar.mux.sel}"
             # f"\nCYCLE#{i} : stream id = {yield decoder.stream_id:#X} pak_tag = {yield decoder.pak_tag:#X}"
             # f" stream_pak_size = {yield decoder.stream_pak_size:#X}"
         )
@@ -125,7 +125,7 @@ def testbench():
         ],
         [
             {"data": 0x22222222, "k": Replicate(0, 4)},
-            {"data": 0xC00010FF, "k": Replicate(0, 4)},
+            {"data": 0xC001BEA0, "k": Replicate(0, 4)},
         ],
         [
             {"data": 0x33333333, "k": Replicate(0, 4)},