WRPLL firmware

satman main & si549: add WRPLL select_recovered_clock
satman main & si549: add helper si549 & interrupt setup
si549: add tag collector to process gtx & main tags
si549: add frequency counter to set BASE_ADPLL
si549: add set_adpll for main & helper PLL
si549: add main & helper PLL
IRQ & si549: add handler for gtx & main tags irq
IRQ: add docs for IRQ id

src/gateware/ddmtd.py

@@ -0,0 +1,72 @@
+from migen import *
+from migen.genlib.cdc import PulseSynchronizer, MultiReg
+from migen.genlib.fsm import FSM
+from misoc.interconnect.csr import *
+
+
+class DDMTDSamplerGTX(Module):
+    def __init__(self, gtx, main_xo_pads):
+        self.gtx_beating = Signal()
+        self.main_beating = Signal()
+
+        # # #
+
+        main_clk_se = Signal()
+        gtx_beating_FF = Signal()
+        main_beating_FF = Signal()
+        self.specials += [
+            Instance("IBUFDS",
+                     i_I=main_xo_pads.p, i_IB=main_xo_pads.n,
+                     o_O=main_clk_se),
+            # Two back to back FFs are used to prevent metastability
+            Instance("FD", i_C=ClockSignal("helper"),
+                     i_D=gtx.cd_rtio_rx0.clk, o_Q=gtx_beating_FF),
+            Instance("FD", i_C=ClockSignal("helper"),
+                     i_D=gtx_beating_FF, o_Q=self.gtx_beating),
+            Instance("FD", i_C=ClockSignal("helper"),
+                     i_D=main_clk_se, o_Q=main_beating_FF,),
+            Instance("FD", i_C=ClockSignal("helper"),
+                     i_D=main_beating_FF, o_Q=self.main_beating,)
+        ]
+
+
+class DDMTDDeglitcherFirstEdge(Module):
+    def __init__(self, input_signal, blind_period=200):
+        self.detect = Signal()
+        rising = Signal()
+        input_signal_r = Signal()
+
+        # # #
+
+        self.sync.helper += [
+            input_signal_r.eq(input_signal),
+            rising.eq(input_signal & ~input_signal_r)
+        ]
+
+        blind_counter = Signal(max=blind_period)
+        self.sync.helper += [
+            If(blind_counter != 0, blind_counter.eq(blind_counter - 1)),
+            If(input_signal_r, blind_counter.eq(blind_period - 1)),
+            self.detect.eq(rising & (blind_counter == 0))
+        ]
+
+
+class DDMTD(Module):
+    def __init__(self, counter, input_signal):
+
+        # in helper clock domain
+        self.h_tag = Signal(len(counter))
+        self.h_tag_update = Signal()
+
+        # # #
+
+        deglitcher = DDMTDDeglitcherFirstEdge(input_signal)
+        self.submodules += deglitcher
+
+        self.sync.helper += [
+            self.h_tag_update.eq(0),
+            If(deglitcher.detect,
+                self.h_tag_update.eq(1),
+                self.h_tag.eq(counter)
+               )
+        ]

src/gateware/kasli_soc.py

@@ -26,6 +26,8 @@ import analyzer
 import acpki
 import drtio_aux_controller
 import zynq_clocking
