added satman firmware from drtio port

src/satman/Cargo.toml

@@ -0,0 +1,28 @@
+[package]
+authors = ["M-Labs"]
+name = "satman"
+version = "0.0.0"
+build = "build.rs"
+
+[features]
+target_zc706 = ["libboard_zynq/target_zc706", "libsupport_zynq/target_zc706", "libconfig/target_zc706", "libboard_artiq/target_zc706"]
+target_kasli_soc = ["libboard_zynq/target_kasli_soc", "libsupport_zynq/target_kasli_soc", "libconfig/target_kasli_soc", "libboard_artiq/target_kasli_soc"]
+default = ["target_zc706", ]
+
+[build-dependencies]
+build_zynq = { path = "../libbuild_zynq" }
+
+[dependencies]
+log = { version = "0.4", default-features = false }
+embedded-hal = "0.2"
+
+libboard_zynq = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git", features = ["ipv6"]}
+libsupport_zynq = { default-features = false, features = ["alloc_core"], git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" }
+libcortex_a9 = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" }
+libasync = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" }
+libregister = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" }
+libconfig = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git", features = ["ipv6"] }
+
+libboard_artiq = { path = "../libboard_artiq" }
+unwind = { path = "../libunwind" }
+libc = { path = "../libc" }

src/satman/build.rs

@@ -0,0 +1,21 @@
+use std::env;
+use std::fs::File;
+use std::io::Write;
+use std::path::PathBuf;
+
+extern crate build_zynq;
+
+fn main() {
+    // Put the linker script somewhere the linker can find it
+    let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap());
+    File::create(out.join("link.x"))
+        .unwrap()
+        .write_all(include_bytes!("link.x"))
+        .unwrap();
+    println!("cargo:rustc-link-search={}", out.display());
+
+    // Only re-run the build script when link.x is changed,
+    // instead of when any part of the source code changes.
+    println!("cargo:rerun-if-changed=link.x");
+    build_zynq::cfg();
+}

src/satman/link.x

@@ -0,0 +1,86 @@
+ENTRY(Reset);
+
+MEMORY
+{
+    SDRAM : ORIGIN = 0x00100000, LENGTH = 0x1FF00000
+}
+
+SECTIONS
+{
+    __text_start = .;
+    .text :
+    {
+        KEEP(*(.text.exceptions));
+        *(.text.boot);
+        *(.text .text.*);
+    } > SDRAM
+    __text_end = .;
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > SDRAM
+    __exidx_end = .;
+
+    .ARM.extab :
+    {
+        * (.ARM.extab*)
+    } > SDRAM
+ 
+    .rodata : ALIGN(4)
+    {
+        *(.rodata .rodata.*);
+    } > SDRAM
+ 
+    .data : ALIGN(4)
+    {
+        *(.data .data.*);
+    } > SDRAM
+ 
+    .bss (NOLOAD) : ALIGN(4)
+    {
+        __bss_start = .;
+        *(.bss .bss.*);
+        . = ALIGN(4);
+        __bss_end = .;
+    } > SDRAM
+
+    .heap (NOLOAD) : ALIGN(8)
+    {
+        __heap0_start = .;
+        . += 0x8000000;
+        __heap0_end = .;
+        __heap1_start = .;
+        . += 0x8000000;
+        __heap1_end = .;
+    } > SDRAM
+
+    .stack1 (NOLOAD) : ALIGN(8)
+    {
+        __stack1_end = .;
+        . += 0x1000000;
+        __stack1_start = .;
+    } > SDRAM
+
+    .stack0 (NOLOAD) : ALIGN(8)
+    {
+        __stack0_end = .;
+        . += 0x20000;
+        __stack0_start = .;
+    } > SDRAM
+
+    .irq_stack1 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack1_end = .;
+        . += 0x100;
+        __irq_stack1_start = .;
+    } > SDRAM
+
+    .irq_stack0 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack0_end = .;
+        . += 0x100;
+        __irq_stack0_start = .;
+    } > SDRAM
+}

src/satman/satman.ld

@@ -0,0 +1,55 @@
+INCLUDE generated/output_format.ld
+INCLUDE generated/regions.ld
+ENTRY(_reset_handler)
+
+SECTIONS
+{
+    .vectors :
+    {
+        *(.vectors)
+    } > main_ram
+
+    .text :
+    {
+        *(.text .text.*)
+    } > main_ram
+
+    /* https://sourceware.org/bugzilla/show_bug.cgi?id=20475 */
+    .got :
+    {
+        PROVIDE(_GLOBAL_OFFSET_TABLE_ = .);
+        *(.got)
+    } > main_ram
+
+    .got.plt :
+    {
+        *(.got.plt)
+    } > main_ram
+
+    .rodata :
+    {
+        _frodata = .;
+        *(.rodata .rodata.*)
+        _erodata = .;
+    } > main_ram
+
+    .data :
+    {
+        *(.data .data.*)
+    } > main_ram
+
+    .bss ALIGN(4) :
+    {
+        _fbss = .;
+        *(.bss .bss.*)
+        . = ALIGN(4);
+        _ebss = .;
+    } > main_ram
+
+    .stack :
+    {
+        _estack = .;
+        . += 0x10000;
+        _fstack = . - 4;
+    } > main_ram
+}

