src/bt/bt_bluez.c

   1 /*
   2  * This file is part of the sigrok project.
   3  *
   4  * Copyright (C) 2018-2019 Gerhard Sittig <gerhard.sittig@gmx.net>
   5  *
   6  * This program is free software: you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation, either version 3 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 /*
  21  * Scan support for Bluetooth LE devices is modelled after the MIT licensed
  22  * https://github.com/carsonmcdonald/bluez-experiments experiments/scantest.c
  23  * example source code which is:
  24  *
  25  * The MIT License (MIT)
  26  *
  27  * Copyright (c) 2013 Carson McDonald
  28  *
  29  * Permission is hereby granted, free of charge, to any person obtaining a copy of
  30  * this software and associated documentation files (the "Software"), to deal in
  31  * the Software without restriction, including without limitation the rights to
  32  * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
  33  * the Software, and to permit persons to whom the Software is furnished to do so,
  34  * subject to the following conditions:
  35  *
  36  * The above copyright notice and this permission notice shall be included in all
  37  * copies or substantial portions of the Software.
  38  *
  39  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  40  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
  41  * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
  42  * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
  43  * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  44  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  45  */
  46
  47 /*
  48  * This file implements an internal platform agnostic API of libsigrok
  49  * for Bluetooth communication, as well as the first implementation on a
  50  * specific platform which is based on the BlueZ library and got tested
  51  * on Linux.
  52  *
  53  * TODO
  54  * - Separate the "common" from the "bluez specific" parts. The current
  55  *   implementation uses the fact that HAVE_BLUETOOTH exclusively depends
  56  *   on HAVE_LIBBLUEZ, and thus both are identical.
  57  * - Add missing features to the Linux platform support: Scan without
  58  *   root privileges, UUID to handle translation.
  59  * - Add support for other platforms.
  60  */
  61
  62 #include "config.h"
  63
  64 /* Unconditionally compile the source, optionally end up empty. */
  65 #ifdef HAVE_BLUETOOTH
  66
  67 #ifdef HAVE_LIBBLUEZ
  68 #include <bluetooth/bluetooth.h>
  69 #include <bluetooth/hci.h>
  70 #include <bluetooth/hci_lib.h>
  71 #include <bluetooth/l2cap.h>
  72 #include <bluetooth/rfcomm.h>
  73 #endif
  74 #include <ctype.h>
  75 #include <errno.h>
  76 #include <glib.h>
  77 #include <poll.h>
  78 #include <stdarg.h>
  79 #include <stdio.h>
  80 #include <stdlib.h>
  81 #include <string.h>
  82 #include <sys/uio.h>
  83 #include <sys/socket.h>
  84 #include <time.h>
  85 #include <unistd.h>
  86
  87 #include <libsigrok/libsigrok.h>
  88 #include "libsigrok-internal.h"
  89
  90 /** @cond PRIVATE */
  91 #define LOG_PREFIX "bt-bluez"
  92 /** @endcond */
  93
  94 #define STORE_MAC_REVERSE       1
  95 #define ACCEPT_NONSEP_MAC       1
  96
  97 /* Silence warning about (currently) unused routine. */
  98 #define WITH_WRITE_TYPE_HANDLE  0
  99
 100 /* {{{ compat decls */
 101 /*
 102  * The availability of conversion helpers in <bluetooth/bluetooth.h>
 103  * appears to be version dependent. Let's provide the helper here if
 104  * the header doesn't.
 105  */
 106
 107 #if !defined HAVE_BT_PUT_LE16
 108 static inline void bt_put_le16(uint16_t v, uint8_t *p)
 109 {
 110         p[0] = (v >> 0) & 0xff;
 111         p[1] = (v >> 8) & 0xff;
 112 }
 113 #endif
 114
 115 /* }}} compat decls */
 116 /* {{{ Linux socket specific decls */
 117
 118 #define BLE_ATT_ERROR_RESP              0x01
 119 #define BLE_ATT_EXCHANGE_MTU_REQ        0x02
 120 #define BLE_ATT_EXCHANGE_MTU_RESP       0x03
 121 #define BLE_ATT_FIND_INFORMATION_REQ    0x04
 122 #define BLE_ATT_FIND_INFORMATION_RESP   0x05
 123 #define BLE_ATT_FIND_BY_TYPE_REQ        0x06
 124 #define BLE_ATT_FIND_BY_TYPE_RESP       0x07
 125 #define BLE_ATT_READ_BY_TYPE_REQ        0x08
 126 #define BLE_ATT_READ_BY_TYPE_RESP       0x09
 127 #define BLE_ATT_READ_REQ                0x0a
 128 #define BLE_ATT_READ_RESP               0x0b
 129 #define BLE_ATT_READ_BLOB_REQ           0x0c
 130 #define BLE_ATT_READ_BLOB_RESP          0x0d
 131 #define BLE_ATT_READ_MULTIPLE_REQ       0x0e
 132 #define BLE_ATT_READ_MULTIPLE_RESP      0x0f
 133 #define BLE_ATT_READ_BY_GROUP_REQ       0x10
 134 #define BLE_ATT_READ_BY_GROUP_RESP      0x11
 135 #define BLE_ATT_WRITE_REQ               0x12
 136 #define BLE_ATT_WRITE_RESP              0x13
 137 #define BLE_ATT_WRITE_CMD               0x16
 138 #define BLE_ATT_HANDLE_NOTIFICATION     0x1b
 139 #define BLE_ATT_HANDLE_INDICATION       0x1d
 140 #define BLE_ATT_HANDLE_CONFIRMATION     0x1e
 141 #define BLE_ATT_SIGNED_WRITE_CMD        0x52
 142
 143 /* }}} Linux socket specific decls */
 144 /* {{{ conversion */
 145
 146 /*
 147  * Convert textual MAC presentation to array of bytes. In contrast to
 148  * BlueZ conversion, accept colon or dash separated input as well as a
 149  * dense format without separators (001122334455). We expect to use the
 150  * library in an environment where colons are not always available as a
 151  * separator in user provided specs, while users do want to use some
 152  * separator for readability.
 153  *
 154  * TODO Instead of doing the actual conversion here (and dealing with
 155  * BlueZ' internal byte order for device address bytes), we might as
 156  * well just transform the input string to an output string, and always
 157  * use the officially provided str2ba() conversion routine.
 158  */
 159 static int sr_bt_mac_text_to_bytes(const char *text, uint8_t *buf)
 160 {
 161         size_t len;
 162         long v;
 163         char *endp;
 164         char numbuf[3];
 165
 166         len = 6;
 167         if (STORE_MAC_REVERSE)
 168                 buf += len;
 169         endp = (char *)text;
 170         while (len && endp && *endp) {
 171                 text = endp;
 172                 if (ACCEPT_NONSEP_MAC) {
 173                         numbuf[0] = endp[0];
 174                         numbuf[1] = endp[0] ? endp[1] : '\0';
 175                         numbuf[2] = '\0';
 176                 }
 177                 endp = NULL;
 178                 v = strtol(ACCEPT_NONSEP_MAC ? numbuf : text, &endp, 16);
 179                 if (!endp)
 180                         break;
 181                 if (*endp != ':' && *endp != '-' && *endp != '\0')
 182                         break;
 183                 if (v < 0 || v > 255)
 184                         break;
 185                 if (STORE_MAC_REVERSE)
 186                         *(--buf) = v;
 187                 else
 188                         *buf++ = v;
 189                 len--;
 190                 if (ACCEPT_NONSEP_MAC)
 191                         endp = (char *)text + (endp - numbuf);
 192                 if (*endp == ':' || *endp == '-')
 193                         endp++;
 194         }
 195
 196         if (len) {
 197                 sr_err("Failed to parse MAC, too few bytes in '%s'", text);
 198                 return -1;
 199         }
 200         while (isspace(*endp))
 201                 endp++;
 202         if (*endp) {
 203                 sr_err("Failed to parse MAC, excess data in '%s'", text);
 204                 return -1;
 205         }
 206
 207         return 0;
 208 }
 209
 210 /* }}} conversion */
 211 /* {{{ helpers */
 212
 213 SR_PRIV const char *sr_bt_adapter_get_address(size_t idx)
 214 {
 215         int rc;
 216         struct hci_dev_info info;
 217         char addr[20];
 218
 219         rc = hci_devinfo(idx, &info);
 220         sr_dbg("DIAG: hci_devinfo(%zu) => rc %d", idx, rc);
 221         if (rc < 0)
 222                 return NULL;
 223
 224         rc = ba2str(&info.bdaddr, addr);
 225         sr_dbg("DIAG: ba2str() => rc %d", rc);
 226         if (rc < 0)
 227                 return NULL;
 228
 229         return g_strdup(addr);
 230 }
 231
 232 /* }}} helpers */
 233 /* {{{ descriptor */
 234
 235 struct sr_bt_desc {
 236         /* User servicable options. */
 237         sr_bt_scan_cb scan_cb;
 238         void *scan_cb_data;
 239         sr_bt_data_cb data_cb;
 240         void *data_cb_data;
 241         char local_addr[20];
 242         char remote_addr[20];
 243         size_t rfcomm_channel;
 244         uint16_t read_handle;
 245         uint16_t write_handle;
 246         uint16_t cccd_handle;
 247         uint16_t cccd_value;
 248         /* Internal state. */
 249         int devid;
 250         int fd;
 251         struct hci_filter orig_filter;
 252 };
 253
 254 static int sr_bt_desc_open(struct sr_bt_desc *desc, int *id_ref);
 255 static void sr_bt_desc_close(struct sr_bt_desc *desc);
 256 static int sr_bt_check_socket_usable(struct sr_bt_desc *desc);
 257 static ssize_t sr_bt_write_type(struct sr_bt_desc *desc, uint8_t type);
 258 #if WITH_WRITE_TYPE_HANDLE
 259 static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
 260         uint8_t type, uint16_t handle);
 261 #endif
 262 static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
 263         uint8_t type, uint16_t handle, const uint8_t *data, size_t len);
 264 static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
 265         uint16_t handle, const void *data, size_t len);
 266
 267 SR_PRIV struct sr_bt_desc *sr_bt_desc_new(void)
 268 {
 269         struct sr_bt_desc *desc;
 270
 271         desc = g_malloc0(sizeof(*desc));
 272         if (!desc)
 273                 return NULL;
 274
 275         desc->devid = -1;
 276         desc->fd = -1;
 277
 278         return desc;
 279 }
 280
 281 SR_PRIV void sr_bt_desc_free(struct sr_bt_desc *desc)
 282 {
 283         if (!desc)
 284                 return;
 285
 286         sr_bt_desc_close(desc);
 287         g_free(desc);
 288 }
 289
 290 SR_PRIV int sr_bt_config_cb_scan(struct sr_bt_desc *desc,
 291         sr_bt_scan_cb cb, void *cb_data)
 292 {
 293         if (!desc)
 294                 return -1;
 295
 296         desc->scan_cb = cb;
 297         desc->scan_cb_data = cb_data;
 298
 299         return 0;
 300 }
 301
 302 SR_PRIV int sr_bt_config_cb_data(struct sr_bt_desc *desc,
 303         sr_bt_data_cb cb, void *cb_data)
 304 {
 305         if (!desc)
 306                 return -1;
 307
 308         desc->data_cb = cb;
 309         desc->data_cb_data = cb_data;
 310
 311         return 0;
 312 }
 313
 314 SR_PRIV int sr_bt_config_addr_local(struct sr_bt_desc *desc, const char *addr)
 315 {
 316         bdaddr_t mac_bytes;
 317         int rc;
 318
 319         if (!desc)
 320                 return -1;
 321
 322         if (!addr || !addr[0]) {
 323                 desc->local_addr[0] = '\0';
 324                 return 0;
 325         }
 326
 327         rc = sr_bt_mac_text_to_bytes(addr, &mac_bytes.b[0]);
 328         if (rc < 0)
 329                 return -1;
 330
 331         rc = ba2str(&mac_bytes, desc->local_addr);
 332         if (rc < 0)
 333                 return -1;
 334
 335         return 0;
 336 }
 337
 338 SR_PRIV int sr_bt_config_addr_remote(struct sr_bt_desc *desc, const char *addr)
 339 {
 340         bdaddr_t mac_bytes;
 341         int rc;
 342
 343         if (!desc)
 344                 return -1;
 345
 346         if (!addr || !addr[0]) {
 347                 desc->remote_addr[0] = '\0';
 348                 return 0;
 349         }
 350
 351         rc = sr_bt_mac_text_to_bytes(addr, &mac_bytes.b[0]);
 352         if (rc < 0)
 353                 return -1;
 354
 355         rc = ba2str(&mac_bytes, desc->remote_addr);
 356         if (rc < 0)
 357                 return -1;
 358
 359         return 0;
 360 }
 361
 362 SR_PRIV int sr_bt_config_rfcomm(struct sr_bt_desc *desc, size_t channel)
 363 {
 364         if (!desc)
 365                 return -1;
 366
 367         desc->rfcomm_channel = channel;
 368
 369         return 0;
 370 }
 371
 372 SR_PRIV int sr_bt_config_notify(struct sr_bt_desc *desc,
 373         uint16_t read_handle, uint16_t write_handle,
 374         uint16_t cccd_handle, uint16_t cccd_value)
 375 {
 376
 377         if (!desc)
 378                 return -1;
 379
 380         desc->read_handle = read_handle;
 381         desc->write_handle = write_handle;
 382         desc->cccd_handle = cccd_handle;
 383         desc->cccd_value = cccd_value;
 384
 385         return 0;
 386 }
 387
 388 static int sr_bt_desc_open(struct sr_bt_desc *desc, int *id_ref)
 389 {
 390         int id, sock;
 391         bdaddr_t mac;
 392
 393         if (!desc)
 394                 return -1;
 395         sr_dbg("BLE open");
 396
 397         sr_spew("get devid");
 398         if (desc->local_addr[0]) {
 399                 id = hci_devid(desc->local_addr);
 400         } else if (desc->remote_addr[0]) {
 401                 str2ba(desc->remote_addr, &mac);
 402                 id = hci_get_route(&mac);
 403         } else {
 404                 id = hci_get_route(NULL);
 405         }
 406         if (id < 0) {
 407                 sr_spew("devid failed");
 408                 return -1;
 409         }
 410         desc->devid = id;
 411         if (id_ref)
 412                 *id_ref = id;
 413
 414         sr_spew("open HCI socket");
 415         sock = hci_open_dev(id);
 416         if (sock < 0) {
 417                 perror("open HCI socket");
 418                 return -1;
 419         }
 420         desc->fd = sock;
 421
 422         return sock;
 423 }
 424
 425 static void sr_bt_desc_close(struct sr_bt_desc *desc)
 426 {
 427         if (!desc)
 428                 return;
 429
 430         sr_dbg("BLE close");
 431         if (desc->fd >= 0) {
 432                 hci_close_dev(desc->fd);
 433                 desc->fd = -1;
 434         }
 435         desc->devid = -1;
 436 }
 437
 438 /* }}} descriptor */
 439 /* {{{ scan */
 440
 441 #define EIR_NAME_COMPLETE       9
 442
 443 static int sr_bt_scan_prep(struct sr_bt_desc *desc)
 444 {
 445         int rc;
 446         uint8_t type, owntype, filter;
 447         uint16_t ival, window;
 448         int timeout;
 449         uint8_t enable, dup;
 450         socklen_t slen;
 451         struct hci_filter scan_filter;
 452
 453         if (!desc)
 454                 return -1;
 455         sr_dbg("BLE scan prep");
 456
 457         /* TODO Replace magic values with symbolic identifiers. */
 458         sr_spew("set LE scan params");
 459         type = 0x01;    /* LE public? */
 460         ival = htobs(0x0010);
 461         window = htobs(0x0010);
 462         owntype = 0x00; /* any? */
 463         filter = 0x00;
 464         timeout = 1000;
 465         rc = hci_le_set_scan_parameters(desc->fd,
 466                 type, ival, window, owntype, filter, timeout);
 467         if (rc < 0) {
 468                 perror("set LE scan params");
 469                 return -1;
 470         }
 471
 472         sr_spew("set LE scan enable");
 473         enable = 1;
 474         dup = 1;
 475         timeout = 1000;
 476         rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
 477         if (rc < 0) {
 478                 perror("set LE scan enable");
 479                 return -1;
 480         }
 481
 482         /* Save the current filter. For later restoration. */
 483         sr_spew("get HCI filter");
 484         slen = sizeof(desc->orig_filter);
 485         rc = getsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 486                 &desc->orig_filter, &slen);
 487         if (rc < 0) {
 488                 perror("getsockopt(HCI_FILTER)");
 489                 return -1;
 490         }
 491
 492         sr_spew("set HCI filter");
 493         hci_filter_clear(&scan_filter);
 494         hci_filter_set_ptype(HCI_EVENT_PKT, &scan_filter);
 495         hci_filter_set_event(EVT_LE_META_EVENT, &scan_filter);
 496         rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 497                 &scan_filter, sizeof(scan_filter));
 498         if (rc < 0) {
 499                 perror("setsockopt(HCI_FILTER)");
 500                 return -1;
 501         }
 502
 503         return 0;
 504 }
 505
 506 static int sr_bt_scan_post(struct sr_bt_desc *desc)
 507 {
 508         int rc;
 509         uint8_t enable, dup;
 510         int timeout;
 511
 512         if (!desc)
 513                 return -1;
 514         sr_dbg("BLE scan post");
 515
 516         /* Restore previous HCI filter. */
 517         sr_spew("set HCI filter");
 518         rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 519                 &desc->orig_filter, sizeof(desc->orig_filter));
 520         if (rc < 0) {
 521                 perror("setsockopt(HCI_FILTER)");
 522                 return -1;
 523         }
 524
 525         sr_spew("set LE scan enable");
 526         enable = 0;
 527         dup = 1;
 528         timeout = 1000;
 529         rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
 530         if (rc < 0)
 531                 return -1;
 532
 533         return 0;
 534 }
 535
 536 static int sr_bt_scan_proc(struct sr_bt_desc *desc,
 537         sr_bt_scan_cb scan_cb, void *cb_data,
 538         uint8_t *data, size_t dlen, le_advertising_info *info)
 539 {
 540         uint8_t type;
 541         char addr[20];
 542         const char *name;
 543
 544         (void)desc;
 545
 546         type = data[0];
 547         if (type == EIR_NAME_COMPLETE) {
 548                 ba2str(&info->bdaddr, addr);
 549                 name = g_strndup((const char *)&data[1], dlen - 1);
 550                 if (scan_cb)
 551                         scan_cb(cb_data, addr, name);
 552                 free((void *)name);
 553                 return 0;
 554         }
 555
 556         /* Unknown or unsupported type, ignore silently. */
 557         return 0;
 558 }
 559
 560 SR_PRIV int sr_bt_scan_le(struct sr_bt_desc *desc, int duration)
 561 {
 562         int rc;
 563         time_t deadline;
 564         uint8_t buf[HCI_MAX_EVENT_SIZE];
 565         ssize_t rdlen, rdpos;
 566         evt_le_meta_event *meta;
 567         le_advertising_info *info;
 568         uint8_t *dataptr;
 569         size_t datalen;
 570
 571         if (!desc)
 572                 return -1;
 573         sr_dbg("BLE scan (LE)");
 574
 575         sr_spew("desc open");
 576         rc = sr_bt_desc_open(desc, NULL);
 577         if (rc < 0)
 578                 return -1;
 579
 580         sr_spew("scan prep");
 581         rc = sr_bt_scan_prep(desc);
 582         if (rc < 0)
 583                 return -1;
 584
 585         sr_spew("scan loop");
 586         deadline = time(NULL);
 587         deadline += duration;
 588         while (time(NULL) <= deadline) {
 589
 590                 if (sr_bt_check_socket_usable(desc) < 0)
 591                         break;
 592                 rdlen = sr_bt_read(desc, buf, sizeof(buf));
 593                 if (rdlen < 0)
 594                         break;
 595                 if (!rdlen) {
 596                         sr_spew("usleep() start");
 597                         g_usleep(50000);
 598                         sr_spew("usleep() done");
 599                         continue;
 600                 }
 601                 if (rdlen < 1 + HCI_EVENT_HDR_SIZE)
 602                         continue;
 603                 meta = (void *)&buf[1 + HCI_EVENT_HDR_SIZE];
 604                 rdlen -= 1 + HCI_EVENT_HDR_SIZE;
 605                 if (meta->subevent != EVT_LE_ADVERTISING_REPORT)
 606                         continue;
 607                 info = (void *)&meta->data[1];
 608                 sr_spew("evt: type %d, len %d", info->evt_type, info->length);
 609                 if (!info->length)
 610                         continue;
 611
 612                 rdpos = 0;
 613                 while (rdpos < rdlen) {
 614                         datalen = info->data[rdpos];
 615                         dataptr = &info->data[1 + rdpos];
 616                         if (rdpos + 1 + datalen > info->length)
 617                                 break;
 618                         rdpos += 1 + datalen;
 619                         rc = sr_bt_scan_proc(desc,
 620                                 desc->scan_cb, desc->scan_cb_data,
 621                                 dataptr, datalen, info);
 622                         if (rc < 0)
 623                                 break;
 624                 }
 625         }
 626
 627         sr_spew("scan post");
 628         rc = sr_bt_scan_post(desc);
 629         if (rc < 0)
 630                 return -1;
 631
 632         sr_bt_desc_close(desc);
 633
 634         return 0;
 635 }
 636
 637 SR_PRIV int sr_bt_scan_bt(struct sr_bt_desc *desc, int duration)
 638 {
 639         int dev_id, sock, rsp_max;
 640         long flags;
 641         inquiry_info *info;
 642         int inq_rc;
 643         size_t rsp_count, idx;
 644         char addr[20];
 645         char name[256];
 646
 647         if (!desc)
 648                 return -1;
 649         sr_dbg("BLE scan (BT)");
 650
 651         sock = sr_bt_desc_open(desc, &dev_id);
 652         if (sock < 0)
 653                 return -1;
 654
 655         rsp_max = 255;
 656         info = g_malloc0(rsp_max * sizeof(*info));
 657         flags = 0 /* | IREQ_CACHE_FLUSH */;
 658         inq_rc = hci_inquiry(dev_id, duration, rsp_max, NULL, &info, flags);
 659         if (inq_rc < 0)
 660                 perror("hci_inquiry");
 661         rsp_count = inq_rc;
 662
 663         for (idx = 0; idx < rsp_count; idx++) {
 664                 memset(addr, 0, sizeof(addr));
 665                 ba2str(&info[idx].bdaddr, addr);
 666                 memset(name, 0, sizeof(name));
 667                 if (hci_read_remote_name(sock, &info[idx].bdaddr, sizeof(name), name, 0) < 0)
 668                         snprintf(name, sizeof(name), "[unknown]");
 669                 if (desc->scan_cb)
 670                         desc->scan_cb(desc->scan_cb_data, addr, name);
 671         }
 672         g_free(info);
 673
 674         sr_bt_desc_close(desc);
 675
 676         return 0;
 677 }
 678
 679 /* }}} scan */
 680 /* {{{ connect/disconnect */
 681
 682 SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
 683 {
 684         struct sockaddr_l2 sl2;
 685         bdaddr_t mac;
 686         int s, ret;
 687
 688         if (!desc)
 689                 return -1;
 690         if (!desc->remote_addr[0])
 691                 return -1;
 692         sr_dbg("BLE connect, remote addr %s", desc->remote_addr);
 693
 694         sr_spew("socket()");
 695         s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, 0);
 696         if (s < 0) {
 697                 perror("socket create");
 698                 return s;
 699         }
 700         desc->fd = s;
 701
 702         sr_spew("bind()");
 703         memset(&sl2, 0, sizeof(sl2));
 704         sl2.l2_family = AF_BLUETOOTH;
 705         sl2.l2_psm = 0;
 706         if (desc->local_addr[0])
 707                 str2ba(desc->local_addr, &mac);
 708         else
 709                 mac = *BDADDR_ANY;
 710         memcpy(&sl2.l2_bdaddr, &mac, sizeof(sl2.l2_bdaddr));
 711         sl2.l2_cid = L2CAP_FC_CONNLESS;
 712         sl2.l2_bdaddr_type = BDADDR_LE_PUBLIC;
 713         ret = bind(s, (void *)&sl2, sizeof(sl2));
 714         if (ret < 0) {
 715                 perror("bind");
 716                 return ret;
 717         }
 718
 719         if (0) {
 720                 struct bt_security buf = {
 721                         .level = BT_SECURITY_LOW,
 722                         .key_size = 0,
 723                 };
 724                 sr_spew("security");
 725                 ret = setsockopt(s, SOL_BLUETOOTH, BT_SECURITY, &buf, sizeof(buf));
 726                 if (ret < 0) {
 727                         perror("setsockopt");
 728                         return ret;
 729                 }
 730         }
 731
 732         sr_spew("connect()");
 733         str2ba(desc->remote_addr, &mac);
 734         memcpy(&sl2.l2_bdaddr, &mac, sizeof(sl2.l2_bdaddr));
 735         sl2.l2_bdaddr_type = BDADDR_LE_PUBLIC;
 736         ret = connect(s, (void *)&sl2, sizeof(sl2));
 737         /*
 738          * Cope with "in progress" condition. Keep polling the status
 739          * until connect() completes, then get the error by means of
 740          * getsockopt(). See the connect(2) manpage for details.
 741          */
 742         if (ret < 0 && errno == EINPROGRESS) {
 743                 struct pollfd fds[1];
 744                 uint32_t soerror;
 745                 socklen_t solen;
 746
 747                 /* TODO
 748                  * We seem to get here ("connect in progress") even when
 749                  * the specified peer is not around at all. Which results
 750                  * in extended periods of time where nothing happens, and
 751                  * an application timeout seems to be required.
 752                  */
 753                 sr_spew("in progress ...");
 754
 755                 do {
 756                         sr_spew("poll(OUT)");
 757                         memset(fds, 0, sizeof(fds));
 758                         fds[0].fd = s;
 759                         fds[0].events = POLLOUT;
 760                         ret = poll(fds, ARRAY_SIZE(fds), -1);
 761                         if (ret < 0) {
 762                                 perror("poll(OUT)");
 763                                 return ret;
 764                         }
 765                         if (!ret)
 766                                 continue;
 767                         if (!(fds[0].revents & POLLOUT))
 768                                 continue;
 769                 } while (1);
 770                 sr_spew("poll(INVAL)");
 771                 memset(fds, 0, sizeof(fds));
 772                 fds[0].fd = s;
 773                 fds[0].events = POLLNVAL;
 774                 ret = poll(fds, 1, 0);
 775                 if (ret < 0) {
 776                         perror("poll(INVAL)");
 777                         return ret;
 778                 }
 779                 if (ret) {
 780                         /* socket fd is invalid(?) */
 781                         desc->fd = -1;
 782                         close(s);
 783                         return -1;
 784                 }
 785                 sr_spew("getsocktop(SO_ERROR)");
 786                 solen = sizeof(soerror);
 787                 ret = getsockopt(s, SOL_SOCKET, SO_ERROR, &soerror, &solen);
 788                 if (ret < 0) {
 789                         perror("getsockopt(SO_ERROR)");
 790                         return ret;
 791                 }
 792                 if (soerror) {
 793                         /* connect(2) failed, SO_ERROR has the error code. */
 794                         errno = soerror;
 795                         perror("connect(PROGRESS)");
 796                         return soerror;
 797                 }
 798
 799                 /*
 800                  * TODO Get the receive MTU here?
 801                  * getsockopt(SOL_BLUETOOTH, BT_RCVMTU, u16);
 802                  */
 803         }
 804         sr_spew("connect() => rc %d, fd %d", ret, desc->fd);
 805         if (ret < 0) {
 806                 perror("connect");
 807                 return ret;
 808         }
 809
 810         return 0;
 811 }
 812
 813 SR_PRIV int sr_bt_connect_rfcomm(struct sr_bt_desc *desc)
 814 {
 815         struct sockaddr_rc addr;
 816         int fd, rc;
 817
 818         if (!desc)
 819                 return -1;
 820         if (!desc->remote_addr[0])
 821                 return -1;
 822         sr_dbg("RFCOMM connect, remote addr %s, channel %zu",
 823                 desc->remote_addr, desc->rfcomm_channel);
 824
 825         if (!desc->rfcomm_channel)
 826                 desc->rfcomm_channel = 1;
 827
 828         sr_spew("socket()");
 829         fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 830         if (fd < 0) {
 831                 perror("socket");
 832                 return -1;
 833         }
 834         desc->fd = fd;
 835
 836         sr_spew("connect()");
 837         memset(&addr, 0, sizeof(addr));
 838         addr.rc_family = AF_BLUETOOTH;
 839         str2ba(desc->remote_addr, &addr.rc_bdaddr);
 840         addr.rc_channel = desc->rfcomm_channel;
 841         rc = connect(fd, (struct sockaddr *)&addr, sizeof(addr));
 842         if (rc < 0) {
 843                 perror("connect");
 844                 return -2;
 845         }
 846         sr_spew("connected");
 847
 848         return 0;
 849 }
 850
 851 SR_PRIV void sr_bt_disconnect(struct sr_bt_desc *desc)
 852 {
 853         sr_dbg("BLE disconnect");
 854
 855         if (!desc)
 856                 return;
 857         sr_bt_desc_close(desc);
 858 }
 859
 860 static int sr_bt_check_socket_usable(struct sr_bt_desc *desc)
 861 {
 862         struct pollfd fds[1];
 863         int ret;
 864
 865         sr_spew("socket usability check");
 866         if (!desc)
 867                 return -1;
 868         if (desc->fd < 0)
 869                 return -1;
 870
 871         memset(fds, 0, sizeof(fds));
 872         fds[0].fd = desc->fd;
 873         fds[0].events = POLLERR | POLLHUP;
 874         ret = poll(fds, ARRAY_SIZE(fds), 0);
 875         if (ret < 0)
 876                 return ret;
 877         if (!ret)
 878                 return 0;
 879         if (fds[0].revents & POLLHUP)
 880                 return -1;
 881         if (fds[0].revents & POLLERR)
 882                 return -2;
 883         if (fds[0].revents & POLLNVAL)
 884                 return -3;
 885
 886         return 0;
 887 }
 888
 889 /* }}} connect/disconnect */
 890 /* {{{ indication/notification */
 891
 892 SR_PRIV int sr_bt_start_notify(struct sr_bt_desc *desc)
 893 {
 894         uint8_t buf[sizeof(desc->cccd_value)];
 895         ssize_t wrlen;
 896
 897         if (!desc)
 898                 return -1;
 899         sr_dbg("BLE start notify");
 900
 901         if (sr_bt_check_socket_usable(desc) < 0)
 902                 return -2;
 903
 904         sr_spew("write()");
 905         bt_put_le16(desc->cccd_value, buf);
 906         wrlen = sr_bt_char_write_req(desc, desc->cccd_handle, buf, sizeof(buf));
 907         if (wrlen != sizeof(buf))
 908                 return -2;
 909
 910         return 0;
 911 }
 912
 913 SR_PRIV int sr_bt_check_notify(struct sr_bt_desc *desc)
 914 {
 915         uint8_t buf[1024];
 916         ssize_t rdlen;
 917         uint8_t packet_type;
 918         uint16_t packet_handle;
 919         uint8_t *packet_data;
 920         size_t packet_dlen;
 921
 922         sr_dbg("BLE check notify");
 923         if (!desc)
 924                 return -1;
 925
 926         if (sr_bt_check_socket_usable(desc) < 0)
 927                 return -2;
 928
 929         /* Get another message from the Bluetooth socket. */
 930         sr_spew("read() non-blocking");
 931         rdlen = sr_bt_read(desc, buf, sizeof(buf));
 932         sr_spew("read() => %zd", rdlen);
 933         if (rdlen < 0)
 934                 return -2;
 935         if (!rdlen)
 936                 return 0;
 937         sr_spew("read() len %zd, type 0x%02x", rdlen, buf[0]);
 938
 939         /* Get header fields and references to the payload data. */
 940         packet_type = 0x00;
 941         packet_handle = 0x0000;
 942         packet_data = NULL;
 943         packet_dlen = 0;
 944         if (rdlen >= 1)
 945                 packet_type = buf[0];
 946         if (rdlen >= 3) {
 947                 packet_handle = bt_get_le16(&buf[1]);
 948                 packet_data = &buf[3];
 949                 packet_dlen = rdlen - 3;
 950         }
 951
 952         /* Dispatch according to the message type. */
 953         switch (packet_type) {
 954         case BLE_ATT_ERROR_RESP:
 955                 sr_spew("error response");
 956                 /* EMPTY */
 957                 break;
 958         case BLE_ATT_WRITE_RESP:
 959                 sr_spew("write response");
 960                 /* EMPTY */
 961                 break;
 962         case BLE_ATT_HANDLE_INDICATION:
 963                 sr_spew("handle indication");
 964                 sr_bt_write_type(desc, BLE_ATT_HANDLE_CONFIRMATION);
 965                 if (packet_handle != desc->read_handle)
 966                         return -4;
 967                 if (!packet_data)
 968                         return -4;
 969                 if (!desc->data_cb)
 970                         return 0;
 971                 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
 972         case BLE_ATT_HANDLE_NOTIFICATION:
 973                 sr_spew("handle notification");
 974                 if (packet_handle != desc->read_handle)
 975                         return -4;
 976                 if (!packet_data)
 977                         return -4;
 978                 if (!desc->data_cb)
 979                         return 0;
 980                 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
 981         default:
 982                 sr_spew("unsupported type 0x%02x", packet_type);
 983                 return -3;
 984         }
 985
 986         return 0;
 987 }
 988
 989 /* }}} indication/notification */
 990 /* {{{ read/write */
 991
 992 SR_PRIV ssize_t sr_bt_write(struct sr_bt_desc *desc,
 993         const void *data, size_t len)
 994 {
 995         sr_dbg("BLE write (raw)");
 996         if (!desc)
 997                 return -1;
 998         if (desc->fd < 0)
 999                 return -1;
1000
1001         if (sr_bt_check_socket_usable(desc) < 0)
1002                 return -2;
1003
1004         /* Send TX data to the writable characteristics for BLE UART services. */
1005         if (desc->write_handle)
1006                 return sr_bt_char_write_req(desc, desc->write_handle, data, len);
1007
1008         /* Send raw TX data to the RFCOMM socket for BT Classic channels. */
1009         return write(desc->fd, data, len);
1010 }
1011
1012 static ssize_t sr_bt_write_type(struct sr_bt_desc *desc, uint8_t type)
1013 {
1014         ssize_t wrlen;
1015
1016         sr_dbg("BLE write (type)");
1017         if (!desc)
1018                 return -1;
1019         if (desc->fd < 0)
1020                 return -1;
1021
1022         if (sr_bt_check_socket_usable(desc) < 0)
1023                 return -2;
1024
1025         wrlen = write(desc->fd, &type, sizeof(type));
1026         if (wrlen < 0)
1027                 return wrlen;
1028         if (wrlen < (ssize_t)sizeof(type))
1029                 return -1;
1030
1031         return 0;
1032 }
1033
1034 #if WITH_WRITE_TYPE_HANDLE
1035 static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
1036         uint8_t type, uint16_t handle)
1037 {
1038         sr_dbg("BLE write (type, handle)");
1039         return sr_bt_write_type_handle_bytes(desc, type, handle, NULL, 0);
1040 }
1041 #endif
1042
1043 static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
1044         uint8_t type, uint16_t handle, const uint8_t *data, size_t len)
1045 {
1046         uint8_t header[sizeof(uint8_t) + sizeof(uint16_t)];
1047         struct iovec iov[2] = {
1048                 { .iov_base = header, .iov_len = sizeof(header), },
1049                 { .iov_base = (void *)data, .iov_len = len, },
1050         };
1051         ssize_t wrlen;
1052
1053         sr_dbg("BLE write (type, handle, data)");
1054         if (!desc)
1055                 return -1;
1056         if (desc->fd < 0)
1057                 return -1;
1058
1059         if (sr_bt_check_socket_usable(desc) < 0)
1060                 return -2;
1061
1062         header[0] = type;
1063         bt_put_le16(handle, &header[1]);
1064
1065         if (data && len)
1066                 wrlen = writev(desc->fd, iov, ARRAY_SIZE(iov));
1067         else
1068                 wrlen = write(desc->fd, header, sizeof(header));
1069
1070         if (wrlen < 0)
1071                 return wrlen;
1072         if (wrlen < (ssize_t)sizeof(header))
1073                 return -1;
1074         wrlen -= sizeof(header);
1075
1076         return wrlen;
1077 }
1078
1079 /* Returns negative upon error, or returns the number of _payload_ bytes written. */
1080 static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
1081         uint16_t handle, const void *data, size_t len)
1082 {
1083         sr_dbg("BLE write-char req");
1084
1085         return sr_bt_write_type_handle_bytes(desc, BLE_ATT_WRITE_REQ,
1086                 handle, data, len);
1087 }
1088
1089 SR_PRIV ssize_t sr_bt_read(struct sr_bt_desc *desc, void *data, size_t len)
1090 {
1091         struct pollfd fds[1];
1092         int ret;
1093         ssize_t rdlen;
1094
1095         sr_dbg("BLE read (non-blocking)");
1096         if (!desc)
1097                 return -1;
1098         if (desc->fd < 0)
1099                 return -1;
1100
1101         if (sr_bt_check_socket_usable(desc) < 0)
1102                 return -2;
1103
1104         sr_spew("poll(POLLIN)");
1105         memset(fds, 0, sizeof(fds));
1106         fds[0].fd = desc->fd;
1107         fds[0].events = POLLIN;
1108         ret = poll(fds, ARRAY_SIZE(fds), 0);
1109         sr_spew("poll(%d, POLLIN) => 0x%x", desc->fd, fds[0].revents);
1110         if (ret < 0)
1111                 return ret;
1112         if (!ret)
1113                 return 0;
1114         if (!(fds[0].revents & POLLIN))
1115                 return 0;
1116
1117         sr_spew("read()");
1118         rdlen = read(desc->fd, data, len);
1119         sr_spew("read() => %zd", rdlen);
1120
1121         return rdlen;
1122 }
1123
1124 /* }}} indication/notification */
1125
1126 #endif