1#![cfg_attr(not(test), no_std)]
19
20mod fmt;
21mod frame;
22
23use core::cell::Cell;
24use core::convert::Infallible;
25use core::future::{poll_fn, Future};
26use core::mem::MaybeUninit;
27use core::pin::Pin;
28use core::task::Poll;
29
30use embassy_futures::select::{select, select_slice, Either};
31use embassy_sync::blocking_mutex::raw::NoopRawMutex;
32use embassy_sync::mutex::Mutex;
33use embassy_sync::pipe::{Pipe, Reader, Writer};
34use embassy_sync::signal::Signal;
35use embassy_sync::waitqueue::AtomicWaker;
36pub use frame::{Break, Control, FrameError};
37use frame::{Frame, Information, MultiplexerCloseDown};
38use heapless::Vec;
39
40use bbqueue::prod_cons::framed::{FramedConsumer, FramedGrantR, FramedProducer};
41use bbqueue::traits::coordination::cs::CsCoord;
42use bbqueue::traits::notifier::maitake::MaiNotSpsc;
43use bbqueue::traits::storage::Inline;
44use bbqueue::BBQueue;
45
46use crate::frame::{FrameType, NonSupportedCommandResponse};
47
48type RxQueue<const BUF: usize> = BBQueue<Inline<BUF>, CsCoord, MaiNotSpsc>;
50
51struct Lines {
52 rx: Cell<(Control, Option<Break>)>,
53 tx: Cell<(Control, Option<Break>)>,
54 opened: Cell<bool>,
55 pending_command: Cell<Option<FrameType>>,
56 hangup: Cell<bool>,
57 hangup_mask: Cell<Option<(u16, u16)>>,
58 hangup_waker: AtomicWaker,
59}
60
61impl Lines {
62 const fn new() -> Self {
63 Self {
64 rx: Cell::new((Control::new(), None)),
65 tx: Cell::new((Control::new(), None)),
66 opened: Cell::new(false),
67 pending_command: Cell::new(None),
68 hangup: Cell::new(false),
69 hangup_mask: Cell::new(None),
70 hangup_waker: AtomicWaker::new(),
71 }
72 }
73
74 fn check_hangup(&self) {
75 if !self.rx.get().0.dv() && !self.hangup.get() {
76 let (control, _brk) = self.rx.get();
77 warn!(
78 "HANGUP detected! Control: rtc={} rtr={} ic={} dv={}, mask={:?}",
79 control.rtc(),
80 control.rtr(),
81 control.ic(),
82 control.dv(),
83 self.hangup_mask.get()
84 );
85 }
86 }
87}
88
89pub struct Mux<const N: usize, const BUF: usize> {
95 tx: [Pipe<NoopRawMutex, BUF>; N],
96 rx: [RxQueue<BUF>; N],
97 lines: [Lines; N],
98 line_status_updated: Signal<NoopRawMutex, ()>,
99}
100
101pub struct Channel<'a, const BUF: usize> {
103 rx: ChannelRx<'a, BUF>,
104 tx: ChannelTx<'a, BUF>,
105 lines: ChannelLines<'a, BUF>,
106}
107
108pub struct ChannelRx<'a, const BUF: usize> {
110 rx: FramedBufReader<'a, BUF>,
111 lines: &'a Lines,
112 line_status_updated: &'a Signal<NoopRawMutex, ()>,
113}
114
115struct FramedBufReader<'a, const BUF: usize> {
122 consumer: FramedConsumer<&'a RxQueue<BUF>, u16>,
123 current_grant: Option<FramedGrantR<&'a RxQueue<BUF>, u16>>,
124 offset: usize,
125}
126
127impl<'a, const BUF: usize> FramedBufReader<'a, BUF> {
128 fn new(consumer: FramedConsumer<&'a RxQueue<BUF>, u16>) -> Self {
129 Self {
130 consumer,
131 current_grant: None,
132 offset: 0,
133 }
134 }
135
136 async fn fill_buf(&mut self) -> &[u8] {
137 if self.current_grant.is_none() {
138 self.current_grant = Some(self.consumer.wait_read().await);
139 self.offset = 0;
140 }
141 &self.current_grant.as_ref().unwrap()[self.offset..]
142 }
143
144 fn consume(&mut self, amt: usize) {
145 self.offset += amt;
146 if let Some(ref grant) = self.current_grant {
147 if self.offset >= grant.len() {
148 self.current_grant.take().unwrap().release();
149 self.offset = 0;
150 }
151 }
152 }
153
154 async fn read(&mut self, buf: &mut [u8]) -> usize {
155 let data = self.fill_buf().await;
156 let n = data.len().min(buf.len());
157 buf[..n].copy_from_slice(&data[..n]);
158 self.consume(n);
159 n
160 }
161}
162
163pub struct ChannelTx<'a, const BUF: usize> {
165 tx: Writer<'a, NoopRawMutex, BUF>,
166 lines: &'a Lines,
167 line_status_updated: &'a Signal<NoopRawMutex, ()>,
168}
169
170#[derive(Clone, Copy)]
172pub struct ChannelLines<'a, const BUF: usize> {
173 lines: &'a Lines,
174 line_status_updated: &'a Signal<NoopRawMutex, ()>,
175}
176
177pub struct Runner<'a, const N: usize, const BUF: usize> {
181 tx: [Reader<'a, NoopRawMutex, BUF>; N],
182 rx: [FramedProducer<&'a RxQueue<BUF>, u16>; N],
183 control_channel_opened: Cell<bool>,
184 control_pending_command: Cell<Option<FrameType>>,
185 lines: &'a [Lines; N],
186 _line_status_updated: &'a Signal<NoopRawMutex, ()>,
187}
188
189impl<const N: usize, const BUF: usize> Mux<N, BUF> {
190 #[allow(clippy::declare_interior_mutable_const)]
191 const ONE_PIPE: Pipe<NoopRawMutex, BUF> = Pipe::new();
192 #[allow(clippy::declare_interior_mutable_const)]
193 const ONE_RX_QUEUE: RxQueue<BUF> = BBQueue::new();
194
195 #[allow(clippy::borrow_interior_mutable_const)]
197 pub const fn new() -> Self {
198 #[allow(clippy::declare_interior_mutable_const)]
199 const LINE: Lines = Lines::new();
200
201 Self {
202 rx: [Self::ONE_RX_QUEUE; N],
203 tx: [Self::ONE_PIPE; N],
204 lines: [LINE; N],
205 line_status_updated: Signal::new(),
206 }
207 }
208
209 pub fn start(&mut self) -> (Runner<'_, N, BUF>, [Channel<'_, BUF>; N]) {
214 let mut chs = MaybeUninitArray::<_, N>::uninit();
215 let mut runner_tx = MaybeUninitArray::<_, N>::uninit();
216 let mut runner_rx = MaybeUninitArray::<_, N>::uninit();
217
218 for (i, (tx, rx)) in self.tx.iter_mut().zip(self.rx.iter()).enumerate() {
219 let (tx_r, tx_w) = tx.split();
220 let lines = &self.lines[i];
221 let ch = Channel {
222 rx: ChannelRx {
223 rx: FramedBufReader::new(rx.framed_consumer()),
224 lines,
225 line_status_updated: &self.line_status_updated,
226 },
227 tx: ChannelTx {
228 tx: tx_w,
229 lines,
230 line_status_updated: &self.line_status_updated,
231 },
232 lines: ChannelLines {
233 lines,
234 line_status_updated: &self.line_status_updated,
235 },
236 };
237 unsafe {
238 chs.set(i, ch);
239 runner_rx.set(i, rx.framed_producer());
240 runner_tx.set(i, tx_r);
241 }
242 }
243 let runner = Runner {
244 rx: unsafe { runner_rx.assume_init() },
245 tx: unsafe { runner_tx.assume_init() },
246 control_channel_opened: Cell::new(false),
247 control_pending_command: Cell::new(None),
248 lines: &self.lines,
249 _line_status_updated: &self.line_status_updated,
250 };
251 (runner, unsafe { chs.assume_init() })
252 }
253}
254
255impl<const N: usize, const BUF: usize> Default for Mux<N, BUF> {
256 fn default() -> Self {
257 Self::new()
258 }
259}
260
261impl<'a, const N: usize, const BUF: usize> Runner<'a, N, BUF> {
262 pub async fn run<R: embedded_io_async::BufRead, W: embedded_io_async::Write>(
271 &mut self,
272 mut port_r: R,
273 port_w: W,
274 max_frame_size: usize,
275 ) -> Result<Infallible, FrameError> {
276 let control_opened = &self.control_channel_opened;
278 let control_pending = &self.control_pending_command;
279 let lines = self.lines;
280 let drop_mux = OnDrop::new(|| {
281 control_opened.set(false);
282 control_pending.set(None);
283
284 for line in lines.iter() {
285 line.opened.set(false);
286 line.pending_command.set(None);
287 line.hangup.set(false);
288 }
289 });
290
291 let port_w = Mutex::<NoopRawMutex, _>::new(port_w);
292
293 if !self.control_channel_opened.get() {
295 let mut w = port_w.lock().await;
296 if let Err(e) = (frame::Sabm { id: 0 }).write(&mut *w).await {
297 error!("Failed to send SABM for control channel: {:?}", e);
298 return Err(e);
299 }
300 self.control_pending_command.set(Some(FrameType::Sabm));
301 }
302
303 for channel_id in 0..N {
304 if !self.lines[channel_id].opened.get() {
305 let mut w = port_w.lock().await;
306 if let Err(e) = (frame::Sabm {
307 id: channel_id as u8 + 1,
308 })
309 .write(&mut *w)
310 .await
311 {
312 error!(
313 "Failed to send SABM for channel {}: {:?}",
314 channel_id + 1,
315 e
316 );
317 return Err(e);
318 }
319 self.lines[channel_id]
320 .pending_command
321 .set(Some(FrameType::Sabm));
322 }
323 }
324
325 let fc_changed: Signal<NoopRawMutex, ()> = Signal::new();
327
328 let tx_readers = &mut self.tx;
331 let rx_writers = &mut self.rx;
332 let lines = self.lines;
333 let control_opened = &self.control_channel_opened;
334 let control_pending = &self.control_pending_command;
335
336 let tx_fut = tx_loop::<N, BUF, W>(tx_readers, lines, &port_w, &fc_changed, max_frame_size);
337 let rx_fut = rx_loop::<N, BUF, R, W>(
338 &mut port_r,
339 rx_writers,
340 lines,
341 control_opened,
342 control_pending,
343 &port_w,
344 &fc_changed,
345 );
346
347 let result = match select(tx_fut, rx_fut).await {
349 Either::First(r) => r,
350 Either::Second(r) => r,
351 };
352
353 {
355 let mut w = port_w.lock().await;
356 for id in (0..N).rev() {
357 let dlci = id + 1;
358 let line = &self.lines[id];
359 if line.opened.get() {
360 if let Err(e) = (frame::Disc { id: dlci as u8 }).write(&mut *w).await {
361 error!("Failed to send DISC for channel {}: {:?}", dlci, e);
362 }
363 }
364 }
365
366 if self.control_channel_opened.get() {
367 if let Err(e) = (frame::Uih {
368 id: 0,
369 information: Information::MultiplexerCloseDown(MultiplexerCloseDown {
370 cr: frame::CR::Command,
371 }),
372 })
373 .write(&mut *w)
374 .await
375 {
376 error!("Failed to send MultiplexerCloseDown: {:?}", e);
377 }
378 }
379 }
380
381 drop_mux.defuse();
382 result
383 }
384}
385
386async fn tx_loop<const N: usize, const BUF: usize, W: embedded_io_async::Write>(
387 tx: &mut [Reader<'_, NoopRawMutex, BUF>; N],
388 lines: &[Lines; N],
389 port_w: &Mutex<NoopRawMutex, W>,
390 fc_changed: &Signal<NoopRawMutex, ()>,
391 max_frame_size: usize,
392) -> Result<Infallible, FrameError> {
393 loop {
394 let mut futs: Vec<_, N> = Vec::new();
396 let mut idx_map: Vec<usize, N> = Vec::new();
397
398 for (i, c) in tx.iter_mut().enumerate() {
399 if lines[i].opened.get() && !lines[i].rx.get().0.fc() {
400 let _ = futs.push(c.fill_buf());
401 let _ = idx_map.push(i);
402 }
403 }
404
405 if futs.is_empty() {
406 fc_changed.reset();
408 select(fc_changed.wait(), embassy_futures::yield_now()).await;
409 continue;
410 }
411
412 let (buf, j) = select_slice(Pin::new(futs.as_mut_slice())).await;
413 let i = idx_map[j];
414 let len = buf.len().min(max_frame_size);
415
416 {
418 let mut w = port_w.lock().await;
419 let frame = frame::Uih {
420 id: i as u8 + 1,
421 information: Information::Data(&buf[..len]),
422 };
423
424 if let Err(e) = frame.write(&mut *w).await {
425 error!(
426 "CMUX Runner: CH{} failed to write UIH frame: {:?}",
427 i + 1,
428 e
429 );
430 return Err(e);
431 }
432 trace!("CMUX TX: CH{} {} bytes", i + 1, len);
433 }
434
435 drop(futs);
437 tx[i].consume(len);
438 }
439}
440
441async fn send_msc_fc<W: embedded_io_async::Write>(
443 port_w: &Mutex<NoopRawMutex, W>,
444 channel_id: usize,
445 fc_on: bool,
446) -> Result<(), FrameError> {
447 let control = frame::Control::new()
448 .with_fc(fc_on)
449 .with_rtc(true)
450 .with_rtr(!fc_on)
451 .with_dv(true);
452
453 let msc = frame::Uih {
454 id: 0,
455 information: Information::ModemStatusCommand(frame::ModemStatusCommand {
456 cr: frame::CR::Command,
457 dlci: (channel_id + 1) as u8,
458 control,
459 brk: None,
460 }),
461 };
462
463 let mut w = port_w.lock().await;
464 msc.write(&mut *w).await
465}
466
467async fn rx_loop<
468 const N: usize,
469 const BUF: usize,
470 R: embedded_io_async::BufRead,
471 W: embedded_io_async::Write,
472>(
473 port_r: &mut R,
474 rx: &mut [FramedProducer<&'_ RxQueue<BUF>, u16>; N],
475 lines: &[Lines; N],
476 control_channel_opened: &Cell<bool>,
477 control_pending_command: &Cell<Option<FrameType>>,
478 port_w: &Mutex<NoopRawMutex, W>,
479 fc_changed: &Signal<NoopRawMutex, ()>,
480) -> Result<Infallible, FrameError> {
481 let mut consecutive_errors = 0u32;
482 const MAX_CONSECUTIVE_ERRORS: u32 = 10;
483
484 loop {
485 let header_result = frame::RxHeader::read(port_r).await;
487
488 match header_result {
489 Err(e) => {
490 consecutive_errors += 1;
491 error!(
492 "CMUX Runner: RX error: {:?}. Consecutive errors: {}/{}",
493 e, consecutive_errors, MAX_CONSECUTIVE_ERRORS
494 );
495
496 if consecutive_errors >= MAX_CONSECUTIVE_ERRORS {
497 error!("Too many consecutive frame errors! Stream is likely corrupted. Terminating CMUX.");
498 return Err(e);
499 }
500 continue;
501 }
502 Ok(mut header) => {
503 consecutive_errors = 0;
504
505 match header.frame_type {
506 FrameType::Ui | FrameType::Uih if header.is_control() => {
507 let info = match header.read_information().await {
508 Ok(info) => info,
509 Err(e) => {
510 consecutive_errors += 1;
511 error!("Failed to read information from control frame: {:?}. Consecutive errors: {}/{}",
512 e, consecutive_errors, MAX_CONSECUTIVE_ERRORS);
513
514 if consecutive_errors >= MAX_CONSECUTIVE_ERRORS {
515 error!("Too many consecutive frame errors! Terminating CMUX.");
516 return Err(e);
517 }
518
519 if let Err(fin_err) = header.finalize().await {
520 error!(
521 "Failed to finalize after info read error: {:?}",
522 fin_err
523 );
524 }
525 continue;
526 }
527 };
528
529 if info.is_command() {
530 let mut supported = true;
531
532 match &info {
533 Information::MultiplexerCloseDown(_cld) => {
534 {
536 let mut w = port_w.lock().await;
537 let _ = info.send_ack(&mut *w).await;
538 }
539 if let Err(e) = header.finalize().await {
540 error!("Failed to finalize CLD frame: {:?}", e);
541 }
542 return Err(FrameError::MultiplexerCloseDown);
543 }
544 Information::TestCommand
545 | Information::PowerSavingControl
546 | Information::RemotePortNegotiationCommand
547 | Information::ServiceNegotiationCommand => {
548 supported = false;
549 }
550 Information::ModemStatusCommand(msc) => {
551 let channel_idx = msc.dlci as usize - 1;
552 if channel_idx >= N {
553 warn!(
554 "Modem Status Command for invalid DLCI {}: channel index {} out of bounds (max {})",
555 msc.dlci, channel_idx, N - 1
556 );
557 {
558 let mut w = port_w.lock().await;
559 if let Err(e) = info.send_ack(&mut *w).await {
560 error!("Failed to send ACK for out-of-bounds MSC: {:?}", e);
561 return Err(e);
562 }
563 }
564 if let Err(e) = header.finalize().await {
565 error!("Failed to finalize: {:?}", e);
566 }
567 continue;
568 }
569
570 let lines = &lines[channel_idx];
571 let new_control = msc.control.with_ea(false);
572 let new_brk = msc.brk.map(|b| b.with_ea(false));
573
574 let old_fc = lines.rx.get().0.fc();
575 let new_fc = new_control.fc();
576 if old_fc != new_fc {
577 if new_fc {
578 warn!("Channel {} flow control ENABLED (modem telling us to STOP sending)", msc.dlci);
579 } else {
580 info!("Channel {} flow control DISABLED (modem ready to receive)", msc.dlci);
581 }
582 }
583
584 let old_dv = lines.rx.get().0.dv();
585 let new_dv = new_control.dv();
586
587 if old_dv && !new_dv && consecutive_errors > 0 {
588 error!("Channel {} data became INVALID (dv: true -> false) after recent errors!", msc.dlci);
589 error!(
590 "Channel likely corrupted. CMUX should be restarted!"
591 );
592 lines.opened.set(false);
593 return Err(FrameError::MalformedFrame);
594 }
595
596 lines.rx.set((new_control, new_brk));
597
598 if old_fc != new_fc {
600 fc_changed.signal(());
601 }
602 }
603 n => {
604 warn!("Unknown command {:?} for the control channel", n);
605
606 {
607 let mut w = port_w.lock().await;
608 if let Err(e) = (frame::Uih {
609 id: 0,
610 information: Information::NonSupportedCommandResponse(
611 NonSupportedCommandResponse {
612 cr: frame::CR::Response,
613 command_type: n.info_type(),
614 },
615 ),
616 })
617 .write(&mut *w)
618 .await
619 {
620 error!(
621 "Failed to send NonSupportedCommandResponse: {:?}",
622 e
623 );
624 return Err(e);
625 }
626 }
627
628 supported = false;
629 }
630 }
631
632 if supported {
633 let mut w = port_w.lock().await;
634 if let Err(e) = info.send_ack(&mut *w).await {
635 error!("Failed to send ACK for command: {:?}", e);
636 return Err(e);
637 }
638 }
639 } else {
640 if let Information::NonSupportedCommandResponse(
642 NonSupportedCommandResponse { command_type, .. },
643 ) = info
644 {
645 warn!("Mobile station didn't support command: {:?}", command_type);
646 }
647 }
648 }
649 FrameType::Ui | FrameType::Uih => {
650 let channel_id = header.id() as usize - 1;
651
652 if channel_id >= N {
653 warn!("Received data frame for invalid channel ID {}: index {} out of bounds (max {})",
654 header.id(), channel_id, N - 1);
655 if let Err(e) = header.finalize().await {
656 error!("Failed to finalize out-of-bounds data frame: {:?}", e);
657 return Err(e);
658 }
659 continue;
660 }
661
662 let frame_len = header.len;
663
664 let mut grant = match rx[channel_id].grant(frame_len as u16) {
670 Ok(grant) => grant,
671 Err(_) => {
672 warn!(
673 "Channel {} bbqueue full ({} bytes). Sending FC=1 and waiting for space.",
674 channel_id + 1,
675 frame_len
676 );
677 send_msc_fc(port_w, channel_id, true).await?;
678 let grant = rx[channel_id].wait_grant(frame_len as u16).await;
679 info!(
680 "Channel {} bbqueue space available, sending FC=0 to resume.",
681 channel_id + 1
682 );
683 send_msc_fc(port_w, channel_id, false).await?;
684 grant
685 }
686 };
687
688 if let Err(e) = header.copy_to_slice(&mut grant[..frame_len]).await {
689 drop(grant);
691 consecutive_errors += 1;
692 error!("Failed to copy data to channel {}: {:?}. Consecutive errors: {}/{}",
693 channel_id + 1, e, consecutive_errors, MAX_CONSECUTIVE_ERRORS);
694
695 if consecutive_errors >= MAX_CONSECUTIVE_ERRORS {
696 error!("Too many consecutive copy errors! Terminating CMUX.");
697 return Err(e);
698 }
699
700 warn!("Attempting to skip corrupted frame and resynchronize...");
701 if let Err(fin_err) = header.finalize().await {
702 error!("Failed to finalize after copy error: {:?}", fin_err);
703 }
704 continue;
705 }
706
707 match header.finalize().await {
710 Ok(()) => {
711 grant.commit(frame_len as u16);
712 trace!("CMUX RX: CH{} {} bytes", channel_id + 1, frame_len);
713 }
714 Err(e) => {
715 drop(grant);
717 consecutive_errors += 1;
718 error!("Frame FCS/finalize failed for channel {}: {:?}. Consecutive errors: {}/{}",
719 channel_id + 1, e, consecutive_errors, MAX_CONSECUTIVE_ERRORS);
720
721 if consecutive_errors >= MAX_CONSECUTIVE_ERRORS {
722 error!("Too many consecutive frame errors! Terminating CMUX.");
723 return Err(e);
724 }
725 continue;
726 }
727 }
728 continue;
730 }
731 FrameType::Sabm if header.is_control() => {
732 control_channel_opened.set(true);
733 let mut w = port_w.lock().await;
734 if let Err(e) = (frame::Ua { id: 0 }).write(&mut *w).await {
735 error!("Failed to send UA for control channel SABM: {:?}", e);
736 return Err(e);
737 }
738 }
739 FrameType::Sabm => {
740 let channel_id = header.id() as usize - 1;
741 if channel_id >= N {
742 warn!("Received SABM for invalid channel ID {}: index {} out of bounds (max {})",
743 header.id(), channel_id, N - 1);
744 let mut w = port_w.lock().await;
745 if let Err(e) = (frame::Dm { id: header.id() }).write(&mut *w).await {
746 error!("Failed to send DM for out-of-bounds SABM: {:?}", e);
747 return Err(e);
748 }
749 if let Err(e) = header.finalize().await {
750 error!("Failed to finalize: {:?}", e);
751 }
752 continue;
753 }
754 lines[channel_id].opened.set(true);
755 fc_changed.signal(());
756 let mut w = port_w.lock().await;
757 if let Err(e) = (frame::Ua { id: header.id() }).write(&mut *w).await {
758 error!(
759 "Failed to send UA for channel {} SABM: {:?}",
760 channel_id + 1,
761 e
762 );
763 return Err(e);
764 }
765 }
766 FrameType::Ua if header.is_control() => {
767 match control_pending_command.get() {
768 Some(FrameType::Sabm) => {
769 control_channel_opened.set(true);
770 }
771 Some(FrameType::Disc) => {
772 control_channel_opened.set(false);
773 }
774 other => {
775 warn!(
776 "Received UA for control channel with no pending command (was: {:?}), ignoring",
777 other
778 );
779 }
780 }
781 control_pending_command.set(None);
782 }
783 FrameType::Ua => {
784 let channel_id = header.id() as usize - 1;
785 if channel_id >= N {
786 warn!("Received UA for invalid channel ID {}: index {} out of bounds (max {})",
787 header.id(), channel_id, N - 1);
788 if let Err(e) = header.finalize().await {
789 error!("Failed to finalize: {:?}", e);
790 }
791 continue;
792 }
793 match lines[channel_id].pending_command.get() {
794 Some(FrameType::Sabm) => {
795 lines[channel_id].opened.set(true);
796 fc_changed.signal(());
797 }
798 Some(FrameType::Disc) => {
799 lines[channel_id].opened.set(false);
800 fc_changed.signal(());
801 }
802 other => {
803 warn!(
804 "Received UA for channel {} with no pending command (was: {:?}), ignoring",
805 channel_id + 1, other
806 );
807 }
808 }
809 lines[channel_id].pending_command.set(None);
810 }
811 FrameType::Dm if header.is_control() => {
812 error!("Couldn't open control channel - terminating CMUX");
813 if let Err(e) = header.finalize().await {
814 error!("Failed to finalize DM frame: {:?}", e);
815 }
816 return Err(FrameError::MalformedFrame);
817 }
818 FrameType::Dm => {
819 warn!("Logical channel {} couldn't be opened", header.id() - 1);
820 }
821 FrameType::Disc if header.is_control() => {
822 if control_channel_opened.get() {
823 control_channel_opened.set(false);
824 let mut w = port_w.lock().await;
825 if let Err(e) = (frame::Ua { id: 0 }).write(&mut *w).await {
826 error!("Failed to send UA for control channel DISC: {:?}", e);
827 return Err(e);
828 }
829 if let Err(e) = header.finalize().await {
830 error!("Failed to finalize: {:?}", e);
831 }
832 return Err(FrameError::MultiplexerCloseDown);
833 } else {
834 let mut w = port_w.lock().await;
835 if let Err(e) = (frame::Dm { id: 0 }).write(&mut *w).await {
836 error!("Failed to send DM for control channel DISC: {:?}", e);
837 return Err(e);
838 }
839 }
840 }
841 FrameType::Disc => {
842 let channel_id = header.id() as usize - 1;
843 if channel_id >= N {
844 warn!("Received DISC for invalid channel ID {}: index {} out of bounds (max {})",
845 header.id(), channel_id, N - 1);
846 let mut w = port_w.lock().await;
847 if let Err(e) = (frame::Dm { id: header.id() }).write(&mut *w).await {
848 error!("Failed to send DM for out-of-bounds DISC: {:?}", e);
849 return Err(e);
850 }
851 if let Err(e) = header.finalize().await {
852 error!("Failed to finalize: {:?}", e);
853 }
854 continue;
855 }
856 if lines[channel_id].opened.get() {
857 lines[channel_id].opened.set(false);
858 fc_changed.signal(());
859 let mut w = port_w.lock().await;
860 if let Err(e) = (frame::Ua { id: header.id() }).write(&mut *w).await {
861 error!(
862 "Failed to send UA for channel {} DISC: {:?}",
863 channel_id + 1,
864 e
865 );
866 return Err(e);
867 }
868 } else {
869 let mut w = port_w.lock().await;
870 if let Err(e) = (frame::Dm { id: header.id() }).write(&mut *w).await {
871 error!(
872 "Failed to send DM for channel {} DISC: {:?}",
873 channel_id + 1,
874 e
875 );
876 return Err(e);
877 }
878 }
879 }
880 }
881
882 if let Err(e) = header.finalize().await {
883 consecutive_errors += 1;
884 error!(
885 "Failed to finalize header: {:?}. Consecutive errors: {}/{}",
886 e, consecutive_errors, MAX_CONSECUTIVE_ERRORS
887 );
888
889 if consecutive_errors >= MAX_CONSECUTIVE_ERRORS {
890 error!("Too many consecutive frame errors! Stream is corrupted. Terminating CMUX.");
891 return Err(e);
892 }
893 continue;
894 }
895 }
896 }
897
898 embassy_futures::yield_now().await;
899 }
900}
901
902struct MaybeUninitArray<T, const N: usize>(MaybeUninit<[T; N]>);
905
906impl<T, const N: usize> MaybeUninitArray<T, N> {
907 fn uninit() -> Self {
908 Self(MaybeUninit::uninit())
909 }
910 unsafe fn set(&mut self, i: usize, val: T) {
911 (self.0.as_mut_ptr() as *mut T).add(i).write(val);
912 }
913 unsafe fn assume_init(self) -> [T; N] {
914 self.0.assume_init()
915 }
916}
917
918impl<'a, const BUF: usize> Channel<'a, BUF> {
919 pub fn split(
921 self,
922 ) -> (
923 ChannelRx<'a, BUF>,
924 ChannelTx<'a, BUF>,
925 ChannelLines<'a, BUF>,
926 ) {
927 (self.rx, self.tx, self.lines)
928 }
929
930 pub fn split_ref(
931 &mut self,
932 ) -> (
933 &mut ChannelRx<'a, BUF>,
934 &mut ChannelTx<'a, BUF>,
935 &ChannelLines<'a, BUF>,
936 ) {
937 (&mut self.rx, &mut self.tx, &self.lines)
938 }
939
940 pub fn split_lines(&self) -> ChannelLines<'a, BUF> {
945 self.lines
946 }
947
948 pub fn set_lines(&self, control: Control, brk: Option<Break>) {
950 self.lines.lines.tx.set((control, brk));
951 self.lines.line_status_updated.signal(());
952 }
953
954 pub fn get_lines(&self) -> (Control, Option<Break>) {
956 self.lines.lines.rx.get()
957 }
958
959 pub fn set_hangup_detection(&self, mask: u16, val: u16) {
961 self.lines.lines.hangup_mask.set(Some((mask, val)));
962 self.lines.lines.check_hangup();
963 }
964
965 pub fn clear_hangup_detection(&self) {
967 self.lines.lines.hangup_mask.set(None);
968 self.lines.lines.check_hangup();
969 }
970}
971
972impl<'a, const BUF: usize> ChannelRx<'a, BUF> {
973 pub fn set_lines(&self, control: Control, brk: Option<Break>) {
975 self.lines.tx.set((control, brk));
976 self.line_status_updated.signal(());
977 }
978
979 pub fn get_lines(&self) -> (Control, Option<Break>) {
981 self.lines.rx.get()
982 }
983}
984
985impl<'a, const BUF: usize> ChannelTx<'a, BUF> {
986 pub fn set_lines(&self, control: Control, brk: Option<Break>) {
988 self.lines.tx.set((control, brk));
989 self.line_status_updated.signal(());
990 }
991
992 pub fn get_lines(&self) -> (Control, Option<Break>) {
994 self.lines.rx.get()
995 }
996}
997
998impl<'a, const BUF: usize> ChannelLines<'a, BUF> {
999 pub fn set_lines(&self, control: Control, brk: Option<Break>) {
1001 self.lines.tx.set((control, brk));
1002 self.line_status_updated.signal(());
1003 }
1004
1005 pub fn get_lines(&self) -> (Control, Option<Break>) {
1007 self.lines.rx.get()
1008 }
1009}
1010
1011#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1013#[cfg_attr(feature = "defmt", derive(defmt::Format))]
1014pub enum ChannelError {
1015 Hangup,
1017}
1018
1019impl core::fmt::Display for ChannelError {
1020 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1021 core::write!(f, "{:?}", self)
1022 }
1023}
1024
1025impl core::error::Error for ChannelError {}
1026
1027impl embedded_io_async::Error for ChannelError {
1028 fn kind(&self) -> embedded_io_async::ErrorKind {
1029 match self {
1030 Self::Hangup => embedded_io_async::ErrorKind::BrokenPipe,
1031 }
1032 }
1033}
1034
1035impl<'a, const BUF: usize> embedded_io_async::ErrorType for Channel<'a, BUF> {
1036 type Error = ChannelError;
1037}
1038
1039impl<'a, const BUF: usize> embedded_io_async::Read for Channel<'a, BUF> {
1040 async fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
1041 self.rx.read(buf).await
1042 }
1043}
1044
1045impl<'a, const BUF: usize> embedded_io_async::BufRead for Channel<'a, BUF> {
1046 async fn fill_buf(&mut self) -> Result<&[u8], Self::Error> {
1047 self.rx.fill_buf().await
1048 }
1049
1050 fn consume(&mut self, amt: usize) {
1051 self.rx.consume(amt);
1052 }
1053}
1054
1055impl<'a, const BUF: usize> embedded_io_async::Write for Channel<'a, BUF> {
1056 async fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
1057 self.tx.write(buf).await
1058 }
1059
1060 async fn flush(&mut self) -> Result<(), Self::Error> {
1061 self.tx.flush().await
1062 }
1063}
1064
1065impl<'a, const BUF: usize> embedded_io_async::ErrorType for ChannelRx<'a, BUF> {
1066 type Error = ChannelError;
1067}
1068
1069impl<'a, const BUF: usize> embedded_io_async::Read for ChannelRx<'a, BUF> {
1070 async fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
1071 check_hangup(self.rx.read(buf), self.lines).await
1072 }
1073}
1074
1075impl<'a, const BUF: usize> embedded_io_async::BufRead for ChannelRx<'a, BUF> {
1076 async fn fill_buf(&mut self) -> Result<&[u8], Self::Error> {
1077 check_hangup(self.rx.fill_buf(), self.lines).await
1078 }
1079
1080 fn consume(&mut self, amt: usize) {
1081 self.rx.consume(amt);
1082 }
1083}
1084
1085impl<'a, const BUF: usize> embedded_io_async::ErrorType for ChannelTx<'a, BUF> {
1086 type Error = ChannelError;
1087}
1088
1089impl<'a, const BUF: usize> embedded_io_async::Write for ChannelTx<'a, BUF> {
1090 async fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
1091 check_hangup(self.tx.write(buf), self.lines).await
1092 }
1093
1094 async fn flush(&mut self) -> Result<(), Self::Error> {
1095 self.tx.flush().await;
1096 Ok(())
1097 }
1098}
1099
1100fn wait_for_hangup(lines: &Lines) -> impl Future<Output = ()> + '_ {
1101 poll_fn(|cx| {
1102 if lines.hangup.get() {
1103 Poll::Ready(())
1104 } else {
1105 lines.hangup_waker.register(cx.waker());
1106 Poll::Pending
1107 }
1108 })
1109}
1110
1111async fn check_hangup<'a, F, R>(fut: F, lines: &'a Lines) -> Result<R, ChannelError>
1112where
1113 F: Future<Output = R> + 'a,
1114{
1115 match select(fut, wait_for_hangup(lines)).await {
1116 Either::First(r) => Ok(r),
1117 Either::Second(()) => Err(ChannelError::Hangup),
1118 }
1119}
1120
1121#[must_use = "to delay the drop handler invocation to the end of the scope"]
1122struct OnDrop<F: FnOnce()> {
1123 f: core::mem::MaybeUninit<F>,
1124}
1125
1126impl<F: FnOnce()> OnDrop<F> {
1127 fn new(f: F) -> Self {
1128 Self {
1129 f: core::mem::MaybeUninit::new(f),
1130 }
1131 }
1132
1133 fn defuse(self) {
1134 core::mem::forget(self)
1135 }
1136}
1137
1138impl<F: FnOnce()> Drop for OnDrop<F> {
1139 fn drop(&mut self) {
1140 unsafe { self.f.as_ptr().read()() }
1141 }
1142}