src/satman/src/jdac_common.rs

@@ -0,0 +1,37 @@
+pub const INIT: u8              = 0x00;
+pub const PRINT_STATUS: u8      = 0x01;
+pub const PRBS: u8              = 0x02;
+pub const STPL: u8              = 0x03;
+
+pub const SYSREF_DELAY_DAC: u8  = 0x10;
+pub const SYSREF_SLIP: u8       = 0x11;
+pub const SYNC: u8              = 0x12;
+
+pub const DDMTD_SYSREF_RAW: u8  = 0x20;
+pub const DDMTD_SYSREF: u8      = 0x21;
+
+
+fn average_2phases(a: i32, b: i32, modulo: i32) -> i32 {
+    let diff = ((a - b + modulo/2 + modulo) % modulo) - modulo/2;
+    return (modulo + b + diff/2) % modulo;
+}
+
+pub fn average_phases(phases: &[i32], modulo: i32) -> i32 {
+    if phases.len() == 1 {
+        panic!("input array length must be a power of 2");
+    } else if phases.len() == 2 {
+        average_2phases(phases[0], phases[1], modulo)
+    } else {
+        let cut = phases.len()/2;
+        average_2phases(
+            average_phases(&phases[..cut], modulo),
+            average_phases(&phases[cut..], modulo),
+            modulo)
+    }
+}
+
+pub const RAW_DDMTD_N_SHIFT: i32 = 6;
+pub const RAW_DDMTD_N: i32 = 1 << RAW_DDMTD_N_SHIFT;
+pub const DDMTD_DITHER_BITS: i32 = 1;
+pub const DDMTD_N_SHIFT: i32 = RAW_DDMTD_N_SHIFT + DDMTD_DITHER_BITS;
+pub const DDMTD_N: i32 = 1 << DDMTD_N_SHIFT;

src/satman/src/main.rs

