From b915176b29d401d51c4476fd18be6dc75ef2f0b3 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Tue, 4 Aug 2020 17:32:43 +0800
Subject: [PATCH] runtime: implement acpki RTIO output

---
 src/Cargo.lock              |  1 +
 src/runtime/Cargo.toml      |  1 +
 src/runtime/src/rtio_acp.rs | 92 +++++++++++++++++++++++++++++++++++--
 3 files changed, 90 insertions(+), 4 deletions(-)

diff --git a/src/Cargo.lock b/src/Cargo.lock
index 02f76969..7cedaea3 100644
--- a/src/Cargo.lock
+++ b/src/Cargo.lock
@@ -428,6 +428,7 @@ dependencies = [
  "num-derive",
  "num-traits",
  "unwind",
+ "vcell",
  "void",
 ]
 
diff --git a/src/runtime/Cargo.toml b/src/runtime/Cargo.toml
index 1c5d6a30..c4b4a7e9 100644
--- a/src/runtime/Cargo.toml
+++ b/src/runtime/Cargo.toml
@@ -24,6 +24,7 @@ async-recursion = "0.3"
 fatfs = { version = "0.3", features = ["core_io"], default-features = false }
 log_buffer = { version = "1.2" }
 libm = { version = "0.2", features = ["unstable"] }
+vcell = "0.1"
 
 libboard_zynq = { git = "https://git.m-labs.hk/M-Labs/zc706.git" }
 libsupport_zynq = { default-features = false, features = ["alloc_core"], git = "https://git.m-labs.hk/M-Labs/zc706.git" }
diff --git a/src/runtime/src/rtio_acp.rs b/src/runtime/src/rtio_acp.rs
index a9c53236..4590c65e 100644
--- a/src/runtime/src/rtio_acp.rs
+++ b/src/runtime/src/rtio_acp.rs
@@ -1,9 +1,18 @@
 use cslice::CSlice;
+use vcell::VolatileCell;
+use libcortex_a9::asm;
 
 use crate::artiq_raise;
 
 use crate::pl::csr;
 
+pub const RTIO_O_STATUS_WAIT:                      i32 = 1;
+pub const RTIO_O_STATUS_UNDERFLOW:                 i32 = 2;
+pub const RTIO_O_STATUS_DESTINATION_UNREACHABLE:   i32 = 4;
+pub const RTIO_I_STATUS_WAIT_EVENT:                i32 = 1;
+pub const RTIO_I_STATUS_OVERFLOW:                  i32 = 2;
+pub const RTIO_I_STATUS_WAIT_STATUS:               i32 = 4;
+pub const RTIO_I_STATUS_DESTINATION_UNREACHABLE:   i32 = 8;
 
 #[repr(C)]
 pub struct TimestampedData {
@@ -11,9 +20,40 @@ pub struct TimestampedData {
     data: i32,
 }
 
+#[repr(C, align(32))]
+struct Transaction {
+    request_cmd: i8,
+    padding0: i8,
+    padding1: i8,
+    padding2: i8,
+    request_target: i32,
+    request_timestamp: i64,
+    request_data: i64,
+    padding: i64,
+    reply_status: VolatileCell<i32>,
+    reply_data: VolatileCell<i32>,
+    reply_timestamp: VolatileCell<u64>
+}
+
+static mut TRANSACTION_BUFFER: Transaction = Transaction {
+    request_cmd: 0,
+    padding0: 0,
+    padding1: 0,
+    padding2: 0,
+    request_target: 0,
+    request_timestamp: 0,
+    request_data: 0,
+    padding: 0,
+    reply_status: VolatileCell::new(0),
+    reply_data: VolatileCell::new(0),
+    reply_timestamp: VolatileCell::new(0)
+};
+
 pub extern fn init() {
     unsafe {
         csr::rtio_core::reset_write(1);
+        csr::rtio::engine_addr_base_write(&TRANSACTION_BUFFER as *const Transaction as u32);
+        csr::rtio::enable_write(1);
     }
 }
 
@@ -30,22 +70,65 @@ pub extern fn get_counter() -> i64 {
 }
 
 pub extern fn now_mu() -> i64 {
-    unimplemented!();
+    unsafe { TRANSACTION_BUFFER.request_timestamp }
 }
 
 pub extern fn at_mu(t: i64) {
-    unimplemented!();
+    unsafe { TRANSACTION_BUFFER.request_timestamp = t }
 }
 
 pub extern fn delay_mu(dt: i64) {
-    unimplemented!();
+    unsafe { TRANSACTION_BUFFER.request_timestamp += dt }
+}
+
+#[inline(never)]
+unsafe fn process_exceptional_status(channel: i32, status: i32) {
+    let timestamp = now_mu();
+    if status & RTIO_O_STATUS_WAIT != 0 {
+        // FIXME: this is a kludge and probably buggy (kernel interrupted?)
+        while csr::rtio::o_status_read() as i32 & RTIO_O_STATUS_WAIT != 0 {}
+    }
+    if status & RTIO_O_STATUS_UNDERFLOW != 0 {
+        artiq_raise!("RTIOUnderflow",
+            "RTIO underflow at {0} mu, channel {1}, slack {2} mu",
+            timestamp, channel as i64, timestamp - get_counter());
+    }
+    if status & RTIO_O_STATUS_DESTINATION_UNREACHABLE != 0 {
+        artiq_raise!("RTIODestinationUnreachable",
+            "RTIO destination unreachable, output, at {0} mu, channel {1}",
+            timestamp, channel as i64, 0);
+    }
 }
 
 pub extern fn output(target: i32, data: i32) {
-    unimplemented!();
+    unsafe {
+        // Clear status so we can observe response
+        TRANSACTION_BUFFER.reply_status.set(0);
+
+        TRANSACTION_BUFFER.request_cmd = 0;
+        TRANSACTION_BUFFER.request_target = target;
+        TRANSACTION_BUFFER.request_data = data as i64;
+
+        asm::dmb();
+        asm::sev();
+
+        let mut status;
+        loop {
+            status = TRANSACTION_BUFFER.reply_status.get();
+            if status != 0 {
+                break
+            }
+        }
+
+        let status = status & !0x10000;
+        if status != 0 {
+            process_exceptional_status(target >> 8, status);
+        }
+    }
 }
 
 pub extern fn output_wide(target: i32, data: CSlice<i32>) {
+    // TODO
     unimplemented!();
 }
 
@@ -62,5 +145,6 @@ pub extern fn input_timestamped_data(timeout: i64, channel: i32) -> TimestampedD
 }
 
 pub fn write_log(data: &[i8]) {
+    // TODO
     unimplemented!();
 }