stackforge_core/layer/dot11/
data.rs

1//! IEEE 802.11 data frame subtypes and QoS handling.
2//!
3//! Data frames carry the actual payload data in 802.11 networks.
4//! QoS data frames include a 2-byte QoS Control field after the
5//! 802.11 header (and before the payload/security headers).
6
7use crate::layer::field::FieldError;
8
9/// QoS Control field length.
10pub const QOS_CTRL_LEN: usize = 2;
11
12/// LLC/SNAP header length.
13pub const LLC_SNAP_LEN: usize = 8;
14
15/// LLC/SNAP header: AA:AA:03:00:00:00 followed by 2-byte EtherType.
16pub const LLC_SNAP_PREFIX: [u8; 6] = [0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00];
17
18// ============================================================================
19// Dot11QoS
20// ============================================================================
21
22/// 802.11 QoS Control field (2 bytes).
23///
24/// Layout (Scapy bit ordering for byte 0):
25/// ```text
26/// Byte 0: [A-MSDU Present(1) | Ack Policy(2) | EOSP(1) | TID(4)]
27/// Byte 1: TXOP Limit / Queue Size
28/// ```
29///
30/// Present in QoS Data frames (subtype >= 8).
31#[derive(Debug, Clone)]
32pub struct Dot11QoS {
33    pub offset: usize,
34}
35
36impl Dot11QoS {
37    pub fn new(offset: usize) -> Self {
38        Self { offset }
39    }
40
41    /// Validate buffer length.
42    pub fn validate(buf: &[u8], offset: usize) -> Result<(), FieldError> {
43        if buf.len() < offset + QOS_CTRL_LEN {
44            return Err(FieldError::BufferTooShort {
45                offset,
46                need: QOS_CTRL_LEN,
47                have: buf.len().saturating_sub(offset),
48            });
49        }
50        Ok(())
51    }
52
53    /// Raw QoS Control field (2 bytes, little-endian).
54    pub fn raw(&self, buf: &[u8]) -> Result<u16, FieldError> {
55        let off = self.offset;
56        if buf.len() < off + 2 {
57            return Err(FieldError::BufferTooShort {
58                offset: off,
59                need: 2,
60                have: buf.len(),
61            });
62        }
63        Ok(u16::from_le_bytes([buf[off], buf[off + 1]]))
64    }
65
66    /// TID (Traffic Identifier, lower 4 bits of byte 0).
67    pub fn tid(&self, buf: &[u8]) -> Result<u8, FieldError> {
68        let off = self.offset;
69        if buf.len() <= off {
70            return Err(FieldError::BufferTooShort {
71                offset: off,
72                need: 1,
73                have: buf.len(),
74            });
75        }
76        Ok(buf[off] & 0x0F)
77    }
78
79    /// EOSP (End Of Service Period, bit 4 of byte 0).
80    pub fn eosp(&self, buf: &[u8]) -> Result<bool, FieldError> {
81        let off = self.offset;
82        if buf.len() <= off {
83            return Err(FieldError::BufferTooShort {
84                offset: off,
85                need: 1,
86                have: buf.len(),
87            });
88        }
89        Ok(buf[off] & 0x10 != 0)
90    }
91
92    /// Ack Policy (bits 5-6 of byte 0).
93    pub fn ack_policy(&self, buf: &[u8]) -> Result<u8, FieldError> {
94        let off = self.offset;
95        if buf.len() <= off {
96            return Err(FieldError::BufferTooShort {
97                offset: off,
98                need: 1,
99                have: buf.len(),
100            });
101        }
102        Ok((buf[off] >> 5) & 0x03)
103    }
104
105    /// A-MSDU Present flag (bit 7 of byte 0).
106    pub fn a_msdu_present(&self, buf: &[u8]) -> Result<bool, FieldError> {
107        let off = self.offset;
108        if buf.len() <= off {
109            return Err(FieldError::BufferTooShort {
110                offset: off,
111                need: 1,
112                have: buf.len(),
113            });
114        }
115        Ok(buf[off] & 0x80 != 0)
116    }
117
118    /// TXOP Limit or Queue Size (byte 1).
119    pub fn txop(&self, buf: &[u8]) -> Result<u8, FieldError> {
120        let off = self.offset + 1;
121        if buf.len() <= off {
122            return Err(FieldError::BufferTooShort {
123                offset: off,
124                need: 1,
125                have: buf.len(),
126            });
127        }
128        Ok(buf[off])
129    }
130
131    /// Header length.
132    pub fn header_len(&self) -> usize {
133        QOS_CTRL_LEN
134    }
135
136    /// Build QoS Control field bytes.
137    pub fn build(tid: u8, eosp: bool, ack_policy: u8, a_msdu: bool, txop: u8) -> Vec<u8> {
138        let byte0 = (tid & 0x0F)
139            | (if eosp { 0x10 } else { 0 })
140            | ((ack_policy & 0x03) << 5)
141            | (if a_msdu { 0x80 } else { 0 });
142        vec![byte0, txop]
143    }
144}
145
146// ============================================================================
147// LLC/SNAP detection
148// ============================================================================
149
150/// Check if the data at the given offset starts with an LLC/SNAP header.
151pub fn is_llc_snap(buf: &[u8], offset: usize) -> bool {
152    if buf.len() < offset + LLC_SNAP_LEN {
153        return false;
154    }
155    buf[offset..offset + 6] == LLC_SNAP_PREFIX
156}
157
158/// Extract the EtherType from an LLC/SNAP header at the given offset.
159pub fn llc_snap_ethertype(buf: &[u8], offset: usize) -> Result<u16, FieldError> {
160    if buf.len() < offset + LLC_SNAP_LEN {
161        return Err(FieldError::BufferTooShort {
162            offset: offset + 6,
163            need: 2,
164            have: buf.len().saturating_sub(offset + 6),
165        });
166    }
167    Ok(u16::from_be_bytes([buf[offset + 6], buf[offset + 7]]))
168}
169
170// ============================================================================
171// A-MSDU subframe
172// ============================================================================
173
174/// A single A-MSDU subframe.
175///
176/// Layout:
177/// - DA (6 bytes)
178/// - SA (6 bytes)
179/// - Length (2 bytes, big-endian)
180/// - MSDU (variable)
181/// - Padding (0-3 bytes to align to 4-byte boundary)
182#[derive(Debug, Clone)]
183pub struct AMsduSubframe {
184    /// Destination address.
185    pub da: [u8; 6],
186    /// Source address.
187    pub sa: [u8; 6],
188    /// MSDU data.
189    pub data: Vec<u8>,
190}
191
192/// A-MSDU subframe header length: DA(6) + SA(6) + Length(2) = 14.
193pub const AMSDU_SUBFRAME_HEADER_LEN: usize = 14;
194
195impl AMsduSubframe {
196    /// Parse A-MSDU subframes from the given buffer.
197    pub fn parse_all(buf: &[u8], offset: usize) -> Result<Vec<AMsduSubframe>, FieldError> {
198        let mut subframes = Vec::new();
199        let mut pos = offset;
200
201        while pos + AMSDU_SUBFRAME_HEADER_LEN <= buf.len() {
202            let mut da = [0u8; 6];
203            let mut sa = [0u8; 6];
204            da.copy_from_slice(&buf[pos..pos + 6]);
205            sa.copy_from_slice(&buf[pos + 6..pos + 12]);
206            let length = u16::from_be_bytes([buf[pos + 12], buf[pos + 13]]) as usize;
207
208            let data_start = pos + AMSDU_SUBFRAME_HEADER_LEN;
209            let data_end = data_start + length;
210            if data_end > buf.len() {
211                break;
212            }
213
214            subframes.push(AMsduSubframe {
215                da,
216                sa,
217                data: buf[data_start..data_end].to_vec(),
218            });
219
220            // Align to 4-byte boundary
221            pos = data_end;
222            let padding = (4 - (pos % 4)) % 4;
223            pos += padding;
224        }
225
226        Ok(subframes)
227    }
228
229    /// Build an A-MSDU subframe.
230    pub fn build(&self) -> Vec<u8> {
231        let length = self.data.len() as u16;
232        let mut out = Vec::with_capacity(AMSDU_SUBFRAME_HEADER_LEN + self.data.len() + 3);
233        out.extend_from_slice(&self.da);
234        out.extend_from_slice(&self.sa);
235        out.extend_from_slice(&length.to_be_bytes());
236        out.extend_from_slice(&self.data);
237        // Add padding to 4-byte boundary
238        let padding = (4 - (out.len() % 4)) % 4;
239        out.extend(std::iter::repeat(0u8).take(padding));
240        out
241    }
242}
243
244#[cfg(test)]
245mod tests {
246    use super::*;
247
248    #[test]
249    fn test_qos_parse() {
250        let buf = vec![0x03, 0x00];
251        let qos = Dot11QoS::new(0);
252
253        assert_eq!(qos.tid(&buf).unwrap(), 3);
254        assert!(!qos.eosp(&buf).unwrap());
255        assert_eq!(qos.ack_policy(&buf).unwrap(), 0);
256        assert!(!qos.a_msdu_present(&buf).unwrap());
257        assert_eq!(qos.txop(&buf).unwrap(), 0);
258    }
259
260    #[test]
261    fn test_qos_with_eosp_and_amsdu() {
262        // TID=5, EOSP=1, AckPolicy=2, A-MSDU=1
263        // byte0: TID=5(0x05) | EOSP(0x10) | AckPolicy=2(0x40) | A-MSDU(0x80) = 0xD5
264        let buf = vec![0xD5, 0x10];
265        let qos = Dot11QoS::new(0);
266
267        assert_eq!(qos.tid(&buf).unwrap(), 5);
268        assert!(qos.eosp(&buf).unwrap());
269        assert_eq!(qos.ack_policy(&buf).unwrap(), 2);
270        assert!(qos.a_msdu_present(&buf).unwrap());
271        assert_eq!(qos.txop(&buf).unwrap(), 0x10);
272    }
273
274    #[test]
275    fn test_qos_build_roundtrip() {
276        let data = Dot11QoS::build(5, true, 2, true, 0x10);
277        assert_eq!(data.len(), QOS_CTRL_LEN);
278
279        let qos = Dot11QoS::new(0);
280        assert_eq!(qos.tid(&data).unwrap(), 5);
281        assert!(qos.eosp(&data).unwrap());
282        assert_eq!(qos.ack_policy(&data).unwrap(), 2);
283        assert!(qos.a_msdu_present(&data).unwrap());
284        assert_eq!(qos.txop(&data).unwrap(), 0x10);
285    }
286
287    #[test]
288    fn test_llc_snap_detection() {
289        let mut buf = vec![0u8; 8];
290        buf[0..6].copy_from_slice(&LLC_SNAP_PREFIX);
291        buf[6] = 0x08;
292        buf[7] = 0x00;
293
294        assert!(is_llc_snap(&buf, 0));
295        assert_eq!(llc_snap_ethertype(&buf, 0).unwrap(), 0x0800);
296    }
297
298    #[test]
299    fn test_llc_snap_not_present() {
300        let buf = vec![0x00; 8];
301        assert!(!is_llc_snap(&buf, 0));
302    }
303
304    #[test]
305    fn test_amsdu_parse() {
306        let subframe1 = AMsduSubframe {
307            da: [0x01, 0x02, 0x03, 0x04, 0x05, 0x06],
308            sa: [0x11, 0x12, 0x13, 0x14, 0x15, 0x16],
309            data: vec![0xAA, 0xBB, 0xCC],
310        };
311        let built = subframe1.build();
312        assert_eq!(built.len(), 20);
313
314        let parsed = AMsduSubframe::parse_all(&built, 0).unwrap();
315        assert_eq!(parsed.len(), 1);
316        assert_eq!(parsed[0].da, [0x01, 0x02, 0x03, 0x04, 0x05, 0x06]);
317        assert_eq!(parsed[0].sa, [0x11, 0x12, 0x13, 0x14, 0x15, 0x16]);
318        assert_eq!(parsed[0].data, vec![0xAA, 0xBB, 0xCC]);
319    }
320
321    #[test]
322    fn test_amsdu_multiple_subframes() {
323        let sf1 = AMsduSubframe {
324            da: [0x01; 6],
325            sa: [0x02; 6],
326            data: vec![0x10, 0x20],
327        };
328        let sf2 = AMsduSubframe {
329            da: [0x03; 6],
330            sa: [0x04; 6],
331            data: vec![0x30, 0x40, 0x50, 0x60],
332        };
333
334        let mut buf = sf1.build();
335        buf.extend(sf2.build());
336
337        let parsed = AMsduSubframe::parse_all(&buf, 0).unwrap();
338        assert_eq!(parsed.len(), 2);
339        assert_eq!(parsed[0].data, vec![0x10, 0x20]);
340        assert_eq!(parsed[1].data, vec![0x30, 0x40, 0x50, 0x60]);
341    }
342
343    #[test]
344    fn test_qos_header_len() {
345        let qos = Dot11QoS::new(0);
346        assert_eq!(qos.header_len(), 2);
347    }
348
349    #[test]
350    fn test_qos_raw() {
351        let buf = vec![0xD5, 0x10];
352        let qos = Dot11QoS::new(0);
353        assert_eq!(qos.raw(&buf).unwrap(), 0x10D5);
354    }
355}
stackforge_core/layer/dot11/data.rs

stackforge_core/layer/dot11/
data.rs
