runtime: switch to libio/libboard_artiq, add DRTIO mastering support

Reviewed-on: M-Labs/artiq-zynq#137
Co-authored-by: mwojcik <mw@m-labs.hk>
Co-committed-by: mwojcik <mw@m-labs.hk>

src/runtime/Cargo.toml

@@ -6,10 +6,13 @@ authors = ["M-Labs"]
 edition = "2018"
 
 [features]
-target_zc706 = ["libboard_zynq/target_zc706", "libsupport_zynq/target_zc706", "libconfig/target_zc706"]
+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"]
+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]
 num-traits = { version = "0.2", default-features = false }
 num-derive = "0.3"
@@ -37,3 +40,5 @@ dyld = { path = "../libdyld" }
 dwarf = { path = "../libdwarf" }
 unwind = { path = "../libunwind" }
 libc = { path = "../libc" }
+io = { path = "../libio" }
+libboard_artiq = { path = "../libboard_artiq" }

src/runtime/build.rs

@@ -1,9 +1,10 @@
 use std::env;
 use std::fs::File;
 use std::io::Write;
-use std::io::{BufRead, BufReader};
 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());
@@ -16,13 +17,5 @@ fn main() {
     // 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();
-    // Handle rustc-cfg file
-    let cfg_path = "../../build/rustc-cfg";
-    println!("cargo:rerun-if-changed={}", cfg_path);
-
-    let f = BufReader::new(File::open(cfg_path).unwrap());
-    for line in f.lines() {
-        println!("cargo:rustc-cfg={}", line.unwrap());
-    }
 }

src/runtime/src/comms.rs

@@ -21,6 +21,7 @@ use libcortex_a9::{semaphore::Semaphore, mutex::Mutex, sync_channel::{Sender, Re
 use futures::{select_biased, future::FutureExt};
 use libasync::{smoltcp::{Sockets, TcpStream}, task};
 use libconfig::{Config, net_settings};
+use libboard_artiq::drtio_routing;
 
 use crate::proto_async::*;
 use crate::kernel;
@@ -28,7 +29,8 @@ use crate::rpc;
 use crate::moninj;
 use crate::mgmt;
 use crate::analyzer;
-
+use crate::rtio_mgt;
+use crate::pl;
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
 pub enum Error {
@@ -387,8 +389,21 @@ pub fn main(timer: GlobalTimer, cfg: Config) {
 
     Sockets::init(32);
 
+    // before, mutex was on io, but now that io isn't used...?
+    let aux_mutex: Rc<Mutex<bool>> = Rc::new(Mutex::new(false));
+    #[cfg(has_drtio)]
+    let drtio_routing_table = Rc::new(RefCell::new(
+        drtio_routing::config_routing_table(pl::csr::DRTIO.len(), &cfg)));
+    #[cfg(not(has_drtio))]
+    let drtio_routing_table = Rc::new(RefCell::new(drtio_routing::RoutingTable::default_empty()));
+    let up_destinations = Rc::new(RefCell::new([false; drtio_routing::DEST_COUNT]));
+    #[cfg(has_drtio_routing)]
+    drtio_routing::interconnect_disable_all();
+
+    rtio_mgt::startup(&aux_mutex, &drtio_routing_table, &up_destinations, timer);
+
     analyzer::start();
-    moninj::start(timer);
+    moninj::start(timer, aux_mutex, drtio_routing_table);
 
     let control: Rc<RefCell<kernel::Control>> = Rc::new(RefCell::new(kernel::Control::start()));
     let idle_kernel = Rc::new(cfg.read("idle").ok());

src/runtime/src/logger.rs

@@ -1,123 +0,0 @@
-use core::cell::Cell;
-use core::fmt::Write;
-use log::{Log, LevelFilter};
-use log_buffer::LogBuffer;
-use libcortex_a9::mutex::{Mutex, MutexGuard};
-use libboard_zynq::{println, timer::GlobalTimer};
-
-pub struct LogBufferRef<'a> {
-    buffer:        MutexGuard<'a, LogBuffer<&'static mut [u8]>>,
-    old_log_level: LevelFilter
-}
-
-impl<'a> LogBufferRef<'a> {
-    fn new(buffer: MutexGuard<'a, LogBuffer<&'static mut [u8]>>) -> LogBufferRef<'a> {
-        let old_log_level = log::max_level();
-        log::set_max_level(LevelFilter::Off);
-        LogBufferRef { buffer, old_log_level }
-    }
-
-    pub fn is_empty(&self) -> bool {
-        self.buffer.is_empty()
-    }
-
-    pub fn clear(&mut self) {
-        self.buffer.clear()
-    }
-
-    pub fn extract(&mut self) -> &str {
-        self.buffer.extract()
-    }
-}
-
-impl<'a> Drop for LogBufferRef<'a> {
-    fn drop(&mut self) {
-        log::set_max_level(self.old_log_level)
-    }
-}
-
-pub struct BufferLogger {
-    buffer:      Mutex<LogBuffer<&'static mut [u8]>>,
-    uart_filter: Cell<LevelFilter>,
-    buffer_filter: Cell<LevelFilter>,
-}
-
-static mut LOGGER: Option<BufferLogger> = None;
-
-impl BufferLogger {
-    pub fn new(buffer: &'static mut [u8]) -> BufferLogger {
-        BufferLogger {
-            buffer: Mutex::new(LogBuffer::new(buffer)),
-            uart_filter: Cell::new(LevelFilter::Info),
-            buffer_filter: Cell::new(LevelFilter::Trace),
-        }
-    }
-
-    pub fn register(self) {
-        unsafe {
-            LOGGER = Some(self);
-            log::set_logger(LOGGER.as_ref().unwrap())
-                .expect("global logger can only be initialized once");
-        }
-    }
-
-    pub unsafe fn get_logger() -> &'static mut Option<BufferLogger> {
-        &mut LOGGER
-    }
-
-    pub fn buffer<'a>(&'a self) -> Option<LogBufferRef<'a>> {
-        self.buffer
-            .try_lock()
-            .map(LogBufferRef::new)
-    }
-
-    pub fn uart_log_level(&self) -> LevelFilter {
-        self.uart_filter.get()
-    }
-
-    pub fn set_uart_log_level(&self, max_level: LevelFilter) {
-        self.uart_filter.set(max_level)
-    }
-
-    pub fn buffer_log_level(&self) -> LevelFilter {
-        self.buffer_filter.get()
-    }
-
-    /// this should be reserved for mgmt module
-    pub fn set_buffer_log_level(&self, max_level: LevelFilter) {
-        self.buffer_filter.set(max_level)
-    }
-}
-
-// required for impl Log
-unsafe impl Sync for BufferLogger {}
-
-impl Log for BufferLogger {
-    fn enabled(&self, _metadata: &log::Metadata) -> bool {
-        true
-    }
-
-    fn log(&self, record: &log::Record) {
-        if self.enabled(record.metadata()) {
-            let timestamp = unsafe {
-                GlobalTimer::get()
-            }.get_us().0;
-            let seconds   = timestamp / 1_000_000;
-            let micros    = timestamp % 1_000_000;
-
-            if record.level() <= self.buffer_log_level() {
-                let mut buffer = self.buffer.lock();
-                writeln!(buffer, "[{:6}.{:06}s] {:>5}({}): {}", seconds, micros,
-                         record.level(), record.target(), record.args()).unwrap();
-            }
-
-            if record.level() <= self.uart_log_level() {
-                println!("[{:6}.{:06}s] {:>5}({}): {}", seconds, micros,
-                         record.level(), record.target(), record.args());
-            }
-        }
-    }
-
-    fn flush(&self) {
-    }
-}

