rename `build` derivation to `board-package-set`

rtio_clocking: update mmcm setting to use HIGH bandwidth

README.md

@@ -62,7 +62,7 @@ Notes:
 License
 -------
 
-Copyright (C) 2019-2023 M-Labs Limited.
+Copyright (C) 2019-2024 M-Labs Limited.
 
 ARTIQ is free software: you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by

flake.lock

@@ -11,11 +11,11 @@
         "src-pythonparser": "src-pythonparser"
       },
       "locked": {
-        "lastModified": 1706785107,
+        "lastModified": 1714013217,
-        "narHash": "sha256-Uj72tqigiOCdewSSBBMg6zUpVKhwjAo1HeLJgvyZ3oc=",
+        "narHash": "sha256-P0pVHTSgAkTYWFPjddcPhufe5oH8mjJAsxG8x8mo4NA=",
-        "ref": "refs/heads/master",
+        "ref": "master",
-        "rev": "3aaa7e04f26a495e8847e47424bfc16d76d82bf8",
+        "rev": "7204feae1f504e4a1dcd54b95c59e8f36c62b701",
-        "revCount": 8672,
+        "revCount": 8776,
         "type": "git",
         "url": "https://github.com/m-labs/artiq.git"
       },
@@ -37,11 +37,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1701573753,
+        "lastModified": 1707216368,
-        "narHash": "sha256-vhEtXjb9AM6/HnsgfVmhJQeqQ9JqysUm7iWNzTIbexs=",
+        "narHash": "sha256-ZXoqzG2QsVsybALLYXs473avXcyKSZNh2kIgcPo60XQ=",
         "owner": "m-labs",
         "repo": "artiq-comtools",
-        "rev": "199bdabf4de49cb7ada8a4ac7133008e0f8434b7",
+        "rev": "e5d0204490bccc07ef9141b0d7c405ab01cb8273",
         "type": "github"
       },
       "original": {
@@ -118,11 +118,11 @@
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1706515015,
+        "lastModified": 1711668574,
-        "narHash": "sha256-eFfY5A7wlYy3jD/75lx6IJRueg4noE+jowl0a8lIlVo=",
+        "narHash": "sha256-u1dfs0ASQIEr1icTVrsKwg2xToIpn7ZXxW3RHfHxshg=",
         "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "f4a8d6d5324c327dcc2d863eb7f3cc06ad630df4",
+        "rev": "219951b495fc2eac67b1456824cc1ec1fd2ee659",
         "type": "github"
       },
       "original": {
@@ -147,11 +147,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1701572254,
+        "lastModified": 1708937641,
-        "narHash": "sha256-ixq8dlpyOytDr+d/OmW8v1Ioy9V2G2ibOlNj8GFDSq4=",
+        "narHash": "sha256-Hkb9VYFzFgkYxfbh4kYcDSn7DbMUYehoQDeTALrxo2Q=",
         "owner": "m-labs",
         "repo": "sipyco",
-        "rev": "cceac0df537887135f99aa6b1bdd82853f16b4d6",
+        "rev": "4a28b311ce0069454b4e8fe1e6049db11b9f1296",
         "type": "github"
       },
       "original": {
@@ -234,11 +234,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1702982463,
+        "lastModified": 1711358419,
-        "narHash": "sha256-jKR3drE2rsTaYGEgIdv5kUo2LOb1JyIb4tJhVuCXTTc=",
+        "narHash": "sha256-LzetYaLsnov9pHVWSCTSowXUAXkxemAa61CIMY98xsc=",
-        "ref": "refs/heads/master",
+        "ref": "master",
-        "rev": "4168eb63a7e846863331ae4e656cfd82a867cca8",
+        "rev": "195a21fe78e4dde1fd705cb2899ab5d2763ae037",
-        "revCount": 636,
+        "revCount": 643,
         "type": "git",
         "url": "https://git.m-labs.hk/m-labs/zynq-rs"
       },

flake.nix

@@ -113,7 +113,7 @@
       "nist_clock_satellite" "nist_qc2_satellite" "acpki_nist_clock_satellite" "acpki_nist_qc2_satellite" 
       "nist_clock_satellite_100mhz" "nist_qc2_satellite_100mhz" "acpki_nist_clock_satellite_100mhz" "acpki_nist_qc2_satellite_100mhz"
     ];
-    build = { target, variant, json ? null }: let
+    board-package-set = { target, variant, json ? null }: let
       szl = zynqpkgs."${target}-szl";
       fsbl = zynqpkgs."${target}-fsbl";
       fwtype = if builtins.elem variant sat_variants then "satman" else "runtime";
@@ -273,7 +273,7 @@
     };
 
     # for hitl-tests
