From 78f18a12eb3903d4712c90284199b530211e0cef Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 7 Nov 2016 19:34:34 +0800
Subject: [PATCH] runtime: basic DRTIO init code

---
 artiq/runtime.rs/src/drtio.rs | 67 +++++++++++++++++++++++++++++++++++
 artiq/runtime.rs/src/lib.rs   |  7 ++++
 2 files changed, 74 insertions(+)
 create mode 100644 artiq/runtime.rs/src/drtio.rs

diff --git a/artiq/runtime.rs/src/drtio.rs b/artiq/runtime.rs/src/drtio.rs
new file mode 100644
index 000000000..8ee5dfc23
--- /dev/null
+++ b/artiq/runtime.rs/src/drtio.rs
@@ -0,0 +1,67 @@
+use board::csr;
+use sched::{Waiter, Spawner};
+
+fn drtio_link_is_up() -> bool {
+    unsafe {
+        csr::drtio::link_status_read() == 4
+    }
+}
+
+fn drtio_sync_tsc() {
+    unsafe {
+        csr::drtio::set_time_write(1);
+        while csr::drtio::set_time_read() == 1 {}
+    }
+}
+
+fn drtio_init_channel(channel: u16) {
+    unsafe {
+        csr::drtio::chan_sel_override_write(channel);
+        csr::drtio::chan_sel_override_en_write(1);
+        
+        csr::drtio::o_reset_channel_status_write(1);
+        csr::drtio::o_get_fifo_space_write(1);
+        while csr::drtio::o_wait_read() == 1 {}  // TODO: timeout
+        info!("FIFO space on channel {} is {}", channel, csr::drtio::o_dbg_fifo_space_read());
+
+        csr::drtio::chan_sel_override_en_write(0);
+    }
+}
+
+pub fn link_thread(waiter: Waiter, _spawner: Spawner) {
+    loop {
+        waiter.until(drtio_link_is_up).unwrap();
+        info!("link is up");
+
+        drtio_sync_tsc();
+        info!("TSC synced");
+        for channel in 0..16 {
+            drtio_init_channel(channel);
+        }
+        info!("link initialization completed");
+
+        waiter.until(|| !drtio_link_is_up()).unwrap();
+        info!("link is down");
+    }
+}
+
+fn drtio_error_present() -> bool {
+    unsafe {
+        csr::drtio::err_present_read() != 0
+    }
+}
+
+fn drtio_get_error() -> u8 {
+    unsafe {
+        let err = csr::drtio::err_code_read();
+        csr::drtio::err_present_write(1);
+        err
+    }
+}
+
+pub fn error_thread(waiter: Waiter, _spawner: Spawner) {
+    loop {
+        waiter.until(drtio_error_present).unwrap();
+        error!("DRTIO error {}", drtio_get_error());   
+    }
+}
diff --git a/artiq/runtime.rs/src/lib.rs b/artiq/runtime.rs/src/lib.rs
index 48efd5d0d..101df55b2 100644
--- a/artiq/runtime.rs/src/lib.rs
+++ b/artiq/runtime.rs/src/lib.rs
@@ -79,6 +79,8 @@ mod session;
 mod moninj;
 #[cfg(has_rtio_analyzer)]
 mod analyzer;
+#[cfg(has_drtio)]
+mod drtio;
 
 extern {
     fn network_init();
@@ -114,6 +116,11 @@ pub unsafe extern fn rust_main() {
         scheduler.spawner().spawn(4096, moninj::thread);
         #[cfg(has_rtio_analyzer)]
         scheduler.spawner().spawn(4096, analyzer::thread);
+        #[cfg(has_drtio)]
+        {
+            scheduler.spawner().spawn(4096, drtio::link_thread);
+            scheduler.spawner().spawn(4096, drtio::error_thread);
+        }
 
         loop {
             scheduler.run();