cxp downconn firmware: GTX setup

testing: add loopmode mode & tsusrclk mmcm drp bitbang
testing: add IDLE word printout
downconn: add QPLL and GTX setup
downconn: add DRP to support all CXP linerate up to 12.5Gbps

flake.lock

@@ -3,19 +3,19 @@
     "artiq": {
       "inputs": {
         "artiq-comtools": "artiq-comtools",
-        "mozilla-overlay": "mozilla-overlay",
         "nixpkgs": "nixpkgs",
+        "rust-overlay": "rust-overlay",
         "sipyco": "sipyco",
         "src-migen": "src-migen",
         "src-misoc": "src-misoc",
         "src-pythonparser": "src-pythonparser"
       },
       "locked": {
-        "lastModified": 1716972728,
+        "lastModified": 1724210813,
-        "narHash": "sha256-88J+eckZamtwhcCQkPpKLu6R1hmgj5+C9n2U5i+sHUE=",
+        "narHash": "sha256-OqQdE2lC0jKNS2fFq0Fda1nBpyT8ijmSXqdkO8xeOJ8=",
         "ref": "refs/heads/master",
-        "rev": "49e402780bebba437c6098047ab1dc68eaf5a17c",
+        "rev": "61e96b37f9c4345e2d7bf71d47ba0b5e947de83e",
-        "revCount": 8808,
+        "revCount": 8985,
         "type": "git",
         "url": "https://github.com/m-labs/artiq.git"
       },
@@ -37,11 +37,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1707216368,
+        "lastModified": 1720768567,
-        "narHash": "sha256-ZXoqzG2QsVsybALLYXs473avXcyKSZNh2kIgcPo60XQ=",
+        "narHash": "sha256-3VoK7o5MtHtbHLrc6Pv+eQWFtaz5Gd/YWyV5TD3c5Ss=",
         "owner": "m-labs",
         "repo": "artiq-comtools",
-        "rev": "e5d0204490bccc07ef9141b0d7c405ab01cb8273",
+        "rev": "f93570d8f2ed5a3cfb3e1c16ab00f2540551e994",
         "type": "github"
       },
       "original": {
@@ -55,11 +55,11 @@
         "systems": "systems"
       },
       "locked": {
-        "lastModified": 1694529238,
+        "lastModified": 1710146030,
-        "narHash": "sha256-zsNZZGTGnMOf9YpHKJqMSsa0dXbfmxeoJ7xHlrt+xmY=",
+        "narHash": "sha256-SZ5L6eA7HJ/nmkzGG7/ISclqe6oZdOZTNoesiInkXPQ=",
         "owner": "numtide",
         "repo": "flake-utils",
-        "rev": "ff7b65b44d01cf9ba6a71320833626af21126384",
+        "rev": "b1d9ab70662946ef0850d488da1c9019f3a9752a",
         "type": "github"
       },
       "original": {
@@ -100,34 +100,18 @@
         "type": "github"
       }
     },
-    "mozilla-overlay_3": {
-      "flake": false,
-      "locked": {
-        "lastModified": 1704373101,
-        "narHash": "sha256-+gi59LRWRQmwROrmE1E2b3mtocwueCQqZ60CwLG+gbg=",
-        "owner": "mozilla",
-        "repo": "nixpkgs-mozilla",
-        "rev": "9b11a87c0cc54e308fa83aac5b4ee1816d5418a2",
-        "type": "github"
-      },
-      "original": {
-        "owner": "mozilla",
-        "repo": "nixpkgs-mozilla",
-        "type": "github"
-      }
-    },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1716542732,
+        "lastModified": 1723362943,
-        "narHash": "sha256-0Y9fRr0CUqWT4KgBITmaGwlnNIGMYuydu2L8iLTfHU4=",
+        "narHash": "sha256-dFZRVSgmJkyM0bkPpaYRtG/kRMRTorUIDj8BxoOt1T4=",
         "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "d12251ef6e8e6a46e05689eeccd595bdbd3c9e60",
+        "rev": "a58bc8ad779655e790115244571758e8de055e3d",
         "type": "github"
       },
       "original": {
         "owner": "NixOS",
-        "ref": "nixos-24.05",
+        "ref": "nixos-unstable",
         "repo": "nixpkgs",
         "type": "github"
       }
@@ -135,10 +119,32 @@
     "root": {
       "inputs": {
         "artiq": "artiq",
-        "mozilla-overlay": "mozilla-overlay_2",
+        "mozilla-overlay": "mozilla-overlay",
         "zynq-rs": "zynq-rs"
       }
     },