+import wrpll
+import si549
 from config import write_csr_file, write_mem_file, write_rustc_cfg_file
 
 eem_iostandard_dict = {
@@ -105,7 +107,7 @@ class GTPBootstrapClock(Module):
 
 
 class GenericStandalone(SoCCore):
-    def __init__(self, description, acpki=False):
+    def __init__(self, description, acpki=False, with_wrpll=False):
         self.acpki = acpki
         clk_freq = description["rtio_frequency"]
 
@@ -205,7 +207,7 @@ class GenericStandalone(SoCCore):
 
 
 class GenericMaster(SoCCore):
-    def __init__(self, description, acpki=False):
+    def __init__(self, description, acpki=False, with_wrpll=False):
         clk_freq = description["rtio_frequency"]
 
         has_drtio_over_eem = any(peripheral["type"] == "shuttler" for peripheral in description["peripherals"])
@@ -398,7 +400,7 @@ class GenericMaster(SoCCore):
 
 
 class GenericSatellite(SoCCore):
-    def __init__(self, description, acpki=False):
+    def __init__(self, description, acpki=False, with_wrpll=False):
         clk_freq = description["rtio_frequency"]
 
         self.acpki = acpki
@@ -551,14 +553,30 @@ class GenericSatellite(SoCCore):
         self.config["RTIO_FREQUENCY"] = str(clk_freq/1e6)
         self.config["CLOCK_FREQUENCY"] = int(clk_freq)
 
-        self.submodules.siphaser = SiPhaser7Series(
-            si5324_clkin=platform.request("cdr_clk"),
-            rx_synchronizer=self.rx_synchronizer,
-            ultrascale=False,
-            rtio_clk_freq=self.gt_drtio.rtio_clk_freq)
-        self.csr_devices.append("siphaser")
-        self.config["HAS_SI5324"] = None
-        self.config["SI5324_SOFT_RESET"] = None
+        if with_wrpll:
+            self.submodules.main_dcxo = si549.Si549(platform.request("ddmtd_main_dcxo_i2c"))
+            self.submodules.helper_dcxo = si549.Si549(platform.request("ddmtd_helper_dcxo_i2c"))
+            self.submodules.wrpll = wrpll.WRPLL(
+                gtx=self.gt_drtio,
+                main_dcxo_pads=platform.request("cdr_clk_clean_fabric"),
+                helper_dcxo_pads=platform.request("ddmtd_helper_clk"))
+            self.csr_devices.append("main_dcxo")
+            self.csr_devices.append("helper_dcxo")
+            self.csr_devices.append("wrpll")
+            self.comb += [
+                self.ps7.interrupt[0].eq(self.wrpll.gtx_tag_ev.irq), # IRQ_ID = 61 
+                self.ps7.interrupt[1].eq(self.wrpll.main_tag_ev.irq) # IRQ_ID = 62 
+            ]
+            self.config["HAS_SI549"] = None
+        else:
+            self.submodules.siphaser = SiPhaser7Series(
+                si5324_clkin=platform.request("cdr_clk"),
+                rx_synchronizer=self.rx_synchronizer,
+                ultrascale=False,
+                rtio_clk_freq=self.gt_drtio.rtio_clk_freq)
+            self.csr_devices.append("siphaser")
+            self.config["HAS_SI5324"] = None
+            self.config["SI5324_SOFT_RESET"] = None
 
         gtx0 = self.gt_drtio.gtxs[0]
         platform.add_false_path_constraints(
@@ -588,6 +606,8 @@ def main():
         help="build gateware into the specified directory")
     parser.add_argument("--acpki", default=False, action="store_true",
         help="enable ACPKI")
+    parser.add_argument("--with-wrpll", default=False, action="store_true",
+        help="enable WRPLL")
     parser.add_argument("description", metavar="DESCRIPTION",
                         help="JSON system description file")
     args = parser.parse_args()
@@ -605,7 +625,7 @@ def main():
     else:
         raise ValueError("Invalid DRTIO role")
 
-    soc = cls(description, acpki=args.acpki)
+    soc = cls(description, acpki=args.acpki, with_wrpll=args.with_wrpll)
     soc.finalize()
 
     if args.r is not None:

src/gateware/si549.py

@@ -0,0 +1,309 @@
+from migen import *
+from migen.genlib.fsm import *
+from migen.genlib.cdc import MultiReg, PulseSynchronizer, BlindTransfer
+
+from misoc.interconnect.csr import *
+
+
+class I2CClockGen(Module):
+    def __init__(self, width):
+        self.load = Signal(width)
+        self.clk2x = Signal()
+
+        cnt = Signal.like(self.load)
+        self.comb += [
+            self.clk2x.eq(cnt == 0),
+        ]
+        self.sync += [
+            If(self.clk2x,
+                cnt.eq(self.load),
+               ).Else(
+                cnt.eq(cnt - 1),
+            )
+        ]
+
+
+class I2CMasterMachine(Module):
+    def __init__(self, clock_width):
+        self.scl = Signal(reset=1)
+        self.sda_o = Signal(reset=1)
+        self.sda_i = Signal()
+
+        self.submodules.cg = CEInserter()(I2CClockGen(clock_width))
+        self.start = Signal()
+        self.stop = Signal()
+        self.write = Signal()
+        self.ack = Signal()
+        self.data = Signal(8)
+        self.ready = Signal()
+
+        ###
+
+        bits = Signal(4)
+        data = Signal(8)
+
+        fsm = CEInserter()(FSM("IDLE"))
+        self.submodules += fsm
+
+        fsm.act("IDLE",
+                self.ready.eq(1),
+                If(self.start,
+                   NextState("START0"),
+                   ).Elif(self.stop,
+                          NextState("STOP0"),
+                          ).Elif(self.write,
+                                 NextValue(bits, 8),
+                                 NextValue(data, self.data),
+                                 NextState("WRITE0")
+                                 )
+                )
+
+        fsm.act("START0",
+                NextValue(self.scl, 1),
+                NextState("START1")
+                )
+        fsm.act("START1",
+                NextValue(self.sda_o, 0),
+                NextState("IDLE")
+                )
+
+        fsm.act("STOP0",
+                NextValue(self.scl, 0),
+                NextState("STOP1")
+                )
+        fsm.act("STOP1",
+                NextValue(self.sda_o, 0),
+                NextState("STOP2")
+                )
+        fsm.act("STOP2",
+                NextValue(self.scl, 1),
+                NextState("STOP3")
+                )
+        fsm.act("STOP3",
+                NextValue(self.sda_o, 1),
+                NextState("IDLE")
+                )
+
+        fsm.act("WRITE0",
+                NextValue(self.scl, 0),
+                NextState("WRITE1")
+                )
+        fsm.act("WRITE1",
+                If(bits == 0,
+                   NextValue(self.sda_o, 1),
+                   NextState("READACK0"),
+                   ).Else(
+                    NextValue(self.sda_o, data[7]),
+                    NextState("WRITE2"),
+                )
+                )
+        fsm.act("WRITE2",
+                NextValue(self.scl, 1),
+                NextValue(data[1:], data[:-1]),
+                NextValue(bits, bits - 1),
+                NextState("WRITE0"),
+                )
+        fsm.act("READACK0",
+                NextValue(self.scl, 1),
+                NextState("READACK1"),
+                )
+        fsm.act("READACK1",
+                NextValue(self.ack, ~self.sda_i),
+                NextState("IDLE")
+                )
+
+        run = Signal()
+        idle = Signal()
+        self.comb += [
+            run.eq((self.start | self.stop | self.write) & self.ready),
+            idle.eq(~run & fsm.ongoing("IDLE")),
+            self.cg.ce.eq(~idle),
+            fsm.ce.eq(run | self.cg.clk2x),
+        ]
+
+
+class ADPLLProgrammer(Module):
+    def __init__(self):
+        self.i2c_divider = Signal(16)
+        self.i2c_address = Signal(7)
+
+        self.adpll = Signal(24)
+        self.stb = Signal()
+        self.busy = Signal()
+        self.nack = Signal()
+
+        self.scl = Signal()
+        self.sda_i = Signal()
+        self.sda_o = Signal()
+
+        self.scl.attr.add("no_retiming")
+        self.sda_o.attr.add("no_retiming")
+
+        # # #
+
+        master = I2CMasterMachine(16)
+        self.submodules += master
+
+        self.comb += [
+            master.cg.load.eq(self.i2c_divider),
+            self.scl.eq(master.scl),
+            master.sda_i.eq(self.sda_i),
+            self.sda_o.eq(master.sda_o)
+        ]
+
+        fsm = FSM()
+        self.submodules += fsm
+
+        adpll = Signal.like(self.adpll)
+
+        fsm.act("IDLE",
+                If(self.stb,
+                   NextValue(adpll, self.adpll),
+                   NextState("START")
+                   )
+                )
+        fsm.act("START",
+                master.start.eq(1),
+                If(master.ready, NextState("DEVADDRESS"))
+                )
+        fsm.act("DEVADDRESS",
+                master.data.eq(self.i2c_address << 1),
+                master.write.eq(1),
+                If(master.ready, NextState("REGADRESS"))
+                )
+        fsm.act("REGADRESS",
+                master.data.eq(231),
+                master.write.eq(1),
+                If(master.ready,
+                   If(master.ack,
+                      NextState("DATA0")
+                      ).Else(
+                       self.nack.eq(1),
+                       NextState("STOP")
+                   )
+                   )
+                )
+        fsm.act("DATA0",
+                master.data.eq(adpll[0:8]),
+                master.write.eq(1),
+                If(master.ready,
+                   If(master.ack,
+                      NextState("DATA1")
+                      ).Else(
+                       self.nack.eq(1),
+                       NextState("STOP")
+                   )
+                   )
+                )
+        fsm.act("DATA1",
+                master.data.eq(adpll[8:16]),
+                master.write.eq(1),
+                If(master.ready,
+                   If(master.ack,
+                      NextState("DATA2")
+                      ).Else(
+                       self.nack.eq(1),
+                       NextState("STOP")
+                   )
+                   )
+                )
+        fsm.act("DATA2",
+                master.data.eq(adpll[16:24]),
+                master.write.eq(1),
+                If(master.ready,
+                   If(~master.ack, self.nack.eq(1)),
+                   NextState("STOP")
+                   )
+                )
+        fsm.act("STOP",
+                master.stop.eq(1),
+                If(master.ready,
+                   If(~master.ack, self.nack.eq(1)),
+                   NextState("IDLE")
+                   )
+                )
+
+        self.comb += self.busy.eq(~fsm.ongoing("IDLE"))
+
+
+class Si549(Module, AutoCSR):
+    def __init__(self, pads):
+        # switching sys clk will reset all csr values
+        self.bitbang_enable = CSRStorage()
+        self.gpio_in = CSRStatus(2)
+        self.gpio_out = CSRStorage(2)
+        self.gpio_oe = CSRStorage(2)
+
+        self.i2c_divider = CSRStorage(16, reset=75)
+        self.i2c_address = CSRStorage(7)
+        self.nack = CSRStatus()
+
+        self.adpll = CSRStorage(24)
+        self.adpll_stb = CSRStorage()
+        self.adpll_busy = CSRStatus()
+
+        # # #
+
+        self.submodules.programmer = ClockDomainsRenamer("helper")(ADPLLProgrammer())
+
+        self.i2c_divider.storage.attr.add("no_retiming")
+        self.i2c_address.storage.attr.add("no_retiming")
+        self.adpll.storage.attr.add("no_retiming")
+        self.adpll_stb.storage.attr.add("no_retiming")
+
+        self.specials += [
+            MultiReg(self.i2c_divider.storage, self.programmer.i2c_divider, "helper"),
+            MultiReg(self.i2c_address.storage, self.programmer.i2c_address, "helper"),
+            MultiReg(self.adpll.storage, self.programmer.adpll, "helper"),
+            MultiReg(self.adpll_stb.storage, self.programmer.stb, "helper"),
+            MultiReg(self.programmer.busy, self.adpll_busy.status)
+        ]
+
+        nack_cdc = PulseSynchronizer("helper", "sys")
+        self.submodules += nack_cdc
+        self.comb += nack_cdc.i.eq(self.programmer.nack)
+        self.sync += [
+            If(self.bitbang_enable.storage, self.nack.status.eq(0)),
+            If(nack_cdc.o, self.nack.status.eq(1))
+        ]
+
+        self.bitbang_enable.storage.attr.add("no_retiming")
+        self.gpio_out.storage.attr.add("no_retiming")
+        self.gpio_oe.storage.attr.add("no_retiming")
+
+        # SCL GPIO and mux
+        ts_scl = TSTriple(1)
+        self.specials += ts_scl.get_tristate(pads.scl)
+
+        status = Signal()
+        self.comb += self.gpio_in.status[0].eq(status)
+
+        self.specials += MultiReg(ts_scl.i, status)
+        self.comb += [
+            If(self.bitbang_enable.storage,
+                ts_scl.o.eq(self.gpio_out.storage[0]),
+                ts_scl.oe.eq(self.gpio_oe.storage[0])
+                ).Else(
+                ts_scl.o.eq(0),
+                ts_scl.oe.eq(~self.programmer.scl)
+            )
+        ]
+
+        # SDA GPIO and mux
+        ts_sda = TSTriple(1)
+        self.specials += ts_sda.get_tristate(pads.sda)
+
+        status = Signal()
+        self.comb += self.gpio_in.status[1].eq(status)
+
+        self.specials += MultiReg(ts_sda.i, status)
+        self.comb += [
+            If(self.bitbang_enable.storage,
+                ts_sda.o.eq(self.gpio_out.storage[1]),
+                ts_sda.oe.eq(self.gpio_oe.storage[1])
+               ).Else(
+                ts_sda.o.eq(0),
+                ts_sda.oe.eq(~self.programmer.sda_o)
+            )
+        ]
+        self.specials += MultiReg(ts_sda.i, self.programmer.sda_i, "helper")

src/gateware/wrpll.py

@@ -0,0 +1,126 @@
+from migen import *
+from migen.genlib.cdc import MultiReg, AsyncResetSynchronizer, PulseSynchronizer
+from misoc.interconnect.csr import *
+from misoc.interconnect.csr_eventmanager import *
+
+from ddmtd import DDMTDSamplerGTX, DDMTD
+
+
+class FrequencyCounter(Module, AutoCSR):
+    def __init__(self, counter_width=24, domains=["gtx0_rtio_rx", "sys", "helper"]):
+        for domain in domains:
+            name = "counter_" + domain
+            counter = CSRStatus(counter_width, name=name)
+            setattr(self, name, counter)
+        self.update_en = CSRStorage()
+
+        timer = Signal(counter_width)
+        timer_tick = Signal()
+        self.sync += Cat(timer, timer_tick).eq(timer + 1)
+
+        for domain in domains:
+            sync_domain = getattr(self.sync, domain)
+            divider = Signal(2)
+            sync_domain += divider.eq(divider + 1)
+
+            divided = Signal()
+            divided.attr.add("no_retiming")
+            sync_domain += divided.eq(divider[-1])
+            divided_sys = Signal()
+            self.specials += MultiReg(divided, divided_sys)
+
+            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):
+    def __init__(self, gtx, main_dcxo_pads, helper_dcxo_pads, COUNTER_BIT=32):
+
+        self.gtx_tag = CSRStatus(COUNTER_BIT)
+        self.main_tag = CSRStatus(COUNTER_BIT)
+
+        ddmtd_counter = Signal(COUNTER_BIT)
+
+        gtx_tag_sys = Signal(COUNTER_BIT)
+        main_tag_sys = Signal(COUNTER_BIT)
+        gtx_tag_stb_sys = Signal()
+        main_tag_stb_sys = Signal()
+
+        # # #
+
+        self.helper_reset = CSRStorage(reset=1)
+        self.clock_domains.cd_helper = ClockDomain()
+        self.helper_reset.storage.attr.add("no_retiming")
+        self.specials += [
+            Instance("IBUFGDS",
+                     i_I=helper_dcxo_pads.p, i_IB=helper_dcxo_pads.n,
+                     o_O=self.cd_helper.clk),
+            AsyncResetSynchronizer(self.cd_helper, self.helper_reset.storage)
+        ]
+
+        self.submodules.frequency_counter = FrequencyCounter()
+
+        self.submodules.ddmtd_sampler = DDMTDSamplerGTX(gtx, main_dcxo_pads)
+
+        self.sync.helper += ddmtd_counter.eq(ddmtd_counter + 1)
+        self.submodules.ddmtd_gtx = DDMTD(ddmtd_counter, self.ddmtd_sampler.gtx_beating)
+        self.submodules.ddmtd_main = DDMTD(ddmtd_counter, self.ddmtd_sampler.main_beating)
+
+        # DDMTD tags collection
+
+        self.specials += [
+            MultiReg(self.ddmtd_gtx.h_tag, gtx_tag_sys),
+            MultiReg(self.ddmtd_main.h_tag, main_tag_sys)
+        ]
+
+        gtx_tag_stb_ps = PulseSynchronizer("helper", "sys")
+        main_tag_stb_ps = PulseSynchronizer("helper", "sys")
+        self.submodules += [
+            gtx_tag_stb_ps,
+            main_tag_stb_ps
+        ]
+        self.sync.helper += [
+            gtx_tag_stb_ps.i.eq(self.ddmtd_gtx.h_tag_update),
+            main_tag_stb_ps.i.eq(self.ddmtd_main.h_tag_update)
+        ]
+        self.sync += [
+            gtx_tag_stb_sys.eq(gtx_tag_stb_ps.o),
+            main_tag_stb_sys.eq(main_tag_stb_ps.o)
+        ]
+
+        self.sync += [
+            If(gtx_tag_stb_sys,
+                self.gtx_tag.status.eq(gtx_tag_sys),
+            ),
+            If(main_tag_stb_sys,
+                self.main_tag.status.eq(main_tag_sys)
+            )
+        ]
+
+        # PL-PS shared peripheral interrupt (SPI)
+
+        self.submodules.gtx_tag_ev = EventManager()
+        self.gtx_tag_ev.stb = EventSourcePulse()
+        self.gtx_tag_ev.finalize()
+
+        self.submodules.main_tag_ev = EventManager()
+        self.main_tag_ev.stb = EventSourcePulse()
+        self.main_tag_ev.finalize()
+
+        self.sync += [
+            self.gtx_tag_ev.stb.trigger.eq(gtx_tag_stb_sys),
+            self.main_tag_ev.stb.trigger.eq(main_tag_stb_sys)
+        ]

