From f57f235dca4cd2796ad9e57ef0930848279d86d0 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 30 Dec 2019 19:47:57 +0800
Subject: [PATCH] wrpll: new frequency meter

As per Mattermost discussion with Tom.
---
 artiq/firmware/libboard_artiq/wrpll.rs | 28 +++++++++----
 artiq/gateware/drtio/wrpll/core.py     | 58 ++++++++++++++++----------
 2 files changed, 56 insertions(+), 30 deletions(-)

diff --git a/artiq/firmware/libboard_artiq/wrpll.rs b/artiq/firmware/libboard_artiq/wrpll.rs
index 2261cb262..8c35c0cb8 100644
--- a/artiq/firmware/libboard_artiq/wrpll.rs
+++ b/artiq/firmware/libboard_artiq/wrpll.rs
@@ -264,12 +264,25 @@ 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() }
+fn get_frequencies() -> (u32, u32, u32) {
+    unsafe {
+        csr::wrpll::frequency_counter_update_en_write(1);
+        clock::spin_us(200_000); // wait for at least one update
+        csr::wrpll::frequency_counter_update_en_write(0);
+        clock::spin_us(1);
+        let helper = csr::wrpll::frequency_counter_counter_helper_read();
+        let main = csr::wrpll::frequency_counter_counter_rtio_read();
+        let cdr = csr::wrpll::frequency_counter_counter_rtio_rx0_read();
+        (helper, main, cdr)
+    }
+}
+
+fn log_frequencies() {
+    let (f_helper, f_main, f_cdr) = get_frequencies();
+    let conv_khz = |f| 4*(f as u64)*(csr::CONFIG_CLOCK_FREQUENCY as u64)/(1000*(1 << 23));
+    info!("helper clock frequency: {}kHz ({})", conv_khz(f_helper), f_helper);
+    info!("main clock frequency: {}kHz ({})", conv_khz(f_main), f_main);
+    info!("CDR clock frequency: {}kHz ({})", conv_khz(f_cdr), f_cdr);
 }
 
 fn get_ddmtd_main_tag() -> u16 {
@@ -309,7 +322,7 @@ pub fn init() {
     unsafe { csr::wrpll::helper_reset_write(0); }
     clock::spin_us(1);
 
-    info!("helper clock frequency: {}MHz", get_helper_frequency()/10000);
+    log_frequencies();
     let mut tags = [0; 10];
     for i in 0..tags.len() {
         tags[i] = get_ddmtd_main_tag();
@@ -319,6 +332,7 @@ pub fn init() {
 
 pub fn select_recovered_clock(rc: bool) {
     info!("select_recovered_clock: {}", rc);
+    log_frequencies();
     if rc {
         let mut tags = [0; 10];
         for i in 0..tags.len() {
diff --git a/artiq/gateware/drtio/wrpll/core.py b/artiq/gateware/drtio/wrpll/core.py
index 35902d752..a37aa1a4b 100644
--- a/artiq/gateware/drtio/wrpll/core.py
+++ b/artiq/gateware/drtio/wrpll/core.py
@@ -9,32 +9,43 @@ from artiq.gateware.drtio.wrpll import thls, filters
 
 
 class FrequencyCounter(Module, AutoCSR):
-    def __init__(self):
-        self.counter = CSRStatus(32)
-        self.start = CSR()
-        self.stop = CSR()
+    def __init__(self, timer_width=23, counter_width=23, domains=["helper", "rtio", "rtio_rx0"]):
+        for domain in domains:
+            name = "counter_" + domain
+            counter = CSRStatus(counter_width, name=name)
+            setattr(self, name, counter)
+        self.update_en = CSRStorage()
 
-        ps_start = PulseSynchronizer("sys", "helper")
-        ps_stop = PulseSynchronizer("sys", "helper")
-        self.submodules += ps_start, ps_stop
+        timer = Signal(timer_width)
+        timer_tick = Signal()
+        self.sync += Cat(timer, timer_tick).eq(timer + 1)
 
-        self.comb += [
-            ps_start.i.eq(self.start.re & self.start.r),
-            ps_stop.i.eq(self.stop.re & self.stop.r)
-        ]
+        for domain in domains:
+            sync_domain = getattr(self.sync, domain)
+            divider = Signal(2)
+            sync_domain += divider.eq(divider + 1)
 
-        counter = Signal(32)
-        self.specials += MultiReg(counter, self.counter.status)
+            divided = Signal()
+            divided.attr.add("no_retiming")
+            sync_domain += divided.eq(divider[-1])
+            divided_sys = Signal()
+            self.specials += MultiReg(divided, divided_sys)
 
-        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))
-        ]
+            divided_sys_r = Signal()
+            divided_tick = Signal()
+            self.sync += divided_sys_r.eq(divided_sys)
+            self.comb += divided_tick.eq(divided_sys & ~divided_sys_r)
+
+            counter = Signal(counter_width)
+            counter_csr = getattr(self, "counter_" + domain)
+            self.sync += [
+                If(timer_tick,
+                    If(self.update_en.storage, counter_csr.status.eq(counter)),
+                    counter.eq(0),
+                ).Else(
+                    If(divided_tick, counter.eq(counter + 1))
+                )
+            ]
 
 
 class WRPLL(Module, AutoCSR):
@@ -52,7 +63,8 @@ class WRPLL(Module, AutoCSR):
         self.submodules.helper_dcxo = Si549(helper_dxco_i2c)
         self.submodules.main_dcxo = Si549(main_dcxo_i2c)
 
-        self.submodules.helper_frequency = FrequencyCounter()  # for diagnostics
+        # for diagnostics and PLL initialization
+        self.submodules.frequency_counter = FrequencyCounter()
 
         ddmtd_counter = Signal(N)
         self.sync.helper += ddmtd_counter.eq(ddmtd_counter + 1)