use core::cell::Cell;
use embassy_futures::select::{Either, select};
use embassy_sync::blocking_mutex::Mutex as BlockingMutex;
use embassy_sync::blocking_mutex::raw::NoopRawMutex;
use embassy_sync::channel::{Channel, DynamicReceiver, DynamicSender};
use embassy_sync::mutex::Mutex;
use embassy_sync::signal::Signal;
use embassy_time::{Duration, Timer, with_timeout};
use heapless::Vec;
use crate::{Address, Flags, Packet, Transceiver, TrxError};
const PAYLOAD_CAP: usize = 61;
pub const LINK_DOWN_STREAK: u8 = 3;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum LinkState {
#[default]
Up,
Down,
}
#[derive(Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum TxError {
AckTimeout,
DataTooLong,
Trx(TrxError),
}
#[derive(Debug, Clone, Copy)]
pub struct MacTiming {
pub ack_tx_delay: Duration,
pub ack_timeout: Duration,
pub tx_retry_delay: Duration,
pub recover_backoff: Duration,
}
impl MacTiming {
pub const fn defaults() -> Self {
Self {
ack_tx_delay: Duration::from_millis(10),
ack_timeout: Duration::from_millis(50),
tx_retry_delay: Duration::from_millis(200),
recover_backoff: Duration::from_millis(500),
}
}
}
impl Default for MacTiming {
fn default() -> Self {
Self::defaults()
}
}
struct TxRequest {
dst: Address,
flags: Flags,
data: Vec<u8, PAYLOAD_CAP>,
}
pub struct StackResources<const N_RX: usize = 4> {
rx: Channel<NoopRawMutex, Packet, N_RX>,
tx_request: Channel<NoopRawMutex, TxRequest, 1>,
tx_response: Signal<NoopRawMutex, Result<(), TxError>>,
tx_mutex: Mutex<NoopRawMutex, ()>,
link_state: BlockingMutex<NoopRawMutex, Cell<LinkState>>,
link_state_signal: Signal<NoopRawMutex, LinkState>,
}
impl<const N_RX: usize> StackResources<N_RX> {
pub const fn new() -> Self {
Self {
rx: Channel::new(),
tx_request: Channel::new(),
tx_response: Signal::new(),
tx_mutex: Mutex::new(()),
link_state: BlockingMutex::new(Cell::new(LinkState::Up)),
link_state_signal: Signal::new(),
}
}
}
impl<const N_RX: usize> Default for StackResources<N_RX> {
fn default() -> Self {
Self::new()
}
}
#[derive(Copy, Clone)]
pub struct Stack<'a> {
address: Address,
rx: DynamicReceiver<'a, Packet>,
tx_request: DynamicSender<'a, TxRequest>,
tx_response: &'a Signal<NoopRawMutex, Result<(), TxError>>,
tx_mutex: &'a Mutex<NoopRawMutex, ()>,
link_state: &'a BlockingMutex<NoopRawMutex, Cell<LinkState>>,
link_state_signal: &'a Signal<NoopRawMutex, LinkState>,
}
pub struct Runner<'a, TRX> {
trx: TRX,
address: Address,
rx: DynamicSender<'a, Packet>,
tx_request: DynamicReceiver<'a, TxRequest>,
tx_response: &'a Signal<NoopRawMutex, Result<(), TxError>>,
timing: MacTiming,
link_state: &'a BlockingMutex<NoopRawMutex, Cell<LinkState>>,
link_state_signal: &'a Signal<NoopRawMutex, LinkState>,
consecutive_errors: u8,
}
impl<'a> Stack<'a> {
pub fn new<TRX, const N_RX: usize>(
trx: TRX,
address: Address,
resources: &'a mut StackResources<N_RX>,
timing: MacTiming,
) -> (Stack<'a>, Runner<'a, TRX>) {
let StackResources {
rx,
tx_request,
tx_response,
tx_mutex,
link_state,
link_state_signal,
} = resources;
link_state.lock(|c| c.set(LinkState::Up));
link_state_signal.reset();
let stack = Stack {
address,
rx: rx.dyn_receiver(),
tx_request: tx_request.dyn_sender(),
tx_response,
tx_mutex,
link_state,
link_state_signal,
};
let runner = Runner {
trx,
address,
rx: rx.dyn_sender(),
tx_request: tx_request.dyn_receiver(),
tx_response,
timing,
link_state,
link_state_signal,
consecutive_errors: 0,
};
(stack, runner)
}
pub fn address(&self) -> Address {
self.address
}
pub fn link_state(&self) -> LinkState {
self.link_state.lock(|c| c.get())
}
pub fn is_link_up(&self) -> bool {
matches!(self.link_state(), LinkState::Up)
}
pub async fn wait_link_up(&self) {
loop {
if matches!(self.link_state(), LinkState::Up) {
return;
}
let _ = self.link_state_signal.wait().await;
}
}
pub async fn wait_link_down(&self) {
loop {
if matches!(self.link_state(), LinkState::Down) {
return;
}
let _ = self.link_state_signal.wait().await;
}
}
pub async fn send(&self, dst: Address, flags: Flags, data: &[u8]) -> Result<(), TxError> {
let mut buf: Vec<u8, PAYLOAD_CAP> = Vec::new();
buf.extend_from_slice(data).map_err(|_| TxError::DataTooLong)?;
let _guard = self.tx_mutex.lock().await;
self.tx_response.reset();
self.tx_request.send(TxRequest { dst, flags, data: buf }).await;
self.tx_response.wait().await
}
pub async fn recv(&self) -> Packet {
self.rx.receive().await
}
}
impl<'a, TRX: Transceiver> Runner<'a, TRX> {
pub async fn run(&mut self) -> ! {
loop {
if matches!(self.link_state.lock(|c| c.get()), LinkState::Down) {
match self.trx.recover().await {
Ok(()) => {
info!("Stack: recover succeeded");
self.record_ok();
}
Err(e) => {
error!("Stack: recover failed: {:?}", e);
Timer::after(self.timing.recover_backoff).await;
continue;
}
}
}
match select(self.tx_request.receive(), self.trx.recv()).await {
Either::First(req) => self.handle_tx(req).await,
Either::Second(Ok(packet)) => {
self.record_ok();
self.handle_rx(packet).await
}
Either::Second(Err(e)) => {
error!("Stack: rx error: {:?}", e);
self.record_err();
}
}
}
}
async fn handle_tx(&mut self, req: TxRequest) {
let result = self.do_send(req).await;
self.tx_response.signal(result);
}
async fn do_send(&mut self, req: TxRequest) -> Result<(), TxError> {
let packet = Packet::new(self.address, req.dst, req.flags, &req.data)
.map_err(|_| TxError::Trx(TrxError::WrongPacketFormat))?;
match req.flags {
Flags::None | Flags::Ack(0) => {
info!("Stack: send (no ack)");
match self.trx.send(&packet).await {
Ok(()) => {
self.record_ok();
Ok(())
}
Err(e) => {
self.record_err();
Err(TxError::Trx(e))
}
}
}
Flags::Ack(retries) => {
for i in 1..=retries {
info!("Stack: send {} of {} (waiting ACK)", i, retries);
match self.trx.send(&packet).await {
Ok(()) => self.record_ok(),
Err(e) => {
self.record_err();
return Err(TxError::Trx(e));
}
}
match with_timeout(self.timing.ack_timeout, self.wait_for_ack(req.dst)).await {
Ok(Ok(())) => return Ok(()),
Ok(Err(e)) => return Err(TxError::Trx(e)),
Err(_) => Timer::after(self.timing.tx_retry_delay).await,
}
}
Err(TxError::AckTimeout)
}
}
}
async fn wait_for_ack(&mut self, from: Address) -> Result<(), TrxError> {
loop {
let packet = match self.trx.recv().await {
Ok(p) => {
self.record_ok();
p
}
Err(e) => {
self.record_err();
return Err(e);
}
};
if packet.src == from && packet.dst == self.address && packet.is_ack() {
info!("Stack: valid ACK");
return Ok(());
}
self.try_deliver(packet);
}
}
async fn handle_rx(&mut self, packet: Packet) {
if let Flags::Ack(n) = packet.flags
&& n > 0
&& packet.dst == self.address
{
let Ok(ack) = Packet::new(self.address, packet.src, Flags::Ack(0), &[]) else {
return;
};
info!("Stack: replying ACK");
Timer::after(self.timing.ack_tx_delay).await;
match self.trx.send(&ack).await {
Ok(()) => self.record_ok(),
Err(e) => {
error!("Stack: ACK send failed: {:?}", e);
self.record_err();
return;
}
}
}
self.try_deliver(packet);
}
fn record_ok(&mut self) {
self.consecutive_errors = 0;
let was_down = self.link_state.lock(|c| {
let prev = c.get();
c.set(LinkState::Up);
matches!(prev, LinkState::Down)
});
if was_down {
info!("Stack: link up");
self.link_state_signal.signal(LinkState::Up);
}
}
fn record_err(&mut self) {
self.consecutive_errors = self.consecutive_errors.saturating_add(1);
if self.consecutive_errors < LINK_DOWN_STREAK {
return;
}
let was_up = self.link_state.lock(|c| {
let prev = c.get();
c.set(LinkState::Down);
matches!(prev, LinkState::Up)
});
if was_up {
warn!("Stack: link down after {} consecutive errors", LINK_DOWN_STREAK);
self.link_state_signal.signal(LinkState::Down);
}
}
fn try_deliver(&self, packet: Packet) {
if packet.dst != Address::Broadcast && packet.dst != self.address {
return;
}
if self.rx.try_send(packet).is_err() {
warn!("Stack: rx queue full, packet dropped");
}
}
}