-    zc706-nist_qc2 = (build { target = "zc706"; variant = "nist_qc2"; });
+    zc706-nist_qc2 = (board-package-set { target = "zc706"; variant = "nist_qc2"; });
     zc706-hitl-tests = pkgs.stdenv.mkDerivation {
       name = "zc706-hitl-tests";
 
@@ -340,29 +340,29 @@
       {
         inherit fastnumbers artiq-netboot ramda migen-axi binutils-arm;
       } //
-      (build { target = "zc706"; variant = "nist_clock"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_clock"; }) //
-      (build { target = "zc706"; variant = "nist_clock_master"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_clock_master"; }) //
-      (build { target = "zc706"; variant = "nist_clock_master_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_clock_master_100mhz"; }) //
-      (build { target = "zc706"; variant = "nist_clock_satellite"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_clock_satellite"; }) //
-      (build { target = "zc706"; variant = "nist_clock_satellite_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_clock_satellite_100mhz"; }) //
-      (build { target = "zc706"; variant = "nist_qc2"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_qc2"; }) //
-      (build { target = "zc706"; variant = "nist_qc2_master"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_qc2_master"; }) //
-      (build { target = "zc706"; variant = "nist_qc2_master_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_qc2_master_100mhz"; }) //
-      (build { target = "zc706"; variant = "nist_qc2_satellite"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_qc2_satellite"; }) //
-      (build { target = "zc706"; variant = "nist_qc2_satellite_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "nist_qc2_satellite_100mhz"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_clock"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_clock"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_clock_master"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_clock_master"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_clock_master_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_clock_master_100mhz"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_clock_satellite"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_clock_satellite"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_clock_satellite_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_clock_satellite_100mhz"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_qc2"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_qc2"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_qc2_master"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_qc2_master"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_qc2_master_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_qc2_master_100mhz"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_qc2_satellite"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_qc2_satellite"; }) //
-      (build { target = "zc706"; variant = "acpki_nist_qc2_satellite_100mhz"; }) //
+      (board-package-set { target = "zc706"; variant = "acpki_nist_qc2_satellite_100mhz"; }) //
-      (build { target = "kasli_soc"; variant = "demo"; json = ./demo.json; }) //
+      (board-package-set { target = "kasli_soc"; variant = "demo"; json = ./demo.json; }) //
-      (build { target = "kasli_soc"; variant = "master"; json = ./kasli-soc-master.json; }) //
+      (board-package-set { target = "kasli_soc"; variant = "master"; json = ./kasli-soc-master.json; }) //
-      (build { target = "kasli_soc"; variant = "satellite"; json = ./kasli-soc-satellite.json; });
+      (board-package-set { target = "kasli_soc"; variant = "satellite"; json = ./kasli-soc-satellite.json; });
 
     hydraJobs = packages.x86_64-linux // { inherit zc706-hitl-tests; inherit gateware-sim; inherit fmt-check; };
 
@@ -390,7 +390,7 @@
       SZL = "${zynqpkgs.szl}";
     };
 
-    makeArtiqZynqPackage = build;
+    makeArtiqZynqPackage = board-package-set;
 
   };
 }

src/gateware/ddmtd.py

@@ -0,0 +1,119 @@
+from migen import *
+from migen.genlib.cdc import PulseSynchronizer, MultiReg
+from misoc.interconnect.csr import *
+
+
+class DDMTDSampler(Module):
+    def __init__(self, cd_ref, main_clk_se):
+        self.ref_beating = Signal()
+        self.main_beating = Signal()
+
+        # # #
+
+        ref_clk = Signal()
+        self.specials +=[
+            # ISERDESE2 can only be driven from fabric via IDELAYE2 (see UG471)
+            Instance("IDELAYE2",
+                    p_DELAY_SRC="DATAIN",
+                    p_HIGH_PERFORMANCE_MODE="TRUE",
+                    p_REFCLK_FREQUENCY=208.3,   # REFCLK frequency from IDELAYCTRL
+                    p_IDELAY_VALUE=0,
+
+                    i_DATAIN=cd_ref.clk,
+
+                    o_DATAOUT=ref_clk
+            ),
+            Instance("ISERDESE2",
+                    p_IOBDELAY="IFD",   # use DDLY as input
+                    p_DATA_RATE="SDR",
+                    p_DATA_WIDTH=2,     # min is 2
+                    p_NUM_CE=1,
+
+                    i_DDLY=ref_clk,
+                    i_CE1=1,
+                    i_CLK=ClockSignal("helper"),
+                    i_CLKDIV=ClockSignal("helper"),
+
+                    o_Q1=self.ref_beating
+            ),
+            Instance("ISERDESE2",
+                    p_DATA_RATE="SDR",
+                    p_DATA_WIDTH=2,     # min is 2
+                    p_NUM_CE=1,
+
+                    i_D=main_clk_se,
+                    i_CE1=1,
+                    i_CLK=ClockSignal("helper"),
+                    i_CLKDIV=ClockSignal("helper"),
+
+                    o_Q1=self.main_beating,
+            ),
+        ]
+
+
+class DDMTDDeglitcherMedianEdge(Module):
+    def __init__(self, counter, input_signal, stable_0_period=100, stable_1_period=100):
+        self.tag = Signal(len(counter))
+        self.detect = Signal()
+
+        stable_0_counter = Signal(reset=stable_0_period - 1, max=stable_0_period)
+        stable_1_counter = Signal(reset=stable_1_period - 1, max=stable_1_period)
+
+        # # #
+
+        # Based on CERN's median edge deglitcher FSM
+        # https://white-rabbit.web.cern.ch/documents/Precise_time_and_frequency_transfer_in_a_White_Rabbit_network.pdf (p.72)
+        fsm = ClockDomainsRenamer("helper")(FSM(reset_state="WAIT_STABLE_0"))
+        self.submodules += fsm
+
+        fsm.act("WAIT_STABLE_0",
+            If(stable_0_counter != 0,
+                NextValue(stable_0_counter, stable_0_counter - 1)
+            ).Else(
+                NextValue(stable_0_counter, stable_0_period - 1),
+                NextState("WAIT_EDGE")
+            ),
+            If(input_signal,
+                NextValue(stable_0_counter, stable_0_period - 1)
+            ),
+        )
+        fsm.act("WAIT_EDGE",
+            If(input_signal,
+                NextValue(self.tag, counter),
+                NextState("GOT_EDGE")
+            )
+        )
+        fsm.act("GOT_EDGE",
+            If(stable_1_counter != 0,
+                NextValue(stable_1_counter, stable_1_counter - 1)
+            ).Else(
+                NextValue(stable_1_counter, stable_1_period - 1),
+                self.detect.eq(1),
+                NextState("WAIT_STABLE_0")
+            ),
+            If(~input_signal,
+                NextValue(self.tag, self.tag + 1),
+                NextValue(stable_1_counter, stable_1_period - 1)
+            ),
+        )
+        
+
+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 = DDMTDDeglitcherMedianEdge(counter, 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(deglitcher.tag)
+               )
+        ]

src/gateware/drtio_aux_controller.py

@@ -1,12 +1,12 @@
 """Auxiliary controller, common to satellite and master"""
 
-from artiq.gateware.drtio.aux_controller import Transmitter, Receiver
+from artiq.gateware.drtio.aux_controller import (max_packet, aux_buffer_count,
+    Transmitter, Receiver)
 from migen.fhdl.simplify import FullMemoryWE
 from misoc.interconnect.csr import *
 from migen_axi.interconnect.sram import SRAM
 from migen_axi.interconnect import axi
 
-max_packet = 1024
 
 class _DRTIOAuxControllerBase(Module):
     def __init__(self, link_layer):
@@ -27,12 +27,12 @@ class DRTIOAuxControllerAxi(_DRTIOAuxControllerBase):
         tx_sdram_if = SRAM(self.transmitter.mem, read_only=False)
         rx_sdram_if = SRAM(self.receiver.mem, read_only=True)
         aw_decoder = axi.AddressDecoder(self.bus.aw,
-            [(lambda a: a[log2_int(max_packet)] == 0, tx_sdram_if.bus.aw),
+            [(lambda a: a[log2_int(max_packet*aux_buffer_count)] == 0, tx_sdram_if.bus.aw),
-             (lambda a: a[log2_int(max_packet)] == 1, rx_sdram_if.bus.aw)],
+             (lambda a: a[log2_int(max_packet*aux_buffer_count)] == 1, rx_sdram_if.bus.aw)],
             register=True)
         ar_decoder = axi.AddressDecoder(self.bus.ar,
-            [(lambda a: a[log2_int(max_packet)] == 0, tx_sdram_if.bus.ar),
+            [(lambda a: a[log2_int(max_packet*aux_buffer_count)] == 0, tx_sdram_if.bus.ar),
-             (lambda a: a[log2_int(max_packet)] == 1, rx_sdram_if.bus.ar)],
+             (lambda a: a[log2_int(max_packet*aux_buffer_count)] == 1, rx_sdram_if.bus.ar)],
             register=True)
         # unlike wb, axi address decoder only connects ar/aw lanes,
         # the rest must also be connected!
@@ -82,4 +82,4 @@ class DRTIOAuxControllerBare(_DRTIOAuxControllerBase):
         return self.receiver.mem.get_port(write_capable=False)
 
     def get_mem_size(self):
-        return max_packet
+        return max_packet*aux_buffer_count

src/gateware/kasli_soc.py

@@ -26,6 +26,7 @@ import analyzer
 import acpki
 import drtio_aux_controller
 import zynq_clocking
+import wrpll
 from config import write_csr_file, write_mem_file, write_rustc_cfg_file
 
 eem_iostandard_dict = {
@@ -108,6 +109,7 @@ class GenericStandalone(SoCCore):
     def __init__(self, description, acpki=False):
         self.acpki = acpki
         clk_freq = description["rtio_frequency"]
+        with_wrpll = description["enable_wrpll"]
 
         platform = kasli_soc.Platform()
         platform.toolchain.bitstream_commands.extend([
@@ -119,13 +121,6 @@ class GenericStandalone(SoCCore):
         SoCCore.__init__(self, platform=platform, csr_data_width=32, ident=ident, ps_cd_sys=False)
 
         self.config["HW_REV"] = description["hw_rev"]
-        
-
-        self.submodules += SMAClkinForward(self.platform)
-
-        self.config["HAS_SI5324"] = None
-        self.config["SI5324_SOFT_RESET"] = None
-
         clk_synth = platform.request("cdr_clk_clean_fabric")
         clk_synth_se = Signal()
         clk_synth_se_buf = Signal()
@@ -149,6 +144,23 @@ class GenericStandalone(SoCCore):
         self.crg = self.ps7 # HACK for eem_7series to find the clock
         self.crg.cd_sys = self.sys_crg.cd_sys
 
+        if with_wrpll:
+            self.submodules.wrpll_refclk = wrpll.SMAFrequencyMultiplier(platform.request("sma_clkin"))
+            self.submodules.wrpll = wrpll.WRPLL(
+                platform=self.platform,
+                cd_ref=self.wrpll_refclk.cd_ref,
+                main_clk_se=clk_synth_se)
+            self.csr_devices.append("wrpll_refclk")
+            self.csr_devices.append("wrpll")
+            self.comb += self.ps7.core.core0.nfiq.eq(self.wrpll.ev.irq)
+            self.config["HAS_SI549"] = None
+            self.config["WRPLL_REF_CLK"] = "SMA_CLKIN"
+        else:
+            self.submodules += SMAClkinForward(self.platform)
+            self.config["HAS_SI5324"] = None
+            self.config["SI5324_SOFT_RESET"] = None
+
+
         self.rtio_channels = []
         has_grabber = any(peripheral["type"] == "grabber" for peripheral in description["peripherals"])
         if has_grabber:
@@ -207,6 +219,7 @@ class GenericStandalone(SoCCore):
 class GenericMaster(SoCCore):
     def __init__(self, description, acpki=False):
         clk_freq = description["rtio_frequency"]
+        with_wrpll = description["enable_wrpll"]
 
         has_drtio_over_eem = any(peripheral["type"] == "shuttler" for peripheral in description["peripherals"])
         self.acpki = acpki
@@ -222,8 +235,6 @@ class GenericMaster(SoCCore):
 
         self.config["HW_REV"] = description["hw_rev"]
 
-        self.submodules += SMAClkinForward(self.platform)
-
         data_pads = [platform.request("sfp", i) for i in range(4)]
 
         self.submodules.gt_drtio = gtx_7series.GTX(
@@ -257,8 +268,25 @@ class GenericMaster(SoCCore):
         self.comb += ext_async_rst.eq(self.sys_crg.clk_sw_fsm.o_clk_sw & ~gtx0.tx_init.done)
         self.specials += MultiReg(self.sys_crg.clk_sw_fsm.o_clk_sw & self.sys_crg.mmcm_locked, self.gt_drtio.clk_path_ready, odomain="bootstrap")
 
-        self.config["HAS_SI5324"] = None
+        if with_wrpll:
-        self.config["SI5324_SOFT_RESET"] = None
+            clk_synth = platform.request("cdr_clk_clean_fabric")
+            clk_synth_se = Signal()
+            platform.add_period_constraint(clk_synth.p, 8.0)
+            self.specials += Instance("IBUFGDS", p_DIFF_TERM="TRUE", p_IBUF_LOW_PWR="FALSE", i_I=clk_synth.p, i_IB=clk_synth.n, o_O=clk_synth_se)
+            self.submodules.wrpll_refclk = wrpll.SMAFrequencyMultiplier(platform.request("sma_clkin"))
+            self.submodules.wrpll = wrpll.WRPLL(
+                platform=self.platform,
+                cd_ref=self.wrpll_refclk.cd_ref,
+                main_clk_se=clk_synth_se)
+            self.csr_devices.append("wrpll_refclk")
+            self.csr_devices.append("wrpll")
+            self.comb += self.ps7.core.core0.nfiq.eq(self.wrpll.ev.irq)
+            self.config["HAS_SI549"] = None
+            self.config["WRPLL_REF_CLK"] = "SMA_CLKIN"
+        else:
+            self.submodules += SMAClkinForward(self.platform)
+            self.config["HAS_SI5324"] = None
+            self.config["SI5324_SOFT_RESET"] = None
 
         self.rtio_channels = []
         has_grabber = any(peripheral["type"] == "grabber" for peripheral in description["peripherals"])
@@ -400,6 +428,7 @@ class GenericMaster(SoCCore):
 class GenericSatellite(SoCCore):
     def __init__(self, description, acpki=False):
         clk_freq = description["rtio_frequency"]
+        with_wrpll = description["enable_wrpll"]
 
         self.acpki = acpki
 
@@ -551,14 +580,30 @@ class GenericSatellite(SoCCore):
         self.config["RTIO_FREQUENCY"] = str(clk_freq/1e6)
         self.config["CLOCK_FREQUENCY"] = int(clk_freq)
 
-        self.submodules.siphaser = SiPhaser7Series(
+        if with_wrpll:
-            si5324_clkin=platform.request("cdr_clk"),
+            clk_synth = platform.request("cdr_clk_clean_fabric")
-            rx_synchronizer=self.rx_synchronizer,
+            clk_synth_se = Signal()
-            ultrascale=False,
+            platform.add_period_constraint(clk_synth.p, 8.0)
-            rtio_clk_freq=self.gt_drtio.rtio_clk_freq)
+            self.specials += Instance("IBUFGDS", p_DIFF_TERM="TRUE", p_IBUF_LOW_PWR="FALSE", i_I=clk_synth.p, i_IB=clk_synth.n, o_O=clk_synth_se)
-        self.csr_devices.append("siphaser")
+            self.submodules.wrpll = wrpll.WRPLL(
-        self.config["HAS_SI5324"] = None
+                platform=self.platform,
-        self.config["SI5324_SOFT_RESET"] = None
+                cd_ref=self.gt_drtio.cd_rtio_rx0,
+                main_clk_se=clk_synth_se)
+            self.submodules.wrpll_skewtester = wrpll.SkewTester(self.rx_synchronizer)
+            self.csr_devices.append("wrpll_skewtester")
+            self.csr_devices.append("wrpll")
+            self.comb += self.ps7.core.core0.nfiq.eq(self.wrpll.ev.irq)
+            self.config["HAS_SI549"] = None
+            self.config["WRPLL_REF_CLK"] = "GT_CDR"
+        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(

src/gateware/si549.py

@@ -0,0 +1,277 @@
+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
+
+        fsm.act("IDLE",
+            If(self.stb,
+                NextValue(self.nack, 0),
+                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(
+                    NextValue(self.nack, 1),
+                    NextState("STOP")
+                )
+            )
+        )
+        fsm.act("DATA0",
+            master.data.eq(self.adpll[0:8]),
+            master.write.eq(1),
+            If(master.ready,
+                If(master.ack,
+                    NextState("DATA1")
+                ).Else(
+                    NextValue(self.nack, 1),
+                    NextState("STOP")
+                )
+            )
+        )
+        fsm.act("DATA1",
+            master.data.eq(self.adpll[8:16]),
+            master.write.eq(1),
+            If(master.ready,
+                If(master.ack,
+                    NextState("DATA2")
+                ).Else(
+                    NextValue(self.nack, 1),
+                    NextState("STOP")
+                )
+            )
+        )
+        fsm.act("DATA2",
+            master.data.eq(self.adpll[16:24]),
+            master.write.eq(1),
+            If(master.ready,
+                If(~master.ack, NextValue(self.nack, 1)),
+                NextState("STOP")
+            )
+        )
+        fsm.act("STOP",
+            master.stop.eq(1),
+            If(master.ready,
+                If(~master.ack, NextValue(self.nack, 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 = CSR()
+        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.sync += self.programmer.stb.eq(self.adpll_stb.re)
+
+        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.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,237 @@
+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 DDMTDSampler, DDMTD
+from si549 import Si549
+
+class FrequencyCounter(Module, AutoCSR):
+    def __init__(self, domains, counter_width=24):
+        self.update = CSR()
+        self.busy = CSRStatus()
+
+        counter_reset = Signal()
+        counter_stb = Signal()
+        timer = Signal(counter_width)
+
+        # # #
+
+        fsm = FSM()
+        self.submodules += fsm
+
+        fsm.act("IDLE",
+            counter_reset.eq(1),
+            If(self.update.re,
+                NextValue(timer, 2**counter_width - 1),
+                NextState("COUNTING")
+            )
+        )
+        fsm.act("COUNTING",
+            self.busy.status.eq(1),
+            If(timer != 0,
+                NextValue(timer, timer - 1)
+            ).Else( 
+                counter_stb.eq(1),
+                NextState("IDLE")
+            )
+        )
+
+        for domain in domains:
+            name = "counter_" + domain
+            counter_csr = CSRStatus(counter_width, name=name)
+            setattr(self, name, counter_csr)
+
+            divider = Signal(2)
+            divided = Signal()
+            divided_sys = Signal()
+            divided_sys_r = Signal()
+            divided_tick = Signal()
+            counter = Signal(counter_width)
+
+            # # #
+
+            sync_domain = getattr(self.sync, domain)
+            sync_domain +=[
+                divider.eq(divider + 1),
+                divided.eq(divider[-1])
+            ]
+            self.specials += MultiReg(divided, divided_sys)
+            self.sync += divided_sys_r.eq(divided_sys)
+            self.comb += divided_tick.eq(divided_sys & ~divided_sys_r)
+
+            self.sync += [
+                If(counter_stb, counter_csr.status.eq(counter)),
+                If(divided_tick, counter.eq(counter + 1)),
+                If(counter_reset, counter.eq(0))
+            ]
+
+class SkewTester(Module, AutoCSR):
+    def __init__(self, rx_synchronizer):
+        self.error = CSR()
+
+        # # #
+        
+        # The RX synchronizer is tested for setup/hold violations by feeding it a
+        # toggling pattern and checking that the same toggling pattern comes out.
+        toggle_in = Signal()
+        self.sync.rtio_rx0 += toggle_in.eq(~toggle_in)
+        toggle_out = rx_synchronizer.resync(toggle_in)
+
+        toggle_out_expected = Signal()
+        self.sync += toggle_out_expected.eq(~toggle_out)
+
+        error = Signal()
+        self.sync += [
+            If(toggle_out != toggle_out_expected, error.eq(1)),
+            If(self.error.re, error.eq(0))
+        ]
+        self.specials += MultiReg(error, self.error.w)
+
+
+class WRPLL(Module, AutoCSR):
+    def __init__(self, platform, cd_ref, main_clk_se, COUNTER_BIT=32):
+        self.helper_reset = CSRStorage(reset=1)
+        self.ref_tag = CSRStatus(COUNTER_BIT)
+        self.main_tag = CSRStatus(COUNTER_BIT)
+
+        ddmtd_counter = Signal(COUNTER_BIT)
+
+        ref_tag_sys = Signal(COUNTER_BIT)
+        main_tag_sys = Signal(COUNTER_BIT)
+        ref_tag_stb_sys = Signal()
+        main_tag_stb_sys = Signal()
+
+        # # #
+
+        self.submodules.main_dcxo = Si549(platform.request("ddmtd_main_dcxo_i2c"))
+        self.submodules.helper_dcxo = Si549(platform.request("ddmtd_helper_dcxo_i2c"))
+
+        helper_dcxo_pads = platform.request("ddmtd_helper_clk")
+        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(["sys", cd_ref.name])
+
+        self.submodules.ddmtd_sampler = DDMTDSampler(cd_ref, main_clk_se)
+
+        self.sync.helper += ddmtd_counter.eq(ddmtd_counter + 1)
+        self.submodules.ddmtd_ref = DDMTD(ddmtd_counter, self.ddmtd_sampler.ref_beating)
+        self.submodules.ddmtd_main = DDMTD(ddmtd_counter, self.ddmtd_sampler.main_beating)
+
+        # DDMTD tags collection
+
+        self.specials += [
+            MultiReg(self.ddmtd_ref.h_tag, ref_tag_sys),
+            MultiReg(self.ddmtd_main.h_tag, main_tag_sys)
+        ]
+
+        ref_tag_stb_ps = PulseSynchronizer("helper", "sys")
+        main_tag_stb_ps = PulseSynchronizer("helper", "sys")
+        self.submodules += [
+            ref_tag_stb_ps,
+            main_tag_stb_ps
+        ]
+        self.sync.helper += [
+            ref_tag_stb_ps.i.eq(self.ddmtd_ref.h_tag_update),
+            main_tag_stb_ps.i.eq(self.ddmtd_main.h_tag_update)
+        ]
+        self.sync += [
+            ref_tag_stb_sys.eq(ref_tag_stb_ps.o),
+            main_tag_stb_sys.eq(main_tag_stb_ps.o)
+        ]
+
+        self.sync += [
+            If(ref_tag_stb_sys,
+                self.ref_tag.status.eq(ref_tag_sys),
+            ),
+            If(main_tag_stb_sys,
+                self.main_tag.status.eq(main_tag_sys)
+            )
+        ]
+
+        # EventMangers for firmware interrupt
+
+        self.submodules.ref_tag_ev = EventManager()
+        self.ref_tag_ev.stb = EventSourcePulse()
+        self.ref_tag_ev.finalize()
+
+        self.submodules.main_tag_ev = EventManager()
+        self.main_tag_ev.stb = EventSourcePulse()
+        self.main_tag_ev.finalize()
+
+        self.sync += [
+            self.ref_tag_ev.stb.trigger.eq(ref_tag_stb_sys),
+            self.main_tag_ev.stb.trigger.eq(main_tag_stb_sys)
+        ]
+
+        self.submodules.ev = SharedIRQ(self.ref_tag_ev, self.main_tag_ev)
+
+
+class SMAFrequencyMultiplier(Module, AutoCSR):
+    def __init__(self, sma_clkin):
+        sma_clkin_se = Signal()
+        mmcm_locked = Signal()
+        mmcm_fb_clk = Signal()
+        ref_clk = Signal()
+        self.clock_domains.cd_ref = ClockDomain()
+        self.refclk_reset = CSRStorage(reset=1)
+
+        self.mmcm_bypass = CSRStorage()
+        self.mmcm_locked = CSRStatus()
+        self.mmcm_reset = CSRStorage(reset=1)
+
+        self.mmcm_daddr = CSRStorage(7)
+        self.mmcm_din = CSRStorage(16)
+        self.mmcm_dwen = CSRStorage()
+        self.mmcm_den = CSRStorage()
+        self.mmcm_dclk = CSRStorage()
+        self.mmcm_dout = CSRStatus(16)
+        self.mmcm_dready = CSRStatus()
+
+        # # #
+
+        self.specials += [
+            Instance("IBUFDS",
+                i_I=sma_clkin.p, i_IB=sma_clkin.n,
+                o_O=sma_clkin_se),
+            # MMCME2 is capable to accept 10MHz input while PLLE2 only support down to 19MHz input (DS191)
+            # The MMCME2 can be reconfiged during runtime using the Dynamic Reconfiguration Ports
+            Instance("MMCME2_ADV",
+                p_BANDWIDTH="HIGH",  # lower output jitter (see https://support.xilinx.com/s/question/0D52E00006iHqRqSAK)
+                o_LOCKED=self.mmcm_locked.status,
+                i_RST=self.mmcm_reset.storage,
+                
+                p_CLKIN1_PERIOD=8, # ns
+                i_CLKIN1=sma_clkin_se,
+                i_CLKINSEL=1,  # 1=CLKIN1 0=CLKIN2
+
+                # VCO @ 1.25GHz 
+                p_CLKFBOUT_MULT_F=10, p_DIVCLK_DIVIDE=1,
+                i_CLKFBIN=mmcm_fb_clk, o_CLKFBOUT=mmcm_fb_clk, 
+
+                # 125MHz for WRPLL
+                p_CLKOUT0_DIVIDE_F=10, p_CLKOUT0_PHASE=0.0, o_CLKOUT0=ref_clk,
+
+                # Dynamic Reconfiguration Ports
+                i_DADDR = self.mmcm_daddr.storage,
+                i_DI = self.mmcm_din.storage,
+                i_DWE = self.mmcm_dwen.storage,
+                i_DEN = self.mmcm_den.storage,
+                i_DCLK = self.mmcm_dclk.storage,
+                o_DO = self.mmcm_dout.status,
+                o_DRDY = self.mmcm_dready.status
+            ),
+            Instance("BUFGMUX",
+                i_I0=ref_clk,
+                i_I1=sma_clkin_se,
+                i_S=self.mmcm_bypass.storage,
+                o_O=self.cd_ref.clk
+            ),
+            AsyncResetSynchronizer(self.cd_ref, self.refclk_reset.storage),
+        ]

src/libboard_artiq/Cargo.toml.tpl

@@ -10,6 +10,7 @@ name = "libboard_artiq"
 [features]
 target_zc706 = ["libboard_zynq/target_zc706", "libconfig/target_zc706"]
 target_kasli_soc = ["libboard_zynq/target_kasli_soc", "libconfig/target_kasli_soc"]
+calibrate_wrpll_skew = []
 
 [build-dependencies]
 build_zynq = { path = "../libbuild_zynq" }

src/libboard_artiq/src/drtioaux.rs

@@ -74,12 +74,15 @@ where F: FnOnce(&[u8]) -> Result<T, Error> {
     let linkidx = linkno as usize;
     unsafe {
         if (DRTIOAUX[linkidx].aux_rx_present_read)() == 1 {
-            let ptr = (DRTIOAUX_MEM[linkidx].base + DRTIOAUX_MEM[linkidx].size / 2) as *mut u32;
+            let read_ptr = (DRTIOAUX[linkidx].aux_read_pointer_read)() as usize;
-            let len = (DRTIOAUX[linkidx].aux_rx_length_read)() as usize;
+            let ptr = (DRTIOAUX_MEM[linkidx].base + DRTIOAUX_MEM[linkidx].size / 2 + read_ptr * 0x400) as *mut u32;
             // work buffer to accomodate axi burst reads
-            let mut buf: [u8; 1024] = [0; 1024];
+            // buffer at maximum proto packet size, not maximum gateware supported size
-            copy_work_buffer(ptr, buf.as_mut_ptr() as *mut u32, len as isize);
+            // to minimize copying time
-            let result = f(&buf[0..len]);
+            const LEN: usize = 512;
+            let mut buf: [u8; LEN] = [0; LEN];
+            copy_work_buffer(ptr, buf.as_mut_ptr() as *mut u32, LEN as isize);
+            let result = f(&buf);
             (DRTIOAUX[linkidx].aux_rx_present_write)(1);
             Ok(Some(result?))
         } else {
@@ -100,15 +103,15 @@ pub fn recv(linkno: u8) -> Result<Option<Packet>, Error> {
 
         let mut reader = Cursor::new(buffer);
 
-        let checksum_at = buffer.len() - 4;
+        let packet = Packet::read_from(&mut reader)?;
+        let padding = (12 - (reader.position() % 8)) % 8;
+        let checksum_at = reader.position() + padding;
         let checksum = crc::crc32::checksum_ieee(&reader.get_ref()[0..checksum_at]);
         reader.set_position(checksum_at);
         if reader.read_u32()? != checksum {
             return Err(Error::CorruptedPacket);
         }
-        reader.set_position(0);
+        Ok(packet)
-
-        Ok(Packet::read_from(&mut reader)?)
     })
 }
 
@@ -130,10 +133,9 @@ where F: FnOnce(&mut [u8]) -> Result<usize, Error> {
     unsafe {
         while (DRTIOAUX[linkno].aux_tx_read)() != 0 {}
         let ptr = DRTIOAUX_MEM[linkno].base as *mut u32;
-        let len = DRTIOAUX_MEM[linkno].size / 2;
         // work buffer, works with unaligned mem access
         let mut buf: [u8; 1024] = [0; 1024];
-        let len = f(&mut buf[0..len])?;
+        let len = f(&mut buf)?;
         copy_work_buffer(buf.as_mut_ptr() as *mut u32, ptr, len as isize);
         (DRTIOAUX[linkno].aux_tx_length_write)(len as u16);
         (DRTIOAUX[linkno].aux_tx_write)(1);

src/libboard_artiq/src/drtioaux_async.rs

@@ -38,12 +38,15 @@ where F: FnOnce(&[u8]) -> Result<T, Error> {
     let linkidx = linkno as usize;
     unsafe {
         if (DRTIOAUX[linkidx].aux_rx_present_read)() == 1 {
-            let ptr = (DRTIOAUX_MEM[linkidx].base + DRTIOAUX_MEM[linkidx].size / 2) as *mut u32;
+            let read_ptr = (DRTIOAUX[linkidx].aux_read_pointer_read)() as usize;
-            let len = (DRTIOAUX[linkidx].aux_rx_length_read)() as usize;
+            let ptr = (DRTIOAUX_MEM[linkidx].base + DRTIOAUX_MEM[linkidx].size / 2 + read_ptr * 0x400) as *mut u32;
             // work buffer to accomodate axi burst reads
-            let mut buf: [u8; 1024] = [0; 1024];
+            // buffer at maximum proto packet size, not maximum gateware supported size
-            copy_work_buffer(ptr, buf.as_mut_ptr() as *mut u32, len as isize);
+            // to minimize required copying time
-            let result = f(&buf[0..len]);
+            const LEN: usize = 512;
+            let mut buf: [u8; LEN] = [0; LEN];
+            copy_work_buffer(ptr, buf.as_mut_ptr() as *mut u32, LEN as isize);
+            let result = f(&buf);
             (DRTIOAUX[linkidx].aux_rx_present_write)(1);
             Ok(Some(result?))
         } else {
@@ -64,15 +67,15 @@ pub async fn recv(linkno: u8) -> Result<Option<Packet>, Error> {
 
         let mut reader = Cursor::new(buffer);
 
-        let checksum_at = buffer.len() - 4;
+        let packet = Packet::read_from(&mut reader)?;
+        let padding = (12 - (reader.position() % 8)) % 8;
+        let checksum_at = reader.position() + padding;
         let checksum = crc::crc32::checksum_ieee(&reader.get_ref()[0..checksum_at]);
         reader.set_position(checksum_at);
         if reader.read_u32()? != checksum {
             return Err(Error::CorruptedPacket);
         }
-        reader.set_position(0);
+        Ok(packet)
-
-        Ok(Packet::read_from(&mut reader)?)
     })
     .await
 }
@@ -103,10 +106,9 @@ where F: FnOnce(&mut [u8]) -> Result<usize, Error> {
     unsafe {
         let _ = block_async!(tx_ready(linkno)).await;
         let ptr = DRTIOAUX_MEM[linkno].base as *mut u32;
-        let len = DRTIOAUX_MEM[linkno].size / 2;
         // work buffer, works with unaligned mem access
         let mut buf: [u8; 1024] = [0; 1024];
-        let len = f(&mut buf[0..len])?;
+        let len = f(&mut buf)?;
         copy_work_buffer(buf.as_mut_ptr() as *mut u32, ptr, len as isize);
         (DRTIOAUX[linkno].aux_tx_length_write)(len as u16);
         (DRTIOAUX[linkno].aux_tx_write)(1);

src/libboard_artiq/src/drtioaux_proto.rs

@@ -89,8 +89,6 @@ pub enum Packet {
     RoutingSetRank {
         rank: u8,
     },
-    RoutingRetrievePackets,
-    RoutingNoPackets,
     RoutingAck,
 
     MonitorRequest {
@@ -325,8 +323,6 @@ impl Packet {
                 rank: reader.read_u8()?,
             },
             0x32 => Packet::RoutingAck,
-            0x33 => Packet::RoutingRetrievePackets,
-            0x34 => Packet::RoutingNoPackets,
 
             0x40 => Packet::MonitorRequest {
                 destination: reader.read_u8()?,
@@ -602,8 +598,6 @@ impl Packet {
                 writer.write_u8(rank)?;
             }
             Packet::RoutingAck => writer.write_u8(0x32)?,
-            Packet::RoutingRetrievePackets => writer.write_u8(0x33)?,
-            Packet::RoutingNoPackets => writer.write_u8(0x34)?,
 
             Packet::MonitorRequest {
                 destination,

src/libboard_artiq/src/fiq.rs

@@ -0,0 +1,22 @@
+use libboard_zynq::{println, stdio};
+use libcortex_a9::{interrupt_handler, regs::MPIDR};
+use libregister::RegisterR;
+
+#[cfg(has_si549)]
+use crate::si549;
+
+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_si549)]
+            si549::wrpll::interrupt_handler();
+            return;
+        }
+        _ => {}
+    };
+
+    stdio::drop_uart();
+    println!("FIQ");
+    loop {}
+});

src/libboard_artiq/src/io_expander.rs

@@ -1,6 +1,7 @@
 use libboard_zynq::i2c;
 use log::info;
 
+#[cfg(has_virtual_leds)]
 use crate::pl::csr;
 
 // Only the bare minimum registers. Bits/IO connections equivalent between IC types.
@@ -19,11 +20,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] = [
@@ -33,6 +38,7 @@ const IODIR1: [u8; 2] = [
 
 pub struct IoExpander {
     address: u8,
+    #[cfg(has_virtual_leds)]
     virtual_led_mapping: &'static [(u8, u8, u8)],
     iodir: [u8; 2],
     out_current: [u8; 2],
@@ -42,17 +48,18 @@ pub struct IoExpander {
 
 impl IoExpander {
     pub fn new(i2c: &mut i2c::I2c, index: u8) -> Result<Self, &'static str> {
-        #[cfg(hw_rev = "v1.0")]
+        #[cfg(all(hw_rev = "v1.0", has_virtual_leds))]
         const VIRTUAL_LED_MAPPING0: [(u8, u8, u8); 2] = [(0, 0, 6), (1, 1, 6)];
-        #[cfg(hw_rev = "v1.1")]
+        #[cfg(all(hw_rev = "v1.1", has_virtual_leds))]
         const VIRTUAL_LED_MAPPING0: [(u8, u8, u8); 2] = [(0, 0, 7), (1, 1, 6)];
-
+        #[cfg(has_virtual_leds)]
         const VIRTUAL_LED_MAPPING1: [(u8, u8, u8); 2] = [(2, 0, 6), (3, 1, 6)];
 
         // Both expanders on SHARED I2C bus
         let mut io_expander = match index {
             0 => IoExpander {
                 address: 0x40,
+                #[cfg(has_virtual_leds)]
                 virtual_led_mapping: &VIRTUAL_LED_MAPPING0,
                 iodir: IODIR0,
                 out_current: [0; 2],
@@ -66,6 +73,7 @@ impl IoExpander {
             },
             1 => IoExpander {
                 address: 0x42,
+                #[cfg(has_virtual_leds)]
                 virtual_led_mapping: &VIRTUAL_LED_MAPPING1,
                 iodir: IODIR1,
                 out_current: [0; 2],

src/libboard_artiq/src/lib.rs

@@ -1,5 +1,7 @@
 #![no_std]
 #![feature(never_type)]
+#![feature(naked_functions)]
+#![feature(asm)]
 
 extern crate core_io;
 extern crate crc;
@@ -19,7 +21,8 @@ pub mod drtioaux;
 #[cfg(has_drtio)]
 pub mod drtioaux_async;
 pub mod drtioaux_proto;
-#[cfg(all(feature = "target_kasli_soc", has_drtio))]
+pub mod fiq;
+#[cfg(feature = "target_kasli_soc")]
 pub mod io_expander;
 pub mod logger;
 #[cfg(has_drtio)]
@@ -35,7 +38,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,816 @@
+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;
+
+const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;
+
+pub struct DividerConfig {
+    pub hsdiv: u16,
+    pub lsdiv: u8,
+    pub fbdiv: u64,
+}
+
+pub struct FrequencySetting {
+    pub main: DividerConfig,
+    pub helper: DividerConfig,
+}
+
+mod i2c {
+    use super::*;
+
+    #[derive(Clone, Copy)]
+    pub enum DCXO {
+        Main,
+        Helper,
+    }
+
+    fn half_period(timer: &mut GlobalTimer) {
+        timer.delay_us(1)
+    }
+
+    fn sda_i(dcxo: DCXO) -> bool {
+        match dcxo {
+            DCXO::Main => unsafe { csr::wrpll::main_dcxo_sda_in_read() == 1 },
+            DCXO::Helper => unsafe { csr::wrpll::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::wrpll::main_dcxo_sda_oe_write(val) },
+            DCXO::Helper => unsafe { csr::wrpll::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::wrpll::main_dcxo_sda_out_write(val) },
+            DCXO::Helper => unsafe { csr::wrpll::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::wrpll::main_dcxo_scl_oe_write(val) },
+            DCXO::Helper => unsafe { csr::wrpll::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::wrpll::main_dcxo_scl_out_write(val) },
+            DCXO::Helper => unsafe { csr::wrpll::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::wrpll::main_dcxo_bitbang_enable_write(1);
+        csr::wrpll::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::wrpll::main_dcxo_bitbang_enable_write(0);
+    }
+
+    info!("Main Si549 started");
+    Ok(())
+}
+
+pub fn helper_setup(timer: &mut GlobalTimer, settings: &FrequencySetting) -> Result<(), &'static str> {
+    unsafe {
+        csr::wrpll::helper_reset_write(1);
+        csr::wrpll::helper_dcxo_bitbang_enable_write(1);
+        csr::wrpll::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::wrpll::helper_dcxo_bitbang_enable_write(0);
+    }
+    info!("Helper Si549 started");
+    Ok(())
+}
+
+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::wrpll::main_dcxo_bitbang_enable_read() == 1 {
+                return Err("Main si549 bitbang mode is active when using gateware i2c");
+            }
+
+            while csr::wrpll::main_dcxo_adpll_busy_read() == 1 {}
+            if csr::wrpll::main_dcxo_nack_read() == 1 {
+                return Err("Main si549 failed to ack adpll write");
+            }
+
+            csr::wrpll::main_dcxo_i2c_address_write(ADDRESS);
+            csr::wrpll::main_dcxo_adpll_write(adpll as u32);
+
+            csr::wrpll::main_dcxo_adpll_stb_write(1);
+        },
+        i2c::DCXO::Helper => unsafe {
+            if csr::wrpll::helper_dcxo_bitbang_enable_read() == 1 {
+                return Err("Helper si549 bitbang mode is active when using gateware i2c");
+            }
+
+            while csr::wrpll::helper_dcxo_adpll_busy_read() == 1 {}
+            if csr::wrpll::helper_dcxo_nack_read() == 1 {
+                return Err("Helper si549 failed to ack adpll write");
+            }
+
+            csr::wrpll::helper_dcxo_i2c_address_write(ADDRESS);
+            csr::wrpll::helper_dcxo_adpll_write(adpll as u32);
+
+            csr::wrpll::helper_dcxo_adpll_stb_write(1);
+        },
+    };
+
+    Ok(())
+}
+
+#[cfg(has_wrpll)]
+pub mod wrpll {
+
+    use super::*;
+
+    const BEATING_PERIOD: i32 = 0x8000;
+    const BEATING_HALFPERIOD: i32 = 0x4000;
+    const COUNTER_WIDTH: u32 = 24;
+    const DIV_WIDTH: u32 = 2;
+
+    const KP: i32 = 6;
+    const KI: i32 = 2;
+    // 4 ppm capture range
+    const ADPLL_LIM: i32 = (4.0 / 0.0001164) as i32;
+
+    static mut BASE_ADPLL: i32 = 0;
+    static mut H_LAST_ADPLL: i32 = 0;
+    static mut LAST_PERIOD_ERR: i32 = 0;
+    static mut M_LAST_ADPLL: i32 = 0;
+    static mut LAST_PHASE_ERR: i32 = 0;
+
+    #[derive(Clone, Copy)]
+    pub enum ISR {
+        RefTag,
+        MainTag,
+    }
+
+    mod tag_collector {
+        use super::*;
+
+        #[cfg(wrpll_ref_clk = "GT_CDR")]
+        static mut TAG_OFFSET: u32 = 8382;
+        #[cfg(wrpll_ref_clk = "SMA_CLKIN")]
+        static mut TAG_OFFSET: u32 = 0;
+        static mut REF_TAG: u32 = 0;
+        static mut REF_TAG_READY: bool = false;
+        static mut MAIN_TAG: u32 = 0;
+        static mut MAIN_TAG_READY: bool = false;
+
+        pub fn reset() {
+            clear_phase_diff_ready();
+            unsafe {
+                REF_TAG = 0;
+                MAIN_TAG = 0;
+            }
+        }
+
+        pub fn clear_phase_diff_ready() {
+            unsafe {
+                REF_TAG_READY = false;
+                MAIN_TAG_READY = false;
+            }
+        }
+
+        pub fn collect_tags(interrupt: ISR) {
+            match interrupt {
+                ISR::RefTag => unsafe {
+                    REF_TAG = csr::wrpll::ref_tag_read();
+                    REF_TAG_READY = true;
+                },
+                ISR::MainTag => unsafe {
+                    MAIN_TAG = csr::wrpll::main_tag_read();
+                    MAIN_TAG_READY = true;
+                },
+            }
+        }
+
+        pub fn phase_diff_ready() -> bool {
+            unsafe { REF_TAG_READY && MAIN_TAG_READY }
+        }
+
+        #[cfg(feature = "calibrate_wrpll_skew")]
+        pub fn set_tag_offset(offset: u32) {
+            unsafe {
+                TAG_OFFSET = offset;
+            }
+        }
+
+        #[cfg(feature = "calibrate_wrpll_skew")]
+        pub fn get_tag_offset() -> u32 {
+            unsafe { TAG_OFFSET }
+        }
+
+        pub fn get_period_error() -> i32 {
+            // n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD
+            let mut period_error = unsafe { REF_TAG.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) - REF_TAG(n) - TAG_OFFSET mod BEATING_PERIOD
+            let mut phase_error = unsafe {
+                MAIN_TAG
+                    .overflowing_sub(REF_TAG + TAG_OFFSET)
+                    .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
+        }
+    }
+
+    fn set_isr(en: bool) {
+        let val = if en { 1 } else { 0 };
+        unsafe {
+            csr::wrpll::ref_tag_ev_enable_write(val);
+            csr::wrpll::main_tag_ev_enable_write(val);
+        }
+    }
+
+    fn set_base_adpll() -> Result<(), &'static str> {
+        let count2adpll =
+            |error: i32| ((error as f64 * 1e6) / (0.0001164 * (1 << (COUNTER_WIDTH - DIV_WIDTH)) as f64)) as i32;
+
+        let (ref_count, main_count) = get_freq_counts();
+        unsafe {
+            BASE_ADPLL = count2adpll(ref_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() -> (u32, u32) {
+        unsafe {
+            csr::wrpll::frequency_counter_update_write(1);
+            while csr::wrpll::frequency_counter_busy_read() == 1 {}
+            #[cfg(wrpll_ref_clk = "GT_CDR")]
+            let ref_count = csr::wrpll::frequency_counter_counter_rtio_rx0_read();
+            #[cfg(wrpll_ref_clk = "SMA_CLKIN")]
+            let ref_count = csr::wrpll::frequency_counter_counter_ref_read();
+            let main_count = csr::wrpll::frequency_counter_counter_sys_read();
+
+            (ref_count, main_count)
+        }
+    }
+
+    fn reset_plls(timer: &mut GlobalTimer) -> Result<(), &'static str> {
+        unsafe {
+            H_LAST_ADPLL = 0;
+            LAST_PERIOD_ERR = 0;
+            M_LAST_ADPLL = 0;
+            LAST_PHASE_ERR = 0;
+        }
+        set_adpll(i2c::DCXO::Main, 0)?;
+        set_adpll(i2c::DCXO::Helper, 0)?;
+        // wait for adpll to transfer and DCXO to settle
+        timer.delay_us(200);
+        Ok(())
+    }
+
+    fn clear_pending(interrupt: ISR) {
+        match interrupt {
+            ISR::RefTag => unsafe { csr::wrpll::ref_tag_ev_pending_write(1) },
+            ISR::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
+        };
+    }
+
+    fn is_pending(interrupt: ISR) -> bool {
+        match interrupt {
+            ISR::RefTag => unsafe { csr::wrpll::ref_tag_ev_pending_read() == 1 },
+            ISR::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 },
+        }
+    }
+
+    pub fn interrupt_handler() {
+        if is_pending(ISR::RefTag) {
+            tag_collector::collect_tags(ISR::RefTag);
+            clear_pending(ISR::RefTag);
+            helper_pll().expect("failed to run helper DCXO PLL");
+        }
+
+        if is_pending(ISR::MainTag) {
+            tag_collector::collect_tags(ISR::MainTag);
+            clear_pending(ISR::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();
+        unsafe {
+            // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting
+            let adpll = (H_LAST_ADPLL + (KP + KI) * period_err - KP * LAST_PERIOD_ERR).clamp(-ADPLL_LIM, ADPLL_LIM);
+            set_adpll(i2c::DCXO::Helper, BASE_ADPLL + adpll)?;
+            H_LAST_ADPLL = adpll;
+            LAST_PERIOD_ERR = period_err;
+        };
+        Ok(())
+    }
+
+    fn main_pll() -> Result<(), &'static str> {
+        let phase_err = tag_collector::get_phase_error();
+        unsafe {
+            // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting
+            let adpll = (M_LAST_ADPLL + (KP + KI) * phase_err - KP * LAST_PHASE_ERR).clamp(-ADPLL_LIM, ADPLL_LIM);
+            set_adpll(i2c::DCXO::Main, BASE_ADPLL + adpll)?;
+            M_LAST_ADPLL = adpll;
+            LAST_PHASE_ERR = phase_err;
+        };
+        Ok(())
+    }
+
+    #[cfg(wrpll_ref_clk = "GT_CDR")]
+    fn test_skew(timer: &mut GlobalTimer) -> Result<(), &'static str> {
+        // wait for PLL to stabilize
+        timer.delay_us(20_000);
+
+        info!("testing the skew of SYS CLK...");
+        if has_timing_error(timer) {
+            return Err("the skew cannot satisfy setup/hold time constraint of RX synchronizer");
+        }
+        info!("the skew of SYS CLK met the timing constraint");
+        Ok(())
+    }
+
+    #[cfg(wrpll_ref_clk = "GT_CDR")]
+    fn has_timing_error(timer: &mut GlobalTimer) -> bool {
+        unsafe {
+            csr::wrpll_skewtester::error_write(1);
+        }
+        timer.delay_us(5_000);
+        unsafe { csr::wrpll_skewtester::error_read() == 1 }
+    }
+
+    #[cfg(feature = "calibrate_wrpll_skew")]
+    fn find_edge(target: bool, timer: &mut GlobalTimer) -> Result<u32, &'static str> {
+        const STEP: u32 = 8;
+        const STABLE_THRESHOLD: u32 = 10;
+
+        enum FSM {
+            Init,
+            WaitEdge,
+            GotEdge,
+        }
+
+        let mut state: FSM = FSM::Init;
+        let mut offset: u32 = tag_collector::get_tag_offset();
+        let mut median_edge: u32 = 0;
+        let mut stable_counter: u32 = 0;
+
+        for _ in 0..(BEATING_PERIOD as u32 / STEP) as usize {
+            tag_collector::set_tag_offset(offset);
+            offset += STEP;
+            // wait for PLL to stabilize
+            timer.delay_us(20_000);
+
+            let error = has_timing_error(timer);
+            // A median edge deglitcher
+            match state {
+                FSM::Init => {
+                    if error != target {
+                        stable_counter += 1;
+                    } else {
+                        stable_counter = 0;
+                    }
+
+                    if stable_counter >= STABLE_THRESHOLD {
+                        state = FSM::WaitEdge;
+                        stable_counter = 0;
+                    }
+                }
+                FSM::WaitEdge => {
+                    if error == target {
+                        state = FSM::GotEdge;
+                        median_edge = offset;
+                    }
+                }
+                FSM::GotEdge => {
+                    if error != target {
+                        median_edge += STEP;
+                        stable_counter = 0;
+                    } else {
+                        stable_counter += 1;
+                    }
+
+                    if stable_counter >= STABLE_THRESHOLD {
+                        return Ok(median_edge);
+                    }
+                }
+            }
+        }
+        return Err("failed to find timing error edge");
+    }
+
+    #[cfg(feature = "calibrate_wrpll_skew")]
+    fn calibrate_skew(timer: &mut GlobalTimer) -> Result<(), &'static str> {
+        info!("calibrating skew to meet timing constraint...");
+
+        // clear calibrated value
+        tag_collector::set_tag_offset(0);
+        let rising = find_edge(true, timer)? as i32;
+        let falling = find_edge(false, timer)? as i32;
+
+        let width = BEATING_PERIOD - (falling - rising);
+        let result = falling + width / 2;
+        tag_collector::set_tag_offset(result as u32);
+
+        info!(
+            "calibration successful, error zone: {} -> {}, width: {} ({}deg), middle of working region: {}",
+            rising,
+            falling,
+            width,
+            360 * width / BEATING_PERIOD,
+            result,
+        );
+
+        Ok(())
+    }
+
+    pub fn select_recovered_clock(rc: bool, timer: &mut GlobalTimer) {
+        set_isr(false);
+
+        if rc {
+            tag_collector::reset();
+            reset_plls(timer).expect("failed to reset main and helper PLL");
+
+            // get within capture range
+            set_base_adpll().expect("failed to set base adpll");
+
+            // clear gateware pending flag
+            clear_pending(ISR::RefTag);
+            clear_pending(ISR::MainTag);
+
+            // use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL
+            set_isr(true);
+            info!("WRPLL interrupt enabled");
+
+            #[cfg(feature = "calibrate_wrpll_skew")]
+            calibrate_skew(timer).expect("failed to set the correct skew");
+
+            #[cfg(wrpll_ref_clk = "GT_CDR")]
+            test_skew(timer).expect("skew test failed");
+        }
+    }
+}
+
+#[cfg(has_wrpll_refclk)]
+pub mod wrpll_refclk {
+    use super::*;
+
+    pub struct MmcmSetting {
+        pub clkout0_reg1: u16,  //0x08
+        pub clkout0_reg2: u16,  //0x09
+        pub clkfbout_reg1: u16, //0x14
+        pub clkfbout_reg2: u16, //0x15
+        pub div_reg: u16,       //0x16
+        pub lock_reg1: u16,     //0x18
+        pub lock_reg2: u16,     //0x19
+        pub lock_reg3: u16,     //0x1A
+        pub power_reg: u16,     //0x28
+        pub filt_reg1: u16,     //0x4E
+        pub filt_reg2: u16,     //0x4F
+    }
+
+    fn one_clock_cycle() {
+        unsafe {
+            csr::wrpll_refclk::mmcm_dclk_write(1);
+            csr::wrpll_refclk::mmcm_dclk_write(0);
+        }
+    }
+
+    fn set_addr(address: u8) {
+        unsafe {
+            csr::wrpll_refclk::mmcm_daddr_write(address);
+        }
+    }
+
+    fn set_data(value: u16) {
+        unsafe {
+            csr::wrpll_refclk::mmcm_din_write(value);
+        }
+    }
+
+    fn set_enable(en: bool) {
+        unsafe {
+            let val = if en { 1 } else { 0 };
+            csr::wrpll_refclk::mmcm_den_write(val);
+        }
+    }
+
+    fn set_write_enable(en: bool) {
+        unsafe {
+            let val = if en { 1 } else { 0 };
+            csr::wrpll_refclk::mmcm_dwen_write(val);
+        }
+    }
+
+    fn get_data() -> u16 {
+        unsafe { csr::wrpll_refclk::mmcm_dout_read() }
+    }
+
+    fn drp_ready() -> bool {
+        unsafe { csr::wrpll_refclk::mmcm_dready_read() == 1 }
+    }
+
+    #[allow(dead_code)]
+    fn read(address: u8) -> u16 {
+        set_addr(address);
+        set_enable(true);
+        // Set DADDR on the mmcm and assert DEN for one clock cycle
+        one_clock_cycle();
+
+        set_enable(false);
+        while !drp_ready() {
+            // keep the clock signal until data is ready
+            one_clock_cycle();
+        }
+        get_data()
+    }
+
+    fn write(address: u8, value: u16) {
+        set_addr(address);
+        set_data(value);
+        set_write_enable(true);
+        set_enable(true);
+        // Set DADDR, DI on the mmcm and assert DWE, DEN for one clock cycle
+        one_clock_cycle();
+
+        set_write_enable(false);
+        set_enable(false);
+        while !drp_ready() {
+            // keep the clock signal until write is finished
+            one_clock_cycle();
+        }
+    }
+
+    fn reset(rst: bool) {
+        unsafe {
+            let val = if rst { 1 } else { 0 };
+            csr::wrpll_refclk::mmcm_reset_write(val)
+        }
+    }
+
+    pub fn setup(timer: &mut GlobalTimer, settings: MmcmSetting, mmcm_bypass: bool) -> Result<(), &'static str> {
+        unsafe {
+            csr::wrpll_refclk::refclk_reset_write(1);
+        }
+
+        if mmcm_bypass {
+            info!("Bypassing mmcm");
+            unsafe {
+                csr::wrpll_refclk::mmcm_bypass_write(1);
+            }
+        } else {
+            // Based on "DRP State Machine" from XAPP888
+            // hold reset HIGH during mmcm config
+            reset(true);
+            write(0x08, settings.clkout0_reg1);
+            write(0x09, settings.clkout0_reg2);
+            write(0x14, settings.clkfbout_reg1);
+            write(0x15, settings.clkfbout_reg2);
+            write(0x16, settings.div_reg);
+            write(0x18, settings.lock_reg1);
+            write(0x19, settings.lock_reg2);
+            write(0x1A, settings.lock_reg3);
+            write(0x28, settings.power_reg);
+            write(0x4E, settings.filt_reg1);
+            write(0x4F, settings.filt_reg2);
+            reset(false);
+
+            // wait for the mmcm to lock
+            timer.delay_us(100);
+
+            let locked = unsafe { csr::wrpll_refclk::mmcm_locked_read() == 1 };
+            if !locked {
+                return Err("mmcm failed to generate 125MHz ref clock from SMA CLKIN");
+            }
+        }
+
+        unsafe {
+            csr::wrpll_refclk::refclk_reset_write(0);
+        }
+
+        Ok(())
+    }
+}

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/runtime/src/analyzer.rs

@@ -60,7 +60,7 @@ pub mod remote_analyzer {
         routing_table: &drtio_routing::RoutingTable,
         up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         timer: GlobalTimer,
-    ) -> Result<RemoteBuffer, &'static str> {
+    ) -> Result<RemoteBuffer, drtio::Error> {
         // gets data from satellites and returns consolidated data
         let mut remote_data: Vec<u8> = Vec::new();
         let mut remote_error = false;

src/runtime/src/main.rs

@@ -8,20 +8,20 @@
 #[macro_use]
 extern crate alloc;
 
-#[cfg(all(feature = "target_kasli_soc", has_drtio))]
+#[cfg(all(feature = "target_kasli_soc", has_virtual_leds))]
 use core::cell::RefCell;
 
 use ksupport;
 use libasync::task;
 #[cfg(has_drtio_eem)]
 use libboard_artiq::drtio_eem;
-#[cfg(all(feature = "target_kasli_soc", has_drtio))]
+#[cfg(feature = "target_kasli_soc")]
 use libboard_artiq::io_expander;
 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,7 +38,12 @@ mod rtio_mgt;
 #[cfg(has_drtio)]
 mod subkernel;
 
-#[cfg(all(feature = "target_kasli_soc", has_drtio))]
+// linker symbols
+extern "C" {
+    static __exceptions_start: u32;
+}
+
+#[cfg(all(feature = "target_kasli_soc", has_virtual_leds))]
 async fn io_expanders_service(
     i2c_bus: RefCell<&mut libboard_zynq::i2c::I2c>,
     io_expander0: RefCell<io_expander::IoExpander>,
@@ -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();
 
@@ -93,7 +101,7 @@ pub fn main_core0() {
     info!("gateware ident: {}", identifier_read(&mut [0; 64]));
 
     ksupport::i2c::init();
-    #[cfg(all(feature = "target_kasli_soc", has_drtio))]
+    #[cfg(feature = "target_kasli_soc")]
     {
         let i2c_bus = unsafe { (ksupport::i2c::I2C_BUS).as_mut().unwrap() };
         let mut io_expander0 = io_expander::IoExpander::new(i2c_bus, 0).unwrap();
@@ -104,6 +112,11 @@ pub fn main_core0() {
         io_expander1
             .init(i2c_bus)
             .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);
@@ -111,6 +124,7 @@ pub fn main_core0() {
         io_expander1.set(1, 1, false);
         io_expander0.service(i2c_bus).unwrap();
         io_expander1.service(i2c_bus).unwrap();
+        #[cfg(has_virtual_leds)]
         task::spawn(io_expanders_service(
             RefCell::new(i2c_bus),
             RefCell::new(io_expander0),

src/runtime/src/moninj.rs

@@ -58,10 +58,11 @@ mod remote_moninj {
     use log::error;
 
     use super::*;
-    use crate::rtio_mgt::drtio;
+    use crate::rtio_mgt::{drtio, drtio::Error as DrtioError};
 
     pub async fn read_probe(
         aux_mutex: &Rc<Mutex<bool>>,
+        routing_table: &drtio_routing::RoutingTable,
         timer: GlobalTimer,
         linkno: u8,
         destination: u8,
@@ -71,6 +72,7 @@ mod remote_moninj {
         let reply = drtio::aux_transact(
             aux_mutex,
             linkno,
+            routing_table,
             &drtioaux_async::Packet::MonitorRequest {
                 destination: destination,
                 channel: channel as _,
@@ -82,8 +84,8 @@ mod remote_moninj {
         match reply {
             Ok(drtioaux_async::Packet::MonitorReply { value }) => return value as i64,
             Ok(packet) => error!("received unexpected aux packet: {:?}", packet),
-            Err("link went down") => {
+            Err(DrtioError::LinkDown) => {
-                debug!("link is down");
+                warn!("link is down");
             }
             Err(e) => error!("aux packet error ({})", e),
         }
@@ -92,6 +94,7 @@ mod remote_moninj {
 
     pub async fn inject(
         aux_mutex: &Rc<Mutex<bool>>,
+        _routing_table: &drtio_routing::RoutingTable,
         _timer: GlobalTimer,
         linkno: u8,
         destination: u8,
@@ -115,6 +118,7 @@ mod remote_moninj {
 
     pub async fn read_injection_status(
         aux_mutex: &Rc<Mutex<bool>>,
+        routing_table: &drtio_routing::RoutingTable,
         timer: GlobalTimer,
         linkno: u8,
         destination: u8,
@@ -124,6 +128,7 @@ mod remote_moninj {
         let reply = drtio::aux_transact(
             aux_mutex,
             linkno,
+            routing_table,
             &drtioaux_async::Packet::InjectionStatusRequest {
                 destination: destination,
                 channel: channel as _,
@@ -135,8 +140,8 @@ mod remote_moninj {
         match reply {
             Ok(drtioaux_async::Packet::InjectionStatusReply { value }) => return value as i8,
             Ok(packet) => error!("received unexpected aux packet: {:?}", packet),
-            Err("link went down") => {
+            Err(DrtioError::LinkDown) => {
-                debug!("link is down");
+                warn!("link is down");
             }
             Err(e) => error!("aux packet error ({})", e),
         }
@@ -183,7 +188,7 @@ macro_rules! dispatch {
             local_moninj::$func(channel.into(), $($param, )*)
         } else {
             let linkno = hop - 1 as u8;
-            remote_moninj::$func($aux_mutex, $timer, linkno, destination, channel, $($param, )*).await
+            remote_moninj::$func($aux_mutex, $routing_table, $timer, linkno, destination, channel, $($param, )*).await
         }
     }}
 }

src/runtime/src/rtio_clocking.rs

@@ -4,6 +4,8 @@ use ksupport::i2c;
 use libboard_artiq::pl;
 #[cfg(has_si5324)]
 use libboard_artiq::si5324;
+#[cfg(has_si549)]
+use libboard_artiq::si549;
 #[cfg(has_si5324)]
 use libboard_zynq::i2c::I2c;
 use libboard_zynq::timer::GlobalTimer;
@@ -260,6 +262,150 @@ fn setup_si5324(i2c: &mut I2c, timer: &mut GlobalTimer, clk: RtioClock) {
     si5324::setup(i2c, &si5324_settings, si5324_ref_input, timer).expect("cannot initialize Si5324");
 }
 
+#[cfg(all(has_si549, has_wrpll))]
+fn wrpll_setup(timer: &mut GlobalTimer, clk: RtioClock, si549_settings: &si549::FrequencySetting) {
+    // register values are directly copied from preconfigured mmcm
+    let (mmcm_setting, mmcm_bypass) = match clk {
+        RtioClock::Ext0_Synth0_10to125 => (
+            si549::wrpll_refclk::MmcmSetting {
+                // CLKFBOUT_MULT = 62.5, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 5
+                clkout0_reg1: 0x1083,
+                clkout0_reg2: 0x0080,
+                clkfbout_reg1: 0x179e,
+                clkfbout_reg2: 0x4c00,
+                div_reg: 0x1041,
+                lock_reg1: 0x00fa,
+                lock_reg2: 0x7c01,
+                lock_reg3: 0xffe9,
+                power_reg: 0x9900,
+                filt_reg1: 0x1008,
+                filt_reg2: 0x8800,
+            },
+            false,
+        ),
+        RtioClock::Ext0_Synth0_80to125 => (
+            si549::wrpll_refclk::MmcmSetting {
+                // CLKFBOUT_MULT = 15.625, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 10
+                clkout0_reg1: 0x1145,
+                clkout0_reg2: 0x0000,
+                clkfbout_reg1: 0x11c7,
+                clkfbout_reg2: 0x5880,
+                div_reg: 0x1041,
+                lock_reg1: 0x028a,
+                lock_reg2: 0x7c01,
+                lock_reg3: 0xffe9,
+                power_reg: 0x9900,
+                filt_reg1: 0x9908,
+                filt_reg2: 0x8100,
+            },
+            false,
+        ),
+        RtioClock::Ext0_Synth0_100to125 => (
+            si549::wrpll_refclk::MmcmSetting {
+                // CLKFBOUT_MULT = 12.5, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 10
+                clkout0_reg1: 0x1145,
+                clkout0_reg2: 0x0000,
+                clkfbout_reg1: 0x1145,
+                clkfbout_reg2: 0x4c00,
+                div_reg: 0x1041,
+                lock_reg1: 0x0339,
+                lock_reg2: 0x7c01,
+                lock_reg3: 0xffe9,
+                power_reg: 0x9900,
+                filt_reg1: 0x9108,
+                filt_reg2: 0x0100,
+            },
+            false,
+        ),
+        RtioClock::Ext0_Synth0_125to125 => (
+            si549::wrpll_refclk::MmcmSetting {
+                // CLKFBOUT_MULT = 10, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 10
+                clkout0_reg1: 0x1145,
+                clkout0_reg2: 0x0000,
+                clkfbout_reg1: 0x1145,
+                clkfbout_reg2: 0x0000,
+                div_reg: 0x1041,
+                lock_reg1: 0x03e8,
+                lock_reg2: 0x7001,
+                lock_reg3: 0xf3e9,
+                power_reg: 0x0100,
+                filt_reg1: 0x9908,
+                filt_reg2: 0x1100,
+            },
+            true,
+        ),
+        _ => unreachable!(),
+    };
+
+    si549::helper_setup(timer, &si549_settings).expect("cannot initialize helper Si549");
+    si549::wrpll_refclk::setup(timer, mmcm_setting, mmcm_bypass).expect("cannot initialize ref clk for wrpll");
+    si549::wrpll::select_recovered_clock(true, timer);
+}
+
+#[cfg(has_si549)]
+fn get_si549_setting(clk: RtioClock) -> si549::FrequencySetting {
+    match clk {
+        RtioClock::Ext0_Synth0_10to125 => {
+            info!("using 10MHz reference to make 125MHz RTIO clock with WRPLL");
+        }
+        RtioClock::Ext0_Synth0_80to125 => {
+            info!("using 80MHz reference to make 125MHz RTIO clock with WRPLL");
+        }
+        RtioClock::Ext0_Synth0_100to125 => {
+            info!("using 100MHz reference to make 125MHz RTIO clock with WRPLL");
+        }
+        RtioClock::Ext0_Synth0_125to125 => {
+            info!("using 125MHz reference to make 125MHz RTIO clock with WRPLL");
+        }
+        RtioClock::Int_100 => {
+            info!("using internal 100MHz RTIO clock");
+        }
+        RtioClock::Int_125 => {
+            info!("using internal 125MHz RTIO clock");
+        }
+        _ => {
+            warn!(
+                "rtio_clock setting '{:?}' is unsupported. Falling back to default internal 125MHz RTIO clock.",
+                clk
+            );
+        }
+    };
+
+    match clk {
+        RtioClock::Int_100 => {
+            si549::FrequencySetting {
+                main: si549::DividerConfig {
+                    hsdiv: 0x06C,
+                    lsdiv: 0,
+                    fbdiv: 0x046C5F49797,
+                },
+                helper: si549::DividerConfig {
+                    // 100MHz*32767/32768
+                    hsdiv: 0x06C,
+                    lsdiv: 0,
+                    fbdiv: 0x046C5670BBD,
+                },
+            }
+        }
+        _ => {
+            // Everything else use 125MHz
+            si549::FrequencySetting {
+                main: si549::DividerConfig {
+                    hsdiv: 0x058,
+                    lsdiv: 0,
+                    fbdiv: 0x04815791F25,
+                },
+                helper: si549::DividerConfig {
+                    // 125MHz*32767/32768
+                    hsdiv: 0x058,
+                    lsdiv: 0,
+                    fbdiv: 0x04814E8F442,
+                },
+            }
+        }
+    }
+}
+
 pub fn init(timer: &mut GlobalTimer, cfg: &Config) {
     let clk = get_rtio_clock_cfg(cfg);
     #[cfg(has_si5324)]
@@ -274,9 +420,29 @@ pub fn init(timer: &mut GlobalTimer, cfg: &Config) {
         }
     }
 
+    #[cfg(has_si549)]
+    let si549_settings = get_si549_setting(clk);
+
+    #[cfg(has_si549)]
+    si549::main_setup(timer, &si549_settings).expect("cannot initialize main Si549");
+
     #[cfg(has_drtio)]
     init_drtio(timer);
 
     #[cfg(not(has_drtio))]
     init_rtio(timer);
+
+    #[cfg(all(has_si549, has_wrpll))]
+    {
+        // SYS CLK switch will reset CSRs that are used by WRPLL
+        match clk {
+            RtioClock::Ext0_Synth0_10to125
+            | RtioClock::Ext0_Synth0_80to125
+            | RtioClock::Ext0_Synth0_100to125
+            | RtioClock::Ext0_Synth0_125to125 => {
+                wrpll_setup(timer, clk, &si549_settings);
+            }
+            _ => {}
+        }
+    }
 }

src/runtime/src/rtio_mgt.rs

@@ -1,19 +1,20 @@
 use alloc::rc::Rc;
 use core::cell::RefCell;
 
-use libboard_artiq::{drtio_routing, pl::csr};
+use libboard_artiq::{drtio_routing, drtio_routing::RoutingTable, pl::csr};
 use libboard_zynq::timer::GlobalTimer;
 use libcortex_a9::mutex::Mutex;
 
 #[cfg(has_drtio)]
 pub mod drtio {
     use alloc::vec::Vec;
+    use core::fmt;
 
     use embedded_hal::blocking::delay::DelayMs;
     use ksupport::{resolve_channel_name, ASYNC_ERROR_BUSY, ASYNC_ERROR_COLLISION, ASYNC_ERROR_SEQUENCE_ERROR,
                    SEEN_ASYNC_ERRORS};
     use libasync::{delay, task};
-    use libboard_artiq::{drtioaux::Error,
+    use libboard_artiq::{drtioaux::Error as DrtioError,
                          drtioaux_async,
                          drtioaux_async::Packet,
                          drtioaux_proto::{PayloadStatus, MASTER_PAYLOAD_MAX_SIZE}};
@@ -23,9 +24,44 @@ pub mod drtio {
     use super::*;
     use crate::{analyzer::remote_analyzer::RemoteBuffer, rtio_dma::remote_dma, subkernel};
 
+    #[derive(Debug, PartialEq, Eq, Clone, Copy)]
+    pub enum Error {
+        Timeout,
+        AuxError,
+        LinkDown,
+        UnexpectedReply,
+        DmaAddTraceFail(u8),
+        DmaEraseFail(u8),
+        DmaPlaybackFail(u8),
+        SubkernelAddFail(u8),
+        SubkernelRunFail(u8),
+    }
+
+    impl fmt::Display for Error {
+        fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+            match self {
+                Error::Timeout => write!(f, "timed out"),
+                Error::AuxError => write!(f, "aux packet error"),
+                Error::LinkDown => write!(f, "link down"),
+                Error::UnexpectedReply => write!(f, "unexpected reply"),
+                Error::DmaAddTraceFail(dest) => write!(f, "error adding DMA trace on satellite #{}", dest),
+                Error::DmaEraseFail(dest) => write!(f, "error erasing DMA trace on satellite #{}", dest),
+                Error::DmaPlaybackFail(dest) => write!(f, "error playing back DMA trace on satellite #{}", dest),
+                Error::SubkernelAddFail(dest) => write!(f, "error adding subkernel on satellite #{}", dest),
+                Error::SubkernelRunFail(dest) => write!(f, "error on subkernel run request on satellite #{}", dest),
+            }
+        }
+    }
+
+    impl From<DrtioError> for Error {
+        fn from(_error: DrtioError) -> Self {
+            Error::AuxError
+        }
+    }
+
     pub fn startup(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>,
+        routing_table: &Rc<RefCell<RoutingTable>>,
         up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         timer: GlobalTimer,
     ) {
@@ -43,128 +79,104 @@ pub mod drtio {
         unsafe { (csr::DRTIO[linkno].rx_up_read)() == 1 }
     }
 
-    async fn link_has_async_ready(linkno: u8) -> bool {
+    async fn process_async_packets(linkno: u8, routing_table: &RoutingTable, packet: Packet) -> Option<Packet> {
-        let linkno = linkno as usize;
+        match packet {
-        let async_ready;
+            Packet::DmaPlaybackStatus {
-        unsafe {
+                id,
-            async_ready = (csr::DRTIO[linkno].async_messages_ready_read)() == 1;
+                source,
-            (csr::DRTIO[linkno].async_messages_ready_write)(1);
+                destination: 0,
-        }
+                error,
-        async_ready
+                channel,
-    }
+                timestamp,
-
+            } => {
-    async fn process_async_packets(
+                remote_dma::playback_done(id, source, error, channel, timestamp).await;
-        aux_mutex: &Mutex<bool>,
+                None
-        linkno: u8,
+            }
-        routing_table: &drtio_routing::RoutingTable,
+            Packet::SubkernelFinished {
-        timer: GlobalTimer,
+                id,
-    ) {
+                destination: 0,
-        if link_has_async_ready(linkno).await {
+                with_exception,
-            loop {
+                exception_src,
-                let reply = aux_transact(aux_mutex, linkno, &Packet::RoutingRetrievePackets, timer).await;
+            } => {
-                if let Ok(packet) = reply {
+                subkernel::subkernel_finished(id, with_exception, exception_src).await;
-                    match packet {
+                None
-                        Packet::DmaPlaybackStatus {
+            }
-                            id,
+            Packet::SubkernelMessage {
-                            source,
+                id,
-                            destination: 0,
+                source,
-                            error,
+                destination: 0,
-                            channel,
+                status,
-                            timestamp,
+                length,
-                        } => {
+                data,
-                            remote_dma::playback_done(id, source, error, channel, timestamp).await;
+            } => {
-                        }
+                subkernel::message_handle_incoming(id, status, length as usize, &data).await;
-                        Packet::SubkernelFinished {
+                // acknowledge receiving part of the message
-                            id,
+                drtioaux_async::send(linkno, &Packet::SubkernelMessageAck { destination: source })
-                            destination: 0,
+                    .await
-                            with_exception,
+                    .unwrap();
-                            exception_src,
+                None
-                        } => {
+            }
-                            subkernel::subkernel_finished(id, with_exception, exception_src).await;
+            // routable packets
-                        }
+            Packet::DmaAddTraceRequest { destination, .. }
-                        Packet::SubkernelMessage {
+            | Packet::DmaAddTraceReply { destination, .. }
-                            id,
+            | Packet::DmaRemoveTraceRequest { destination, .. }
-                            source,
+            | Packet::DmaRemoveTraceReply { destination, .. }
-                            destination: 0,
+            | Packet::DmaPlaybackRequest { destination, .. }
-                            status,
+            | Packet::DmaPlaybackReply { destination, .. }
-                            length,
+            | Packet::SubkernelLoadRunRequest { destination, .. }
-                            data,
+            | Packet::SubkernelLoadRunReply { destination, .. }
-                        } => {
+            | Packet::SubkernelMessage { destination, .. }
-                            subkernel::message_handle_incoming(id, status, length as usize, &data).await;
+            | Packet::SubkernelMessageAck { destination, .. }
-                            // acknowledge receiving part of the message
+            | Packet::DmaPlaybackStatus { destination, .. }
-                            let _lock = aux_mutex.async_lock().await;
+            | Packet::SubkernelFinished { destination, .. } => {
-                            drtioaux_async::send(linkno, &Packet::SubkernelMessageAck { destination: source })
+                if destination == 0 {
-                                .await
+                    Some(packet)
-                                .unwrap();
-                            let mut countdown = timer.countdown();
-                            // give the satellites some time to process
-                            delay(&mut countdown, Milliseconds(10)).await;
-                        }
-                        // routable packets
-                        Packet::DmaAddTraceRequest { destination, .. }
-                        | Packet::DmaAddTraceReply { destination, .. }
-                        | Packet::DmaRemoveTraceRequest { destination, .. }
-                        | Packet::DmaRemoveTraceReply { destination, .. }
-                        | Packet::DmaPlaybackRequest { destination, .. }
-                        | Packet::DmaPlaybackReply { destination, .. }
-                        | Packet::SubkernelLoadRunRequest { destination, .. }
-                        | Packet::SubkernelLoadRunReply { destination, .. }
-                        | Packet::SubkernelMessage { destination, .. }
-                        | Packet::SubkernelMessageAck { destination, .. }
-                        | Packet::DmaPlaybackStatus { destination, .. }
-                        | Packet::SubkernelFinished { destination, .. } => {
-                            let dest_link = routing_table.0[destination as usize][0] - 1;
-                            if dest_link == linkno {
-                                warn!(
-                                    "[LINK#{}] Re-routed packet would return to the same link, dropping: {:?}",
-                                    linkno, packet
-                                );
-                            } else if destination == 0 {
-                                warn!("[LINK#{}] Received invalid routable packet: {:?}", linkno, packet)
-                            } else {
-                                drtioaux_async::send(dest_link, &packet).await.unwrap();
-                            }
-                        }
-
-                        Packet::RoutingNoPackets => break,
-
-                        other => warn!("[LINK#{}] Received an unroutable packet: {:?}", linkno, other),
-                    }
                 } else {
-                    warn!(
+                    let dest_link = routing_table.0[destination as usize][0] - 1;
-                        "[LINK#{}] Error handling async packets ({})",
+                    if dest_link == linkno {
-                        linkno,
+                        warn!(
-                        reply.unwrap_err()
+                            "[LINK#{}] Re-routed packet would return to the same link, dropping: {:?}",
-                    );
+                            linkno, packet
-                    return;
+                        );
+                    } else {
+                        drtioaux_async::send(dest_link, &packet).await.unwrap();
+                    }
+                    None
                 }
             }
+            other => Some(other),
         }
     }
 
-    async fn recv_aux_timeout(linkno: u8, timeout: u64, timer: GlobalTimer) -> Result<Packet, &'static str> {
+    async fn recv_aux_timeout(linkno: u8, timeout: u64, timer: GlobalTimer) -> Result<Packet, Error> {
         if !link_rx_up(linkno).await {
-            return Err("link went down");
+            return Err(Error::LinkDown);
         }
         match drtioaux_async::recv_timeout(linkno, Some(timeout), timer).await {
             Ok(packet) => return Ok(packet),
-            Err(Error::TimedOut) => return Err("timed out"),
+            Err(DrtioError::TimedOut) => return Err(Error::Timeout),
-            Err(_) => return Err("aux packet error"),
+            Err(_) => return Err(Error::AuxError),
         }
     }
 
     pub async fn aux_transact(
         aux_mutex: &Mutex<bool>,
         linkno: u8,
+        routing_table: &RoutingTable,
         request: &Packet,
         timer: GlobalTimer,
-    ) -> Result<Packet, &'static str> {
+    ) -> Result<Packet, Error> {
         if !link_rx_up(linkno).await {
-            return Err("link went down");
+            return Err(Error::LinkDown);
         }
         let _lock = aux_mutex.async_lock().await;
         drtioaux_async::send(linkno, request).await.unwrap();
-        Ok(recv_aux_timeout(linkno, 200, timer).await?)
+        loop {
+            let packet = recv_aux_timeout(linkno, 200, timer).await?;
+            if let Some(packet) = process_async_packets(linkno, routing_table, packet).await {
+                return Ok(packet);
+            }
+        }
     }
 
     async fn drain_buffer(linkno: u8, draining_time: Milliseconds, timer: GlobalTimer) {
@@ -172,12 +184,17 @@ pub mod drtio {
         loop {
             if timer.get_time() > max_time {
                 return;
-            } //could this be cut short?
+            }
             let _ = drtioaux_async::recv(linkno).await;
         }
     }
 
-    async fn ping_remote(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, timer: GlobalTimer) -> u32 {
+    async fn ping_remote(
+        aux_mutex: &Rc<Mutex<bool>>,
+        linkno: u8,
+        routing_table: &RoutingTable,
+        timer: GlobalTimer,
+    ) -> u32 {
         let mut count = 0;
         loop {
             if !link_rx_up(linkno).await {
@@ -187,7 +204,7 @@ pub mod drtio {
             if count > 100 {
                 return 0;
             }
-            let reply = aux_transact(aux_mutex, linkno, &Packet::EchoRequest, timer).await;
+            let reply = aux_transact(aux_mutex, linkno, routing_table, &Packet::EchoRequest, timer).await;
             match reply {
                 Ok(Packet::EchoReply) => {
                     // make sure receive buffer is drained
@@ -200,7 +217,7 @@ pub mod drtio {
         }
     }
 
-    async fn sync_tsc(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, timer: GlobalTimer) -> Result<(), &'static str> {
+    async fn sync_tsc(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, timer: GlobalTimer) -> Result<(), Error> {
         let _lock = aux_mutex.async_lock().await;
 
         unsafe {
@@ -211,22 +228,23 @@ pub mod drtio {
         // by the satellite, in response to a TSC set on the RT link.
         let reply = recv_aux_timeout(linkno, 10000, timer).await?;
         if reply == Packet::TSCAck {
-            return Ok(());
+            Ok(())
         } else {
-            return Err("unexpected reply");
+            Err(Error::UnexpectedReply)
         }
     }
 
     async fn load_routing_table(
         aux_mutex: &Rc<Mutex<bool>>,
         linkno: u8,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         for i in 0..drtio_routing::DEST_COUNT {
             let reply = aux_transact(
                 aux_mutex,
                 linkno,
+                routing_table,
                 &Packet::RoutingSetPath {
                     destination: i as u8,
                     hops: routing_table.0[i],
@@ -235,7 +253,7 @@ pub mod drtio {
             )
             .await?;
             if reply != Packet::RoutingAck {
-                return Err("unexpected reply");
+                return Err(Error::UnexpectedReply);
             }
         }
         Ok(())
@@ -245,13 +263,21 @@ pub mod drtio {
         aux_mutex: &Rc<Mutex<bool>>,
         linkno: u8,
         rank: u8,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
-        let reply = aux_transact(aux_mutex, linkno, &Packet::RoutingSetRank { rank: rank }, timer).await?;
+        let reply = aux_transact(
-        if reply != Packet::RoutingAck {
+            aux_mutex,
-            return Err("unexpected reply");
+            linkno,
+            routing_table,
+            &Packet::RoutingSetRank { rank: rank },
+            timer,
+        )
+        .await?;
+        match reply {
+            Packet::RoutingAck => Ok(()),
+            _ => Err(Error::UnexpectedReply),
         }
-        Ok(())
     }
 
     async fn init_buffer_space(destination: u8, linkno: u8) {
@@ -270,10 +296,14 @@ pub mod drtio {
         }
     }
 
-    async fn process_unsolicited_aux(aux_mutex: &Rc<Mutex<bool>>, linkno: u8) {
+    async fn process_unsolicited_aux(aux_mutex: &Mutex<bool>, linkno: u8, routing_table: &RoutingTable) {
         let _lock = aux_mutex.async_lock().await;
         match drtioaux_async::recv(linkno).await {
-            Ok(Some(packet)) => warn!("[LINK#{}] unsolicited aux packet: {:?}", linkno, packet),
+            Ok(Some(packet)) => {
+                if let Some(packet) = process_async_packets(linkno, routing_table, packet).await {
+                    warn!("[LINK#{}] unsolicited aux packet: {:?}", linkno, packet);
+                }
+            }
             Ok(None) => (),
             Err(_) => warn!("[LINK#{}] aux packet error", linkno),
         }
@@ -301,7 +331,7 @@ pub mod drtio {
     }
 
     async fn destination_set_up(
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         destination: u8,
         up: bool,
@@ -324,7 +354,7 @@ pub mod drtio {
 
     async fn destination_survey(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         up_links: &[bool],
         up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         timer: GlobalTimer,
@@ -345,6 +375,7 @@ pub mod drtio {
                         let reply = aux_transact(
                             aux_mutex,
                             linkno,
+                            routing_table,
                             &Packet::DestinationStatusRequest {
                                 destination: destination,
                             },
@@ -400,6 +431,7 @@ pub mod drtio {
                         let reply = aux_transact(
                             aux_mutex,
                             linkno,
+                            routing_table,
                             &Packet::DestinationStatusRequest {
                                 destination: destination,
                             },
@@ -427,7 +459,7 @@ pub mod drtio {
 
     pub async fn link_task(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         timer: GlobalTimer,
     ) {
@@ -438,8 +470,7 @@ pub mod drtio {
                 if up_links[linkno as usize] {
                     /* link was previously up */
                     if link_rx_up(linkno).await {
-                        process_async_packets(aux_mutex, linkno, routing_table, timer).await;
+                        process_unsolicited_aux(aux_mutex, linkno, routing_table).await;
-                        process_unsolicited_aux(aux_mutex, linkno).await;
                         process_local_errors(linkno).await;
                     } else {
                         info!("[LINK#{}] link is down", linkno);
@@ -449,7 +480,7 @@ pub mod drtio {
                     /* link was previously down */
                     if link_rx_up(linkno).await {
                         info!("[LINK#{}] link RX became up, pinging", linkno);
-                        let ping_count = ping_remote(aux_mutex, linkno, timer).await;
+                        let ping_count = ping_remote(aux_mutex, linkno, routing_table, timer).await;
                         if ping_count > 0 {
                             info!("[LINK#{}] remote replied after {} packets", linkno, ping_count);
                             up_links[linkno as usize] = true;
@@ -459,7 +490,7 @@ pub mod drtio {
                             if let Err(e) = load_routing_table(aux_mutex, linkno, routing_table, timer).await {
                                 error!("[LINK#{}] failed to load routing table ({})", linkno, e);
                             }
-                            if let Err(e) = set_rank(aux_mutex, linkno, 1 as u8, timer).await {
+                            if let Err(e) = set_rank(aux_mutex, linkno, 1 as u8, routing_table, timer).await {
                                 error!("[LINK#{}] failed to set rank ({})", linkno, e);
                             }
                             info!("[LINK#{}] link initialization completed", linkno);
@@ -476,7 +507,7 @@ pub mod drtio {
     }
 
     #[allow(dead_code)]
-    pub fn reset(aux_mutex: Rc<Mutex<bool>>, mut timer: GlobalTimer) {
+    pub fn reset(aux_mutex: Rc<Mutex<bool>>, routing_table: &RoutingTable, mut timer: GlobalTimer) {
         for linkno in 0..csr::DRTIO.len() {
             unsafe {
                 (csr::DRTIO[linkno].reset_write)(1);
@@ -492,7 +523,13 @@ pub mod drtio {
         for linkno in 0..csr::DRTIO.len() {
             let linkno = linkno as u8;
             if task::block_on(link_rx_up(linkno)) {
-                let reply = task::block_on(aux_transact(&aux_mutex, linkno, &Packet::ResetRequest, timer));
+                let reply = task::block_on(aux_transact(
+                    &aux_mutex,
+                    linkno,
+                    routing_table,
+                    &Packet::ResetRequest,
+                    timer,
+                ));
                 match reply {
                     Ok(Packet::ResetAck) => (),
                     Ok(_) => error!("[LINK#{}] reset failed, received unexpected aux packet", linkno),
@@ -505,14 +542,15 @@ pub mod drtio {
     async fn partition_data<PacketF, HandlerF>(
         linkno: u8,
         aux_mutex: &Rc<Mutex<bool>>,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         data: &[u8],
         packet_f: PacketF,
         reply_handler_f: HandlerF,
-    ) -> Result<(), &'static str>
+    ) -> Result<(), Error>
     where
         PacketF: Fn(&[u8; MASTER_PAYLOAD_MAX_SIZE], PayloadStatus, usize) -> Packet,
-        HandlerF: Fn(&Packet) -> Result<(), &'static str>,
+        HandlerF: Fn(&Packet) -> Result<(), Error>,
     {
         let mut i = 0;
         while i < data.len() {
@@ -528,7 +566,7 @@ pub mod drtio {
             i += len;
             let status = PayloadStatus::from_status(first, last);
             let packet = packet_f(&slice, status, len);
-            let reply = aux_transact(aux_mutex, linkno, &packet, timer).await?;
+            let reply = aux_transact(aux_mutex, linkno, routing_table, &packet, timer).await?;
             reply_handler_f(&reply)?;
         }
         Ok(())
@@ -536,16 +574,17 @@ pub mod drtio {
 
     pub async fn ddma_upload_trace(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         id: u32,
         destination: u8,
         trace: &Vec<u8>,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         partition_data(
             linkno,
             aux_mutex,
+            routing_table,
             timer,
             trace,
             |slice, status, len| Packet::DmaAddTraceRequest {
@@ -566,8 +605,8 @@ pub mod drtio {
                     destination: 0,
                     succeeded: false,
                     ..
-                } => Err("error adding trace on satellite"),
+                } => Err(Error::DmaAddTraceFail(destination)),
-                _ => Err("adding DMA trace failed, unexpected aux packet"),
+                _ => Err(Error::UnexpectedReply),
             },
         )
         .await
@@ -575,15 +614,16 @@ pub mod drtio {
 
     pub async fn ddma_send_erase(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         id: u32,
         destination: u8,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         let reply = aux_transact(
             aux_mutex,
             linkno,
+            routing_table,
             &Packet::DmaRemoveTraceRequest {
                 id: id,
                 source: 0,
@@ -591,33 +631,33 @@ pub mod drtio {
             },
             timer,
         )
-        .await;
+        .await?;
         match reply {
-            Ok(Packet::DmaRemoveTraceReply {
+            Packet::DmaRemoveTraceReply {
                 destination: 0,
                 succeeded: true,
-            }) => Ok(()),
+            } => Ok(()),
-            Ok(Packet::DmaRemoveTraceReply {
+            Packet::DmaRemoveTraceReply {
                 destination: 0,
                 succeeded: false,
-            }) => Err("satellite DMA erase error"),
+            } => Err(Error::DmaEraseFail(destination)),
-            Ok(_) => Err("adding trace failed, unexpected aux packet"),
+            _ => Err(Error::UnexpectedReply),
-            Err(_) => Err("erasing trace failed, aux error"),
         }
     }
 
     pub async fn ddma_send_playback(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         id: u32,
         destination: u8,
         timestamp: u64,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         let reply = aux_transact(
             aux_mutex,
             linkno,
+            routing_table,
             &Packet::DmaPlaybackRequest {
                 id: id,
                 source: 0,
@@ -626,45 +666,44 @@ pub mod drtio {
             },
             timer,
         )
-        .await;
+        .await?;
         match reply {
-            Ok(Packet::DmaPlaybackReply {
+            Packet::DmaPlaybackReply {
                 destination: 0,
                 succeeded: true,
-            }) => Ok(()),
+            } => Ok(()),
-            Ok(Packet::DmaPlaybackReply {
+            Packet::DmaPlaybackReply {
                 destination: 0,
                 succeeded: false,
-            }) => Err("error on DMA playback request"),
+            } => Err(Error::DmaPlaybackFail(destination)),
-            Ok(_) => Err("received unexpected aux packet during DMA playback"),
+            _ => Err(Error::UnexpectedReply),
-            Err(_) => Err("aux error on DMA playback"),
         }
     }
 
     async fn analyzer_get_data(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         destination: u8,
-    ) -> Result<RemoteBuffer, &'static str> {
+    ) -> Result<RemoteBuffer, Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         let reply = aux_transact(
             aux_mutex,
             linkno,
+            routing_table,
             &Packet::AnalyzerHeaderRequest {
                 destination: destination,
             },
             timer,
         )
-        .await;
+        .await?;
         let (sent, total, overflow) = match reply {
-            Ok(Packet::AnalyzerHeader {
+            Packet::AnalyzerHeader {
                 sent_bytes,
                 total_byte_count,
                 overflow_occurred,
-            }) => (sent_bytes, total_byte_count, overflow_occurred),
+            } => (sent_bytes, total_byte_count, overflow_occurred),
-            Ok(_) => return Err("received unexpected aux packet during remote analyzer header request"),
+            _ => return Err(Error::UnexpectedReply),
-            Err(e) => return Err(e),
         };
 
         let mut remote_data: Vec<u8> = Vec::new();
@@ -674,19 +713,19 @@ pub mod drtio {
                 let reply = aux_transact(
                     aux_mutex,
                     linkno,
+                    routing_table,
                     &Packet::AnalyzerDataRequest {
                         destination: destination,
                     },
                     timer,
                 )
-                .await;
+                .await?;
                 match reply {
-                    Ok(Packet::AnalyzerData { last, length, data }) => {
+                    Packet::AnalyzerData { last, length, data } => {
                         last_packet = last;
                         remote_data.extend(&data[0..length as usize]);
                     }
-                    Ok(_) => return Err("received unexpected aux packet during remote analyzer data request"),
+                    _ => return Err(Error::UnexpectedReply),
-                    Err(e) => return Err(e),
                 }
             }
         }
@@ -701,10 +740,10 @@ pub mod drtio {
 
     pub async fn analyzer_query(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         timer: GlobalTimer,
-    ) -> Result<Vec<RemoteBuffer>, &'static str> {
+    ) -> Result<Vec<RemoteBuffer>, Error> {
         let mut remote_buffers: Vec<RemoteBuffer> = Vec::new();
         for i in 1..drtio_routing::DEST_COUNT {
             if destination_up(up_destinations, i as u8).await {
@@ -716,16 +755,17 @@ pub mod drtio {
 
     pub async fn subkernel_upload(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         id: u32,
         destination: u8,
         data: &Vec<u8>,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         partition_data(
             linkno,
             aux_mutex,
+            routing_table,
             timer,
             data,
             |slice, status, len| Packet::SubkernelAddDataRequest {
@@ -737,8 +777,8 @@ pub mod drtio {
             },
             |reply| match reply {
                 Packet::SubkernelAddDataReply { succeeded: true } => Ok(()),
-                Packet::SubkernelAddDataReply { succeeded: false } => Err("error adding subkernel on satellite"),
+                Packet::SubkernelAddDataReply { succeeded: false } => Err(Error::SubkernelAddFail(destination)),
-                _ => Err("adding subkernel failed, unexpected aux packet"),
+                _ => Err(Error::UnexpectedReply),
             },
         )
         .await
@@ -746,16 +786,17 @@ pub mod drtio {
 
     pub async fn subkernel_load(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         id: u32,
         destination: u8,
         run: bool,
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         let reply = aux_transact(
             aux_mutex,
             linkno,
+            routing_table,
             &Packet::SubkernelLoadRunRequest {
                 id: id,
                 source: 0,
@@ -773,23 +814,24 @@ pub mod drtio {
             Packet::SubkernelLoadRunReply {
                 destination: 0,
                 succeeded: false,
-            } => return Err("error on subkernel run request"),
+            } => return Err(Error::SubkernelRunFail(destination)),
-            _ => return Err("received unexpected aux packet during subkernel run"),
+            _ => Err(Error::UnexpectedReply),
         }
     }
 
     pub async fn subkernel_retrieve_exception(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         destination: u8,
-    ) -> Result<Vec<u8>, &'static str> {
+    ) -> Result<Vec<u8>, Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         let mut remote_data: Vec<u8> = Vec::new();
         loop {
             let reply = aux_transact(
                 aux_mutex,
                 linkno,
+                routing_table,
                 &Packet::SubkernelExceptionRequest {
                     destination: destination,
                 },
@@ -803,23 +845,24 @@ pub mod drtio {
                         return Ok(remote_data);
                     }
                 }
-                _ => return Err("received unexpected aux packet during subkernel exception request"),
+                _ => return Err(Error::UnexpectedReply),
             }
         }
     }
 
     pub async fn subkernel_send_message(
         aux_mutex: &Rc<Mutex<bool>>,
-        routing_table: &drtio_routing::RoutingTable,
+        routing_table: &RoutingTable,
         timer: GlobalTimer,
         id: u32,
         destination: u8,
         message: &[u8],
-    ) -> Result<(), &'static str> {
+    ) -> Result<(), Error> {
         let linkno = routing_table.0[destination as usize][0] - 1;
         partition_data(
             linkno,
             aux_mutex,
+            routing_table,
             timer,
             message,
             |slice, status, len| Packet::SubkernelMessage {
@@ -832,7 +875,7 @@ pub mod drtio {
             },
             |reply| match reply {
                 Packet::SubkernelMessageAck { .. } => Ok(()),
-                _ => Err("sending message to subkernel failed, unexpected aux packet"),
+                _ => Err(Error::UnexpectedReply),
             },
         )
         .await
@@ -845,19 +888,19 @@ pub mod drtio {
 
     pub fn startup(
         _aux_mutex: &Rc<Mutex<bool>>,
-        _routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>,
+        _routing_table: &Rc<RefCell<RoutingTable>>,
         _up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
         _timer: GlobalTimer,
     ) {
     }
 
     #[allow(dead_code)]
-    pub fn reset(_aux_mutex: Rc<Mutex<bool>>, mut _timer: GlobalTimer) {}
+    pub fn reset(_aux_mutex: Rc<Mutex<bool>>, _routing_table: &RoutingTable, mut _timer: GlobalTimer) {}
 }
 
 pub fn startup(
     aux_mutex: &Rc<Mutex<bool>>,
-    routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>,
+    routing_table: &Rc<RefCell<RoutingTable>>,
     up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
     timer: GlobalTimer,
 ) {
@@ -868,9 +911,9 @@ pub fn startup(
 }
 
 #[allow(dead_code)]
-pub fn reset(aux_mutex: Rc<Mutex<bool>>, timer: GlobalTimer) {
+pub fn reset(aux_mutex: Rc<Mutex<bool>>, routing_table: &RoutingTable, timer: GlobalTimer) {
     unsafe {
         csr::rtio_core::reset_write(1);
     }
-    drtio::reset(aux_mutex, timer)
+    drtio::reset(aux_mutex, routing_table, timer)
 }

src/runtime/src/subkernel.rs

@@ -7,7 +7,7 @@ use libboard_zynq::{time::Milliseconds, timer::GlobalTimer};
 use libcortex_a9::mutex::Mutex;
 use log::{error, warn};
 
-use crate::rtio_mgt::drtio;
+use crate::rtio_mgt::{drtio, drtio::Error as DrtioError};
 
 #[derive(Debug, PartialEq, Clone, Copy)]
 pub enum FinishStatus {
@@ -31,11 +31,11 @@ pub enum Error {
     SubkernelNotFound,
     SubkernelException,
     CommLost,
-    DrtioError(&'static str),
+    DrtioError(DrtioError),
 }
 
-impl From<&'static str> for Error {
+impl From<DrtioError> for Error {
-    fn from(value: &'static str) -> Error {
+    fn from(value: DrtioError) -> Error {
         Error::DrtioError(value)
     }
 }

src/satman/Cargo.toml.tpl

@@ -7,6 +7,7 @@ build = "build.rs"
 [features]
 target_zc706 = ["libboard_zynq/target_zc706", "libsupport_zynq/target_zc706", "libconfig/target_zc706", "libboard_artiq/target_zc706"]
 target_kasli_soc = ["libboard_zynq/target_kasli_soc", "libsupport_zynq/target_kasli_soc", "libconfig/target_kasli_soc", "libboard_artiq/target_kasli_soc"]
+calibrate_wrpll_skew = ["libboard_artiq/calibrate_wrpll_skew"]
 default = ["target_zc706", ]
 
 [build-dependencies]

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 });
@@ -74,12 +81,6 @@ fn drtiosat_tsc_loaded() -> bool {
     }
 }
 
-fn drtiosat_async_ready() {
-    unsafe {
-        csr::drtiosat::async_messages_ready_write(1);
-    }
-}
-
 #[cfg(has_drtio_routing)]
 macro_rules! forward {
     ($routing_table:expr, $destination:expr, $rank:expr, $repeaters:expr, $packet:expr, $timer:expr) => {{
@@ -238,14 +239,6 @@ fn process_aux_packet(
         #[cfg(not(has_drtio_routing))]
         drtioaux::Packet::RoutingSetRank { rank: _ } => drtioaux::send(0, &drtioaux::Packet::RoutingAck),
 
-        drtioaux::Packet::RoutingRetrievePackets => {
-            let packet = router
-                .get_upstream_packet()
-                .or(Some(drtioaux::Packet::RoutingNoPackets))
-                .unwrap();
-            drtioaux::send(0, &packet)
-        }
-
         drtioaux::Packet::MonitorRequest {
             destination: _destination,
             channel,
@@ -823,10 +816,43 @@ 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,
+    },
+    helper: si549::DividerConfig {
+        // 125MHz*32767/32768
+        hsdiv: 0x058,
+        lsdiv: 0,
+        fbdiv: 0x04814E8F442,
+    },
+};
+
+#[cfg(all(has_si549, rtio_frequency = "100.0"))]
+const SI549_SETTINGS: si549::FrequencySetting = si549::FrequencySetting {
+    main: si549::DividerConfig {
+        hsdiv: 0x06C,
+        lsdiv: 0,
+        fbdiv: 0x046C5F49797,
+    },
+    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 +882,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 +898,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 +917,8 @@ pub extern "C" fn main_core0() -> i32 {
     unsafe {
         csr::gt_drtio::txenable_write(0xffffffffu32 as _);
     }
+    #[cfg(has_si549)]
+    si549::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 +964,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.
@@ -1015,8 +1053,8 @@ pub extern "C" fn main_core0() -> i32 {
                 }
             }
 
-            if router.any_upstream_waiting() {
+            if let Some(packet) = router.get_upstream_packet() {
-                drtiosat_async_ready();
+                drtioaux::send(0, &packet).unwrap();
             }
         }
 
@@ -1026,6 +1064,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);
     }
 }
 

src/satman/src/repeater.rs

@@ -119,16 +119,11 @@ impl Repeater {
                 }
             }
             RepeaterState::Up => {
-                self.process_unsolicited_aux();
+                self.process_unsolicited_aux(routing_table, rank, destination, router);
                 if !rep_link_rx_up(self.repno) {
                     info!("[REP#{}] link is down", self.repno);
                     self.state = RepeaterState::Down;
                 }
-                if self.async_messages_ready() {
-                    if let Err(e) = self.handle_async(routing_table, rank, destination, router, timer) {
-                        warn!("[REP#{}] Error handling async messages ({:?})", self.repno, e);
-                    }
-                }
             }
             RepeaterState::Failed => {
                 if !rep_link_rx_up(self.repno) {
@@ -139,9 +134,15 @@ impl Repeater {
         }
     }
 
-    fn process_unsolicited_aux(&self) {
+    fn process_unsolicited_aux(
+        &self,
+        routing_table: &drtio_routing::RoutingTable,
+        rank: u8,
+        destination: u8,
+        router: &mut Router,
+    ) {
         match drtioaux::recv(self.auxno) {
-            Ok(Some(packet)) => warn!("[REP#{}] unsolicited aux packet: {:?}", self.repno, packet),
+            Ok(Some(packet)) => router.route(packet, routing_table, rank, destination),
             Ok(None) => (),
             Err(_) => warn!("[REP#{}] aux packet error", self.repno),
         }
@@ -186,34 +187,6 @@ impl Repeater {
         }
     }
 
-    fn async_messages_ready(&self) -> bool {
-        let async_rdy;
-        unsafe {
-            async_rdy = (csr::DRTIOREP[self.repno as usize].async_messages_ready_read)();
-            (csr::DRTIOREP[self.repno as usize].async_messages_ready_write)(0);
-        }
-        async_rdy == 1
-    }
-
-    fn handle_async(
-        &self,
-        routing_table: &drtio_routing::RoutingTable,
-        rank: u8,
-        self_destination: u8,
-        router: &mut Router,
-        timer: &mut GlobalTimer,
-    ) -> Result<(), drtioaux::Error> {
-        loop {
-            drtioaux::send(self.auxno, &drtioaux::Packet::RoutingRetrievePackets).unwrap();
-            let reply = self.recv_aux_timeout(200, timer)?;
-            match reply {
-                drtioaux::Packet::RoutingNoPackets => break,
-                packet => router.route(packet, routing_table, rank, self_destination),
-            }
-        }
-        Ok(())
-    }
-
     fn recv_aux_timeout(&self, timeout: u32, timer: &mut GlobalTimer) -> Result<drtioaux::Packet, drtioaux::Error> {
         let max_time = timer.get_time() + Milliseconds(timeout.into());
         loop {

src/satman/src/routing.rs

@@ -75,7 +75,6 @@ pub struct Router {
     local_queue: VecDeque<drtioaux::Packet>,
     #[cfg(has_drtio_routing)]
     downstream_queue: VecDeque<(usize, drtioaux::Packet)>,
-    upstream_notified: bool,
 }
 
 impl Router {
@@ -85,7 +84,6 @@ impl Router {
             local_queue: VecDeque::new(),
             #[cfg(has_drtio_routing)]
             downstream_queue: VecDeque::new(),
-            upstream_notified: false,
         }
     }
 
@@ -161,22 +159,8 @@ impl Router {
         }
     }
 
-    pub fn any_upstream_waiting(&mut self) -> bool {
-        let empty = self.upstream_queue.is_empty();
-        if !empty && !self.upstream_notified {
-            self.upstream_notified = true; // so upstream will not get spammed with notifications
-            true
-        } else {
-            false
-        }
-    }
-
     pub fn get_upstream_packet(&mut self) -> Option<drtioaux::Packet> {
-        let packet = self.upstream_queue.pop_front();
+        self.upstream_queue.pop_front()
-        if packet.is_none() {
-            self.upstream_notified = false;
-        }
-        packet
     }
 
     #[cfg(has_drtio_routing)]