src/libboard_artiq/src/io_expander.rs

@@ -19,11 +19,15 @@ const IODIR_OUT_SFP_LED: u8 = 0x40;
 const IODIR_OUT_SFP0_LED: u8 = 0x40;
 #[cfg(hw_rev = "v1.1")]
 const IODIR_OUT_SFP0_LED: u8 = 0x80;
+#[cfg(has_si549)]
+const IODIR_CLK_SEL: u8 = 0x80; // out
+#[cfg(has_si5324)]
+const IODIR_CLK_SEL: u8 = 0x00; // in
 
 //IO expander port direction
 const IODIR0: [u8; 2] = [
     0xFF & !IODIR_OUT_SFP_TX_DISABLE & !IODIR_OUT_SFP0_LED,
-    0xFF & !IODIR_OUT_SFP_TX_DISABLE & !IODIR_OUT_SFP_LED,
+    0xFF & !IODIR_OUT_SFP_TX_DISABLE & !IODIR_OUT_SFP_LED & !IODIR_CLK_SEL,
 ];
 
 const IODIR1: [u8; 2] = [

src/libboard_artiq/src/lib.rs

@@ -35,7 +35,8 @@ pub mod drtio_eem;
 pub mod grabber;
 #[cfg(has_si5324)]
 pub mod si5324;
-
+#[cfg(has_si549)]
+pub mod si549;
 use core::{cmp, str};
 
 pub fn identifier_read(buf: &mut [u8]) -> &str {

src/libboard_artiq/src/si549.rs

@@ -0,0 +1,606 @@
+use core::result::Result::Ok;
+
+use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
+use libboard_zynq::timer::GlobalTimer;
+use log::info;
+
+use crate::pl::csr;
+
+#[cfg(feature = "target_kasli_soc")]
+const ADDRESS: u8 = 0x67;
+
+mod i2c {
+    use super::*;
+
+    #[derive(Clone, Copy)]
+    pub enum DCXO {
+        Main,
+        #[cfg(has_wrpll)]
+        Helper,
+    }
+
+    fn half_period(timer: &mut GlobalTimer) {
+        timer.delay_us(1)
+    }
+    const SDA_MASK: u8 = 2;
+    const SCL_MASK: u8 = 1;
+
+    fn sda_i(dcxo: DCXO) -> bool {
+        let reg = match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_in_read() },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_in_read() },
+        };
+        reg & SDA_MASK != 0
+    }
+
+    fn sda_oe(dcxo: DCXO, oe: bool) {
+        let reg = match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_read() },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_read() },
+        };
+        let reg = if oe { reg | SDA_MASK } else { reg & !SDA_MASK };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_write(reg) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_write(reg) },
+        }
+    }
+
+    fn sda_o(dcxo: DCXO, o: bool) {
+        let reg = match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_out_read() },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_read() },
+        };
+        let reg = if o { reg | SDA_MASK } else { reg & !SDA_MASK };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_out_write(reg) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_write(reg) },
+        }
+    }
+
+    fn scl_oe(dcxo: DCXO, oe: bool) {
+        let reg = match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_read() },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_read() },
+        };
+        let reg = if oe { reg | SCL_MASK } else { reg & !SCL_MASK };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_write(reg) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_write(reg) },
+        }
+    }
+
+    fn scl_o(dcxo: DCXO, o: bool) {
+        let reg = match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_out_read() },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_read() },
+        };
+        let reg = if o { reg | SCL_MASK } else { reg & !SCL_MASK };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::gpio_out_write(reg) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_write(reg) },
+        }
+    }
+
+    pub fn init(dcxo: DCXO, timer: &mut GlobalTimer) -> Result<(), &'static str> {
+        // Set SCL as output, and high level
+        scl_o(dcxo, true);
+        scl_oe(dcxo, true);
+        // Prepare a zero level on SDA so that sda_oe pulls it down
+        sda_o(dcxo, false);
+        // Release SDA
+        sda_oe(dcxo, false);
+
+        // Check the I2C bus is ready
+        half_period(timer);
+        half_period(timer);
+        if !sda_i(dcxo) {
+            // Try toggling SCL a few times
+            for _bit in 0..8 {
+                scl_o(dcxo, false);
+                half_period(timer);
+                scl_o(dcxo, true);
+                half_period(timer);
+            }
+        }
+
+        if !sda_i(dcxo) {
+            return Err("SDA is stuck low and doesn't get unstuck");
+        }
+        Ok(())
+    }
+
+    pub fn start(dcxo: DCXO, timer: &mut GlobalTimer) {
+        // Set SCL high then SDA low
+        scl_o(dcxo, true);
+        half_period(timer);
+        sda_oe(dcxo, true);
+        half_period(timer);
+    }
+
+    pub fn stop(dcxo: DCXO, timer: &mut GlobalTimer) {
+        // First, make sure SCL is low, so that the target releases the SDA line
+        scl_o(dcxo, false);
+        half_period(timer);
+        // Set SCL high then SDA high
+        sda_oe(dcxo, true);
+        scl_o(dcxo, true);
+        half_period(timer);
+        sda_oe(dcxo, false);
+        half_period(timer);
+    }
+
+    pub fn write(dcxo: DCXO, data: u8, timer: &mut GlobalTimer) -> bool {
+        // MSB first
+        for bit in (0..8).rev() {
+            // Set SCL low and set our bit on SDA
+            scl_o(dcxo, false);
+            sda_oe(dcxo, data & (1 << bit) == 0);
+            half_period(timer);
+            // Set SCL high ; data is shifted on the rising edge of SCL
+            scl_o(dcxo, true);
+            half_period(timer);
+        }
+        // Check ack
+        // Set SCL low, then release SDA so that the I2C target can respond
+        scl_o(dcxo, false);
+        half_period(timer);
+        sda_oe(dcxo, false);
+        // Set SCL high and check for ack
+        scl_o(dcxo, true);
+        half_period(timer);
+        // returns true if acked (I2C target pulled SDA low)
+        !sda_i(dcxo)
+    }
+
+    pub fn read(dcxo: DCXO, ack: bool, timer: &mut GlobalTimer) -> u8 {
+        // Set SCL low first, otherwise setting SDA as input may cause a transition
+        // on SDA with SCL high which will be interpreted as START/STOP condition.
+        scl_o(dcxo, false);
+        half_period(timer); // make sure SCL has settled low
+        sda_oe(dcxo, false);
+
+        let mut data: u8 = 0;
+
+        // MSB first
+        for bit in (0..8).rev() {
+            scl_o(dcxo, false);
+            half_period(timer);
+            // Set SCL high and shift data
+            scl_o(dcxo, true);
+            half_period(timer);
+            if sda_i(dcxo) {
+                data |= 1 << bit
+            }
+        }
+        // Send ack
+        // Set SCL low and pull SDA low when acking
+        scl_o(dcxo, false);
+        if ack {
+            sda_oe(dcxo, true)
+        }
+        half_period(timer);
+        // then set SCL high
+        scl_o(dcxo, true);
+        half_period(timer);
+
+        data
+    }
+}
+
+fn write(dcxo: i2c::DCXO, reg: u8, val: u8, timer: &mut GlobalTimer) -> Result<(), &'static str> {
+    i2c::start(dcxo, timer);
+    if !i2c::write(dcxo, ADDRESS << 1, timer) {
+        return Err("Si549 failed to ack write address");
+    }
+    if !i2c::write(dcxo, reg, timer) {
+        return Err("Si549 failed to ack register");
+    }
+    if !i2c::write(dcxo, val, timer) {
+        return Err("Si549 failed to ack value");
+    }
+    i2c::stop(dcxo, timer);
+    Ok(())
+}
+
+fn read(dcxo: i2c::DCXO, reg: u8, timer: &mut GlobalTimer) -> Result<u8, &'static str> {
+    i2c::start(dcxo, timer);
+    if !i2c::write(dcxo, ADDRESS << 1, timer) {
+        return Err("Si549 failed to ack write address");
+    }
+    if !i2c::write(dcxo, reg, timer) {
+        return Err("Si549 failed to ack register");
+    }
+    i2c::stop(dcxo, timer);
+
+    i2c::start(dcxo, timer);
+    if !i2c::write(dcxo, (ADDRESS << 1) | 1, timer) {
+        return Err("Si549 failed to ack read address");
+    }
+    let val = i2c::read(dcxo, false, timer);
+    i2c::stop(dcxo, timer);
+    Ok(val)
+}
+
+fn setup(dcxo: i2c::DCXO, hsdiv: u16, lsdiv: u8, fbdiv: u64, timer: &mut GlobalTimer) -> Result<(), &'static str> {
+    i2c::init(dcxo, timer)?;
+
+    write(dcxo, 255, 0x00, timer)?; // PAGE
+    write(dcxo, 69, 0x00, timer)?; // Disable FCAL override.
+    write(dcxo, 17, 0x00, timer)?; // Synchronously disable output
+
+    // The Si549 has no ID register, so we check that it responds correctly
+    // by writing values to a RAM-like register and reading them back.
+    for test_value in 0..255 {
+        write(dcxo, 23, test_value, timer)?;
+        let readback = read(dcxo, 23, timer)?;
+        if readback != test_value {
+            return Err("Si549 detection failed");
+        }
+    }
+
+    write(dcxo, 23, hsdiv as u8, timer)?;
+    write(dcxo, 24, (hsdiv >> 8) as u8 | (lsdiv << 4), timer)?;
+    write(dcxo, 26, fbdiv as u8, timer)?;
+    write(dcxo, 27, (fbdiv >> 8) as u8, timer)?;
+    write(dcxo, 28, (fbdiv >> 16) as u8, timer)?;
+    write(dcxo, 29, (fbdiv >> 24) as u8, timer)?;
+    write(dcxo, 30, (fbdiv >> 32) as u8, timer)?;
+    write(dcxo, 31, (fbdiv >> 40) as u8, timer)?;
+
+    write(dcxo, 7, 0x08, timer)?; // Start FCAL
+    timer.delay_us(30_000); // Internal FCAL VCO calibration
+    write(dcxo, 17, 0x01, timer)?; // Synchronously enable output
+
+    Ok(())
+}
+
+pub fn main_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> {
+    unsafe {
+        csr::main_dcxo::bitbang_enable_write(1);
+        csr::main_dcxo::i2c_address_write(ADDRESS);
+    }
+
+    #[cfg(rtio_frequency = "125.0")]
+    let (m_hsdiv, m_lsdiv, m_fbdiv) = (0x058, 0, 0x04815791F25);
+
+    setup(i2c::DCXO::Main, m_hsdiv, m_lsdiv, m_fbdiv, timer)?;
+
+    // Si549 maximum settling time for large frequency change.
+    timer.delay_us(40_000);
+
+    unsafe {
+        csr::main_dcxo::bitbang_enable_write(0);
+    }
+
+    info!("Main Si549 started");
+    Ok(())
+}
+
+#[cfg(has_wrpll)]
+pub mod wrpll {
+
+    use libboard_zynq::gic;
+
+    use super::*;
+
+    const IRQ_ID: [u8; 2] = [61, 62];
+
+    const TIMER_WIDTH: u32 = 24;
+    const COUNTER_DIV: u32 = 2;
+
+    const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;
+
+    static mut BASE_ADPLL: i32 = 0;
+    static mut H_INTEGRATOR: i32 = 0;
+    static mut M_INTEGRATOR: i32 = 0;
+
+    #[derive(Clone, Copy)]
+    pub enum IRQ {
+        GTXTag,
+        MainTag,
+    }
+
+    mod tag_collector {
+        use super::IRQ;
+        use crate::pl::csr;
+
+        const BEATING_PERIOD: i32 = 0x4000;
+        const BEATING_HALFPERIOD: i32 = 0x2000;
+
+        // for helper PLL
+        static mut LAST_GTX_TAG: u32 = 0;
+        static mut FIRST_GTX_INTERRUPT: bool = true;
+        static mut PERIOD_DET_READY: bool = false;
+
+        // for main PLL
+        static mut GTX_TAG: u32 = 0;
+        static mut GTX_TAG_READY: bool = false;
+        static mut MAIN_TAG: u32 = 0;
+        static mut MAIN_TAG_READY: bool = false;
+
+        pub fn reset() {
+            clear_period_det_ready();
+            clear_phase_det_ready();
+            unsafe {
+                LAST_GTX_TAG = 0;
+                FIRST_GTX_INTERRUPT = true;
+                GTX_TAG = 0;
+                MAIN_TAG = 0;
+            }
+        }
+
+        pub fn clear_period_det_ready() {
+            unsafe {
+                PERIOD_DET_READY = false;
+            }
+        }
+
+        pub fn clear_phase_det_ready() {
+            unsafe {
+                GTX_TAG_READY = false;
+                MAIN_TAG_READY = false;
+            }
+        }
+
+        pub fn collect_tags(interrupt: IRQ) {
+            match interrupt {
+                IRQ::GTXTag => unsafe {
+                    if !FIRST_GTX_INTERRUPT {
+                        LAST_GTX_TAG = GTX_TAG;
+                        PERIOD_DET_READY = true;
+                    }
+
+                    GTX_TAG = csr::wrpll::gtx_tag_read();
+                    FIRST_GTX_INTERRUPT = false;
+                    GTX_TAG_READY = true;
+                },
+                IRQ::MainTag => unsafe {
+                    MAIN_TAG = csr::wrpll::main_tag_read();
+                    MAIN_TAG_READY = true;
+                },
+            }
+        }
+
+        pub fn period_det_ready() -> bool {
+            unsafe { PERIOD_DET_READY }
+        }
+
+        pub fn phase_det_ready() -> bool {
+            unsafe { GTX_TAG_READY && MAIN_TAG_READY }
+        }
+
+        pub fn get_period_error() -> i32 {
+            // when gtx tag roll over, overflowing_sub prevent the difference to overflow and still get the correct period
+            unsafe { BEATING_PERIOD - (GTX_TAG.overflowing_sub(LAST_GTX_TAG).0) as i32 }
+        }
+
+        pub fn get_phase_error() -> i32 {
+            let mut phase_error: i32;
+
+            unsafe {
+                // unwrap tag difference
+                phase_error = MAIN_TAG.overflowing_sub(GTX_TAG).0.rem_euclid(BEATING_PERIOD as u32) as i32;
+            }
+
+            // mapping tags from [0, 2π] -> [-π, π]
+            if phase_error > BEATING_HALFPERIOD {
+                phase_error -= BEATING_PERIOD
+            }
+            phase_error
+        }
+    }
+
+    pub fn helper_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> {
+        unsafe {
+            csr::wrpll::helper_reset_write(1);
+            csr::helper_dcxo::bitbang_enable_write(1);
+            csr::helper_dcxo::i2c_address_write(ADDRESS);
+        }
+
+        #[cfg(rtio_frequency = "125.0")]
+        let (h_hsdiv, h_lsdiv, h_fbdiv) = (0x058, 0, 0x0481458C94D); // 125Mhz*16383/16384
+
+        setup(i2c::DCXO::Helper, h_hsdiv, h_lsdiv, h_fbdiv, timer)?;
+
+        // Si549 maximum settling time for large frequency change.
+        timer.delay_us(40_000);
+
+        unsafe {
+            csr::wrpll::helper_reset_write(0);
+            csr::helper_dcxo::bitbang_enable_write(0);
+        }
+        info!("Helper Si549 started");
+        Ok(())
+    }
+
+    pub fn interrupt_setup(interrupt_controller: &mut gic::InterruptController) {
+        for id in IRQ_ID.iter() {
+            // setup shared peripheral interrupts (SPI)
+            interrupt_controller.enable(
+                gic::InterruptId(*id),
+                gic::CPUCore::Core0,
+                gic::InterruptSensitivity::Edge,
+                0,
+            );
+        }
+    }
+
+    fn set_irq(en: bool) {
+        let val = if en { 1 } else { 0 };
+        unsafe {
+            csr::wrpll::gtx_tag_ev_enable_write(val);
+            csr::wrpll::main_tag_ev_enable_write(val);
+        }
+    }
+
+    /// set adpll using gateware i2c
+    /// Note: disable main/helper i2c bitbang before using this function
+    fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> {
+        if adpll.abs() > ADPLL_MAX {
+            return Err("adpll is too large");
+        }
+
+        match dcxo {
+            i2c::DCXO::Main => unsafe {
+                if csr::main_dcxo::bitbang_enable_read() == 1 {
+                    return Err("Main si549 bitbang mode is active when using gateware i2c");
+                }
+
+                while csr::main_dcxo::adpll_busy_read() == 1 {}
+                csr::main_dcxo::i2c_address_write(ADDRESS);
+                csr::main_dcxo::adpll_write(adpll as u32);
+
+                csr::main_dcxo::adpll_stb_write(1);
+                csr::main_dcxo::adpll_stb_write(0);
+
+                if csr::main_dcxo::nack_read() == 1 {
+                    return Err("Main si549 failed to ack adpll write");
+                }
+            },
+            #[cfg(has_wrpll)]
+            i2c::DCXO::Helper => unsafe {
+                if csr::helper_dcxo::bitbang_enable_read() == 1 {
+                    return Err("Helper si549 bitbang mode is active when using gateware i2c");
+                }
+
+                while csr::helper_dcxo::adpll_busy_read() == 1 {}
+                csr::helper_dcxo::i2c_address_write(ADDRESS);
+                csr::helper_dcxo::adpll_write(adpll as u32);
+
+                csr::helper_dcxo::adpll_stb_write(1);
+                csr::helper_dcxo::adpll_stb_write(0);
+
+                if csr::helper_dcxo::nack_read() == 1 {
+                    return Err("Helper si549 failed to ack adpll write");
+                }
+            },
+        };
+
+        Ok(())
+    }
+
+    fn set_base_adpll(timer: &mut GlobalTimer) -> Result<(), &'static str> {
+        let count2adpll =
+            |error: i32| (((error) as f64 * 1e6) / (0.0001164 * (1 << (TIMER_WIDTH - COUNTER_DIV)) as f64)) as i32;
+
+        let (gtx_count, main_count, _helper_count) = get_freq_counts(timer);
+        unsafe {
+            BASE_ADPLL = count2adpll(gtx_count as i32 - main_count as i32);
+            set_adpll(i2c::DCXO::Main, BASE_ADPLL)?;
+            set_adpll(i2c::DCXO::Helper, BASE_ADPLL)?;
+        }
+
+        Ok(())
+    }
+
+    fn get_freq_counts(timer: &mut GlobalTimer) -> (u32, u32, u32) {
+        unsafe {
+            csr::wrpll::frequency_counter_update_en_write(1);
+            timer.delay_us(150_000); // 8ns << TIMER_WIDTH
+            csr::wrpll::frequency_counter_update_en_write(0);
+            let gtx = csr::wrpll::frequency_counter_counter_gtx0_rtio_rx_read();
+            let main = csr::wrpll::frequency_counter_counter_sys_read();
+            let helper = csr::wrpll::frequency_counter_counter_helper_read();
+
+            (gtx, main, helper)
+        }
+    }
+
+    fn reset_plls() -> Result<(), &'static str> {
+        unsafe {
+            H_INTEGRATOR = 0;
+            M_INTEGRATOR = 0;
+        }
+        set_adpll(i2c::DCXO::Main, 0)?;
+        set_adpll(i2c::DCXO::Helper, 0)?;
+        Ok(())
+    }
+
+    fn clear_pending(interrupt: IRQ) {
+        match interrupt {
+            IRQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) },
+            IRQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
+        }
+    }
+
+    pub fn interrupt_handler(interrupt: IRQ) {
+        tag_collector::collect_tags(interrupt);
+
+        if tag_collector::period_det_ready() {
+            helper_pll().expect("failed to run helper DCXO PLL");
+            tag_collector::clear_period_det_ready();
+        }
+
+        if tag_collector::phase_det_ready() {
+            main_pll().expect("failed to run main DCXO PLL");
+            tag_collector::clear_phase_det_ready();
+        }
+
+        clear_pending(interrupt);
+    }
+
+    pub fn helper_pll() -> Result<(), &'static str> {
+        let period_err = tag_collector::get_period_error();
+
+        const H_KP: i32 = 2;
+        const H_KI: f32 = 0.5;
+
+        let mut h_adpll: i32;
+        unsafe {
+            H_INTEGRATOR += (period_err as f32 * H_KI) as i32;
+            h_adpll = BASE_ADPLL + period_err * H_KP + H_INTEGRATOR;
+        }
+
+        h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
+        set_adpll(i2c::DCXO::Helper, h_adpll)?;
+
+        Ok(())
+    }
+
+    pub fn main_pll() -> Result<(), &'static str> {
+        let phase_err = tag_collector::get_phase_error();
+
+        const M_KP: i32 = 12;
+        const M_KI: i32 = 2;
+
+        let mut m_adpll: i32;
+        unsafe {
+            M_INTEGRATOR += phase_err * M_KI;
+            m_adpll = BASE_ADPLL + phase_err * M_KP + M_INTEGRATOR;
+        }
+
+        m_adpll = m_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
+        set_adpll(i2c::DCXO::Main, m_adpll)?;
+
+        Ok(())
+    }
+
+    pub fn select_recovered_clock(rc: bool, timer: &mut GlobalTimer) {
+        set_irq(false);
+
+        if rc {
+            tag_collector::reset();
+            reset_plls().expect("failed to reset main and helper PLL");
+
+            info!("warming up GTX CDR...");
+            // gtx need a couple seconds for freq counter to read it properly
+            timer.delay_us(20_000_000);
+            set_base_adpll(timer).expect("failed to set base adpll");
+
+            // clear gateware pending flag
+            clear_pending(IRQ::GTXTag);
+            clear_pending(IRQ::MainTag);
+            set_irq(true);
+            info!("WRPLL interrupt enabled");
+        }
+    }
+}

