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