+    "rust-overlay": {
+      "inputs": {
+        "nixpkgs": [
+          "artiq",
+          "nixpkgs"
+        ]
+      },
+      "locked": {
+        "lastModified": 1719454714,
+        "narHash": "sha256-MojqG0lyUINkEk0b3kM2drsU5vyaF8DFZe/FAlZVOGs=",
+        "owner": "oxalica",
+        "repo": "rust-overlay",
+        "rev": "d1c527659cf076ecc4b96a91c702d080b213801e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "oxalica",
+        "ref": "snapshot/2024-08-01",
+        "repo": "rust-overlay",
+        "type": "github"
+      }
+    },
     "sipyco": {
       "inputs": {
         "nixpkgs": [
@@ -147,11 +153,11 @@
         ]
       },
       "locked": {
-        "lastModified": 1708937641,
+        "lastModified": 1717637367,
-        "narHash": "sha256-Hkb9VYFzFgkYxfbh4kYcDSn7DbMUYehoQDeTALrxo2Q=",
+        "narHash": "sha256-4mSm9wl5EMgzzrW6w86IDUevkEOT99FESHGcxcyQbD0=",
         "owner": "m-labs",
         "repo": "sipyco",
-        "rev": "4a28b311ce0069454b4e8fe1e6049db11b9f1296",
+        "rev": "02b96ec2473a3c3d3c980899de2564ddce949dab",
         "type": "github"
       },
       "original": {
@@ -163,11 +169,11 @@
     "src-migen": {
       "flake": false,
       "locked": {
-        "lastModified": 1715484909,
+        "lastModified": 1721561053,
-        "narHash": "sha256-4DCHBUBfc/VA+7NW2Hr0+JP4NnKPru2uVJyZjCCk0Ws=",
+        "narHash": "sha256-z3LRhNmKZrjr6rFD0yxtccSa/SWvFIYmb+G/D5d2Jd8=",
         "owner": "m-labs",
         "repo": "migen",
-        "rev": "4790bb577681a8c3a8d226bc196a4e5deb39e4df",
+        "rev": "9279e8623f8433bc4f23ac51e5e2331bfe544417",
         "type": "github"
       },
       "original": {
@@ -227,18 +233,18 @@
     },
     "zynq-rs": {
       "inputs": {
-        "mozilla-overlay": "mozilla-overlay_3",
+        "mozilla-overlay": "mozilla-overlay_2",
         "nixpkgs": [
           "artiq",
           "nixpkgs"
         ]
       },
       "locked": {
-        "lastModified": 1716519432,
+        "lastModified": 1720537402,
-        "narHash": "sha256-vgKBJCQRPCutJ4n+FtJNczMZULWW7J3B8icf/PUothw=",
+        "narHash": "sha256-ybvaQ48SVBqYVqgYmGUdefGZkni7PJ90qYQPHnFOwDs=",
         "ref": "refs/heads/master",
-        "rev": "46dc25b89e46b9043129d77e3c9348916748e325",
+        "rev": "b2b3e5c933cbc4b7cb14adde480d7561a3ae71ee",
-        "revCount": 645,
+        "revCount": 648,
         "type": "git",
         "url": "https://git.m-labs.hk/m-labs/zynq-rs"
       },

flake.nix

@@ -18,11 +18,11 @@
 
     fastnumbers = pkgs.python3Packages.buildPythonPackage rec {
       pname = "fastnumbers";
-      version = "2.2.1";
+      version = "5.1.0";
 
       src = pkgs.python3Packages.fetchPypi {
         inherit pname version;
-        sha256 = "0j15i54p7nri6hkzn1wal9pxri4pgql01wgjccig6ar0v5jjbvsy";
+        sha256 = "sha256-4JLTP4uVwxcaL7NOV57+DFSwKQ3X+W/6onYkN2AdkKc=";
       };
     };
 
@@ -126,6 +126,7 @@
           lockFile = src/Cargo.lock;
           outputHashes = {
               "tar-no-std-0.1.8" = "sha256-xm17108v4smXOqxdLvHl9CxTCJslmeogjm4Y87IXFuM=";
+              "nalgebra-0.32.6" = "sha256-L/YudkVOtfGYoNQKBD7LMk/sMYgRDzPDdpGL5rO7G2I=";
             };
         };
 
@@ -341,6 +342,7 @@
       {
         inherit fastnumbers artiq-netboot ramda migen-axi binutils-arm;
       } //
+      (board-package-set { target = "zc706"; variant = "cxp_demo"; }) //
       (board-package-set { target = "zc706"; variant = "nist_clock"; }) //
       (board-package-set { target = "zc706"; variant = "nist_clock_master"; }) //
       (board-package-set { target = "zc706"; variant = "nist_clock_master_100mhz"; }) //

src/Cargo.lock

@@ -2,6 +2,15 @@
 # It is not intended for manual editing.
 version = 3
 
+[[package]]
+name = "approx"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
+dependencies = [
+ "num-traits",
+]
+
 [[package]]
 name = "arrayvec"
 version = "0.7.4"
@@ -246,6 +255,7 @@ dependencies = [
  "libsupport_zynq",
  "log",
  "log_buffer",
+ "nalgebra",
  "nb 0.1.3",
  "unwind",
  "vcell",
@@ -382,6 +392,19 @@ version = "0.7.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577"
 
+[[package]]
+name = "nalgebra"
+version = "0.32.6"
+source = "git+https://git.m-labs.hk/M-Labs/nalgebra.git?rev=dd00f9b#dd00f9b46046e0b931d1b470166db02fd29591be"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-rational",
+ "num-traits",
+ "simba",
+ "typenum",
+]
+
 [[package]]
 name = "nb"
 version = "0.1.3"
@@ -397,6 +420,15 @@ version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"
 
+[[package]]
+name = "num-complex"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "26873667bbbb7c5182d4a37c1add32cdf09f841af72da53318fdb81543c15085"
+dependencies = [
+ "num-traits",
+]
+
 [[package]]
 name = "num-derive"
 version = "0.3.3"
@@ -408,6 +440,26 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "num-integer"
+version = "0.1.46"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-rational"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d41702bd167c2df5520b384281bc111a4b5efcf7fbc4c9c222c815b07e0a6a6a"
+dependencies = [
+ "autocfg",
+ "num-integer",
+ "num-traits",
+]
+
 [[package]]
 name = "num-traits"
 version = "0.2.15"
@@ -415,8 +467,15 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd"
 dependencies = [
  "autocfg",
+ "libm",
 ]
 
+[[package]]
+name = "paste"
+version = "1.0.15"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
+
 [[package]]
 name = "pin-project-lite"
 version = "0.2.9"
@@ -523,6 +582,18 @@ version = "0.1.20"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d4f410fedcf71af0345d7607d246e7ad15faaadd49d240ee3b24e5dc21a820ac"
 
+[[package]]
+name = "simba"
+version = "0.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "50582927ed6f77e4ac020c057f37a268fc6aebc29225050365aacbb9deeeddc4"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-traits",
+ "paste",
+]
+
 [[package]]
 name = "smoltcp"
 version = "0.7.5"
@@ -555,6 +626,12 @@ dependencies = [
  "log",
 ]
 
+[[package]]
+name = "typenum"
+version = "1.17.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825"
+
 [[package]]
 name = "unicode-ident"
 version = "1.0.5"

src/gateware/cxp.py

@@ -0,0 +1,74 @@
+from migen import *
+from misoc.interconnect.csr import *
+from misoc.cores.liteeth_mini.mac.crc import LiteEthMACCRCEngine
+
+from cxp_downconn import CXP_DownConn
+from cxp_upconn import CXP_UpConn
+
+class CXP(Module, AutoCSR):
+    def __init__(self, refclk, pads, sys_clk_freq, debug_sma, pmod_pads):
+        self.submodules.crc = CXP_CRC(8)
+        # FIFOs with transmission priority
+        # 0: Trigger packet
+        # 1: IO acknowledgment for trigger packet
+        # 2: All other packets
+        self.submodules.upconn = CXP_UpConn(debug_sma, sys_clk_freq, pmod_pads)
+
+        self.submodules.downconn = CXP_DownConn(refclk, pads, sys_clk_freq, debug_sma, pmod_pads)
+
+
+class CXP_CRC(Module, AutoCSR):
+    width = 32
+    polynom = 0x04C11DB7
+    seed = 2**width-1
+    def __init__(self, data_width):
+        self.d = Signal(data_width)
+        self.stb = Signal()
+        self.reset = Signal()
+        self.val = Signal(self.width, reset=self.seed)
+
+        self.data = CSR(data_width)
+        self.en = CSR()
+        self.value = CSRStatus(self.width)
+        self.processed = CSRStatus(self.width)
+
+        # # #
+        
+        self.submodules.engine = LiteEthMACCRCEngine(data_width, self.width, self.polynom)
+
+        self.sync += [
+            self.val.eq(self.engine.next),
+            If(self.stb,
+                self.engine.data.eq(self.d), 
+
+                If(self.reset,
+                    self.engine.last.eq(self.seed), 
+                    # clear reset bit
+                    self.reset.eq(0),
+                ).Else(
+                    self.engine.last.eq(self.val), 
+                )
+            ),
+        ]
+
+        # DEBUG: remove those csr
+        # TODO: do char bit reverse outside of this submodule
+
+        p0 = Signal(8)
+        p1 = Signal(8)
+        p2 = Signal(8)
+        p3 = Signal(8)
+        self.comb += [
+            p3.eq(self.engine.next[:8][::-1]),
+            p2.eq(self.engine.next[8:16][::-1]),
+            p1.eq(self.engine.next[16:24][::-1]),
+            p0.eq(self.engine.next[24:32][::-1]),
+        ]
+        self.sync += [
+            self.d.eq(self.data.r),
+            self.stb.eq(self.data.re),
+            If(self.en.re, self.reset.eq(1)),
+
+            self.value.status.eq(self.engine.next),
+            self.processed.status.eq(Cat(p3, p2, p1, p0)),
+        ]

src/gateware/cxp_4r_fmc.py

@@ -0,0 +1,87 @@
+from migen.build.generic_platform import *
+
+fmc_adapter_io = [
+
+    # CoaXPress high speed link
+    ("CXP_HS", 0,
+        Subsignal("txp", Pins("HPC:DP0_C2M_P")),
+        Subsignal("txn", Pins("HPC:DP0_C2M_N")),
+        Subsignal("rxp", Pins("HPC:DP0_M2C_P")),
+        Subsignal("rxn", Pins("HPC:DP0_M2C_N")),
+    ),
+    ("CXP_HS", 1,
+        Subsignal("txp", Pins("HPC:DP1_C2M_P")),
+        Subsignal("txn", Pins("HPC:DP1_C2M_N")),
+        Subsignal("rxp", Pins("HPC:DP1_M2C_P")),
+        Subsignal("rxn", Pins("HPC:DP1_M2C_N")),
+    ),
+    ("CXP_HS", 2,
+        Subsignal("txp", Pins("HPC:DP2_C2M_P")),
+        Subsignal("txn", Pins("HPC:DP2_C2M_N")),
+        Subsignal("rxp", Pins("HPC:DP2_M2C_P")),
+        Subsignal("rxn", Pins("HPC:DP2_M2C_n")),
+    ),
+    ("CXP_HS", 3,
+        Subsignal("txp", Pins("HPC:DP3_C2M_P")),
+        Subsignal("txn", Pins("HPC:DP3_C2M_N")),
+        Subsignal("rxp", Pins("HPC:DP3_M2C_P")),
+        Subsignal("rxn", Pins("HPC:DP3_M2C_N")),
+    ),
+
+    # CoaXPress low speed link
+    ("CXP_LS", 0, Pins("HPC:LA00_CC_P"), IOStandard("LVCMOS25")),
+    ("CXP_LS", 1, Pins("HPC:LA01_CC_N"), IOStandard("LVCMOS25")),
+    ("CXP_LS", 2, Pins("HPC:LA01_CC_P"), IOStandard("LVCMOS25")),
+    ("CXP_LS", 3, Pins("HPC:LA02_N"), IOStandard("LVCMOS25")),
+
+    # CoaXPress green and red LED
+    ("CXP_LED", 0,
+        Subsignal("green", Pins("HPC:LA11_P"), IOStandard("LVCMOS25")),
+        Subsignal("red", Pins("HPC:LA11_N"), IOStandard("LVCMOS25")),
+    ),
+    ("CXP_LED", 1,
+        Subsignal("green", Pins("HPC:LA12_P"), IOStandard("LVCMOS25")),
+        Subsignal("red", Pins("HPC:LA12_N"), IOStandard("LVCMOS25")),
+    ),
+    ("CXP_LED", 2,
+        Subsignal("green", Pins("HPC:LA13_P"), IOStandard("LVCMOS25")),
+        Subsignal("red", Pins("HPC:LA13_N"), IOStandard("LVCMOS25")),
+    ),
+    ("CXP_LED", 3,
+        Subsignal("green", Pins("HPC:LA14_P"), IOStandard("LVCMOS25")),
+        Subsignal("red", Pins("HPC:LA14_N"), IOStandard("LVCMOS25")),
+    ),
+
+    # Power over CoaXPress 
+    ("PoCXP", 0,
+        Subsignal("enable", Pins("HPC:LA21_N"), IOStandard("LVCMOS25")),
+        Subsignal("alert", Pins("HPC:LA18_CC_P"), IOStandard("LVCMOS25")),
+    ),
+    ("PoCXP", 1,
+        Subsignal("enable", Pins("HPC:LA21_P"), IOStandard("LVCMOS25")),
+        Subsignal("alert", Pins("HPC:LA19_N"), IOStandard("LVCMOS25")),
+    ),
+    ("PoCXP", 2,
+        Subsignal("enable", Pins("HPC:LA22_N"), IOStandard("LVCMOS25")),
+        Subsignal("alert", Pins("HPC:LA19_P"), IOStandard("LVCMOS25")),
+    ),
+    ("PoCXP", 3,
+        Subsignal("enable", Pins("HPC:LA22_P"), IOStandard("LVCMOS25")),
+        Subsignal("alert", Pins("HPC:LA20_N"), IOStandard("LVCMOS25")),
+    ),
+
+    ("i2c_fmc", 0,
+        Subsignal("scl", Pins("HPC:IIC_SCL")),
+        Subsignal("sda", Pins("HPC:IIC_SDA")),
+        IOStandard("LVCMOS25")
+    ),
+
+    ("3V3", 0, Pins("HPC:PG_M2C")),
+    ("GND", 0, Pins("HPC:PRSNT_M2C_L HPC:CLK0_M2C_P")),
+    ("VADJ", 0, Pins("HPC:GBTCLK1_M2C_N CLK0_M2C_N")),
+
+    ("clk125_fmc", 0,
+        Subsignal("p", Pins("HPC:GBTCLK0_M2C_P")),
+        Subsignal("n", Pins("HPC:GBTCLK0_M2C_n")),
+    ),
+]

src/gateware/cxp_downconn.py

@@ -0,0 +1,820 @@
+from migen import *
+from migen.genlib.cdc import MultiReg
+from migen.genlib.resetsync import AsyncResetSynchronizer
+
+from misoc.cores.code_8b10b import Encoder, Decoder
+from misoc.interconnect.csr import *
+
+from artiq.gateware.drtio.transceiver.gtx_7series_init import *
+
+from functools import reduce
+from operator import add
+
+class CXP_DownConn(Module, AutoCSR):
+    def __init__(self, refclk, pads, sys_clk_freq, debug_sma, pmod_pads):
+        self.rx_start_init = CSRStorage()
+        self.rx_restart = CSR()
+
+        self.tx_start_init = CSRStorage()
+        self.tx_restart = CSR()
+        self.txenable = CSRStorage()
+
+        self.txinit_phaligndone = CSRStatus()
+        self.rxinit_phaligndone = CSRStatus()
+        self.rx_ready = CSRStatus()
+
+        self.qpll_reset = CSR()
+        self.qpll_locked = CSRStatus()
+
+        # # #
+
+        self.submodules.qpll = qpll = QPLL(refclk, sys_clk_freq)
+
+        # single & master tx_mode can lock with rx in loopback 
+        self.submodules.gtx = gtx = GTX(self.qpll, pads, sys_clk_freq, tx_mode="single", rx_mode="single")
+        
+        # NOTE: No need to connect cxp_gtx_tx, we don't use tx anyway (just for loopback)
+
+        # TODO: Connect slave cxp_gtx_rx clock tgt
+        # checkout channel interfaces & drtio_gtx
+        # checkout GTPTXPhaseAlignement for inspiration
+
+        self.sync += [
+            # PLL
+            qpll.reset.eq(self.qpll_reset.re),
+            self.qpll_locked.status.eq(qpll.lock),
+            # GTX
+            self.txinit_phaligndone.status.eq(gtx.tx_init.Xxphaligndone),
+            self.rxinit_phaligndone.status.eq(gtx.rx_init.Xxphaligndone),
+            self.rx_ready.status.eq(gtx.rx_ready),
+
+            gtx.txenable.eq(self.txenable.storage[0]),
+            gtx.tx_restart.eq(self.tx_restart.re),
+            gtx.rx_restart.eq(self.rx_restart.re),
+            gtx.tx_init.clk_path_ready.eq(self.tx_start_init.storage),
+            gtx.rx_init.clk_path_ready.eq(self.rx_start_init.storage),
+        ]
+
+        # GTX Channels DRP
+        self.tx_div = CSRStorage(3)
+        self.rx_div = CSRStorage(3)
+
+        self.gtx_daddr = CSRStorage(9)
+        self.gtx_dread = CSR()
+        self.gtx_din_stb = CSR()
+        self.gtx_din = CSRStorage(16)
+
+        self.gtx_dout = CSRStatus(16)
+        self.gtx_dready = CSR()
+
+        self.comb += gtx.dclk.eq(ClockSignal("sys"))
+        self.sync += [
+            gtx.tx_rate.eq(self.tx_div.storage),
+            gtx.rx_rate.eq(self.rx_div.storage),
+
+            gtx.den.eq(0),
+            gtx.dwen.eq(0),
+            If(self.gtx_dread.re,
+                gtx.den.eq(1),
+                gtx.daddr.eq(self.gtx_daddr.storage),
+            ).Elif(self.gtx_din_stb.re,
+                gtx.den.eq(1),
+                gtx.dwen.eq(1),
+                gtx.daddr.eq(self.gtx_daddr.storage),
+                gtx.din.eq(self.gtx_din.storage),
+            ),
+            If(gtx.dready,
+               self.gtx_dready.w.eq(1),
+               self.gtx_dout.status.eq(gtx.dout),
+            ),
+            If(self.gtx_dready.re,
+               self.gtx_dready.w.eq(0),
+            ),
+        ]
+
+        # QPLL DRP
+
+        self.qpll_daddr = CSRStorage(8)
+        self.qpll_dread = CSR()
+        self.qpll_din_stb = CSR()
+        self.qpll_din = CSRStorage(16)
+
+        self.qpll_dout = CSRStatus(16)
+        self.qpll_dready = CSR()
+
+        self.comb += qpll.dclk.eq(ClockSignal("sys"))
+        self.sync += [
+            qpll.den.eq(0),
+            qpll.dwen.eq(0),
+
+            If(self.qpll_dread.re,
+                qpll.den.eq(1),
+                qpll.daddr.eq(self.qpll_daddr.storage),
+            ).Elif(self.qpll_din_stb.re,
+                qpll.den.eq(1),
+                qpll.dwen.eq(1),
+                qpll.daddr.eq(self.qpll_daddr.storage),
+                qpll.din.eq(self.qpll_din.storage),
+            ),
+            If(qpll.dready,
+               self.qpll_dready.w.eq(1),
+               self.qpll_dout.status.eq(qpll.dout),
+            ),
+            If(self.qpll_dready.re,
+               self.qpll_dready.w.eq(0),
+            ),
+        ]
+
+        # DEBUG: txusrclk PLL DRG
+        
+        self.txpll_reset = CSRStorage()
+        self.pll_daddr = CSRStorage(7)
+        self.pll_dclk = CSRStorage()
+        self.pll_den = CSRStorage()
+        self.pll_din = CSRStorage(16)
+        self.pll_dwen = CSRStorage()
+
+        self.txpll_locked = CSRStatus()
+        self.pll_dout = CSRStatus(16)
+        self.pll_dready = CSRStatus()
+
+        self.comb += [
+        gtx.txpll_reset.eq(self.txpll_reset.storage),
+        gtx.pll_daddr.eq(self.pll_daddr.storage),
+        gtx.pll_dclk.eq(self.pll_dclk.storage),
+        gtx.pll_den.eq(self.pll_den.storage),
+        gtx.pll_din.eq(self.pll_din.storage),
+        gtx.pll_dwen.eq(self.pll_dwen.storage),
+
+        self.txpll_locked.status.eq(gtx.txpll_locked),
+        self.pll_dout.status.eq(gtx.pll_dout),
+        self.pll_dready.status.eq(gtx.pll_dready),
+        ]
+
+        # DEBUG:loopback 
+        self.loopback_mode = CSRStorage(3)
+        self.comb += gtx.loopback_mode.eq(self.loopback_mode.storage) 
+
+        # DEBUG: IO SMA & PMOD
+        self.specials += [
+            Instance("OBUF", i_I=gtx.cd_cxp_gtx_rx.clk, o_O=debug_sma.p_tx),
+            Instance("OBUF", i_I=gtx.cd_cxp_gtx_tx.clk, o_O=debug_sma.n_rx),
+
+            # pmod 0-7 pin
+            Instance("OBUF", i_I=gtx.comma_checker.comma_aligned, o_O=pmod_pads[0]),
+            Instance("OBUF", i_I=gtx.comma_checker.comma_det, o_O=pmod_pads[1]),
+            Instance("OBUF", i_I=gtx.comma_checker.restart_sys, o_O=pmod_pads[2]),
+            Instance("OBUF", i_I=gtx.comma_checker.aligner_en, o_O=pmod_pads[3]),
+            Instance("OBUF", i_I=gtx.comma_checker.check_reset, o_O=pmod_pads[4]),
+            Instance("OBUF", i_I=gtx.comma_checker.has_comma, o_O=pmod_pads[5]),
+            Instance("OBUF", i_I=gtx.comma_checker.has_error, o_O=pmod_pads[6]),
+            Instance("OBUF", i_I=gtx.comma_checker.ready_sys, o_O=pmod_pads[7]),
+
+            # Instance("OBUF", i_I=gtx.dclk, o_O=pmod_pads[0]),
+            # Instance("OBUF", i_I=gtx.den, o_O=pmod_pads[1]),
+            # Instance("OBUF", i_I=gtx.dwen, o_O=pmod_pads[2]),
+            # Instance("OBUF", i_I=gtx.dready, o_O=pmod_pads[3]),
+        ]
+
+        # DEBUG: datain
+
+        
+        self.sync.cxp_gtx_tx += [
+            self.gtx.encoder.d[0].eq(0xBC),
+            self.gtx.encoder.k[0].eq(1),
+            self.gtx.encoder.d[1].eq(0x3C),
+            self.gtx.encoder.k[1].eq(1),
+            self.gtx.encoder.d[2].eq(0x3C),
+            self.gtx.encoder.k[2].eq(1),
+            self.gtx.encoder.d[3].eq(0xB5),
+            self.gtx.encoder.k[3].eq(0),
+        ]
+
+        self.rxdata_0 = CSRStatus(10)
+        self.rxdata_1 = CSRStatus(10)
+        self.rxdata_2 = CSRStatus(10)
+        self.rxdata_3 = CSRStatus(10)
+        self.decoded_data_0 = CSRStatus(8)
+        self.decoded_data_1 = CSRStatus(8)
+        self.decoded_data_2 = CSRStatus(8)
+        self.decoded_data_3 = CSRStatus(8)
+        self.decoded_k_0 = CSRStatus()
+        self.decoded_k_1 = CSRStatus()
+        self.decoded_k_2 = CSRStatus()
+        self.decoded_k_3 = CSRStatus()
+
+        self.sync.cxp_gtx_rx += [
+            self.rxdata_0.status.eq(self.gtx.decoders[0].input),
+            self.decoded_data_0.status.eq(self.gtx.decoders[0].d),
+            self.decoded_k_0.status.eq(self.gtx.decoders[0].k),
+
+            self.rxdata_1.status.eq(self.gtx.decoders[1].input),
+            self.decoded_data_1.status.eq(self.gtx.decoders[1].d),
+            self.decoded_k_1.status.eq(self.gtx.decoders[1].k),
+
+            self.rxdata_2.status.eq(self.gtx.decoders[2].input),
+            self.decoded_data_2.status.eq(self.gtx.decoders[2].d),
+            self.decoded_k_2.status.eq(self.gtx.decoders[2].k),
+
+            self.rxdata_3.status.eq(self.gtx.decoders[3].input),
+            self.decoded_data_3.status.eq(self.gtx.decoders[3].d),
+            self.decoded_k_3.status.eq(self.gtx.decoders[3].k),
+        ]
+
+
+class QPLL(Module):
+    def __init__(self, refclk, sys_clk_freq):
+        self.clk = Signal()
+        self.refclk = Signal()
+        self.lock = Signal()
+        self.reset = Signal()
+
+        # Dynamic Reconfiguration Ports
+        self.daddr = Signal(8)
+        self.dclk = Signal()
+        self.den = Signal()
+        self.dwen = Signal()
+        self.din = Signal(16)
+
+        self.dout = Signal(16)
+        self.dready = Signal()
+        # # #
+
+        # VCO @ 10GHz, linerate = 1.25Gbps
+        # feedback divider = 80
+        qpll_fbdiv = 0b0100100000
+        qpll_fbdiv_ratio = 1
+        refclk_div = 1
+        self.Xxout_div = 8
+
+        # DEBUG: txuserclk
+        fbdiv_real = 80
+        self.tx_usrclk_freq = (sys_clk_freq*fbdiv_real/self.Xxout_div)/40
+        
+        self.specials += [
+            Instance("GTXE2_COMMON",
+                i_QPLLREFCLKSEL=0b001,
+                i_GTREFCLK0=refclk,         
+
+                i_QPLLPD=0,
+                i_QPLLRESET=self.reset,
+                i_QPLLLOCKEN=1,
+                o_QPLLLOCK=self.lock,
+                o_QPLLOUTCLK=self.clk,
+                o_QPLLOUTREFCLK=self.refclk,
+
+                # See UG476 (v1.12.1) Table 2-16
+                p_QPLL_FBDIV=qpll_fbdiv,
+                p_QPLL_FBDIV_RATIO=qpll_fbdiv_ratio,
+                p_QPLL_REFCLK_DIV=refclk_div,
+
+                # From 7 Series FPGAs Transceivers Wizard
+                p_BIAS_CFG=0x0000040000001000,
+                p_COMMON_CFG=0x00000000,
+                p_QPLL_CFG=0x0680181,
+                p_QPLL_CLKOUT_CFG=0b0000,
+                p_QPLL_COARSE_FREQ_OVRD=0b010000,
+                p_QPLL_COARSE_FREQ_OVRD_EN=0b0,
+                p_QPLL_CP=0b0000011111,
+                p_QPLL_CP_MONITOR_EN=0b0,
+                p_QPLL_DMONITOR_SEL=0b0,
+                p_QPLL_FBDIV_MONITOR_EN= 0b0,
+                p_QPLL_INIT_CFG=0x000006,
+                p_QPLL_LOCK_CFG=0x21E8,
+                p_QPLL_LPF=0b1111,
+             
+                # Reserved, values cannot be modified
+                i_BGBYPASSB=0b1,
+                i_BGMONITORENB=0b1,
+                i_BGPDB=0b1,
+                i_BGRCALOVRD=0b11111,
+                i_RCALENB=0b1,
+                i_QPLLRSVD1=0b0,
+                i_QPLLRSVD2=0b11111,
+
+                # Dynamic Reconfiguration Ports
+                i_DRPADDR=self.daddr,
+                i_DRPCLK=self.dclk,
+                i_DRPEN=self.den,
+                i_DRPWE=self.dwen,
+                i_DRPDI=self.din,
+                o_DRPDO=self.dout,
+                o_DRPRDY=self.dready,
+            )
+        ]
+
+# Warning: Xilinx transceivers are LSB first, and comma needs to be flipped
+# compared to the usual 8b10b binary representation.
+class Comma_Checker(Module):
+    def __init__(self, comma, reset_period=10_000_000):
+        self.data = Signal(20)
+        self.comma_aligned = Signal()
+        self.comma_realigned = Signal()
+        self.comma_det = Signal()
+
+        self.aligner_en = Signal()
+        self.ready_sys = Signal()
+        self.restart_sys = Signal()
+
+        # # #
+
+
+        # periodically reset rx until rx is connected and receiving valid data
+        # as after connecting RXP/RXN, the whole RX need to be reset 
+        
+        reset_counter = Signal(reset=reset_period-1, max=reset_period)
+        self.sync += [
+            self.restart_sys.eq(0),
+            If(~self.ready_sys,
+                If(reset_counter == 0,
+                    reset_counter.eq(reset_counter.reset),
+                    self.restart_sys.eq(1),
+                ).Else(
+                    reset_counter.eq(reset_counter - 1),
+                )
+            )
+        ]
+
+
+        # Data and comma checker
+        # From UG476 (v1.12.1) p.228
+        # The built-in RXBYTEISALIGNED can be falsely asserted at linerate higher than 5Gbps
+        # The validity of data and comma needed to be checked externally
+
+        comma_n = ~comma & 0b1111111111
+
+        # DEBUG: remove after use
+        self.has_comma = Signal()
+        self.has_error = Signal()
+
+        comma_seen = Signal()
+        error_seen = Signal()
+        one_counts = Signal(max=11)
+
+        # From CXP-001-2021 section 9.2.5.1
+        # For high speed connection an IDLE word shall be transmitted at least once every 100 words
+        counter_period = 200
+
+        counter = Signal(reset=counter_period-1, max=counter_period)
+        check_reset = Signal()
+        check = Signal()
+
+        self.sync.cxp_gtx_rx += [
+            If(check_reset,
+                counter.eq(counter.reset),   
+                check.eq(0),
+            ).Elif(counter == 0,
+                check.eq(1),
+            ).Else(
+                counter.eq(counter - 1),   
+            ),
+
+            If(check_reset,
+                comma_seen.eq(0),
+            ).Elif((self.data[:10] == comma) | (self.data[:10] == comma_n),
+                comma_seen.eq(1)
+            ),
+
+            one_counts.eq(reduce(add, [self.data[i] for i in range(10)])),
+            If(check_reset,
+                error_seen.eq(0),
+            ).Elif((one_counts != 4) & (one_counts != 5) & (one_counts != 6),
+                error_seen.eq(1),
+            ),
+
+            # DEBUG: 
+            self.has_comma.eq(0),
+            If((self.data[:10] == comma) | (self.data[:10] == comma_n),
+               self.has_comma.eq(1),
+            ),
+
+            self.has_error.eq(0),
+            If((one_counts != 4) & (one_counts != 5) & (one_counts != 6),
+                self.has_error.eq(1),
+            ),
+        ]
+
+        # DEBUG: expose signal
+        self.check_reset = Signal()
+        self.comb +=[
+            self.check_reset.eq(check_reset),
+        ]
+
+        self.submodules.rxfsm = rxfsm = ClockDomainsRenamer("cxp_gtx_rx")(FSM(reset_state="WAIT_COMMA"))
+
+        rxfsm.act("WAIT_COMMA",
+            If(self.comma_det,
+                # # start aligner early, so word aligned will fall
+                # self.aligner_en_rxclk.eq(1),
+                NextState("ALIGNING"),
+            )
+        )
+
+        rxfsm.act("ALIGNING",
+            If(self.comma_aligned & (~self.comma_realigned),
+                NextState("WAIT_ALIGNED_DATA"),               
+            ).Else(
+                self.aligner_en.eq(1),
+            )
+        )
+
+        # wait for the aligned data to arrive at the FPGA RX interface 
+        # as there is a delay before the data is avaiable after RXBYTEISALIGNED is asserted
+        self.submodules.timer = timer = ClockDomainsRenamer("cxp_gtx_rx")(WaitTimer(10_000))
+
+        rxfsm.act("WAIT_ALIGNED_DATA",
+            timer.wait.eq(1),
+            If(timer.done,
+                check_reset.eq(1),
+                NextState("CHECKING"),
+            )
+        )
+
+        rxfsm.act("CHECKING",
+            If(check,
+                check_reset.eq(1),
+                If(comma_seen & (~error_seen),
+                    NextState("READY"),
+                ).Else(
+                    NextState("WAIT_COMMA")
+                )
+            )
+        )
+
+        ready = Signal()
+        self.specials += MultiReg(ready, self.ready_sys)
+        rxfsm.act("READY",
+            ready.eq(1),
+            If(check,
+                check_reset.eq(1),
+                If(~(comma_seen & (~error_seen)),
+                    NextState("WAIT_COMMA"),
+                )
+            )
+        )
+
+
+class GTX(Module):
+    # Settings:
+    # * GTX reference clock @ 125MHz
+    # * GTX data width = 20
+    # * GTX PLL frequency @ 3.125GHz
+    # * GTX line rate (TX & RX) @ 3.125Gb/s
+    # * GTX TX/RX USRCLK @ PLL/datawidth = 156MHz
+    def __init__(self, qpll, pads, sys_clk_freq, tx_mode="single", rx_mode="single"):
+        assert tx_mode in ["single", "master", "slave"]
+        assert rx_mode in ["single", "master", "slave"]
+
+        # linerate = USRCLK * datawidth
+        pll_fbout_mult = 8
+        txusr_pll_div = pll_fbout_mult*sys_clk_freq/qpll.tx_usrclk_freq
+
+        self.tx_restart = Signal()
+        self.rx_restart = Signal()
+        self.loopback_mode = Signal(3)
+
+        self.txenable = Signal()
+        self.rx_ready = Signal()
+
+        self.tx_rate = Signal(3)
+        self.rx_rate = Signal(3)
+
+        # Dynamic Reconfiguration Ports
+        self.daddr = Signal(9)
+        self.dclk = Signal()
+        self.den = Signal()
+        self.dwen = Signal()
+        self.din = Signal(16)
+        self.dout = Signal(16)
+        self.dready = Signal()
+
+        self.submodules.encoder = ClockDomainsRenamer("cxp_gtx_tx")(Encoder(4, True))
+        self.submodules.decoders = [ClockDomainsRenamer("cxp_gtx_rx")(
+            (Decoder(True))) for _ in range(4)]
+
+
+        # transceiver direct clock outputs
+        # useful to specify clock constraints in a way palatable to Vivado
+        self.txoutclk = Signal()
+        self.rxoutclk = Signal()
+
+        # # #
+
+        # TX generates cxp_tx clock, init must be in system domain
+        # FIXME: 500e6 is used to fix Xx reset by holding gtxXxreset for a couple cycle more
+        self.submodules.tx_init = tx_init = GTXInit(500e6, False, mode=tx_mode)
+        self.submodules.rx_init = rx_init = GTXInit(sys_clk_freq, True, mode=rx_mode)
+
+        # RX receives restart commands from txusrclk domain
+        # self.submodules.rx_init = rx_init = ClockDomainsRenamer("cxp_gtx_tx")(GTXInit(500e6, True, mode=rx_mode))
+
+        self.comb += [
+            tx_init.cplllock.eq(qpll.lock),
+            rx_init.cplllock.eq(qpll.lock)
+        ]
+
+        txdata = Signal(40)
+        rxdata = Signal(40)
+
+        comma_aligned = Signal()
+        comma_realigned = Signal()
+        comma_det = Signal()
+        comma_aligner_en = Signal()
+        # Note: the following parameters were set after consulting AR45360
+        self.specials += \
+            Instance("GTXE2_CHANNEL",
+                # PMA Attributes
+                p_PMA_RSV=0x001E7080,
+                p_PMA_RSV2=0x2050,              # PMA_RSV2[5] = 0: Eye scan feature disabled
+                p_PMA_RSV3=0,
+                p_PMA_RSV4=1,                   # PMA_RSV[4],RX_CM_TRIM[2:0] = 0b1010: Common mode 800mV
+                p_RX_BIAS_CFG=0b000000000100,
+                p_RX_OS_CFG=0b0000010000000,
+                p_RX_CLK25_DIV=5,
+                p_TX_CLK25_DIV=5,
+
+                # Power-Down Attributes
+                p_PD_TRANS_TIME_FROM_P2=0x3c,
+                p_PD_TRANS_TIME_NONE_P2=0x3c,
+                p_PD_TRANS_TIME_TO_P2=0x64,
+                i_CPLLPD=1,
+
+                # Dynamic Tx/Rx divider
+                i_TXRATE=self.tx_rate,
+                i_RXRATE=self.rx_rate,
+
+                # QPLL
+                i_QPLLCLK=qpll.clk,
+                i_QPLLREFCLK=qpll.refclk,
+                p_RXOUT_DIV=qpll.Xxout_div,
+                p_TXOUT_DIV=qpll.Xxout_div,
+                i_RXSYSCLKSEL=0b11, # use QPLL & QPLL's REFCLK
+                i_TXSYSCLKSEL=0b11, # use QPLL & CPLL's REFCLK
+
+                # TX clock
+                p_TXBUF_EN="FALSE",
+                p_TX_XCLK_SEL="TXUSR",
+                o_TXOUTCLK=self.txoutclk,
+                # i_TXSYSCLKSEL=0b00,
+                i_TXOUTCLKSEL=0b11,
+
+                # TX Startup/Reset
+                i_TXPHDLYRESET=0,
+                i_TXDLYBYPASS=0,
+                i_TXPHALIGNEN=1 if tx_mode != "single" else 0,
+                i_GTTXRESET=tx_init.gtXxreset,
+                o_TXRESETDONE=tx_init.Xxresetdone,
+                i_TXDLYSRESET=tx_init.Xxdlysreset,
+                o_TXDLYSRESETDONE=tx_init.Xxdlysresetdone,
+                i_TXPHINIT=tx_init.txphinit if tx_mode != "single" else 0,
+                o_TXPHINITDONE=tx_init.txphinitdone if tx_mode != "single" else Signal(),
+                i_TXPHALIGN=tx_init.Xxphalign if tx_mode != "single" else 0,
+                i_TXDLYEN=tx_init.Xxdlyen if tx_mode != "single" else 0,
+                o_TXPHALIGNDONE=tx_init.Xxphaligndone,
+                i_TXUSERRDY=tx_init.Xxuserrdy,
+                p_TXPMARESET_TIME=1,
+                p_TXPCSRESET_TIME=1,
+                i_TXINHIBIT=~self.txenable,
+
+                # TX data
+                p_TX_DATA_WIDTH=40,
+                p_TX_INT_DATAWIDTH=1, # 1 if a line rate is greater than 6.6 Gbps
+                i_TXCHARDISPMODE=Cat(txdata[9], txdata[19], txdata[29], txdata[39]),
+                i_TXCHARDISPVAL=Cat(txdata[8], txdata[18], txdata[28], txdata[38]),
+                i_TXDATA=Cat(txdata[:8], txdata[10:18], txdata[20:28], txdata[30:38]),
+                i_TXUSRCLK=ClockSignal("cxp_gtx_tx"),
+                i_TXUSRCLK2=ClockSignal("cxp_gtx_tx"),
+
+                # TX electrical
+                i_TXBUFDIFFCTRL=0b100,
+                i_TXDIFFCTRL=0b1000,
+
+                # RX Startup/Reset
+                i_RXPHDLYRESET=0,
+                i_RXDLYBYPASS=0,
+                i_RXPHALIGNEN=1 if rx_mode != "single" else 0,
+                i_GTRXRESET=rx_init.gtXxreset,
+                o_RXRESETDONE=rx_init.Xxresetdone,
+                i_RXDLYSRESET=rx_init.Xxdlysreset,
+                o_RXDLYSRESETDONE=rx_init.Xxdlysresetdone,
+                i_RXPHALIGN=rx_init.Xxphalign if rx_mode != "single" else 0,
+                i_RXDLYEN=rx_init.Xxdlyen if rx_mode != "single" else 0,
+                o_RXPHALIGNDONE=rx_init.Xxphaligndone,
+                i_RXUSERRDY=rx_init.Xxuserrdy,
+                p_RXPMARESET_TIME=1,
+                p_RXPCSRESET_TIME=1,
+
+                # RX AFE
+                p_RX_DFE_XYD_CFG=0,
+                p_RX_CM_SEL=0b11,               # RX_CM_SEL = 0b11: Common mode is programmable
+                p_RX_CM_TRIM=0b010,             # PMA_RSV[4],RX_CM_TRIM[2:0] = 0b1010: Common mode 800mV
+                i_RXDFEXYDEN=1,
+                i_RXDFEXYDHOLD=0,
+                i_RXDFEXYDOVRDEN=0,
+                i_RXLPMEN=1,                    # RXLPMEN = 1: LPM mode is enable for non scramble 8b10b data
+                p_RXLPM_HF_CFG=0b00000011110000,
+                p_RXLPM_LF_CFG=0b00000011110000,
+
+                p_RX_DFE_GAIN_CFG=0x0207EA,
+                p_RX_DFE_VP_CFG=0b00011111100000011,
+                p_RX_DFE_UT_CFG=0b10001000000000000,
+                p_RX_DFE_KL_CFG=0b0000011111110,
+                p_RX_DFE_KL_CFG2=0x3788140A,
+                p_RX_DFE_H2_CFG=0b000110000000,
+                p_RX_DFE_H3_CFG=0b000110000000,
+                p_RX_DFE_H4_CFG=0b00011100000,
+                p_RX_DFE_H5_CFG=0b00011100000,
+                p_RX_DFE_LPM_CFG=0x0904,        # RX_DFE_LPM_CFG = 0x0904: linerate <= 6.6Gb/s
+                                                #                = 0x0104: linerate > 6.6Gb/s
+
+                # RX clock
+                i_RXDDIEN=1,
+                # i_RXSYSCLKSEL=0b00,
+                i_RXOUTCLKSEL=0b010,
+                o_RXOUTCLK=self.rxoutclk,
+                i_RXUSRCLK=ClockSignal("cxp_gtx_rx"),
+                i_RXUSRCLK2=ClockSignal("cxp_gtx_rx"),
+
+                # RX Clock Correction Attributes
+                p_CLK_CORRECT_USE="FALSE",
+                p_CLK_COR_SEQ_1_1=0b0100000000,
+                p_CLK_COR_SEQ_2_1=0b0100000000,
+                p_CLK_COR_SEQ_1_ENABLE=0b1111,
+                p_CLK_COR_SEQ_2_ENABLE=0b1111,
+
+                # RX data
+                p_RX_DATA_WIDTH=40,
+                p_RX_INT_DATAWIDTH=1,   # 1 if a line rate is greater than 6.6 Gbps
+                o_RXDISPERR=Cat(rxdata[9], rxdata[19], rxdata[29], rxdata[39]),
+                o_RXCHARISK=Cat(rxdata[8], rxdata[18], rxdata[28], rxdata[38]),
+                o_RXDATA=Cat(rxdata[:8], rxdata[10:18], rxdata[20:28], rxdata[30:38]),
+
+                # RX Byte and Word Alignment Attributes
+                p_ALIGN_COMMA_DOUBLE="FALSE",
+                p_ALIGN_COMMA_ENABLE=0b1111111111,
+                p_ALIGN_COMMA_WORD=4, # align comma to rxdata[:10] only
+                p_ALIGN_MCOMMA_DET="TRUE",
+                p_ALIGN_MCOMMA_VALUE=0b1010000011,
+                p_ALIGN_PCOMMA_DET="TRUE",
+                p_ALIGN_PCOMMA_VALUE=0b0101111100,
+                p_SHOW_REALIGN_COMMA="FALSE",
+                p_RXSLIDE_AUTO_WAIT=7,
+                p_RXSLIDE_MODE="OFF",
+                p_RX_SIG_VALID_DLY=10,
+                i_RXPCOMMAALIGNEN=comma_aligner_en,
+                i_RXMCOMMAALIGNEN=comma_aligner_en,
+                i_RXCOMMADETEN=1,
+                i_RXSLIDE=0,
+                o_RXBYTEISALIGNED=comma_aligned,
+                o_RXBYTEREALIGN=comma_realigned,
+                o_RXCOMMADET=comma_det,
+
+                # RX 8B/10B Decoder Attributes
+                p_RX_DISPERR_SEQ_MATCH="FALSE",
+                p_DEC_MCOMMA_DETECT="TRUE",
+                p_DEC_PCOMMA_DETECT="TRUE",
+                p_DEC_VALID_COMMA_ONLY="FALSE",
+
+                # RX Buffer Attributes
+                p_RXBUF_ADDR_MODE="FAST",
+                p_RXBUF_EIDLE_HI_CNT=0b1000,
+                p_RXBUF_EIDLE_LO_CNT=0b0000,
+                p_RXBUF_EN="FALSE",
+                p_RX_BUFFER_CFG=0b000000,
+                p_RXBUF_RESET_ON_CB_CHANGE="TRUE",
+                p_RXBUF_RESET_ON_COMMAALIGN="FALSE",
+                p_RXBUF_RESET_ON_EIDLE="FALSE",     # RXBUF_RESET_ON_EIDLE = FALSE: OOB is disabled
+                p_RXBUF_RESET_ON_RATE_CHANGE="TRUE",
+                p_RXBUFRESET_TIME=0b00001,
+                p_RXBUF_THRESH_OVFLW=61,
+                p_RXBUF_THRESH_OVRD="FALSE",
+                p_RXBUF_THRESH_UNDFLW=4,
+                p_RXDLY_CFG=0x001F,
+                p_RXDLY_LCFG=0x030,
+                p_RXDLY_TAP_CFG=0x0000,
+                p_RXPH_CFG=0xC00002,
+                p_RXPHDLY_CFG=0x084020,
+                p_RXPH_MONITOR_SEL=0b00000,
+                p_RX_XCLK_SEL="RXUSR",
+                p_RX_DDI_SEL=0b000000,
+                p_RX_DEFER_RESET_BUF_EN="TRUE",
+
+                # CDR Attributes
+                p_RXCDR_CFG=0x03_0000_23FF_1008_0020,   # LPM @ 0.5G-1.5625G , 8B/10B encoded data, CDR setting < +/- 200ppm
+                                                        # (See UG476 (v1.12.1), p.206)
+                p_RXCDR_FR_RESET_ON_EIDLE=0b0,
+                p_RXCDR_HOLD_DURING_EIDLE=0b0,
+                p_RXCDR_PH_RESET_ON_EIDLE=0b0,
+                p_RXCDR_LOCK_CFG=0b010101,
+
+                # Pads
+                i_GTXRXP=pads.rxp,
+                i_GTXRXN=pads.rxn,
+                o_GTXTXP=pads.txp,
+                o_GTXTXN=pads.txn,
+
+                # Dynamic Reconfiguration Ports
+                p_IS_DRPCLK_INVERTED=0b0,
+                i_DRPADDR=self.daddr,
+                i_DRPCLK=self.dclk,
+                i_DRPEN=self.den,
+                i_DRPWE=self.dwen,
+                i_DRPDI=self.din,
+                o_DRPDO=self.dout,
+                o_DRPRDY=self.dready,
+
+                # ! loopback for debugging
+                i_LOOPBACK = self.loopback_mode,
+                p_TX_LOOPBACK_DRIVE_HIZ = "FALSE",
+                p_RXPRBS_ERR_LOOPBACK = 0b0,
+
+                # Other parameters
+                p_PCS_RSVD_ATTR=(
+                    (tx_mode != "single") << 1 |    # PCS_RSVD_ATTR[1] = 0: TX Single Lane Auto Mode
+                                                    #                  = 1: TX Manual Mode
+                    (rx_mode != "single") << 2 |    #              [2] = 0: RX Single Lane Auto Mode
+                                                    #                  = 1: RX Manual Mode
+                    0 << 8                          #              [8] = 0: OOB is disabled
+                ),
+                i_RXELECIDLEMODE=0b11,              # RXELECIDLEMODE = 0b11: OOB is disabled
+                p_RX_DFE_LPM_HOLD_DURING_EIDLE=0b0,
+                p_ES_EYE_SCAN_EN="TRUE",            # Must be TRUE for GTX
+            )
+
+
+        # TX clocking
+        # A PLL is used to generate the correct frequency for TXUSRCLK (UG476 Equation 3-1)
+        self.clock_domains.cd_cxp_gtx_tx = ClockDomain()
+        txpll_fb_clk = Signal()
+        txoutclk_buf = Signal()
+        txpll_clkout = Signal()
+
+        self.txpll_reset = Signal()
+        self.pll_daddr = Signal(7)
+        self.pll_dclk = Signal()
+        self.pll_den = Signal()
+        self.pll_din = Signal(16)
+        self.pll_dwen = Signal()
+
+        self.txpll_locked = Signal()
+        self.pll_dout = Signal(16)
+        self.pll_dready = Signal()
+        self.specials += [
+            Instance("PLLE2_ADV",
+                p_BANDWIDTH="HIGH",
+                o_LOCKED=self.txpll_locked,
+                i_RST=self.txpll_reset,
+                
+                p_CLKIN1_PERIOD=1e9/sys_clk_freq, # ns
+                i_CLKIN1=txoutclk_buf,
+
+                # VCO @ 1.25GHz 
+                p_CLKFBOUT_MULT=pll_fbout_mult, p_DIVCLK_DIVIDE=1,
+                i_CLKFBIN=txpll_fb_clk, o_CLKFBOUT=txpll_fb_clk, 
+
+                # frequency = linerate/40
+                p_CLKOUT0_DIVIDE=txusr_pll_div, p_CLKOUT0_PHASE=0.0, o_CLKOUT0=txpll_clkout,
+
+                # Dynamic Reconfiguration Ports
+                i_DADDR = self.pll_daddr,
+                i_DCLK = self.pll_dclk,
+                i_DEN = self.pll_den,
+                i_DI = self.pll_din,
+                i_DWE = self.pll_dwen,
+                o_DO = self.pll_dout,
+                o_DRDY = self.pll_dready,
+            ),
+            Instance("BUFG", i_I=self.txoutclk, o_O=txoutclk_buf),
+            Instance("BUFG", i_I=txpll_clkout, o_O=self.cd_cxp_gtx_tx.clk),
+            AsyncResetSynchronizer(self.cd_cxp_gtx_tx, ~self.txpll_locked & ~tx_init.done)
+        ]
+
+        # RX clocking
+        # the CDR matches the required frequency for RXUSRCLK, no need for PLL 
+        self.clock_domains.cd_cxp_gtx_rx = ClockDomain()
+        self.specials += [
+            Instance("BUFG", i_I=self.rxoutclk, o_O=self.cd_cxp_gtx_rx.clk),
+            AsyncResetSynchronizer(self.cd_cxp_gtx_rx, ~rx_init.done)
+        ]
+
+        self.comb += [
+            txdata.eq(Cat(self.encoder.output[0], self.encoder.output[1], self.encoder.output[2], self.encoder.output[3])),
+            self.decoders[0].input.eq(rxdata[:10]),
+            self.decoders[1].input.eq(rxdata[10:20]),
+            self.decoders[2].input.eq(rxdata[20:30]),
+            self.decoders[3].input.eq(rxdata[30:]),
+        ]
+
+
+        self.submodules.comma_checker = comma_checker = Comma_Checker(0b0101111100)
+        self.comb += [
+            comma_checker.data.eq(rxdata),
+            comma_checker.comma_aligned.eq(comma_aligned),
+            comma_checker.comma_realigned.eq(comma_realigned),
+            comma_checker.comma_det.eq(comma_det),
+            comma_aligner_en.eq(comma_checker.aligner_en),
+            self.rx_ready.eq(comma_checker.ready_sys),
+
+            rx_init.restart.eq(self.rx_restart | comma_checker.restart_sys),
+            tx_init.restart.eq(self.tx_restart),
+        ]

src/gateware/cxp_upconn.py

@@ -0,0 +1,279 @@
+from migen import *
+from migen.genlib.resetsync import AsyncResetSynchronizer
+from migen.genlib.coding import PriorityEncoder
+
+from misoc.cores.code_8b10b import SingleEncoder
+from misoc.interconnect import stream
+from misoc.interconnect.csr import *
+
+
+class CXP_UpConn(Module, AutoCSR):
+    nfifos = 3
+    def __init__(self, pads, sys_clk_freq, pmod, fifo_depth=32):
+        self.clock_domains.cd_cxp_upconn = ClockDomain()
+        self.clk_reset = CSRStorage(reset=1)
+        self.bitrate2x_enable = CSRStorage()
+        self.tx_enable = CSRStorage()
+
+        # # #
+
+        pll_locked = Signal()
+        pll_fb_clk = Signal()
+        pll_cxpclk = Signal()
+        pll_cxpclk2x = Signal()
+
+        self.specials += [
+            Instance("PLLE2_ADV",
+                p_BANDWIDTH="HIGH",
+                o_LOCKED=pll_locked,
+                i_RST=ResetSignal("sys"),
+                
+                p_CLKIN1_PERIOD=1e9/sys_clk_freq, # ns
+                i_CLKIN1=ClockSignal("sys"),
+
+                # VCO @ 1.25GHz 
+                p_CLKFBOUT_MULT=1.25e9/sys_clk_freq, p_DIVCLK_DIVIDE=1,
+                i_CLKFBIN=pll_fb_clk, o_CLKFBOUT=pll_fb_clk, 
+
+                # 20.83MHz (48ns)
+                p_CLKOUT0_DIVIDE=60, p_CLKOUT0_PHASE=0.0, o_CLKOUT0=pll_cxpclk,
+
+                # 41.66MHz (24ns) for downconnection over 6.25Gpbs
+                p_CLKOUT1_DIVIDE=30, p_CLKOUT1_PHASE=0.0, o_CLKOUT1=pll_cxpclk2x,
+            ),
+            Instance("BUFGMUX",
+                i_I0=pll_cxpclk,
+                i_I1=pll_cxpclk2x,
+                i_S=self.bitrate2x_enable.storage,
+                o_O=self.cd_cxp_upconn.clk
+            ),
+            AsyncResetSynchronizer(self.cd_cxp_upconn, ~pll_locked | self.clk_reset.storage)
+        ]
+
+        self.submodules.fsm = ClockDomainsRenamer("cxp_upconn")(FSM(reset_state="WAIT_TX_ENABLE"))
+        self.submodules.tx_fifos = TxFIFOs(self.nfifos, fifo_depth)
+        self.submodules.tx_idle = TxIdle()
+
+        o = Signal()
+        tx_en = Signal()
+        tx_bitcount = Signal(max=10)
+        tx_wordcount = Signal(max=4)
+        tx_reg = Signal(10)
+
+        disp = Signal()
+        priorities = Signal(max=self.nfifos)
+        idling = Signal()
+
+        # startup sequence
+        self.fsm.act("WAIT_TX_ENABLE",
+            If(self.tx_enable.storage,
+                NextValue(self.tx_idle.word_idx, 0),
+                NextValue(tx_wordcount, 0),
+                NextValue(tx_bitcount, 0),
+                NextState("LOAD_CHAR")
+            )
+        )
+
+        self.fsm.act("LOAD_CHAR",
+            NextValue(idling, 1),
+            NextValue(self.tx_idle.source_ack, 1),
+            NextValue(tx_reg, self.tx_idle.source_data),
+            NextValue(disp, self.tx_idle.disp_out),
+            NextState("START_TX")
+        )
+
+        self.fsm.act("START_TX",
+            tx_en.eq(1),
+            If((~self.tx_enable.storage) & (tx_wordcount == 3),
+                NextState("WAIT_TX_ENABLE")
+            )
+        )
+
+        self.sync.cxp_upconn += [
+            self.tx_fifos.disp_in.eq(disp),
+            self.tx_idle.disp_in.eq(disp),
+
+            If(tx_en,
+                o.eq(tx_reg[0]),
+                tx_reg.eq(Cat(tx_reg[1:], 0)),
+                tx_bitcount.eq(tx_bitcount + 1),   
+
+
+                # char boundary
+                If(tx_bitcount == 9,
+                    tx_bitcount.eq(0),
+                    If((~self.tx_fifos.pe.n) & (self.tx_fifos.pe.o == 0),
+                        # trigger packets are inserted at char boundary and don't contribute to word count
+                        tx_reg.eq(self.tx_fifos.source_data[0]),
+                        self.tx_fifos.source_ack[0].eq(1),
+                        disp.eq(self.tx_fifos.disp_out[0]),
+                    ).Else(
+                        # word boundary
+                        If(tx_wordcount == 3,
+                            tx_wordcount.eq(0),
+                            If(~self.tx_fifos.pe.n,
+                                # priority lv 1 & 2 packets are inserted at word boundary
+                                idling.eq(0),
+                                priorities.eq(self.tx_fifos.pe.o),
+                                self.tx_fifos.source_ack[self.tx_fifos.pe.o].eq(1),
+                                tx_reg.eq(self.tx_fifos.source_data[self.tx_fifos.pe.o]),
+                                disp.eq(self.tx_fifos.disp_out[self.tx_fifos.pe.o]),
+                            ).Else(
+                                idling.eq(1),
+                                self.tx_idle.source_ack.eq(1),
+                                tx_reg.eq(self.tx_idle.source_data),    
+                                disp.eq(self.tx_idle.disp_out),
+                            )
+                        ).Else(
+                            tx_wordcount.eq(tx_wordcount + 1),
+                            If(~idling,
+                                self.tx_fifos.source_ack[priorities].eq(1),
+                                tx_reg.eq(self.tx_fifos.source_data[priorities]),
+                                disp.eq(self.tx_fifos.disp_out[priorities]),
+                            ).Else(
+                                self.tx_idle.source_ack.eq(1),
+                                tx_reg.eq(self.tx_idle.source_data),    
+                                disp.eq(self.tx_idle.disp_out),
+                            )
+                        ),       
+                    )
+                )
+            ).Else(
+                o.eq(0)
+            )
+        ]
+        # DEBUG: remove pads
+        self.encoded_data = CSRStatus(10)
+        self.sync.cxp_upconn +=[
+            If(tx_bitcount == 0,
+                self.encoded_data.status.eq(tx_reg),
+            )
+        ]
+       
+        ninth_bit = Signal()
+        word_bound = Signal()
+
+        p0 = Signal()
+        p3 = Signal()
+        self.comb += [
+           ninth_bit.eq(tx_bitcount == 9),
+           word_bound.eq(tx_wordcount == 3),
+           p0.eq(self.tx_idle.word_idx == 0),
+           p3.eq(self.tx_idle.word_idx == 3),
+        ] 
+        self.specials += [
+            # # debug sma
+            # Instance("OBUF", i_I=o, o_O=pads.p_tx),
+            # Instance("OBUF", i_I=self.cd_cxp_upconn.clk, o_O=pads.n_rx),
+
+            # # pmod 0-7 pin
+            # Instance("OBUF", i_I=o, o_O=pmod[0]),
+            # Instance("OBUF", i_I=self.cd_cxp_upconn.clk, o_O=pmod[1]),
+            # Instance("OBUF", i_I=~self.tx_fifos.pe.n, o_O=pmod[2]),
+            # Instance("OBUF", i_I=ninth_bit, o_O=pmod[3]),
+            # Instance("OBUF", i_I=word_bound, o_O=pmod[4]),
+            # Instance("OBUF", i_I=idling, o_O=pmod[5]),
+            # # Instance("OBUF", i_I=self.tx_fifos.source_ack[0], o_O=pmod[6]),
+            # # Instance("OBUF", i_I=self.tx_fifos.source_ack[2], o_O=pmod[6]),
+            # # Instance("OBUF", i_I=self.tx_fifos.source_ack[1], o_O=pmod[7]),
+            # Instance("OBUF", i_I=p0, o_O=pmod[6]),
+            # Instance("OBUF", i_I=p3, o_O=pmod[7]),
+        ]
+        self.symbol0 = CSR(9)
+        self.symbol1 = CSR(9)
+        self.symbol2 = CSR(9)
+
+        self.sync += [
+            self.tx_fifos.sink_stb[0].eq(self.symbol0.re),
+            self.tx_fifos.sink_data[0].eq(self.symbol0.r),
+            self.tx_fifos.sink_stb[1].eq(self.symbol1.re),
+            self.tx_fifos.sink_data[1].eq(self.symbol1.r),
+            self.tx_fifos.sink_stb[2].eq(self.symbol2.re),
+            self.tx_fifos.sink_data[2].eq(self.symbol2.r),
+        ]
+
+class TxFIFOs(Module):
+    def __init__(self, nfifos, fifo_depth):
+        self.disp_in = Signal()
+        self.disp_out = Array(Signal() for _ in range(nfifos))
+        
+        self.sink_stb = Signal(nfifos)
+        self.sink_ack = Signal(nfifos) 
+        self.sink_data = [Signal(9) for _ in range(nfifos)]
+        
+        self.source_ack = Array(Signal() for _ in range(nfifos))
+        self.source_data = Array(Signal(10) for _ in range(nfifos))
+
+        # # #
+
+        source_stb = Signal(nfifos)
+
+        for i in range(nfifos):
+            cdr = ClockDomainsRenamer({"write": "sys", "read": "cxp_upconn"})
+            fifo = cdr(stream.AsyncFIFO([("data", 9)], fifo_depth))
+            encoder = ClockDomainsRenamer("cxp_upconn")(SingleEncoder(True))
+            setattr(self.submodules, "tx_fifo" + str(i), fifo)
+            setattr(self.submodules, "tx_encoder" + str(i), encoder)
+            self.sync += [
+                fifo.sink.stb.eq(self.sink_stb[i]),
+                self.sink_ack[i].eq(fifo.sink.ack),
+                fifo.sink.data.eq(self.sink_data[i]),
+            ]
+            self.sync.cxp_upconn += [
+                encoder.d.eq(fifo.source.data[:8]),
+                encoder.k.eq(fifo.source.data[8]),
+                encoder.disp_in.eq(self.disp_in),
+                self.disp_out[i].eq(encoder.disp_out),
+
+                source_stb[i].eq(fifo.source.stb),
+                fifo.source.ack.eq(self.source_ack[i]),
+                self.source_data[i].eq(encoder.output),
+                # reset ack after asserted 
+                If(self.source_ack[i], self.source_ack[i].eq(0)),
+            ]
+
+        # FIFOs transmission priority
+        self.submodules.pe = PriorityEncoder(nfifos)
+        self.comb += self.pe.i.eq(source_stb)
+
+class TxIdle(Module):
+    def __init__(self):
+        self.disp_in = Signal()
+        self.disp_out = Signal()
+
+        self.word_idx = Signal(max=4)
+        self.source_ack = Signal()
+        self.source_data = Signal(10)
+        
+        # # #
+
+        # CXP 2.1 section 9.2.5 
+        IDLE_CHARS = Array([
+            #[char, k]
+            [0b10111100, 1], #K28.5
+            [0b00111100, 1], #K28.1
+            [0b00111100, 1], #K28.1
+            [0b10111100, 0], #D28.5
+        ])
+
+        encoder = ClockDomainsRenamer("cxp_upconn")(SingleEncoder(True))
+        self.submodules += encoder
+
+        self.sync.cxp_upconn += [
+            encoder.d.eq(IDLE_CHARS[self.word_idx][0]),
+            encoder.k.eq(IDLE_CHARS[self.word_idx][1]),
+            encoder.disp_in.eq(self.disp_in),
+            self.disp_out.eq(encoder.disp_out),
+            self.source_data.eq(encoder.output),
+
+            If(self.source_ack,
+                # reset after asserted
+                self.source_ack.eq(0),
+
+                If(self.word_idx != 3,
+                   self.word_idx.eq(self.word_idx + 1),
+                ).Else(
+                    self.word_idx.eq(0),
+                )
+            ),
+        ]

src/gateware/kasli_soc.py

@@ -560,7 +560,10 @@ class GenericSatellite(SoCCore):
         self.submodules.local_io = SyncRTIO(
             self.rtio_tsc, self.rtio_channels, lane_count=description["sed_lanes"]
         )
-        self.comb += self.drtiosat.async_errors.eq(self.local_io.async_errors)
+        self.comb += [
+            self.drtiosat.async_errors.eq(self.local_io.async_errors),
+            self.local_io.sed_spread_enable.eq(self.drtiosat.sed_spread_enable.storage)
+        ]
 
         self.submodules.cri_con = rtio.CRIInterconnectShared(
             [self.drtiosat.cri, self.rtio_dma.cri, self.rtio.cri],

src/gateware/zc706.py

@@ -25,6 +25,7 @@ import analyzer
 import acpki
 import drtio_aux_controller
 import zynq_clocking
+import cxp_4r_fmc, cxp
 from config import write_csr_file, write_mem_file, write_rustc_cfg_file
 
 class SMAClkinForward(Module):
@@ -138,7 +139,7 @@ class ZC706(SoCCore):
         platform.add_extension(si5324_fmc33)
         self.comb += platform.request("si5324_33").rst_n.eq(1)
 
-        cdr_clk = Signal()
+        self.cdr_clk = Signal()
         cdr_clk_buf = Signal()
         si5324_out = platform.request("si5324_clkout")
         platform.add_period_constraint(si5324_out.p, 8.0)
@@ -146,11 +147,11 @@ class ZC706(SoCCore):
             Instance("IBUFDS_GTE2",
                 i_CEB=0,
                 i_I=si5324_out.p, i_IB=si5324_out.n,
-                o_O=cdr_clk,
+                o_O=self.cdr_clk,
                 p_CLKCM_CFG="TRUE",
                 p_CLKRCV_TRST="TRUE",
                 p_CLKSWING_CFG=3),
-            Instance("BUFG", i_I=cdr_clk, o_O=cdr_clk_buf)
+            Instance("BUFG", i_I=self.cdr_clk, o_O=cdr_clk_buf)
         ]
         self.config["HAS_SI5324"] = None
         self.config["SI5324_AS_SYNTHESIZER"] = None
@@ -487,6 +488,10 @@ class _SatelliteBase(SoCCore):
         self.csr_devices.append("rtio_dma")
 
         self.submodules.local_io = SyncRTIO(self.rtio_tsc, rtio_channels)
+        self.comb += [
+            self.drtiosat.async_errors.eq(self.local_io.async_errors),
+            self.local_io.sed_spread_enable.eq(self.drtiosat.sed_spread_enable.storage)
+        ]
         self.submodules.cri_con = rtio.CRIInterconnectShared(
             [self.drtiosat.cri, self.rtio_dma.cri, self.rtio.cri],
             [self.local_io.cri] + self.drtio_cri,
@@ -648,6 +653,67 @@ class _NIST_QC2_RTIO:
         self.add_rtio(rtio_channels)
 
 
+class CXP_FMC():
+    """
+    CoaXpress FMC with 4 CXP channel and 1 SMA trigger
+    """
+    def __init__(self):
+        platform = self.platform
+        platform.add_extension(cxp_4r_fmc.fmc_adapter_io)
+        platform.add_extension(leds_fmc33)
+
+        debug_sma = [
+            ("user_sma_clock_33", 0,
+                Subsignal("p_tx", Pins("AD18"), IOStandard("LVCMOS33")),
+                Subsignal("n_rx", Pins("AD19"), IOStandard("LVCMOS33")),
+            ),
+        ]
+
+        pmod1_33 = [
+            ("pmod1_33", 0, Pins("AJ21"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 1, Pins("AK21"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 2, Pins("AB21"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 3, Pins("AB16"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 4, Pins("Y20"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 5, Pins("AA20"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 6, Pins("AC18"), IOStandard("LVCMOS33")),
+            ("pmod1_33", 7, Pins("AC19"), IOStandard("LVCMOS33")),
+        ]
+
+        platform.add_extension(debug_sma)
+        platform.add_extension(pmod1_33)
+        pmod_pads = [platform.request("pmod1_33", i) for i in range(8)]
+
+        clk_freq = 125e6
+
+        self.submodules.cxp = cxp.CXP(
+            refclk=self.cdr_clk,
+            pads=platform.request("CXP_HS", 0),
+            sys_clk_freq=clk_freq,
+            debug_sma=platform.request("user_sma_clock_33"),
+            pmod_pads = pmod_pads
+        )
+        self.csr_devices.append("cxp")
+
+        # max freq of cxp_gtx_rx = linerate/internal_datawidth = 12.5Gbps/40 = 312.5MHz
+        platform.add_period_constraint(self.cxp.downconn.gtx.cd_cxp_gtx_tx.clk, 3.2)
+        platform.add_period_constraint(self.cxp.downconn.gtx.cd_cxp_gtx_rx.clk, 3.2)
+        platform.add_false_path_constraints(self.cxp.downconn.gtx.cd_cxp_gtx_tx.clk, self.cxp.downconn.gtx.cd_cxp_gtx_rx.clk)
+
+        rtio_channels = []
+        # FIXME remove this placeholder RTIO channel
+        # There are too few RTIO channels and cannot be compiled (adr width issue of the lane distributor)
+        # see https://github.com/m-labs/artiq/pull/2158 for similar issue
+        print("USER LED at RTIO channel 0x{:06x}".format(len(rtio_channels)))
+        phy = ttl_simple.Output(self.platform.request("user_led_33", 0))
+        self.submodules += phy
+        rtio_channels.append(rtio.Channel.from_phy(phy))
+        self.config["HAS_RTIO_LOG"] = None
+        rtio_channels.append(rtio.LogChannel())
+
+        self.config["RTIO_LOG_CHANNEL"] = len(rtio_channels)
+        self.add_rtio(rtio_channels)
+
 class NIST_CLOCK(ZC706, _NIST_CLOCK_RTIO):
     def __init__(self, acpki, drtio100mhz):
         ZC706.__init__(self, acpki)
@@ -680,8 +746,13 @@ class NIST_QC2_Satellite(_SatelliteBase, _NIST_QC2_RTIO):
         _SatelliteBase.__init__(self, acpki, drtio100mhz)
         _NIST_QC2_RTIO.__init__(self)
 
+class CXP_Demo(ZC706, CXP_FMC):
+    def __init__(self, acpki, drtio100mhz):
+        ZC706.__init__(self, acpki)
+        CXP_FMC.__init__(self)
+
 VARIANTS = {cls.__name__.lower(): cls for cls in [NIST_CLOCK, NIST_CLOCK_Master, NIST_CLOCK_Satellite,
-                                                  NIST_QC2, NIST_QC2_Master, NIST_QC2_Satellite]}
+                                                  NIST_QC2, NIST_QC2_Master, NIST_QC2_Satellite, CXP_Demo]}
 
 def main():
     parser = argparse.ArgumentParser(

src/libboard_artiq/src/cxp_downconn.rs

@@ -0,0 +1,554 @@
+use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
+use libboard_zynq::{println, timer::GlobalTimer};
+use log::info;
+
+// use log::info;
+use crate::pl::csr;
+
+pub struct CXP_DownConn_Settings {
+    pub rxdiv: u8,
+    pub qpll_fbdiv: u8,
+}
+
+#[derive(Clone, Copy, Debug)]
+#[allow(non_camel_case_types)]
+pub enum CXP_SPEED {
+    CXP_1,
+    CXP_2,
+    CXP_3,
+    CXP_5,
+    CXP_6,
+    CXP_10,
+    CXP_12,
+}
+
+pub fn loopback_testing(timer: &mut GlobalTimer, speed: CXP_SPEED) {
+    println!("==============================================================================");
+    CXP_GTX::change_linerate(timer, speed);
+
+    unsafe {
+        info!("waiting for tx&rx setup...");
+        timer.delay_us(50_000);
+        info!(
+            "tx_phaligndone = {} | rx_phaligndone = {}",
+            csr::cxp::downconn_txinit_phaligndone_read(),
+            csr::cxp::downconn_rxinit_phaligndone_read(),
+        );
+
+        // enable txdata tranmission thought MGTXTXP, required by PMA loopback
+        csr::cxp::downconn_txenable_write(1);
+
+        info!("waiting for rx to align...");
+        while csr::cxp::downconn_rx_ready_read() != 1 {}
+        info!("rx ready!");
+
+        // loop {
+        for _ in 0..20 {
+            // NOTE: raw bits
+            // let data0 = csr::cxp::downconn_rxdata_0_read();
+            // let data1 = csr::cxp::downconn_rxdata_1_read();
+            // let data2 = csr::cxp::downconn_rxdata_2_read();
+            // let data3 = csr::cxp::downconn_rxdata_3_read();
+            // let rxready = csr::cxp::downconn_rx_ready_read();
+            // timer.delay_us(100);
+            // if data0 == 0b0101111100 || data0 == 0b1010000011 {
+            //     println!(
+            //         "data[0] = {:#012b} comma = {} | rx ready = {}",
+            //         data0,
+            //         data0 == 0b0101111100 || data0 == 0b1010000011,
+            //         rxready,
+            //     );
+            //     timer.delay_us(1_000_000);
+            // } else if data0 == 0b1001111100 || data0 == 0b0110000011 {
+            //     println!(
+            //         "data[0] = {:#012b} K28.1 | rx ready = {}",
+            //         data0,
+            //         rxready,
+            //     );
+            //     timer.delay_us(1_000_000);
+            // } else {
+            //     println!(
+            //         "data[0] = {:#012b} | rx ready = {}",
+            //         data0,
+            //         rxready,
+            //     );
+            //     timer.delay_us(1_000_000);
+            // }
+
+            // timer.delay_us(1_000_000);
+            // NOTE: raw bits
+            // let data0 = csr::cxp::downconn_rxdata_0_read();
+            // let data1 = csr::cxp::downconn_rxdata_1_read();
+            // let data2 = csr::cxp::downconn_rxdata_2_read();
+            // let data3 = csr::cxp::downconn_rxdata_3_read();
+            // println!(
+            //     "0b{:010b} {:010b} {:010b} {:010b}",
+            //     data0, data1, data2, data3
+            // );
+
+            // NOTE:decode data
+            // let data0_k = csr::cxp::downconn_decoded_k_0_read();
+            // let data1_k = csr::cxp::downconn_decoded_k_1_read();
+            // let data2_k = csr::cxp::downconn_decoded_k_2_read();
+            // let data3_k = csr::cxp::downconn_decoded_k_3_read();
+            let data0_decoded = csr::cxp::downconn_decoded_data_0_read();
+            let data1_decoded = csr::cxp::downconn_decoded_data_1_read();
+            let data2_decoded = csr::cxp::downconn_decoded_data_2_read();
+            let data3_decoded = csr::cxp::downconn_decoded_data_3_read();
+            println!(
+                "{:#04x} {:#04x} {:#04x} {:#04x}",
+                data0_decoded, data1_decoded, data2_decoded, data3_decoded,
+            );
+            // println!(
+            //     "decoded_data[0] = {:#04x} decoded_k[0] = {:#b} decoded_data[1] = {:#04x} decoded_k[1] = {:#b}",
+            //     data0_decoded,
+            //     data0_k,
+            //     data1_decoded,
+            //     data1_k,
+            // );
+        }
+    }
+}
+
+pub fn setup(timer: &mut GlobalTimer) {
+    unsafe {
+        info!("turning on pmc loopback mode...");
+        csr::cxp::downconn_loopback_mode_write(0b010); // Near-End PMA Loopback
+
+        // QPLL setup
+        csr::cxp::downconn_qpll_reset_write(1);
+        info!("waiting for QPLL/CPLL to lock...");
+        while csr::cxp::downconn_qpll_locked_read() != 1 {}
+        info!("QPLL locked");
+
+        // tx/rx setup
+        csr::cxp::downconn_tx_start_init_write(1);
+        csr::cxp::downconn_rx_start_init_write(1);
+
+        info!("waiting for tx & rx setup...");
+        timer.delay_us(50_000);
+        info!(
+            "tx_phaligndone = {} | rx_phaligndone = {}",
+            csr::cxp::downconn_txinit_phaligndone_read(),
+            csr::cxp::downconn_rxinit_phaligndone_read(),
+        );
+    }
+
+    CXP_GTX::change_linerate(timer, CXP_SPEED::CXP_1);
+}
+
+pub mod CXP_GTX {
+    use super::*;
+
+    struct RX_CDR_CFG {
+        pub cfg_reg0: u16, //0x0A8
+        pub cfg_reg1: u16, //0x0A9
+        pub cfg_reg2: u16, //0x0AA
+        pub cfg_reg3: u16, //0x0AB
+        pub cfg_reg4: u16, //0x0AC
+    }
+
+    pub fn change_linerate(timer: &mut GlobalTimer, speed: CXP_SPEED) {
+        info!("Changing datarate to {:?}", speed);
+        // DEBUG: DRP pll for TXUSRCLK = freq(linerate)/20
+        let settings = txusrclk::get_txusrclk_config(speed);
+        txusrclk::setup(timer, settings);
+
+        change_qpll_settings(speed);
+        change_cdr_cfg(speed);
+
+        unsafe {
+            csr::cxp::downconn_qpll_reset_write(1);
+            info!("waiting for QPLL/CPLL to lock...");
+            while csr::cxp::downconn_qpll_locked_read() != 1 {}
+            info!("QPLL locked");
+        }
+
+        unsafe {
+            csr::cxp::downconn_tx_restart_write(1);
+            csr::cxp::downconn_rx_restart_write(1);
+        }
+    }
+
+    fn change_qpll_settings(speed: CXP_SPEED) {
+        // Change QPLL_FBDIV
+        let qpll_div_reg = match speed {
+            CXP_SPEED::CXP_1 | CXP_SPEED::CXP_2 | CXP_SPEED::CXP_5 | CXP_SPEED::CXP_10 => 0x0120, // FB_Divider = 80
+            CXP_SPEED::CXP_3 | CXP_SPEED::CXP_6 | CXP_SPEED::CXP_12 => 0x0170,                    // FB_Divider = 100
+        };
+
+        println!("0x36 = {:#018b}", qpll_read(0x36));
+        qpll_write(0x36, qpll_div_reg);
+        println!("0x36 = {:#018b}", qpll_read(0x36));
+
+        let rxout_div = match speed {
+            CXP_SPEED::CXP_1 => 0b100,                      // 8
+            CXP_SPEED::CXP_2 | CXP_SPEED::CXP_3 => 0b011,   // 4
+            CXP_SPEED::CXP_5 | CXP_SPEED::CXP_6 => 0b010,   // 2
+            CXP_SPEED::CXP_10 | CXP_SPEED::CXP_12 => 0b001, // 1
+        };
+
+        unsafe {
+            csr::cxp::downconn_rx_div_write(rxout_div);
+            csr::cxp::downconn_tx_div_write(rxout_div);
+        }
+    }
+
+    fn change_cdr_cfg(speed: CXP_SPEED) {
+        let cdr_cfg = match speed {
+            // rxout_div = 8
+            CXP_SPEED::CXP_1 => {
+                RX_CDR_CFG {
+                    cfg_reg0: 0x0020, //0x0A8
+                    cfg_reg1: 0x1008, //0x0A9
+                    cfg_reg2: 0x23FF, //0x0AA
+                    cfg_reg3: 0x0000, //0x0AB
+                    cfg_reg4: 0x0003, //0x0AC
+                }
+            }
+            // rxout_div = 4
+            CXP_SPEED::CXP_2 | CXP_SPEED::CXP_5 => {
+                RX_CDR_CFG {
+                    cfg_reg0: 0x0020, //0x0A8
+                    cfg_reg1: 0x1010, //0x0A9
+                    cfg_reg2: 0x23FF, //0x0AA
+                    cfg_reg3: 0x0000, //0x0AB
+                    cfg_reg4: 0x0003, //0x0AC
+                }
+            }
+            // rxout_div = 2
+            CXP_SPEED::CXP_3 | CXP_SPEED::CXP_6 => {
+                RX_CDR_CFG {
+                    cfg_reg0: 0x0020, //0x0A8
+                    cfg_reg1: 0x1020, //0x0A9
+                    cfg_reg2: 0x23FF, //0x0AA
+                    cfg_reg3: 0x0000, //0x0AB
+                    cfg_reg4: 0x0003, //0x0AC
+                }
+            }
+            // // Divided by 1
+            // CXP_SPEED::CXP_6 => {
+            //     RX_CDR_CFG {
+            //         cfg_reg0: 0x0020, //0x0A8
+            //         cfg_reg1: 0x1040, //0x0A9
+            //         cfg_reg2: 0x23FF, //0x0AA
+            //         cfg_reg3: 0x0000, //0x0AB
+            //         cfg_reg4: 0x0003, //0x0AC
+            //     }
+            // }
+            // rxout_div = 1
+            CXP_SPEED::CXP_10 | CXP_SPEED::CXP_12 => {
+                RX_CDR_CFG {
+                    cfg_reg0: 0x0020, //0x0A8
+                    cfg_reg1: 0x1040, //0x0A9
+                    cfg_reg2: 0x23FF, //0x0AA
+                    cfg_reg3: 0x0000, //0x0AB
+                    cfg_reg4: 0x000B, //0x0AC
+                }
+            }
+        };
+
+        gtx_write(0x0A8, cdr_cfg.cfg_reg0);
+        gtx_write(0x0A9, cdr_cfg.cfg_reg1);
+        gtx_write(0x0AA, cdr_cfg.cfg_reg2);
+        gtx_write(0x0AB, cdr_cfg.cfg_reg3);
+        gtx_write(0x0AC, cdr_cfg.cfg_reg4);
+    }
+
+    fn gtx_read(address: u16) -> u16 {
+        // DEBUG: DRPCLK need to be on for a few cycle before accessing other DRP ports
+        unsafe {
+            csr::cxp::downconn_gtx_daddr_write(address);
+            csr::cxp::downconn_gtx_dread_write(1);
+            while (csr::cxp::downconn_gtx_dready_read() != 1) {}
+            csr::cxp::downconn_gtx_dout_read()
+        }
+    }
+
+    fn gtx_write(address: u16, value: u16) {
+        // DEBUG: DRPCLK need to be on for a few cycle before accessing other DRP ports
+        unsafe {
+            csr::cxp::downconn_gtx_daddr_write(address);
+            csr::cxp::downconn_gtx_din_write(value);
+            csr::cxp::downconn_gtx_din_stb_write(1);
+            while (csr::cxp::downconn_gtx_dready_read() != 1) {}
+        }
+    }
+
+    fn qpll_read(address: u8) -> u16 {
+        unsafe {
+            csr::cxp::downconn_qpll_daddr_write(address);
+            csr::cxp::downconn_qpll_dread_write(1);
+            while (csr::cxp::downconn_qpll_dready_read() != 1) {}
+            csr::cxp::downconn_qpll_dout_read()
+        }
+    }
+
+    fn qpll_write(address: u8, value: u16) {
+        unsafe {
+            csr::cxp::downconn_qpll_daddr_write(address);
+            csr::cxp::downconn_qpll_din_write(value);
+            csr::cxp::downconn_qpll_din_stb_write(1);
+            while (csr::cxp::downconn_qpll_dready_read() != 1) {}
+        }
+    }
+}
+
+pub mod txusrclk {
+    use super::*;
+
+    pub struct PLLSetting {
+        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::cxp::downconn_pll_dclk_write(1);
+            csr::cxp::downconn_pll_dclk_write(0);
+        }
+    }
+
+    fn set_addr(address: u8) {
+        unsafe {
+            csr::cxp::downconn_pll_daddr_write(address);
+        }
+    }
+
+    fn set_data(value: u16) {
+        unsafe {
+            csr::cxp::downconn_pll_din_write(value);
+        }
+    }
+
+    fn set_enable(en: bool) {
+        unsafe {
+            let val = if en { 1 } else { 0 };
+            csr::cxp::downconn_pll_den_write(val);
+        }
+    }
+
+    fn set_write_enable(en: bool) {
+        unsafe {
+            let val = if en { 1 } else { 0 };
+            csr::cxp::downconn_pll_dwen_write(val);
+        }
+    }
+
+    fn get_data() -> u16 {
+        unsafe { csr::cxp::downconn_pll_dout_read() }
+    }
+
+    fn drp_ready() -> bool {
+        unsafe { csr::cxp::downconn_pll_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::cxp::downconn_txpll_reset_write(val)
+        }
+    }
+
+    pub fn setup(timer: &mut GlobalTimer, settings: PLLSetting) {
+        if false {
+            info!("0x08 = {:#06x}", read(0x08));
+            info!("0x09 = {:#06x}", read(0x09));
+            info!("0x14 = {:#06x}", read(0x14));
+            info!("0x15 = {:#06x}", read(0x15));
+            info!("0x16 = {:#06x}", read(0x16));
+            info!("0x18 = {:#06x}", read(0x18));
+            info!("0x19 = {:#06x}", read(0x19));
+            info!("0x1A = {:#06x}", read(0x1A));
+            info!("0x28 = {:#06x}", read(0x28));
+            info!("0x4E = {:#06x}", read(0x4E));
+            info!("0x4F = {:#06x}", read(0x4F));
+        } else {
+            // Based on "DRP State Machine" from XAPP888
+            // hold reset HIGH during pll 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 pll to lock
+            timer.delay_us(100);
+
+            let locked = unsafe { csr::cxp::downconn_txpll_locked_read() == 1 };
+            info!("txusrclk locked = {}", locked);
+        }
+    }
+
+    pub fn get_txusrclk_config(speed: CXP_SPEED) -> PLLSetting {
+        match speed {
+            CXP_SPEED::CXP_1 => {
+                // CLKFBOUT_MULT = 8, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 32
+                // TXUSRCLK=62.5MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1410,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1104, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x5801,     //0x19
+                    lock_reg3: 0xdbe9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9808,     //0x4E
+                    filt_reg2: 0x9100,     //0x4F
+                }
+            }
+            CXP_SPEED::CXP_2 => {
+                // CLKFBOUT_MULT = 8, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 16
+                // TXUSRCLK=62.5MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1208,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1104, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x5801,     //0x19
+                    lock_reg3: 0xdbe9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9808,     //0x4E
+                    filt_reg2: 0x9100,     //0x4F
+                }
+            }
+            CXP_SPEED::CXP_3 => {
+                // CLKFBOUT_MULT = 10, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 16
+                // TXUSRCLK=78.125MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1208,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1145, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x7001,     //0x19
+                    lock_reg3: 0xf3e9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9908,     //0x4E
+                    filt_reg2: 0x1900,     //0x4F
+                }
+            }
+            CXP_SPEED::CXP_5 => {
+                // CLKFBOUT_MULT = 8, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 8
+                // TXUSRCLK=125MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1104,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1104, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x5801,     //0x19
+                    lock_reg3: 0xdbe9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9808,     //0x4E
+                    filt_reg2: 0x9100,     //0x4F
+                }
+            }
+            CXP_SPEED::CXP_6 => {
+                // CLKFBOUT_MULT = 10, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 8
+                // TXUSRCLK=156.25MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1104,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1145, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x7001,     //0x19
+                    lock_reg3: 0xf3e9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9908,     //0x4E
+                    filt_reg2: 0x1900,     //0x4F
+                }
+            }
+            CXP_SPEED::CXP_10 => {
+                // CLKFBOUT_MULT = 8, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 4
+                // TXUSRCLK=250MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1082,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1104, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x5801,     //0x19
+                    lock_reg3: 0xdbe9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9808,     //0x4E
+                    filt_reg2: 0x9100,     //0x4F
+                }
+            }
+            CXP_SPEED::CXP_12 => {
+                // CLKFBOUT_MULT = 10, DIVCLK_DIVIDE = 1 , CLKOUT0_DIVIDE = 4
+                // TXUSRCLK=312.5MHz
+                PLLSetting {
+                    clkout0_reg1: 0x1082,  //0x08
+                    clkout0_reg2: 0x0000,  //0x09
+                    clkfbout_reg1: 0x1145, //0x14
+                    clkfbout_reg2: 0x0000, //0x15
+                    div_reg: 0x1041,       //0x16
+                    lock_reg1: 0x03e8,     //0x18
+                    lock_reg2: 0x7001,     //0x19
+                    lock_reg3: 0xf3e9,     //0x1A
+                    power_reg: 0x0000,     //0x28
+                    filt_reg1: 0x9908,     //0x4E
+                    filt_reg2: 0x1900,     //0x4F
+                }
+            }
+        }
+    }
+}

src/libboard_artiq/src/cxp_upconn.rs

@@ -0,0 +1,88 @@
+use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
+use libboard_zynq::{println, timer::GlobalTimer};
+
+use crate::pl::csr;
+
+pub fn crc_test() {
+    let arr = [
+        0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, // CXP CRC-32
+        0x56, 0x86, 0x5D, 0x6f,
+    ];
+    let mut crc: u32; // seed = 0xFFFFFFFF
+
+    unsafe {
+        csr::cxp::crc_en_write(1);
+
+        for a in arr.iter() {
+            csr::cxp::crc_data_write(*a);
+            crc = csr::cxp::crc_value_read();
+            println!("input = {:#04x}", *a);
+            println!("CRC NOT(val.reverse) = {:#010x}", !crc.reverse_bits());
+            // since the input bit are reversed when entering the crc engine, the output char need to be reversed to cancel out on the receiver side
+            println!("CRC CXP = {:#010x}", crc);
+        }
+    }
+}
+
+pub fn tx_test(timer: &mut GlobalTimer) {
+    // the 8bit shift is k symbol
+    // const K28_1: u16 = 0x3C | (1 << 8);
+    // const K28_5: u16 = 0xBC | (1 << 8);
+    const D31_1: u16 = 0x3F;
+    const D01_1: u16 = 0x21;
+
+    const LEN: usize = 100;
+    let mut arr: [u16; LEN] = [0; LEN];
+
+    unsafe {
+        csr::cxp::upconn_clk_reset_write(1);
+        // csr::cxp::upconn_bitrate2x_enable_write(1);
+        csr::cxp::upconn_clk_reset_write(0);
+        loop {
+            // TODO: verify the char & word boundary thingy
+            for _ in 0..8 {
+                csr::cxp::upconn_symbol1_write(D01_1);
+            }
+
+            for _ in 0..4 {
+                csr::cxp::upconn_symbol2_write(D31_1);
+            }
+
+            timer.delay_us(1);
+            csr::cxp::upconn_tx_enable_write(1);
+
+            for i in 0..LEN {
+                arr[i] = get_encoded();
+            }
+            for i in 0..LEN {
+                match arr[i] {
+                    0b1010111001 | 0b0101001001 => {
+                        println!("encoded = {:#012b} D31.1", arr[i])
+                    }
+                    0b0111011001 | 0b1000101001 => {
+                        println!("encoded = {:#012b} D01.1", arr[i])
+                    }
+                    0b0011111010 | 0b1100000101 => {
+                        println!("encoded = {:#012b} K28.5 start idling....", arr[i])
+                    }
+                    0b0011111001 | 0b1100000110 => {
+                        println!("encoded = {:#012b} K28.1 idling...", arr[i])
+                    }
+                    0b0011101010 => {
+                        println!("encoded = {:#012b} D28.5 END idle", arr[i])
+                    }
+                    _ => {
+                        println!("encoded = {:#012b}", arr[i])
+                    }
+                }
+            }
+            println!("-------------------------------------");
+
+            csr::cxp::upconn_tx_enable_write(0);
+            timer.delay_us(2_000_000);
+        }
+    }
+    fn get_encoded() -> u16 {
+        unsafe { csr::cxp::upconn_encoded_data_read().reverse_bits() >> 6 }
+    }
+}

src/libboard_artiq/src/drtioaux_proto.rs

@@ -267,12 +267,14 @@ pub enum Packet {
         exception_src: u8,
     },
     SubkernelExceptionRequest {
+        source: u8,
         destination: u8,
     },
     SubkernelException {
+        destination: u8,
         last: bool,
         length: u16,
-        data: [u8; SAT_PAYLOAD_MAX_SIZE],
+        data: [u8; MASTER_PAYLOAD_MAX_SIZE],
     },
     SubkernelMessage {
         source: u8,
@@ -524,14 +526,17 @@ impl Packet {
                 exception_src: reader.read_u8()?,
             },
             0xc9 => Packet::SubkernelExceptionRequest {
+                source: reader.read_u8()?,
                 destination: reader.read_u8()?,
             },
             0xca => {
+                let destination = reader.read_u8()?;
                 let last = reader.read_bool()?;
                 let length = reader.read_u16()?;
-                let mut data: [u8; SAT_PAYLOAD_MAX_SIZE] = [0; SAT_PAYLOAD_MAX_SIZE];
+                let mut data: [u8; MASTER_PAYLOAD_MAX_SIZE] = [0; MASTER_PAYLOAD_MAX_SIZE];
                 reader.read_exact(&mut data[0..length as usize])?;
                 Packet::SubkernelException {
+                    destination: destination,
                     last: last,
                     length: length,
                     data: data,
@@ -896,12 +901,19 @@ impl Packet {
                 writer.write_bool(with_exception)?;
                 writer.write_u8(exception_src)?;
             }
-            Packet::SubkernelExceptionRequest { destination } => {
+            Packet::SubkernelExceptionRequest { source, destination } => {
                 writer.write_u8(0xc9)?;
+                writer.write_u8(source)?;
                 writer.write_u8(destination)?;
             }
-            Packet::SubkernelException { last, length, data } => {
+            Packet::SubkernelException {
+                destination,
+                last,
+                length,
+                data,
+            } => {
                 writer.write_u8(0xca)?;
+                writer.write_u8(destination)?;
                 writer.write_bool(last)?;
                 writer.write_u16(length)?;
                 writer.write_all(&data[0..length as usize])?;
@@ -943,6 +955,8 @@ impl Packet {
             Packet::SubkernelLoadRunReply { destination, .. } => Some(*destination),
             Packet::SubkernelMessage { destination, .. } => Some(*destination),
             Packet::SubkernelMessageAck { destination } => Some(*destination),
+            Packet::SubkernelExceptionRequest { destination, .. } => Some(*destination),
+            Packet::SubkernelException { destination, .. } => Some(*destination),
             Packet::DmaPlaybackStatus { destination, .. } => Some(*destination),
             Packet::SubkernelFinished { destination, .. } => Some(*destination),
             _ => None,

src/libboard_artiq/src/lib.rs

@@ -42,6 +42,11 @@ pub mod si5324;
 pub mod si549;
 use core::{cmp, str};
 
+#[cfg(has_cxp)]
+pub mod cxp_downconn;
+#[cfg(has_cxp)]
+pub mod cxp_upconn;
+
 pub fn identifier_read(buf: &mut [u8]) -> &str {
     unsafe {
         pl::csr::identifier::address_write(0);

src/libksupport/Cargo.toml.tpl

@@ -32,3 +32,9 @@ unwind = { path = "../libunwind" }
 libc = { path = "../libc" }
 io = { path = "../libio" }
 libboard_artiq = { path = "../libboard_artiq" }
+
+[dependencies.nalgebra]
+git = "https://git.m-labs.hk/M-Labs/nalgebra.git"
+rev = "dd00f9b"
+default-features = false
+features = ["libm", "alloc"]

src/libksupport/src/eh_artiq.rs

@@ -14,8 +14,10 @@
 
 use core::mem;
 
-use cslice::CSlice;
+use core_io::Error as ReadError;
+use cslice::{AsCSlice, CSlice};
 use dwarf::eh::{self, EHAction, EHContext};
+use io::{Cursor, ProtoRead};
 use libc::{c_int, c_void, uintptr_t};
 use log::{error, trace};
 use unwind as uw;
@@ -220,8 +222,6 @@ pub unsafe fn artiq_personality(
 }
 
 pub unsafe extern "C" fn raise(exception: *const Exception) -> ! {
-    use cslice::AsCSlice;
-
     let count = EXCEPTION_BUFFER.exception_count;
     let stack = &mut EXCEPTION_BUFFER.exception_stack;
     let diff = exception as isize - EXCEPTION_BUFFER.exceptions.as_ptr() as isize;
@@ -295,6 +295,60 @@ pub unsafe extern "C" fn raise(exception: *const Exception) -> ! {
     unreachable!();
 }
 
+fn read_exception_string<'a>(reader: &mut Cursor<&[u8]>) -> Result<CSlice<'a, u8>, ReadError> {
+    let len = reader.read_u32()? as usize;
+    if len == usize::MAX {
+        let data = reader.read_u32()?;
+        Ok(unsafe { CSlice::new(data as *const u8, len) })
+    } else {
+        let pos = reader.position();
+        let slice = unsafe {
+            let ptr = reader.get_ref().as_ptr().offset(pos as isize);
+            CSlice::new(ptr, len)
+        };
+        reader.set_position(pos + len);
+        Ok(slice)
+    }
+}
+
+fn read_exception(raw_exception: &[u8]) -> Result<Exception, ReadError> {
+    let mut reader = Cursor::new(raw_exception);
+
+    let mut byte = reader.read_u8()?;
+    // to sync
+    while byte != 0x5a {
+        byte = reader.read_u8()?;
+    }
+    // skip sync bytes, 0x09 indicates exception
+    while byte != 0x09 {
+        byte = reader.read_u8()?;
+    }
+    let _len = reader.read_u32()?;
+    // ignore the remaining exceptions, stack traces etc. - unwinding from another device would be unwise anyway
+    Ok(Exception {
+        id: reader.read_u32()?,
+        message: read_exception_string(&mut reader)?,
+        param: [
+            reader.read_u64()? as i64,
+            reader.read_u64()? as i64,
+            reader.read_u64()? as i64,
+        ],
+        file: read_exception_string(&mut reader)?,
+        line: reader.read_u32()?,
+        column: reader.read_u32()?,
+        function: read_exception_string(&mut reader)?,
+    })
+}
+
+pub fn raise_raw(raw_exception: &[u8]) -> ! {
+    use crate::artiq_raise;
+    if let Ok(exception) = read_exception(raw_exception) {
+        unsafe { raise(&exception) };
+    } else {
+        artiq_raise!("SubkernelError", "Error passing exception");
+    }
+}
+
 pub unsafe extern "C" fn resume() -> ! {
     trace!("resume");
     assert!(EXCEPTION_BUFFER.exception_count != 0);
@@ -421,20 +475,28 @@ extern "C" fn stop_fn(
     }
 }
 
-// Must be kept in sync with preallocate_runtime_exception_names() in artiq/language/embedding_map.py
+// Must be kept in sync with preallocate_runtime_exception_names() in `artiq.compiler.embedding`
-static EXCEPTION_ID_LOOKUP: [(&str, u32); 12] = [
+static EXCEPTION_ID_LOOKUP: [(&str, u32); 20] = [
-    ("RuntimeError", 0),
+    ("RTIOUnderflow", 0),
-    ("RTIOUnderflow", 1),
+    ("RTIOOverflow", 1),
-    ("RTIOOverflow", 2),
+    ("RTIODestinationUnreachable", 2),
-    ("RTIODestinationUnreachable", 3),
+    ("DMAError", 3),
-    ("DMAError", 4),
+    ("I2CError", 4),
-    ("I2CError", 5),
+    ("CacheError", 5),
-    ("CacheError", 6),
+    ("SPIError", 6),
-    ("SPIError", 7),
+    ("SubkernelError", 7),
-    ("ZeroDivisionError", 8),
+    ("AssertionError", 8),
-    ("IndexError", 9),
+    ("AttributeError", 9),
-    ("UnwrapNoneError", 10),
+    ("IndexError", 10),
-    ("SubkernelError", 11),
+    ("IOError", 11),
+    ("KeyError", 12),
+    ("NotImplementedError", 13),
+    ("OverflowError", 14),
+    ("RuntimeError", 15),
+    ("TimeoutError", 16),
+    ("TypeError", 17),
+    ("ValueError", 18),
+    ("ZeroDivisionError", 19),
 ];
 
 pub fn get_exception_id(name: &str) -> u32 {
@@ -469,3 +531,29 @@ macro_rules! artiq_raise {
     }};
     ($name:expr, $message:expr) => {{ artiq_raise!($name, $message, 0, 0, 0) }};
 }
+
+/// Takes as input exception id from host
+/// Generates a new exception with:
+///   * `id` set to `exn_id`
+///   * `message` set to corresponding exception name from `EXCEPTION_ID_LOOKUP`
+///
+/// The message is matched on host to ensure correct exception is being referred
+/// This test checks the synchronization of exception ids for runtime errors
+#[no_mangle]
+pub extern "C" fn test_exception_id_sync(exn_id: u32) {
+    let message = EXCEPTION_ID_LOOKUP
+        .iter()
+        .find_map(|&(name, id)| if id == exn_id { Some(name) } else { None })
+        .unwrap_or("unallocated internal exception id");
+
+    let exn = Exception {
+        id: exn_id,
+        file: file!().as_c_slice(),
+        line: 0,
+        column: 0,
+        function: "test_exception_id_sync".as_c_slice(),
+        message: message.as_c_slice(),
+        param: [0, 0, 0],
+    };
+    unsafe { raise(&exn) };
+}

src/libksupport/src/kernel/api.rs

@@ -9,7 +9,7 @@ use log::{info, warn};
 use super::subkernel;
 use super::{cache,
             core1::rtio_get_destination_status,
-            dma,
+            dma, linalg,
             rpc::{rpc_recv, rpc_send, rpc_send_async}};
 use crate::{eh_artiq, i2c, rtio};
 
@@ -303,6 +303,7 @@ pub fn resolve(required: &[u8]) -> Option<u32> {
         api_libm_f64f64f64!(nextafter),
         api_libm_f64f64f64!(pow),
         api_libm_f64f64!(round),
+        api_libm_f64f64!(rint),
         api_libm_f64f64!(sin),
         api_libm_f64f64!(sinh),
         api_libm_f64f64!(sqrt),
@@ -318,6 +319,26 @@ pub fn resolve(required: &[u8]) -> Option<u32> {
             }
             api!(yn = yn)
         },
+
+        // linalg
+        api!(np_linalg_cholesky = linalg::np_linalg_cholesky),
+        api!(np_linalg_qr = linalg::np_linalg_qr),
+        api!(np_linalg_svd = linalg::np_linalg_svd),
+        api!(np_linalg_inv = linalg::np_linalg_inv),
+        api!(np_linalg_pinv = linalg::np_linalg_pinv),
+        api!(np_linalg_matrix_power = linalg::np_linalg_matrix_power),
+        api!(np_linalg_det = linalg::np_linalg_det),
+        api!(sp_linalg_lu = linalg::sp_linalg_lu),
+        api!(sp_linalg_schur = linalg::sp_linalg_schur),
+        api!(sp_linalg_hessenberg = linalg::sp_linalg_hessenberg),
+
+        /*
+         * syscall for unit tests
+         * Used in `artiq.tests.coredevice.test_exceptions.ExceptionTest.test_raise_exceptions_kernel`
+         * This syscall checks that the exception IDs used in the Python `EmbeddingMap` (in `artiq.language.embedding`)
+         * match the `EXCEPTION_ID_LOOKUP` defined in the firmware (`libksupport::src::eh_artiq`)
+         */
+        api!(test_exception_id_sync = eh_artiq::test_exception_id_sync)
     ];
     api.iter()
         .find(|&&(exported, _)| exported.as_bytes() == required)

src/libksupport/src/kernel/linalg.rs

@@ -0,0 +1,440 @@
+// Uses `nalgebra` crate to invoke `np_linalg` and `sp_linalg` functions
+// When converting between `nalgebra::Matrix` and `NDArray` following considerations are necessary
+//
+// * Both `nalgebra::Matrix` and `NDArray` require their content to be stored in row-major order
+// * `NDArray` data pointer can be directly read and converted to `nalgebra::Matrix` (row and column number must be known)
+// * `nalgebra::Matrix::as_slice` returns the content of matrix in column-major order and initial data needs to be transposed before storing it in `NDArray` data pointer
+
+use alloc::vec::Vec;
+use core::slice;
+
+use nalgebra::DMatrix;
+
+use crate::artiq_raise;
+
+pub struct InputMatrix {
+    pub ndims: usize,
+    pub dims: *const usize,
+    pub data: *mut f64,
+}
+
+impl InputMatrix {
+    fn get_dims(&mut self) -> Vec<usize> {
+        let dims = unsafe { slice::from_raw_parts(self.dims, self.ndims) };
+        dims.to_vec()
+    }
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_cholesky(mat1: *mut InputMatrix, out: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out = out.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+    if dim1[0] != dim1[1] {
+        artiq_raise!(
+            "ValueError",
+            "last 2 dimensions of the array must be square: {1} != {2}",
+            0,
+            dim1[0] as i64,
+            dim1[1] as i64
+        );
+    }
+
+    let outdim = out.get_dims();
+    let out_slice = slice::from_raw_parts_mut(out.data, outdim[0] * outdim[1]);
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+
+    let matrix1 = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    let result = matrix1.cholesky();
+    match result {
+        Some(res) => {
+            out_slice.copy_from_slice(res.unpack().transpose().as_slice());
+        }
+        None => {
+            artiq_raise!("LinAlgError", "Matrix is not positive definite");
+        }
+    };
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_qr(mat1: *mut InputMatrix, out_q: *mut InputMatrix, out_r: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out_q = out_q.as_mut().unwrap();
+    let out_r = out_r.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+    let outq_dim = (*out_q).get_dims();
+    let outr_dim = (*out_r).get_dims();
+
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+    let out_q_slice = slice::from_raw_parts_mut(out_q.data, outq_dim[0] * outq_dim[1]);
+    let out_r_slice = slice::from_raw_parts_mut(out_r.data, outr_dim[0] * outr_dim[1]);
+
+    // Refer to https://github.com/dimforge/nalgebra/issues/735
+    let matrix1 = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+
+    let res = matrix1.qr();
+    let (q, r) = res.unpack();
+
+    // Uses different algo need to match numpy
+    out_q_slice.copy_from_slice(q.transpose().as_slice());
+    out_r_slice.copy_from_slice(r.transpose().as_slice());
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_svd(
+    mat1: *mut InputMatrix,
+    outu: *mut InputMatrix,
+    outs: *mut InputMatrix,
+    outvh: *mut InputMatrix,
+) {
+    let mat1 = mat1.as_mut().unwrap();
+    let outu = outu.as_mut().unwrap();
+    let outs = outs.as_mut().unwrap();
+    let outvh = outvh.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+    let outu_dim = (*outu).get_dims();
+    let outs_dim = (*outs).get_dims();
+    let outvh_dim = (*outvh).get_dims();
+
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+    let out_u_slice = slice::from_raw_parts_mut(outu.data, outu_dim[0] * outu_dim[1]);
+    let out_s_slice = slice::from_raw_parts_mut(outs.data, outs_dim[0]);
+    let out_vh_slice = slice::from_raw_parts_mut(outvh.data, outvh_dim[0] * outvh_dim[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    let result = matrix.svd(true, true);
+    out_u_slice.copy_from_slice(result.u.unwrap().transpose().as_slice());
+    out_s_slice.copy_from_slice(result.singular_values.as_slice());
+    out_vh_slice.copy_from_slice(result.v_t.unwrap().transpose().as_slice());
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_inv(mat1: *mut InputMatrix, out: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out = out.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+    let dim1 = (*mat1).get_dims();
+
+    if dim1[0] != dim1[1] {
+        artiq_raise!(
+            "ValueError",
+            "last 2 dimensions of the array must be square: {1} != {2}",
+            0,
+            dim1[0] as i64,
+            dim1[1] as i64
+        );
+    }
+
+    let outdim = out.get_dims();
+    let out_slice = slice::from_raw_parts_mut(out.data, outdim[0] * outdim[1]);
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    if !matrix.is_invertible() {
+        artiq_raise!("LinAlgError", "no inverse for Singular Matrix");
+    }
+    let inv = matrix.try_inverse().unwrap();
+    out_slice.copy_from_slice(inv.transpose().as_slice());
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_pinv(mat1: *mut InputMatrix, out: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out = out.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+    let dim1 = (*mat1).get_dims();
+    let outdim = out.get_dims();
+    let out_slice = slice::from_raw_parts_mut(out.data, outdim[0] * outdim[1]);
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    let svd = matrix.svd(true, true);
+    let inv = svd.pseudo_inverse(1e-15);
+
+    match inv {
+        Ok(m) => {
+            out_slice.copy_from_slice(m.transpose().as_slice());
+        }
+        Err(_) => {
+            artiq_raise!("LinAlgError", "SVD computation does not converge");
+        }
+    }
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_matrix_power(mat1: *mut InputMatrix, mat2: *mut InputMatrix, out: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let mat2 = mat2.as_mut().unwrap();
+    let out = out.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+    let power = slice::from_raw_parts_mut(mat2.data, 1);
+    let power = power[0];
+    let outdim = out.get_dims();
+    let out_slice = slice::from_raw_parts_mut(out.data, outdim[0] * outdim[1]);
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+    let mut abs_power = power;
+    if abs_power < 0.0 {
+        abs_power = abs_power * -1.0;
+    }
+    let matrix1 = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    if !matrix1.is_square() {
+        artiq_raise!(
+            "ValueError",
+            "last 2 dimensions of the array must be square: {1} != {2}",
+            0,
+            dim1[0] as i64,
+            dim1[1] as i64
+        );
+    }
+    let mut result = matrix1.pow(abs_power as u32);
+
+    if power < 0.0 {
+        if !matrix1.is_invertible() {
+            artiq_raise!("LinAlgError", "no inverse for Singular Matrix");
+        }
+        result = result.try_inverse().unwrap();
+    }
+    out_slice.copy_from_slice(result.transpose().as_slice());
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn np_linalg_det(mat1: *mut InputMatrix, out: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out = out.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+    let dim1 = (*mat1).get_dims();
+    let out_slice = slice::from_raw_parts_mut(out.data, 1);
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    if !matrix.is_square() {
+        artiq_raise!(
+            "ValueError",
+            "last 2 dimensions of the array must be square: {1} != {2}",
+            0,
+            dim1[0] as i64,
+            dim1[1] as i64
+        );
+    }
+    out_slice[0] = matrix.determinant();
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn sp_linalg_lu(mat1: *mut InputMatrix, out_l: *mut InputMatrix, out_u: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out_l = out_l.as_mut().unwrap();
+    let out_u = out_u.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+    let outl_dim = (*out_l).get_dims();
+    let outu_dim = (*out_u).get_dims();
+
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+    let out_l_slice = slice::from_raw_parts_mut(out_l.data, outl_dim[0] * outl_dim[1]);
+    let out_u_slice = slice::from_raw_parts_mut(out_u.data, outu_dim[0] * outu_dim[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    let (_, l, u) = matrix.lu().unpack();
+
+    out_l_slice.copy_from_slice(l.transpose().as_slice());
+    out_u_slice.copy_from_slice(u.transpose().as_slice());
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn sp_linalg_schur(mat1: *mut InputMatrix, out_t: *mut InputMatrix, out_z: *mut InputMatrix) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out_t = out_t.as_mut().unwrap();
+    let out_z = out_z.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+
+    if dim1[0] != dim1[1] {
+        artiq_raise!(
+            "ValueError",
+            "last 2 dimensions of the array must be square: {1} != {2}",
+            0,
+            dim1[0] as i64,
+            dim1[1] as i64
+        );
+    }
+
+    let out_t_dim = (*out_t).get_dims();
+    let out_z_dim = (*out_z).get_dims();
+
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+    let out_t_slice = slice::from_raw_parts_mut(out_t.data, out_t_dim[0] * out_t_dim[1]);
+    let out_z_slice = slice::from_raw_parts_mut(out_z.data, out_z_dim[0] * out_z_dim[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    let (z, t) = matrix.schur().unpack();
+
+    out_t_slice.copy_from_slice(t.transpose().as_slice());
+    out_z_slice.copy_from_slice(z.transpose().as_slice());
+}
+
+/// # Safety
+///
+/// `mat1` should point to a valid 2DArray of `f64` floats in row-major order
+#[no_mangle]
+pub unsafe extern "C" fn sp_linalg_hessenberg(
+    mat1: *mut InputMatrix,
+    out_h: *mut InputMatrix,
+    out_q: *mut InputMatrix,
+) {
+    let mat1 = mat1.as_mut().unwrap();
+    let out_h = out_h.as_mut().unwrap();
+    let out_q = out_q.as_mut().unwrap();
+
+    if mat1.ndims != 2 {
+        artiq_raise!(
+            "ValueError",
+            "expected 2D Vector Input, but received {1}D input)",
+            0,
+            mat1.ndims as i64,
+            0
+        );
+    }
+
+    let dim1 = (*mat1).get_dims();
+
+    if dim1[0] != dim1[1] {
+        artiq_raise!(
+            "ValueError",
+            "last 2 dimensions of the array must be square: {1} != {2}",
+            0,
+            dim1[0] as i64,
+            dim1[1] as i64
+        );
+    }
+
+    let out_h_dim = (*out_h).get_dims();
+    let out_q_dim = (*out_q).get_dims();
+
+    let data_slice1 = slice::from_raw_parts_mut(mat1.data, dim1[0] * dim1[1]);
+    let out_h_slice = slice::from_raw_parts_mut(out_h.data, out_h_dim[0] * out_h_dim[1]);
+    let out_q_slice = slice::from_raw_parts_mut(out_q.data, out_q_dim[0] * out_q_dim[1]);
+
+    let matrix = DMatrix::from_row_slice(dim1[0], dim1[1], data_slice1);
+    let (q, h) = matrix.hessenberg().unpack();
+
+    out_h_slice.copy_from_slice(h.transpose().as_slice());
+    out_q_slice.copy_from_slice(q.transpose().as_slice());
+}

src/libksupport/src/kernel/mod.rs

@@ -13,6 +13,7 @@ mod dma;
 mod rpc;
 pub use dma::DmaRecorder;
 mod cache;
+mod linalg;
 #[cfg(has_drtio)]
 mod subkernel;
 
@@ -23,6 +24,7 @@ pub enum SubkernelStatus {
     Timeout,
     IncorrectState,
     CommLost,
+    Exception(Vec<u8>),
     OtherError,
 }
 
@@ -90,9 +92,7 @@ pub enum Message {
         timeout: i64,
     },
     #[cfg(has_drtio)]
-    SubkernelAwaitFinishReply {
+    SubkernelAwaitFinishReply,
-        status: SubkernelStatus,
-    },
     #[cfg(has_drtio)]
     SubkernelMsgSend {
         id: u32,
@@ -109,9 +109,10 @@ pub enum Message {
     },
     #[cfg(has_drtio)]
     SubkernelMsgRecvReply {
-        status: SubkernelStatus,
         count: u8,
     },
+    #[cfg(has_drtio)]
+    SubkernelError(SubkernelStatus),
 }
 
 static CHANNEL_0TO1: Mutex<Option<sync_channel::Sender<'static, Message>>> = Mutex::new(None);

src/libksupport/src/kernel/subkernel.rs

@@ -3,7 +3,7 @@ use alloc::vec::Vec;
 use cslice::CSlice;
 
 use super::{Message, SubkernelStatus, KERNEL_CHANNEL_0TO1, KERNEL_CHANNEL_1TO0};
-use crate::{artiq_raise, rpc::send_args};
+use crate::{artiq_raise, eh_artiq, rpc::send_args};
 
 pub extern "C" fn load_run(id: u32, destination: u8, run: bool) {
     unsafe {
@@ -36,21 +36,18 @@ pub extern "C" fn await_finish(id: u32, timeout: i64) {
             });
     }
     match unsafe { KERNEL_CHANNEL_0TO1.as_mut().unwrap() }.recv() {
-        Message::SubkernelAwaitFinishReply {
+        Message::SubkernelAwaitFinishReply => (),
-            status: SubkernelStatus::NoError,
+        Message::SubkernelError(SubkernelStatus::IncorrectState) => {
-        } => (),
+            artiq_raise!("SubkernelError", "Subkernel not running")
-        Message::SubkernelAwaitFinishReply {
+        }
-            status: SubkernelStatus::IncorrectState,
+        Message::SubkernelError(SubkernelStatus::Timeout) => artiq_raise!("SubkernelError", "Subkernel timed out"),
-        } => artiq_raise!("SubkernelError", "Subkernel not running"),
+        Message::SubkernelError(SubkernelStatus::CommLost) => {
-        Message::SubkernelAwaitFinishReply {
+            artiq_raise!("SubkernelError", "Lost communication with satellite")
-            status: SubkernelStatus::Timeout,
+        }
-        } => artiq_raise!("SubkernelError", "Subkernel timed out"),
+        Message::SubkernelError(SubkernelStatus::OtherError) => {
-        Message::SubkernelAwaitFinishReply {
+            artiq_raise!("SubkernelError", "An error occurred during subkernel operation")
-            status: SubkernelStatus::CommLost,
+        }
-        } => artiq_raise!("SubkernelError", "Lost communication with satellite"),
+        Message::SubkernelError(SubkernelStatus::Exception(raw_exception)) => eh_artiq::raise_raw(&raw_exception),
-        Message::SubkernelAwaitFinishReply {
-            status: SubkernelStatus::OtherError,
-        } => artiq_raise!("SubkernelError", "An error occurred during subkernel operation"),
         _ => panic!("expected SubkernelAwaitFinishReply after SubkernelAwaitFinishRequest"),
     }
 }
@@ -92,30 +89,22 @@ pub extern "C" fn await_message(id: i32, timeout: i64, tags: &CSlice<u8>, min: u
             });
     }
     match unsafe { KERNEL_CHANNEL_0TO1.as_mut().unwrap() }.recv() {
-        Message::SubkernelMsgRecvReply {
+        Message::SubkernelMsgRecvReply { count } => {
-            status: SubkernelStatus::NoError,
-            count,
-        } => {
             if min > count || count > max {
                 artiq_raise!("SubkernelError", "Received more or less arguments than required")
             }
         }
-        Message::SubkernelMsgRecvReply {
+        Message::SubkernelError(SubkernelStatus::IncorrectState) => {
-            status: SubkernelStatus::IncorrectState,
+            artiq_raise!("SubkernelError", "Subkernel not running")
-            ..
+        }
-        } => artiq_raise!("SubkernelError", "Subkernel not running"),
+        Message::SubkernelError(SubkernelStatus::Timeout) => artiq_raise!("SubkernelError", "Subkernel timed out"),
-        Message::SubkernelMsgRecvReply {
+        Message::SubkernelError(SubkernelStatus::CommLost) => {
-            status: SubkernelStatus::Timeout,
+            artiq_raise!("SubkernelError", "Lost communication with satellite")
-            ..
+        }
-        } => artiq_raise!("SubkernelError", "Subkernel timed out"),
+        Message::SubkernelError(SubkernelStatus::OtherError) => {
-        Message::SubkernelMsgRecvReply {
+            artiq_raise!("SubkernelError", "An error occurred during subkernel operation")
-            status: SubkernelStatus::CommLost,
+        }
-            ..
+        Message::SubkernelError(SubkernelStatus::Exception(raw_exception)) => eh_artiq::raise_raw(&raw_exception),
-        } => artiq_raise!("SubkernelError", "Lost communication with satellite"),
-        Message::SubkernelMsgRecvReply {
-            status: SubkernelStatus::OtherError,
-            ..
-        } => artiq_raise!("SubkernelError", "An error occurred during subkernel operation"),
         _ => panic!("expected SubkernelMsgRecvReply after SubkernelMsgRecvRequest"),
     }
     // RpcRecvRequest should be called after this to receive message data

src/runtime/src/comms.rs

@@ -422,33 +422,23 @@ async fn handle_run_kernel(
             #[cfg(has_drtio)]
             kernel::Message::SubkernelAwaitFinishRequest { id, timeout } => {
                 let res = subkernel::await_finish(aux_mutex, routing_table, timer, id, timeout).await;
-                let status = match res {
+                let response = match res {
-                    Ok(ref res) => {
+                    Ok(res) => {
                         if res.status == subkernel::FinishStatus::CommLost {
-                            kernel::SubkernelStatus::CommLost
+                            kernel::Message::SubkernelError(kernel::SubkernelStatus::CommLost)
-                        } else if let Some(exception) = &res.exception {
+                        } else if let Some(exception) = res.exception {
-                            error!("Exception in subkernel");
+                            kernel::Message::SubkernelError(kernel::SubkernelStatus::Exception(exception))
-                            match stream {
-                                None => (),
-                                Some(stream) => {
-                                    write_chunk(stream, exception).await?;
-                                }
-                            }
-                            // will not be called after exception is served
-                            kernel::SubkernelStatus::OtherError
                         } else {
-                            kernel::SubkernelStatus::NoError
+                            kernel::Message::SubkernelAwaitFinishReply
                         }
                     }
-                    Err(SubkernelError::Timeout) => kernel::SubkernelStatus::Timeout,
+                    Err(SubkernelError::Timeout) => kernel::Message::SubkernelError(kernel::SubkernelStatus::Timeout),
-                    Err(SubkernelError::IncorrectState) => kernel::SubkernelStatus::IncorrectState,
+                    Err(SubkernelError::IncorrectState) => {
-                    Err(_) => kernel::SubkernelStatus::OtherError,
+                        kernel::Message::SubkernelError(kernel::SubkernelStatus::IncorrectState)
+                    }
+                    Err(_) => kernel::Message::SubkernelError(kernel::SubkernelStatus::OtherError),
                 };
-                control
+                control.borrow_mut().tx.async_send(response).await;
-                    .borrow_mut()
-                    .tx
-                    .async_send(kernel::Message::SubkernelAwaitFinishReply { status: status })
-                    .await;
             }
             #[cfg(has_drtio)]
             kernel::Message::SubkernelMsgSend { id, destination, data } => {
@@ -469,35 +459,23 @@ async fn handle_run_kernel(
             #[cfg(has_drtio)]
             kernel::Message::SubkernelMsgRecvRequest { id, timeout, tags } => {
                 let message_received = subkernel::message_await(id as u32, timeout, timer).await;
-                let (status, count) = match message_received {
+                let response = match message_received {
-                    Ok(ref message) => (kernel::SubkernelStatus::NoError, message.count),
+                    Ok(ref message) => kernel::Message::SubkernelMsgRecvReply { count: message.count },
-                    Err(SubkernelError::Timeout) => (kernel::SubkernelStatus::Timeout, 0),
+                    Err(SubkernelError::Timeout) => kernel::Message::SubkernelError(kernel::SubkernelStatus::Timeout),
-                    Err(SubkernelError::IncorrectState) => (kernel::SubkernelStatus::IncorrectState, 0),
+                    Err(SubkernelError::IncorrectState) => {
-                    Err(SubkernelError::CommLost) => (kernel::SubkernelStatus::CommLost, 0),
+                        kernel::Message::SubkernelError(kernel::SubkernelStatus::IncorrectState)
+                    }
+                    Err(SubkernelError::CommLost) => kernel::Message::SubkernelError(kernel::SubkernelStatus::CommLost),
                     Err(SubkernelError::SubkernelException) => {
-                        error!("Exception in subkernel");
                         // just retrieve the exception
                         let status = subkernel::await_finish(aux_mutex, routing_table, timer, id as u32, timeout)
                             .await
                             .unwrap();
-                        match stream {
+                        kernel::Message::SubkernelError(kernel::SubkernelStatus::Exception(status.exception.unwrap()))
-                            None => (),
-                            Some(stream) => {
-                                write_chunk(stream, &status.exception.unwrap()).await?;
                     }
-                        }
+                    Err(_) => kernel::Message::SubkernelError(kernel::SubkernelStatus::OtherError),
-                        (kernel::SubkernelStatus::OtherError, 0)
-                    }
-                    Err(_) => (kernel::SubkernelStatus::OtherError, 0),
                 };
-                control
+                control.borrow_mut().tx.async_send(response).await;
-                    .borrow_mut()
-                    .tx
-                    .async_send(kernel::Message::SubkernelMsgRecvReply {
-                        status: status,
-                        count: count,
-                    })
-                    .await;
                 if let Ok(message) = message_received {
                     // receive code almost identical to RPC recv, except we are not reading from a stream
                     let mut reader = Cursor::new(message.data);
@@ -529,7 +507,7 @@ async fn handle_run_kernel(
                             .async_send(kernel::Message::RpcRecvReply(Ok(0)))
                             .await;
                         i += 1;
-                        if i < count {
+                        if i < message.count {
                             current_tags = remaining_tags;
                         } else {
                             break;
@@ -718,6 +696,27 @@ async fn handle_connection(
     }
 }
 
+async fn load_and_run_idle_kernel(
+    buffer: &Vec<u8>,
+    control: &Rc<RefCell<kernel::Control>>,
+    up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
+    aux_mutex: &Rc<Mutex<bool>>,
+    routing_table: &drtio_routing::RoutingTable,
+    timer: GlobalTimer,
+) {
+    info!("Loading idle kernel");
+    let res = handle_flash_kernel(buffer, control, up_destinations, aux_mutex, routing_table, timer).await;
+    match res {
+        Err(_) => warn!("error loading idle kernel"),
+        _ => (),
+    }
+    info!("Running idle kernel");
+    let _ = handle_run_kernel(None, control, up_destinations, aux_mutex, routing_table, timer)
+        .await
+        .map_err(|_| warn!("error running idle kernel"));
+    info!("Idle kernel terminated");
+}
+
 pub fn main(timer: GlobalTimer, cfg: Config) {
     let net_addresses = net_settings::get_addresses(&cfg);
     info!("network addresses: {}", net_addresses);
@@ -771,7 +770,7 @@ pub fn main(timer: GlobalTimer, cfg: Config) {
     #[cfg(has_drtio_routing)]
     drtio_routing::interconnect_disable_all();
 
-    rtio_mgt::startup(&aux_mutex, &drtio_routing_table, &up_destinations, timer);
+    rtio_mgt::startup(&aux_mutex, &drtio_routing_table, &up_destinations, &cfg, timer);
     ksupport::setup_device_map(&cfg);
 
     analyzer::start(&aux_mutex, &drtio_routing_table, &up_destinations, timer);
@@ -808,8 +807,30 @@ pub fn main(timer: GlobalTimer, cfg: Config) {
     mgmt::start(cfg);
 
     task::spawn(async move {
-        let connection = Rc::new(Semaphore::new(1, 1));
+        let connection = Rc::new(Semaphore::new(0, 1));
         let terminate = Rc::new(Semaphore::new(0, 1));
+        {
+            let control = control.clone();
+            let idle_kernel = idle_kernel.clone();
+            let connection = connection.clone();
+            let terminate = terminate.clone();
+            let up_destinations = up_destinations.clone();
+            let aux_mutex = aux_mutex.clone();
+            let routing_table = drtio_routing_table.clone();
+            task::spawn(async move {
+                let routing_table = routing_table.borrow();
+                select_biased! {
+                    _ = (async {
+                        if let Some(buffer) = &*idle_kernel {
+                            load_and_run_idle_kernel(&buffer, &control, &up_destinations, &aux_mutex, &routing_table, timer).await;
+                        }
+                    }).fuse() => (),
+                    _ = terminate.async_wait().fuse() => ()
+                }
+                connection.signal();
+            });
+        }
+
         loop {
             let mut stream = TcpStream::accept(1381, 0x10_000, 0x10_000).await.unwrap();
 
@@ -837,22 +858,7 @@ pub fn main(timer: GlobalTimer, cfg: Config) {
                             .await
                             .map_err(|e| warn!("connection terminated: {}", e));
                         if let Some(buffer) = &*idle_kernel {
-                            info!("Loading idle kernel");
+                            load_and_run_idle_kernel(&buffer, &control, &up_destinations, &aux_mutex, &routing_table, timer).await;
-                            let res = handle_flash_kernel(&buffer, &control, &up_destinations,  &aux_mutex, &routing_table, timer)
-                                .await;
-                            match res {
-                                #[cfg(has_drtio)]
-                                Err(Error::DestinationDown) => {
-                                    let mut countdown = timer.countdown();
-                                    delay(&mut countdown, Milliseconds(500)).await;
-                                }
-                                Err(_) => warn!("error loading idle kernel"),
-                                _ => (),
-                            }
-                            info!("Running idle kernel");
-                            let _ = handle_run_kernel(None, &control, &up_destinations, &aux_mutex, &routing_table, timer)
-                                .await.map_err(|_| warn!("error running idle kernel"));
-                            info!("Idle kernel terminated");
                         }
                     }).fuse() => (),
                     _ = terminate.async_wait().fuse() => ()

src/runtime/src/rtio_mgt.rs

@@ -3,7 +3,9 @@ use core::cell::RefCell;
 
 use libboard_artiq::{drtio_routing, drtio_routing::RoutingTable, pl::csr};
 use libboard_zynq::timer::GlobalTimer;
+use libconfig::Config;
 use libcortex_a9::mutex::Mutex;
+use log::{info, warn};
 
 #[cfg(has_drtio)]
 pub mod drtio {
@@ -127,6 +129,8 @@ pub mod drtio {
             | Packet::SubkernelLoadRunReply { destination, .. }
             | Packet::SubkernelMessage { destination, .. }
             | Packet::SubkernelMessageAck { destination, .. }
+            | Packet::SubkernelException { destination, .. }
+            | Packet::SubkernelExceptionRequest { destination, .. }
             | Packet::DmaPlaybackStatus { destination, .. }
             | Packet::SubkernelFinished { destination, .. } => {
                 if destination == 0 {
@@ -181,10 +185,7 @@ pub mod drtio {
 
     async fn drain_buffer(linkno: u8, draining_time: Milliseconds, timer: GlobalTimer) {
         let max_time = timer.get_time() + draining_time;
-        loop {
+        while timer.get_time() < max_time {
-            if timer.get_time() > max_time {
-                return;
-            }
             let _ = drtioaux_async::recv(linkno).await;
         }
     }
@@ -833,13 +834,19 @@ pub mod drtio {
                 linkno,
                 routing_table,
                 &Packet::SubkernelExceptionRequest {
+                    source: 0,
                     destination: destination,
                 },
                 timer,
             )
             .await?;
             match reply {
-                Packet::SubkernelException { last, length, data } => {
+                Packet::SubkernelException {
+                    destination: 0,
+                    last,
+                    length,
+                    data,
+                } => {
                     remote_data.extend(&data[0..length as usize]);
                     if last {
                         return Ok(remote_data);
@@ -898,12 +905,36 @@ pub mod drtio {
     pub fn reset(_aux_mutex: Rc<Mutex<bool>>, _routing_table: &RoutingTable, mut _timer: GlobalTimer) {}
 }
 
+fn toggle_sed_spread(val: u8) {
+    unsafe {
+        csr::rtio_core::sed_spread_enable_write(val);
+    }
+}
+
+fn setup_sed_spread(cfg: &Config) {
+    if let Ok(spread_enable) = cfg.read_str("sed_spread_enable") {
+        match spread_enable.as_ref() {
+            "1" => toggle_sed_spread(1),
+            "0" => toggle_sed_spread(0),
+            _ => {
+                warn!("sed_spread_enable value not supported (only 1, 0 allowed), disabling by default");
+                toggle_sed_spread(0)
+            }
+        };
+    } else {
+        info!("SED spreading disabled by default");
+        toggle_sed_spread(0);
+    }
+}
+
 pub fn startup(
     aux_mutex: &Rc<Mutex<bool>>,
     routing_table: &Rc<RefCell<RoutingTable>>,
     up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
+    cfg: &Config,
     timer: GlobalTimer,
 ) {
+    setup_sed_spread(cfg);
     drtio::startup(aux_mutex, routing_table, up_destinations, timer);
     unsafe {
         csr::rtio_core::reset_phy_write(1);

src/satman/src/main.rs

@@ -12,6 +12,7 @@ extern crate io;
 extern crate ksupport;
 extern crate libboard_artiq;
 extern crate libboard_zynq;
+extern crate libconfig;
 extern crate libcortex_a9;
 extern crate libregister;
 extern crate libsupport_zynq;
@@ -38,6 +39,7 @@ use libboard_artiq::{drtio_routing, drtioaux,
 #[cfg(feature = "target_kasli_soc")]
 use libboard_zynq::error_led::ErrorLED;
 use libboard_zynq::{i2c::I2c, print, println, time::Milliseconds, timer::GlobalTimer};
+use libconfig::Config;
 use libcortex_a9::{l2c::enable_l2_cache, regs::MPIDR};
 use libregister::RegisterR;
 use libsupport_zynq::{exception_vectors, ram};
@@ -81,15 +83,37 @@ fn drtiosat_tsc_loaded() -> bool {
     }
 }
 
+fn toggle_sed_spread(val: u8) {
+    unsafe {
+        csr::drtiosat::sed_spread_enable_write(val);
+    }
+}
+
 #[cfg(has_drtio_routing)]
 macro_rules! forward {
-    ($routing_table:expr, $destination:expr, $rank:expr, $repeaters:expr, $packet:expr, $timer:expr) => {{
+    (
+        $router:expr,
+        $routing_table:expr,
+        $destination:expr,
+        $rank:expr,
+        $self_destination:expr,
+        $repeaters:expr,
+        $packet:expr,
+        $timer:expr
+    ) => {{
         let hop = $routing_table.0[$destination as usize][$rank as usize];
         if hop != 0 {
             let repno = (hop - 1) as usize;
             if repno < $repeaters.len() {
                 if $packet.expects_response() {
-                    return $repeaters[repno].aux_forward($packet, $timer);
+                    return $repeaters[repno].aux_forward(
+                        $packet,
+                        $router,
+                        $routing_table,
+                        $rank,
+                        $self_destination,
+                        $timer,
+                    );
                 } else {
                     return $repeaters[repno].aux_send($packet);
                 }
@@ -102,7 +126,16 @@ macro_rules! forward {
 
 #[cfg(not(has_drtio_routing))]
 macro_rules! forward {
-    ($routing_table:expr, $destination:expr, $rank:expr, $repeaters:expr, $packet:expr, $timer:expr) => {};
+    (
+        $router:expr,
+        $routing_table:expr,
+        $destination:expr,
+        $rank:expr,
+        $self_destination:expr,
+        $repeaters:expr,
+        $packet:expr,
+        $timer:expr
+    ) => {};
 }
 
 fn process_aux_packet(
@@ -183,6 +216,10 @@ fn process_aux_packet(
                             &drtioaux::Packet::DestinationStatusRequest {
                                 destination: destination,
                             },
+                            router,
+                            _routing_table,
+                            *rank,
+                            *self_destination,
                             timer,
                         ) {
                             Ok(()) => (),
@@ -244,7 +281,16 @@ fn process_aux_packet(
             channel,
             probe,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let value;
             #[cfg(has_rtio_moninj)]
             unsafe {
@@ -266,7 +312,16 @@ fn process_aux_packet(
             overrd,
             value,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             #[cfg(has_rtio_moninj)]
             unsafe {
                 csr::rtio_moninj::inj_chan_sel_write(channel as _);
@@ -280,7 +335,16 @@ fn process_aux_packet(
             channel,
             overrd,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let value;
             #[cfg(has_rtio_moninj)]
             unsafe {
@@ -299,7 +363,16 @@ fn process_aux_packet(
             destination: _destination,
             busno: _busno,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let succeeded = i2c.start().is_ok();
             drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded })
         }
@@ -307,7 +380,16 @@ fn process_aux_packet(
             destination: _destination,
             busno: _busno,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let succeeded = i2c.restart().is_ok();
             drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded })
         }
@@ -315,7 +397,16 @@ fn process_aux_packet(
             destination: _destination,
             busno: _busno,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let succeeded = i2c.stop().is_ok();
             drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded })
         }
@@ -324,7 +415,16 @@ fn process_aux_packet(
             busno: _busno,
             data,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             match i2c.write(data) {
                 Ok(ack) => drtioaux::send(
                     0,
@@ -347,7 +447,16 @@ fn process_aux_packet(
             busno: _busno,
             ack,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             match i2c.read(ack) {
                 Ok(data) => drtioaux::send(
                     0,
@@ -371,7 +480,16 @@ fn process_aux_packet(
             address,
             mask,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let ch = match mask {
                 //decode from mainline, PCA9548-centric API
                 0x00 => None,
@@ -397,7 +515,16 @@ fn process_aux_packet(
             div: _div,
             cs: _cs,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             // todo: reimplement when/if SPI is available
             //let succeeded = spi::set_config(busno, flags, length, div, cs).is_ok();
             drtioaux::send(0, &drtioaux::Packet::SpiBasicReply { succeeded: false })
@@ -407,7 +534,16 @@ fn process_aux_packet(
             busno: _busno,
             data: _data,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             // todo: reimplement when/if SPI is available
             //let succeeded = spi::write(busno, data).is_ok();
             drtioaux::send(0, &drtioaux::Packet::SpiBasicReply { succeeded: false })
@@ -416,7 +552,16 @@ fn process_aux_packet(
             destination: _destination,
             busno: _busno,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             // todo: reimplement when/if SPI is available
             // match spi::read(busno) {
             //     Ok(data) => drtioaux::send(0,
@@ -436,7 +581,16 @@ fn process_aux_packet(
         drtioaux::Packet::AnalyzerHeaderRequest {
             destination: _destination,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let header = analyzer.get_header();
             drtioaux::send(
                 0,
@@ -450,7 +604,16 @@ fn process_aux_packet(
         drtioaux::Packet::AnalyzerDataRequest {
             destination: _destination,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let mut data_slice: [u8; SAT_PAYLOAD_MAX_SIZE] = [0; SAT_PAYLOAD_MAX_SIZE];
             let meta = analyzer.get_data(&mut data_slice);
             drtioaux::send(
@@ -471,7 +634,16 @@ fn process_aux_packet(
             length,
             trace,
         } => {
-            forward!(_routing_table, destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             *self_destination = destination;
             let succeeded = dma_manager.add(source, id, status, &trace, length as usize).is_ok();
             router.send(
@@ -492,7 +664,16 @@ fn process_aux_packet(
             id,
             succeeded,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             dma_manager.ack_upload(
                 kernel_manager,
                 source,
@@ -510,7 +691,16 @@ fn process_aux_packet(
             destination: _destination,
             id,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let succeeded = dma_manager.erase(source, id).is_ok();
             router.send(
                 drtioaux::Packet::DmaRemoveTraceReply {
@@ -526,7 +716,16 @@ fn process_aux_packet(
             destination: _destination,
             succeeded: _,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             Ok(())
         }
         drtioaux::Packet::DmaPlaybackRequest {
@@ -535,7 +734,16 @@ fn process_aux_packet(
             id,
             timestamp,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let succeeded = if !kernel_manager.running() {
                 dma_manager.playback(source, id, timestamp).is_ok()
             } else {
@@ -555,7 +763,16 @@ fn process_aux_packet(
             destination: _destination,
             succeeded,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             if !succeeded {
                 kernel_manager.ddma_nack();
             }
@@ -569,7 +786,16 @@ fn process_aux_packet(
             channel,
             timestamp,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             dma_manager.remote_finished(kernel_manager, id, error, channel, timestamp);
             Ok(())
         }
@@ -581,7 +807,16 @@ fn process_aux_packet(
             length,
             data,
         } => {
-            forward!(_routing_table, destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             *self_destination = destination;
             let succeeded = kernel_manager.add(id, status, &data, length as usize).is_ok();
             drtioaux::send(0, &drtioaux::Packet::SubkernelAddDataReply { succeeded: succeeded })
@@ -592,7 +827,16 @@ fn process_aux_packet(
             id,
             run,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             let mut succeeded = kernel_manager.load(id).is_ok();
             // allow preloading a kernel with delayed run
             if run {
@@ -617,7 +861,16 @@ fn process_aux_packet(
             destination: _destination,
             succeeded,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             // received if local subkernel started another, remote subkernel
             kernel_manager.subkernel_load_run_reply(succeeded);
             Ok(())
@@ -628,25 +881,73 @@ fn process_aux_packet(
             with_exception,
             exception_src,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             kernel_manager.remote_subkernel_finished(id, with_exception, exception_src);
             Ok(())
         }
         drtioaux::Packet::SubkernelExceptionRequest {
+            source,
             destination: _destination,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
-            let mut data_slice: [u8; SAT_PAYLOAD_MAX_SIZE] = [0; SAT_PAYLOAD_MAX_SIZE];
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
+            let mut data_slice: [u8; MASTER_PAYLOAD_MAX_SIZE] = [0; MASTER_PAYLOAD_MAX_SIZE];
             let meta = kernel_manager.exception_get_slice(&mut data_slice);
-            drtioaux::send(
+            router.send(
-                0,
+                drtioaux::Packet::SubkernelException {
-                &drtioaux::Packet::SubkernelException {
+                    destination: source,
                     last: meta.status.is_last(),
                     length: meta.len,
                     data: data_slice,
                 },
+                _routing_table,
+                *rank,
+                *self_destination,
             )
         }
+        drtioaux::Packet::SubkernelException {
+            destination: _destination,
+            last,
+            length,
+            data,
+        } => {
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
+            kernel_manager.received_exception(
+                &data[..length as usize],
+                last,
+                router,
+                _routing_table,
+                *rank,
+                *self_destination,
+            );
+            Ok(())
+        }
         drtioaux::Packet::SubkernelMessage {
             source,
             destination: _destination,
@@ -655,7 +956,16 @@ fn process_aux_packet(
             length,
             data,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             kernel_manager.message_handle_incoming(status, id, length as usize, &data);
             router.send(
                 drtioaux::Packet::SubkernelMessageAck { destination: source },
@@ -667,7 +977,16 @@ fn process_aux_packet(
         drtioaux::Packet::SubkernelMessageAck {
             destination: _destination,
         } => {
-            forward!(_routing_table, _destination, *rank, _repeaters, &packet, timer);
+            forward!(
+                router,
+                _routing_table,
+                _destination,
+                *rank,
+                *self_destination,
+                _repeaters,
+                &packet,
+                timer
+            );
             if kernel_manager.message_ack_slice() {
                 let mut data_slice: [u8; MASTER_PAYLOAD_MAX_SIZE] = [0; MASTER_PAYLOAD_MAX_SIZE];
                 if let Some(meta) = kernel_manager.message_get_slice(&mut data_slice) {
@@ -920,6 +1239,28 @@ pub extern "C" fn main_core0() -> i32 {
     #[cfg(has_si549)]
     si549::helper_setup(&mut timer, &SI549_SETTINGS).expect("cannot initialize helper Si549");
 
+    let cfg = match Config::new() {
+        Ok(cfg) => cfg,
+        Err(err) => {
+            warn!("config initialization failed: {}", err);
+            Config::new_dummy()
+        }
+    };
+
+    if let Ok(spread_enable) = cfg.read_str("sed_spread_enable") {
+        match spread_enable.as_ref() {
+            "1" => toggle_sed_spread(1),
+            "0" => toggle_sed_spread(0),
+            _ => {
+                warn!("sed_spread_enable value not supported (only 1, 0 allowed), disabling by default");
+                toggle_sed_spread(0)
+            }
+        };
+    } else {
+        info!("SED spreading disabled by default");
+        toggle_sed_spread(0);
+    }
+
     #[cfg(has_drtio_routing)]
     let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
     #[cfg(not(has_drtio_routing))]

src/satman/src/repeater.rs

@@ -87,6 +87,10 @@ impl Repeater {
                 if rep_link_rx_up(self.repno) {
                     if let Ok(Some(drtioaux::Packet::EchoReply)) = drtioaux::recv(self.auxno) {
                         info!("[REP#{}] remote replied after {} packets", self.repno, ping_count);
+                        let max_time = timer.get_time() + Milliseconds(200);
+                        while timer.get_time() < max_time {
+                            let _ = drtioaux::recv(self.auxno);
+                        }
                         self.state = RepeaterState::Up;
                         if let Err(e) = self.sync_tsc(timer) {
                             error!("[REP#{}] failed to sync TSC ({:?})", self.repno, e);
@@ -204,10 +208,37 @@ impl Repeater {
         }
     }
 
-    pub fn aux_forward(&self, request: &drtioaux::Packet, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+    pub fn aux_forward(
+        &self,
+        request: &drtioaux::Packet,
+        router: &mut Router,
+        routing_table: &drtio_routing::RoutingTable,
+        rank: u8,
+        self_destination: u8,
+        timer: &mut GlobalTimer,
+    ) -> Result<(), drtioaux::Error> {
         self.aux_send(request)?;
+        loop {
             let reply = self.recv_aux_timeout(200, timer)?;
+            match reply {
+                // async/locally requested packets to be consumed or routed
+                // these may come while a packet would be forwarded
+                drtioaux::Packet::DmaPlaybackStatus { .. }
+                | drtioaux::Packet::SubkernelFinished { .. }
+                | drtioaux::Packet::SubkernelMessage { .. }
+                | drtioaux::Packet::SubkernelMessageAck { .. }
+                | drtioaux::Packet::SubkernelLoadRunReply { .. }
+                | drtioaux::Packet::SubkernelException { .. }
+                | drtioaux::Packet::DmaAddTraceReply { .. }
+                | drtioaux::Packet::DmaPlaybackReply { .. } => {
+                    router.route(reply, routing_table, rank, self_destination);
+                }
+                _ => {
                     drtioaux::send(0, &reply).unwrap();
+                    break;
+                }
+            }
+        }
         Ok(())
     }
 

src/satman/src/routing.rs

@@ -4,7 +4,7 @@ use core::cmp::min;
 #[cfg(has_drtio_routing)]
 use libboard_artiq::pl::csr;
 use libboard_artiq::{drtio_routing, drtioaux,
-                     drtioaux_proto::{PayloadStatus, MASTER_PAYLOAD_MAX_SIZE, SAT_PAYLOAD_MAX_SIZE}};
+                     drtioaux_proto::{PayloadStatus, MASTER_PAYLOAD_MAX_SIZE}};
 
 pub struct SliceMeta {
     pub destination: u8,
@@ -57,7 +57,6 @@ impl Sliceable {
         self.data.extend(data);
     }
 
-    get_slice_fn!(get_slice_sat, SAT_PAYLOAD_MAX_SIZE);
     get_slice_fn!(get_slice_master, MASTER_PAYLOAD_MAX_SIZE);
 }
 

src/satman/src/subkernel.rs

@@ -11,7 +11,7 @@ use io::{Cursor, ProtoWrite};
 use ksupport::{eh_artiq, kernel, rpc};
 use libboard_artiq::{drtio_routing::RoutingTable,
                      drtioaux,
-                     drtioaux_proto::{PayloadStatus, MASTER_PAYLOAD_MAX_SIZE, SAT_PAYLOAD_MAX_SIZE},
+                     drtioaux_proto::{PayloadStatus, MASTER_PAYLOAD_MAX_SIZE},
                      pl::csr};
 use libboard_zynq::{time::Milliseconds, timer::GlobalTimer};
 use libcortex_a9::sync_channel::Receiver;
@@ -47,6 +47,9 @@ enum KernelState {
     DmaAwait {
         max_time: Milliseconds,
     },
+    SubkernelRetrievingException {
+        destination: u8,
+    },
 }
 
 #[derive(Debug)]
@@ -123,10 +126,11 @@ struct MessageManager {
 struct Session {
     id: u32,
     kernel_state: KernelState,
-    last_exception: Option<Sliceable>,
+    last_exception: Option<Sliceable>,   // exceptions raised locally
+    external_exception: Option<Vec<u8>>, // exceptions from sub-subkernels
     messages: MessageManager,
     source: u8, // which destination requested running the kernel
-    subkernels_finished: Vec<u32>,
+    subkernels_finished: Vec<(u32, Option<u8>)>,
 }
 
 impl Session {
@@ -135,6 +139,7 @@ impl Session {
             id: id,
             kernel_state: KernelState::Absent,
             last_exception: None,
+            external_exception: None,
             messages: MessageManager::new(),
             source: 0,
             subkernels_finished: Vec::new(),
@@ -410,9 +415,9 @@ impl<'a> Manager<'_> {
         }
     }
 
-    pub fn exception_get_slice(&mut self, data_slice: &mut [u8; SAT_PAYLOAD_MAX_SIZE]) -> SliceMeta {
+    pub fn exception_get_slice(&mut self, data_slice: &mut [u8; MASTER_PAYLOAD_MAX_SIZE]) -> SliceMeta {
         match self.session.last_exception.as_mut() {
-            Some(exception) => exception.get_slice_sat(data_slice),
+            Some(exception) => exception.get_slice_master(data_slice),
             None => SliceMeta {
                 destination: 0,
                 len: 0,
@@ -540,7 +545,7 @@ impl<'a> Manager<'_> {
             return;
         }
 
-        match self.process_external_messages(timer) {
+        match self.process_external_messages(router, routing_table, rank, destination, timer) {
             Ok(()) => (),
             Err(Error::AwaitingMessage) => return, // kernel still waiting, do not process kernel messages
             Err(Error::KernelException(exception)) => {
@@ -596,6 +601,41 @@ impl<'a> Manager<'_> {
         }
     }
 
+    fn check_finished_kernels(
+        &mut self,
+        id: u32,
+        router: &mut Router,
+        routing_table: &RoutingTable,
+        rank: u8,
+        self_destination: u8,
+    ) {
+        for (i, (status, exception_source)) in self.session.subkernels_finished.iter().enumerate() {
+            if *status == id {
+                if exception_source.is_none() {
+                    self.control.tx.send(kernel::Message::SubkernelAwaitFinishReply);
+                    self.session.kernel_state = KernelState::Running;
+                    self.session.subkernels_finished.swap_remove(i);
+                } else {
+                    let destination = exception_source.unwrap();
+                    self.session.external_exception = Some(Vec::new());
+                    self.session.kernel_state = KernelState::SubkernelRetrievingException {
+                        destination: destination,
+                    };
+                    router.route(
+                        drtioaux::Packet::SubkernelExceptionRequest {
+                            source: self_destination,
+                            destination: destination,
+                        },
+                        &routing_table,
+                        rank,
+                        self_destination,
+                    );
+                }
+                break;
+            }
+        }
+    }
+
     pub fn subkernel_load_run_reply(&mut self, succeeded: bool) {
         if self.session.kernel_state == KernelState::SubkernelAwaitLoad {
             self.control
@@ -608,16 +648,46 @@ impl<'a> Manager<'_> {
     }
 
     pub fn remote_subkernel_finished(&mut self, id: u32, with_exception: bool, exception_source: u8) {
-        if with_exception {
+        let exception_src = if with_exception { Some(exception_source) } else { None };
-            self.kernel_stop();
+        self.session.subkernels_finished.push((id, exception_src));
-            self.last_finished = Some(SubkernelFinished {
+    }
-                source: self.session.source,
+
-                id: self.session.id,
+    pub fn received_exception(
-                with_exception: true,
+        &mut self,
-                exception_source: exception_source,
+        exception_data: &[u8],
-            })
+        last: bool,
+        router: &mut Router,
+        routing_table: &RoutingTable,
+        rank: u8,
+        self_destination: u8,
+    ) {
+        if let KernelState::SubkernelRetrievingException { destination } = self.session.kernel_state {
+            self.session
+                .external_exception
+                .as_mut()
+                .unwrap()
+                .extend_from_slice(exception_data);
+            if last {
+                self.control
+                    .tx
+                    .send(kernel::Message::SubkernelError(kernel::SubkernelStatus::Exception(
+                        self.session.external_exception.take().unwrap(),
+                    )));
+                self.session.kernel_state = KernelState::Running;
             } else {
-            self.session.subkernels_finished.push(id);
+                /* fetch another slice */
+                router.route(
+                    drtioaux::Packet::SubkernelExceptionRequest {
+                        source: self_destination,
+                        destination: destination,
+                    },
+                    routing_table,
+                    rank,
+                    self_destination,
+                );
+            }
+        } else {
+            warn!("Received unsolicited exception data");
         }
     }
 
@@ -780,28 +850,35 @@ impl<'a> Manager<'_> {
         Ok(false)
     }
 
-    fn process_external_messages(&mut self, timer: &GlobalTimer) -> Result<(), Error> {
+    fn process_external_messages(
+        &mut self,
+        router: &mut Router,
+        routing_table: &RoutingTable,
+        rank: u8,
+        self_destination: u8,
+        timer: &GlobalTimer,
+    ) -> Result<(), Error> {
         match &self.session.kernel_state {
             KernelState::MsgAwait { max_time, id, tags } => {
                 if let Some(max_time) = *max_time {
                     if timer.get_time() > max_time {
-                        self.control.tx.send(kernel::Message::SubkernelMsgRecvReply {
+                        self.control
-                            status: kernel::SubkernelStatus::Timeout,
+                            .tx
-                            count: 0,
+                            .send(kernel::Message::SubkernelError(kernel::SubkernelStatus::Timeout));
-                        });
                         self.session.kernel_state = KernelState::Running;
                         return Ok(());
                     }
                 }
                 if let Some(message) = self.session.messages.get_incoming(*id) {
-                    self.control.tx.send(kernel::Message::SubkernelMsgRecvReply {
+                    self.control
-                        status: kernel::SubkernelStatus::NoError,
+                        .tx
-                        count: message.count,
+                        .send(kernel::Message::SubkernelMsgRecvReply { count: message.count });
-                    });
                     let tags = tags.clone();
                     self.session.kernel_state = KernelState::Running;
                     self.pass_message_to_kernel(&message, tags, timer)
                 } else {
+                    let id = *id;
+                    self.check_finished_kernels(id, router, routing_table, rank, self_destination);
                     Err(Error::AwaitingMessage)
                 }
             }
@@ -817,27 +894,18 @@ impl<'a> Manager<'_> {
             KernelState::SubkernelAwaitFinish { max_time, id } => {
                 if let Some(max_time) = *max_time {
                     if timer.get_time() > max_time {
-                        self.control.tx.send(kernel::Message::SubkernelAwaitFinishReply {
+                        self.control
-                            status: kernel::SubkernelStatus::Timeout,
+                            .tx
-                        });
+                            .send(kernel::Message::SubkernelError(kernel::SubkernelStatus::Timeout));
                         self.session.kernel_state = KernelState::Running;
                         return Ok(());
                     }
                 }
-                let mut i = 0;
+                let id = *id;
-                for status in &self.session.subkernels_finished {
+                self.check_finished_kernels(id, router, routing_table, rank, self_destination);
-                    if *status == *id {
-                        self.control.tx.send(kernel::Message::SubkernelAwaitFinishReply {
-                            status: kernel::SubkernelStatus::NoError,
-                        });
-                        self.session.kernel_state = KernelState::Running;
-                        self.session.subkernels_finished.swap_remove(i);
-                        break;
-                    }
-                    i += 1;
-                }
                 Ok(())
             }
+            KernelState::SubkernelRetrievingException { .. } => Err(Error::AwaitingMessage),
             KernelState::DmaAwait { max_time } | KernelState::DmaPendingAwait { max_time, .. } => {
                 if timer.get_time() > *max_time {
                     self.control.tx.send(kernel::Message::DmaAwaitRemoteReply {