src/libksupport/src/irq.rs

@@ -1,5 +1,7 @@
 use core::sync::atomic::{AtomicBool, Ordering};
 
+#[cfg(has_wrpll)]
+use libboard_artiq::si549;
 use libboard_zynq::{gic, mpcore, println, stdio};
 use libcortex_a9::{asm, interrupt_handler, notify_spin_lock, regs::MPIDR, spin_lock_yield};
 use libregister::RegisterR;
@@ -12,16 +14,37 @@ extern "C" {
 static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
 
 interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
-    if MPIDR.read().cpu_id() == 1 {
-        let mpcore = mpcore::RegisterBlock::mpcore();
-        let mut gic = gic::InterruptController::gic(mpcore);
-        let id = gic.get_interrupt_id();
-        if id.0 == 0 {
-            gic.end_interrupt(id);
-            asm::exit_irq();
-            asm!("b core1_restart");
+    let mpcore = mpcore::RegisterBlock::mpcore();
+    let mut gic = gic::InterruptController::gic(mpcore);
+    let id = gic.get_interrupt_id();
+    // IRQ ID information at https://docs.xilinx.com/r/en-US/ug585-zynq-7000-SoC-TRM/Software-Generated-Interrupts-SGI?tocId=D777grsNOem_mnEV7glhfg
+
+    match MPIDR.read().cpu_id() {
+        0 => match id.0 {
+            61 => {
+                #[cfg(has_wrpll)]
+                si549::wrpll::interrupt_handler(si549::wrpll::IRQ::GTXTag);
+                gic.end_interrupt(id);
+                return;
+            }
+            62 => {
+                #[cfg(has_wrpll)]
+                si549::wrpll::interrupt_handler(si549::wrpll::IRQ::MainTag);
+                gic.end_interrupt(id);
+                return;
+            }
+            _ => {}
+        },
+        1 => {
+            if id.0 == 0 {
+                gic.end_interrupt(id);
+                asm::exit_irq();
+                asm!("b core1_restart");
+            }
         }
-    }
+        _ => {}
+    };
+
     stdio::drop_uart();
     println!("IRQ");
     loop {}

src/satman/src/main.rs

@@ -29,12 +29,16 @@ use libboard_artiq::grabber;
 use libboard_artiq::io_expander;
 #[cfg(has_si5324)]
 use libboard_artiq::si5324;
+#[cfg(has_si549)]
+use libboard_artiq::si549;
 use libboard_artiq::{drtio_routing, drtioaux,
                      drtioaux_proto::{MASTER_PAYLOAD_MAX_SIZE, SAT_PAYLOAD_MAX_SIZE},
                      identifier_read, logger,
                      pl::csr};
 #[cfg(feature = "target_kasli_soc")]
 use libboard_zynq::error_led::ErrorLED;
+#[cfg(has_wrpll)]
+use libboard_zynq::{gic::InterruptController, mpcore::RegisterBlock};
 use libboard_zynq::{i2c::I2c, print, println, time::Milliseconds, timer::GlobalTimer};
 use libcortex_a9::{l2c::enable_l2_cache, regs::MPIDR};
 use libregister::RegisterR;
@@ -737,6 +741,9 @@ pub extern "C" fn main_core0() -> i32 {
 
     let mut timer = GlobalTimer::start();
 
+    #[cfg(has_wrpll)]
+    let mut interrupt_controller = InterruptController::gic(RegisterBlock::mpcore());
+
     let buffer_logger = unsafe { logger::BufferLogger::new(&mut LOG_BUFFER[..]) };
     buffer_logger.set_uart_log_level(log::LevelFilter::Info);
     buffer_logger.register();
@@ -762,6 +769,11 @@ pub extern "C" fn main_core0() -> i32 {
         io_expander1
             .init(&mut i2c)
             .expect("I2C I/O expander #1 initialization failed");
+
+        // Drive CLK_SEL to true
+        #[cfg(has_si549)]
+        io_expander0.set(1, 7, true);
+
         // Drive TX_DISABLE to false on SFP0..3
         io_expander0.set(0, 1, false);
         io_expander1.set(0, 1, false);
@@ -773,6 +785,8 @@ pub extern "C" fn main_core0() -> i32 {
 
     #[cfg(has_si5324)]
     si5324::setup(&mut i2c, &SI5324_SETTINGS, si5324::Input::Ckin1, &mut timer).expect("cannot initialize Si5324");
+    #[cfg(has_si549)]
+    si549::main_setup(&mut timer).expect("cannot initialize main Si549");
 
     timer.delay_us(100_000);
     info!("Switching SYS clocks...");
@@ -790,6 +804,11 @@ pub extern "C" fn main_core0() -> i32 {
     unsafe {
         csr::gt_drtio::txenable_write(0xffffffffu32 as _);
     }
+    #[cfg(has_wrpll)]
+    {
+        si549::wrpll::helper_setup(&mut timer).expect("cannot initialize helper Si549");
+        si549::wrpll::interrupt_setup(&mut interrupt_controller);
+    }
 
     #[cfg(has_drtio_routing)]
     let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
@@ -832,6 +851,9 @@ pub extern "C" fn main_core0() -> i32 {
             si5324::siphaser::calibrate_skew(&mut timer).expect("failed to calibrate skew");
         }
 
+        #[cfg(has_wrpll)]
+        si549::wrpll::select_recovered_clock(true, &mut timer);
+
         // Various managers created here, so when link is dropped, all DMA traces
         // are cleared out for a clean slate on subsequent connections,
         // without a manual intervention.
@@ -889,6 +911,8 @@ pub extern "C" fn main_core0() -> i32 {
         info!("uplink is down, switching to local oscillator clock");
         #[cfg(has_siphaser)]
         si5324::siphaser::select_recovered_clock(&mut i2c, false, &mut timer).expect("failed to switch clocks");
+        #[cfg(has_wrpll)]
+        si549::wrpll::select_recovered_clock(false, &mut timer);
     }
 }