@@ -0,0 +1,634 @@
+#![no_std]
+#![no_main]
+#![feature(never_type, panic_info_message, asm, naked_functions)]
+#![feature(alloc_error_handler)]
+
+#[macro_use]
+extern crate log;
+
+extern crate embedded_hal;
+
+extern crate libboard_zynq;
+extern crate libboard_artiq;
+extern crate libsupport_zynq;
+extern crate libcortex_a9;
+extern crate libregister;
+
+extern crate unwind;
+
+extern crate alloc;
+
+use libboard_zynq::{i2c::I2c, timer::GlobalTimer, time::Milliseconds, print, println, mpcore, gic, stdio};
+use libsupport_zynq::ram;
+#[cfg(has_si5324)]
+use libboard_artiq::si5324;
+use libboard_artiq::{pl::csr, drtio_routing, drtioaux, logger, identifier_read, init_gateware};
+use libcortex_a9::{spin_lock_yield, interrupt_handler, regs::{MPIDR, SP}, notify_spin_lock, asm, l2c::enable_l2_cache};
+use libregister::{RegisterW, RegisterR};
+
+use embedded_hal::blocking::delay::DelayUs;
+use core::sync::atomic::{AtomicBool, Ordering};
+
+mod repeater;
+
+fn drtiosat_reset(reset: bool) {
+    unsafe {
+        csr::drtiosat::reset_write(if reset { 1 } else { 0 });
+    }
+}
+
+fn drtiosat_reset_phy(reset: bool) {
+    unsafe {
+        csr::drtiosat::reset_phy_write(if reset { 1 } else { 0 });
+    }
+}
+
+fn drtiosat_link_rx_up() -> bool {
+    unsafe {
+        csr::drtiosat::rx_up_read() == 1
+    }
+}
+
+fn drtiosat_tsc_loaded() -> bool {
+    unsafe {
+        let tsc_loaded = csr::drtiosat::tsc_loaded_read() == 1;
+        if tsc_loaded {
+            csr::drtiosat::tsc_loaded_write(1);
+        }
+        tsc_loaded
+    }
+}
+
+
+#[cfg(has_drtio_routing)]
+macro_rules! forward {
+    ($routing_table:expr, $destination:expr, $rank: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() {
+                return $repeaters[repno].aux_forward($packet, $timer);
+            } else {
+                return Err(drtioaux::Error::RoutingError);
+            }
+        }
+    }}
+}
+
+#[cfg(not(has_drtio_routing))]
+macro_rules! forward {
+    ($routing_table:expr, $destination:expr, $rank:expr, $repeaters:expr, $packet:expr, $timer:expr) => {}
+}
+
+fn process_aux_packet(_repeaters: &mut [repeater::Repeater],
+        _routing_table: &mut drtio_routing::RoutingTable, _rank: &mut u8,
+        packet: drtioaux::Packet, timer: &mut GlobalTimer, i2c: &mut I2c) -> Result<(), drtioaux::Error> {
+    // In the code below, *_chan_sel_write takes an u8 if there are fewer than 256 channels,
+    // and u16 otherwise; hence the `as _` conversion.
+    match packet {
+        drtioaux::Packet::EchoRequest =>
+            drtioaux::send(0, &drtioaux::Packet::EchoReply),
+        drtioaux::Packet::ResetRequest => {
+            info!("resetting RTIO");
+            drtiosat_reset(true);
+            timer.delay_us(100);
+            drtiosat_reset(false);
+            for rep in _repeaters.iter() {
+                if let Err(e) = rep.rtio_reset(timer) {
+                    error!("failed to issue RTIO reset ({:?})", e);
+                }
+            }
+            drtioaux::send(0, &drtioaux::Packet::ResetAck)
+        },
+
+        drtioaux::Packet::DestinationStatusRequest { destination: _destination } => {
+            #[cfg(has_drtio_routing)]
+            let hop = _routing_table.0[_destination as usize][*_rank as usize];
+            #[cfg(not(has_drtio_routing))]
+            let hop = 0;
+
+            if hop == 0 {
+                let errors;
+                unsafe {
+                    errors = csr::drtiosat::rtio_error_read();
+                }
+                if errors & 1 != 0 {
+                    let channel;
+                    unsafe {
+                        channel = csr::drtiosat::sequence_error_channel_read();
+                        csr::drtiosat::rtio_error_write(1);
+                    }
+                    drtioaux::send(0,
+                        &drtioaux::Packet::DestinationSequenceErrorReply { channel })?;
+                } else if errors & 2 != 0 {
+                    let channel;
+                    unsafe {
+                        channel = csr::drtiosat::collision_channel_read();
+                        csr::drtiosat::rtio_error_write(2);
+                    }
+                    drtioaux::send(0,
+                        &drtioaux::Packet::DestinationCollisionReply { channel })?;
+                } else if errors & 4 != 0 {
+                    let channel;
+                    unsafe {
+                        channel = csr::drtiosat::busy_channel_read();
+                        csr::drtiosat::rtio_error_write(4);
+                    }
+                    drtioaux::send(0,
+                        &drtioaux::Packet::DestinationBusyReply { channel })?;
+                }
+                else {
+                    drtioaux::send(0, &drtioaux::Packet::DestinationOkReply)?;
+                }
+            }
+
+            #[cfg(has_drtio_routing)]
+            {
+                if hop != 0 {
+                    let hop = hop as usize;
+                    if hop <= csr::DRTIOREP.len() {
+                        let repno = hop - 1;
+                        match _repeaters[repno].aux_forward(&drtioaux::Packet::DestinationStatusRequest {
+                            destination: _destination
+                        }, timer) {
+                            Ok(()) => (),
+                            Err(drtioaux::Error::LinkDown) => drtioaux::send(0, &drtioaux::Packet::DestinationDownReply)?,
+                            Err(e) => {
+                                drtioaux::send(0, &drtioaux::Packet::DestinationDownReply)?;
+                                error!("aux error when handling destination status request: {:?}", e);
+                            },
+                        }
+                    } else {
+                        drtioaux::send(0, &drtioaux::Packet::DestinationDownReply)?;
+                    }
+                }
+            }
+
+            Ok(())
+        }
+
+        #[cfg(has_drtio_routing)]
+        drtioaux::Packet::RoutingSetPath { destination, hops } => {
+            _routing_table.0[destination as usize] = hops;
+            for rep in _repeaters.iter() {
+                if let Err(e) = rep.set_path(destination, &hops, timer) {
+                    error!("failed to set path ({:?})", e);
+                }
+            }
+            drtioaux::send(0, &drtioaux::Packet::RoutingAck)
+        }
+        #[cfg(has_drtio_routing)]
+        drtioaux::Packet::RoutingSetRank { rank } => {
+            *_rank = rank;
+            drtio_routing::interconnect_enable_all(_routing_table, rank);
+
+            let rep_rank = rank + 1;
+            for rep in _repeaters.iter() {
+                if let Err(e) = rep.set_rank(rep_rank, timer) {
+                    error!("failed to set rank ({:?})", e);
+                }
+            }
+
+            info!("rank: {}", rank);
+            info!("routing table: {}", _routing_table);
+
+            drtioaux::send(0, &drtioaux::Packet::RoutingAck)
+        }
+
+        #[cfg(not(has_drtio_routing))]
+        drtioaux::Packet::RoutingSetPath { destination: _, hops: _ } => {
+            drtioaux::send(0, &drtioaux::Packet::RoutingAck)
+        }
+        #[cfg(not(has_drtio_routing))]
+        drtioaux::Packet::RoutingSetRank { rank: _ } => {
+            drtioaux::send(0, &drtioaux::Packet::RoutingAck)
+        }
+
+        drtioaux::Packet::MonitorRequest { destination: _destination, channel: _channel, probe: _probe } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let value;
+            #[cfg(has_rtio_moninj)]
+            unsafe {
+                csr::rtio_moninj::mon_chan_sel_write(channel as _);
+                csr::rtio_moninj::mon_probe_sel_write(probe);
+                csr::rtio_moninj::mon_value_update_write(1);
+                value = csr::rtio_moninj::mon_value_read();
+            }
+            #[cfg(not(has_rtio_moninj))]
+            {
+                value = 0;
+            }
+            let reply = drtioaux::Packet::MonitorReply { value: value as u32 };
+            drtioaux::send(0, &reply)
+        },
+        drtioaux::Packet::InjectionRequest { destination: _destination, channel: _channel, 
+                                             overrd: _overrd, value: _value } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            #[cfg(has_rtio_moninj)]
+            unsafe {
+                csr::rtio_moninj::inj_chan_sel_write(channel as _);
+                csr::rtio_moninj::inj_override_sel_write(overrd);
+                csr::rtio_moninj::inj_value_write(value);
+            }
+            Ok(())
+        },
+        drtioaux::Packet::InjectionStatusRequest { destination: _destination, 
+                                                   channel: _channel, overrd: _overrd } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let value;
+            #[cfg(has_rtio_moninj)]
+            unsafe {
+                csr::rtio_moninj::inj_chan_sel_write(channel as _);
+                csr::rtio_moninj::inj_override_sel_write(overrd);
+                value = csr::rtio_moninj::inj_value_read();
+            }
+            #[cfg(not(has_rtio_moninj))]
+            {
+                value = 0;
+            }
+            drtioaux::send(0, &drtioaux::Packet::InjectionStatusReply { value: value })
+        },
+
+        drtioaux::Packet::I2cStartRequest { destination: _destination, busno: _busno } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let succeeded = i2c.start().is_ok();
+            drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded })
+        }
+        drtioaux::Packet::I2cRestartRequest { destination: _destination, busno: _busno } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let succeeded = i2c.restart().is_ok();
+            drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded })
+        }
+        drtioaux::Packet::I2cStopRequest { destination: _destination, busno: _busno } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let succeeded = i2c.stop().is_ok();
+            drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded })
+        }
+        drtioaux::Packet::I2cWriteRequest { destination: _destination, busno: _busno, data } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            match i2c.write(data) {
+                Ok(ack) => drtioaux::send(0,
+                    &drtioaux::Packet::I2cWriteReply { succeeded: true, ack: ack }),
+                Err(_) => drtioaux::send(0,
+                    &drtioaux::Packet::I2cWriteReply { succeeded: false, ack: false })
+            }
+        }
+        drtioaux::Packet::I2cReadRequest { destination: _destination, busno: _busno, ack } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            match i2c.read(ack) {
+                Ok(data) => drtioaux::send(0,
+                    &drtioaux::Packet::I2cReadReply { succeeded: true, data: data }),
+                Err(_) => drtioaux::send(0,
+                    &drtioaux::Packet::I2cReadReply { succeeded: false, data: 0xff })
+            }
+        }
+
+        drtioaux::Packet::SpiSetConfigRequest { destination: _destination, busno: _busno, 
+                                                flags: _flags, length: _length, div: _div, cs: _cs } => {
+            forward!(_routing_table, _destination, *_rank, _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 })
+        },
+        drtioaux::Packet::SpiWriteRequest { destination: _destination, busno: _busno, data: _data } => {
+            forward!(_routing_table, _destination, *_rank, _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 })
+        }
+        drtioaux::Packet::SpiReadRequest { destination: _destination, busno: _busno } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            // todo: reimplement when/if SPI is available
+            // match spi::read(busno) {
+            //     Ok(data) => drtioaux::send(0,
+            //         &drtioaux::Packet::SpiReadReply { succeeded: true, data: data }),
+            //     Err(_) => drtioaux::send(0,
+            //         &drtioaux::Packet::SpiReadReply { succeeded: false, data: 0 })
+            // }
+            drtioaux::send(0,
+                &drtioaux::Packet::SpiReadReply { succeeded: false, data: 0 })
+        }
+
+        drtioaux::Packet::JdacBasicRequest { destination: _destination, dacno: _dacno,
+                                             reqno: _reqno, param: _param } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let (succeeded, retval) = (false, 0);
+            drtioaux::send(0,
+                &drtioaux::Packet::JdacBasicReply { succeeded: succeeded, retval: retval })
+        }
+
+        _ => {
+            warn!("received unexpected aux packet");
+            Ok(())
+        }
+    }
+}
+
+fn process_aux_packets(repeaters: &mut [repeater::Repeater],
+        routing_table: &mut drtio_routing::RoutingTable, rank: &mut u8, 
+        timer: &mut GlobalTimer, i2c: &mut I2c) {
+    let result =
+        drtioaux::recv(0).and_then(|packet| {
+            if let Some(packet) = packet {
+                process_aux_packet(repeaters, routing_table, rank, packet, timer, i2c)
+            } else {
+                Ok(())
+            }
+        });
+    match result {
+        Ok(()) => (),
+        Err(e) => warn!("aux packet error ({:?})", e)
+    }
+}
+
+fn drtiosat_process_errors() {
+    let errors;
+    unsafe {
+        errors = csr::drtiosat::protocol_error_read();
+    }
+    if errors & 1 != 0 {
+        error!("received packet of an unknown type");
+    }
+    if errors & 2 != 0 {
+        error!("received truncated packet");
+    }
+    if errors & 4 != 0 {
+        let destination;
+        unsafe {
+            destination = csr::drtiosat::buffer_space_timeout_dest_read();
+        }
+        error!("timeout attempting to get buffer space from CRI, destination=0x{:02x}", destination)
+    }
+    if errors & 8 != 0 {
+        let channel;
+        let timestamp_event;
+        let timestamp_counter;
+        unsafe {
+            channel = csr::drtiosat::underflow_channel_read();
+            timestamp_event = csr::drtiosat::underflow_timestamp_event_read() as i64;
+            timestamp_counter = csr::drtiosat::underflow_timestamp_counter_read() as i64;
+        }
+        error!("write underflow, channel={}, timestamp={}, counter={}, slack={}",
+               channel, timestamp_event, timestamp_counter, timestamp_event-timestamp_counter);
+    }
+    if errors & 16 != 0 {
+        error!("write overflow");
+    }
+    unsafe {
+        csr::drtiosat::protocol_error_write(errors);
+    }
+}
+
+
+#[cfg(has_rtio_crg)]
+fn init_rtio_crg(timer: GlobalTimer) {
+    unsafe {
+        csr::rtio_crg::pll_reset_write(0);
+    }
+    timer.delay_us(150);
+    let locked = unsafe { csr::rtio_crg::pll_locked_read() != 0 };
+    if !locked {
+        error!("RTIO clock failed");
+    }
+}
+
+#[cfg(not(has_rtio_crg))]
+fn init_rtio_crg(_timer: GlobalTimer) { }
+
+fn hardware_tick(ts: &mut u64, timer: &mut GlobalTimer) {
+    let now = timer.get_time();
+    let mut ts_ms = Milliseconds(*ts);
+    if now > ts_ms {
+        ts_ms = now + Milliseconds(200);
+        *ts = ts_ms.0;
+    }
+}
+
+#[cfg(has_si5324)]
+const SI5324_SETTINGS: si5324::FrequencySettings
+    = si5324::FrequencySettings {
+    n1_hs  : 5,
+    nc1_ls : 8,
+    n2_hs  : 7,
+    n2_ls  : 360,
+    n31    : 63,
+    n32    : 63,
+    bwsel  : 4,
+    crystal_ref: true
+};
+
+static mut LOG_BUFFER: [u8; 1<<17] = [0; 1<<17];
+
+#[no_mangle]
+pub extern fn main_core0() -> i32 {
+    enable_l2_cache(0x8);
+
+    let mut timer = GlobalTimer::start();
+
+    let buffer_logger = unsafe {
+        logger::BufferLogger::new(&mut LOG_BUFFER[..])
+    };
+    buffer_logger.set_uart_log_level(log::LevelFilter::Info);
+    buffer_logger.register();
+    log::set_max_level(log::LevelFilter::Info);
+    
+    init_gateware();
+
+    info!("ARTIQ satellite manager starting...");
+    info!("gateware ident {}", identifier_read(&mut [0; 64]));
+
+    ram::init_alloc_core0();
+
+    let mut i2c = I2c::i2c0();
+    i2c.init().expect("I2C initialization failed");
+
+    #[cfg(has_si5324)]
+    si5324::setup(&mut i2c, &SI5324_SETTINGS, si5324::Input::Ckin1, &mut timer).expect("cannot initialize Si5324");
+
+    unsafe {
+        csr::drtio_transceiver::stable_clkin_write(1);
+    }
+    timer.delay_us(1500); // wait for CPLL/QPLL lock
+
+    unsafe {
+        csr::drtio_transceiver::txenable_write(0xffffffffu32 as _);
+    }
+    init_rtio_crg(timer);
+
+    #[cfg(has_drtio_routing)]
+    let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
+    #[cfg(not(has_drtio_routing))]
+    let mut repeaters = [repeater::Repeater::default(); 0];
+    for i in 0..repeaters.len() {
+        repeaters[i] = repeater::Repeater::new(i as u8);
+    } 
+    let mut routing_table = drtio_routing::RoutingTable::default_empty();
+    let mut rank = 1;
+
+    let mut hardware_tick_ts = 0;
+
+    loop {
+        while !drtiosat_link_rx_up() {
+            drtiosat_process_errors();
+            #[allow(unused_mut)]
+            for mut rep in repeaters.iter_mut() {
+                rep.service(&routing_table, rank, &mut timer);
+            }
+            hardware_tick(&mut hardware_tick_ts, &mut timer);
+        }
+
+        info!("uplink is up, switching to recovered clock");
+        #[cfg(has_siphaser)]
+        {
+            si5324::siphaser::select_recovered_clock(&mut i2c, true, &mut timer).expect("failed to switch clocks");
+            si5324::siphaser::calibrate_skew(&mut timer).expect("failed to calibrate skew");
+        }
+
+        drtioaux::reset(0);
+        drtiosat_reset(false);
+        drtiosat_reset_phy(false);
+
+        while drtiosat_link_rx_up() {
+            drtiosat_process_errors();
+            process_aux_packets(&mut repeaters, &mut routing_table, &mut rank, &mut timer, &mut i2c);
+            #[allow(unused_mut)]
+            for mut rep in repeaters.iter_mut() {
+                rep.service(&routing_table, rank, &mut timer);
+            }
+            hardware_tick(&mut hardware_tick_ts, &mut timer);
+            if drtiosat_tsc_loaded() {
+                info!("TSC loaded from uplink");
+                for rep in repeaters.iter() {
+                    if let Err(e) = rep.sync_tsc(&mut timer) {
+                        error!("failed to sync TSC ({:?})", e);
+                    }
+                }
+                if let Err(e) = drtioaux::send(0, &drtioaux::Packet::TSCAck) {
+                    error!("aux packet error: {:?}", e);
+                }
+            }
+        }
+
+        drtiosat_reset_phy(true);
+        drtiosat_reset(true);
+        drtiosat_tsc_loaded();
+        info!("uplink is down, switching to local oscillator clock");
+        #[cfg(has_siphaser)]
+        si5324::siphaser::select_recovered_clock(&mut i2c, false, &mut timer).expect("failed to switch clocks");
+    }
+}
+
+extern "C" {
+    static mut __stack1_start: u32;
+}
+
+interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
+    if MPIDR.read().cpu_id() == 1{
+        let mpcore = mpcore::RegisterBlock::mpcore();
+        let mut gic = gic::InterruptController::gic(mpcore);
+        let id = gic.get_interrupt_id();
+        if id.0 == 0 {
+            gic.end_interrupt(id);
+            asm::exit_irq();
+            SP.write(&mut __stack1_start as *mut _ as u32);
+            asm::enable_irq();
+            CORE1_RESTART.store(false, Ordering::Relaxed);
+            notify_spin_lock();
+            main_core1();
+        }
+    stdio::drop_uart();
+    }
+    loop {}
+});
+
+static mut PANICKED: [bool; 2] = [false; 2];
+
+static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
+
+pub fn restart_core1() {
+    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
+    CORE1_RESTART.store(true, Ordering::Relaxed);
+    interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core1.into());
+    while CORE1_RESTART.load(Ordering::Relaxed) {
+        spin_lock_yield();
+    }
+}
+
+#[no_mangle]
+pub fn main_core1() {
+    let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
+    interrupt_controller.enable_interrupts();
+
+    loop {}
+}
+
+#[no_mangle]
+pub extern fn exception(_vect: u32, _regs: *const u32, pc: u32, ea: u32) {
+
+    fn hexdump(addr: u32) {
+        let addr = (addr - addr % 4) as *const u32;
+        let mut ptr  = addr;
+        println!("@ {:08p}", ptr);
+        for _ in 0..4 {
+            print!("+{:04x}: ", ptr as usize - addr as usize);
+            print!("{:08x} ",   unsafe { *ptr }); ptr = ptr.wrapping_offset(1);
+            print!("{:08x} ",   unsafe { *ptr }); ptr = ptr.wrapping_offset(1);
+            print!("{:08x} ",   unsafe { *ptr }); ptr = ptr.wrapping_offset(1);
+            print!("{:08x}\n",  unsafe { *ptr }); ptr = ptr.wrapping_offset(1);
+        }
+    }
+
+    hexdump(pc);
+    hexdump(ea);
+    panic!("exception at PC 0x{:x}, EA 0x{:x}", pc, ea)
+}
+
+#[no_mangle] // https://github.com/rust-lang/rust/issues/{38281,51647}
+#[panic_handler]
+pub fn panic_fmt(info: &core::panic::PanicInfo) -> ! {
+    let id = MPIDR.read().cpu_id() as usize;
+    print!("Core {} ", id);
+    unsafe {
+        if PANICKED[id] {
+            println!("nested panic!");
+            loop {}
+        }
+        PANICKED[id] = true;
+    }
+    print!("panic at ");
+    if let Some(location) = info.location() {
+        print!("{}:{}:{}", location.file(), location.line(), location.column());
+    } else {
+        print!("unknown location");
+    }
+    if let Some(message) = info.message() {
+        println!(": {}", message);
+    } else {
+        println!("");
+    }
+
+
+    loop {}
+}
+
+// linker symbols
+extern "C" {
+    static __text_start: u32;
+    static __text_end: u32;
+    static __exidx_start: u32;
+    static __exidx_end: u32;
+}
+
+#[no_mangle]
+extern fn dl_unwind_find_exidx(_pc: *const u32, len_ptr: *mut u32) -> *const u32 {
+    let length;
+    let start: *const u32;
+    unsafe {
+        length = (&__exidx_end as *const u32).offset_from(&__exidx_start) as u32;
+        start = &__exidx_start;
+        *len_ptr = length;
+    }
+    start
+}

