From 1c742496387254b495eb10ec94c2aabe7e08526d Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 12 Dec 2016 17:30:41 +0800
Subject: [PATCH] runtime: reset local DRTIO state

---
 artiq/runtime.rs/src/rtio_mgt.rs | 26 +++++++++++++++++++-------
 1 file changed, 19 insertions(+), 7 deletions(-)

diff --git a/artiq/runtime.rs/src/rtio_mgt.rs b/artiq/runtime.rs/src/rtio_mgt.rs
index 4c39755d2..88ce44e72 100644
--- a/artiq/runtime.rs/src/rtio_mgt.rs
+++ b/artiq/runtime.rs/src/rtio_mgt.rs
@@ -42,7 +42,7 @@ mod drtio {
     use board::csr;
     use sched::{Scheduler, Waiter, Spawner};
 
-    pub fn init(scheduler: &Scheduler) {
+    pub fn startup(scheduler: &Scheduler) {
         scheduler.spawner().spawn(4096, link_thread);
         scheduler.spawner().spawn(4096, error_thread);
     }
@@ -64,7 +64,7 @@ mod drtio {
         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 {}
@@ -74,6 +74,18 @@ mod drtio {
         }
     }
 
+    pub fn init() {
+        if link_is_up() {
+            unsafe {
+                csr::drtio::reset_write(1);
+                while csr::drtio::o_wait_read() == 1 {}
+            }
+            for channel in 0..16 {
+                init_channel(channel);
+            }
+        }
+    }
+
     pub fn link_thread(waiter: Waiter, _spawner: Spawner) {
         loop {
             waiter.until(link_is_up).unwrap();
@@ -84,9 +96,7 @@ mod drtio {
 
             sync_tsc();
             info!("TSC synced");
-            for channel in 0..16 {
-                init_channel(channel);
-            }
+            init();
             info!("link initialization completed");
 
             waiter.until(|| !link_is_up()).unwrap();
@@ -121,7 +131,8 @@ mod drtio {
 mod drtio {
     use sched::Scheduler;
 
-    pub fn init(_scheduler: &Scheduler) {}
+    pub fn startup(_scheduler: &Scheduler) {}
+    pub fn init() {}
 }
 
 pub fn startup(scheduler: &Scheduler) {
@@ -150,7 +161,7 @@ pub fn startup(scheduler: &Scheduler) {
         warn!("fix clocking and reset the device");
     }
 
-    drtio::init(scheduler);
+    drtio::startup(scheduler);
     init_core()
 }
 
@@ -158,4 +169,5 @@ pub fn init_core() {
     unsafe {
         csr::rtio_core::reset_write(1);
     }
+    drtio::init()
 }