1 /*
2 * Copyright (c) 2023 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdint.h>
8
9 #include <zephyr/kernel.h>
10
11 #include <zephyr/sys/util.h>
12 #include <zephyr/sys/printk.h>
13
14 #include <zephyr/bluetooth/ead.h>
15 #include <zephyr/bluetooth/gap.h>
16 #include <zephyr/bluetooth/addr.h>
17 #include <zephyr/bluetooth/conn.h>
18 #include <zephyr/bluetooth/gatt.h>
19 #include <zephyr/bluetooth/uuid.h>
20 #include <zephyr/bluetooth/hci.h>
21 #include <zephyr/bluetooth/bluetooth.h>
22
23 #include <zephyr/logging/log.h>
24
25 #include "data.h"
26
27 LOG_MODULE_REGISTER(ead_peripheral_sample, CONFIG_BT_EAD_LOG_LEVEL);
28
29 static struct bt_le_ext_adv_cb adv_cb;
30 static struct bt_conn_cb peripheral_cb;
31 static struct bt_conn_auth_cb peripheral_auth_cb;
32
33 static struct bt_conn *default_conn;
34
35 static struct k_poll_signal disconn_signal;
36 static struct k_poll_signal passkey_enter_signal;
37
read_key_material(struct bt_conn * conn,const struct bt_gatt_attr * attr,void * buf,uint16_t len,uint16_t offset)38 static ssize_t read_key_material(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf,
39 uint16_t len, uint16_t offset)
40 {
41 return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data,
42 sizeof(struct key_material));
43 }
44
45 BT_GATT_SERVICE_DEFINE(key_mat, BT_GATT_PRIMARY_SERVICE(BT_UUID_CUSTOM_SERVICE),
46 BT_GATT_CHARACTERISTIC(BT_UUID_GATT_EDKM, BT_GATT_CHRC_READ,
47 BT_GATT_PERM_READ_AUTHEN, read_key_material, NULL,
48 &mk));
49
update_ad_data(struct bt_le_ext_adv * adv)50 static int update_ad_data(struct bt_le_ext_adv *adv)
51 {
52 int err;
53 size_t offset;
54
55 /* Encrypt ad structure 1 */
56
57 size_t size_ad_1 = BT_DATA_SERIALIZED_SIZE(ad[1].data_len);
58 uint8_t ad_1[size_ad_1];
59 size_t size_ead_1 = BT_EAD_ENCRYPTED_PAYLOAD_SIZE(size_ad_1);
60 uint8_t ead_1[size_ead_1];
61
62 bt_data_serialize(&ad[1], ad_1);
63
64 err = bt_ead_encrypt(mk.session_key, mk.iv, ad_1, size_ad_1, ead_1);
65 if (err != 0) {
66 LOG_ERR("Error during first encryption");
67 return -1;
68 }
69
70 /* Encrypt ad structures 3 and 4 */
71
72 size_t size_ad_3_4 =
73 BT_DATA_SERIALIZED_SIZE(ad[3].data_len) + BT_DATA_SERIALIZED_SIZE(ad[4].data_len);
74 uint8_t ad_3_4[size_ad_3_4];
75 size_t size_ead_2 = BT_EAD_ENCRYPTED_PAYLOAD_SIZE(size_ad_3_4);
76 uint8_t ead_2[size_ead_2];
77
78 offset = bt_data_serialize(&ad[3], &ad_3_4[0]);
79 bt_data_serialize(&ad[4], &ad_3_4[offset]);
80
81 err = bt_ead_encrypt(mk.session_key, mk.iv, ad_3_4, size_ad_3_4, ead_2);
82 if (err != 0) {
83 LOG_ERR("Error during second encryption");
84 return -1;
85 }
86
87 /* Concatenate and update the advertising data */
88 struct bt_data ad_structs[] = {
89 ad[0],
90 BT_DATA(BT_DATA_ENCRYPTED_AD_DATA, ead_1, size_ead_1),
91 ad[2],
92 BT_DATA(BT_DATA_ENCRYPTED_AD_DATA, ead_2, size_ead_2),
93 };
94
95 LOG_INF("Advertising data size: %zu", bt_data_get_len(ad_structs, ARRAY_SIZE(ad_structs)));
96
97 err = bt_le_ext_adv_set_data(adv, ad_structs, ARRAY_SIZE(ad_structs), NULL, 0);
98 if (err) {
99 LOG_ERR("Failed to set advertising data (%d)", err);
100 return -1;
101 }
102
103 LOG_DBG("Advertising Data Updated");
104
105 return 0;
106 }
107
set_ad_data(struct bt_le_ext_adv * adv)108 static int set_ad_data(struct bt_le_ext_adv *adv)
109 {
110 return update_ad_data(adv);
111 }
112
rpa_expired_cb(struct bt_le_ext_adv * adv)113 static bool rpa_expired_cb(struct bt_le_ext_adv *adv)
114 {
115 LOG_DBG("RPA expired");
116
117 /* The Bluetooth Core Specification say that the Randomizer and thus the
118 * Advertising Data shall be updated each time the address is changed.
119 *
120 * ref:
121 * Supplement to the Bluetooth Core Specification | v11, Part A 1.23.4
122 */
123 update_ad_data(adv);
124
125 return true;
126 }
127
create_adv(struct bt_le_ext_adv ** adv)128 static int create_adv(struct bt_le_ext_adv **adv)
129 {
130 int err;
131 struct bt_le_adv_param params;
132
133 memset(¶ms, 0, sizeof(struct bt_le_adv_param));
134
135 params.options |= BT_LE_ADV_OPT_CONNECTABLE;
136 params.options |= BT_LE_ADV_OPT_EXT_ADV;
137
138 params.id = BT_ID_DEFAULT;
139 params.sid = 0;
140 params.interval_min = BT_GAP_ADV_FAST_INT_MIN_2;
141 params.interval_max = BT_GAP_ADV_FAST_INT_MAX_2;
142
143 adv_cb.rpa_expired = rpa_expired_cb;
144
145 err = bt_le_ext_adv_create(¶ms, &adv_cb, adv);
146 if (err) {
147 LOG_ERR("Failed to create advertiser (%d)", err);
148 return -1;
149 }
150
151 return 0;
152 }
153
start_adv(struct bt_le_ext_adv * adv)154 static int start_adv(struct bt_le_ext_adv *adv)
155 {
156 int err;
157 int32_t timeout = 0;
158 uint8_t num_events = 0;
159
160 struct bt_le_ext_adv_start_param start_params;
161
162 start_params.timeout = timeout;
163 start_params.num_events = num_events;
164
165 err = bt_le_ext_adv_start(adv, &start_params);
166 if (err) {
167 LOG_ERR("Failed to start advertiser (%d)", err);
168 return -1;
169 }
170
171 LOG_DBG("Advertiser started");
172
173 return 0;
174 }
175
stop_and_delete_adv(struct bt_le_ext_adv * adv)176 static int stop_and_delete_adv(struct bt_le_ext_adv *adv)
177 {
178 int err;
179
180 err = bt_le_ext_adv_stop(adv);
181 if (err) {
182 LOG_ERR("Failed to stop advertiser (err %d)", err);
183 return -1;
184 }
185
186 err = bt_le_ext_adv_delete(adv);
187 if (err) {
188 LOG_ERR("Failed to delete advertiser (err %d)", err);
189 return -1;
190 }
191
192 return 0;
193 }
194
connected(struct bt_conn * conn,uint8_t err)195 static void connected(struct bt_conn *conn, uint8_t err)
196 {
197 char addr[BT_ADDR_LE_STR_LEN];
198
199 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
200
201 if (err) {
202 LOG_ERR("Failed to connect to %s %u %s", addr, err, bt_hci_err_to_str(err));
203 return;
204 }
205
206 LOG_DBG("Connected to %s", addr);
207
208 default_conn = conn;
209 }
210
disconnected(struct bt_conn * conn,uint8_t reason)211 static void disconnected(struct bt_conn *conn, uint8_t reason)
212 {
213 char addr[BT_ADDR_LE_STR_LEN];
214
215 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
216
217 LOG_DBG("Disconnected from %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));
218
219 k_poll_signal_raise(&disconn_signal, 0);
220 }
221
security_changed(struct bt_conn * conn,bt_security_t level,enum bt_security_err err)222 static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
223 {
224 char addr[BT_ADDR_LE_STR_LEN];
225
226 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
227
228 if (!err) {
229 LOG_DBG("Security changed: %s level %u", addr, level);
230 } else {
231 LOG_DBG("Security failed: %s level %u err %s(%d)", addr, level,
232 bt_security_err_to_str(err), err);
233 }
234 }
235
auth_passkey_confirm(struct bt_conn * conn,unsigned int passkey)236 static void auth_passkey_confirm(struct bt_conn *conn, unsigned int passkey)
237 {
238 char passkey_str[7];
239 char addr[BT_ADDR_LE_STR_LEN];
240
241 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
242
243 snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);
244
245 printk("Passkey for %s: %s\n", addr, passkey_str);
246
247 k_poll_signal_raise(&passkey_enter_signal, 0);
248 }
249
auth_passkey_display(struct bt_conn * conn,unsigned int passkey)250 static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
251 {
252 char passkey_str[7];
253 char addr[BT_ADDR_LE_STR_LEN];
254
255 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
256
257 snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);
258
259 LOG_DBG("Passkey for %s: %s", addr, passkey_str);
260 }
261
auth_cancel(struct bt_conn * conn)262 static void auth_cancel(struct bt_conn *conn)
263 {
264 char addr[BT_ADDR_LE_STR_LEN];
265
266 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
267
268 LOG_DBG("Pairing cancelled: %s", addr);
269 }
270
init_bt(void)271 static int init_bt(void)
272 {
273 int err;
274
275 k_poll_signal_init(&disconn_signal);
276 k_poll_signal_init(&passkey_enter_signal);
277
278 err = bt_enable(NULL);
279 if (err) {
280 LOG_ERR("Bluetooth init failed (err %d)", err);
281 return -1;
282 }
283
284 LOG_DBG("Bluetooth initialized");
285
286 err = bt_unpair(BT_ID_DEFAULT, BT_ADDR_LE_ANY);
287 if (err) {
288 LOG_ERR("Unpairing failed (err %d)", err);
289 }
290
291 peripheral_cb.connected = connected;
292 peripheral_cb.disconnected = disconnected;
293 peripheral_cb.security_changed = security_changed;
294
295 bt_conn_cb_register(&peripheral_cb);
296
297 peripheral_auth_cb.pairing_confirm = NULL;
298 peripheral_auth_cb.passkey_confirm = auth_passkey_confirm;
299 peripheral_auth_cb.passkey_display = auth_passkey_display;
300 peripheral_auth_cb.passkey_entry = NULL;
301 peripheral_auth_cb.oob_data_request = NULL;
302 peripheral_auth_cb.cancel = auth_cancel;
303
304 err = bt_conn_auth_cb_register(&peripheral_auth_cb);
305 if (err) {
306 LOG_ERR("Failed to register bt_conn_auth_cb (err %d)", err);
307 return -1;
308 }
309
310 return 0;
311 }
312
run_peripheral_sample(int get_passkey_confirmation (struct bt_conn * conn))313 int run_peripheral_sample(int get_passkey_confirmation(struct bt_conn *conn))
314 {
315 int err;
316 struct bt_le_ext_adv *adv = NULL;
317
318 err = init_bt();
319 if (err) {
320 return -1;
321 }
322
323 /* Setup advertiser */
324 err = create_adv(&adv);
325 if (err) {
326 return -2;
327 }
328
329 err = start_adv(adv);
330 if (err) {
331 return -3;
332 }
333
334 err = set_ad_data(adv);
335 if (err) {
336 return -4;
337 }
338
339 /* Wait for the peer to update security */
340 await_signal(&passkey_enter_signal);
341
342 err = get_passkey_confirmation(default_conn);
343 if (err) {
344 LOG_ERR("Failure during security update");
345 return -5;
346 }
347
348 /* Wait for the peer to disconnect */
349 await_signal(&disconn_signal);
350
351 /* Restart advertising */
352 err = start_adv(adv);
353 if (err) {
354 return -3;
355 }
356
357 err = set_ad_data(adv);
358 if (err) {
359 return -4;
360 }
361
362 /* Wait 10s before stopping and deleting the advertiser */
363 k_sleep(K_SECONDS(10));
364
365 err = stop_and_delete_adv(adv);
366 if (err) {
367 return -6;
368 }
369
370 return 0;
371 }
372
