diff --git a/litex/soc/interconnect/packet.py b/litex/soc/interconnect/packet.py
index c55369717..a9a98130d 100644
--- a/litex/soc/interconnect/packet.py
+++ b/litex/soc/interconnect/packet.py
@@ -156,274 +156,230 @@ class Header:
 
 class Packetizer(Module):
     def __init__(self, sink_description, source_description, header):
-        self.sink = sink = stream.Endpoint(sink_description)
+        self.sink   = sink   = stream.Endpoint(sink_description)
         self.source = source = stream.Endpoint(source_description)
         self.header = Signal(header.length*8)
 
         # # #
 
-        dw = len(self.sink.data)
-        cw = dw // 8
-        header_reg = Signal(header.length*8, reset_less=True)
-        header_words = (header.length*8)//dw
-        header_residue = header.length % cw
-        if header_residue:
-            header_leftover = Signal(header_residue*8)
-        load = Signal()
-        shift = Signal()
-        counter = Signal(max=max(header_words, 2))
-        counter_reset = Signal()
-        counter_ce = Signal()
-        self.sync += \
-            If(counter_reset,
-                counter.eq(0)
-            ).Elif(counter_ce,
-                counter.eq(counter + 1)
-            )
+        # Parameters -------------------------------------------------------------------------------
+        data_width      = len(self.sink.data)
+        bytes_per_clk   = data_width//8
+        header_words    = (header.length*8)//data_width
+        header_leftover = header.length%bytes_per_clk
 
+        # Signals ----------------------------------------------------------------------------------
+        sr       = Signal(header.length*8, reset_less=True)
+        sr_load  = Signal()
+        sr_shift = Signal()
+        count    = Signal(max=max(header_words, 2))
+        sink_d   = stream.Endpoint(sink_description)
+
+        # Header Encode/Load/Shift -----------------------------------------------------------------
         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
-        self.transitioning = transitioning = Signal()  # TODO: Perhaps fsm already has a transitioning signal
-
-        # TODO: What is the recommended way to delay a record, a FIFO?
-        last_buf, valid_buf = Signal(), Signal()
-        self.data_buf = data_buf = Signal(len(sink.data))
-        if header_words == 1 and header_residue == 0:
-            idle_next_state = "COPY"
-        elif header_words == 1 and header_residue:
-            idle_next_state = "STAGGERCOPY"
-        else:
-            idle_next_state = "SEND_HEADER"
+        self.sync += [
+            If(sr_load,
+                sr.eq(self.header)
+            ).Elif(sr_shift,
+                sr.eq(sr[data_width:])
+            )
+        ]
 
