veridian_kernel/drivers/bluetooth/
l2cap.rs

1//! Bluetooth L2CAP (Logical Link Control and Adaptation Protocol)
2//!
3//! Implements the L2CAP layer for Bluetooth, providing connection-oriented
4//! and connectionless data transport between upper-layer protocols and the
5//! HCI/baseband layer. Supports segmentation and reassembly (SAR), MTU
6//! negotiation, and signaling command processing.
7//!
8//! Reference: Bluetooth Core Specification v5.4, Volume 3, Part A (L2CAP)
9
10#![allow(dead_code)]
11
12#[cfg(feature = "alloc")]
13use alloc::collections::BTreeMap;
14#[cfg(feature = "alloc")]
15use alloc::vec::Vec;
16
17use crate::error::KernelError;
18
19// ---------------------------------------------------------------------------
20// L2CAP Channel IDs
21// ---------------------------------------------------------------------------
22
23/// L2CAP Signaling channel (CID 0x0001)
24pub const CID_SIGNALING: u16 = 0x0001;
25
26/// L2CAP Connectionless channel (CID 0x0002)
27pub const CID_CONNECTIONLESS: u16 = 0x0002;
28
29/// L2CAP ATT fixed channel (CID 0x0004)
30pub const CID_ATT: u16 = 0x0004;
31
32/// L2CAP LE Signaling channel (CID 0x0005)
33pub const CID_LE_SIGNALING: u16 = 0x0005;
34
35/// L2CAP SMP channel (CID 0x0006)
36pub const CID_SMP: u16 = 0x0006;
37
38/// First dynamically allocated CID
39pub const CID_DYNAMIC_START: u16 = 0x0040;
40
41/// Last dynamically allocated CID
42pub const CID_DYNAMIC_END: u16 = 0xFFFF;
43
44// ---------------------------------------------------------------------------
45// Well-known PSM values
46// ---------------------------------------------------------------------------
47
48/// SDP PSM
49pub const PSM_SDP: u16 = 0x0001;
50
51/// RFCOMM PSM
52pub const PSM_RFCOMM: u16 = 0x0003;
53
54/// BNEP (Bluetooth Network Encapsulation Protocol) PSM
55pub const PSM_BNEP: u16 = 0x000F;
56
57/// HID Control PSM
58pub const PSM_HID_CONTROL: u16 = 0x0011;
59
60/// HID Interrupt PSM
61pub const PSM_HID_INTERRUPT: u16 = 0x0013;
62
63/// AVCTP PSM (Audio/Video Control Transport Protocol)
64pub const PSM_AVCTP: u16 = 0x0017;
65
66/// AVDTP PSM (Audio/Video Distribution Transport Protocol)
67pub const PSM_AVDTP: u16 = 0x0019;
68
69// ---------------------------------------------------------------------------
70// L2CAP Signaling Command Codes
71// ---------------------------------------------------------------------------
72
73/// L2CAP signaling command codes
74#[derive(Debug, Clone, Copy, PartialEq, Eq)]
75#[repr(u8)]
76pub enum SignalingCode {
77    /// Command Reject
78    CommandReject = 0x01,
79    /// Connection Request
80    ConnectionReq = 0x02,
81    /// Connection Response
82    ConnectionResp = 0x03,
83    /// Configuration Request
84    ConfigReq = 0x04,
85    /// Configuration Response
86    ConfigResp = 0x05,
87    /// Disconnection Request
88    DisconnReq = 0x06,
89    /// Disconnection Response
90    DisconnResp = 0x07,
91    /// Echo Request
92    EchoReq = 0x08,
93    /// Echo Response
94    EchoResp = 0x09,
95    /// Information Request
96    InfoReq = 0x0A,
97    /// Information Response
98    InfoResp = 0x0B,
99}
100
101impl SignalingCode {
102    /// Parse signaling code from raw byte
103    pub fn from_u8(val: u8) -> Option<Self> {
104        match val {
105            0x01 => Some(Self::CommandReject),
106            0x02 => Some(Self::ConnectionReq),
107            0x03 => Some(Self::ConnectionResp),
108            0x04 => Some(Self::ConfigReq),
109            0x05 => Some(Self::ConfigResp),
110            0x06 => Some(Self::DisconnReq),
111            0x07 => Some(Self::DisconnResp),
112            0x08 => Some(Self::EchoReq),
113            0x09 => Some(Self::EchoResp),
114            0x0A => Some(Self::InfoReq),
115            0x0B => Some(Self::InfoResp),
116            _ => None,
117        }
118    }
119}
120
121// ---------------------------------------------------------------------------
122// Connection Response Result Codes
123// ---------------------------------------------------------------------------
124
125/// L2CAP Connection Response result codes
126#[derive(Debug, Clone, Copy, PartialEq, Eq)]
127#[repr(u16)]
128pub enum ConnectionResult {
129    /// Connection successful
130    Success = 0x0000,
131    /// Connection pending
132    Pending = 0x0001,
133    /// Connection refused - PSM not supported
134    PsmNotSupported = 0x0002,
135    /// Connection refused - security block
136    SecurityBlock = 0x0003,
137    /// Connection refused - no resources available
138    NoResources = 0x0004,
139}
140
141/// L2CAP Configuration Response result codes
142#[derive(Debug, Clone, Copy, PartialEq, Eq)]
143#[repr(u16)]
144pub enum ConfigResult {
145    /// Configuration successful
146    Success = 0x0000,
147    /// Failure - unacceptable parameters
148    UnacceptableParams = 0x0001,
149    /// Failure - rejected
150    Rejected = 0x0002,
151    /// Failure - unknown options
152    UnknownOptions = 0x0003,
153}
154
155// ---------------------------------------------------------------------------
156// L2CAP PDU
157// ---------------------------------------------------------------------------
158
159/// Maximum L2CAP payload size (before segmentation)
160pub const L2CAP_MAX_PAYLOAD: usize = 65535;
161
162/// Default L2CAP MTU
163pub const L2CAP_DEFAULT_MTU: u16 = 672;
164
165/// Minimum L2CAP MTU
166pub const L2CAP_MIN_MTU: u16 = 48;
167
168/// L2CAP header size (length + CID)
169pub const L2CAP_HEADER_SIZE: usize = 4;
170
171/// L2CAP Protocol Data Unit
172#[derive(Debug, Clone)]
173pub struct L2capPdu {
174    /// Payload length (excluding header)
175    pub length: u16,
176    /// Channel ID
177    pub channel_id: u16,
178    /// Payload data
179    #[cfg(feature = "alloc")]
180    pub payload: Vec<u8>,
181    #[cfg(not(feature = "alloc"))]
182    pub payload: [u8; L2CAP_DEFAULT_MTU as usize],
183    #[cfg(not(feature = "alloc"))]
184    pub payload_len: usize,
185}
186
187impl L2capPdu {
188    /// Create a new L2CAP PDU
189    #[cfg(feature = "alloc")]
190    pub fn new(channel_id: u16, data: &[u8]) -> Self {
191        Self {
192            length: data.len() as u16,
193            channel_id,
194            payload: Vec::from(data),
195        }
196    }
197
198    /// Create a new L2CAP PDU (no-alloc)
199    #[cfg(not(feature = "alloc"))]
200    pub fn new(channel_id: u16, data: &[u8]) -> Self {
201        let copy_len = data.len().min(L2CAP_DEFAULT_MTU as usize);
202        let mut payload = [0u8; L2CAP_DEFAULT_MTU as usize];
203        payload[..copy_len].copy_from_slice(&data[..copy_len]);
204        Self {
205            length: copy_len as u16,
206            channel_id,
207            payload,
208            payload_len: copy_len,
209        }
210    }
211
212    /// Serialize PDU to buffer, returns bytes written
213    pub fn serialize(&self, buf: &mut [u8]) -> Result<usize, KernelError> {
214        let total = L2CAP_HEADER_SIZE + self.length as usize;
215        if buf.len() < total {
216            return Err(KernelError::InvalidArgument {
217                name: "buffer",
218                value: "too small for L2CAP PDU",
219            });
220        }
221        buf[0..2].copy_from_slice(&self.length.to_le_bytes());
222        buf[2..4].copy_from_slice(&self.channel_id.to_le_bytes());
223        #[cfg(feature = "alloc")]
224        {
225            buf[4..4 + self.length as usize].copy_from_slice(&self.payload[..self.length as usize]);
226        }
227        #[cfg(not(feature = "alloc"))]
228        {
229            buf[4..4 + self.length as usize].copy_from_slice(&self.payload[..self.length as usize]);
230        }
231        Ok(total)
232    }
233
234    /// Parse a PDU from raw buffer
235    #[cfg(feature = "alloc")]
236    pub fn parse(buf: &[u8]) -> Result<Self, KernelError> {
237        if buf.len() < L2CAP_HEADER_SIZE {
238            return Err(KernelError::InvalidArgument {
239                name: "buffer",
240                value: "too short for L2CAP header",
241            });
242        }
243        let length = u16::from_le_bytes([buf[0], buf[1]]);
244        let channel_id = u16::from_le_bytes([buf[2], buf[3]]);
245        if buf.len() < L2CAP_HEADER_SIZE + length as usize {
246            return Err(KernelError::InvalidArgument {
247                name: "buffer",
248                value: "truncated L2CAP payload",
249            });
250        }
251        let payload = Vec::from(&buf[4..4 + length as usize]);
252        Ok(Self {
253            length,
254            channel_id,
255            payload,
256        })
257    }
258}
259
260// ---------------------------------------------------------------------------
261// L2CAP Signaling Command
262// ---------------------------------------------------------------------------
263
264/// L2CAP signaling command (within the signaling channel)
265#[derive(Debug, Clone)]
266pub struct SignalingCommand {
267    /// Command code
268    pub code: u8,
269    /// Identifier (for request/response matching)
270    pub identifier: u8,
271    /// Command data length
272    pub data_length: u16,
273    /// Command-specific data
274    #[cfg(feature = "alloc")]
275    pub data: Vec<u8>,
276    #[cfg(not(feature = "alloc"))]
277    pub data: [u8; 64],
278    #[cfg(not(feature = "alloc"))]
279    pub data_len: usize,
280}
281
282impl SignalingCommand {
283    /// Create a new signaling command
284    #[cfg(feature = "alloc")]
285    pub fn new(code: u8, identifier: u8, data: &[u8]) -> Self {
286        Self {
287            code,
288            identifier,
289            data_length: data.len() as u16,
290            data: Vec::from(data),
291        }
292    }
293
294    /// Create a Connection Request command
295    #[cfg(feature = "alloc")]
296    pub fn connection_request(identifier: u8, psm: u16, source_cid: u16) -> Self {
297        let mut data = [0u8; 4];
298        data[0..2].copy_from_slice(&psm.to_le_bytes());
299        data[2..4].copy_from_slice(&source_cid.to_le_bytes());
300        Self::new(SignalingCode::ConnectionReq as u8, identifier, &data)
301    }
302
303    /// Create a Connection Response command
304    #[cfg(feature = "alloc")]
305    pub fn connection_response(
306        identifier: u8,
307        dest_cid: u16,
308        source_cid: u16,
309        result: ConnectionResult,
310        status: u16,
311    ) -> Self {
312        let mut data = [0u8; 8];
313        data[0..2].copy_from_slice(&dest_cid.to_le_bytes());
314        data[2..4].copy_from_slice(&source_cid.to_le_bytes());
315        data[4..6].copy_from_slice(&(result as u16).to_le_bytes());
316        data[6..8].copy_from_slice(&status.to_le_bytes());
317        Self::new(SignalingCode::ConnectionResp as u8, identifier, &data)
318    }
319
320    /// Create a Configuration Request command
321    #[cfg(feature = "alloc")]
322    pub fn config_request(identifier: u8, dest_cid: u16, mtu: u16) -> Self {
323        // Flags (2 bytes) + MTU option (type=0x01, len=2, value=mtu)
324        let mut data = [0u8; 8];
325        data[0..2].copy_from_slice(&dest_cid.to_le_bytes());
326        data[2..4].copy_from_slice(&0u16.to_le_bytes()); // flags
327        data[4] = 0x01; // MTU option type
328        data[5] = 0x02; // MTU option length
329        data[6..8].copy_from_slice(&mtu.to_le_bytes());
330        Self::new(SignalingCode::ConfigReq as u8, identifier, &data)
331    }
332
333    /// Create a Disconnection Request command
334    #[cfg(feature = "alloc")]
335    pub fn disconnection_request(identifier: u8, dest_cid: u16, source_cid: u16) -> Self {
336        let mut data = [0u8; 4];
337        data[0..2].copy_from_slice(&dest_cid.to_le_bytes());
338        data[2..4].copy_from_slice(&source_cid.to_le_bytes());
339        Self::new(SignalingCode::DisconnReq as u8, identifier, &data)
340    }
341
342    /// Serialize command to buffer, returns bytes written
343    pub fn serialize(&self, buf: &mut [u8]) -> Result<usize, KernelError> {
344        let total = 4 + self.data_length as usize;
345        if buf.len() < total {
346            return Err(KernelError::InvalidArgument {
347                name: "buffer",
348                value: "too small for signaling command",
349            });
350        }
351        buf[0] = self.code;
352        buf[1] = self.identifier;
353        buf[2..4].copy_from_slice(&self.data_length.to_le_bytes());
354        #[cfg(feature = "alloc")]
355        {
356            buf[4..4 + self.data_length as usize]
357                .copy_from_slice(&self.data[..self.data_length as usize]);
358        }
359        #[cfg(not(feature = "alloc"))]
360        {
361            buf[4..4 + self.data_length as usize]
362                .copy_from_slice(&self.data[..self.data_length as usize]);
363        }
364        Ok(total)
365    }
366
367    /// Parse a signaling command from raw buffer
368    #[cfg(feature = "alloc")]
369    pub fn parse(buf: &[u8]) -> Result<Self, KernelError> {
370        if buf.len() < 4 {
371            return Err(KernelError::InvalidArgument {
372                name: "buffer",
373                value: "too short for signaling command header",
374            });
375        }
376        let code = buf[0];
377        let identifier = buf[1];
378        let data_length = u16::from_le_bytes([buf[2], buf[3]]);
379        if buf.len() < 4 + data_length as usize {
380            return Err(KernelError::InvalidArgument {
381                name: "buffer",
382                value: "truncated signaling command data",
383            });
384        }
385        let data = Vec::from(&buf[4..4 + data_length as usize]);
386        Ok(Self {
387            code,
388            identifier,
389            data_length,
390            data,
391        })
392    }
393}
394
395// ---------------------------------------------------------------------------
396// L2CAP Channel State Machine
397// ---------------------------------------------------------------------------
398
399/// L2CAP channel state
400#[derive(Debug, Clone, Copy, PartialEq, Eq)]
401pub enum ChannelState {
402    /// Channel is closed / not allocated
403    Closed,
404    /// Waiting for local connect decision
405    WaitConnect,
406    /// Connection request sent, awaiting response
407    WaitConnectRsp,
408    /// Configuration phase (both sides exchange config)
409    Config,
410    /// Channel is open and data can be transferred
411    Open,
412    /// Disconnection request sent, awaiting response
413    WaitDisconnect,
414}
415
416// ---------------------------------------------------------------------------
417// L2CAP Channel
418// ---------------------------------------------------------------------------
419
420/// An L2CAP logical channel
421#[derive(Debug, Clone)]
422pub struct L2capChannel {
423    /// Local Channel ID
424    pub local_cid: u16,
425    /// Remote Channel ID (assigned by peer)
426    pub remote_cid: u16,
427    /// Current channel state
428    pub state: ChannelState,
429    /// Protocol/Service Multiplexer (identifies upper layer)
430    pub psm: u16,
431    /// Maximum Transmission Unit (negotiated)
432    pub mtu: u16,
433    /// Remote peer's MTU
434    pub remote_mtu: u16,
435    /// Flush timeout in milliseconds (0xFFFF = infinite)
436    pub flush_timeout: u16,
437    /// Whether local configuration is complete
438    pub local_config_done: bool,
439    /// Whether remote configuration is complete
440    pub remote_config_done: bool,
441    /// HCI connection handle this channel belongs to
442    pub hci_handle: u16,
443    /// Reassembly buffer for incoming fragments
444    #[cfg(feature = "alloc")]
445    pub reassembly_buf: Vec<u8>,
446    /// Expected total length for reassembly
447    pub reassembly_expected: u16,
448}
449
450impl L2capChannel {
451    /// Create a new L2CAP channel
452    pub fn new(local_cid: u16, psm: u16, hci_handle: u16) -> Self {
453        Self {
454            local_cid,
455            remote_cid: 0,
456            state: ChannelState::Closed,
457            psm,
458            mtu: L2CAP_DEFAULT_MTU,
459            remote_mtu: L2CAP_DEFAULT_MTU,
460            flush_timeout: 0xFFFF,
461            local_config_done: false,
462            remote_config_done: false,
463            hci_handle,
464            #[cfg(feature = "alloc")]
465            reassembly_buf: Vec::new(),
466            reassembly_expected: 0,
467        }
468    }
469
470    /// Check if the channel is open for data transfer
471    pub fn is_open(&self) -> bool {
472        self.state == ChannelState::Open
473    }
474
475    /// Check if both sides have completed configuration
476    pub fn is_configured(&self) -> bool {
477        self.local_config_done && self.remote_config_done
478    }
479}
480
481// ---------------------------------------------------------------------------
482// SAR (Segmentation and Reassembly)
483// ---------------------------------------------------------------------------
484
485/// A single SAR fragment
486#[derive(Debug, Clone)]
487pub struct SarFragment {
488    /// Whether this is the first fragment
489    pub is_first: bool,
490    /// Fragment data
491    #[cfg(feature = "alloc")]
492    pub data: Vec<u8>,
493    #[cfg(not(feature = "alloc"))]
494    pub data: [u8; L2CAP_DEFAULT_MTU as usize],
495    #[cfg(not(feature = "alloc"))]
496    pub data_len: usize,
497}
498
499/// Segment data into MTU-sized L2CAP PDU fragments
500///
501/// The first fragment includes the L2CAP header (length + CID).
502/// Subsequent fragments are continuation packets.
503#[cfg(feature = "alloc")]
504pub fn segment_data(channel_id: u16, data: &[u8], mtu: u16) -> Vec<SarFragment> {
505    let mut fragments = Vec::new();
506    let effective_mtu = mtu as usize;
507
508    if data.is_empty() {
509        // Empty payload: single PDU with zero-length body
510        let mut hdr = [0u8; L2CAP_HEADER_SIZE];
511        hdr[0..2].copy_from_slice(&0u16.to_le_bytes());
512        hdr[2..4].copy_from_slice(&channel_id.to_le_bytes());
513        fragments.push(SarFragment {
514            is_first: true,
515            data: Vec::from(&hdr[..]),
516        });
517        return fragments;
518    }
519
520    // First fragment: L2CAP header + as much data as fits in MTU
521    let total_length = data.len() as u16;
522    let first_data_len = effective_mtu
523        .saturating_sub(L2CAP_HEADER_SIZE)
524        .min(data.len());
525    let mut first = Vec::with_capacity(L2CAP_HEADER_SIZE + first_data_len);
526    first.extend_from_slice(&total_length.to_le_bytes());
527    first.extend_from_slice(&channel_id.to_le_bytes());
528    first.extend_from_slice(&data[..first_data_len]);
529    fragments.push(SarFragment {
530        is_first: true,
531        data: first,
532    });
533
534    // Remaining fragments: continuation packets (no L2CAP header)
535    let mut offset = first_data_len;
536    while offset < data.len() {
537        let chunk_len = effective_mtu.min(data.len() - offset);
538        fragments.push(SarFragment {
539            is_first: false,
540            data: Vec::from(&data[offset..offset + chunk_len]),
541        });
542        offset += chunk_len;
543    }
544
545    fragments
546}
547
548/// Reassemble fragments into a complete L2CAP payload
549#[cfg(feature = "alloc")]
550pub fn reassemble_fragments(fragments: &[SarFragment]) -> Result<Vec<u8>, KernelError> {
551    if fragments.is_empty() {
552        return Err(KernelError::InvalidArgument {
553            name: "fragments",
554            value: "empty fragment list",
555        });
556    }
557    if !fragments[0].is_first {
558        return Err(KernelError::InvalidArgument {
559            name: "fragments",
560            value: "first fragment not marked as first",
561        });
562    }
563
564    // First fragment contains L2CAP header
565    let first = &fragments[0].data;
566    if first.len() < L2CAP_HEADER_SIZE {
567        return Err(KernelError::InvalidArgument {
568            name: "first_fragment",
569            value: "too short for L2CAP header",
570        });
571    }
572    let total_length = u16::from_le_bytes([first[0], first[1]]) as usize;
573
574    let mut result = Vec::with_capacity(total_length);
575    // Data portion of first fragment (after header)
576    result.extend_from_slice(&first[L2CAP_HEADER_SIZE..]);
577
578    // Continuation fragments
579    for frag in &fragments[1..] {
580        result.extend_from_slice(&frag.data);
581    }
582
583    if result.len() < total_length {
584        return Err(KernelError::InvalidArgument {
585            name: "fragments",
586            value: "insufficient data for declared length",
587        });
588    }
589
590    result.truncate(total_length);
591    Ok(result)
592}
593
594// ---------------------------------------------------------------------------
595// L2CAP Manager
596// ---------------------------------------------------------------------------
597
598/// L2CAP connection and channel manager
599#[cfg(feature = "alloc")]
600pub struct L2capManager {
601    /// Active channels indexed by local CID
602    channels: BTreeMap<u16, L2capChannel>,
603    /// Next available dynamic CID
604    next_cid: u16,
605    /// Next signaling identifier
606    next_identifier: u8,
607}
608
609#[cfg(feature = "alloc")]
610impl Default for L2capManager {
611    fn default() -> Self {
612        Self::new()
613    }
614}
615
616#[cfg(feature = "alloc")]
617impl L2capManager {
618    /// Create a new L2CAP manager
619    pub fn new() -> Self {
620        Self {
621            channels: BTreeMap::new(),
622            next_cid: CID_DYNAMIC_START,
623            next_identifier: 1,
624        }
625    }
626
627    /// Allocate a new dynamic CID
628    fn alloc_cid(&mut self) -> Result<u16, KernelError> {
629        let start = self.next_cid;
630        loop {
631            let cid = self.next_cid;
632            self.next_cid = if cid == CID_DYNAMIC_END {
633                CID_DYNAMIC_START
634            } else {
635                cid + 1
636            };
637            if !self.channels.contains_key(&cid) {
638                return Ok(cid);
639            }
640            // Wrapped around without finding a free CID
641            if self.next_cid == start {
642                return Err(KernelError::ResourceExhausted {
643                    resource: "L2CAP CIDs",
644                });
645            }
646        }
647    }
648
649    /// Allocate the next signaling identifier
650    fn alloc_identifier(&mut self) -> u8 {
651        let id = self.next_identifier;
652        self.next_identifier = self.next_identifier.wrapping_add(1);
653        if self.next_identifier == 0 {
654            self.next_identifier = 1; // 0 is reserved
655        }
656        id
657    }
658
659    /// Get number of active channels
660    pub fn channel_count(&self) -> usize {
661        self.channels.len()
662    }
663
664    /// Get a channel by local CID
665    pub fn get_channel(&self, local_cid: u16) -> Option<&L2capChannel> {
666        self.channels.get(&local_cid)
667    }
668
669    /// Get a mutable reference to a channel by local CID
670    pub fn get_channel_mut(&mut self, local_cid: u16) -> Option<&mut L2capChannel> {
671        self.channels.get_mut(&local_cid)
672    }
673
674    /// Open a new L2CAP channel for the given PSM on the given HCI connection
675    ///
676    /// Returns (local_cid, SignalingCommand to send to peer)
677    pub fn open_channel(
678        &mut self,
679        psm: u16,
680        hci_handle: u16,
681    ) -> Result<(u16, SignalingCommand), KernelError> {
682        let local_cid = self.alloc_cid()?;
683        let mut channel = L2capChannel::new(local_cid, psm, hci_handle);
684        channel.state = ChannelState::WaitConnectRsp;
685
686        let identifier = self.alloc_identifier();
687        let cmd = SignalingCommand::connection_request(identifier, psm, local_cid);
688
689        self.channels.insert(local_cid, channel);
690        Ok((local_cid, cmd))
691    }
692
693    /// Close an existing L2CAP channel
694    ///
695    /// Returns a SignalingCommand (Disconnection Request) to send to peer
696    pub fn close_channel(&mut self, local_cid: u16) -> Result<SignalingCommand, KernelError> {
697        let channel = self
698            .channels
699            .get_mut(&local_cid)
700            .ok_or(KernelError::InvalidArgument {
701                name: "local_cid",
702                value: "channel not found",
703            })?;
704
705        if channel.state == ChannelState::Closed {
706            return Err(KernelError::InvalidState {
707                expected: "not Closed",
708                actual: "Closed",
709            });
710        }
711
712        let remote_cid = channel.remote_cid;
713        channel.state = ChannelState::WaitDisconnect;
714        let identifier = self.alloc_identifier();
715
716        Ok(SignalingCommand::disconnection_request(
717            identifier, remote_cid, local_cid,
718        ))
719    }
720
721    /// Remove a channel entirely (after disconnection completes)
722    pub fn remove_channel(&mut self, local_cid: u16) -> Option<L2capChannel> {
723        self.channels.remove(&local_cid)
724    }
725
726    /// Send data on an open channel, segmenting into PDU fragments if needed
727    ///
728    /// Returns a list of SAR fragments ready to be sent over HCI ACL
729    pub fn send_data(&self, local_cid: u16, data: &[u8]) -> Result<Vec<SarFragment>, KernelError> {
730        let channel = self
731            .channels
732            .get(&local_cid)
733            .ok_or(KernelError::InvalidArgument {
734                name: "local_cid",
735                value: "channel not found",
736            })?;
737
738        if channel.state != ChannelState::Open {
739            return Err(KernelError::InvalidState {
740                expected: "Open",
741                actual: "not Open",
742            });
743        }
744
745        // Segment using the remote peer's MTU
746        Ok(segment_data(channel.remote_cid, data, channel.remote_mtu))
747    }
748
749    /// Receive and reassemble data for a channel
750    ///
751    /// Feeds a raw fragment into the reassembly buffer. Returns completed
752    /// payload when all fragments have been received.
753    pub fn receive_data(
754        &mut self,
755        local_cid: u16,
756        fragment: &[u8],
757        is_first: bool,
758    ) -> Result<Option<Vec<u8>>, KernelError> {
759        let channel = self
760            .channels
761            .get_mut(&local_cid)
762            .ok_or(KernelError::InvalidArgument {
763                name: "local_cid",
764                value: "channel not found",
765            })?;
766
767        if channel.state != ChannelState::Open {
768            return Err(KernelError::InvalidState {
769                expected: "Open",
770                actual: "not Open",
771            });
772        }
773
774        if is_first {
775            // First fragment contains L2CAP header
776            if fragment.len() < L2CAP_HEADER_SIZE {
777                return Err(KernelError::InvalidArgument {
778                    name: "fragment",
779                    value: "first fragment too short for L2CAP header",
780                });
781            }
782            let total_length = u16::from_le_bytes([fragment[0], fragment[1]]);
783            channel.reassembly_expected = total_length;
784            channel.reassembly_buf.clear();
785            channel
786                .reassembly_buf
787                .extend_from_slice(&fragment[L2CAP_HEADER_SIZE..]);
788        } else {
789            // Continuation fragment
790            channel.reassembly_buf.extend_from_slice(fragment);
791        }
792
793        // Check if reassembly is complete
794        if channel.reassembly_buf.len() >= channel.reassembly_expected as usize {
795            let mut result = Vec::new();
796            core::mem::swap(&mut result, &mut channel.reassembly_buf);
797            result.truncate(channel.reassembly_expected as usize);
798            channel.reassembly_expected = 0;
799            Ok(Some(result))
800        } else {
801            Ok(None)
802        }
803    }
804
805    /// Process an incoming L2CAP signaling command
806    ///
807    /// Returns an optional response command to send back
808    pub fn process_signaling(
809        &mut self,
810        cmd: &SignalingCommand,
811    ) -> Result<Option<SignalingCommand>, KernelError> {
812        let code = SignalingCode::from_u8(cmd.code);
813
814        match code {
815            Some(SignalingCode::ConnectionReq) => self.handle_connection_request(cmd),
816            Some(SignalingCode::ConnectionResp) => self.handle_connection_response(cmd),
817            Some(SignalingCode::ConfigReq) => self.handle_config_request(cmd),
818            Some(SignalingCode::ConfigResp) => self.handle_config_response(cmd),
819            Some(SignalingCode::DisconnReq) => self.handle_disconnection_request(cmd),
820            Some(SignalingCode::DisconnResp) => self.handle_disconnection_response(cmd),
821            Some(SignalingCode::EchoReq) => {
822                // Echo: reflect back with same identifier
823                Ok(Some(SignalingCommand::new(
824                    SignalingCode::EchoResp as u8,
825                    cmd.identifier,
826                    &cmd.data,
827                )))
828            }
829            Some(SignalingCode::InfoReq) => {
830                // Information Request: respond with "not supported"
831                let mut data = [0u8; 4];
832                // Copy info type from request
833                if cmd.data.len() >= 2 {
834                    data[0..2].copy_from_slice(&cmd.data[0..2]);
835                }
836                data[2..4].copy_from_slice(&0x0001u16.to_le_bytes()); // Not supported
837                Ok(Some(SignalingCommand::new(
838                    SignalingCode::InfoResp as u8,
839                    cmd.identifier,
840                    &data,
841                )))
842            }
843            _ => {
844                // Unknown or unhandled: Command Reject
845                let mut data = [0u8; 2];
846                data[0..2].copy_from_slice(&0x0000u16.to_le_bytes()); // Not understood
847                Ok(Some(SignalingCommand::new(
848                    SignalingCode::CommandReject as u8,
849                    cmd.identifier,
850                    &data,
851                )))
852            }
853        }
854    }
855
856    /// Handle incoming Connection Request
857    fn handle_connection_request(
858        &mut self,
859        cmd: &SignalingCommand,
860    ) -> Result<Option<SignalingCommand>, KernelError> {
861        if cmd.data.len() < 4 {
862            return Err(KernelError::InvalidArgument {
863                name: "connection_request",
864                value: "data too short",
865            });
866        }
867        let psm = u16::from_le_bytes([cmd.data[0], cmd.data[1]]);
868        let source_cid = u16::from_le_bytes([cmd.data[2], cmd.data[3]]);
869
870        // Allocate a local CID for this incoming connection
871        match self.alloc_cid() {
872            Ok(local_cid) => {
873                let mut channel = L2capChannel::new(local_cid, psm, 0);
874                channel.remote_cid = source_cid;
875                channel.state = ChannelState::Config;
876                self.channels.insert(local_cid, channel);
877
878                Ok(Some(SignalingCommand::connection_response(
879                    cmd.identifier,
880                    local_cid,
881                    source_cid,
882                    ConnectionResult::Success,
883                    0,
884                )))
885            }
886            Err(_) => Ok(Some(SignalingCommand::connection_response(
887                cmd.identifier,
888                0,
889                source_cid,
890                ConnectionResult::NoResources,
891                0,
892            ))),
893        }
894    }
895
896    /// Handle incoming Connection Response
897    fn handle_connection_response(
898        &mut self,
899        cmd: &SignalingCommand,
900    ) -> Result<Option<SignalingCommand>, KernelError> {
901        if cmd.data.len() < 8 {
902            return Err(KernelError::InvalidArgument {
903                name: "connection_response",
904                value: "data too short",
905            });
906        }
907        let dest_cid = u16::from_le_bytes([cmd.data[0], cmd.data[1]]);
908        let source_cid = u16::from_le_bytes([cmd.data[2], cmd.data[3]]);
909        let result = u16::from_le_bytes([cmd.data[4], cmd.data[5]]);
910
911        // Find the channel by source_cid (our local CID)
912        let channel_mtu = if let Some(channel) = self.channels.get_mut(&source_cid) {
913            if result == ConnectionResult::Success as u16 {
914                channel.remote_cid = dest_cid;
915                channel.state = ChannelState::Config;
916                Some(channel.mtu)
917            } else {
918                channel.state = ChannelState::Closed;
919                None
920            }
921        } else {
922            None
923        };
924
925        if let Some(mtu) = channel_mtu {
926            let id = self.alloc_identifier();
927            return Ok(Some(SignalingCommand::config_request(id, dest_cid, mtu)));
928        }
929        Ok(None)
930    }
931
932    /// Handle incoming Configuration Request
933    fn handle_config_request(
934        &mut self,
935        cmd: &SignalingCommand,
936    ) -> Result<Option<SignalingCommand>, KernelError> {
937        if cmd.data.len() < 4 {
938            return Err(KernelError::InvalidArgument {
939                name: "config_request",
940                value: "data too short",
941            });
942        }
943        let dest_cid = u16::from_le_bytes([cmd.data[0], cmd.data[1]]);
944        // flags at [2..4]
945
946        // Parse MTU option if present
947        let mut remote_mtu = L2CAP_DEFAULT_MTU;
948        if cmd.data.len() >= 8 && cmd.data[4] == 0x01 && cmd.data[5] == 0x02 {
949            remote_mtu = u16::from_le_bytes([cmd.data[6], cmd.data[7]]);
950            if remote_mtu < L2CAP_MIN_MTU {
951                remote_mtu = L2CAP_MIN_MTU;
952            }
953        }
954
955        if let Some(channel) = self.channels.get_mut(&dest_cid) {
956            channel.remote_mtu = remote_mtu;
957            channel.remote_config_done = true;
958
959            // Transition to Open if both sides configured
960            if channel.local_config_done && channel.remote_config_done {
961                channel.state = ChannelState::Open;
962            }
963
964            // Config Response: success
965            let mut resp_data = [0u8; 6];
966            resp_data[0..2].copy_from_slice(&channel.remote_cid.to_le_bytes());
967            resp_data[2..4].copy_from_slice(&0u16.to_le_bytes()); // flags
968            resp_data[4..6].copy_from_slice(&(ConfigResult::Success as u16).to_le_bytes());
969            Ok(Some(SignalingCommand::new(
970                SignalingCode::ConfigResp as u8,
971                cmd.identifier,
972                &resp_data,
973            )))
974        } else {
975            Ok(None)
976        }
977    }
978
979    /// Handle incoming Configuration Response
980    fn handle_config_response(
981        &mut self,
982        cmd: &SignalingCommand,
983    ) -> Result<Option<SignalingCommand>, KernelError> {
984        if cmd.data.len() < 6 {
985            return Err(KernelError::InvalidArgument {
986                name: "config_response",
987                value: "data too short",
988            });
989        }
990        let source_cid = u16::from_le_bytes([cmd.data[0], cmd.data[1]]);
991        // flags at [2..4]
992        let result = u16::from_le_bytes([cmd.data[4], cmd.data[5]]);
993
994        // Find channel by remote_cid matching source_cid
995        for channel in self.channels.values_mut() {
996            if channel.remote_cid == source_cid {
997                if result == ConfigResult::Success as u16 {
998                    channel.local_config_done = true;
999                    if channel.local_config_done && channel.remote_config_done {
1000                        channel.state = ChannelState::Open;
1001                    }
1002                }
1003                break;
1004            }
1005        }
1006        Ok(None)
1007    }
1008
1009    /// Handle incoming Disconnection Request
1010    fn handle_disconnection_request(
1011        &mut self,
1012        cmd: &SignalingCommand,
1013    ) -> Result<Option<SignalingCommand>, KernelError> {
1014        if cmd.data.len() < 4 {
1015            return Err(KernelError::InvalidArgument {
1016                name: "disconnection_request",
1017                value: "data too short",
1018            });
1019        }
1020        let dest_cid = u16::from_le_bytes([cmd.data[0], cmd.data[1]]);
1021        let source_cid = u16::from_le_bytes([cmd.data[2], cmd.data[3]]);
1022
1023        if let Some(channel) = self.channels.get_mut(&dest_cid) {
1024            channel.state = ChannelState::Closed;
1025        }
1026
1027        // Respond with Disconnection Response
1028        let mut resp_data = [0u8; 4];
1029        resp_data[0..2].copy_from_slice(&dest_cid.to_le_bytes());
1030        resp_data[2..4].copy_from_slice(&source_cid.to_le_bytes());
1031        Ok(Some(SignalingCommand::new(
1032            SignalingCode::DisconnResp as u8,
1033            cmd.identifier,
1034            &resp_data,
1035        )))
1036    }
1037
1038    /// Handle incoming Disconnection Response
1039    fn handle_disconnection_response(
1040        &mut self,
1041        cmd: &SignalingCommand,
1042    ) -> Result<Option<SignalingCommand>, KernelError> {
1043        if cmd.data.len() < 4 {
1044            return Err(KernelError::InvalidArgument {
1045                name: "disconnection_response",
1046                value: "data too short",
1047            });
1048        }
1049        let dest_cid = u16::from_le_bytes([cmd.data[0], cmd.data[1]]);
1050
1051        if let Some(channel) = self.channels.get_mut(&dest_cid) {
1052            channel.state = ChannelState::Closed;
1053        }
1054        Ok(None)
1055    }
1056
1057    /// Configure a channel's MTU and flush timeout
1058    pub fn configure_channel(
1059        &mut self,
1060        local_cid: u16,
1061        mtu: u16,
1062        flush_timeout: u16,
1063    ) -> Result<(), KernelError> {
1064        let channel = self
1065            .channels
1066            .get_mut(&local_cid)
1067            .ok_or(KernelError::InvalidArgument {
1068                name: "local_cid",
1069                value: "channel not found",
1070            })?;
1071
1072        if mtu < L2CAP_MIN_MTU {
1073            return Err(KernelError::InvalidArgument {
1074                name: "mtu",
1075                value: "below minimum L2CAP MTU (48)",
1076            });
1077        }
1078
1079        channel.mtu = mtu;
1080        channel.flush_timeout = flush_timeout;
1081        Ok(())
1082    }
1083}
1084
1085// ---------------------------------------------------------------------------
1086// Tests
1087// ---------------------------------------------------------------------------
1088
1089#[cfg(test)]
1090mod tests {
1091    #[cfg(feature = "alloc")]
1092    #[allow(unused_imports)]
1093    use alloc::vec;
1094
1095    use super::*;
1096
1097    #[test]
1098    fn test_signaling_code_roundtrip() {
1099        assert_eq!(
1100            SignalingCode::from_u8(0x02),
1101            Some(SignalingCode::ConnectionReq)
1102        );
1103        assert_eq!(
1104            SignalingCode::from_u8(0x03),
1105            Some(SignalingCode::ConnectionResp)
1106        );
1107        assert_eq!(SignalingCode::from_u8(0x04), Some(SignalingCode::ConfigReq));
1108        assert_eq!(
1109            SignalingCode::from_u8(0x05),
1110            Some(SignalingCode::ConfigResp)
1111        );
1112        assert_eq!(
1113            SignalingCode::from_u8(0x06),
1114            Some(SignalingCode::DisconnReq)
1115        );
1116        assert_eq!(
1117            SignalingCode::from_u8(0x07),
1118            Some(SignalingCode::DisconnResp)
1119        );
1120        assert_eq!(SignalingCode::from_u8(0x0A), Some(SignalingCode::InfoReq));
1121        assert_eq!(SignalingCode::from_u8(0x0B), Some(SignalingCode::InfoResp));
1122        assert_eq!(SignalingCode::from_u8(0xFF), None);
1123    }
1124
1125    #[test]
1126    fn test_channel_state_defaults() {
1127        let ch = L2capChannel::new(0x0040, PSM_RFCOMM, 1);
1128        assert_eq!(ch.state, ChannelState::Closed);
1129        assert_eq!(ch.mtu, L2CAP_DEFAULT_MTU);
1130        assert_eq!(ch.flush_timeout, 0xFFFF);
1131        assert!(!ch.is_open());
1132        assert!(!ch.is_configured());
1133    }
1134
1135    #[test]
1136    fn test_pdu_serialize_parse() {
1137        let data = [0x01, 0x02, 0x03, 0x04];
1138        let pdu = L2capPdu::new(0x0040, &data);
1139        assert_eq!(pdu.length, 4);
1140        assert_eq!(pdu.channel_id, 0x0040);
1141
1142        let mut buf = [0u8; 32];
1143        let written = pdu.serialize(&mut buf).unwrap();
1144        assert_eq!(written, 8); // 4 header + 4 data
1145        assert_eq!(u16::from_le_bytes([buf[0], buf[1]]), 4);
1146        assert_eq!(u16::from_le_bytes([buf[2], buf[3]]), 0x0040);
1147    }
1148
1149    #[cfg(feature = "alloc")]
1150    #[test]
1151    fn test_pdu_parse_roundtrip() {
1152        let data = [0xAA, 0xBB, 0xCC];
1153        let pdu = L2capPdu::new(0x0041, &data);
1154        let mut buf = [0u8; 32];
1155        let written = pdu.serialize(&mut buf).unwrap();
1156        let parsed = L2capPdu::parse(&buf[..written]).unwrap();
1157        assert_eq!(parsed.length, 3);
1158        assert_eq!(parsed.channel_id, 0x0041);
1159        assert_eq!(parsed.payload, data);
1160    }
1161
1162    #[test]
1163    fn test_pdu_serialize_buffer_too_small() {
1164        let data = [0u8; 10];
1165        let pdu = L2capPdu::new(0x0040, &data);
1166        let mut buf = [0u8; 8]; // needs 14
1167        assert!(pdu.serialize(&mut buf).is_err());
1168    }
1169
1170    #[cfg(feature = "alloc")]
1171    #[test]
1172    fn test_signaling_command_serialize_parse() {
1173        let cmd = SignalingCommand::connection_request(1, PSM_RFCOMM, 0x0040);
1174        let mut buf = [0u8; 32];
1175        let written = cmd.serialize(&mut buf).unwrap();
1176        assert_eq!(written, 8); // 4 header + 4 data
1177
1178        let parsed = SignalingCommand::parse(&buf[..written]).unwrap();
1179        assert_eq!(parsed.code, SignalingCode::ConnectionReq as u8);
1180        assert_eq!(parsed.identifier, 1);
1181        assert_eq!(parsed.data_length, 4);
1182    }
1183
1184    #[cfg(feature = "alloc")]
1185    #[test]
1186    fn test_segment_small_data() {
1187        let data = [0x01, 0x02, 0x03];
1188        let fragments = segment_data(0x0040, &data, 672);
1189        assert_eq!(fragments.len(), 1);
1190        assert!(fragments[0].is_first);
1191        // First fragment: 4 header + 3 data
1192        assert_eq!(fragments[0].data.len(), 7);
1193    }
1194
1195    #[cfg(feature = "alloc")]
1196    #[test]
1197    fn test_segment_large_data() {
1198        // Data larger than MTU should produce multiple fragments
1199        let mtu = 10u16;
1200        let data = [0xAA; 20];
1201        let fragments = segment_data(0x0040, &data, mtu);
1202        assert!(fragments.len() > 1);
1203        assert!(fragments[0].is_first);
1204        for frag in &fragments[1..] {
1205            assert!(!frag.is_first);
1206        }
1207    }
1208
1209    #[cfg(feature = "alloc")]
1210    #[test]
1211    fn test_segment_empty_data() {
1212        let fragments = segment_data(0x0040, &[], 672);
1213        assert_eq!(fragments.len(), 1);
1214        assert!(fragments[0].is_first);
1215        assert_eq!(fragments[0].data.len(), 4); // header only
1216    }
1217
1218    #[cfg(feature = "alloc")]
1219    #[test]
1220    fn test_reassemble_single_fragment() {
1221        let data = [0x01, 0x02, 0x03];
1222        let fragments = segment_data(0x0040, &data, 672);
1223        let result = reassemble_fragments(&fragments).unwrap();
1224        assert_eq!(result, data);
1225    }
1226
1227    #[cfg(feature = "alloc")]
1228    #[test]
1229    fn test_reassemble_multiple_fragments() {
1230        let data = [0xAA; 20];
1231        let fragments = segment_data(0x0040, &data, 10);
1232        let result = reassemble_fragments(&fragments).unwrap();
1233        assert_eq!(result, data);
1234    }
1235
1236    #[cfg(feature = "alloc")]
1237    #[test]
1238    fn test_manager_open_close_channel() {
1239        let mut mgr = L2capManager::new();
1240        let (cid, cmd) = mgr.open_channel(PSM_RFCOMM, 1).unwrap();
1241        assert!(cid >= CID_DYNAMIC_START);
1242        assert_eq!(cmd.code, SignalingCode::ConnectionReq as u8);
1243        assert_eq!(mgr.channel_count(), 1);
1244
1245        let channel = mgr.get_channel(cid).unwrap();
1246        assert_eq!(channel.state, ChannelState::WaitConnectRsp);
1247        assert_eq!(channel.psm, PSM_RFCOMM);
1248
1249        // Close
1250        let disc_cmd = mgr.close_channel(cid).unwrap();
1251        assert_eq!(disc_cmd.code, SignalingCode::DisconnReq as u8);
1252    }
1253
1254    #[cfg(feature = "alloc")]
1255    #[test]
1256    fn test_manager_configure_channel() {
1257        let mut mgr = L2capManager::new();
1258        let (cid, _) = mgr.open_channel(PSM_SDP, 1).unwrap();
1259
1260        mgr.configure_channel(cid, 1024, 5000).unwrap();
1261        let ch = mgr.get_channel(cid).unwrap();
1262        assert_eq!(ch.mtu, 1024);
1263        assert_eq!(ch.flush_timeout, 5000);
1264    }
1265
1266    #[cfg(feature = "alloc")]
1267    #[test]
1268    fn test_manager_configure_mtu_too_small() {
1269        let mut mgr = L2capManager::new();
1270        let (cid, _) = mgr.open_channel(PSM_SDP, 1).unwrap();
1271        assert!(mgr.configure_channel(cid, 10, 0xFFFF).is_err());
1272    }
1273
1274    #[cfg(feature = "alloc")]
1275    #[test]
1276    fn test_manager_process_echo_request() {
1277        let mut mgr = L2capManager::new();
1278        let echo = SignalingCommand::new(SignalingCode::EchoReq as u8, 5, &[0x01, 0x02]);
1279        let resp = mgr.process_signaling(&echo).unwrap().unwrap();
1280        assert_eq!(resp.code, SignalingCode::EchoResp as u8);
1281        assert_eq!(resp.identifier, 5);
1282        assert_eq!(resp.data, echo.data);
1283    }
1284}
veridian_kernel/drivers/bluetooth/l2cap.rs

veridian_kernel/drivers/bluetooth/
l2cap.rs
