From da366b7a0d878a3c6a41a8341a530344b2589095 Mon Sep 17 00:00:00 2001
From: mwojcik <mw@m-labs.hk>
Date: Tue, 21 Sep 2021 11:13:36 +0200
Subject: [PATCH] runtime: added drtio init function

---
 src/runtime/src/main.rs | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs
index 1b5561c5..a5df6ada 100644
--- a/src/runtime/src/main.rs
+++ b/src/runtime/src/main.rs
@@ -89,6 +89,19 @@ fn init_rtio(timer: &mut GlobalTimer, cfg: &Config) {
     }
 }
 
+#[cfg(has_drtio)]
+fn init_drtio(timer: &mut GlobalTimer)
+{
+    unsafe {
+        csr::drtio_transceiver::stable_clkin_write(1);
+    }
+    timer.delay_ms(2); // wait for CPLL/QPLL lock
+    unsafe {
+        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 {
@@ -177,7 +190,9 @@ pub fn main_core0() {
     let up_destinations = [false; drtio_routing::DEST_COUNT];
     #[cfg(has_drtio_routing)]
     drtio_routing::interconnect_disable_all();
-
+    #[cfg(has_drtio)]
+    init_drtio(&mut timer);
+    
     init_rtio(&mut timer, &cfg);
     task::spawn(report_async_rtio_errors());