From 1cbc468bdaf0d07cc3b44dc3ee70c50df239ef98 Mon Sep 17 00:00:00 2001
From: Florent Kermarrec <florent@enjoy-digital.fr>
Date: Tue, 28 Apr 2015 18:44:05 +0200
Subject: [PATCH] migen/actorlib/packet: add Packetizer and Depacketizer

---
 migen/actorlib/packet.py | 164 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 163 insertions(+), 1 deletion(-)

diff --git a/migen/actorlib/packet.py b/migen/actorlib/packet.py
index 98d2d5f6..6a35cd42 100644
--- a/migen/actorlib/packet.py
+++ b/migen/actorlib/packet.py
@@ -4,7 +4,7 @@ from migen.genlib.record import *
 from migen.flow.actor import *
 from migen.actorlib.fifo import SyncFIFO
 from migen.genlib.fsm import FSM, NextState
-from migen.genlib.misc import reverse_bytes
+from migen.genlib.misc import reverse_bytes, Counter
 
 
 class Status(Module):
@@ -135,6 +135,168 @@ class Header:
         return r
 
 
+class Packetizer(Module):
+    def __init__(self, sink_description, source_description, header):
+        self.sink = sink = Sink(sink_description)
+        self.source = source = Source(source_description)
+        self.header = Signal(header.length*8)
+
+        # # #
+
+        dw = flen(self.sink.data)
+
+        header_reg = Signal(header.length*8)
+        header_words = (header.length*8)//dw
+        load = Signal()
+        shift = Signal()
+        counter = Counter(max=max(header_words, 2))
+        self.submodules += counter
+
+        self.comb += header.encode(sink, self.header)
+        if header_words == 1:
+            self.sync += [
+                If(load,
+                    header_reg.eq(self.header)
+                )
+            ]
+        else:
+            self.sync += [
+                If(load,
+                    header_reg.eq(self.header)
+                ).Elif(shift,
+                    header_reg.eq(Cat(header_reg[dw:], Signal(dw)))
+                )
+            ]
+
+        fsm = FSM(reset_state="IDLE")
+        self.submodules += fsm
+
+        if header_words == 1:
+            idle_next_state = "COPY"
+        else:
+            idle_next_state = "SEND_HEADER"
+
+        fsm.act("IDLE",
+            sink.ack.eq(1),
+            counter.reset.eq(1),
+            If(sink.stb & sink.sop,
+                sink.ack.eq(0),
+                source.stb.eq(1),
+                source.sop.eq(1),
+                source.eop.eq(0),
+                source.data.eq(self.header[:dw]),
+                If(source.stb & source.ack,
+                    load.eq(1),
+                    NextState(idle_next_state)
+                )
+            )
+        )
+        if header_words != 1:
+            fsm.act("SEND_HEADER",
+                source.stb.eq(1),
+                source.sop.eq(0),
+                source.eop.eq(0),
+                source.data.eq(header_reg[dw:2*dw]),
+                If(source.stb & source.ack,
+                    shift.eq(1),
+                    counter.ce.eq(1),
+                    If(counter.value == header_words-2,
+                        NextState("COPY")
+                    )
+                )
+            )
+        fsm.act("COPY",
+            source.stb.eq(sink.stb),
+            source.sop.eq(0),
+            source.eop.eq(sink.eop),
+            source.data.eq(sink.data),
+            source.error.eq(sink.error),
+            If(source.stb & source.ack,
+                sink.ack.eq(1),
+                If(source.eop,
+                    NextState("IDLE")
+                )
+            )
+        )
+
+
+class Depacketizer(Module):
+    def __init__(self, sink_description, source_description, header):
+        self.sink = sink = Sink(sink_description)
+        self.source = source = Source(source_description)
+        self.header = Signal(header.length*8)
+
+        # # #
+
+        dw = flen(sink.data)
+
+        header_words = (header.length*8)//dw
+
+        shift = Signal()
+        counter = Counter(max=max(header_words, 2))
+        self.submodules += counter
+
+        if header_words == 1:
+            self.sync += \
+                If(shift,
+                    self.header.eq(sink.data)
+                )
+        else:
+            self.sync += \
+                If(shift,
+                    self.header.eq(Cat(self.header[dw:], sink.data))
+                )
+
+        fsm = FSM(reset_state="IDLE")
+        self.submodules += fsm
+
+        if header_words == 1:
+            idle_next_state = "COPY"
+        else:
+            idle_next_state = "RECEIVE_HEADER"
+
+        fsm.act("IDLE",
+            sink.ack.eq(1),
+            counter.reset.eq(1),
+            If(sink.stb,
+                shift.eq(1),
+                NextState(idle_next_state)
+            )
+        )
+        if header_words != 1:
+            fsm.act("RECEIVE_HEADER",
+                sink.ack.eq(1),
+                If(sink.stb,
+                    counter.ce.eq(1),
+                    shift.eq(1),
+                    If(counter.value == header_words-2,
+                        NextState("COPY")
+                    )
+                )
+            )
+        no_payload = Signal()
+        self.sync += \
+            If(fsm.before_entering("COPY"),
+                source.sop.eq(1),
+                no_payload.eq(sink.eop)
+            ).Elif(source.stb & source.ack,
+                source.sop.eq(0)
+            )
+        self.comb += [
+            source.eop.eq(sink.eop | no_payload),
+            source.data.eq(sink.data),
+            source.error.eq(sink.error),
+            header.decode(self.header, source)
+        ]
+        fsm.act("COPY",
+            sink.ack.eq(source.ack),
+            source.stb.eq(sink.stb | no_payload),
+            If(source.stb & source.ack & source.eop,
+                NextState("IDLE")
+            )
+        )
+
+
 class Buffer(Module):
     def __init__(self, description, data_depth, cmd_depth=4, almost_full=None):
         self.sink = sink = Sink(description)
-- 
2.30.2