moninj: process host messages

This commit is contained in:
Sebastien Bourdeauducq 2020-04-25 18:17:18 +08:00
parent dcb45d7466
commit 9ecabfc251
2 changed files with 62 additions and 11 deletions

View File

@ -1,5 +1,6 @@
use core::fmt; use core::fmt;
use log::{info, warn}; use alloc::collections::BTreeMap;
use log::warn;
use libboard_zynq::smoltcp; use libboard_zynq::smoltcp;
use libasync::task; use libasync::task;
@ -9,6 +10,7 @@ use num_derive::{FromPrimitive, ToPrimitive};
use num_traits::{FromPrimitive, ToPrimitive}; use num_traits::{FromPrimitive, ToPrimitive};
use crate::proto::*; use crate::proto::*;
use crate::pl::csr;
#[derive(Debug, Clone, Copy, PartialEq, Eq)] #[derive(Debug, Clone, Copy, PartialEq, Eq)]
@ -51,33 +53,77 @@ enum DeviceMessage {
InjectionStatus = 1 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<()> { async fn handle_connection(stream: &TcpStream) -> Result<()> {
if !expect(&stream, b"ARTIQ moninj\n").await? { if !expect(&stream, b"ARTIQ moninj\n").await? {
return Err(Error::UnexpectedPattern); 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 { loop {
let message: HostMessage = FromPrimitive::from_i8(read_i8(&stream).await?) let message: HostMessage = FromPrimitive::from_i8(read_i8(&stream).await?)
.ok_or(Error::UnrecognizedPacket)?; .ok_or(Error::UnrecognizedPacket)?;
info!("{:?}", message);
match message { match message {
HostMessage::MonitorProbe => { HostMessage::MonitorProbe => {
let enable = read_bool(&stream).await?; let enable = read_bool(&stream).await?;
let channel = read_i32(&stream).await?; let channel = read_i32(&stream).await?;
let probe = read_i8(&stream).await?; let probe = read_i8(&stream).await?;
}, if enable {
HostMessage::Inject => { let _ = probe_watch_list.entry((channel, probe)).or_insert(None);
let channel = read_i32(&stream).await?; } else {
let overrd = read_i8(&stream).await?; let _ = probe_watch_list.remove(&(channel, probe));
let value = read_i8(&stream).await?; }
},
HostMessage::GetInjectionStatus => {
let channel = read_i32(&stream).await?;
let overrd = read_i8(&stream).await?;
}, },
HostMessage::MonitorInjection => { HostMessage::MonitorInjection => {
let enable = read_bool(&stream).await?; let enable = read_bool(&stream).await?;
let channel = read_i32(&stream).await?; let channel = read_i32(&stream).await?;
let overrd = read_i8(&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?;
}, },
} }
} }

View File

@ -61,6 +61,11 @@ pub async fn read_drain(stream: &TcpStream, total: usize) -> Result<()> {
Ok(()) 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<()> { pub async fn write_i32(stream: &TcpStream, value: i32) -> Result<()> {
stream.send([ stream.send([
(value >> 24) as u8, (value >> 24) as u8,