WRPLL firmware

satman main & si549: add WRPLL select_recovered_clock
satman main & si549: add helper si549 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
FIQ & si549: add handler for gtx & main tags FIQ

flake.lock

@@ -234,11 +234,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1702982463,
-        "narHash": "sha256-jKR3drE2rsTaYGEgIdv5kUo2LOb1JyIb4tJhVuCXTTc=",
+        "lastModified": 1709785588,
+        "narHash": "sha256-2Pik/AP05ZKOrCfsXVZfdRijAWSyya9mrNTXzljFskM=",
         "ref": "refs/heads/master",
-        "rev": "4168eb63a7e846863331ae4e656cfd82a867cca8",
-        "revCount": 636,
+        "rev": "7c58c0cf434f37504c62caa03c2c0d6b863da9bf",
+        "revCount": 641,
         "type": "git",
         "url": "https://git.m-labs.hk/m-labs/zynq-rs"
       },

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_dcxo_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_dcxo_pads.p, i_IB=main_dcxo_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=300):
+        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,27 @@ 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.core.core0.nfiq.eq(self.wrpll.ev.irq)
+            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 +603,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 +622,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,278 @@
+from migen import *
+from migen.genlib.fsm import *
+
+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()
+
+        # # #
+
+        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):
+        self.i2c_divider = CSRStorage(16, reset=75)
+        self.i2c_address = CSRStorage(7)
+
+        self.adpll = CSRStorage(24)
+        self.adpll_stb = CSRStorage()
+        self.adpll_busy = CSRStatus()
+        self.nack = CSRStatus()
+
+        self.bitbang_enable = CSRStorage()
+
+        self.sda_oe = CSRStorage()
+        self.sda_out = CSRStorage()
+        self.sda_in = CSRStatus()
+        self.scl_oe = CSRStorage()
+        self.scl_out = CSRStorage()
+
+        # # #
+
+        self.submodules.programmer = ADPLLProgrammer()
+
+        self.comb += [
+            self.programmer.i2c_divider.eq(self.i2c_divider.storage),
+            self.programmer.i2c_address.eq(self.i2c_address.storage),
+            self.programmer.adpll.eq(self.adpll.storage),
+            self.programmer.stb.eq(self.adpll_stb.storage),
+            self.adpll_busy.status.eq(self.programmer.busy),
+            self.nack.status.eq(self.programmer.nack)
+        ]
+
+        # I2C with bitbang/gateware mode select
+        sda_t = TSTriple(1)
+        scl_t = TSTriple(1)
+        self.specials += [
+            sda_t.get_tristate(pads.sda),
+            scl_t.get_tristate(pads.scl)
+        ]
+
+        self.comb += [
+            If(self.bitbang_enable.storage,
+                sda_t.oe.eq(self.sda_oe.storage),
+                sda_t.o.eq(self.sda_out.storage),
+                self.sda_in.status.eq(sda_t.i),
+                scl_t.oe.eq(self.scl_oe.storage),
+                scl_t.o.eq(self.scl_out.storage)
+            ).Else(
+                sda_t.oe.eq(~self.programmer.sda_o),
+                sda_t.o.eq(0),
+                self.programmer.sda_i.eq(sda_t.i),
+                scl_t.oe.eq(~self.programmer.scl),
+                scl_t.o.eq(0),
+            )
+        ]

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"]):
+        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()
+            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.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 interrupt
+
+        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)
+        ]
+
+        self.submodules.ev = SharedIRQ(self.gtx_tag_ev, self.main_tag_ev)

src/libboard_artiq/Cargo.toml.tpl

@@ -25,7 +25,7 @@ void = { version = "1", default-features = false }
 
 io = { path = "../libio", features = ["byteorder"] }
 libboard_zynq = { path = "@@ZYNQ_RS@@/libboard_zynq" }
