streamer.rs (4118B)
1 use embassy_net::{ 2 udp::{UdpSocket, PacketMetadata}, 3 IpEndpoint, 4 Stack 5 }; 6 use embassy_time::{Duration, Instant, Timer}; 7 use embassy_futures::select::select; 8 9 use alloc::vec::Vec; 10 11 use log::{warn,info}; 12 13 use crate::ecu::Ecu; 14 use crate::wifi::SessionStore; 15 16 17 #[derive(Debug)] 18 enum SubscriptionKind { 19 // only subscribable value currently is a raw table, but we expect 20 // in future to have interpreted values as well 21 Table(u8, usize, usize) 22 } 23 24 #[derive(Debug)] 25 struct Subscription { 26 kind: SubscriptionKind, 27 interval: Duration, 28 next_wake: Instant 29 } 30 impl Subscription { 31 fn new(kind: SubscriptionKind, interval: Duration) -> Self { 32 Self { 33 kind, 34 interval, 35 next_wake: Instant::now() + interval 36 } 37 } 38 } 39 40 struct Subscriber { 41 subscriptions: Vec<Subscription>, 42 endpoint: IpEndpoint, 43 until: Instant 44 } 45 46 impl Subscriber { 47 fn next_wake(&self) -> Option<Instant> { 48 self.subscriptions.iter().map(|s| s.next_wake).min() 49 } 50 51 fn update_subscriptions(&mut self, message: &[u8]) { 52 let mut subs = Vec::new(); 53 for c in message.chunks(6) { 54 let interval : u64 = (u64::from(c[0]) << 8) + u64::from(c[1]); 55 let sub_type = c[2]; 56 let table_number = c[3]; 57 let (start, end) = (c[4].into(), c[5].into()); 58 if interval > 0 && sub_type == b'T' { 59 subs.push(Subscription::new( 60 SubscriptionKind::Table(table_number, start, end), 61 Duration::from_millis(interval))) 62 } 63 } 64 info!("subs {:?}", subs); 65 self.subscriptions = subs; 66 } 67 } 68 69 // endpoint and timeout are per-subscriber not per subscription. 70 // interval is per subscription: e.g. we want to get the rpm much 71 // more often than the ecm id 72 73 pub async fn loop_udp_thing(stack : Stack<'_>, ecu: &mut Ecu<'_>, sessions: &SessionStore ) { 74 let mut rx_buffer = [0u8; 4096]; 75 let mut tx_buffer = [0u8; 4096]; 76 let mut rx_meta = [PacketMetadata::EMPTY; 512]; 77 let mut tx_meta = [PacketMetadata::EMPTY; 512]; 78 79 let mut sock = UdpSocket::new( 80 stack, 81 &mut rx_meta, 82 &mut rx_buffer, 83 &mut tx_meta, 84 &mut tx_buffer, 85 ); 86 87 log::info!("sock.bind 5000 res: {:?}", sock.bind(5000)); 88 89 // TODO we need a map(dictionary) of endpoint -> subscriber 90 91 let mut subscriber = None; 92 93 loop { 94 let mut buf = [0; 4096]; 95 if sock.may_recv() { 96 info!("packet"); 97 let res = sock.recv_from(&mut buf).await; 98 if let Ok((n, meta)) = res { 99 info!("packet from {:?}", meta.endpoint); 100 101 if sessions.is_registered(&buf[..32]).await { 102 let mut s = Subscriber { 103 subscriptions: Vec::from([]), 104 endpoint: meta.endpoint, 105 until: Instant::now() + Duration::from_secs(60), 106 }; 107 108 s.update_subscriptions(&buf[32..n]); 109 subscriber = Some(s); 110 } else { 111 warn!("subscription attempt without registered session {:?}", 112 &buf[..32]); 113 subscriber = None; 114 } 115 } 116 } 117 if let Some(ref mut sub) = subscriber { 118 let mut buf = [0u8; 1024]; 119 let now = Instant::now(); 120 if sub.until < now { 121 subscriber = None; 122 } else { 123 let subn = &mut sub.subscriptions[0]; 124 if subn.next_wake <= now { 125 match subn.kind { 126 SubscriptionKind::Table(t, _s, _e) => { 127 let tbl = ecu.fetch_table(t, &mut buf).await; 128 sock.send_to(tbl, sub.endpoint).await; 129 } 130 } 131 subn.next_wake = now + subn.interval; 132 } 133 if let Some(later) = sub.next_wake() { 134 select(Timer::at(later), sock.wait_recv_ready()).await; 135 } else { 136 sock.wait_recv_ready().await; 137 } 138 } 139 } else { 140 sock.wait_recv_ready().await; 141 } 142 } 143 }