1use std::collections::HashMap;
10
11use crate::crypto::{self, Key};
12
13#[derive(Debug, Clone, Copy, PartialEq, Eq)]
15pub enum AccessKey<'a> {
16 App(&'a Key, u8),
18 Device(&'a Key),
20}
21
22fn nonce(device: bool, szmic: bool, seq: u32, src: u16, dst: u16, iv_index: u32) -> [u8; 13] {
24 let mut n = [0u8; 13];
25 n[0] = if device { 0x02 } else { 0x01 };
26 n[1] = u8::from(szmic) << 7;
27 n[2..5].copy_from_slice(&seq.to_be_bytes()[1..]);
28 n[5..7].copy_from_slice(&src.to_be_bytes());
29 n[7..9].copy_from_slice(&dst.to_be_bytes());
30 n[9..13].copy_from_slice(&iv_index.to_be_bytes());
31 n
32}
33
34#[derive(Debug, Clone, Copy, PartialEq, Eq)]
36pub struct Meta {
37 pub seq: u32,
39 pub src: u16,
41 pub dst: u16,
43 pub iv_index: u32,
45 pub label: Option<[u8; 16]>,
47}
48
49pub fn encrypt_access(key: AccessKey<'_>, m: &Meta, payload: &[u8], big_mic: bool) -> Vec<u8> {
52 let (k, device) = match key {
53 AccessKey::App(k, _) => (k, false),
54 AccessKey::Device(k) => (k, true),
55 };
56 let aad = m.label.as_ref().map(|l| &l[..]).unwrap_or(&[]);
57 crypto::ccm_encrypt(
58 k,
59 &nonce(device, big_mic, m.seq, m.src, m.dst, m.iv_index),
60 aad,
61 payload,
62 if big_mic { 8 } else { 4 },
63 )
64}
65
66pub fn decrypt_access(
68 key: AccessKey<'_>,
69 m: &Meta,
70 sealed: &[u8],
71 big_mic: bool,
72) -> Option<Vec<u8>> {
73 let (k, device) = match key {
74 AccessKey::App(k, _) => (k, false),
75 AccessKey::Device(k) => (k, true),
76 };
77 let aad = m.label.as_ref().map(|l| &l[..]).unwrap_or(&[]);
78 crypto::ccm_decrypt(
79 k,
80 &nonce(device, big_mic, m.seq, m.src, m.dst, m.iv_index),
81 aad,
82 sealed,
83 if big_mic { 8 } else { 4 },
84 )
85}
86
87pub const MAX_UNSEGMENTED_ACCESS: usize = 15;
89pub const SEGMENT_SIZE: usize = 12;
91pub const MAX_SEGMENTS: usize = 32;
93
94pub fn segment_access(
99 akf_aid: Option<u8>,
100 big_mic: bool,
101 seq_zero: u32,
102 upper: &[u8],
103 force_segmented: bool,
104) -> Option<Vec<Vec<u8>>> {
105 let header = match akf_aid {
106 Some(aid) => 0x40 | (aid & 0x3f),
107 None => 0x00,
108 };
109 if !force_segmented && !big_mic && upper.len() <= MAX_UNSEGMENTED_ACCESS {
110 let mut p = vec![header];
111 p.extend_from_slice(upper);
112 return Some(vec![p]);
113 }
114 let n = upper.len().div_ceil(SEGMENT_SIZE).max(1);
115 if n > MAX_SEGMENTS {
116 return None;
117 }
118 let seq_zero = (seq_zero & 0x1fff) as u16;
119 Some(
120 upper
121 .chunks(SEGMENT_SIZE)
122 .enumerate()
123 .map(|(o, c)| {
124 let (o, last) = (o as u32, (n - 1) as u32);
125 let bits: u32 =
126 (u32::from(big_mic) << 23) | (u32::from(seq_zero) << 10) | (o << 5) | last;
127 let mut p = vec![0x80 | header];
128 p.extend_from_slice(&bits.to_be_bytes()[1..]);
129 p.extend_from_slice(c);
130 p
131 })
132 .collect(),
133 )
134}
135
136#[derive(Debug, Clone, PartialEq, Eq)]
138pub enum Lower {
139 Access {
141 aid: Option<u8>,
143 upper: Vec<u8>,
145 },
146 Segment {
148 aid: Option<u8>,
150 big_mic: bool,
152 seq_zero: u16,
154 seg_o: u8,
156 seg_n: u8,
158 data: Vec<u8>,
160 },
161 Control {
163 opcode: u8,
165 params: Vec<u8>,
167 },
168 Ack {
170 obo: bool,
172 seq_zero: u16,
174 block: u32,
176 },
177}
178
179pub fn parse(ctl: bool, p: &[u8]) -> Option<Lower> {
181 let (&h, rest) = p.split_first()?;
182 let seg = h & 0x80 != 0;
183 match (ctl, seg) {
184 (false, false) => Some(Lower::Access {
185 aid: (h & 0x40 != 0).then_some(h & 0x3f),
186 upper: rest.to_vec(),
187 }),
188 (false, true) => {
189 if rest.len() < 4 {
190 return None;
191 }
192 let bits = u32::from_be_bytes([0, rest[0], rest[1], rest[2]]);
193 let (seg_o, seg_n) = (((bits >> 5) & 0x1f) as u8, (bits & 0x1f) as u8);
194 if seg_o > seg_n {
195 return None;
196 }
197 Some(Lower::Segment {
198 aid: (h & 0x40 != 0).then_some(h & 0x3f),
199 big_mic: bits & 0x80_0000 != 0,
200 seq_zero: ((bits >> 10) & 0x1fff) as u16,
201 seg_o,
202 seg_n,
203 data: rest[3..].to_vec(),
204 })
205 }
206 (true, false) if h & 0x7f == 0 => {
207 if rest.len() != 6 {
208 return None;
209 }
210 let v = u16::from_be_bytes([rest[0], rest[1]]);
211 Some(Lower::Ack {
212 obo: v & 0x8000 != 0,
213 seq_zero: (v >> 2) & 0x1fff,
214 block: u32::from_be_bytes([rest[2], rest[3], rest[4], rest[5]]),
215 })
216 }
217 (true, false) => Some(Lower::Control {
218 opcode: h & 0x7f,
219 params: rest.to_vec(),
220 }),
221 (true, true) => None,
223 }
224}
225
226pub fn ack(seq_zero: u16, block: u32) -> Vec<u8> {
228 let v = (seq_zero & 0x1fff) << 2;
229 let mut p = vec![0x00];
230 p.extend_from_slice(&v.to_be_bytes());
231 p.extend_from_slice(&block.to_be_bytes());
232 p
233}
234
235#[derive(Debug, Clone, PartialEq, Eq)]
237pub enum Reassembled {
238 Partial {
240 block: u32,
242 },
243 Complete {
245 aid: Option<u8>,
247 big_mic: bool,
249 upper: Vec<u8>,
251 block: u32,
253 },
254}
255
256struct Incoming {
257 seq_zero: u16,
258 aid: Option<u8>,
259 big_mic: bool,
260 seg_n: u8,
261 parts: Vec<Option<Vec<u8>>>,
262}
263
264#[derive(Default)]
267pub struct Reassembly {
268 incoming: HashMap<u16, Incoming>,
269}
270
271impl Reassembly {
272 pub fn add(&mut self, src: u16, s: &Lower) -> Option<Reassembled> {
274 let Lower::Segment {
275 aid,
276 big_mic,
277 seq_zero,
278 seg_o,
279 seg_n,
280 data,
281 } = s
282 else {
283 return None;
284 };
285 let e = self.incoming.entry(src).or_insert_with(|| Incoming {
286 seq_zero: *seq_zero,
287 aid: *aid,
288 big_mic: *big_mic,
289 seg_n: *seg_n,
290 parts: vec![None; usize::from(*seg_n) + 1],
291 });
292 if e.seq_zero != *seq_zero || e.seg_n != *seg_n {
293 *e = Incoming {
294 seq_zero: *seq_zero,
295 aid: *aid,
296 big_mic: *big_mic,
297 seg_n: *seg_n,
298 parts: vec![None; usize::from(*seg_n) + 1],
299 };
300 }
301 if (*seg_o < *seg_n && data.len() != SEGMENT_SIZE)
303 || data.is_empty()
304 || data.len() > SEGMENT_SIZE
305 {
306 return None;
307 }
308 e.parts[usize::from(*seg_o)] = Some(data.clone());
309 let block = e
310 .parts
311 .iter()
312 .enumerate()
313 .filter(|(_, p)| p.is_some())
314 .fold(0u32, |b, (i, _)| b | (1 << i));
315 if e.parts.iter().all(Option::is_some) {
316 let e = self.incoming.remove(&src)?;
317 let upper = e.parts.into_iter().flatten().flatten().collect();
318 return Some(Reassembled::Complete {
319 aid: e.aid,
320 big_mic: e.big_mic,
321 upper,
322 block,
323 });
324 }
325 Some(Reassembled::Partial { block })
326 }
327}
328
329#[cfg(test)]
330mod tests {
331 use super::*;
332
333 #[test]
334 fn access_encryption_round_trips_with_either_key_and_mic() {
335 let app = [3u8; 16];
336 let dev = [4u8; 16];
337 let m = Meta {
338 seq: 0x3129ab,
339 src: 0x0003,
340 dst: 0x1201,
341 iv_index: 0x12345678,
342 label: None,
343 };
344 for key in [
345 AccessKey::App(&app, crypto::k4(&app)),
346 AccessKey::Device(&dev),
347 ] {
348 for big in [false, true] {
349 let c = encrypt_access(key, &m, b"\x82\x02\x01", big);
350 assert_eq!(decrypt_access(key, &m, &c, big).unwrap(), b"\x82\x02\x01");
351 let other = Meta {
352 seq: m.seq + 1,
353 ..m
354 };
355 assert!(
356 decrypt_access(key, &other, &c, big).is_none(),
357 "the nonce binds the sequence"
358 );
359 }
360 }
361 }
362
363 #[test]
364 fn segments_and_reassembles_out_of_order() {
365 let upper: Vec<u8> = (0..30u8).collect();
366 let segs = segment_access(Some(0x26), false, 0x1234, &upper, false).unwrap();
367 assert_eq!(segs.len(), 3);
368 let parsed: Vec<Lower> = segs.iter().map(|p| parse(false, p).unwrap()).collect();
369 let mut r = Reassembly::default();
370 assert_eq!(
371 r.add(7, &parsed[2]),
372 Some(Reassembled::Partial { block: 0b100 })
373 );
374 assert_eq!(
375 r.add(7, &parsed[0]),
376 Some(Reassembled::Partial { block: 0b101 })
377 );
378 assert_eq!(
379 r.add(7, &parsed[1]),
380 Some(Reassembled::Complete {
381 aid: Some(0x26),
382 big_mic: false,
383 upper,
384 block: 0b111
385 })
386 );
387 let one = segment_access(None, false, 1, &[1, 2, 3], false).unwrap();
389 assert_eq!(
390 parse(false, &one[0]),
391 Some(Lower::Access {
392 aid: None,
393 upper: vec![1, 2, 3]
394 })
395 );
396 assert!(segment_access(None, false, 1, &[0; 12 * 33], false).is_none());
397 }
398
399 #[test]
400 fn acks_round_trip() {
401 let p = ack(0x1234, 0b1011);
402 assert_eq!(
403 parse(true, &p),
404 Some(Lower::Ack {
405 obo: false,
406 seq_zero: 0x1234,
407 block: 0b1011
408 })
409 );
410 assert!(parse(true, &p[..4]).is_none());
411 assert!(parse(false, &[]).is_none());
412 }
413}