-libsupport_zynq = { path = "@@ZYNQ_RS@@/libsupport_zynq", default-features = false, features = ["alloc_core"] }
+libsupport_zynq = { path = "@@ZYNQ_RS@@/libsupport_zynq", default-features = false, features = ["alloc_core", "dummy_fiq_handler"] }
 libregister = { path = "@@ZYNQ_RS@@/libregister" }
 libconfig = { path = "@@ZYNQ_RS@@/libconfig", features = ["fat_lfn"] }
 libcortex_a9 = { path = "@@ZYNQ_RS@@/libcortex_a9" }

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,551 @@
+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;
+
+pub struct DividerConfig {
+    pub hsdiv: u16,
+    pub lsdiv: u8,
+    pub fbdiv: u64,
+}
+
+pub struct FrequencySetting {
+    pub main: DividerConfig,
+    #[cfg(has_wrpll)]
+    pub helper: DividerConfig,
+}
+
+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)
+    }
+
+    fn sda_i(dcxo: DCXO) -> bool {
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::sda_in_read() == 1 },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::sda_in_read() == 1 },
+        }
+    }
+
+    fn sda_oe(dcxo: DCXO, oe: bool) {
+        let val = if oe { 1 } else { 0 };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::sda_oe_write(val) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::sda_oe_write(val) },
+        };
+    }
+
+    fn sda_o(dcxo: DCXO, o: bool) {
+        let val = if o { 1 } else { 0 };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::sda_out_write(val) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::sda_out_write(val) },
+        };
+    }
+
+    fn scl_oe(dcxo: DCXO, oe: bool) {
+        let val = if oe { 1 } else { 0 };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::scl_oe_write(val) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::scl_oe_write(val) },
+        };
+    }
+
+    fn scl_o(dcxo: DCXO, o: bool) {
+        let val = if o { 1 } else { 0 };
+        match dcxo {
+            DCXO::Main => unsafe { csr::main_dcxo::scl_out_write(val) },
+            #[cfg(has_wrpll)]
+            DCXO::Helper => unsafe { csr::helper_dcxo::scl_out_write(val) },
+        };
+    }
+
+    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, config: DividerConfig, 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, config.hsdiv as u8, timer)?;
+    write(dcxo, 24, (config.hsdiv >> 8) as u8 | (config.lsdiv << 4), timer)?;
+    write(dcxo, 26, config.fbdiv as u8, timer)?;
+    write(dcxo, 27, (config.fbdiv >> 8) as u8, timer)?;
+    write(dcxo, 28, (config.fbdiv >> 16) as u8, timer)?;
+    write(dcxo, 29, (config.fbdiv >> 24) as u8, timer)?;
+    write(dcxo, 30, (config.fbdiv >> 32) as u8, timer)?;
+    write(dcxo, 31, (config.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, settings: FrequencySetting) -> Result<(), &'static str> {
+    unsafe {
+        csr::main_dcxo::bitbang_enable_write(1);
+        csr::main_dcxo::i2c_address_write(ADDRESS);
+    }
+
+    setup(i2c::DCXO::Main, settings.main, 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 libcortex_a9::mutex::Mutex;
+
+    use super::*;
+
+    const TIMER_WIDTH: u32 = 24;
+    const COUNTER_DIV: u32 = 2;
+
+    const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;
+
+    const KP: i32 = 6;
+    const KI: i32 = 2;
+
+    static BASE_ADPLL: Mutex<i32> = Mutex::new(0);
+    static H_INTEGRATOR: Mutex<i32> = Mutex::new(0);
+    static M_INTEGRATOR: Mutex<i32> = Mutex::new(0);
+
+    #[derive(Clone, Copy)]
+    pub enum FIQ {
+        GTXTag,
+        MainTag,
+    }
+
+    mod tag_collector {
+        use super::*;
+
+        const BEATING_PERIOD: i32 = 0x8000;
+        const BEATING_HALFPERIOD: i32 = 0x4000;
+
+        static GTX_TAG: Mutex<u32> = Mutex::new(0);
+        static GTX_TAG_READY: Mutex<bool> = Mutex::new(false);
+        static MAIN_TAG: Mutex<u32> = Mutex::new(0);
+        static MAIN_TAG_READY: Mutex<bool> = Mutex::new(false);
+
+        pub fn reset() {
+            clear_phase_diff_ready();
+            *GTX_TAG.lock() = 0;
+            *MAIN_TAG.lock() = 0;
+        }
+
+        pub fn clear_phase_diff_ready() {
+            *GTX_TAG_READY.lock() = false;
+            *MAIN_TAG_READY.lock() = false;
+        }
+
+        pub fn collect_tags(interrupt: FIQ) {
+            match interrupt {
+                FIQ::GTXTag => {
+                    *GTX_TAG.lock() = unsafe { csr::wrpll::gtx_tag_read() };
+                    *GTX_TAG_READY.lock() = true;
+                }
+                FIQ::MainTag => {
+                    *MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() };
+                    *MAIN_TAG_READY.lock() = true;
+                }
+            }
+        }
+
+        pub fn phase_diff_ready() -> bool {
+            *GTX_TAG_READY.lock() && *MAIN_TAG_READY.lock()
+        }
+
+        pub fn get_period_error() -> i32 {
+            // n * BEATING_PERIOD - GTX_TAG(n) mod BEATING_PERIOD
+            let mut period_error = (*GTX_TAG.lock()).overflowing_neg().0.rem_euclid(BEATING_PERIOD as u32) as i32;
+
+            // mapping tags from [0, 2π] -> [-π, π]
+            if period_error > BEATING_HALFPERIOD {
+                period_error -= BEATING_PERIOD
+            }
+            period_error
+        }
+
+        pub fn get_phase_error() -> i32 {
+            // MAIN_TAG(n) - GTX_TAG(n) mod BEATING_PERIOD
+            let mut phase_error = (*MAIN_TAG.lock())
+                .overflowing_sub(*GTX_TAG.lock())
+                .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, settings: FrequencySetting) -> Result<(), &'static str> {
+        unsafe {
+            csr::wrpll::helper_reset_write(1);
+            csr::helper_dcxo::bitbang_enable_write(1);
+            csr::helper_dcxo::i2c_address_write(ADDRESS);
+        }
+
+        setup(i2c::DCXO::Helper, settings.helper, 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(())
+    }
+
+    fn set_fiq(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");
+                }
+            },
+            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(())
+    }
+
+    /// To get within capture range
+    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) = get_freq_counts(timer);
+        let mut base_adpll_lock = BASE_ADPLL.lock();
+        *base_adpll_lock = count2adpll(gtx_count as i32 - main_count as i32);
+        set_adpll(i2c::DCXO::Main, *base_adpll_lock)?;
+        set_adpll(i2c::DCXO::Helper, *base_adpll_lock)?;
+
+        Ok(())
+    }
+
+    fn get_freq_counts(timer: &mut GlobalTimer) -> (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();
+
+            (gtx, main)
+        }
+    }
+
+    fn reset_plls() -> Result<(), &'static str> {
+        *H_INTEGRATOR.lock() = 0;
+        *M_INTEGRATOR.lock() = 0;
+        set_adpll(i2c::DCXO::Main, 0)?;
+        set_adpll(i2c::DCXO::Helper, 0)?;
+        Ok(())
+    }
+
+    fn clear_pending(interrupt: FIQ) {
+        match interrupt {
+            FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) },
+            FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
+        };
+    }
+
+    fn is_pending(interrupt: FIQ) -> bool {
+        match interrupt {
+            FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_read() == 1 },
+            FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 },
+        }
+    }
+
+    pub fn interrupt_handler() {
+        if is_pending(FIQ::GTXTag) {
+            tag_collector::collect_tags(FIQ::GTXTag);
+            clear_pending(FIQ::GTXTag);
+
+            helper_pll().expect("failed to run helper DCXO PLL");
+        }
+
+        if is_pending(FIQ::MainTag) {
+            tag_collector::collect_tags(FIQ::MainTag);
+            clear_pending(FIQ::MainTag);
+        }
+
+        if tag_collector::phase_diff_ready() {
+            main_pll().expect("failed to run main DCXO PLL");
+            tag_collector::clear_phase_diff_ready();
+        }
+    }
+
+    fn helper_pll() -> Result<(), &'static str> {
+        let period_err = tag_collector::get_period_error();
+        let mut integrator_lock = H_INTEGRATOR.lock();
+
+        *integrator_lock += period_err * KI;
+        let mut h_adpll = *BASE_ADPLL.lock() + period_err * KP + *integrator_lock;
+
+        h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
+        set_adpll(i2c::DCXO::Helper, h_adpll)?;
+
+        Ok(())
+    }
+
+    fn main_pll() -> Result<(), &'static str> {
+        let phase_err = tag_collector::get_phase_error();
+        let mut integrator_lock = M_INTEGRATOR.lock();
+
+        *integrator_lock += phase_err * KI;
+        let mut m_adpll = *BASE_ADPLL.lock() + phase_err * KP + *integrator_lock;
+
+        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_fiq(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(FIQ::GTXTag);
+            clear_pending(FIQ::MainTag);
+
+            // use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL
+            set_fiq(true);
+            info!("WRPLL interrupt enabled");
+        }
+    }
+}