src/runtime/src/main.rs

@@ -11,82 +11,43 @@
 
 extern crate alloc;
 
-use core::{cmp, str};
 use log::{info, warn, error};
 
-use libboard_zynq::{timer::GlobalTimer, mpcore, gic, slcr};
+use libboard_zynq::{timer::GlobalTimer, mpcore, gic};
 use libasync::{task, block_async};
 use libsupport_zynq::ram;
 use nb;
 use void::Void;
 use embedded_hal::blocking::delay::DelayMs;
 use libconfig::Config;
-use libregister::RegisterW;
 use libcortex_a9::l2c::enable_l2_cache;
+use libboard_artiq::{logger, identifier_read, init_gateware, pl};
+#[cfg(has_si5324)]
+use libboard_artiq::si5324;
 
-mod proto_core_io;
 mod proto_async;
 mod comms;
 mod rpc;
-#[path = "../../../build/pl.rs"]
-mod pl;
 #[cfg(ki_impl = "csr")]
 #[path = "rtio_csr.rs"]
 mod rtio;
 #[cfg(ki_impl = "acp")]
 #[path = "rtio_acp.rs"]
 mod rtio;
+mod rtio_mgt;
 mod kernel;
 mod moninj;
 mod eh_artiq;
 mod panic;
-mod logger;
 mod mgmt;
 mod analyzer;
 mod irq;
 mod i2c;
-#[cfg(has_si5324)]
-mod si5324;
 
