From 9ecabfc251c631fe0ac8f108a9966d4148d28782 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Sat, 25 Apr 2020 18:17:18 +0800
Subject: [PATCH] moninj: process host messages

---
 runtime/src/moninj.rs | 68 ++++++++++++++++++++++++++++++++++++-------
 runtime/src/proto.rs  |  5 ++++
 2 files changed, 62 insertions(+), 11 deletions(-)

diff --git a/runtime/src/moninj.rs b/runtime/src/moninj.rs
index 3802e7a..b0098d7 100644
--- a/runtime/src/moninj.rs
+++ b/runtime/src/moninj.rs
@@ -1,5 +1,6 @@
 use core::fmt;
-use log::{info, warn};
+use alloc::collections::BTreeMap;
+use log::warn;
 
 use libboard_zynq::smoltcp;
 use libasync::task;
@@ -9,6 +10,7 @@ use num_derive::{FromPrimitive, ToPrimitive};
 use num_traits::{FromPrimitive, ToPrimitive};
 
 use crate::proto::*;
+use crate::pl::csr;
 
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
@@ -51,33 +53,77 @@ enum DeviceMessage {
     InjectionStatus = 1
 }
 
+fn read_probe(channel: i32, probe: u8) -> i32 {
+    unsafe {
+        csr::rtio_moninj::mon_chan_sel_write(channel as _);
+        csr::rtio_moninj::mon_probe_sel_write(probe as _);
+        csr::rtio_moninj::mon_value_update_write(1);
+        csr::rtio_moninj::mon_value_read() as i32
+    }
+}
+
+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 _);
+        csr::rtio_moninj::inj_value_write(value as _);
+    }
+}
+
+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) -> Result<()> {
     if !expect(&stream, b"ARTIQ moninj\n").await? {
         return Err(Error::UnexpectedPattern);
     }
+
+    let mut probe_watch_list: BTreeMap<(i32, i8), Option<i32>> = BTreeMap::new();
+    let mut inject_watch_list: BTreeMap<(i32, i8), Option<i8>> = BTreeMap::new();
+
     loop {
         let message: HostMessage = FromPrimitive::from_i8(read_i8(&stream).await?)
             .ok_or(Error::UnrecognizedPacket)?;
-        info!("{:?}", message);
         match message {
             HostMessage::MonitorProbe => {
                 let enable = read_bool(&stream).await?;
                 let channel = read_i32(&stream).await?;
                 let probe = read_i8(&stream).await?;
-            },
-            HostMessage::Inject => {
-                let channel = read_i32(&stream).await?;
-                let overrd = read_i8(&stream).await?;
-                let value = read_i8(&stream).await?;
-            },
-            HostMessage::GetInjectionStatus => {
-                let channel = read_i32(&stream).await?;
-                let overrd = read_i8(&stream).await?;
+                if enable {
+                    let _ = probe_watch_list.entry((channel, probe)).or_insert(None);
+                } else {
+                    let _ = probe_watch_list.remove(&(channel, probe));
+                }
             },
             HostMessage::MonitorInjection => {
                 let enable = read_bool(&stream).await?;
                 let channel = read_i32(&stream).await?;
                 let overrd = read_i8(&stream).await?;
+                if enable {
+                    let _ = inject_watch_list.entry((channel, overrd)).or_insert(None);
+                } else {
+                    let _ = inject_watch_list.remove(&(channel, overrd));
+                }
+            },
+            HostMessage::Inject => {
+                let channel = read_i32(&stream).await?;
+                let overrd = read_i8(&stream).await?;
+                let value = read_i8(&stream).await?;
+                inject(channel, overrd, value);
+            },
+            HostMessage::GetInjectionStatus => {
+                let channel = read_i32(&stream).await?;
+                let overrd = read_i8(&stream).await?;
+                let value = read_injection_status(channel, overrd);
+                write_i8(&stream, DeviceMessage::InjectionStatus.to_i8().unwrap()).await?;
+                write_i32(&stream, channel).await?;
+                write_i8(&stream, overrd).await?;
+                write_i8(&stream, value).await?;
             },
         }
     }
diff --git a/runtime/src/proto.rs b/runtime/src/proto.rs
index f01dca2..b8a2845 100644
--- a/runtime/src/proto.rs
+++ b/runtime/src/proto.rs
@@ -61,6 +61,11 @@ pub async fn read_drain(stream: &TcpStream, total: usize) -> Result<()> {
     Ok(())
 }
 
+pub async fn write_i8(stream: &TcpStream, value: i8) -> Result<()> {
+    stream.send([value as u8].iter().copied()).await?;
+    Ok(())
+}
+
 pub async fn write_i32(stream: &TcpStream, value: i32) -> Result<()> {
     stream.send([
         (value >> 24) as u8,