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
132 adv_cb.rpa_expired = rpa_expired_cb;
133
134 err = bt_le_ext_adv_create(BT_LE_EXT_ADV_CONN, &adv_cb, adv);
135 if (err) {
136 LOG_ERR("Failed to create advertiser (%d)", err);
137 return -1;
138 }
139
140 return 0;
141 }
142
start_adv(struct bt_le_ext_adv * adv)143 static int start_adv(struct bt_le_ext_adv *adv)
144 {
145 int err;
146 int32_t timeout = 0;
147 uint8_t num_events = 0;
148
149 struct bt_le_ext_adv_start_param start_params;
150
151 start_params.timeout = timeout;
152 start_params.num_events = num_events;
153
154 err = bt_le_ext_adv_start(adv, &start_params);
155 if (err) {
156 LOG_ERR("Failed to start advertiser (%d)", err);
157 return -1;
158 }
159
160 LOG_DBG("Advertiser started");
161
162 return 0;
163 }
164
stop_and_delete_adv(struct bt_le_ext_adv * adv)165 static int stop_and_delete_adv(struct bt_le_ext_adv *adv)
166 {
167 int err;
168
169 err = bt_le_ext_adv_stop(adv);
170 if (err) {
171 LOG_ERR("Failed to stop advertiser (err %d)", err);
172 return -1;
173 }
174
175 err = bt_le_ext_adv_delete(adv);
176 if (err) {
177 LOG_ERR("Failed to delete advertiser (err %d)", err);
178 return -1;
179 }
180
181 return 0;
182 }
183
connected(struct bt_conn * conn,uint8_t err)184 static void connected(struct bt_conn *conn, uint8_t err)
185 {
186 char addr[BT_ADDR_LE_STR_LEN];
187
188 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
189
190 if (err) {
191 LOG_ERR("Failed to connect to %s %u %s", addr, err, bt_hci_err_to_str(err));
192 return;
193 }
194
195 LOG_DBG("Connected to %s", addr);
196
197 default_conn = conn;
198 }
199
disconnected(struct bt_conn * conn,uint8_t reason)200 static void disconnected(struct bt_conn *conn, uint8_t reason)
201 {
202 char addr[BT_ADDR_LE_STR_LEN];
203
204 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
205
206 LOG_DBG("Disconnected from %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));
207
208 k_poll_signal_raise(&disconn_signal, 0);
209 }
210
security_changed(struct bt_conn * conn,bt_security_t level,enum bt_security_err err)211 static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
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 if (!err) {
218 LOG_DBG("Security changed: %s level %u", addr, level);
219 } else {
220 LOG_DBG("Security failed: %s level %u err %s(%d)", addr, level,
221 bt_security_err_to_str(err), err);
222 }
223 }
224
auth_passkey_confirm(struct bt_conn * conn,unsigned int passkey)225 static void auth_passkey_confirm(struct bt_conn *conn, unsigned int passkey)
226 {
227 char passkey_str[7];
228 char addr[BT_ADDR_LE_STR_LEN];
229
230 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
231
232 snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);
233
234 printk("Passkey for %s: %s\n", addr, passkey_str);
235
236 k_poll_signal_raise(&passkey_enter_signal, 0);
237 }
238
auth_passkey_display(struct bt_conn * conn,unsigned int passkey)239 static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
240 {
241 char passkey_str[7];
242 char addr[BT_ADDR_LE_STR_LEN];
243
244 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
245
246 snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);
247
248 LOG_DBG("Passkey for %s: %s", addr, passkey_str);
249 }
250
auth_cancel(struct bt_conn * conn)251 static void auth_cancel(struct bt_conn *conn)
252 {
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 LOG_DBG("Pairing cancelled: %s", addr);
258 }
259
init_bt(void)260 static int init_bt(void)
261 {
262 int err;
263
264 k_poll_signal_init(&disconn_signal);
265 k_poll_signal_init(&passkey_enter_signal);
266
267 err = bt_enable(NULL);
268 if (err) {
269 LOG_ERR("Bluetooth init failed (err %d)", err);
270 return -1;
271 }
272
273 LOG_DBG("Bluetooth initialized");
274
275 err = bt_unpair(BT_ID_DEFAULT, BT_ADDR_LE_ANY);
276 if (err) {
277 LOG_ERR("Unpairing failed (err %d)", err);
278 }
279
280 peripheral_cb.connected = connected;
281 peripheral_cb.disconnected = disconnected;
282 peripheral_cb.security_changed = security_changed;
283
284 bt_conn_cb_register(&peripheral_cb);
285
286 peripheral_auth_cb.pairing_confirm = NULL;
287 peripheral_auth_cb.passkey_confirm = auth_passkey_confirm;
288 peripheral_auth_cb.passkey_display = auth_passkey_display;
289 peripheral_auth_cb.passkey_entry = NULL;
290 peripheral_auth_cb.oob_data_request = NULL;
291 peripheral_auth_cb.cancel = auth_cancel;
292
293 err = bt_conn_auth_cb_register(&peripheral_auth_cb);
294 if (err) {
295 LOG_ERR("Failed to register bt_conn_auth_cb (err %d)", err);
296 return -1;
297 }
298
299 return 0;
300 }
301
run_peripheral_sample(int get_passkey_confirmation (struct bt_conn * conn))302 int run_peripheral_sample(int get_passkey_confirmation(struct bt_conn *conn))
303 {
304 int err;
305 struct bt_le_ext_adv *adv = NULL;
306
307 err = init_bt();
308 if (err) {
309 return -1;
310 }
311
312 /* Setup advertiser */
313 err = create_adv(&adv);
314 if (err) {
315 return -2;
316 }
317
318 err = start_adv(adv);
319 if (err) {
320 return -3;
321 }
322
323 err = set_ad_data(adv);
324 if (err) {
325 return -4;
326 }
327
328 /* Wait for the peer to update security */
329 await_signal(&passkey_enter_signal);
330
331 err = get_passkey_confirmation(default_conn);
332 if (err) {
333 LOG_ERR("Failure during security update");
334 return -5;
335 }
336
337 /* Wait for the peer to disconnect */
338 await_signal(&disconn_signal);
339
340 /* Restart advertising */
341 err = start_adv(adv);
342 if (err) {
343 return -3;
344 }
345
346 err = set_ad_data(adv);
347 if (err) {
348 return -4;
349 }
350
351 /* Wait 10s before stopping and deleting the advertiser */
352 k_sleep(K_SECONDS(10));
353
354 err = stop_and_delete_adv(adv);
355 if (err) {
356 return -6;
357 }
358
359 return 0;
360 }
361
