1use aes::cipher::{BlockCipherEncrypt, KeyInit};
10use aes::Aes128;
11use cmac::Mac;
12use p256::elliptic_curve::sec1::ToSec1Point;
13
14pub fn random_bytes(buf: &mut [u8]) {
16 getrandom::fill(buf).expect("the operating system's random number generator failed");
18}
19
20pub fn random_u128() -> u128 {
22 let mut b = [0u8; 16];
23 random_bytes(&mut b);
24 u128::from_le_bytes(b)
25}
26
27pub fn le(v: u128) -> [u8; 16] {
29 v.to_le_bytes()
30}
31
32pub fn from_le(b: [u8; 16]) -> u128 {
34 u128::from_le_bytes(b)
35}
36
37pub fn e(key: u128, plaintext: u128) -> u128 {
39 let cipher = Aes128::new(&key.to_be_bytes().into());
40 let mut block = aes::Block::from(plaintext.to_be_bytes());
41 cipher.encrypt_block(&mut block);
42 u128::from_be_bytes(block.into())
43}
44
45fn cmac(key: u128, msg: &[u8]) -> u128 {
46 let mut m = <cmac::Cmac<Aes128> as KeyInit>::new(&key.to_be_bytes().into());
47 m.update(msg);
48 u128::from_be_bytes(m.finalize().into_bytes().into())
49}
50
51pub fn ah(irk: u128, prand: u32) -> u32 {
53 (e(irk, u128::from(prand & 0x00ff_ffff)) & 0x00ff_ffff) as u32
54}
55
56pub fn resolves(irk: u128, addr: &[u8; 6]) -> bool {
59 let hash = u32::from_le_bytes([addr[0], addr[1], addr[2], 0]);
60 let prand = u32::from_le_bytes([addr[3], addr[4], addr[5], 0]);
61 prand >> 22 == 0b01 && ah(irk, prand) == hash
62}
63
64pub fn new_rpa(irk: u128) -> [u8; 6] {
66 let mut p = [0u8; 3];
67 random_bytes(&mut p);
68 p[2] = (p[2] & 0x3f) | 0x40;
70 if p[0] == 0 && p[1] == 0 && p[2] == 0x40 {
71 p[0] = 1;
72 }
73 if p[0] == 0xff && p[1] == 0xff && p[2] == 0x7f {
74 p[0] = 0xfe;
75 }
76 let prand = u32::from_le_bytes([p[0], p[1], p[2], 0]);
77 let hash = ah(irk, prand).to_le_bytes();
78 [hash[0], hash[1], hash[2], p[0], p[1], p[2]]
79}
80
81#[allow(clippy::too_many_arguments)]
87pub fn c1(
88 k: u128,
89 r: u128,
90 preq: &[u8; 7],
91 pres: &[u8; 7],
92 iat: u8,
93 rat: u8,
94 ia: &[u8; 6],
95 ra: &[u8; 6],
96) -> u128 {
97 let mut p1 = [0u8; 16];
98 p1[0] = iat;
99 p1[1] = rat;
100 p1[2..9].copy_from_slice(preq);
101 p1[9..16].copy_from_slice(pres);
102 let mut p2 = [0u8; 16];
103 p2[0..6].copy_from_slice(ra);
104 p2[6..12].copy_from_slice(ia);
105 e(
106 k,
107 e(k, r ^ u128::from_le_bytes(p1)) ^ u128::from_le_bytes(p2),
108 )
109}
110
111pub fn s1(k: u128, r1: u128, r2: u128) -> u128 {
113 let mask = u128::from(u64::MAX);
114 e(k, ((r1 & mask) << 64) | (r2 & mask))
115}
116
117pub fn f4(u: &[u8; 32], v: &[u8; 32], x: u128, z: u8) -> u128 {
119 let mut m = [0u8; 65];
120 m[..32].copy_from_slice(u);
121 m[32..64].copy_from_slice(v);
122 m[64] = z;
123 cmac(x, &m)
124}
125
126pub fn addr56(kind: u8, addr: &[u8; 6]) -> [u8; 7] {
128 let mut a = [0u8; 7];
129 a[0] = kind & 1;
130 for (i, b) in addr.iter().rev().enumerate() {
131 a[1 + i] = *b;
132 }
133 a
134}
135
136pub fn f5(w: &[u8; 32], n1: u128, n2: u128, a1: &[u8; 7], a2: &[u8; 7]) -> (u128, u128) {
138 const SALT: u128 = 0x6c88_8391_aaf5_a538_6037_0bdb_5a60_83be;
139 let t = cmac(SALT, w);
140 let mut m = [0u8; 53];
141 m[1..5].copy_from_slice(&0x6274_6c65u32.to_be_bytes());
142 m[5..21].copy_from_slice(&n1.to_be_bytes());
143 m[21..37].copy_from_slice(&n2.to_be_bytes());
144 m[37..44].copy_from_slice(a1);
145 m[44..51].copy_from_slice(a2);
146 m[51..53].copy_from_slice(&256u16.to_be_bytes());
147 let mac_key = cmac(t, &m);
148 m[0] = 1;
149 let ltk = cmac(t, &m);
150 (mac_key, ltk)
151}
152
153pub fn f6(
155 w: u128,
156 n1: u128,
157 n2: u128,
158 r: u128,
159 io_cap: &[u8; 3],
160 a1: &[u8; 7],
161 a2: &[u8; 7],
162) -> u128 {
163 let mut m = [0u8; 65];
164 m[..16].copy_from_slice(&n1.to_be_bytes());
165 m[16..32].copy_from_slice(&n2.to_be_bytes());
166 m[32..48].copy_from_slice(&r.to_be_bytes());
167 m[48..51].copy_from_slice(io_cap);
168 m[51..58].copy_from_slice(a1);
169 m[58..65].copy_from_slice(a2);
170 cmac(w, &m)
171}
172
173pub fn g2(u: &[u8; 32], v: &[u8; 32], x: u128, y: u128) -> u32 {
176 let mut m = [0u8; 80];
177 m[..32].copy_from_slice(u);
178 m[32..64].copy_from_slice(v);
179 m[64..80].copy_from_slice(&y.to_be_bytes());
180 cmac(x, &m) as u32
181}
182
183pub fn h6(w: u128, key_id: u32) -> u128 {
185 cmac(w, &key_id.to_be_bytes())
186}
187
188pub fn h7(salt: u128, w: u128) -> u128 {
190 cmac(salt, &w.to_be_bytes())
191}
192
193pub const DEBUG_PUBLIC_X: [u8; 32] = [
196 0x20, 0xb0, 0x03, 0xd2, 0xf2, 0x97, 0xbe, 0x2c, 0x5e, 0x2c, 0x83, 0xa7, 0xe9, 0xf9, 0xa5, 0xb9,
197 0xef, 0xf4, 0x91, 0x11, 0xac, 0xf4, 0xfd, 0xdb, 0xcc, 0x03, 0x01, 0x48, 0x0e, 0x35, 0x9d, 0xe6,
198];
199
200pub struct EcdhKey {
202 secret: p256::SecretKey,
203 pub x: [u8; 32],
205 pub y: [u8; 32],
207}
208
209impl EcdhKey {
210 pub fn generate() -> Self {
212 loop {
213 let mut b = [0u8; 32];
214 random_bytes(&mut b);
215 if let Ok(secret) = p256::SecretKey::from_slice(&b) {
216 return Self::from_secret(secret);
217 }
218 }
219 }
220
221 fn from_secret(secret: p256::SecretKey) -> Self {
222 let point = secret.public_key().to_sec1_point(false);
223 let mut x = [0u8; 32];
224 let mut y = [0u8; 32];
225 x.copy_from_slice(point.x().map(|v| v.as_slice()).unwrap_or(&[0; 32]));
226 y.copy_from_slice(point.y().map(|v| v.as_slice()).unwrap_or(&[0; 32]));
227 EcdhKey { secret, x, y }
228 }
229
230 pub fn from_private(d: &[u8; 32]) -> Option<Self> {
232 p256::SecretKey::from_slice(d).ok().map(Self::from_secret)
233 }
234
235 pub fn dh_key(&self, peer_x: &[u8; 32], peer_y: &[u8; 32]) -> Option<[u8; 32]> {
238 let mut sec1 = [0u8; 65];
239 sec1[0] = 0x04;
240 sec1[1..33].copy_from_slice(peer_x);
241 sec1[33..].copy_from_slice(peer_y);
242 let peer = p256::PublicKey::from_sec1_bytes(&sec1).ok()?;
243 let shared = p256::ecdh::diffie_hellman(self.secret.to_nonzero_scalar(), peer.as_affine());
244 let mut out = [0u8; 32];
245 out.copy_from_slice(shared.raw_secret_bytes().as_slice());
246 Some(out)
247 }
248}
249
250#[cfg(test)]
251mod tests {
252 use super::*;
253
254 fn h(s: &str) -> Vec<u8> {
255 hex::decode(s.replace(' ', "")).unwrap()
256 }
257 fn n(s: &str) -> u128 {
258 u128::from_str_radix(&s.replace(' ', ""), 16).unwrap()
259 }
260 fn a32(s: &str) -> [u8; 32] {
261 h(s).try_into().unwrap()
262 }
263
264 const U: &str = "20b003d2 f297be2c 5e2c83a7 e9f9a5b9 eff49111 acf4fddb cc030148 0e359de6";
265 const V: &str = "55188b3d 32f6bb9a 900afcfb eed4e72a 59cb9ac2 f19d7cfb 6b4fdd49 f47fc5fd";
266 const X: &str = "d5cb8454 d177733e ffffb2ec 712baeab";
267 const N2: &str = "a6e8e7cc 25a75f6e 216583f7 ff3dc4cf";
268 const W: &str = "ec0234a3 57c8ad05 341010a6 0a397d9b 99796b13 b4f866f1 868d34f3 73bfa698";
269
270 #[test]
272 fn f4_vector() {
273 assert_eq!(
274 f4(&a32(U), &a32(V), n(X), 0),
275 n("f2c916f1 07a9bd1c f1eda1be a974872d")
276 );
277 }
278
279 #[test]
280 fn f5_vector() {
281 let a1: [u8; 7] = h("00561237 37bfce").try_into().unwrap();
282 let a2: [u8; 7] = h("00a71370 2dcfc1").try_into().unwrap();
283 let (mac, ltk) = f5(&a32(W), n(X), n(N2), &a1, &a2);
284 assert_eq!(mac, n("2965f176 a1084a02 fd3f6a20 ce636e20"));
285 assert_eq!(ltk, n("69867911 69d7cd23 980522b5 94750a38"));
286 }
287
288 #[test]
289 fn f6_vector() {
290 let a1: [u8; 7] = h("00561237 37bfce").try_into().unwrap();
291 let a2: [u8; 7] = h("00a71370 2dcfc1").try_into().unwrap();
292 let v = f6(
293 n("2965f176 a1084a02 fd3f6a20 ce636e20"),
294 n(X),
295 n(N2),
296 n("12a3343b b453bb54 08da42d2 0c2d0fc8"),
297 &[0x01, 0x01, 0x02],
298 &a1,
299 &a2,
300 );
301 assert_eq!(v, n("e3c47398 9cd0e8c5 d26c0b09 da958f61"));
302 }
303
304 #[test]
305 fn g2_vector() {
306 assert_eq!(g2(&a32(U), &a32(V), n(X), n(N2)), 0x2f9e_d5ba);
307 }
308
309 #[test]
310 fn h6_h7_vectors() {
311 let w = n("ec0234a3 57c8ad05 341010a6 0a397d9b");
312 assert_eq!(h6(w, 0x6c65_6272), n("2d9ae102 e76dc91c e8d3a9e2 80b16399"));
313 assert_eq!(h7(0x746d_7031, w), n("fb173597 c6a3c0ec d2998c2a 75a57011"));
314 }
315
316 #[test]
317 fn ah_vector_and_rpa() {
318 let irk = n("ec0234a3 57c8ad05 341010a6 0a397d9b");
319 assert_eq!(ah(irk, 0x70_8194), 0x0d_fbaa);
320 let rpa = new_rpa(irk);
321 assert!(resolves(irk, &rpa));
322 assert!(!resolves(irk ^ 1, &rpa));
323 }
324
325 #[test]
327 fn c1_s1_vectors() {
328 let preq: [u8; 7] = [0x01, 0x01, 0x00, 0x00, 0x10, 0x07, 0x07];
329 let pres: [u8; 7] = [0x02, 0x03, 0x00, 0x00, 0x08, 0x00, 0x05];
330 let ia = [0xa6, 0xa5, 0xa4, 0xa3, 0xa2, 0xa1];
331 let ra = [0xb6, 0xb5, 0xb4, 0xb3, 0xb2, 0xb1];
332 let c = c1(
333 0,
334 n("5783D52156AD6F0E6388274EC6702EE0"),
335 &preq,
336 &pres,
337 1,
338 0,
339 &ia,
340 &ra,
341 );
342 assert_eq!(c, n("1e1e3fef878988ead2a74dc5bef13b86"));
343 let s = s1(
344 0,
345 n("000F0E0D0C0B0A091122334455667788"),
346 n("010203040506070899AABBCCDDEEFF00"),
347 );
348 assert_eq!(s, n("9a1fe1f0e8b0f49b5b4216ae796da062"));
349 }
350
351 #[test]
352 fn ecdh_with_the_debug_key() {
353 let d = a32("3f49f6d4 a3c55f38 74c9b3e3 d2103f50 4aff607b eb40b799 5899b8a6 cd3c1abd");
354 let debug = EcdhKey::from_private(&d).unwrap();
355 assert_eq!(debug.x, DEBUG_PUBLIC_X);
356 let a = EcdhKey::generate();
357 let b = EcdhKey::generate();
358 assert_eq!(a.dh_key(&b.x, &b.y), b.dh_key(&a.x, &a.y));
359 assert!(a.dh_key(&[1; 32], &[2; 32]).is_none());
360 }
361}