Add rtio.sed.lane_distributor

README.md

@@ -11,7 +11,7 @@ Formally verified implementation of the ARTIQ RTIO core in nMigen
 - - [x] `rtio.channel`
 - - [ ] `rtio.sed.core`
 - - [x] `rtio.sed.layouts`
-- - [ ] `rtio.sed.lane_distributor`
+- - [x] `rtio.sed.lane_distributor`
 - - [ ] `rtio.sed.fifos`
 - - [ ] `rtio.sed.gates`
 - - [ ] `rtio.sed.output_driver`

rtio/sed/lane_distributor.py

@@ -31,6 +31,7 @@ class LaneDistributor(Elaboratable):
         self.glbl_fine_ts_width = glbl_fine_ts_width
         self.quash_channels = quash_channels
         self.compensation = compensation
+        self.enable_spread = enable_spread
 
     def elaborate(self, platform):
         m = Module()
@@ -101,14 +102,69 @@ class LaneDistributor(Elaboratable):
         m.d.comb += latency_compensation_rdport.addr.eq(self.cri.chan_sel[:16])
 
         # cycle #2, write
-        # TODO
+        compensation = Signal(Shape(14, True))
+        m.d.comb += compensation.eq(latency_compensation_rdport.data)
+        timestamp_above_min = Signal()
+        timestamp_above_last = Signal()
+        timestamp_above_laneA_min = Signal()
+        timestamp_above_laneB_min = Signal()
+        timestamp_above_lane_min = Signal()
+        force_laneB = Signal()
+        use_laneB = Signal()
+        use_lanen = Signal(range(self.lane_count))
+
+        do_write = Signal()
+        do_underflow = Signal()
+        do_sequence_error = Signal()
+        m.d.comb += timestamp_above_min.eq(min_minus_timestamp - compensation < 0)
+        m.d.comb += timestamp_above_laneA_min.eq(laneAmin_minus_timestamp - compensation < 0)
+        m.d.comb += timestamp_above_laneB_min.eq(laneBmin_minus_timestamp - compensation < 0)
+        m.d.comb += timestamp_above_last.eq(last_minus_timestamp - compensation < 0)
+        with m.If(force_laneB | ~timestamp_above_last):
+            m.d.comb += use_lanen.eq(current_lane_plus_one)
+            m.d.comb += use_laneB.eq(1)
+        with m.Else():
+            m.d.comb += use_lanen.eq(current_lane)
+            m.d.comb += use_laneB.eq(0)
+
+        m.d.comb += timestamp_above_lane_min.eq(Mux(use_laneB, timestamp_above_laneB_min, timestamp_above_laneA_min))
+        with m.If(~quash & (self.cri.cmd == cri.commands["write"])):
+            with m.If(timestamp_above_min):
+                with m.If(timestamp_above_lane_min):
+                    m.d.comb += do_write.eq(1)
+                with m.Else():
+                    m.d.comb += do_sequence_error.eq(1)
+            with m.Else():
+                m.d.comb += do_underflow.eq(1)
+        m.d.comb += Array(lio.we for lio in self.output)[use_lanen].eq(do_write)
+        compensated_timestamp = Signal(64)
+        m.d.comb += compensated_timestamp.eq(self.cri.o_timestamp + (compensation << self.glbl_fine_ts_width))
+        with m.If(do_write):
+            m.d.sync += current_lane.eq(use_lanen)
+            m.d.sync += last_coarse_timestamp.eq(compensated_timestamp[self.glbl_fine_ts_width:])
+            m.d.sync += last_lane_coarse_timestamps[use_lanen].eq(compensated_timestamp[self.glbl_fine_ts_width:])
+            m.d.sync += seqn.eq(seqn + 1)
+        for lio in self.output:
+            m.d.comb += lio.payload.timestamp.eq(compensated_timestamp)
 
         # cycle #3, read status
-        # TODO
+        current_lane_writable = Signal()
+        m.d.comb += current_lane_writable.eq(Array(lio.writable for lio in self.output)[current_lane])
+        m.d.comb += o_status_wait.eq(~current_lane_writable)
+        with m.If(self.cri.cmd == cri.commands["write"]):
+            m.d.sync += o_status_underflow.eq(0)
+        with m.If(do_underflow):
+            m.d.sync += o_status_underflow.eq(1)
+        m.d.sync += self.sequence_error.eq(do_sequence_error)
+        m.d.sync += self.sequence_error_channel.eq(self.cri.chan_sel[:16])
 
         # current lane has been full, spread events by switching to the next.
-        # TODO
+        if self.enable_spread:
+            current_lane_writable_r = Signal(reset=1)
+            m.d.sync += current_lane_writable_r.eq(current_lane_writable)
+            with m.If(~current_lane_writable_r & current_lane_writable):
+                m.d.sync += force_laneB.eq(1)
+            with m.If(do_write):
+                m.d.sync += force_laneB.eq(0)
 
         return m
-
-# LaneDistributor(1, 1, [('channel', 16), ('timestamp', 64)], [], 1).elaborate(None)