2021-10-06 13:05:45 +08:00
|
|
|
use core::{fmt, cell::RefCell};
|
|
|
|
use alloc::{collections::BTreeMap, rc::Rc};
|
2021-10-06 14:26:37 +08:00
|
|
|
use log::{debug, info, warn};
|
2020-04-25 20:31:38 +08:00
|
|
|
use void::Void;
|
2020-04-24 14:37:16 +08:00
|
|
|
|
2021-10-06 13:05:45 +08:00
|
|
|
use libboard_artiq::drtio_routing;
|
|
|
|
|
2020-04-25 20:31:38 +08:00
|
|
|
use libboard_zynq::{smoltcp, timer::GlobalTimer, time::Milliseconds};
|
|
|
|
use libasync::{task, smoltcp::TcpStream, block_async, nb};
|
2021-10-06 13:05:45 +08:00
|
|
|
use libcortex_a9::mutex::Mutex;
|
2020-04-24 14:37:16 +08:00
|
|
|
|
|
|
|
use num_derive::{FromPrimitive, ToPrimitive};
|
|
|
|
use num_traits::{FromPrimitive, ToPrimitive};
|
2020-04-25 20:31:38 +08:00
|
|
|
use futures::{pin_mut, select_biased, FutureExt};
|
2020-04-24 14:37:16 +08:00
|
|
|
|
2020-06-05 17:14:36 +08:00
|
|
|
use crate::proto_async::*;
|
2020-04-24 14:37:16 +08:00
|
|
|
|
|
|
|
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
|
|
|
pub enum Error {
|
|
|
|
NetworkError(smoltcp::Error),
|
|
|
|
UnexpectedPattern,
|
|
|
|
UnrecognizedPacket,
|
|
|
|
}
|
|
|
|
|
|
|
|
pub type Result<T> = core::result::Result<T, Error>;
|
|
|
|
|
|
|
|
impl fmt::Display for Error {
|
|
|
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
|
|
|
match self {
|
|
|
|
&Error::NetworkError(error) => write!(f, "network error: {}", error),
|
|
|
|
&Error::UnexpectedPattern => write!(f, "unexpected pattern"),
|
|
|
|
&Error::UnrecognizedPacket => write!(f, "unrecognized packet"),
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
impl From<smoltcp::Error> for Error {
|
|
|
|
fn from(error: smoltcp::Error) -> Self {
|
|
|
|
Error::NetworkError(error)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#[derive(Debug, FromPrimitive, ToPrimitive)]
|
|
|
|
enum HostMessage {
|
|
|
|
MonitorProbe = 0,
|
|
|
|
MonitorInjection = 3,
|
|
|
|
Inject = 1,
|
|
|
|
GetInjectionStatus = 2
|
|
|
|
}
|
|
|
|
|
|
|
|
#[derive(Debug, FromPrimitive, ToPrimitive)]
|
|
|
|
enum DeviceMessage {
|
|
|
|
MonitorStatus = 0,
|
|
|
|
InjectionStatus = 1
|
|
|
|
}
|
|
|
|
|
2021-10-06 13:05:45 +08:00
|
|
|
#[cfg(has_drtio)]
|
|
|
|
mod remote_moninj {
|
|
|
|
use super::*;
|
2022-05-13 11:35:52 +08:00
|
|
|
use libboard_artiq::drtioaux_async;
|
2021-10-06 13:05:45 +08:00
|
|
|
use crate::rtio_mgt::drtio;
|
2021-10-06 14:26:37 +08:00
|
|
|
use log::error;
|
2021-10-06 13:05:45 +08:00
|
|
|
|
2022-05-13 11:35:52 +08:00
|
|
|
pub async fn read_probe(aux_mutex: &Rc<Mutex<bool>>, timer: GlobalTimer, linkno: u8, destination: u8, channel: i32, probe: i8) -> i64 {
|
|
|
|
let reply = drtio::aux_transact(aux_mutex, linkno, &drtioaux_async::Packet::MonitorRequest {
|
2021-10-06 13:05:45 +08:00
|
|
|
destination: destination,
|
|
|
|
channel: channel as _,
|
|
|
|
probe: probe as _},
|
2022-05-13 11:35:52 +08:00
|
|
|
timer).await;
|
2021-10-06 13:05:45 +08:00
|
|
|
match reply {
|
2022-05-13 11:35:52 +08:00
|
|
|
Ok(drtioaux_async::Packet::MonitorReply { value }) => return value as i64,
|
2021-10-06 13:05:45 +08:00
|
|
|
Ok(packet) => error!("received unexpected aux packet: {:?}", packet),
|
|
|
|
Err(e) => error!("aux packet error ({})", e)
|
|
|
|
}
|
|
|
|
0
|
2020-04-25 18:17:18 +08:00
|
|
|
}
|
|
|
|
|
2022-05-13 11:35:52 +08:00
|
|
|
pub async fn inject(aux_mutex: &Rc<Mutex<bool>>, _timer: GlobalTimer, linkno: u8, destination: u8, channel: i32, overrd: i8, value: i8) {
|
2021-10-06 13:05:45 +08:00
|
|
|
let _lock = aux_mutex.lock();
|
2022-05-13 11:35:52 +08:00
|
|
|
drtioaux_async::send(linkno, &drtioaux_async::Packet::InjectionRequest {
|
2021-10-06 13:05:45 +08:00
|
|
|
destination: destination,
|
|
|
|
channel: channel as _,
|
|
|
|
overrd: overrd as _,
|
|
|
|
value: value as _
|
2022-05-13 11:35:52 +08:00
|
|
|
}).await.unwrap();
|
2021-10-06 13:05:45 +08:00
|
|
|
}
|
|
|
|
|
2022-05-13 11:35:52 +08:00
|
|
|
pub async fn read_injection_status(aux_mutex: &Rc<Mutex<bool>>, timer: GlobalTimer, linkno: u8, destination: u8, channel: i32, overrd: i8) -> i8 {
|
|
|
|
let reply = drtio::aux_transact(aux_mutex,
|
2021-10-06 13:05:45 +08:00
|
|
|
linkno,
|
2022-05-13 11:35:52 +08:00
|
|
|
&drtioaux_async::Packet::InjectionStatusRequest {
|
2021-10-06 13:05:45 +08:00
|
|
|
destination: destination,
|
|
|
|
channel: channel as _,
|
|
|
|
overrd: overrd as _},
|
2022-05-13 11:35:52 +08:00
|
|
|
timer).await;
|
2021-10-06 13:05:45 +08:00
|
|
|
match reply {
|
2022-05-13 11:35:52 +08:00
|
|
|
Ok(drtioaux_async::Packet::InjectionStatusReply { value }) => return value as i8,
|
2021-10-06 13:05:45 +08:00
|
|
|
Ok(packet) => error!("received unexpected aux packet: {:?}", packet),
|
|
|
|
Err(e) => error!("aux packet error ({})", e)
|
|
|
|
}
|
|
|
|
0
|
2020-04-25 18:17:18 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-10-06 13:05:45 +08:00
|
|
|
mod local_moninj {
|
|
|
|
use libboard_artiq::pl::csr;
|
|
|
|
|
2022-03-17 20:26:44 +08:00
|
|
|
pub fn read_probe(channel: i32, probe: i8) -> i64 {
|
2021-10-06 13:05:45 +08:00
|
|
|
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);
|
2022-03-17 20:26:44 +08:00
|
|
|
csr::rtio_moninj::mon_value_read() as i64
|
2021-10-06 13:05:45 +08:00
|
|
|
}
|
2020-04-25 18:17:18 +08:00
|
|
|
}
|
2021-10-06 13:05:45 +08:00
|
|
|
|
|
|
|
pub 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 _);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
pub 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
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#[cfg(has_drtio)]
|
|
|
|
macro_rules! dispatch {
|
|
|
|
($timer:ident, $aux_mutex:ident, $routing_table:ident, $channel:expr, $func:ident $(, $param:expr)*) => {{
|
|
|
|
let destination = ($channel >> 16) as u8;
|
|
|
|
let channel = $channel;
|
2022-03-02 15:45:17 +08:00
|
|
|
let hop = $routing_table.0[destination as usize][0];
|
2021-10-06 13:05:45 +08:00
|
|
|
if hop == 0 {
|
|
|
|
local_moninj::$func(channel.into(), $($param, )*)
|
|
|
|
} else {
|
|
|
|
let linkno = hop - 1 as u8;
|
2022-05-13 11:35:52 +08:00
|
|
|
remote_moninj::$func($aux_mutex, $timer, linkno, destination, channel, $($param, )*).await
|
2021-10-06 13:05:45 +08:00
|
|
|
}
|
|
|
|
}}
|
|
|
|
}
|
|
|
|
|
|
|
|
#[cfg(not(has_drtio))]
|
|
|
|
macro_rules! dispatch {
|
|
|
|
($timer:ident, $aux_mutex:ident, $routing_table:ident, $channel:expr, $func:ident $(, $param:expr)*) => {{
|
|
|
|
let channel = $channel as u16;
|
2021-10-06 14:26:37 +08:00
|
|
|
local_moninj::$func(channel.into(), $($param, )*)
|
2021-10-06 13:05:45 +08:00
|
|
|
}}
|
2020-04-25 18:17:18 +08:00
|
|
|
}
|
|
|
|
|
2021-10-06 13:05:45 +08:00
|
|
|
async fn handle_connection(stream: &TcpStream, timer: GlobalTimer,
|
2022-03-02 15:45:17 +08:00
|
|
|
_aux_mutex: &Rc<Mutex<bool>>, _routing_table: &drtio_routing::RoutingTable) -> Result<()> {
|
2020-04-24 14:37:16 +08:00
|
|
|
if !expect(&stream, b"ARTIQ moninj\n").await? {
|
|
|
|
return Err(Error::UnexpectedPattern);
|
|
|
|
}
|
2020-04-25 18:17:18 +08:00
|
|
|
|
2022-03-17 20:26:44 +08:00
|
|
|
let mut probe_watch_list: BTreeMap<(i32, i8), Option<i64>> = BTreeMap::new();
|
2020-04-25 18:17:18 +08:00
|
|
|
let mut inject_watch_list: BTreeMap<(i32, i8), Option<i8>> = BTreeMap::new();
|
2022-05-13 11:35:52 +08:00
|
|
|
let mut next_check = timer.get_time();
|
2020-04-24 14:37:16 +08:00
|
|
|
loop {
|
2020-04-25 20:31:38 +08:00
|
|
|
// TODO: we don't need fuse() here.
|
|
|
|
// remove after https://github.com/rust-lang/futures-rs/issues/1989 lands
|
|
|
|
let read_message_f = read_i8(&stream).fuse();
|
|
|
|
let next_check_c = next_check.clone();
|
|
|
|
let timeout = || -> nb::Result<(), Void> {
|
|
|
|
if timer.get_time() < next_check_c {
|
|
|
|
Err(nb::Error::WouldBlock)
|
|
|
|
} else {
|
|
|
|
Ok(())
|
|
|
|
}
|
|
|
|
};
|
|
|
|
let timeout_f = block_async!(timeout()).fuse();
|
|
|
|
pin_mut!(read_message_f, timeout_f);
|
|
|
|
select_biased! {
|
|
|
|
message = read_message_f => {
|
|
|
|
let message: HostMessage = FromPrimitive::from_i8(message?)
|
|
|
|
.ok_or(Error::UnrecognizedPacket)?;
|
|
|
|
match message {
|
|
|
|
HostMessage::MonitorProbe => {
|
|
|
|
let enable = read_bool(&stream).await?;
|
|
|
|
let channel = read_i32(&stream).await?;
|
|
|
|
let probe = read_i8(&stream).await?;
|
|
|
|
if enable {
|
|
|
|
let _ = probe_watch_list.entry((channel, probe)).or_insert(None);
|
|
|
|
debug!("START monitoring channel {}, probe {}", channel, probe);
|
|
|
|
} else {
|
|
|
|
let _ = probe_watch_list.remove(&(channel, probe));
|
|
|
|
debug!("END monitoring channel {}, probe {}", 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);
|
|
|
|
debug!("START monitoring channel {}, overrd {}", channel, overrd);
|
|
|
|
} else {
|
|
|
|
let _ = inject_watch_list.remove(&(channel, overrd));
|
|
|
|
debug!("END monitoring channel {}, overrd {}", channel, overrd);
|
|
|
|
}
|
|
|
|
},
|
|
|
|
HostMessage::Inject => {
|
|
|
|
let channel = read_i32(&stream).await?;
|
|
|
|
let overrd = read_i8(&stream).await?;
|
|
|
|
let value = read_i8(&stream).await?;
|
2021-10-06 13:05:45 +08:00
|
|
|
dispatch!(timer, _aux_mutex, _routing_table, channel, inject, overrd, value);
|
2020-04-25 20:31:38 +08:00
|
|
|
debug!("INJECT channel {}, overrd {}, value {}", channel, overrd, value);
|
|
|
|
},
|
|
|
|
HostMessage::GetInjectionStatus => {
|
|
|
|
let channel = read_i32(&stream).await?;
|
|
|
|
let overrd = read_i8(&stream).await?;
|
2021-10-06 13:05:45 +08:00
|
|
|
let value = dispatch!(timer, _aux_mutex, _routing_table, channel, read_injection_status, overrd);
|
2020-04-25 20:31:38 +08:00
|
|
|
write_i8(&stream, DeviceMessage::InjectionStatus.to_i8().unwrap()).await?;
|
|
|
|
write_i32(&stream, channel).await?;
|
|
|
|
write_i8(&stream, overrd).await?;
|
|
|
|
write_i8(&stream, value).await?;
|
|
|
|
},
|
2020-04-25 18:17:18 +08:00
|
|
|
}
|
2020-04-24 15:17:41 +08:00
|
|
|
},
|
2020-04-25 20:31:38 +08:00
|
|
|
_ = timeout_f => {
|
2020-04-25 20:45:25 +08:00
|
|
|
for (&(channel, probe), previous) in probe_watch_list.iter_mut() {
|
2021-10-06 13:05:45 +08:00
|
|
|
let current = dispatch!(timer, _aux_mutex, _routing_table, channel, read_probe, probe);
|
2020-04-25 20:45:25 +08:00
|
|
|
if previous.is_none() || previous.unwrap() != current {
|
|
|
|
write_i8(&stream, DeviceMessage::MonitorStatus.to_i8().unwrap()).await?;
|
|
|
|
write_i32(&stream, channel).await?;
|
|
|
|
write_i8(&stream, probe).await?;
|
2022-03-17 20:26:44 +08:00
|
|
|
write_i64(&stream, current).await?;
|
2020-04-25 20:45:25 +08:00
|
|
|
*previous = Some(current);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
for (&(channel, overrd), previous) in inject_watch_list.iter_mut() {
|
2021-10-06 13:05:45 +08:00
|
|
|
let current = dispatch!(timer, _aux_mutex, _routing_table, channel, read_injection_status, overrd);
|
2020-04-25 20:45:25 +08:00
|
|
|
if previous.is_none() || previous.unwrap() != current {
|
|
|
|
write_i8(&stream, DeviceMessage::InjectionStatus.to_i8().unwrap()).await?;
|
|
|
|
write_i32(&stream, channel).await?;
|
|
|
|
write_i8(&stream, overrd).await?;
|
|
|
|
write_i8(&stream, current).await?;
|
|
|
|
*previous = Some(current);
|
|
|
|
}
|
|
|
|
}
|
2020-04-25 20:31:38 +08:00
|
|
|
next_check = next_check + Milliseconds(200);
|
|
|
|
}
|
2020-04-24 15:17:41 +08:00
|
|
|
}
|
2020-04-24 14:37:16 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-10-06 13:05:45 +08:00
|
|
|
pub fn start(timer: GlobalTimer, aux_mutex: Rc<Mutex<bool>>, routing_table: Rc<RefCell<drtio_routing::RoutingTable>>) {
|
2020-04-24 14:37:16 +08:00
|
|
|
task::spawn(async move {
|
|
|
|
loop {
|
2021-10-06 13:05:45 +08:00
|
|
|
let aux_mutex = aux_mutex.clone();
|
|
|
|
let routing_table = routing_table.clone();
|
2020-04-24 14:37:16 +08:00
|
|
|
let stream = TcpStream::accept(1383, 2048, 2048).await.unwrap();
|
2020-04-25 20:31:38 +08:00
|
|
|
task::spawn(async move {
|
2020-07-20 19:07:44 +08:00
|
|
|
info!("received connection");
|
2022-03-02 15:45:17 +08:00
|
|
|
let routing_table = routing_table.borrow();
|
2021-10-06 13:05:45 +08:00
|
|
|
let result = handle_connection(&stream, timer, &aux_mutex, &routing_table).await;
|
2020-07-20 19:07:44 +08:00
|
|
|
match result {
|
2021-05-29 17:13:22 +08:00
|
|
|
Err(Error::NetworkError(smoltcp::Error::Finished)) => info!("peer closed connection"),
|
2020-07-20 19:07:44 +08:00
|
|
|
Err(error) => warn!("connection terminated: {}", error),
|
|
|
|
_ => (),
|
|
|
|
}
|
2020-04-24 14:37:16 +08:00
|
|
|
let _ = stream.flush().await;
|
|
|
|
let _ = stream.abort().await;
|
|
|
|
});
|
|
|
|
}
|
|
|
|
});
|
|
|
|
}
|