1 /* Copyright (c) 2022 Nordic Semiconductor ASA
2 * SPDX-License-Identifier: Apache-2.0
3 */
4
5 #include <errno.h>
6 #include <string.h>
7
8 #include <zephyr/sys/byteorder.h>
9
10 #include "psa/crypto.h"
11
12 #include "common/bt_str.h"
13 #include "bt_crypto.h"
14
15 #define LOG_LEVEL CONFIG_BT_CRYPTO_LOG_LEVEL
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_REGISTER(bt_crypto);
18
bt_crypto_f4(const uint8_t * u,const uint8_t * v,const uint8_t * x,uint8_t z,uint8_t res[16])19 int bt_crypto_f4(const uint8_t *u, const uint8_t *v, const uint8_t *x, uint8_t z, uint8_t res[16])
20 {
21 uint8_t xs[16];
22 uint8_t m[65];
23 int err;
24
25 LOG_DBG("u %s", bt_hex(u, 32));
26 LOG_DBG("v %s", bt_hex(v, 32));
27 LOG_DBG("x %s z 0x%x", bt_hex(x, 16), z);
28
29 /*
30 * U, V and Z are concatenated and used as input m to the function
31 * AES-CMAC and X is used as the key k.
32 *
33 * Core Spec 4.2 Vol 3 Part H 2.2.5
34 *
35 * note:
36 * bt_smp_aes_cmac uses BE data and smp_f4 accept LE so we swap
37 */
38 sys_memcpy_swap(m, u, 32);
39 sys_memcpy_swap(m + 32, v, 32);
40 m[64] = z;
41
42 sys_memcpy_swap(xs, x, 16);
43
44 err = bt_crypto_aes_cmac(xs, m, sizeof(m), res);
45 if (err) {
46 return err;
47 }
48
49 sys_mem_swap(res, 16);
50
51 LOG_DBG("res %s", bt_hex(res, 16));
52
53 return err;
54 }
55
bt_crypto_f5(const uint8_t * w,const uint8_t * n1,const uint8_t * n2,const bt_addr_le_t * a1,const bt_addr_le_t * a2,uint8_t * mackey,uint8_t * ltk)56 int bt_crypto_f5(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, const bt_addr_le_t *a1,
57 const bt_addr_le_t *a2, uint8_t *mackey, uint8_t *ltk)
58 {
59 static const uint8_t salt[16] = {0x6c, 0x88, 0x83, 0x91, 0xaa, 0xf5, 0xa5, 0x38,
60 0x60, 0x37, 0x0b, 0xdb, 0x5a, 0x60, 0x83, 0xbe};
61 uint8_t m[53] = {0x00, /* counter */
62 0x62, 0x74, 0x6c, 0x65, /* keyID */
63 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /*n1*/
64 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
65 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /*2*/
66 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
67 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* a1 */
68 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* a2 */
69 0x01, 0x00 /* length */};
70 uint8_t t[16], ws[32];
71 int err;
72
73 LOG_DBG("w %s", bt_hex(w, 32));
74 LOG_DBG("n1 %s", bt_hex(n1, 16));
75 LOG_DBG("n2 %s", bt_hex(n2, 16));
76
77 sys_memcpy_swap(ws, w, 32);
78
79 err = bt_crypto_aes_cmac(salt, ws, 32, t);
80 if (err) {
81 return err;
82 }
83
84 LOG_DBG("t %s", bt_hex(t, 16));
85
86 sys_memcpy_swap(m + 5, n1, 16);
87 sys_memcpy_swap(m + 21, n2, 16);
88 m[37] = a1->type;
89 sys_memcpy_swap(m + 38, a1->a.val, 6);
90 m[44] = a2->type;
91 sys_memcpy_swap(m + 45, a2->a.val, 6);
92
93 err = bt_crypto_aes_cmac(t, m, sizeof(m), mackey);
94 if (err) {
95 return err;
96 }
97
98 LOG_DBG("mackey %1s", bt_hex(mackey, 16));
99
100 sys_mem_swap(mackey, 16);
101
102 /* counter for ltk is 1 */
103 m[0] = 0x01;
104
105 err = bt_crypto_aes_cmac(t, m, sizeof(m), ltk);
106 if (err) {
107 return err;
108 }
109
110 LOG_DBG("ltk %s", bt_hex(ltk, 16));
111
112 sys_mem_swap(ltk, 16);
113
114 return 0;
115 }
116
bt_crypto_f6(const uint8_t * w,const uint8_t * n1,const uint8_t * n2,const uint8_t * r,const uint8_t * iocap,const bt_addr_le_t * a1,const bt_addr_le_t * a2,uint8_t * check)117 int bt_crypto_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2, const uint8_t *r,
118 const uint8_t *iocap, const bt_addr_le_t *a1, const bt_addr_le_t *a2,
119 uint8_t *check)
120 {
121 uint8_t ws[16];
122 uint8_t m[65];
123 int err;
124
125 LOG_DBG("w %s", bt_hex(w, 16));
126 LOG_DBG("n1 %s", bt_hex(n1, 16));
127 LOG_DBG("n2 %s", bt_hex(n2, 16));
128 LOG_DBG("r %s", bt_hex(r, 16));
129 LOG_DBG("io_cap %s", bt_hex(iocap, 3));
130 LOG_DBG("a1 %s", bt_hex(a1, 7));
131 LOG_DBG("a2 %s", bt_hex(a2, 7));
132
133 sys_memcpy_swap(m, n1, 16);
134 sys_memcpy_swap(m + 16, n2, 16);
135 sys_memcpy_swap(m + 32, r, 16);
136 sys_memcpy_swap(m + 48, iocap, 3);
137
138 m[51] = a1->type;
139 memcpy(m + 52, a1->a.val, 6);
140 sys_memcpy_swap(m + 52, a1->a.val, 6);
141
142 m[58] = a2->type;
143 memcpy(m + 59, a2->a.val, 6);
144 sys_memcpy_swap(m + 59, a2->a.val, 6);
145
146 sys_memcpy_swap(ws, w, 16);
147
148 err = bt_crypto_aes_cmac(ws, m, sizeof(m), check);
149 if (err) {
150 return err;
151 }
152
153 LOG_DBG("res %s", bt_hex(check, 16));
154
155 sys_mem_swap(check, 16);
156
157 return 0;
158 }
159
bt_crypto_g2(const uint8_t u[32],const uint8_t v[32],const uint8_t x[16],const uint8_t y[16],uint32_t * passkey)160 int bt_crypto_g2(const uint8_t u[32], const uint8_t v[32], const uint8_t x[16], const uint8_t y[16],
161 uint32_t *passkey)
162 {
163 uint8_t m[80], xs[16];
164 int err;
165
166 LOG_DBG("u %s", bt_hex(u, 32));
167 LOG_DBG("v %s", bt_hex(v, 32));
168 LOG_DBG("x %s", bt_hex(x, 16));
169 LOG_DBG("y %s", bt_hex(y, 16));
170
171 sys_memcpy_swap(m, u, 32);
172 sys_memcpy_swap(m + 32, v, 32);
173 sys_memcpy_swap(m + 64, y, 16);
174
175 sys_memcpy_swap(xs, x, 16);
176
177 /* reuse xs (key) as buffer for result */
178 err = bt_crypto_aes_cmac(xs, m, sizeof(m), xs);
179 if (err) {
180 return err;
181 }
182 LOG_DBG("res %s", bt_hex(xs, 16));
183
184 memcpy(passkey, xs + 12, 4);
185 *passkey = sys_be32_to_cpu(*passkey) % 1000000;
186
187 LOG_DBG("passkey %u", *passkey);
188
189 return 0;
190 }
191
bt_crypto_h6(const uint8_t w[16],const uint8_t key_id[4],uint8_t res[16])192 int bt_crypto_h6(const uint8_t w[16], const uint8_t key_id[4], uint8_t res[16])
193 {
194 uint8_t ws[16];
195 uint8_t key_id_s[4];
196 int err;
197
198 LOG_DBG("w %s", bt_hex(w, 16));
199 LOG_DBG("key_id %s", bt_hex(key_id, 4));
200
201 sys_memcpy_swap(ws, w, 16);
202 sys_memcpy_swap(key_id_s, key_id, 4);
203
204 err = bt_crypto_aes_cmac(ws, key_id_s, 4, res);
205 if (err) {
206 return err;
207 }
208
209 LOG_DBG("res %s", bt_hex(res, 16));
210
211 sys_mem_swap(res, 16);
212
213 return 0;
214 }
215
bt_crypto_h7(const uint8_t salt[16],const uint8_t w[16],uint8_t res[16])216 int bt_crypto_h7(const uint8_t salt[16], const uint8_t w[16], uint8_t res[16])
217 {
218 uint8_t ws[16];
219 uint8_t salt_s[16];
220 int err;
221
222 LOG_DBG("w %s", bt_hex(w, 16));
223 LOG_DBG("salt %s", bt_hex(salt, 16));
224
225 sys_memcpy_swap(ws, w, 16);
226 sys_memcpy_swap(salt_s, salt, 16);
227
228 err = bt_crypto_aes_cmac(salt_s, ws, 16, res);
229 if (err) {
230 return err;
231 }
232
233 LOG_DBG("res %s", bt_hex(res, 16));
234
235 sys_mem_swap(res, 16);
236
237 return 0;
238 }
239
bt_crypto_h8(const uint8_t k[16],const uint8_t s[16],const uint8_t key_id[4],uint8_t res[16])240 int bt_crypto_h8(const uint8_t k[16], const uint8_t s[16], const uint8_t key_id[4], uint8_t res[16])
241 {
242 uint8_t key_id_s[4];
243 uint8_t iks[16];
244 uint8_t ks[16];
245 uint8_t ss[16];
246 int err;
247
248 LOG_DBG("k %s", bt_hex(k, 16));
249 LOG_DBG("s %s", bt_hex(s, 16));
250 LOG_DBG("key_id %s", bt_hex(key_id, 4));
251
252 sys_memcpy_swap(ks, k, 16);
253 sys_memcpy_swap(ss, s, 16);
254
255 err = bt_crypto_aes_cmac(ss, ks, 16, iks);
256 if (err) {
257 return err;
258 }
259
260 sys_memcpy_swap(key_id_s, key_id, 4);
261
262 err = bt_crypto_aes_cmac(iks, key_id_s, 4, res);
263 if (err) {
264 return err;
265 }
266
267 LOG_DBG("res %s", bt_hex(res, 16));
268
269 sys_mem_swap(res, 16);
270
271 return 0;
272 }
273
