use crate::error::{Error, Result};
use crate::transport::async_transport::BoxFuture;
use crate::transport::device::{AsyncTransportDevice, TransportDeviceInfo};
use crate::transport::fdcanusb::{is_retryable_error, FdcanusbOptions, FdcanusbProtocol};
use crate::transport::transaction::{dispatch_frame, Request};
use moteus_protocol::fdcanusb as codec;
use moteus_protocol::CanFdFrame;
use tokio::io::{
AsyncBufReadExt, AsyncRead, AsyncWrite, AsyncWriteExt, BufReader, ReadHalf, WriteHalf,
};
use tokio_serial::SerialStream;
pub struct AsyncFdcanusbDevice<S = SerialStream> {
reader: BufReader<ReadHalf<S>>,
writer: WriteHalf<S>,
timeout: std::time::Duration,
disable_brs: bool,
uart_mode: bool,
checksum_active: bool,
max_retries: u32,
line_buffer: String,
pending_frames: Vec<CanFdFrame>,
pub(crate) info: TransportDeviceInfo,
needs_recovery: bool,
needs_flush: bool,
}
impl<S> std::fmt::Debug for AsyncFdcanusbDevice<S> {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
f.debug_struct("AsyncFdcanusbDevice")
.field("info", &self.info)
.field("timeout", &self.timeout)
.field("disable_brs", &self.disable_brs)
.field("uart_mode", &self.uart_mode)
.field("checksum_active", &self.checksum_active)
.field("pending_frames", &self.pending_frames.len())
.field("needs_recovery", &self.needs_recovery)
.field("needs_flush", &self.needs_flush)
.finish()
}
}
impl AsyncFdcanusbDevice {
pub async fn open(path: &str) -> Result<Self> {
Self::open_with(path, &FdcanusbOptions::new()).await
}
pub async fn open_with_brs(path: &str, disable_brs: bool) -> Result<Self> {
Self::open_with(path, &FdcanusbOptions::new().disable_brs(disable_brs)).await
}
pub async fn open_with_options(
path: &str,
timeout: std::time::Duration,
disable_brs: bool,
) -> Result<Self> {
Self::open_with(
path,
&FdcanusbOptions::new()
.timeout(timeout)
.disable_brs(disable_brs),
)
.await
}
pub async fn open_with(path: &str, options: &FdcanusbOptions) -> Result<Self> {
let builder = tokio_serial::new(path, options.baudrate);
let mut port = SerialStream::open(&builder).map_err(|e| Error::Io(e.into()))?;
{
use tokio_serial::SerialPort;
let _ = port.write_data_terminal_ready(true);
}
let mut device = Self::from_stream_with(port, options);
if options.uart_mode.is_none() && !crate::transport::discovery::is_fdcanusb_path(path) {
device.uart_mode = true;
device.checksum_active = true;
}
Ok(device)
}
}
impl<S: AsyncRead + AsyncWrite + Send> AsyncFdcanusbDevice<S> {
pub fn from_stream_with(stream: S, options: &FdcanusbOptions) -> Self {
let (reader, writer) = tokio::io::split(stream);
let reader = BufReader::new(reader);
Self {
reader,
writer,
timeout: options.timeout,
disable_brs: options.disable_brs,
uart_mode: options.uart_mode.unwrap_or(false),
checksum_active: options.checksum_enabled,
max_retries: options.max_retries,
line_buffer: String::new(),
pending_frames: Vec::new(),
info: TransportDeviceInfo::new(0, "AsyncFdcanusb"),
needs_recovery: false,
needs_flush: false,
}
}
pub fn set_disable_brs(&mut self, disable: bool) {
self.disable_brs = disable;
}
pub fn uart_mode(&self) -> bool {
self.uart_mode
}
pub fn checksum_active(&self) -> bool {
self.checksum_active
}
async fn write_frame(&mut self, frame: &CanFdFrame) -> Result<()> {
let cmd = FdcanusbProtocol::encode_frame_with_options(
frame,
self.disable_brs,
self.checksum_active,
);
self.writer.write_all(cmd.as_bytes()).await?;
Ok(())
}
async fn send_frame(&mut self, frame: &CanFdFrame) -> Result<()> {
let mut timeout = self.timeout;
let mut attempt = 0;
loop {
self.write_frame(frame).await?;
self.writer.flush().await?;
match self.wait_for_ok(timeout).await {
Ok(()) => {
if attempt > 0 {
self.needs_flush = true;
}
return Ok(());
}
Err(e)
if self.uart_mode && attempt < self.max_retries && is_retryable_error(&e) =>
{
attempt += 1;
timeout += timeout / 2;
}
Err(e) => {
self.needs_flush = true;
return Err(e);
}
}
}
}
async fn read_line_deadline(&mut self, deadline: tokio::time::Instant) -> Result<bool> {
loop {
let remaining = deadline.saturating_duration_since(tokio::time::Instant::now());
if remaining.is_zero() {
return Ok(false);
}
let available = match tokio::time::timeout(remaining, self.reader.fill_buf()).await {
Err(_) => return Ok(false), Ok(Err(e)) => return Err(Error::Io(e)),
Ok(Ok([])) => {
return Err(Error::Io(std::io::Error::new(
std::io::ErrorKind::UnexpectedEof,
"Serial port closed",
)));
}
Ok(Ok(available)) => available,
};
let (take, complete) = match available.iter().position(|&b| b == b'\n') {
Some(pos) => (pos + 1, true),
None => (available.len(), false),
};
self.line_buffer
.push_str(&String::from_utf8_lossy(&available[..take]));
self.reader.consume(take);
if complete {
return Ok(true);
}
}
}
fn apply_checksum_policy(&mut self) -> bool {
let (content_len, had_checksum, valid) = {
let trimmed = self.line_buffer.trim_end();
let (content, had_checksum, valid) = codec::strip_checksum(trimmed.as_bytes());
(content.len(), had_checksum, valid)
};
if had_checksum {
if !valid {
return false;
}
self.line_buffer.truncate(content_len);
true
} else {
!self.checksum_active
}
}
async fn wait_for_ok(&mut self, timeout: std::time::Duration) -> Result<()> {
let deadline = tokio::time::Instant::now() + timeout;
loop {
if !self.read_line_deadline(deadline).await? {
return Err(Error::Timeout);
}
if !self.apply_checksum_policy() {
self.line_buffer.clear();
continue;
}
let line = self.line_buffer.trim();
if FdcanusbProtocol::is_ok_response(line) {
self.line_buffer.clear();
return Ok(());
}
if FdcanusbProtocol::is_error_response(line) {
if FdcanusbProtocol::is_checksum_error(line) {
self.checksum_active = true;
}
let error = Error::Device {
message: line.to_string(),
retryable: FdcanusbProtocol::is_retryable_error_response(line),
};
self.line_buffer.clear();
return Err(error);
}
if let Some(frame) = FdcanusbProtocol::parse_frame(&self.line_buffer) {
self.pending_frames.push(frame);
}
self.line_buffer.clear();
}
}
async fn receive_frames(&mut self, expected_count: usize) -> Result<Vec<CanFdFrame>> {
let mut frames: Vec<CanFdFrame> = self.pending_frames.drain(..).collect();
if frames.len() >= expected_count {
return Ok(frames);
}
let deadline = tokio::time::Instant::now() + self.timeout;
while frames.len() < expected_count {
if !self.read_line_deadline(deadline).await? {
self.needs_flush = true;
break;
}
if !self.apply_checksum_policy() {
self.line_buffer.clear();
continue;
}
if let Some(frame) = FdcanusbProtocol::parse_frame(&self.line_buffer) {
frames.push(frame);
}
self.line_buffer.clear();
}
Ok(frames)
}
async fn receive_replies_uart(
&mut self,
requests: &mut [Request],
target: usize,
) -> Result<()> {
for frame in std::mem::take(&mut self.pending_frames) {
dispatch_frame(&frame, requests);
}
let expected = requests[target].expected_reply_count as usize;
let deadline = tokio::time::Instant::now() + self.timeout;
while requests[target].responses.len() < expected {
if !self.read_line_deadline(deadline).await? {
self.needs_flush = true;
break;
}
if !self.apply_checksum_policy() {
self.line_buffer.clear();
continue;
}
if let Some(frame) = FdcanusbProtocol::parse_frame(&self.line_buffer) {
dispatch_frame(&frame, requests);
}
self.line_buffer.clear();
}
Ok(())
}
async fn execute_cycle_pipelined(&mut self, requests: &mut [Request]) -> Result<()> {
let mut frames_sent = 0usize;
for req in requests.iter() {
if let Some(frame) = &req.frame {
self.write_frame(frame).await?;
frames_sent += 1;
}
}
if frames_sent > 0 {
self.writer.flush().await?;
}
for _ in 0..frames_sent {
self.wait_for_ok(self.timeout).await?;
}
let expected: usize = Request::total_expected_replies(requests);
if expected > 0 {
let responses = self.receive_frames(expected).await?;
for frame in responses {
dispatch_frame(&frame, requests);
}
}
Ok(())
}
async fn execute_cycle_uart(&mut self, requests: &mut [Request]) -> Result<()> {
for i in 0..requests.len() {
let Some(frame) = requests[i].frame.clone() else {
continue;
};
self.send_frame(&frame).await?;
if requests[i].expected_reply_count > 0 {
self.receive_replies_uart(requests, i).await?;
}
}
Ok(())
}
async fn execute_cycle(&mut self, requests: &mut [Request]) -> Result<()> {
debug_assert!(
requests.iter().all(|r| r.child_device.is_none()),
"AsyncFdcanusbDevice does not support child devices"
);
self.recover_impl().await?;
self.needs_recovery = true;
let result = if self.uart_mode {
self.execute_cycle_uart(requests).await
} else {
self.execute_cycle_pipelined(requests).await
};
match &result {
Ok(()) => self.needs_recovery = false,
Err(_) => self.needs_flush = true,
}
result
}
async fn recover_impl(&mut self) -> Result<()> {
if !self.needs_recovery && !self.needs_flush {
return Ok(());
}
self.pending_frames.clear();
if self.needs_recovery {
let _ = self.writer.write_all(b"\n").await;
let _ = self.writer.flush().await;
}
let drain_timeout = std::time::Duration::from_millis(20);
let deadline = tokio::time::Instant::now() + drain_timeout;
while let Ok(true) = self.read_line_deadline(deadline).await {
self.line_buffer.clear();
}
self.line_buffer.clear();
self.needs_recovery = false;
self.needs_flush = false;
Ok(())
}
}
impl<S: AsyncRead + AsyncWrite + Send> AsyncTransportDevice for AsyncFdcanusbDevice<S> {
fn recover(&mut self) -> BoxFuture<'_, Result<()>> {
Box::pin(self.recover_impl())
}
fn transaction<'a>(&'a mut self, requests: &'a mut [Request]) -> BoxFuture<'a, Result<()>> {
Box::pin(self.execute_cycle(requests))
}
fn write<'a>(&'a mut self, frame: &'a CanFdFrame) -> BoxFuture<'a, Result<()>> {
Box::pin(async move {
self.recover_impl().await?;
self.send_frame(frame).await
})
}
fn read(&mut self) -> BoxFuture<'_, Result<Option<CanFdFrame>>> {
Box::pin(async move {
if let Some(frame) = self.pending_frames.pop() {
return Ok(Some(frame));
}
loop {
let available = self.reader.fill_buf().await?;
if available.is_empty() {
return Err(Error::Io(std::io::Error::new(
std::io::ErrorKind::UnexpectedEof,
"Serial port closed",
)));
}
let (take, complete) = match available.iter().position(|&b| b == b'\n') {
Some(pos) => (pos + 1, true),
None => (available.len(), false),
};
self.line_buffer
.push_str(&String::from_utf8_lossy(&available[..take]));
self.reader.consume(take);
if !complete {
continue;
}
if !self.apply_checksum_policy() {
self.line_buffer.clear();
continue;
}
let frame = FdcanusbProtocol::parse_frame(&self.line_buffer);
self.line_buffer.clear();
if frame.is_some() {
return Ok(frame);
}
}
})
}
fn flush(&mut self) -> BoxFuture<'_, Result<()>> {
Box::pin(async move {
self.pending_frames.clear();
let flush_timeout = std::time::Duration::from_millis(50);
let deadline = tokio::time::Instant::now() + flush_timeout;
while let Ok(true) = self.read_line_deadline(deadline).await {
self.line_buffer.clear(); }
self.line_buffer.clear();
self.needs_flush = false;
Ok(())
})
}
fn info(&self) -> &TransportDeviceInfo {
&self.info
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::transport::transaction::FrameFilter;
#[test]
fn test_protocol_reuse() {
let mut frame = CanFdFrame::new();
frame.arbitration_id = 0x8001;
frame.data[0..3].copy_from_slice(&[0x01, 0x00, 0x0A]);
frame.size = 3;
frame.set_brs(true);
frame.set_fdcan(true);
let encoded = FdcanusbProtocol::encode_frame(&frame, false);
assert!(encoded.starts_with("can send 8001"));
}
fn test_frame() -> CanFdFrame {
let mut frame = CanFdFrame::new();
frame.arbitration_id = 0x0105;
frame.data[..4].copy_from_slice(b"0123");
frame.size = 4;
frame
}
fn checksummed(content: &str) -> String {
let with_space = format!("{} ", content);
format!(
"{}*{:02X}\n",
with_space,
codec::compute_crc8(with_space.as_bytes())
)
}
fn uart_options() -> FdcanusbOptions {
FdcanusbOptions::new()
.uart_mode(true)
.timeout(std::time::Duration::from_millis(20))
}
async fn read_command(device_side: &mut tokio::io::DuplexStream) -> String {
use tokio::io::AsyncReadExt;
let mut line = Vec::new();
let mut byte = [0u8; 1];
loop {
device_side.read_exact(&mut byte).await.unwrap();
if byte[0] == b'\n' {
break;
}
line.push(byte[0]);
}
String::from_utf8(line).unwrap()
}
#[tokio::test]
async fn test_uart_checksum_send_and_receive() {
use tokio::io::AsyncWriteExt;
let (host_side, mut device_side) = tokio::io::duplex(1024);
let mut device = AsyncFdcanusbDevice::from_stream_with(
host_side,
&uart_options().checksum_enabled(true),
);
let server = tokio::spawn(async move {
let command = read_command(&mut device_side).await;
let (content, had, valid) = codec::strip_checksum(command.as_bytes());
assert!(had, "no checksum on: {}", command);
assert!(valid);
assert!(content.starts_with(b"can send 0105 30313233"));
let mut response = checksummed("OK");
response.push_str(&checksummed("rcv 0105 2030"));
device_side.write_all(response.as_bytes()).await.unwrap();
device_side
});
let mut requests = [Request::new(test_frame())
.with_filter(FrameFilter::Any)
.with_expected_replies(1)];
device.transaction(&mut requests).await.unwrap();
let responses = requests[0].responses.take();
assert_eq!(responses.len(), 1);
assert_eq!(responses[0].payload(), &[0x20, 0x30]);
server.await.unwrap();
}
#[tokio::test]
async fn test_uart_retry_on_timeout() {
use tokio::io::AsyncWriteExt;
let (host_side, mut device_side) = tokio::io::duplex(1024);
let mut device = AsyncFdcanusbDevice::from_stream_with(host_side, &uart_options());
let server = tokio::spawn(async move {
let _ = read_command(&mut device_side).await;
let _ = read_command(&mut device_side).await;
device_side.write_all(b"OK\n").await.unwrap();
device_side
});
device.write(&test_frame()).await.unwrap();
server.await.unwrap();
}
#[tokio::test]
async fn test_uart_err_checksum_enables_checksums() {
use tokio::io::AsyncWriteExt;
let (host_side, mut device_side) = tokio::io::duplex(1024);
let mut device = AsyncFdcanusbDevice::from_stream_with(host_side, &uart_options());
assert!(!device.checksum_active());
let server = tokio::spawn(async move {
let first = read_command(&mut device_side).await;
assert!(!first.contains('*'), "first send unchecksummed");
device_side
.write_all(b"ERR missing checksum\n")
.await
.unwrap();
let second = read_command(&mut device_side).await;
let (_, had, valid) = codec::strip_checksum(second.as_bytes());
assert!(had && valid, "retry must carry a checksum");
device_side
.write_all(checksummed("OK").as_bytes())
.await
.unwrap();
device_side
});
device.write(&test_frame()).await.unwrap();
assert!(device.checksum_active());
server.await.unwrap();
}
#[tokio::test]
async fn test_uart_retries_exhausted() {
let (host_side, _device_side) = tokio::io::duplex(1024);
let mut device =
AsyncFdcanusbDevice::from_stream_with(host_side, &uart_options().max_retries(2));
assert!(matches!(
device.write(&test_frame()).await,
Err(Error::Timeout)
));
}
#[tokio::test]
async fn test_partial_line_survives_deadline() {
use tokio::io::AsyncWriteExt;
let (host_side, mut device_side) = tokio::io::duplex(1024);
let mut device = AsyncFdcanusbDevice::from_stream_with(host_side, &uart_options());
let server = tokio::spawn(async move {
let _ = read_command(&mut device_side).await;
device_side.write_all(b"O").await.unwrap();
let _ = read_command(&mut device_side).await;
tokio::time::sleep(std::time::Duration::from_millis(5)).await;
device_side.write_all(b"K\n").await.unwrap();
tokio::time::sleep(std::time::Duration::from_millis(200)).await;
device_side
});
device.write(&test_frame()).await.unwrap();
server.await.unwrap();
}
}