diff --git a/runtime/src/moninj.rs b/runtime/src/moninj.rs index 3802e7aa..b0098d73 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> = BTreeMap::new(); + let mut inject_watch_list: BTreeMap<(i32, i8), Option> = 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 f01dca2e..b8a28452 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,