src/libbuild_zynq/link.x

@@ -10,7 +10,9 @@ SECTIONS
     __text_start = .;
     .text :
     {
+        __exceptions_start = .;
         KEEP(*(.text.exceptions));
+        __exceptions_end = .;
         *(.text.boot);
         *(.text .text.*);
     } > SDRAM

src/libksupport/src/fiq.rs

@@ -0,0 +1,21 @@
+#[cfg(has_wrpll)]
+use libboard_artiq::si549;
+use libboard_zynq::{println, stdio};
+use libcortex_a9::{interrupt_handler, regs::MPIDR};
+use libregister::RegisterR;
+
+interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
+    match MPIDR.read().cpu_id() {
+        0 => {
+            // nFIQ is driven directly and bypass GIC
+            #[cfg(has_wrpll)]
+            si549::wrpll::interrupt_handler();
+            return;
+        }
+        _ => {}
+    };
+
+    stdio::drop_uart();
+    println!("FIQ");
+    loop {}
+});

src/libksupport/src/lib.rs

@@ -21,6 +21,7 @@ pub use pl::csr::rtio_core;
 use void::Void;
 
 pub mod eh_artiq;
+pub mod fiq;
 pub mod i2c;
 pub mod irq;
 pub mod kernel;

src/runtime/src/main.rs

