From dfa033eb872bc4112f5f1f401c9f33926184db3c Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Fri, 24 Jan 2020 10:31:52 +0800
Subject: [PATCH] wrpll: new collector from Weida/Tom

---
 artiq/gateware/drtio/wrpll/core.py  |  5 ++++-
 artiq/gateware/drtio/wrpll/ddmtd.py | 30 +++++++++++++++++++++--------
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/artiq/gateware/drtio/wrpll/core.py b/artiq/gateware/drtio/wrpll/core.py
index a8d374ca8..793219043 100644
--- a/artiq/gateware/drtio/wrpll/core.py
+++ b/artiq/gateware/drtio/wrpll/core.py
@@ -79,6 +79,9 @@ class WRPLL(Module, AutoCSR):
         self.submodules.ddmtd_helper = DDMTD(ddmtd_counter, ddmtd_inputs.rec_clk)
         self.submodules.ddmtd_main = DDMTD(ddmtd_counter, ddmtd_inputs.main_xo)
 
+        collector_update = Signal()
+        self.sync.helper += collector_update.eq(ddmtd_counter == (2**N - 1))
+
         filter_cd = ClockDomainsRenamer("filter")
         self.submodules.collector = filter_cd(Collector(N))
         self.submodules.filter_helper = filter_cd(thls.make(filters.helper, data_width=48))
@@ -98,7 +101,7 @@ class WRPLL(Module, AutoCSR):
         ]
         self.comb += [
             self.filter_main.input.eq(self.collector.output),
-            self.filter_main.input_stb.eq(self.collector.output_update)
+            self.filter_main.input_stb.eq(collector_update)
         ]
 
         self.sync.helper += [
diff --git a/artiq/gateware/drtio/wrpll/ddmtd.py b/artiq/gateware/drtio/wrpll/ddmtd.py
index 4b4f30324..58002779d 100644
--- a/artiq/gateware/drtio/wrpll/ddmtd.py
+++ b/artiq/gateware/drtio/wrpll/ddmtd.py
@@ -131,20 +131,19 @@ class DDMTD(Module, AutoCSR):
 
 class Collector(Module):
     def __init__(self, N):
-        self.tag_helper = Signal(N)
+        self.tag_helper = Signal((N, True))
         self.tag_helper_update = Signal()
-        self.tag_main = Signal(N)
+        self.tag_main = Signal((N, True))
         self.tag_main_update = Signal()
 
-        self.output = Signal((N, True))
-        self.output_update = Signal(N)
+        self.output = Signal((N + 1, True))
 
         # # #
 
         fsm = FSM(reset_state="IDLE")
         self.submodules += fsm
 
-        tag_collector = Signal(N)
+        tag_collector = Signal((N + 1, True))
         fsm.act("IDLE",
             If(self.tag_main_update & self.tag_helper_update,
                 NextValue(tag_collector, 0),
@@ -160,17 +159,32 @@ class Collector(Module):
         fsm.act("WAITHELPER",
             If(self.tag_helper_update,
                 NextValue(tag_collector, tag_collector - self.tag_helper),
-                NextState("UPDATE")
+                NextState("LEADCHECK")
             )
         )
         fsm.act("WAITMAIN",
             If(self.tag_main_update,
                 NextValue(tag_collector, tag_collector + self.tag_main),
-                NextState("UPDATE")
+                NextState("LAGCHECK")
             )
         )
+        # To compensate DDMTD counter roll-over when main is ahead of roll-over
+        # and helper is after roll-over
+        fsm.act("LEADCHECK",
+            If(tag_collector > 0,
+                NextValue(tag_collector, tag_collector - (2**N - 1))
+            ),
+            NextState("UPDATE")
+        )
+        # To compensate DDMTD counter roll-over when helper is ahead of roll-over
+        # and main is after roll-over
+        fsm.act("LAGCHECK",
+            If(tag_collector < 0,
+                NextValue(tag_collector, tag_collector + (2**N - 1))
+            ),
+            NextState("UPDATE")
+        )
         fsm.act("UPDATE",
             NextValue(self.output, tag_collector),
             NextState("IDLE")
         )
-        self.sync += self.output_update.eq(self.tag_helper_update)