From 0499f83580c6fb65117fb73f4360d0868929e37c Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Sun, 8 Dec 2019 23:46:33 +0800
Subject: [PATCH] wrpll: helper clock sanity check

---
 artiq/firmware/libboard_artiq/wrpll.rs | 11 +++++++++
 artiq/gateware/drtio/wrpll/core.py     | 32 ++++++++++++++++++++++++++
 2 files changed, 43 insertions(+)

diff --git a/artiq/firmware/libboard_artiq/wrpll.rs b/artiq/firmware/libboard_artiq/wrpll.rs
index 84c34b67e..3e554064c 100644
--- a/artiq/firmware/libboard_artiq/wrpll.rs
+++ b/artiq/firmware/libboard_artiq/wrpll.rs
@@ -264,6 +264,14 @@ mod si549 {
     }
 }
 
+fn get_helper_frequency() -> u32 {
+    unsafe { csr::wrpll::helper_frequency_start_write(1); }
+    clock::spin_us(10_000);
+    unsafe { csr::wrpll::helper_frequency_stop_write(1); }
+    clock::spin_us(1);
+    unsafe { csr::wrpll::helper_frequency_counter_read() }
+}
+
 pub fn init() {
     info!("initializing...");
 
@@ -281,6 +289,9 @@ pub fn init() {
 
     clock::spin_us(10_000); // Settling Time after FS Change
     unsafe { csr::wrpll::helper_reset_write(0); }
+    clock::spin_us(1);
+
+    info!("helper clock frequency: {}MHz", get_helper_frequency()/10000);
 
     info!("DDMTD test:");
     for _ in 0..20 {
diff --git a/artiq/gateware/drtio/wrpll/core.py b/artiq/gateware/drtio/wrpll/core.py
index 81527ae58..37467008a 100644
--- a/artiq/gateware/drtio/wrpll/core.py
+++ b/artiq/gateware/drtio/wrpll/core.py
@@ -1,11 +1,41 @@
 from migen import *
 from migen.genlib.resetsync import AsyncResetSynchronizer
+from migen.genlib.cdc import MultiReg, PulseSynchronizer
 from misoc.interconnect.csr import *
 
 from artiq.gateware.drtio.wrpll.si549 import Si549
 from artiq.gateware.drtio.wrpll.ddmtd import DDMTD
 
 
+class FrequencyCounter(Module, AutoCSR):
+    def __init__(self):
+        self.counter = CSRStatus(32)
+        self.start = CSR()
+        self.stop = CSR()
+
+        ps_start = PulseSynchronizer("sys", "helper")
+        ps_stop = PulseSynchronizer("sys", "helper")
+        self.submodules += ps_start, ps_stop
+
+        self.comb += [
+            ps_start.i.eq(self.start.re & self.start.r),
+            ps_stop.i.eq(self.stop.re & self.stop.r)
+        ]
+
+        counter = Signal(32)
+        self.specials += MultiReg(counter, self.counter.status)
+
+        counting = Signal()
+        self.sync.helper += [
+            If(counting, counter.eq(counter + 1)),
+            If(ps_start.o,
+                counter.eq(0),
+                counting.eq(1)
+            ),
+            If(ps_stop.o, counting.eq(0))
+        ]
+
+
 class WRPLL(Module, AutoCSR):
     def __init__(self, helper_clk_pads, main_dcxo_i2c, helper_dxco_i2c, ddmtd_inputs, N=15):
         self.helper_reset = CSRStorage(reset=1)
@@ -21,6 +51,8 @@ class WRPLL(Module, AutoCSR):
         self.submodules.main_dcxo = Si549(main_dcxo_i2c)
         self.submodules.helper_dcxo = Si549(helper_dxco_i2c)
 
+        self.submodules.helper_frequency = FrequencyCounter()
+
         ddmtd_counter = Signal(N)
         self.sync.helper += ddmtd_counter.eq(ddmtd_counter + 1)
         self.submodules.ddmtd_helper = DDMTD(ddmtd_counter, ddmtd_inputs.rec_clk)