1 /*
2 * Copyright (c) 2017 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/sys/printk.h>
9 #include <zephyr/device.h>
10 #include <string.h>
11
12 #include <zephyr/display/mb_display.h>
13
14 #include <zephyr/bluetooth/bluetooth.h>
15 #include <zephyr/bluetooth/uuid.h>
16 #include <zephyr/bluetooth/conn.h>
17 #include <zephyr/bluetooth/gatt.h>
18 #include <zephyr/bluetooth/hci.h>
19
20
21 #include "pong.h"
22
23 #define SCAN_TIMEOUT K_SECONDS(2)
24
25 #define APPEARANCE 0
26
27 #define PONG_SVC_UUID \
28 BT_UUID_128_ENCODE(0xf94fea38, 0x4e24, 0x7ea1, 0x0d4d, 0x6fee0f556c90)
29 #define PONG_CHR_UUID \
30 BT_UUID_128_ENCODE(0xabbf8f1c, 0xc56a, 0x82b5, 0xc640, 0x2ccdd7af94dd)
31
32 static const struct bt_uuid_128 pong_svc_uuid = BT_UUID_INIT_128(PONG_SVC_UUID);
33 static const struct bt_uuid_128 pong_chr_uuid = BT_UUID_INIT_128(PONG_CHR_UUID);
34 static const struct bt_uuid *gatt_ccc_uuid = BT_UUID_GATT_CCC;
35
36 static struct bt_gatt_discover_params discov_param;
37 static struct bt_gatt_subscribe_params subscribe_param;
38
39 static const struct bt_data ad[] = {
40 BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
41 BT_DATA_BYTES(BT_DATA_UUID128_ALL, PONG_SVC_UUID),
42 };
43
44 static const struct bt_data sd[] = {
45 BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
46 };
47
48 static struct bt_conn *default_conn;
49
50 static const struct bt_gatt_attr *local_attr;
51 static uint16_t remote_handle;
52 static bool remote_ready;
53 static bool initiator;
54
55 static struct k_work_delayable ble_work;
56
57 static bool connect_canceled;
58
59 static enum {
60 BLE_DISCONNECTED,
61 BLE_SCAN_START,
62 BLE_SCAN,
63 BLE_CONNECT_CREATE,
64 BLE_CONNECT_CANCEL,
65 BLE_ADV_START,
66 BLE_ADVERTISING,
67 BLE_CONNECTED,
68 } ble_state;
69
70 enum {
71 BLE_BALL_INFO = 0x00,
72 BLE_LOST = 0x01,
73 };
74
75 struct ble_ball_info {
76 int8_t x_pos;
77 int8_t y_pos;
78 int8_t x_vel;
79 int8_t y_vel;
80 } __packed;
81
82 struct ble_data {
83 uint8_t op;
84 union {
85 struct ble_ball_info ball;
86 };
87 } __packed;
88
89 #define BALL_INFO_LEN (1 + sizeof(struct ble_ball_info))
90
ble_send_ball(int8_t x_pos,int8_t y_pos,int8_t x_vel,int8_t y_vel)91 void ble_send_ball(int8_t x_pos, int8_t y_pos, int8_t x_vel, int8_t y_vel)
92 {
93 struct ble_data data = {
94 .op = BLE_BALL_INFO,
95 .ball.x_pos = x_pos,
96 .ball.y_pos = y_pos,
97 .ball.x_vel = x_vel,
98 .ball.y_vel = y_vel,
99 };
100 int err;
101
102 if (!default_conn || !remote_ready) {
103 printk("ble_send_ball(): not ready\n");
104 return;
105 }
106
107 printk("ble_send_ball(%d, %d, %d, %d)\n", x_pos, y_pos, x_vel, y_vel);
108
109 err = bt_gatt_notify(default_conn, local_attr, &data, BALL_INFO_LEN);
110 if (err) {
111 printk("GATT notify failed (err %d)\n", err);
112 }
113 }
114
ble_send_lost(void)115 void ble_send_lost(void)
116 {
117 uint8_t lost = BLE_LOST;
118 int err;
119
120 if (!default_conn || !remote_ready) {
121 printk("ble_send_lost(): not ready\n");
122 return;
123 }
124
125 err = bt_gatt_notify(default_conn, local_attr, &lost, sizeof(lost));
126 if (err) {
127 printk("GATT notify failed (err %d)\n", err);
128 }
129 }
130
notify_func(struct bt_conn * conn,struct bt_gatt_subscribe_params * param,const void * buf,uint16_t len)131 static uint8_t notify_func(struct bt_conn *conn,
132 struct bt_gatt_subscribe_params *param,
133 const void *buf, uint16_t len)
134 {
135 const struct ble_data *data = buf;
136
137 printk("notify_func() data %p len %u\n", data, len);
138
139 if (!data || !len) {
140 printk("Unsubscribed, disconnecting...\n");
141 remote_handle = 0U;
142 if (default_conn) {
143 bt_conn_disconnect(default_conn,
144 BT_HCI_ERR_REMOTE_USER_TERM_CONN);
145 }
146 return BT_GATT_ITER_STOP;
147 }
148
149 switch (data->op) {
150 case BLE_BALL_INFO:
151 if (len < BALL_INFO_LEN) {
152 printk("Too small ball info\n");
153 break;
154 }
155
156 pong_ball_received(data->ball.x_pos, data->ball.y_pos,
157 data->ball.x_vel, data->ball.y_vel);
158 break;
159 case BLE_LOST:
160 pong_remote_lost();
161 break;
162 default:
163 printk("Unknown op 0x%02x\n", data->op);
164 }
165
166 return BT_GATT_ITER_CONTINUE;
167 }
168
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * param)169 static uint8_t discover_func(struct bt_conn *conn,
170 const struct bt_gatt_attr *attr,
171 struct bt_gatt_discover_params *param)
172 {
173 int err;
174
175 if (!attr) {
176 printk("Discover complete\n");
177 (void)memset(&discov_param, 0, sizeof(discov_param));
178 return BT_GATT_ITER_STOP;
179 }
180
181 printk("Attribute handle %u\n", attr->handle);
182
183 if (param->uuid == &pong_svc_uuid.uuid) {
184 printk("Pong service discovered\n");
185 discov_param.uuid = &pong_chr_uuid.uuid;
186 discov_param.start_handle = attr->handle + 1;
187 discov_param.type = BT_GATT_DISCOVER_CHARACTERISTIC;
188
189 err = bt_gatt_discover(conn, &discov_param);
190 if (err) {
191 printk("Char Discovery failed (err %d)\n", err);
192 }
193 } else if (param->uuid == &pong_chr_uuid.uuid) {
194 printk("Pong characteristic discovered\n");
195 discov_param.uuid = gatt_ccc_uuid;
196 discov_param.start_handle = attr->handle + 2;
197 discov_param.type = BT_GATT_DISCOVER_DESCRIPTOR;
198 subscribe_param.value_handle = attr->handle + 1;
199
200 err = bt_gatt_discover(conn, &discov_param);
201 if (err) {
202 printk("CCC Discovery failed (err %d)\n", err);
203 }
204 } else {
205 printk("Pong CCC discovered\n");
206
207 subscribe_param.notify = notify_func;
208 subscribe_param.value = BT_GATT_CCC_NOTIFY;
209 subscribe_param.ccc_handle = attr->handle;
210
211 printk("CCC handle 0x%04x Value handle 0x%04x\n",
212 subscribe_param.ccc_handle,
213 subscribe_param.value_handle);
214
215 err = bt_gatt_subscribe(conn, &subscribe_param);
216 if (err && err != -EALREADY) {
217 printk("Subscribe failed (err %d)\n", err);
218 } else {
219 printk("Subscribed\n");
220 }
221
222 remote_handle = attr->handle;
223 }
224
225 if (remote_handle && remote_ready) {
226 pong_conn_ready(initiator);
227 }
228
229 return BT_GATT_ITER_STOP;
230 }
231
connected(struct bt_conn * conn,uint8_t err)232 static void connected(struct bt_conn *conn, uint8_t err)
233 {
234 struct bt_conn_info info;
235
236 if (err) {
237 printk("Connection failed, err 0x%02x %s\n", err, bt_hci_err_to_str(err));
238 return;
239 }
240
241 if (ble_state == BLE_ADVERTISING) {
242 bt_le_adv_stop();
243 }
244
245 if (!default_conn) {
246 default_conn = bt_conn_ref(conn);
247 }
248
249 bt_conn_get_info(conn, &info);
250 initiator = (info.role == BT_CONN_ROLE_CENTRAL);
251 remote_ready = false;
252 remote_handle = 0U;
253
254 printk("Connected\n");
255 ble_state = BLE_CONNECTED;
256
257 k_work_reschedule(&ble_work, K_NO_WAIT);
258 }
259
disconnected(struct bt_conn * conn,uint8_t reason)260 static void disconnected(struct bt_conn *conn, uint8_t reason)
261 {
262 printk("Disconnected, reason 0x%02x %s\n", reason, bt_hci_err_to_str(reason));
263
264 if (default_conn) {
265 bt_conn_unref(default_conn);
266 default_conn = NULL;
267 }
268
269 remote_handle = 0U;
270
271 if (ble_state == BLE_CONNECTED) {
272 ble_state = BLE_DISCONNECTED;
273 pong_remote_disconnected();
274 }
275 }
276
277 BT_CONN_CB_DEFINE(conn_callbacks) = {
278 .connected = connected,
279 .disconnected = disconnected,
280 };
281
ble_connect(void)282 void ble_connect(void)
283 {
284 if (ble_state != BLE_DISCONNECTED) {
285 printk("Not ready to connect\n");
286 return;
287 }
288
289 ble_state = BLE_SCAN_START;
290 k_work_reschedule(&ble_work, K_NO_WAIT);
291 }
292
ble_cancel_connect(void)293 void ble_cancel_connect(void)
294 {
295 printk("ble_cancel_connect()\n");
296
297 switch (ble_state) {
298 case BLE_SCAN_START:
299 ble_state = BLE_DISCONNECTED;
300 __fallthrough;
301 case BLE_DISCONNECTED:
302 /* If this fails, the handler will run without doing anything,
303 * as the switch case for BLE_DISCONNECTED is empty.
304 */
305 k_work_cancel_delayable(&ble_work);
306 break;
307 case BLE_SCAN:
308 connect_canceled = true;
309 k_work_reschedule(&ble_work, K_NO_WAIT);
310 break;
311 case BLE_ADV_START:
312 ble_state = BLE_DISCONNECTED;
313 break;
314 case BLE_ADVERTISING:
315 connect_canceled = true;
316 k_work_reschedule(&ble_work, K_NO_WAIT);
317 break;
318 case BLE_CONNECT_CREATE:
319 ble_state = BLE_CONNECT_CANCEL;
320 __fallthrough;
321 case BLE_CONNECTED:
322 connect_canceled = true;
323 k_work_reschedule(&ble_work, K_NO_WAIT);
324 break;
325 case BLE_CONNECT_CANCEL:
326 break;
327 }
328 }
329
pong_uuid_match(const uint8_t * data,uint8_t len)330 static bool pong_uuid_match(const uint8_t *data, uint8_t len)
331 {
332 while (len >= 16U) {
333 if (!memcmp(data, pong_svc_uuid.val, 16)) {
334 return true;
335 }
336
337 len -= 16U;
338 data += 16;
339 }
340
341 return false;
342 }
343
create_conn(const bt_addr_le_t * addr)344 static void create_conn(const bt_addr_le_t *addr)
345 {
346 int err;
347
348 if (default_conn) {
349 return;
350 }
351
352 printk("Found matching device, initiating connection...\n");
353
354 err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
355 BT_LE_CONN_PARAM_DEFAULT, &default_conn);
356 if (err) {
357 printk("Failed to initiate connection");
358 return;
359 }
360
361 ble_state = BLE_CONNECT_CREATE;
362 k_work_reschedule(&ble_work, SCAN_TIMEOUT);
363 }
364
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad_buf)365 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
366 struct net_buf_simple *ad_buf)
367 {
368 if (type != BT_GAP_ADV_TYPE_ADV_IND) {
369 return;
370 }
371
372 while (ad_buf->len > 1) {
373 uint8_t len = net_buf_simple_pull_u8(ad_buf);
374 uint8_t ad_type;
375
376 /* Check for early termination */
377 if (len == 0U) {
378 return;
379 }
380
381 if (len > ad_buf->len) {
382 printk("AD malformed\n");
383 return;
384 }
385
386 ad_type = net_buf_simple_pull_u8(ad_buf);
387 if (ad_type == BT_DATA_UUID128_ALL &&
388 pong_uuid_match(ad_buf->data, len - 1)) {
389 bt_le_scan_stop();
390 create_conn(addr);
391 return;
392 }
393
394 net_buf_simple_pull(ad_buf, len - 1);
395 }
396 }
397
adv_timeout(void)398 static uint32_t adv_timeout(void)
399 {
400 uint32_t timeout;
401
402 if (bt_rand(&timeout, sizeof(timeout)) < 0) {
403 return 10 * MSEC_PER_SEC;
404 }
405
406 timeout %= (10 * MSEC_PER_SEC);
407
408 return timeout + (1 * MSEC_PER_SEC);
409 }
410
cancel_connect(void)411 static void cancel_connect(void)
412 {
413 connect_canceled = false;
414
415 switch (ble_state) {
416 case BLE_SCAN:
417 bt_le_scan_stop();
418 break;
419 case BLE_ADVERTISING:
420 bt_le_adv_stop();
421 break;
422 case BLE_CONNECT_CREATE:
423 case BLE_CONNECTED:
424 bt_conn_disconnect(default_conn,
425 BT_HCI_ERR_REMOTE_USER_TERM_CONN);
426 break;
427 default:
428 break;
429 }
430
431 /* For CONNECTED the state will be updated in the disconnected cb */
432 if (ble_state != BLE_CONNECTED) {
433 ble_state = BLE_DISCONNECTED;
434 }
435 }
436
ble_timeout(struct k_work * work)437 static void ble_timeout(struct k_work *work)
438 {
439 int err;
440
441 if (connect_canceled) {
442 cancel_connect();
443 return;
444 }
445
446 switch (ble_state) {
447 case BLE_DISCONNECTED:
448 break;
449 case BLE_SCAN_START:
450 err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, device_found);
451 if (err) {
452 printk("Scanning failed to start (err %d)\n", err);
453 }
454
455 printk("Started scanning for devices\n");
456 ble_state = BLE_SCAN;
457 k_work_reschedule(&ble_work, SCAN_TIMEOUT);
458 break;
459 case BLE_CONNECT_CREATE:
460 printk("Connection attempt timed out\n");
461 bt_conn_disconnect(default_conn,
462 BT_HCI_ERR_REMOTE_USER_TERM_CONN);
463 ble_state = BLE_ADV_START;
464 k_work_reschedule(&ble_work, K_NO_WAIT);
465 break;
466 case BLE_SCAN:
467 printk("No devices found during scan\n");
468 bt_le_scan_stop();
469 ble_state = BLE_ADV_START;
470 k_work_reschedule(&ble_work, K_NO_WAIT);
471 break;
472 case BLE_ADV_START:
473 err = bt_le_adv_start(BT_LE_ADV_CONN_FAST_1, ad, ARRAY_SIZE(ad), sd,
474 ARRAY_SIZE(sd));
475 if (err) {
476 printk("Advertising failed to start (err %d)\n", err);
477 return;
478 }
479
480 printk("Advertising successfully started\n");
481 ble_state = BLE_ADVERTISING;
482 k_work_reschedule(&ble_work, K_MSEC(adv_timeout()));
483 break;
484 case BLE_ADVERTISING:
485 printk("Timed out advertising\n");
486 bt_le_adv_stop();
487 ble_state = BLE_SCAN_START;
488 k_work_reschedule(&ble_work, K_NO_WAIT);
489 break;
490 case BLE_CONNECTED:
491 discov_param.uuid = &pong_svc_uuid.uuid;
492 discov_param.func = discover_func;
493 discov_param.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
494 discov_param.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
495 discov_param.type = BT_GATT_DISCOVER_PRIMARY;
496
497 err = bt_gatt_discover(default_conn, &discov_param);
498 if (err) {
499 printk("Discover failed (err %d)\n", err);
500 return;
501 }
502 break;
503 case BLE_CONNECT_CANCEL:
504 break;
505 }
506 }
507
pong_ccc_cfg_changed(const struct bt_gatt_attr * attr,uint16_t val)508 static void pong_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t val)
509 {
510 printk("val %u\n", val);
511
512 remote_ready = (val == BT_GATT_CCC_NOTIFY);
513
514 if (remote_ready && remote_handle) {
515 pong_conn_ready(initiator);
516 }
517 }
518
519 BT_GATT_SERVICE_DEFINE(pong_svc,
520 /* Vendor Primary Service Declaration */
521 BT_GATT_PRIMARY_SERVICE(&pong_svc_uuid.uuid),
522 BT_GATT_CHARACTERISTIC(&pong_chr_uuid.uuid, BT_GATT_CHRC_NOTIFY,
523 BT_GATT_PERM_NONE, NULL, NULL, NULL),
524 BT_GATT_CCC(pong_ccc_cfg_changed,
525 BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
526 );
527
ble_init(void)528 void ble_init(void)
529 {
530 int err;
531
532 err = bt_enable(NULL);
533 if (err) {
534 printk("Enabling Bluetooth failed (err %d)\n", err);
535 return;
536 }
537
538 k_work_init_delayable(&ble_work, ble_timeout);
539
540 local_attr = &pong_svc.attrs[1];
541 }
542