+        # FSM --------------------------------------------------------------------------------------
+        self.submodules.fsm = fsm = FSM(reset_state="IDLE")
+        fsm_from_idle = Signal()
         fsm.act("IDLE",
             sink.ready.eq(1),
-            counter_reset.eq(1),
+            NextValue(count, 1),
             If(sink.valid,
-               sink.ready.eq(0),
-               source.valid.eq(1),
-               source.last.eq(0),
-               source.data.eq(self.header[:dw]),
-               If(source.valid & source.ready,
-                  load.eq(1),
-                  NextValue(transitioning, 1),
-                  NextState(idle_next_state)
+                sink.ready.eq(0),
+                source.valid.eq(1),
+                source.last.eq(0),
+                source.data.eq(self.header[:data_width]),
+                If(source.valid & source.ready,
+                    sr_load.eq(1),
+                    NextValue(fsm_from_idle, 1),
+                    If(header_words == 1,
+                        If(header_leftover != 0,
+                            NextState("UNALIGNED-DATA-COPY")
+                        ).Else(
+                            NextState("ALIGNED-DATA-COPY")
+                        )
+                    ).Else(
+                        NextState("HEADER-SEND")
+                    )
                )
             )
         )
-        # if header_residue and header_words >= 2:
-        #     self.sync += [header_leftover.eq(header_reg[2*dw:(2*dw+header_residue*8)]),
-        #     ]
-
-        self.sync += [last_buf.eq(sink.last),
-                      data_buf.eq(sink.data),
-                      valid_buf.eq(sink.valid),
-        ]
-        if header_words != 1:
-            fsm.act("SEND_HEADER",
-                source.valid.eq(1),
-                source.last.eq(0),
-                source.data.eq(header_reg[dw:2*dw]),
-                If(source.valid & source.ready,
-                   shift.eq(1),
-                   counter_ce.eq(1),
-                   If(counter == header_words-2,
-                      shift.eq(0),
-                      counter_ce.eq(1 if header_residue else 0),
-                      NextValue(transitioning, 1),
-                      NextState("STAGGERCOPY" if header_residue else "COPY")
-                   )
+        fsm.act("HEADER-SEND",
+            source.valid.eq(1),
+            source.last.eq(0),
+            source.data.eq(sr[data_width:]),
+            If(source.valid & source.ready,
+                sr_shift.eq(1),
+                If(count == (header_words - 1),
+                    sr_shift.eq(0),
+                    If(header_leftover,
+                        NextState("UNALIGNED-DATA-COPY"),
+                        NextValue(count, count + 1)
+                    ).Else(
+                        NextState("ALIGNED-DATA-COPY")
+                    )
+               ).Else(
+                    NextValue(count, count + 1),
+               )
+            )
+        )
+        fsm.act("ALIGNED-DATA-COPY",
+            source.valid.eq(sink.valid),
+            source.last.eq(sink.last),
+            source.data.eq(sink.data),
+            If(source.valid & source.ready,
+               sink.ready.eq(1),
+               If(source.last,
+                  NextState("IDLE")
+               )
+            )
+        )
+        header_offset_multiplier = 1 if header_words == 1 else 2
+        self.sync += If(source.valid & source.ready, sink_d.eq(sink))
+        fsm.act("UNALIGNED-DATA-COPY",
+            source.valid.eq(sink_d.valid),
+            source.last.eq(sink_d.last),
+            If(fsm_from_idle,
+                source.data[:header_leftover*8].eq(sr[header_offset_multiplier*data_width:])
+            ).Else(
+                source.data[:header_leftover*8].eq(sink_d.data[(bytes_per_clk-header_leftover)*8:])
+            ),
+            source.data[header_leftover*8:].eq(sink.data),
+            If(source.valid & source.ready,
+                sink.ready.eq(1),
+                If(sink.valid, NextValue(fsm_from_idle, 0)),
+                If(source.last,
+                    NextState("IDLE")
                 )
             )
+        )
 
+        # Error ------------------------------------------------------------------------------------
         if hasattr(sink, "error"):
             self.comb += source.error.eq(sink.error)
-        if hasattr(sink, "last_be"):
-            # header_lengh + last_be
-            cw = dw//8
-            rotate_by = header.length % cw
-            x = [sink.last_be[(i + rotate_by) % cw] for i in range(cw)]
-            self.comb += source.last_be.eq(Cat(*x))
-        if header_residue:
-            header_offset_multiplier = hom = 1 if header_words == 1 else 2
-            fsm.act("STAGGERCOPY",
-                    source.valid.eq(valid_buf),
-                    source.last.eq(last_buf),
-                    If(transitioning,
-                       source.data.eq(Cat(header_reg[hom*dw:hom*dw+header_residue*8],
-                                          sink.data[:(cw-header_residue)*8]))
-                    ).Else(
-                       source.data.eq(Cat(data_buf[(cw-header_residue)*8:],
-                                          sink.data[:(cw-header_residue)*8]))
-                    ),
-                    If(source.valid & source.ready,
-                       sink.ready.eq(1),
-                       If(sink.valid,
-                          NextValue(transitioning, 0)
-                       ),
-                       If(source.last,
-                          NextState("IDLE")
-                       )
-                    ),
-            )
-        else:
-            fsm.act("COPY",
-                    source.valid.eq(sink.valid),
-                    source.last.eq(sink.last),
-                    source.data.eq(sink.data),
-                    If(source.valid & source.ready,
-                       NextValue(transitioning, 0),
-                       sink.ready.eq(1),
-                       If(source.last,
-                          NextState("IDLE")
-                       )
-                    ),
-            )
 
+        # Last BE ----------------------------------------------------------------------------------
+        if hasattr(sink, "last_be"):
+            rotate_by = header.length%bytes_per_clk
+            x = [sink.last_be[(i + rotate_by)%bytes_per_clk] for i in range(bytes_per_clk)]
+            self.comb += source.last_be.eq(Cat(*x))
 
 # Depacketizer -------------------------------------------------------------------------------------
 
 class Depacketizer(Module):
     def __init__(self, sink_description, source_description, header):
-        self.sink = sink = stream.Endpoint(sink_description)
+        self.sink   = sink   = stream.Endpoint(sink_description)
         self.source = source = stream.Endpoint(source_description)
         self.header = Signal(header.length*8)
 
         # # #
 
-        dw = len(sink.data)
+        # Parameters -------------------------------------------------------------------------------
+        data_width      = len(sink.data)
+        bytes_per_clk   = data_width// 8
+        header_words    = (header.length*8)//data_width
+        header_leftover = header.length%bytes_per_clk
 
-        cw = dw // 8
+        # Signals ----------------------------------------------------------------------------------
+        sr                = Signal(header.length*8, reset_less=True)
+        sr_shift          = Signal()
+        sr_shift_leftover = Signal()
+        no_payload        = Signal()
+        count             = Signal(max=max(header_words, 2))
+        sink_d            = stream.Endpoint(sink_description)
 
-        header_reg = Signal(header.length*8, reset_less=True)
-        header_words = (header.length*8)//dw
-        header_residue = header.length % cw
-
-        shift = Signal()
-        counter = Signal(max=max(header_words, 2))
-        counter_reset = Signal()
-        counter_ce = Signal()
-        self.sync += \
-            If(counter_reset,
-                counter.eq(0)
-            ).Elif(counter_ce,
-                counter.eq(counter + 1)
+        # Header Shift/Decode ----------------------------------------------------------------------
+        self.sync += [
+            If((header_words == 1) & (header_leftover == 0),
+                If(sr_shift, sr.eq(sink.data))
+            ).Else(
+                If(sr_shift,          sr.eq(Cat(sr[bytes_per_clk*8:],   sink.data))),
+                If(sr_shift_leftover, sr.eq(Cat(sr[header_leftover*8:], sink.data)))
             )
+        ]
+        self.comb += self.header.eq(sr)
+        self.comb += header.decode(self.header, source)
 
-        if header_words == 1 and header_residue == 0:
-            self.sync += \
-                If(shift,
-                   header_reg.eq(sink.data)
-                )
-        else:
-            self.sync += \
-                If(shift,
-                   header_reg.eq(Cat(header_reg[dw:], sink.data))
-                )
-        self.comb += self.header.eq(header_reg)
-
-        fsm = FSM(reset_state="IDLE")
-        self.submodules += fsm
-        # TODO: Perhaps fsm already has a transitioning signal
-        self.transitioning = transitioning = Signal()
-
-        last_buf, valid_buf = Signal(), Signal()
-        self.data_buf = data_buf = Signal(len(sink.data))
-
-        if header_words == 1 and header_residue == 0:
-            idle_next_state = "COPY"
-        elif header_words == 1 and header_residue:
-            idle_next_state = "STAGGERCOPY"
-        else:
-            idle_next_state = "RECEIVE_HEADER"
-
+        # FSM --------------------------------------------------------------------------------------
+        self.submodules.fsm = fsm = FSM(reset_state="IDLE")
+        fsm_from_idle = Signal()
         fsm.act("IDLE",
             sink.ready.eq(1),
-            counter_reset.eq(1),
+            NextValue(count, 1),
             If(sink.valid,
-               shift.eq(1),
-               NextValue(transitioning, 1),
-               NextState(idle_next_state)
+                sr_shift.eq(1),
+                NextValue(fsm_from_idle, 1),
+                If(header_words == 1,
+                    NextValue(no_payload, sink.last),
+                    If(header_leftover,
+                        NextState("UNALIGNED-DATA-COPY")
+                    ).Else(
+                        NextState("ALIGNED-DATA-COPY")
+                    ),
+                ).Else(
+                    NextState("HEADER-RECEIVE")
+                )
             )
         )
-
-        self.sync += [If(sink.ready, data_buf.eq(sink.data)),
-                      valid_buf.eq(sink.valid),
-        ]
-
-        if header_words != 1:
-            fsm.act("RECEIVE_HEADER",
-                sink.ready.eq(1),
-                If(sink.valid,
-                    counter_ce.eq(1),
-                    shift.eq(1),
-                    If(counter == header_words-2,
-                       counter_ce.eq(1 if header_residue else 0),
-                       NextValue(transitioning, 1),
-                       NextState("STAGGERCOPY" if header_residue else "COPY")
+        fsm.act("HEADER-RECEIVE",
+            sink.ready.eq(1),
+            If(sink.valid,
+                NextValue(count, count + 1),
+                sr_shift.eq(1),
+                If(count == (header_words - 1),
+                    NextValue(no_payload, sink.last),
+                    If(header_leftover,
+                        NextValue(count, count + 1),
+                        NextState("UNALIGNED-DATA-COPY")
+                    ).Else(
+                        NextState("ALIGNED-DATA-COPY")
                     )
                 )
             )
-        no_payload = Signal()
-        self.sync += \
-            If(fsm.before_entering("COPY") | fsm.before_entering("STAGGERCOPY"),
-                no_payload.eq(sink.last)
+        )
+        self.sync += If(sink.valid & sink.ready, sink_d.eq(sink))
+        fsm.act("UNALIGNED-DATA-COPY",
+            source.valid.eq((sink.valid & ~fsm_from_idle) | no_payload),
+            source.last.eq(sink.last | no_payload),
+            sink.ready.eq(source.ready),
+            If(sink.valid & sink.ready,
+                NextValue(fsm_from_idle, 0),
+                If(fsm_from_idle,
+                    sr_shift_leftover.eq(1),
+                ).Else(
+                    source.data.eq(sink_d.data[header_leftover*8:]),
+                    source.data[(bytes_per_clk-header_leftover)*8:].eq(sink.data)
+                ),
+                If(source.last,
+                    NextState("IDLE")
+                )
             )
+        )
+        fsm.act("ALIGNED-DATA-COPY",
+            source.last.eq(sink.last | no_payload),
+            source.data.eq(sink.data),
+            sink.ready.eq(source.ready),
+            source.valid.eq(sink.valid | no_payload),
+            If(source.valid & source.ready,
+               If(source.last,
+                  NextState("IDLE")
+               )
+            )
+        )
 
+        # Error ------------------------------------------------------------------------------------
         if hasattr(sink, "error"):
             self.comb += source.error.eq(sink.error)
+
+        # Last BE ----------------------------------------------------------------------------------
         if hasattr(sink, "last_be"):
-            # header_lengh + last_be
-            cw = dw//8
-            x = [sink.last_be[(i - (cw - header_residue)) % cw] for i in range(cw)]
+            x = [sink.last_be[(i - (bytes_per_clk - header_leftover))%bytes_per_clk]
+                for i in range(bytes_per_clk)]
             self.comb += source.last_be.eq(Cat(*x))
-        self.comb += [
-            header.decode(self.header, source)
-        ]
-        if header_residue:
-            fsm.act("STAGGERCOPY",
-                    source.last.eq(sink.last | no_payload),
-                    sink.ready.eq(source.ready),
-                    source.valid.eq(sink.valid & ~transitioning | no_payload),
-                    If(sink.valid & source.ready,
-                       If(transitioning,
-                          NextValue(header_reg, Cat(header_reg[header_residue*8:],
-                                                    sink.data[:header_residue*8]))
-                       ).Else(
-                           source.data.eq(Cat(data_buf[header_residue*8:],
-                                              sink.data[:header_residue*8])),
-                       ),
-                       NextValue(transitioning, 0)
-                    ),
-                    If(source.valid & source.ready & source.last,
-                       NextState("IDLE")
-                    ),
-            )
-        else:
-             fsm.act("COPY",
-                     source.last.eq(sink.last | no_payload),
-                     source.data.eq(sink.data),
-                     sink.ready.eq(source.ready),
-                     source.valid.eq(sink.valid | no_payload),
-                     If(source.valid & source.ready,
-                        NextValue(transitioning, 0),
-                        If(source.last,
-                           NextState("IDLE")
-                        )
-                     )
-             )