@@ -21,7 +21,7 @@ use libboard_artiq::{identifier_read, logger, pl};
 use libboard_zynq::{gic, mpcore, timer::GlobalTimer};
 use libconfig::Config;
 use libcortex_a9::l2c::enable_l2_cache;
-use libsupport_zynq::ram;
+use libsupport_zynq::{exception_vectors, ram};
 use log::{info, warn};
 
 mod analyzer;
@@ -38,6 +38,11 @@ mod rtio_mgt;
 #[cfg(has_drtio)]
 mod subkernel;
 
+// linker symbols
+extern "C" {
+    static __exceptions_start: u32;
+}
+
 #[cfg(all(feature = "target_kasli_soc", has_drtio))]
 async fn io_expanders_service(
     i2c_bus: RefCell<&mut libboard_zynq::i2c::I2c>,
@@ -77,6 +82,9 @@ static mut LOG_BUFFER: [u8; 1 << 17] = [0; 1 << 17];
 
 #[no_mangle]
 pub fn main_core0() {
+    unsafe {
+        exception_vectors::set_vector_table(&__exceptions_start as *const u32 as u32);
+    }
     enable_l2_cache(0x8);
     let mut timer = GlobalTimer::start();
 

src/satman/src/main.rs

@@ -29,6 +29,8 @@ 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,
@@ -38,7 +40,7 @@ use libboard_zynq::error_led::ErrorLED;
 use libboard_zynq::{i2c::I2c, print, println, time::Milliseconds, timer::GlobalTimer};
 use libcortex_a9::{l2c::enable_l2_cache, regs::MPIDR};
 use libregister::RegisterR;
-use libsupport_zynq::ram;
+use libsupport_zynq::{exception_vectors, ram};
 use routing::Router;
 use subkernel::Manager as KernelManager;
 
@@ -48,6 +50,11 @@ mod repeater;
 mod routing;
 mod subkernel;
 
+// linker symbols
+extern "C" {
+    static __exceptions_start: u32;
+}
+
 fn drtiosat_reset(reset: bool) {
     unsafe {
         csr::drtiosat::reset_write(if reset { 1 } else { 0 });
@@ -823,10 +830,45 @@ const SI5324_SETTINGS: si5324::FrequencySettings = si5324::FrequencySettings {
     crystal_as_ckin2: true,
 };
 
+#[cfg(all(has_si549, rtio_frequency = "125.0"))]
+const SI549_SETTINGS: si549::FrequencySetting = si549::FrequencySetting {
+    main: si549::DividerConfig {
+        hsdiv: 0x058,
+        lsdiv: 0,
+        fbdiv: 0x04815791F25,
+    },
+    #[cfg(has_wrpll)]
+    helper: si549::DividerConfig {
+        // 125Mhz*32767/32768
+        hsdiv: 0x058,
+        lsdiv: 0,
+        fbdiv: 0x04814E8F442,
+    },
+};
+
+#[cfg(all(has_si549, rtio_frequency = "100.0"))]
+pub const SI549_SETTINGS: si549::FrequencySetting = si549::FrequencySetting {
+    main: si549::DividerConfig {
+        hsdiv: 0x06C,
+        lsdiv: 0,
+        fbdiv: 0x046C5F49797,
+    },
+    #[cfg(has_wrpll)]
+    helper: si549::DividerConfig {
+        // 100Mhz*32767/32768
+        hsdiv: 0x06C,
+        lsdiv: 0,
+        fbdiv: 0x046C5670BBD,
+    },
+};
+
 static mut LOG_BUFFER: [u8; 1 << 17] = [0; 1 << 17];
 
 #[no_mangle]
 pub extern "C" fn main_core0() -> i32 {
+    unsafe {
+        exception_vectors::set_vector_table(&__exceptions_start as *const u32 as u32);
+    }
     enable_l2_cache(0x8);
 
     let mut timer = GlobalTimer::start();
@@ -856,6 +898,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);
@@ -867,6 +914,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, SI549_SETTINGS).expect("cannot initialize main Si549");
 
     timer.delay_us(100_000);
     info!("Switching SYS clocks...");
@@ -884,6 +933,8 @@ pub extern "C" fn main_core0() -> i32 {
     unsafe {
         csr::gt_drtio::txenable_write(0xffffffffu32 as _);
     }
+    #[cfg(has_wrpll)]
+    si549::wrpll::helper_setup(&mut timer, SI549_SETTINGS).expect("cannot initialize helper Si549");
 
     #[cfg(has_drtio_routing)]
     let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
@@ -929,6 +980,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.
@@ -1026,6 +1080,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);
     }
 }