1use std::collections::HashMap;
11use std::sync::atomic::{AtomicU8, Ordering};
12use std::sync::{Arc, Mutex};
13use std::time::Duration;
14
15use tokio::sync::{mpsc, oneshot, Semaphore};
16
17use crate::connection::{ConnState, Connection, ParamsRequest, Role};
18use crate::error::{Error, Result};
19use crate::host::Host;
20
21pub mod cid {
23 pub const ATT: u16 = 0x0004;
25 pub const SIGNALING: u16 = 0x0005;
27 pub const SMP: u16 = 0x0006;
29 pub const DYNAMIC_FIRST: u16 = 0x0040;
31 pub const DYNAMIC_LAST: u16 = 0x007f;
33}
34
35mod code {
36 pub const COMMAND_REJECT: u8 = 0x01;
37 pub const DISCONNECTION_REQ: u8 = 0x06;
38 pub const DISCONNECTION_RSP: u8 = 0x07;
39 pub const CONN_PARAM_UPDATE_REQ: u8 = 0x12;
40 pub const CONN_PARAM_UPDATE_RSP: u8 = 0x13;
41 pub const LE_CREDIT_CONN_REQ: u8 = 0x14;
42 pub const LE_CREDIT_CONN_RSP: u8 = 0x15;
43 pub const FLOW_CONTROL_CREDIT: u8 = 0x16;
44 pub const CREDIT_CONN_REQ: u8 = 0x17;
45 pub const CREDIT_CONN_RSP: u8 = 0x18;
46}
47
48pub(crate) fn dispatch(host: &Host, handle: u16, pdu: Vec<u8>) {
50 let Some(conn) = host.state(handle) else {
51 return;
52 };
53 if pdu.len() < 4 {
54 return;
55 }
56 let channel = u16::from_le_bytes([pdu[2], pdu[3]]);
57 let payload = pdu[4..].to_vec();
58 let sent = match channel {
59 cid::ATT => conn.att_tx.try_send(payload).is_ok(),
60 cid::SIGNALING => conn.sig_tx.try_send(payload).is_ok(),
61 cid::SMP => conn.smp_tx.try_send(payload).is_ok(),
62 c => {
63 on_k_frame(host, &conn, c, payload);
64 true
65 }
66 };
67 if !sent {
68 tracing::warn!(
69 "connection {handle:#05x}: channel {channel:#06x} is backed up; dropping a PDU"
70 );
71 }
72}
73
74type Response = (u8, Vec<u8>);
76
77#[derive(Default)]
79pub(crate) struct Signaling {
80 next_id: AtomicU8,
81 pending: Mutex<HashMap<u8, oneshot::Sender<Response>>>,
82}
83
84impl Signaling {
85 fn id(&self) -> u8 {
86 loop {
87 let id = self.next_id.fetch_add(1, Ordering::Relaxed);
88 if id != 0 {
89 return id;
90 }
91 }
92 }
93}
94
95const RTX: Duration = Duration::from_secs(30);
96
97async fn request(host: &Host, conn: &ConnState, code: u8, data: &[u8]) -> Result<(u8, Vec<u8>)> {
99 let id = conn.sig.id();
100 let (tx, rx) = oneshot::channel();
101 conn.sig
102 .pending
103 .lock()
104 .unwrap_or_else(|e| e.into_inner())
105 .insert(id, tx);
106 send_signal(host, conn, code, id, data).await?;
107 match tokio::time::timeout(RTX, rx).await {
108 Ok(Ok(r)) => Ok(r),
109 Ok(Err(_)) => Err(Error::Disconnected),
110 Err(_) => {
111 conn.sig
112 .pending
113 .lock()
114 .unwrap_or_else(|e| e.into_inner())
115 .remove(&id);
116 Err(Error::Timeout("an L2CAP signaling response"))
117 }
118 }
119}
120
121async fn send_signal(host: &Host, conn: &ConnState, code: u8, id: u8, data: &[u8]) -> Result<()> {
122 let mut v = Vec::with_capacity(4 + data.len());
123 v.push(code);
124 v.push(id);
125 v.extend_from_slice(&(data.len() as u16).to_le_bytes());
126 v.extend_from_slice(data);
127 conn.send(host, cid::SIGNALING, &v).await
128}
129
130pub(crate) async fn run_signaling(
132 host: Host,
133 conn: Arc<ConnState>,
134 mut rx: mpsc::Receiver<Vec<u8>>,
135) {
136 loop {
137 let pdu = tokio::select! {
138 p = rx.recv() => match p { Some(p) => p, None => return },
139 _ = conn.wait_closed() => return,
140 };
141 if pdu.len() < 4 {
143 continue;
144 }
145 let (code, id) = (pdu[0], pdu[1]);
146 let len = usize::from(u16::from_le_bytes([pdu[2], pdu[3]]));
147 let Some(data) = pdu.get(4..4 + len) else {
148 let _ = send_signal(&host, &conn, code::COMMAND_REJECT, id, &[0x00, 0x00]).await;
149 continue;
150 };
151 if let Err(e) = on_signal(&host, &conn, code, id, data).await {
152 tracing::debug!("signaling on {:#05x}: {e}", conn.handle);
153 }
154 }
155}
156
157fn u16_at(d: &[u8], i: usize) -> u16 {
158 match d.get(i..i + 2) {
159 Some(b) => u16::from_le_bytes([b[0], b[1]]),
160 None => 0,
161 }
162}
163
164async fn on_signal(
165 host: &Host,
166 conn: &Arc<ConnState>,
167 code: u8,
168 id: u8,
169 data: &[u8],
170) -> Result<()> {
171 match code {
172 code::COMMAND_REJECT
174 | code::DISCONNECTION_RSP
175 | code::CONN_PARAM_UPDATE_RSP
176 | code::LE_CREDIT_CONN_RSP
177 | code::CREDIT_CONN_RSP => {
178 if let Some(tx) = conn
179 .sig
180 .pending
181 .lock()
182 .unwrap_or_else(|e| e.into_inner())
183 .remove(&id)
184 {
185 let _ = tx.send((code, data.to_vec()));
186 }
187 Ok(())
188 }
189 code::CONN_PARAM_UPDATE_REQ if data.len() == 8 => {
190 let p = ParamsRequest {
191 interval_min: u16_at(data, 0),
192 interval_max: u16_at(data, 2),
193 latency: u16_at(data, 4),
194 timeout: u16_at(data, 6),
195 };
196 if conn.role != Role::Central {
197 return send_signal(host, conn, code::COMMAND_REJECT, id, &[0x00, 0x00]).await;
199 }
200 let ok = p.check().is_ok();
201 send_signal(
202 host,
203 conn,
204 code::CONN_PARAM_UPDATE_RSP,
205 id,
206 &u16::from(!ok).to_le_bytes(),
207 )
208 .await?;
209 if ok {
210 let c = Connection::new(host.clone(), conn.clone());
211 tokio::spawn(async move {
212 if let Err(e) = c.update_params(p).await {
213 tracing::info!(
214 "peripheral's parameter request on {:#05x} failed: {e}",
215 c.handle()
216 );
217 }
218 });
219 }
220 Ok(())
221 }
222 code::LE_CREDIT_CONN_REQ if data.len() == 10 => {
223 let (spsm, scid, mtu, mps, credits) = (
224 u16_at(data, 0),
225 u16_at(data, 2),
226 u16_at(data, 4),
227 u16_at(data, 6),
228 u16_at(data, 8),
229 );
230 let (result, dcid, our_mtu) = accept(host, conn, spsm, &[scid], mtu, mps, credits);
231 let mut r = Vec::with_capacity(10);
232 r.extend_from_slice(&dcid.first().copied().unwrap_or(0).to_le_bytes());
233 r.extend_from_slice(&our_mtu.to_le_bytes());
234 r.extend_from_slice(&LOCAL_MPS.to_le_bytes());
235 r.extend_from_slice(&LOCAL_CREDITS.to_le_bytes());
236 r.extend_from_slice(&result.to_le_bytes());
237 send_signal(host, conn, code::LE_CREDIT_CONN_RSP, id, &r).await
238 }
239 code::CREDIT_CONN_REQ
240 if data.len() >= 10 && data.len() <= 18 && data.len().is_multiple_of(2) =>
241 {
242 let (spsm, mtu, mps, credits) = (
243 u16_at(data, 0),
244 u16_at(data, 2),
245 u16_at(data, 4),
246 u16_at(data, 6),
247 );
248 let scids: Vec<u16> = data[8..]
249 .as_chunks::<2>()
250 .0
251 .iter()
252 .map(|c| u16::from_le_bytes(*c))
253 .collect();
254 let (result, dcids, our_mtu) = accept(host, conn, spsm, &scids, mtu, mps, credits);
255 let mut r = Vec::new();
256 r.extend_from_slice(&our_mtu.to_le_bytes());
257 r.extend_from_slice(&LOCAL_MPS.to_le_bytes());
258 r.extend_from_slice(&LOCAL_CREDITS.to_le_bytes());
259 r.extend_from_slice(&result.to_le_bytes());
260 for i in 0..scids.len() {
261 r.extend_from_slice(&dcids.get(i).copied().unwrap_or(0).to_le_bytes());
262 }
263 send_signal(host, conn, code::CREDIT_CONN_RSP, id, &r).await
264 }
265 code::FLOW_CONTROL_CREDIT if data.len() == 4 => {
266 let (c, n) = (u16_at(data, 0), u16_at(data, 2));
267 let coc = conn.coc.lock().unwrap_or_else(|e| e.into_inner());
268 if let Some(end) = coc.values().find(|e| e.remote_cid == c) {
269 if end.tx_credits.available_permits() + usize::from(n) <= 65_535 {
271 end.tx_credits.add_permits(usize::from(n));
272 }
273 }
274 Ok(())
275 }
276 code::DISCONNECTION_REQ if data.len() == 4 => {
277 let (dcid, scid) = (u16_at(data, 0), u16_at(data, 2));
278 conn.coc
279 .lock()
280 .unwrap_or_else(|e| e.into_inner())
281 .remove(&dcid);
282 send_signal(host, conn, code::DISCONNECTION_RSP, id, data).await?;
283 tracing::debug!("peer closed channel {dcid:#06x}/{scid:#06x}");
284 Ok(())
285 }
286 _ => send_signal(host, conn, code::COMMAND_REJECT, id, &[0x00, 0x00]).await,
287 }
288}
289
290const LOCAL_MPS: u16 = 247;
293const LOCAL_CREDITS: u16 = 16;
295
296#[allow(clippy::too_many_arguments)]
298fn accept(
299 host: &Host,
300 conn: &Arc<ConnState>,
301 spsm: u16,
302 scids: &[u16],
303 mtu: u16,
304 mps: u16,
305 credits: u16,
306) -> (u16, Vec<u16>, u16) {
307 let server = host
308 .inner
309 .l2cap_servers
310 .lock()
311 .unwrap_or_else(|e| e.into_inner())
312 .get(&spsm)
313 .cloned();
314 let Some((local_mtu, listener)) = server else {
315 return (0x0002, vec![], 0); };
317 if mtu < 23 || mps < 23 {
318 return (0x000c, vec![], 0); }
320 let mut dcids = Vec::new();
321 for &scid in scids {
322 if !(cid::DYNAMIC_FIRST..=cid::DYNAMIC_LAST).contains(&scid) {
323 return (0x0009, vec![], 0); }
325 match open_end(conn, scid, mtu, mps, credits, local_mtu) {
326 Some((local, ch)) => {
327 dcids.push(local);
328 let channel = Channel::new(Connection::new(host.clone(), conn.clone()), ch);
329 if listener.try_send(channel).is_err() {
330 return (0x0004, vec![], 0); }
332 }
333 None => return (0x0004, vec![], 0),
334 }
335 }
336 (0, dcids, local_mtu)
337}
338
339pub(crate) struct ChannelEnd {
341 remote_cid: u16,
342 tx_credits: Arc<Semaphore>,
343 sdus: mpsc::Sender<(Vec<u8>, u16)>,
344 partial: Option<(usize, Vec<u8>)>,
345 frames: u16,
347 rx_credits: u16,
349 local_mtu: u16,
350}
351
352struct NewChannel {
353 local_cid: u16,
354 remote_cid: u16,
355 remote_mtu: u16,
356 remote_mps: u16,
357 local_mtu: u16,
358 tx_credits: Arc<Semaphore>,
359 sdus: mpsc::Receiver<(Vec<u8>, u16)>,
360}
361
362fn open_end(
363 conn: &ConnState,
364 remote_cid: u16,
365 remote_mtu: u16,
366 remote_mps: u16,
367 credits: u16,
368 local_mtu: u16,
369) -> Option<(u16, NewChannel)> {
370 let mut coc = conn.coc.lock().unwrap_or_else(|e| e.into_inner());
371 let local = (cid::DYNAMIC_FIRST..=cid::DYNAMIC_LAST).find(|c| !coc.contains_key(c))?;
372 let tx_credits = Arc::new(Semaphore::new(usize::from(credits)));
373 let (tx, rx) = mpsc::channel(usize::from(LOCAL_CREDITS));
374 coc.insert(
375 local,
376 ChannelEnd {
377 remote_cid,
378 tx_credits: tx_credits.clone(),
379 sdus: tx,
380 partial: None,
381 frames: 0,
382 rx_credits: LOCAL_CREDITS,
383 local_mtu,
384 },
385 );
386 Some((
387 local,
388 NewChannel {
389 local_cid: local,
390 remote_cid,
391 remote_mtu,
392 remote_mps,
393 local_mtu,
394 tx_credits,
395 sdus: rx,
396 },
397 ))
398}
399
400fn on_k_frame(host: &Host, conn: &Arc<ConnState>, local: u16, frame: Vec<u8>) {
401 let mut coc = conn.coc.lock().unwrap_or_else(|e| e.into_inner());
402 let Some(end) = coc.get_mut(&local) else {
403 tracing::debug!("K-frame for unknown channel {local:#06x}");
404 return;
405 };
406 end.frames = end.frames.saturating_add(1);
407 if end.rx_credits == 0 {
408 tracing::warn!("channel {local:#06x}: peer exceeded its credits; closing it");
410 let remote = end.remote_cid;
411 coc.remove(&local);
412 drop(coc);
413 let (host, conn) = (host.clone(), conn.clone());
414 tokio::spawn(async move {
415 let mut d = local.to_le_bytes().to_vec();
416 d.extend_from_slice(&remote.to_le_bytes());
417 let _ = request(&host, &conn, code::DISCONNECTION_REQ, &d).await;
418 });
419 return;
420 }
421 end.rx_credits -= 1;
422 let (sdu_len, data) = match end.partial.take() {
423 None => {
424 if frame.len() < 2 {
425 return;
426 }
427 (
428 usize::from(u16::from_le_bytes([frame[0], frame[1]])),
429 frame[2..].to_vec(),
430 )
431 }
432 Some((want, mut have)) => {
433 have.extend_from_slice(&frame);
434 (want, have)
435 }
436 };
437 if sdu_len > usize::from(end.local_mtu) || data.len() > sdu_len {
438 tracing::warn!("channel {local:#06x}: SDU larger than our MTU or its length; dropping it");
440 let frames = std::mem::take(&mut end.frames);
441 let _ = end.sdus.try_send((Vec::new(), frames));
442 return;
443 }
444 if data.len() == sdu_len {
445 let frames = std::mem::take(&mut end.frames);
446 if end.sdus.try_send((data, frames)).is_err() {
447 tracing::warn!("channel {local:#06x}: application is not reading; dropping an SDU");
448 }
449 } else {
450 end.partial = Some((sdu_len, data));
451 }
452}
453
454pub fn listen(host: &Host, spsm: u16, mtu: u16) -> mpsc::Receiver<Channel> {
456 let (tx, rx) = mpsc::channel(8);
457 host.inner
458 .l2cap_servers
459 .lock()
460 .unwrap_or_else(|e| e.into_inner())
461 .insert(spsm, (mtu.max(23), tx));
462 rx
463}
464
465pub(crate) async fn connect(conn: &Connection, spsm: u16, mtu: u16) -> Result<Channel> {
467 let mtu = mtu.max(23);
468 let (local, new) = open_end(&conn.state, 0, 0, 0, 0, mtu)
469 .ok_or(Error::Protocol("no free channel identifiers".into()))?;
470 let mut d = Vec::with_capacity(10);
471 d.extend_from_slice(&spsm.to_le_bytes());
472 d.extend_from_slice(&local.to_le_bytes());
473 d.extend_from_slice(&mtu.to_le_bytes());
474 d.extend_from_slice(&LOCAL_MPS.to_le_bytes());
475 d.extend_from_slice(&LOCAL_CREDITS.to_le_bytes());
476 let r = request(&conn.host, &conn.state, code::LE_CREDIT_CONN_REQ, &d).await;
477 let fail = |e| {
478 conn.state
479 .coc
480 .lock()
481 .unwrap_or_else(|e| e.into_inner())
482 .remove(&local);
483 e
484 };
485 let (code, rsp) = r.map_err(fail)?;
486 if code != code::LE_CREDIT_CONN_RSP || rsp.len() != 10 {
487 return Err(fail(Error::Protocol("channel request rejected".into())));
488 }
489 let (dcid, rmtu, rmps, credits, result) = (
490 u16_at(&rsp, 0),
491 u16_at(&rsp, 2),
492 u16_at(&rsp, 4),
493 u16_at(&rsp, 6),
494 u16_at(&rsp, 8),
495 );
496 if result != 0 {
497 return Err(fail(Error::Protocol(format!(
498 "channel refused, result {result:#06x}"
499 ))));
500 }
501 {
502 let mut coc = conn.state.coc.lock().unwrap_or_else(|e| e.into_inner());
503 if let Some(end) = coc.get_mut(&local) {
504 end.remote_cid = dcid;
505 end.tx_credits.add_permits(usize::from(credits));
506 }
507 }
508 Ok(Channel::new(
509 conn.clone(),
510 NewChannel {
511 remote_cid: dcid,
512 remote_mtu: rmtu,
513 remote_mps: rmps,
514 ..new
515 },
516 ))
517}
518
519pub struct Channel {
521 conn: Connection,
522 local_cid: u16,
523 remote_cid: u16,
524 remote_mtu: u16,
525 remote_mps: u16,
526 local_mtu: u16,
527 tx_credits: Arc<Semaphore>,
528 sdus: mpsc::Receiver<(Vec<u8>, u16)>,
529}
530
531impl std::fmt::Debug for Channel {
532 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
533 f.debug_struct("Channel")
534 .field("local_cid", &self.local_cid)
535 .field("remote_cid", &self.remote_cid)
536 .field("remote_mtu", &self.remote_mtu)
537 .finish()
538 }
539}
540
541impl Channel {
542 fn new(conn: Connection, n: NewChannel) -> Self {
543 Channel {
544 conn,
545 local_cid: n.local_cid,
546 remote_cid: n.remote_cid,
547 remote_mtu: n.remote_mtu,
548 remote_mps: n.remote_mps.max(23),
549 local_mtu: n.local_mtu,
550 tx_credits: n.tx_credits,
551 sdus: n.sdus,
552 }
553 }
554
555 pub fn peer_mtu(&self) -> u16 {
557 self.remote_mtu
558 }
559
560 pub fn mtu(&self) -> u16 {
562 self.local_mtu
563 }
564
565 pub fn cid(&self) -> u16 {
567 self.local_cid
568 }
569
570 pub async fn send(&self, sdu: &[u8]) -> Result<()> {
572 if sdu.len() > usize::from(self.remote_mtu) {
573 return Err(Error::InvalidParameter(format!(
574 "SDU of {} octets over the peer's MTU {}",
575 sdu.len(),
576 self.remote_mtu
577 )));
578 }
579 let mps = usize::from(self.remote_mps);
580 let mut first = Vec::with_capacity(mps.min(sdu.len() + 2));
581 first.extend_from_slice(&(sdu.len() as u16).to_le_bytes());
582 let n = sdu.len().min(mps - 2);
583 first.extend_from_slice(&sdu[..n]);
584 let mut frames = vec![first];
585 frames.extend(sdu[n..].chunks(mps).map(|c| c.to_vec()));
586 for f in frames {
587 let permit = tokio::select! {
588 p = self.tx_credits.acquire() => p.map_err(|_| Error::Disconnected)?,
589 _ = self.conn.state.wait_closed() => return Err(Error::Disconnected),
590 };
591 permit.forget();
592 self.conn
593 .state
594 .send(&self.conn.host, self.remote_cid, &f)
595 .await?;
596 }
597 Ok(())
598 }
599
600 pub async fn recv(&mut self) -> Option<Vec<u8>> {
602 let (sdu, frames) = loop {
603 let (sdu, frames) = tokio::select! {
604 s = self.sdus.recv() => s?,
605 _ = self.conn.state.wait_closed() => return None,
606 };
607 if !sdu.is_empty() || frames == 0 {
608 break (sdu, frames);
609 }
610 self.grant(frames).await;
612 };
613 self.grant(frames).await;
614 Some(sdu)
615 }
616
617 async fn grant(&self, frames: u16) {
619 if let Some(end) = self
620 .conn
621 .state
622 .coc
623 .lock()
624 .unwrap_or_else(|e| e.into_inner())
625 .get_mut(&self.local_cid)
626 {
627 end.rx_credits = end.rx_credits.saturating_add(frames);
628 }
629 let mut d = self.local_cid.to_le_bytes().to_vec();
630 d.extend_from_slice(&frames.to_le_bytes());
631 let id = self.conn.state.sig.id();
632 let _ = send_signal(
633 &self.conn.host,
634 &self.conn.state,
635 code::FLOW_CONTROL_CREDIT,
636 id,
637 &d,
638 )
639 .await;
640 }
641
642 pub async fn close(self) -> Result<()> {
644 self.conn
645 .state
646 .coc
647 .lock()
648 .unwrap_or_else(|e| e.into_inner())
649 .remove(&self.local_cid);
650 let mut d = self.remote_cid.to_le_bytes().to_vec();
651 d.extend_from_slice(&self.local_cid.to_le_bytes());
652 request(
653 &self.conn.host,
654 &self.conn.state,
655 code::DISCONNECTION_REQ,
656 &d,
657 )
658 .await
659 .map(|_| ())
660 }
661}
662
663pub(crate) async fn request_param_update(
666 host: &Host,
667 conn: &Arc<ConnState>,
668 p: ParamsRequest,
669) -> Result<()> {
670 let mut d = Vec::with_capacity(8);
671 for v in [p.interval_min, p.interval_max, p.latency, p.timeout] {
672 d.extend_from_slice(&v.to_le_bytes());
673 }
674 match request(host, conn, code::CONN_PARAM_UPDATE_REQ, &d).await? {
675 (code::CONN_PARAM_UPDATE_RSP, r) if u16_at(&r, 0) == 0 => Ok(()),
676 (code::CONN_PARAM_UPDATE_RSP, _) => Err(Error::Protocol(
677 "the central rejected the parameters".into(),
678 )),
679 _ => Err(Error::Protocol(
680 "the central does not take parameter requests".into(),
681 )),
682 }
683}