144 lines
4.0 KiB
Rust
144 lines
4.0 KiB
Rust
use embassy_net::{
|
|
udp::{UdpSocket, PacketMetadata},
|
|
IpEndpoint,
|
|
Stack
|
|
};
|
|
use embassy_time::{Duration, Instant, Timer};
|
|
use embassy_futures::select::select;
|
|
|
|
use alloc::vec::Vec;
|
|
|
|
use log::{warn,info};
|
|
|
|
use crate::ecu::Ecu;
|
|
use crate::wifi::SessionStore;
|
|
|
|
|
|
#[derive(Debug)]
|
|
enum SubscriptionKind {
|
|
// only subscribable value currently is a raw table, but we expect
|
|
// in future to have interpreted values as well
|
|
Table(u8, usize, usize)
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
struct Subscription {
|
|
kind: SubscriptionKind,
|
|
interval: Duration,
|
|
next_wake: Instant
|
|
}
|
|
impl Subscription {
|
|
fn new(kind: SubscriptionKind, interval: Duration) -> Self {
|
|
Self {
|
|
kind,
|
|
interval,
|
|
next_wake: Instant::now() + interval
|
|
}
|
|
}
|
|
}
|
|
|
|
struct Subscriber {
|
|
subscriptions: Vec<Subscription>,
|
|
endpoint: IpEndpoint,
|
|
until: Instant
|
|
}
|
|
|
|
impl Subscriber {
|
|
fn next_wake(&self) -> Option<Instant> {
|
|
self.subscriptions.iter().map(|s| s.next_wake).min()
|
|
}
|
|
|
|
fn update_subscriptions(&mut self, message: &[u8]) {
|
|
let mut subs = Vec::new();
|
|
for c in message.chunks(6) {
|
|
let interval : u64 = (u64::from(c[0]) << 8) + u64::from(c[1]);
|
|
let sub_type = c[2];
|
|
let table_number = c[3];
|
|
let (start, end) = (c[4].into(), c[5].into());
|
|
if interval > 0 && sub_type == b'T' {
|
|
subs.push(Subscription::new(
|
|
SubscriptionKind::Table(table_number, start, end),
|
|
Duration::from_millis(interval)))
|
|
}
|
|
}
|
|
info!("subs {:?}", subs);
|
|
self.subscriptions = subs;
|
|
}
|
|
}
|
|
|
|
// endpoint and timeout are per-subscriber not per subscription.
|
|
// interval is per subscription: e.g. we want to get the rpm much
|
|
// more often than the ecm id
|
|
|
|
pub async fn loop_udp_thing(stack : Stack<'_>, ecu: &mut Ecu<'_>, sessions: &SessionStore ) {
|
|
let mut rx_buffer = [0u8; 4096];
|
|
let mut tx_buffer = [0u8; 4096];
|
|
let mut rx_meta = [PacketMetadata::EMPTY; 512];
|
|
let mut tx_meta = [PacketMetadata::EMPTY; 512];
|
|
|
|
let mut sock = UdpSocket::new(
|
|
stack,
|
|
&mut rx_meta,
|
|
&mut rx_buffer,
|
|
&mut tx_meta,
|
|
&mut tx_buffer,
|
|
);
|
|
|
|
log::info!("sock.bind 5000 res: {:?}", sock.bind(5000));
|
|
|
|
// TODO we need a map(dictionary) of endpoint -> subscriber
|
|
|
|
let mut subscriber = None;
|
|
|
|
loop {
|
|
let mut buf = [0; 4096];
|
|
if sock.may_recv() {
|
|
info!("packet");
|
|
let res = sock.recv_from(&mut buf).await;
|
|
if let Ok((n, meta)) = res {
|
|
info!("packet from {:?}", meta.endpoint);
|
|
|
|
if sessions.is_registered(&buf[..32]).await {
|
|
let mut s = Subscriber {
|
|
subscriptions: Vec::from([]),
|
|
endpoint: meta.endpoint,
|
|
until: Instant::now() + Duration::from_secs(60),
|
|
};
|
|
|
|
s.update_subscriptions(&buf[32..n]);
|
|
subscriber = Some(s);
|
|
} else {
|
|
warn!("subscription attempt without registered session {:?}",
|
|
&buf[..32]);
|
|
subscriber = None;
|
|
}
|
|
}
|
|
}
|
|
if let Some(ref mut sub) = subscriber {
|
|
let mut buf = [0u8; 1024];
|
|
let now = Instant::now();
|
|
if sub.until < now {
|
|
subscriber = None;
|
|
} else {
|
|
let subn = &mut sub.subscriptions[0];
|
|
if subn.next_wake <= now {
|
|
match subn.kind {
|
|
SubscriptionKind::Table(t, _s, _e) => {
|
|
let tbl = ecu.fetch_table(t, &mut buf).await;
|
|
sock.send_to(tbl, sub.endpoint).await;
|
|
}
|
|
}
|
|
subn.next_wake = now + subn.interval;
|
|
}
|
|
if let Some(later) = sub.next_wake() {
|
|
select(Timer::at(later), sock.wait_recv_ready()).await;
|
|
} else {
|
|
sock.wait_recv_ready().await;
|
|
}
|
|
}
|
|
} else {
|
|
sock.wait_recv_ready().await;
|
|
}
|
|
}
|
|
}
|