src/satman/src/repeater.rs

@@ -0,0 +1,290 @@
+use libboard_artiq::{drtioaux, drtio_routing};
+use libboard_zynq::timer::GlobalTimer;
+
+#[cfg(has_drtio_routing)]
+use libboard_artiq::{pl::csr};
+#[cfg(has_drtio_routing)]
+use libboard_zynq::time::Milliseconds;
+#[cfg(has_drtio_routing)]
+use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
+
+#[cfg(has_drtio_routing)]
+fn rep_link_rx_up(repno: u8) -> bool {
+    let repno = repno as usize;
+    unsafe {
+        (csr::DRTIOREP[repno].rx_up_read)() == 1
+    }
+}
+
+#[cfg(has_drtio_routing)]
+#[derive(Clone, Copy, PartialEq)]
+enum RepeaterState {
+    Down,
+    SendPing { ping_count: u16 },
+    WaitPingReply { ping_count: u16, timeout: Milliseconds },
+    Up,
+    Failed
+}
+
+#[cfg(has_drtio_routing)]
+impl Default for RepeaterState {
+    fn default() -> RepeaterState { RepeaterState::Down }
+}
+
+#[cfg(has_drtio_routing)]
+#[derive(Clone, Copy, Default)]
+pub struct Repeater {
+    repno: u8,
+    auxno: u8,
+    state: RepeaterState
+}
+
+#[cfg(has_drtio_routing)]
+impl Repeater {
+    pub fn new(repno: u8) -> Repeater {
+        Repeater {
+            repno: repno,
+            auxno: repno + 1,
+            state: RepeaterState::Down
+        }
+    }
+
+    #[allow(dead_code)]
+    pub fn is_up(&self) -> bool {
+        self.state == RepeaterState::Up
+    }
+
+    pub fn service(&mut self, routing_table: &drtio_routing::RoutingTable, rank: u8,
+        timer: &mut GlobalTimer) {
+        self.process_local_errors();
+
+        match self.state {
+            RepeaterState::Down => {
+                if rep_link_rx_up(self.repno) {
+                    info!("[REP#{}] link RX became up, pinging", self.repno);
+                    self.state = RepeaterState::SendPing { ping_count: 0 };
+                }
+            }
+            RepeaterState::SendPing { ping_count } => {
+                if rep_link_rx_up(self.repno) {
+                    drtioaux::send(self.auxno, &drtioaux::Packet::EchoRequest).unwrap();
+                    self.state = RepeaterState::WaitPingReply {
+                        ping_count: ping_count + 1,
+                        timeout: timer.get_time() + Milliseconds(100)
+                    }
+                } else {
+                    error!("[REP#{}] link RX went down during ping", self.repno);
+                    self.state = RepeaterState::Down;
+                }
+            }
+            RepeaterState::WaitPingReply { ping_count, timeout } => {
+                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);
+                        self.state = RepeaterState::Up;
+                        if let Err(e) = self.sync_tsc(timer) {
+                            error!("[REP#{}] failed to sync TSC ({:?})", self.repno, e);
+                            self.state = RepeaterState::Failed;
+                            return;
+                        }
+                        if let Err(e) = self.load_routing_table(routing_table, timer) {
+                            error!("[REP#{}] failed to load routing table ({:?})", self.repno, e);
+                            self.state = RepeaterState::Failed;
+                            return;
+                        }
+                        if let Err(e) = self.set_rank(rank + 1, timer) {
+                            error!("[REP#{}] failed to set rank ({:?})", self.repno, e);
+                            self.state = RepeaterState::Failed;
+                            return;
+                        }
+                    } else {
+                        if timer.get_time() > timeout {
+                            if ping_count > 200 {
+                                error!("[REP#{}] ping failed", self.repno);
+                                self.state = RepeaterState::Failed;
+                            } else {
+                                self.state = RepeaterState::SendPing { ping_count: ping_count };
+                            }
+                        }
+                    }
+                } else {
+                    error!("[REP#{}] link RX went down during ping", self.repno);
+                    self.state = RepeaterState::Down;
+                }
+            }
+            RepeaterState::Up => {
+                self.process_unsolicited_aux();
+                if !rep_link_rx_up(self.repno) {
+                    info!("[REP#{}] link is down", self.repno);
+                    self.state = RepeaterState::Down;
+                }
+            }
+            RepeaterState::Failed => {
+                if !rep_link_rx_up(self.repno) {
+                    info!("[REP#{}] link is down", self.repno);
+                    self.state = RepeaterState::Down;
+                }
+            }
+        }
+    }
+
+    fn process_unsolicited_aux(&self) {
+        match drtioaux::recv(self.auxno) {
+            Ok(Some(packet)) => warn!("[REP#{}] unsolicited aux packet: {:?}", self.repno, packet),
+            Ok(None) => (),
+            Err(_) => warn!("[REP#{}] aux packet error", self.repno)
+        }
+    }
+
+    fn process_local_errors(&self) {
+        let repno = self.repno as usize;
+        let errors;
+        unsafe {
+            errors = (csr::DRTIOREP[repno].protocol_error_read)();
+        }
+        if errors & 1 != 0 {
+            error!("[REP#{}] received packet of an unknown type", repno);
+        }
+        if errors & 2 != 0 {
+            error!("[REP#{}] received truncated packet", repno);
+        }
+        if errors & 4 != 0 {
+            let cmd;
+            let chan_sel;
+            unsafe {
+                cmd = (csr::DRTIOREP[repno].command_missed_cmd_read)();
+                chan_sel = (csr::DRTIOREP[repno].command_missed_chan_sel_read)();
+            }
+            error!("[REP#{}] CRI command missed, cmd={}, chan_sel=0x{:06x}", repno, cmd, chan_sel)
+        }
+        if errors & 8 != 0 {
+            let destination;
+            unsafe {
+                destination = (csr::DRTIOREP[repno].buffer_space_timeout_dest_read)();
+            }
+            error!("[REP#{}] timeout attempting to get remote buffer space, destination=0x{:02x}", repno, destination);
+        }
+        unsafe {
+            (csr::DRTIOREP[repno].protocol_error_write)(errors);
+        }
+    }
+
+    fn recv_aux_timeout(&self, timeout: u32, timer: &mut GlobalTimer) -> Result<drtioaux::Packet, drtioaux::Error> {
+        let max_time = timer.get_time() + Milliseconds(timeout.into());
+        loop {
+            if !rep_link_rx_up(self.repno) {
+                return Err(drtioaux::Error::LinkDown);
+            }
+            if timer.get_time() > max_time {
+                return Err(drtioaux::Error::TimedOut);
+            }
+            match drtioaux::recv(self.auxno) {
+                Ok(Some(packet)) => return Ok(packet),
+                Ok(None) => (),
+                Err(e) => return Err(e)
+            }
+        }
+    }
+
+    pub fn aux_forward(&self, request: &drtioaux::Packet, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+        if self.state != RepeaterState::Up {
+            return Err(drtioaux::Error::LinkDown);
+        }
+        drtioaux::send(self.auxno, request).unwrap();
+        let reply = self.recv_aux_timeout(200, timer)?;
+        drtioaux::send(0, &reply).unwrap();
+        Ok(())
+    }
+
+    pub fn sync_tsc(&self, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+        if self.state != RepeaterState::Up {
+            return Ok(());
+        }
+
+        let repno = self.repno as usize;
+        unsafe {
+            (csr::DRTIOREP[repno].set_time_write)(1);
+            while (csr::DRTIOREP[repno].set_time_read)() == 1 {}
+        }
+
+        // TSCAck is the only aux packet that is sent spontaneously
+        // by the satellite, in response to a TSC set on the RT link.
+        let reply = self.recv_aux_timeout(10000, timer)?;
+        if reply == drtioaux::Packet::TSCAck {
+            return Ok(());
+        } else {
+            return Err(drtioaux::Error::UnexpectedReply);
+        }
+    }
+
+    pub fn set_path(&self, destination: u8, hops: &[u8; drtio_routing::MAX_HOPS], timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+        if self.state != RepeaterState::Up {
+            return Ok(());
+        }
+
+        drtioaux::send(self.auxno, &drtioaux::Packet::RoutingSetPath {
+            destination: destination,
+            hops: *hops
+        }).unwrap();
+        let reply = self.recv_aux_timeout(200, timer)?;
+        if reply != drtioaux::Packet::RoutingAck {
+            return Err(drtioaux::Error::UnexpectedReply);
+        }
+        Ok(())
+    }
+
+    pub fn load_routing_table(&self, routing_table: &drtio_routing::RoutingTable, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+        for i in 0..drtio_routing::DEST_COUNT {
+            self.set_path(i as u8, &routing_table.0[i], timer)?;
+        }
+        Ok(())
+    }
+
+    pub fn set_rank(&self, rank: u8, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+        if self.state != RepeaterState::Up {
+            return Ok(());
+        }
+        drtioaux::send(self.auxno, &drtioaux::Packet::RoutingSetRank {
+            rank: rank
+        }).unwrap();
+        let reply = self.recv_aux_timeout(200, timer)?;
+        if reply != drtioaux::Packet::RoutingAck {
+            return Err(drtioaux::Error::UnexpectedReply);
+        }
+        Ok(())
+    }
+
+    pub fn rtio_reset(&self, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> {
+        let repno = self.repno as usize;
+        unsafe { (csr::DRTIOREP[repno].reset_write)(1); }
+        timer.delay_us(100);
+        unsafe { (csr::DRTIOREP[repno].reset_write)(0); }
+
+        if self.state != RepeaterState::Up {
+            return Ok(());
+        }
+
+        drtioaux::send(self.auxno, &drtioaux::Packet::ResetRequest).unwrap();
+        let reply = self.recv_aux_timeout(200, timer)?;
+        if reply != drtioaux::Packet::ResetAck {
+            return Err(drtioaux::Error::UnexpectedReply);
+        }
+        Ok(())
+    }
+}
+
+#[cfg(not(has_drtio_routing))]
+#[derive(Clone, Copy, Default)]
+pub struct Repeater {
+}
+
+#[cfg(not(has_drtio_routing))]
+impl Repeater {
+    pub fn new(_repno: u8) -> Repeater { Repeater::default() }
+
+    pub fn service(&self, _routing_table: &drtio_routing::RoutingTable, _rank: u8, _timer: &mut GlobalTimer) { }
+
+    pub fn sync_tsc(&self, _timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { Ok(()) }
+
+    pub fn rtio_reset(&self, _timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { Ok(()) }
+}