-fn init_gateware() {
+fn init_rtio(timer: &mut GlobalTimer, _cfg: &Config) {
-    // Set up PS->PL clocks
+    #[cfg(has_rtio_crg_clock_sel)]
-    slcr::RegisterBlock::unlocked(|slcr| {
-        // As we are touching the mux, the clock may glitch, so reset the PL.
-        slcr.fpga_rst_ctrl.write(
-            slcr::FpgaRstCtrl::zeroed()
-                .fpga0_out_rst(true)
-                .fpga1_out_rst(true)
-                .fpga2_out_rst(true)
-                .fpga3_out_rst(true)
-        );
-        slcr.fpga0_clk_ctrl.write(
-            slcr::Fpga0ClkCtrl::zeroed()
-                .src_sel(slcr::PllSource::IoPll)
-                .divisor0(8)
-                .divisor1(1)
-        );
-        slcr.fpga_rst_ctrl.write(
-            slcr::FpgaRstCtrl::zeroed()
-        );
-    });
-}
-
-fn identifier_read(buf: &mut [u8]) -> &str {
-    unsafe {
-        pl::csr::identifier::address_write(0);
-        let len = pl::csr::identifier::data_read();
-        let len = cmp::min(len, buf.len() as u8);
-        for i in 0..len {
-            pl::csr::identifier::address_write(1 + i);
-            buf[i as usize] = pl::csr::identifier::data_read();
-        }
-        str::from_utf8_unchecked(&buf[..len as usize])
-    }
-}
-
-fn init_rtio(timer: &mut GlobalTimer, cfg: &Config) {
     let clock_sel =
-        if let Ok(rtioclk) = cfg.read_str("rtioclk") {
+        if let Ok(rtioclk) = _cfg.read_str("rtioclk") {
             match rtioclk.as_ref() {
                 "internal" => {
                     info!("using internal RTIO clock");
@@ -130,6 +91,19 @@ fn init_rtio(timer: &mut GlobalTimer, cfg: &Config) {
     }
 }
 
+#[cfg(has_drtio)]
+fn init_drtio(timer: &mut GlobalTimer)
+{
+    unsafe {
+        pl::csr::drtio_transceiver::stable_clkin_write(1);
+    }
+    timer.delay_ms(2); // wait for CPLL/QPLL lock
+    unsafe {
+        pl::csr::drtio_transceiver::txenable_write(0xffffffffu32 as _);
+    }
+}
+
+
 fn wait_for_async_rtio_error() -> nb::Result<(), Void> {
     unsafe {
         if pl::csr::rtio_core::async_error_read() != 0 {
@@ -201,7 +175,7 @@ pub fn main_core0() {
     i2c::init();
     #[cfg(has_si5324)]
     si5324::setup(unsafe { (&mut i2c::I2C_BUS).as_mut().unwrap() },
-        &SI5324_SETTINGS, si5324::Input::Ckin2, timer).expect("cannot initialize Si5324");
+        &SI5324_SETTINGS, si5324::Input::Ckin2, &mut timer).expect("cannot initialize Si5324");
 
     let cfg = match Config::new() {
         Ok(cfg) => cfg,
@@ -211,6 +185,9 @@ pub fn main_core0() {
         }
     };
 
+    #[cfg(has_drtio)]
+    init_drtio(&mut timer);
+
     init_rtio(&mut timer, &cfg);
     task::spawn(report_async_rtio_errors());
 

src/runtime/src/mgmt.rs

@@ -6,7 +6,7 @@ use core::cell::RefCell;
 use alloc::{rc::Rc, vec::Vec, string::String};
 use log::{self, info, debug, warn, error, LevelFilter};
 
-use crate::logger::{BufferLogger, LogBufferRef};
+use libboard_artiq::logger::{BufferLogger, LogBufferRef};
 use crate::proto_async::*;
 use num_derive::FromPrimitive;
 use num_traits::FromPrimitive;

src/runtime/src/moninj.rs

@@ -1,17 +1,19 @@
-use core::fmt;
+use core::{fmt, cell::RefCell};
-use alloc::collections::BTreeMap;
+use alloc::{collections::BTreeMap, rc::Rc};
-use log::{debug, info, warn};
+use log::{debug, info, warn, error};
 use void::Void;
 
+use libboard_artiq::drtio_routing;
+
 use libboard_zynq::{smoltcp, timer::GlobalTimer, time::Milliseconds};
 use libasync::{task, smoltcp::TcpStream, block_async, nb};
+use libcortex_a9::mutex::Mutex;
 
 use num_derive::{FromPrimitive, ToPrimitive};
 use num_traits::{FromPrimitive, ToPrimitive};
 use futures::{pin_mut, select_biased, FutureExt};
 
 use crate::proto_async::*;
-use crate::pl::csr;
 
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
@@ -19,7 +21,6 @@ pub enum Error {
     NetworkError(smoltcp::Error),
     UnexpectedPattern,
     UnrecognizedPacket,
-
 }
 
 pub type Result<T> = core::result::Result<T, Error>;
@@ -54,7 +55,57 @@ enum DeviceMessage {
     InjectionStatus = 1
 }
 
-fn read_probe(channel: i32, probe: i8) -> i32 {
+#[cfg(has_drtio)]
+mod remote_moninj {
+    use super::*;
+    use libboard_artiq::drtioaux;
+    use crate::rtio_mgt::drtio;
+
+    pub fn read_probe(aux_mutex: &Rc<Mutex<bool>>, timer: GlobalTimer, linkno: u8, destination: u8, channel: i32, probe: i8) -> i32 {
+        let reply = task::block_on(drtio::aux_transact(aux_mutex, linkno, &drtioaux::Packet::MonitorRequest { 
+            destination: destination,
+            channel: channel as _,
+            probe: probe as _},
+            timer));
+        match reply {
+            Ok(drtioaux::Packet::MonitorReply { value }) => return value as i32,
+            Ok(packet) => error!("received unexpected aux packet: {:?}", packet),
+            Err(e) => error!("aux packet error ({})", e)
+        }
+        0
+    }
+
+    pub fn inject(aux_mutex: &Rc<Mutex<bool>>, _timer: GlobalTimer, linkno: u8, destination: u8, channel: i32, overrd: i8, value: i8) {
+        let _lock = aux_mutex.lock();
+        drtioaux::send(linkno, &drtioaux::Packet::InjectionRequest {
+            destination: destination,
+            channel: channel as _,
+            overrd: overrd as _,
+            value: value as _
+        }).unwrap();
+    }
+
+    pub fn read_injection_status(aux_mutex: &Rc<Mutex<bool>>, timer: GlobalTimer, linkno: u8, destination: u8, channel: i32, overrd: i8) -> i8 {
+        let reply = task::block_on(drtio::aux_transact(aux_mutex, 
+            linkno, 
+            &drtioaux::Packet::InjectionStatusRequest {
+                destination: destination,
+                channel: channel as _,
+                overrd: overrd as _},
+            timer));
+        match reply {
+            Ok(drtioaux::Packet::InjectionStatusReply { value }) => return value as i8,
+            Ok(packet) => error!("received unexpected aux packet: {:?}", packet),
+            Err(e) => error!("aux packet error ({})", e)
+        }
+        0
+    }
+}
+
+mod local_moninj {
+    use libboard_artiq::pl::csr;
+
+    pub fn read_probe(channel: i32, probe: i8) -> i32 {
         unsafe {
             csr::rtio_moninj::mon_chan_sel_write(channel as _);
             csr::rtio_moninj::mon_probe_sel_write(probe as _);
@@ -63,7 +114,7 @@ fn read_probe(channel: i32, probe: i8) -> i32 {
         }
     }
 
-fn inject(channel: i32, overrd: i8, value: i8) {
+    pub fn inject(channel: i32, overrd: i8, value: i8) {
         unsafe {
             csr::rtio_moninj::inj_chan_sel_write(channel as _);
             csr::rtio_moninj::inj_override_sel_write(overrd as _);
@@ -71,15 +122,41 @@ fn inject(channel: i32, overrd: i8, value: i8) {
         }
     }
 
-fn read_injection_status(channel: i32, overrd: i8) -> i8 {
+    pub fn read_injection_status(channel: i32, overrd: i8) -> i8 {
         unsafe {
             csr::rtio_moninj::inj_chan_sel_write(channel as _);
             csr::rtio_moninj::inj_override_sel_write(overrd as _);
             csr::rtio_moninj::inj_value_read() as i8
         }
     }
+}
 
-async fn handle_connection(stream: &TcpStream, timer: GlobalTimer) -> Result<()> {
+#[cfg(has_drtio)]
+macro_rules! dispatch {
+    ($timer:ident, $aux_mutex:ident, $routing_table:ident, $channel:expr, $func:ident $(, $param:expr)*) => {{
+        let destination = ($channel >> 16) as u8;
+        let channel = $channel;
+        let routing_table = $routing_table.borrow_mut();
+        let hop = routing_table.0[destination as usize][0];
+        if hop == 0 {
+            local_moninj::$func(channel.into(), $($param, )*)
+        } else {
+            let linkno = hop - 1 as u8;
+            remote_moninj::$func($aux_mutex, $timer, linkno, destination, channel, $($param, )*)
+        }
+    }}
+}
+
+#[cfg(not(has_drtio))]
+macro_rules! dispatch {
+    ($timer:ident, $aux_mutex:ident, $routing_table:ident, $channel:expr, $func:ident $(, $param:expr)*) => {{
+        let channel = $channel as u16;
+        local_moninj::$func(channel, $($param, )*)
+    }}
+}
+
+async fn handle_connection(stream: &TcpStream, timer: GlobalTimer, 
+        _aux_mutex: &Rc<Mutex<bool>>, _routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>) -> Result<()> {
     if !expect(&stream, b"ARTIQ moninj\n").await? {
         return Err(Error::UnexpectedPattern);
     }
@@ -135,13 +212,13 @@ async fn handle_connection(stream: &TcpStream, timer: GlobalTimer) -> Result<()>
                         let channel = read_i32(&stream).await?;
                         let overrd = read_i8(&stream).await?;
                         let value = read_i8(&stream).await?;
-                        inject(channel, overrd, value);
+                        dispatch!(timer, _aux_mutex, _routing_table, channel, inject, overrd, value);
                         debug!("INJECT channel {}, overrd {}, value {}", channel, overrd, value);
                     },
                     HostMessage::GetInjectionStatus => {
                         let channel = read_i32(&stream).await?;
                         let overrd = read_i8(&stream).await?;
-                        let value = read_injection_status(channel, overrd);
+                        let value = dispatch!(timer, _aux_mutex, _routing_table, channel, read_injection_status, overrd);
                         write_i8(&stream, DeviceMessage::InjectionStatus.to_i8().unwrap()).await?;
                         write_i32(&stream, channel).await?;
                         write_i8(&stream, overrd).await?;
@@ -151,7 +228,7 @@ async fn handle_connection(stream: &TcpStream, timer: GlobalTimer) -> Result<()>
             },
             _ = timeout_f => {
                 for (&(channel, probe), previous) in probe_watch_list.iter_mut() {
-                    let current = read_probe(channel, probe);
+                    let current = dispatch!(timer, _aux_mutex, _routing_table, channel, read_probe, probe);
                     if previous.is_none() || previous.unwrap() != current {
                         write_i8(&stream, DeviceMessage::MonitorStatus.to_i8().unwrap()).await?;
                         write_i32(&stream, channel).await?;
@@ -161,7 +238,7 @@ async fn handle_connection(stream: &TcpStream, timer: GlobalTimer) -> Result<()>
                     }
                 }
                 for (&(channel, overrd), previous) in inject_watch_list.iter_mut() {
-                    let current = read_injection_status(channel, overrd);
+                    let current = dispatch!(timer, _aux_mutex, _routing_table, channel, read_injection_status, overrd);
                     if previous.is_none() || previous.unwrap() != current {
                         write_i8(&stream, DeviceMessage::InjectionStatus.to_i8().unwrap()).await?;
                         write_i32(&stream, channel).await?;
@@ -176,13 +253,15 @@ async fn handle_connection(stream: &TcpStream, timer: GlobalTimer) -> Result<()>
     }
 }
 
-pub fn start(timer: GlobalTimer) {
+pub fn start(timer: GlobalTimer, aux_mutex: Rc<Mutex<bool>>, routing_table: Rc<RefCell<drtio_routing::RoutingTable>>) {
     task::spawn(async move {
         loop {
+            let aux_mutex = aux_mutex.clone();
+            let routing_table = routing_table.clone();
             let stream = TcpStream::accept(1383, 2048, 2048).await.unwrap();
             task::spawn(async move {
                 info!("received connection");
-                let result = handle_connection(&stream, timer).await;
+                let result = handle_connection(&stream, timer, &aux_mutex, &routing_table).await;
                 match result {
                     Err(Error::NetworkError(smoltcp::Error::Finished)) => info!("peer closed connection"),
                     Err(error) => warn!("connection terminated: {}", error),

src/runtime/src/proto_core_io.rs

@@ -1,158 +0,0 @@
-use core::str::Utf8Error;
-use byteorder::{ByteOrder, NativeEndian};
-use alloc::vec;
-use alloc::string::String;
-
-use core_io::{Read, Write, Error as IoError};
-
-#[allow(dead_code)]
-#[derive(Debug, Clone, PartialEq)]
-pub enum ReadStringError<T> {
-    Utf8(Utf8Error),
-    Other(T)
-}
-
-pub trait ProtoRead {
-    type ReadError;
-
-    fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), Self::ReadError>;
-
-    #[inline]
-    fn read_u8(&mut self) -> Result<u8, Self::ReadError> {
-        let mut bytes = [0; 1];
-        self.read_exact(&mut bytes)?;
-        Ok(bytes[0])
-    }
-
-    #[inline]
-    fn read_u16(&mut self) -> Result<u16, Self::ReadError> {
-        let mut bytes = [0; 2];
-        self.read_exact(&mut bytes)?;
-        Ok(NativeEndian::read_u16(&bytes))
-    }
-
-    #[inline]
-    fn read_u32(&mut self) -> Result<u32, Self::ReadError> {
-        let mut bytes = [0; 4];
-        self.read_exact(&mut bytes)?;
-        Ok(NativeEndian::read_u32(&bytes))
-    }
-
-    #[inline]
-    fn read_u64(&mut self) -> Result<u64, Self::ReadError> {
-        let mut bytes = [0; 8];
-        self.read_exact(&mut bytes)?;
-        Ok(NativeEndian::read_u64(&bytes))
-    }
-
-    #[inline]
-    fn read_bool(&mut self) -> Result<bool, Self::ReadError> {
-        Ok(self.read_u8()? != 0)
-    }
-
-    #[inline]
-    fn read_bytes(&mut self) -> Result<::alloc::vec::Vec<u8>, Self::ReadError> {
-        let length = self.read_u32()?;
-        let mut value = vec![0; length as usize];
-        self.read_exact(&mut value)?;
-        Ok(value)
-    }
-
-    #[inline]
-    fn read_string(&mut self) -> Result<::alloc::string::String, ReadStringError<Self::ReadError>> {
-        let bytes = self.read_bytes().map_err(ReadStringError::Other)?;
-        String::from_utf8(bytes).map_err(|err| ReadStringError::Utf8(err.utf8_error()))
-    }
-}
-
-pub trait ProtoWrite {
-    type WriteError;
-
-    fn write_all(&mut self, buf: &[u8]) -> Result<(), Self::WriteError>;
-
-    #[inline]
-    fn write_u8(&mut self, value: u8) -> Result<(), Self::WriteError> {
-        let bytes = [value; 1];
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_i8(&mut self, value: i8) -> Result<(), Self::WriteError> {
-        let bytes = [value as u8; 1];
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_u16(&mut self, value: u16) -> Result<(), Self::WriteError> {
-        let mut bytes = [0; 2];
-        NativeEndian::write_u16(&mut bytes, value);
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_i16(&mut self, value: i16) -> Result<(), Self::WriteError> {
-        let mut bytes = [0; 2];
-        NativeEndian::write_i16(&mut bytes, value);
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_u32(&mut self, value: u32) -> Result<(), Self::WriteError> {
-        let mut bytes = [0; 4];
-        NativeEndian::write_u32(&mut bytes, value);
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_i32(&mut self, value: i32) -> Result<(), Self::WriteError> {
-        let mut bytes = [0; 4];
-        NativeEndian::write_i32(&mut bytes, value);
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_u64(&mut self, value: u64) -> Result<(), Self::WriteError> {
-        let mut bytes = [0; 8];
-        NativeEndian::write_u64(&mut bytes, value);
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_i64(&mut self, value: i64) -> Result<(), Self::WriteError> {
-        let mut bytes = [0; 8];
-        NativeEndian::write_i64(&mut bytes, value);
-        self.write_all(&bytes)
-    }
-
-    #[inline]
-    fn write_bool(&mut self, value: bool) -> Result<(), Self::WriteError> {
-        self.write_u8(value as u8)
-    }
-
-    #[inline]
-    fn write_bytes(&mut self, value: &[u8]) -> Result<(), Self::WriteError> {
-        self.write_u32(value.len() as u32)?;
-        self.write_all(value)
-    }
-
-    #[inline]
-    fn write_string(&mut self, value: &str) -> Result<(), Self::WriteError> {
-        self.write_bytes(value.as_bytes())
-    }
-}
-
-impl<T> ProtoRead for T where T: Read + ?Sized {
-    type ReadError = IoError;
-
-    fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), Self::ReadError> {
-        T::read_exact(self, buf)
-    }
-}
-
-impl<T> ProtoWrite for T where T: Write + ?Sized {
-    type WriteError = IoError;
-
-    fn write_all(&mut self, buf: &[u8]) -> Result<(), Self::WriteError> {
-        T::write_all(self, buf)
-    }
-}

src/runtime/src/rpc.rs

@@ -10,7 +10,7 @@ use libasync::smoltcp::TcpStream;
 use alloc::boxed::Box;
 use async_recursion::async_recursion;
 
-use crate::proto_core_io::ProtoWrite;
+use io::proto::ProtoWrite;
 use crate::proto_async;
 use self::tag::{Tag, TagIterator, split_tag};
 

src/runtime/src/rtio_mgt.rs

@@ -0,0 +1,351 @@
+use core::cell::RefCell;
+use alloc::rc::Rc;
+use libboard_zynq::{timer::GlobalTimer, time::Milliseconds};
+use libboard_artiq::{pl::csr, drtio_routing};
+use libcortex_a9::mutex::Mutex;
+
+
+#[cfg(has_drtio)]
+pub mod drtio {
+    use super::*;
+    use libboard_artiq::drtioaux_async;
+    use libboard_artiq::drtioaux_async::Packet;
+    use libboard_artiq::drtioaux::Error;
+    use log::{warn, error, info};
+    use embedded_hal::blocking::delay::DelayMs;
+    use libasync::{task, delay};
+
+    pub fn startup(aux_mutex: &Rc<Mutex<bool>>,
+            routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>,
+            up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
+            timer: GlobalTimer) {
+        let aux_mutex = aux_mutex.clone();
+        let routing_table = routing_table.clone();
+        let up_destinations = up_destinations.clone();
+        task::spawn(async move {
+            let routing_table = routing_table.borrow();
+            link_thread(&aux_mutex, &routing_table, &up_destinations, timer).await;
+        });
+    }
+
+    async fn link_rx_up(linkno: u8) -> bool {
+        let linkno = linkno as usize;
+        unsafe {
+            (csr::DRTIO[linkno].rx_up_read)() == 1
+        }
+    }
+
+    async fn recv_aux_timeout(linkno: u8, timeout: u64, timer: GlobalTimer) -> Result<Packet, &'static str> {
+        if !link_rx_up(linkno).await {
+            return Err("link went down");
+        }
+        match drtioaux_async::recv_timeout(linkno, Some(timeout), timer).await {
+            Ok(packet) => return Ok(packet),
+            Err(Error::TimedOut) => return Err("timed out"),
+            Err(_) => return Err("aux packet error"),
+        }
+    }
+
+    pub async fn aux_transact(aux_mutex: &Mutex<bool>, linkno: u8, request: &Packet,
+            timer: GlobalTimer) -> Result<Packet, &'static str> {
+        let _lock = aux_mutex.lock();
+        drtioaux_async::send(linkno, request).await.unwrap();
+        recv_aux_timeout(linkno, 200, timer).await
+    }
+
+    async fn drain_buffer(linkno: u8, draining_time: Milliseconds, timer: GlobalTimer) {
+        let max_time = timer.get_time() + draining_time;
+        loop {
+            if timer.get_time() > max_time {
+                return;
+            } //could this be cut short?
+            let _ = drtioaux_async::recv(linkno).await;
+        }
+    }
+
+    async fn ping_remote(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, timer: GlobalTimer) -> u32 {
+        let mut count = 0;
+        loop {
+            if !link_rx_up(linkno).await {
+                return 0
+            }
+            count += 1;
+            if count > 100 {
+                return 0;
+            }
+            let reply = aux_transact(aux_mutex, linkno, &Packet::EchoRequest, timer).await;
+            match reply {
+                Ok(Packet::EchoReply) => {
+                    // make sure receive buffer is drained
+                    let draining_time = Milliseconds(200);
+                    drain_buffer(linkno, draining_time, timer).await;
+                    return count;
+                }
+                _ => {}
+            }
+        }
+    }
+
+    async fn sync_tsc(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, timer: GlobalTimer) -> Result<(), &'static str> {
+        let _lock = aux_mutex.lock();
+
+        unsafe {
+            (csr::DRTIO[linkno as usize].set_time_write)(1);
+            while (csr::DRTIO[linkno as usize].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 = recv_aux_timeout(linkno, 10000, timer).await?;
+        if reply == Packet::TSCAck {
+            return Ok(());
+        } else {
+            return Err("unexpected reply");
+        }
+    }
+
+    async fn load_routing_table(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, routing_table: &drtio_routing::RoutingTable,
+        timer: GlobalTimer) -> Result<(), &'static str> {
+        for i in 0..drtio_routing::DEST_COUNT {
+            let reply = aux_transact(aux_mutex, linkno, &Packet::RoutingSetPath {
+                destination: i as u8,
+                hops: routing_table.0[i]
+            }, timer).await?;
+            if reply != Packet::RoutingAck {
+                return Err("unexpected reply");
+            }
+        }
+        Ok(())
+    }
+
+    async fn set_rank(aux_mutex: &Rc<Mutex<bool>>, linkno: u8, rank: u8, timer: GlobalTimer) -> Result<(), &'static str> {
+        let reply = aux_transact(aux_mutex, linkno, &Packet::RoutingSetRank {
+            rank: rank
+        }, timer).await?;
+        if reply != Packet::RoutingAck {
+            return Err("unexpected reply");
+        }
+        Ok(())
+    }
+
+    async fn init_buffer_space(destination: u8, linkno: u8) {
+        let linkno = linkno as usize;
+        unsafe {
+            (csr::DRTIO[linkno].destination_write)(destination);
+            (csr::DRTIO[linkno].force_destination_write)(1);
+            (csr::DRTIO[linkno].o_get_buffer_space_write)(1);
+            while (csr::DRTIO[linkno].o_wait_read)() == 1 {}
+            info!("[DEST#{}] buffer space is {}",
+                destination, (csr::DRTIO[linkno].o_dbg_buffer_space_read)());
+            (csr::DRTIO[linkno].force_destination_write)(0);
+        }
+    }
+
+    async fn process_unsolicited_aux(aux_mutex: &Rc<Mutex<bool>>, linkno: u8) {
+        let _lock = aux_mutex.lock();
+        match drtioaux_async::recv(linkno).await {
+            Ok(Some(packet)) => warn!("[LINK#{}] unsolicited aux packet: {:?}", linkno, packet),
+            Ok(None) => (),
+            Err(_) => warn!("[LINK#{}] aux packet error", linkno)
+        }
+    }
+
+    async fn process_local_errors(linkno: u8) {
+        let errors;
+        let linkidx = linkno as usize;
+        unsafe {
+            errors = (csr::DRTIO[linkidx].protocol_error_read)();
+            (csr::DRTIO[linkidx].protocol_error_write)(errors);
+        }
+        if errors != 0 {
+            error!("[LINK#{}] error(s) found (0x{:02x}):", linkno, errors);
+            if errors & 1 != 0 {
+                error!("[LINK#{}] received packet of an unknown type", linkno);
+            }
+            if errors & 2 != 0 {
+                error!("[LINK#{}] received truncated packet", linkno);
+            }
+            if errors & 4 != 0 {
+                error!("[LINK#{}] timeout attempting to get remote buffer space", linkno);
+            }
+        }
+    }
+
+    async fn destination_set_up(routing_table: &drtio_routing::RoutingTable,
+            up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
+            destination: u8, up: bool) {
+        let mut up_destinations = up_destinations.borrow_mut();
+        up_destinations[destination as usize] = up;
+        if up {
+            drtio_routing::interconnect_enable(routing_table, 0, destination);
+            info!("[DEST#{}] destination is up", destination);
+        } else {
+            drtio_routing::interconnect_disable(destination);
+            info!("[DEST#{}] destination is down", destination);
+        }
+    }
+
+    async fn destination_up(up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>, destination: u8) -> bool {
+        let up_destinations = up_destinations.borrow();
+        up_destinations[destination as usize]
+    }
+
+    async fn destination_survey(aux_mutex: &Rc<Mutex<bool>>, routing_table: &drtio_routing::RoutingTable,
+            up_links: &[bool],
+            up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
+            timer: GlobalTimer) {
+        for destination in 0..drtio_routing::DEST_COUNT {
+            let hop = routing_table.0[destination][0];
+            let destination = destination as u8;
+
+            if hop == 0 {
+                /* local RTIO */
+                if !destination_up(up_destinations, destination).await {
+                    destination_set_up(routing_table, up_destinations, destination, true).await;
+                }
+            } else if hop as usize <= csr::DRTIO.len() {
+                let linkno = hop - 1;
+                if destination_up(up_destinations, destination).await {
+                    if up_links[linkno as usize] {
+                        let reply = aux_transact(aux_mutex, linkno, &Packet::DestinationStatusRequest {
+                            destination: destination
+                        }, timer).await;
+                        match reply {
+                            Ok(Packet::DestinationDownReply) =>
+                                destination_set_up(routing_table, up_destinations, destination, false).await,
+                            Ok(Packet::DestinationOkReply) => (),
+                            Ok(Packet::DestinationSequenceErrorReply { channel }) =>
+                                error!("[DEST#{}] RTIO sequence error involving channel 0x{:04x}", destination, channel),
+                            Ok(Packet::DestinationCollisionReply { channel }) =>
+                                error!("[DEST#{}] RTIO collision involving channel 0x{:04x}", destination, channel),
+                            Ok(Packet::DestinationBusyReply { channel }) =>
+                                error!("[DEST#{}] RTIO busy error involving channel 0x{:04x}", destination, channel),
+                            Ok(packet) => error!("[DEST#{}] received unexpected aux packet: {:?}", destination, packet),
+                            Err(e) => error!("[DEST#{}] communication failed ({})", destination, e)
+                        }
+                    } else {
+                        destination_set_up(routing_table, up_destinations, destination, false).await;
+                    }
+                } else {
+                    if up_links[linkno as usize] {
+                        let reply = aux_transact(aux_mutex, linkno, &Packet::DestinationStatusRequest {
+                            destination: destination
+                        }, timer).await;
+                        match reply {
+                            Ok(Packet::DestinationDownReply) => (),
+                            Ok(Packet::DestinationOkReply) => {
+                                destination_set_up(routing_table, up_destinations, destination, true).await;
+                                init_buffer_space(destination as u8, linkno).await;
+                            },
+                            Ok(packet) => error!("[DEST#{}] received unexpected aux packet: {:?}", destination, packet),
+                            Err(e) => error!("[DEST#{}] communication failed ({})", destination, e)
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    pub async fn link_thread(aux_mutex: &Rc<Mutex<bool>>,
+            routing_table: &drtio_routing::RoutingTable,
+            up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>,
+            timer: GlobalTimer) {
+        let mut up_links = [false; csr::DRTIO.len()];
+        loop {
+            for linkno in 0..csr::DRTIO.len() {
+                let linkno = linkno as u8;
+                if up_links[linkno as usize] {
+                    /* link was previously up */
+                    if link_rx_up(linkno).await {
+                        process_unsolicited_aux(aux_mutex, linkno).await;
+                        process_local_errors(linkno).await;
+                    } else {
+                        info!("[LINK#{}] link is down", linkno);
+                        up_links[linkno as usize] = false;
+                    }
+                } else {
+                    /* link was previously down */
+                    if link_rx_up(linkno).await {
+                        info!("[LINK#{}] link RX became up, pinging", linkno);
+                        let ping_count = ping_remote(aux_mutex, linkno, timer).await;
+                        if ping_count > 0 {
+                            info!("[LINK#{}] remote replied after {} packets", linkno, ping_count);
+                            up_links[linkno as usize] = true;
+                            if let Err(e) = sync_tsc(aux_mutex, linkno, timer).await {
+                                error!("[LINK#{}] failed to sync TSC ({})", linkno, e);
+                            }
+                            if let Err(e) = load_routing_table(aux_mutex, linkno, routing_table, timer).await {
+                                error!("[LINK#{}] failed to load routing table ({})", linkno, e);
+                            }
+                            if let Err(e) = set_rank(aux_mutex, linkno, 1 as u8, timer).await {
+                                error!("[LINK#{}] failed to set rank ({})", linkno, e);
+                            }
+                            info!("[LINK#{}] link initialization completed", linkno);
+                        } else {
+                            error!("[LINK#{}] ping failed", linkno);
+                        }
+                    }
+                }
+            }
+            destination_survey(aux_mutex, routing_table, &up_links, up_destinations, timer).await;
+            let mut countdown = timer.countdown();
+            delay(&mut countdown, Milliseconds(200)).await;
+        }
+    }
+
+    #[allow(dead_code)]
+    pub fn reset(aux_mutex: Rc<Mutex<bool>>, mut timer: GlobalTimer) {
+        for linkno in 0..csr::DRTIO.len() {
+            unsafe {
+                (csr::DRTIO[linkno].reset_write)(1);
+            }
+        }
+        timer.delay_ms(1);
+        for linkno in 0..csr::DRTIO.len() {
+            unsafe {
+                (csr::DRTIO[linkno].reset_write)(0);
+            }
+        }
+
+        for linkno in 0..csr::DRTIO.len() {
+            let linkno = linkno as u8;
+            if task::block_on(link_rx_up(linkno)) {
+                let reply = task::block_on(aux_transact(&aux_mutex, linkno,
+                    &Packet::ResetRequest, timer));
+                match reply {
+                    Ok(Packet::ResetAck) => (),
+                    Ok(_) => error!("[LINK#{}] reset failed, received unexpected aux packet", linkno),
+                    Err(e) => error!("[LINK#{}] reset failed, aux packet error ({})", linkno, e)
+                }
+            }
+        }
+    }
+}
+
+#[cfg(not(has_drtio))]
+pub mod drtio {
+    use super::*;
+
+    pub fn startup(_aux_mutex: &Rc<Mutex<bool>>, _routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>,
+        _up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>, _timer: GlobalTimer) {}
+    
+    #[allow(dead_code)]
+    pub fn reset(_aux_mutex: Rc<Mutex<bool>>, mut _timer: GlobalTimer) {}
+}
+
+pub fn startup(aux_mutex: &Rc<Mutex<bool>>,
+        routing_table: &Rc<RefCell<drtio_routing::RoutingTable>>,
+        up_destinations: &Rc<RefCell<[bool; drtio_routing::DEST_COUNT]>>, 
+        timer: GlobalTimer) {
+    drtio::startup(aux_mutex, routing_table, up_destinations, timer);
+    unsafe {
+        csr::rtio_core::reset_phy_write(1);
+    }
+}
+
+#[allow(dead_code)]
+pub fn reset(aux_mutex: Rc<Mutex<bool>>, timer: GlobalTimer) {
+    unsafe {
+        csr::rtio_core::reset_write(1);
+    }
+    drtio::reset(aux_mutex, timer)
+}

src/runtime/src/si5324.rs

@@ -1,273 +0,0 @@
-use core::result;
-use log::info;
-use libboard_zynq::{i2c::I2c, timer::GlobalTimer, time::Milliseconds};
-use embedded_hal::blocking::delay::DelayUs;
-#[cfg(not(si5324_soft_reset))]
-use pl::csr;
-
-type Result<T> = result::Result<T, &'static str>;
-
-const ADDRESS: u8 = 0x68;
-
-#[cfg(not(si5324_soft_reset))]
-fn hard_reset(timer: GlobalTimer) {
-    unsafe { csr::si5324_rst_n::out_write(0); }
-    timer.delay_us(1_000);
-    unsafe { csr::si5324_rst_n::out_write(1); }
-    timer.delay_us(10_000);
-}
-
-// NOTE: the logical parameters DO NOT MAP to physical values written
-// into registers. They have to be mapped; see the datasheet.
-// DSPLLsim reports the logical parameters in the design summary, not
-// the physical register values.
-pub struct FrequencySettings {
-    pub n1_hs: u8,
-    pub nc1_ls: u32,
-    pub n2_hs: u8,
-    pub n2_ls: u32,
-    pub n31: u32,
-    pub n32: u32,
-    pub bwsel: u8,
-    pub crystal_ref: bool
-}
-
-pub enum Input {
-    Ckin1,
-    Ckin2,
-}
-
-fn map_frequency_settings(settings: &FrequencySettings) -> Result<FrequencySettings> {
-    if settings.nc1_ls != 0 && (settings.nc1_ls % 2) == 1 {
-        return Err("NC1_LS must be 0 or even")
-    }
-    if settings.nc1_ls > (1 << 20) {
-        return Err("NC1_LS is too high")
-    }
-    if (settings.n2_ls % 2) == 1 {
-        return Err("N2_LS must be even")
-    }
-    if settings.n2_ls > (1 << 20) {
-        return Err("N2_LS is too high")
-    }
-    if settings.n31 > (1 << 19) {
-        return Err("N31 is too high")
-    }
-    if settings.n32 > (1 << 19) {
-        return Err("N32 is too high")
-    }
-    let r = FrequencySettings {
-        n1_hs: match settings.n1_hs {
-            4  => 0b000,
-            5  => 0b001,
-            6  => 0b010,
-            7  => 0b011,
-            8  => 0b100,
-            9  => 0b101,
-            10 => 0b110,
-            11 => 0b111,
-            _  => return Err("N1_HS has an invalid value")
-        },
-        nc1_ls: settings.nc1_ls - 1,
-        n2_hs: match settings.n2_hs {
-            4  => 0b000,
-            5  => 0b001,
-            6  => 0b010,
-            7  => 0b011,
-            8  => 0b100,
-            9  => 0b101,
-            10 => 0b110,
-            11 => 0b111,
-            _  => return Err("N2_HS has an invalid value")
-        },
-        n2_ls: settings.n2_ls - 1,
-        n31: settings.n31 - 1,
-        n32: settings.n32 - 1,
-        bwsel: settings.bwsel,
-        crystal_ref: settings.crystal_ref
-    };
-    Ok(r)
-}
-
-fn write(i2c: &mut I2c, reg: u8, val: u8) -> Result<()> {
-    i2c.start().unwrap();
-    if !i2c.write(ADDRESS << 1).unwrap() {
-        return Err("Si5324 failed to ack write address")
-    }
-    if !i2c.write(reg).unwrap() {
-        return Err("Si5324 failed to ack register")
-    }
-    if !i2c.write(val).unwrap() {
-        return Err("Si5324 failed to ack value")
-    }
-    i2c.stop().unwrap();
-    Ok(())
-}
-
-fn write_no_ack_value(i2c: &mut I2c, reg: u8, val: u8) -> Result<()> {
-    i2c.start().unwrap();
-    if !i2c.write(ADDRESS << 1).unwrap() {
-        return Err("Si5324 failed to ack write address")
-    }
-    if !i2c.write(reg).unwrap() {
-        return Err("Si5324 failed to ack register")
-    }
-    i2c.write(val).unwrap();
-    i2c.stop().unwrap();
-    Ok(())
-}
-
-fn read(i2c: &mut I2c, reg: u8) -> Result<u8> {
-    i2c.start().unwrap();
-    if !i2c.write(ADDRESS << 1).unwrap() {
-        return Err("Si5324 failed to ack write address")
-    }
-    if !i2c.write(reg).unwrap() {
-        return Err("Si5324 failed to ack register")
-    }
-    i2c.restart().unwrap();
-    if !i2c.write((ADDRESS << 1) | 1).unwrap() {
-        return Err("Si5324 failed to ack read address")
-    }
-    let val = i2c.read(false).unwrap();
-    i2c.stop().unwrap();
-    Ok(val)
-}
-
-fn rmw<F>(i2c: &mut I2c, reg: u8, f: F) -> Result<()> where
-        F: Fn(u8) -> u8 {
-    let value = read(i2c, reg)?;
-    write(i2c, reg, f(value))?;
-    Ok(())
-}
-
-fn ident(i2c: &mut I2c) -> Result<u16> {
-    Ok(((read(i2c, 134)? as u16) << 8) | (read(i2c, 135)? as u16))
-}
-
-#[cfg(si5324_soft_reset)]
-fn soft_reset(i2c: &mut I2c, timer: GlobalTimer) -> Result<()> {
-    write_no_ack_value(i2c, 136, read(i2c, 136)? | 0x80)?;
-    timer.delay_us(10_000);
-    Ok(())
-}
-
-fn has_xtal(i2c: &mut I2c) -> Result<bool> {
-    Ok((read(i2c, 129)? & 0x01) == 0)  // LOSX_INT=0
-}
-
-fn has_ckin(i2c: &mut I2c, input: Input) -> Result<bool> {
-    match input {
-        Input::Ckin1 => Ok((read(i2c, 129)? & 0x02) == 0),  // LOS1_INT=0
-        Input::Ckin2 => Ok((read(i2c, 129)? & 0x04) == 0),  // LOS2_INT=0
-    }
-}
-
-fn locked(i2c: &mut I2c) -> Result<bool> {
-    Ok((read(i2c, 130)? & 0x01) == 0)  // LOL_INT=0
-}
-
-fn monitor_lock(i2c: &mut I2c, timer: GlobalTimer) -> Result<()> {
-    info!("waiting for Si5324 lock...");
-    let timeout = timer.get_time() + Milliseconds(20_000);
-    while !locked(i2c)? {
-        // Yes, lock can be really slow.
-        if timer.get_time() > timeout {
-            return Err("Si5324 lock timeout");
-        }
-    }
-    info!("  ...locked");
-    Ok(())
-}
-
-fn init(i2c: &mut I2c, timer: GlobalTimer) -> Result<()> {
-    #[cfg(not(si5324_soft_reset))]
-    hard_reset(timer);
-
-    #[cfg(feature = "target_kasli_soc")]
-    {
-        i2c.pca9548_select(0x70, 0)?;
-        i2c.pca9548_select(0x71, 1 << 3)?;
-    }
-
-    if ident(i2c)? != 0x0182 {
-        return Err("Si5324 does not have expected product number");
-    }
-
-    #[cfg(si5324_soft_reset)]
-    soft_reset(i2c, timer)?;
-    Ok(())
-}
-
-pub fn bypass(i2c: &mut I2c, input: Input, timer: GlobalTimer) -> Result<()> {
-    let cksel_reg = match input {
-        Input::Ckin1 => 0b00,
-        Input::Ckin2 => 0b01,
-    };
-    init(i2c, timer)?;
-    rmw(i2c, 21,  |v| v & 0xfe)?;                        // CKSEL_PIN=0
-    rmw(i2c, 3,   |v| (v & 0x3f) | (cksel_reg << 6))?;   // CKSEL_REG
-    rmw(i2c, 4,   |v| (v & 0x3f) | (0b00 << 6))?;        // AUTOSEL_REG=b00
-    rmw(i2c, 6,   |v| (v & 0xc0) | 0b111111)?;           // SFOUT2_REG=b111 SFOUT1_REG=b111
-    rmw(i2c, 0,   |v| (v & 0xfd) | 0x02)?;               // BYPASS_REG=1
-    Ok(())
-}
-
-pub fn setup(i2c: &mut I2c, settings: &FrequencySettings, input: Input, timer: GlobalTimer) -> Result<()> {
-    let s = map_frequency_settings(settings)?;
-    let cksel_reg = match input {
-        Input::Ckin1 => 0b00,
-        Input::Ckin2 => 0b01,
-    };
-
-    init(i2c, timer)?;
-    if settings.crystal_ref {
-        rmw(i2c, 0,   |v| v | 0x40)?;                     // FREE_RUN=1
-    }
-    rmw(i2c, 2,   |v| (v & 0x0f) | (s.bwsel << 4))?;
-    rmw(i2c, 21,  |v| v & 0xfe)?;                        // CKSEL_PIN=0
-    rmw(i2c, 3,   |v| (v & 0x2f) | (cksel_reg << 6) | 0x10)?;  // CKSEL_REG, SQ_ICAL=1
-    rmw(i2c, 4,   |v| (v & 0x3f) | (0b00 << 6))?;        // AUTOSEL_REG=b00
-    rmw(i2c, 6,   |v| (v & 0xc0) | 0b111111)?;           // SFOUT2_REG=b111 SFOUT1_REG=b111
-    write(i2c, 25,  (s.n1_hs  << 5 ) as u8)?;
-    write(i2c, 31,  (s.nc1_ls >> 16) as u8)?;
-    write(i2c, 32,  (s.nc1_ls >> 8 ) as u8)?;
-    write(i2c, 33,  (s.nc1_ls)       as u8)?;
-    write(i2c, 34,  (s.nc1_ls >> 16) as u8)?;            // write to NC2_LS as well
-    write(i2c, 35,  (s.nc1_ls >> 8 ) as u8)?;
-    write(i2c, 36,  (s.nc1_ls)       as u8)?;
-    write(i2c, 40,  (s.n2_hs  << 5 ) as u8 | (s.n2_ls  >> 16) as u8)?;
-    write(i2c, 41,  (s.n2_ls  >> 8 ) as u8)?;
-    write(i2c, 42,  (s.n2_ls)        as u8)?;
-    write(i2c, 43,  (s.n31    >> 16) as u8)?;
-    write(i2c, 44,  (s.n31    >> 8)  as u8)?;
-    write(i2c, 45,  (s.n31)          as u8)?;
-    write(i2c, 46,  (s.n32    >> 16) as u8)?;
-    write(i2c, 47,  (s.n32    >> 8)  as u8)?;
-    write(i2c, 48,  (s.n32)          as u8)?;
-    rmw(i2c, 137, |v| v | 0x01)?;                       // FASTLOCK=1
-    rmw(i2c, 136, |v| v | 0x40)?;                       // ICAL=1
-
-    if !has_xtal(i2c)? {
-        return Err("Si5324 misses XA/XB signal");
-    }
-    if !has_ckin(i2c, input)? {
-        return Err("Si5324 misses clock input signal");
-    }
-
-    monitor_lock(i2c, timer)?;
-    Ok(())
-}
-
-pub fn select_input(i2c: &mut I2c, input: Input, timer: GlobalTimer) -> Result<()> {
-    let cksel_reg = match input {
-        Input::Ckin1 => 0b00,
-        Input::Ckin2 => 0b01,
-    };
-    rmw(i2c, 3,   |v| (v & 0x3f) | (cksel_reg << 6))?;
-    if !has_ckin(i2c, input)? {
-        return Err("Si5324 misses clock input signal");
-    }
-    monitor_lock(i2c, timer)?;
-    Ok(())
-}