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 #define LOG_PREFIX "bt-bluez"
  91
  92 #define CONNECT_BLE_TIMEOUT     20      /* Connect timeout in seconds. */
  93 #define STORE_MAC_REVERSE       1
  94 #define ACCEPT_NONSEP_MAC       1
  95
  96 #define CONNECT_RFCOMM_TRIES    3
  97 #define CONNECT_RFCOMM_RETRY_MS 100
  98
  99 /* Silence warning about (currently) unused routine. */
 100 #define WITH_WRITE_TYPE_HANDLE  0
 101
 102 /* {{{ compat decls */
 103 /*
 104  * The availability of conversion helpers in <bluetooth/bluetooth.h>
 105  * appears to be version dependent. Let's provide the helper here if
 106  * the header doesn't.
 107  */
 108
 109 #if !defined HAVE_BT_PUT_LE16
 110 static inline void bt_put_le16(uint16_t v, uint8_t *p)
 111 {
 112         p[0] = (v >> 0) & 0xff;
 113         p[1] = (v >> 8) & 0xff;
 114 }
 115 #endif
 116
 117 /* }}} compat decls */
 118 /* {{{ Linux socket specific decls */
 119
 120 #define BLE_ATT_ERROR_RESP              0x01
 121 #define BLE_ATT_EXCHANGE_MTU_REQ        0x02
 122 #define BLE_ATT_EXCHANGE_MTU_RESP       0x03
 123 #define BLE_ATT_FIND_INFORMATION_REQ    0x04
 124 #define BLE_ATT_FIND_INFORMATION_RESP   0x05
 125 #define BLE_ATT_FIND_BY_TYPE_REQ        0x06
 126 #define BLE_ATT_FIND_BY_TYPE_RESP       0x07
 127 #define BLE_ATT_READ_BY_TYPE_REQ        0x08
 128 #define BLE_ATT_READ_BY_TYPE_RESP       0x09
 129 #define BLE_ATT_READ_REQ                0x0a
 130 #define BLE_ATT_READ_RESP               0x0b
 131 #define BLE_ATT_READ_BLOB_REQ           0x0c
 132 #define BLE_ATT_READ_BLOB_RESP          0x0d
 133 #define BLE_ATT_READ_MULTIPLE_REQ       0x0e
 134 #define BLE_ATT_READ_MULTIPLE_RESP      0x0f
 135 #define BLE_ATT_READ_BY_GROUP_REQ       0x10
 136 #define BLE_ATT_READ_BY_GROUP_RESP      0x11
 137 #define BLE_ATT_WRITE_REQ               0x12
 138 #define BLE_ATT_WRITE_RESP              0x13
 139 #define BLE_ATT_WRITE_CMD               0x16
 140 #define BLE_ATT_HANDLE_NOTIFICATION     0x1b
 141 #define BLE_ATT_HANDLE_INDICATION       0x1d
 142 #define BLE_ATT_HANDLE_CONFIRMATION     0x1e
 143 #define BLE_ATT_SIGNED_WRITE_CMD        0x52
 144
 145 /* }}} Linux socket specific decls */
 146 /* {{{ conversion */
 147
 148 /*
 149  * Convert textual MAC presentation to array of bytes. In contrast to
 150  * BlueZ conversion, accept colon or dash separated input as well as a
 151  * dense format without separators (001122334455). We expect to use the
 152  * library in an environment where colons are not always available as a
 153  * separator in user provided specs, while users do want to use some
 154  * separator for readability.
 155  *
 156  * TODO Instead of doing the actual conversion here (and dealing with
 157  * BlueZ' internal byte order for device address bytes), we might as
 158  * well just transform the input string to an output string, and always
 159  * use the officially provided str2ba() conversion routine.
 160  */
 161 static int sr_bt_mac_text_to_bytes(const char *text, uint8_t *buf)
 162 {
 163         size_t len;
 164         long v;
 165         char *endp;
 166         char numbuf[3];
 167
 168         len = 6;
 169         if (STORE_MAC_REVERSE)
 170                 buf += len;
 171         endp = (char *)text;
 172         while (len && endp && *endp) {
 173                 text = endp;
 174                 if (ACCEPT_NONSEP_MAC) {
 175                         numbuf[0] = endp[0];
 176                         numbuf[1] = endp[0] ? endp[1] : '\0';
 177                         numbuf[2] = '\0';
 178                 }
 179                 endp = NULL;
 180                 v = strtol(ACCEPT_NONSEP_MAC ? numbuf : text, &endp, 16);
 181                 if (!endp)
 182                         break;
 183                 if (*endp != ':' && *endp != '-' && *endp != '\0')
 184                         break;
 185                 if (v < 0 || v > 255)
 186                         break;
 187                 if (STORE_MAC_REVERSE)
 188                         *(--buf) = v;
 189                 else
 190                         *buf++ = v;
 191                 len--;
 192                 if (ACCEPT_NONSEP_MAC)
 193                         endp = (char *)text + (endp - numbuf);
 194                 if (*endp == ':' || *endp == '-')
 195                         endp++;
 196         }
 197
 198         if (len) {
 199                 sr_err("Failed to parse MAC, too few bytes in '%s'", text);
 200                 return -1;
 201         }
 202         while (isspace(*endp))
 203                 endp++;
 204         if (*endp) {
 205                 sr_err("Failed to parse MAC, excess data in '%s'", text);
 206                 return -1;
 207         }
 208
 209         return 0;
 210 }
 211
 212 /* }}} conversion */
 213 /* {{{ helpers */
 214
 215 SR_PRIV const char *sr_bt_adapter_get_address(size_t idx)
 216 {
 217         int rc;
 218         struct hci_dev_info info;
 219         char addr[20];
 220
 221         rc = hci_devinfo(idx, &info);
 222         sr_spew("DIAG: hci_devinfo(%zu) => rc %d", idx, rc);
 223         if (rc < 0)
 224                 return NULL;
 225
 226         rc = ba2str(&info.bdaddr, addr);
 227         sr_spew("DIAG: ba2str() => rc %d", rc);
 228         if (rc < 0)
 229                 return NULL;
 230
 231         return g_strdup(addr);
 232 }
 233
 234 /* }}} helpers */
 235 /* {{{ descriptor */
 236
 237 struct sr_bt_desc {
 238         /* User servicable options. */
 239         sr_bt_scan_cb scan_cb;
 240         void *scan_cb_data;
 241         sr_bt_data_cb data_cb;
 242         void *data_cb_data;
 243         char local_addr[20];
 244         char remote_addr[20];
 245         size_t rfcomm_channel;
 246         uint16_t read_handle;
 247         uint16_t write_handle;
 248         uint16_t cccd_handle;
 249         uint16_t cccd_value;
 250         /* Internal state. */
 251         int devid;
 252         int fd;
 253         struct hci_filter orig_filter;
 254 };
 255
 256 static int sr_bt_desc_open(struct sr_bt_desc *desc, int *id_ref);
 257 static void sr_bt_desc_close(struct sr_bt_desc *desc);
 258 static int sr_bt_check_socket_usable(struct sr_bt_desc *desc);
 259 static ssize_t sr_bt_write_type(struct sr_bt_desc *desc, uint8_t type);
 260 #if WITH_WRITE_TYPE_HANDLE
 261 static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
 262         uint8_t type, uint16_t handle);
 263 #endif
 264 static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
 265         uint8_t type, uint16_t handle, const uint8_t *data, size_t len);
 266 static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
 267         uint16_t handle, const void *data, size_t len);
 268
 269 SR_PRIV struct sr_bt_desc *sr_bt_desc_new(void)
 270 {
 271         struct sr_bt_desc *desc;
 272
 273         desc = g_malloc0(sizeof(*desc));
 274         if (!desc)
 275                 return NULL;
 276
 277         desc->devid = -1;
 278         desc->fd = -1;
 279
 280         return desc;
 281 }
 282
 283 SR_PRIV void sr_bt_desc_free(struct sr_bt_desc *desc)
 284 {
 285         if (!desc)
 286                 return;
 287
 288         sr_bt_desc_close(desc);
 289         g_free(desc);
 290 }
 291
 292 SR_PRIV int sr_bt_config_cb_scan(struct sr_bt_desc *desc,
 293         sr_bt_scan_cb cb, void *cb_data)
 294 {
 295         if (!desc)
 296                 return -1;
 297
 298         desc->scan_cb = cb;
 299         desc->scan_cb_data = cb_data;
 300
 301         return 0;
 302 }
 303
 304 SR_PRIV int sr_bt_config_cb_data(struct sr_bt_desc *desc,
 305         sr_bt_data_cb cb, void *cb_data)
 306 {
 307         if (!desc)
 308                 return -1;
 309
 310         desc->data_cb = cb;
 311         desc->data_cb_data = cb_data;
 312
 313         return 0;
 314 }
 315
 316 SR_PRIV int sr_bt_config_addr_local(struct sr_bt_desc *desc, const char *addr)
 317 {
 318         bdaddr_t mac_bytes;
 319         int rc;
 320
 321         if (!desc)
 322                 return -1;
 323
 324         if (!addr || !addr[0]) {
 325                 desc->local_addr[0] = '\0';
 326                 return 0;
 327         }
 328
 329         rc = sr_bt_mac_text_to_bytes(addr, &mac_bytes.b[0]);
 330         if (rc < 0)
 331                 return -1;
 332
 333         rc = ba2str(&mac_bytes, desc->local_addr);
 334         if (rc < 0)
 335                 return -1;
 336
 337         return 0;
 338 }
 339
 340 SR_PRIV int sr_bt_config_addr_remote(struct sr_bt_desc *desc, const char *addr)
 341 {
 342         bdaddr_t mac_bytes;
 343         int rc;
 344
 345         if (!desc)
 346                 return -1;
 347
 348         if (!addr || !addr[0]) {
 349                 desc->remote_addr[0] = '\0';
 350                 return 0;
 351         }
 352
 353         rc = sr_bt_mac_text_to_bytes(addr, &mac_bytes.b[0]);
 354         if (rc < 0)
 355                 return -1;
 356
 357         rc = ba2str(&mac_bytes, desc->remote_addr);
 358         if (rc < 0)
 359                 return -1;
 360
 361         return 0;
 362 }
 363
 364 SR_PRIV int sr_bt_config_rfcomm(struct sr_bt_desc *desc, size_t channel)
 365 {
 366         if (!desc)
 367                 return -1;
 368
 369         desc->rfcomm_channel = channel;
 370
 371         return 0;
 372 }
 373
 374 SR_PRIV int sr_bt_config_notify(struct sr_bt_desc *desc,
 375         uint16_t read_handle, uint16_t write_handle,
 376         uint16_t cccd_handle, uint16_t cccd_value)
 377 {
 378
 379         if (!desc)
 380                 return -1;
 381
 382         desc->read_handle = read_handle;
 383         desc->write_handle = write_handle;
 384         desc->cccd_handle = cccd_handle;
 385         desc->cccd_value = cccd_value;
 386
 387         return 0;
 388 }
 389
 390 static int sr_bt_desc_open(struct sr_bt_desc *desc, int *id_ref)
 391 {
 392         int id, sock;
 393         bdaddr_t mac;
 394
 395         if (!desc)
 396                 return -1;
 397         sr_dbg("BLE open");
 398
 399         if (desc->local_addr[0]) {
 400                 id = hci_devid(desc->local_addr);
 401         } else if (desc->remote_addr[0]) {
 402                 str2ba(desc->remote_addr, &mac);
 403                 id = hci_get_route(&mac);
 404         } else {
 405                 id = hci_get_route(NULL);
 406         }
 407         if (id < 0) {
 408                 sr_err("devid failed");
 409                 return -1;
 410         }
 411         desc->devid = id;
 412         if (id_ref)
 413                 *id_ref = id;
 414
 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
 456         /* TODO Replace magic values with symbolic identifiers. */
 457         type = 0x01;    /* LE public? */
 458         ival = htobs(0x0010);
 459         window = htobs(0x0010);
 460         owntype = 0x00; /* any? */
 461         filter = 0x00;
 462         timeout = 1000;
 463         rc = hci_le_set_scan_parameters(desc->fd,
 464                 type, ival, window, owntype, filter, timeout);
 465         if (rc < 0) {
 466                 perror("set LE scan params");
 467                 return -1;
 468         }
 469
 470         enable = 1;
 471         dup = 1;
 472         timeout = 1000;
 473         rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
 474         if (rc < 0) {
 475                 perror("set LE scan enable");
 476                 return -1;
 477         }
 478
 479         /* Save the current filter. For later restoration. */
 480         slen = sizeof(desc->orig_filter);
 481         rc = getsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 482                 &desc->orig_filter, &slen);
 483         if (rc < 0) {
 484                 perror("getsockopt(HCI_FILTER)");
 485                 return -1;
 486         }
 487
 488         hci_filter_clear(&scan_filter);
 489         hci_filter_set_ptype(HCI_EVENT_PKT, &scan_filter);
 490         hci_filter_set_event(EVT_LE_META_EVENT, &scan_filter);
 491         rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 492                 &scan_filter, sizeof(scan_filter));
 493         if (rc < 0) {
 494                 perror("setsockopt(HCI_FILTER)");
 495                 return -1;
 496         }
 497
 498         return 0;
 499 }
 500
 501 static int sr_bt_scan_post(struct sr_bt_desc *desc)
 502 {
 503         int rc;
 504         uint8_t enable, dup;
 505         int timeout;
 506
 507         if (!desc)
 508                 return -1;
 509
 510         /* Restore previous HCI filter. */
 511         rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 512                 &desc->orig_filter, sizeof(desc->orig_filter));
 513         if (rc < 0) {
 514                 perror("setsockopt(HCI_FILTER)");
 515                 return -1;
 516         }
 517
 518         enable = 0;
 519         dup = 1;
 520         timeout = 1000;
 521         rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
 522         if (rc < 0)
 523                 return -1;
 524
 525         return 0;
 526 }
 527
 528 static int sr_bt_scan_proc(struct sr_bt_desc *desc,
 529         sr_bt_scan_cb scan_cb, void *cb_data,
 530         uint8_t *data, size_t dlen, le_advertising_info *info)
 531 {
 532         uint8_t type;
 533         char addr[20];
 534         const char *name;
 535
 536         (void)desc;
 537
 538         type = data[0];
 539         if (type == EIR_NAME_COMPLETE) {
 540                 ba2str(&info->bdaddr, addr);
 541                 name = g_strndup((const char *)&data[1], dlen - 1);
 542                 if (scan_cb)
 543                         scan_cb(cb_data, addr, name);
 544                 free((void *)name);
 545                 return 0;
 546         }
 547
 548         /* Unknown or unsupported type, ignore silently. */
 549         return 0;
 550 }
 551
 552 SR_PRIV int sr_bt_scan_le(struct sr_bt_desc *desc, int duration)
 553 {
 554         int rc;
 555         time_t deadline;
 556         uint8_t buf[HCI_MAX_EVENT_SIZE];
 557         ssize_t rdlen, rdpos;
 558         evt_le_meta_event *meta;
 559         le_advertising_info *info;
 560         uint8_t *dataptr;
 561         size_t datalen;
 562
 563         if (!desc)
 564                 return -1;
 565         sr_dbg("BLE scan (LE)");
 566
 567         rc = sr_bt_desc_open(desc, NULL);
 568         if (rc < 0)
 569                 return -1;
 570
 571         rc = sr_bt_scan_prep(desc);
 572         if (rc < 0)
 573                 return -1;
 574
 575         deadline = time(NULL);
 576         deadline += duration;
 577         while (time(NULL) <= deadline) {
 578
 579                 if (sr_bt_check_socket_usable(desc) < 0)
 580                         break;
 581                 rdlen = sr_bt_read(desc, buf, sizeof(buf));
 582                 if (rdlen < 0)
 583                         break;
 584                 if (!rdlen) {
 585                         g_usleep(50000);
 586                         continue;
 587                 }
 588                 if (rdlen < 1 + HCI_EVENT_HDR_SIZE)
 589                         continue;
 590                 meta = (void *)&buf[1 + HCI_EVENT_HDR_SIZE];
 591                 rdlen -= 1 + HCI_EVENT_HDR_SIZE;
 592                 if (meta->subevent != EVT_LE_ADVERTISING_REPORT)
 593                         continue;
 594                 info = (void *)&meta->data[1];
 595                 sr_spew("evt: type %d, len %d", info->evt_type, info->length);
 596                 if (!info->length)
 597                         continue;
 598
 599                 rdpos = 0;
 600                 while (rdpos < rdlen) {
 601                         datalen = info->data[rdpos];
 602                         dataptr = &info->data[1 + rdpos];
 603                         if (rdpos + 1 + datalen > info->length)
 604                                 break;
 605                         rdpos += 1 + datalen;
 606                         rc = sr_bt_scan_proc(desc,
 607                                 desc->scan_cb, desc->scan_cb_data,
 608                                 dataptr, datalen, info);
 609                         if (rc < 0)
 610                                 break;
 611                 }
 612         }
 613
 614         rc = sr_bt_scan_post(desc);
 615         if (rc < 0)
 616                 return -1;
 617
 618         sr_bt_desc_close(desc);
 619
 620         return 0;
 621 }
 622
 623 SR_PRIV int sr_bt_scan_bt(struct sr_bt_desc *desc, int duration)
 624 {
 625         int dev_id, sock, rsp_max;
 626         long flags;
 627         inquiry_info *info;
 628         int inq_rc;
 629         size_t rsp_count, idx;
 630         char addr[20];
 631         char name[256];
 632
 633         if (!desc)
 634                 return -1;
 635         sr_dbg("BLE scan (BT)");
 636
 637         sock = sr_bt_desc_open(desc, &dev_id);
 638         if (sock < 0)
 639                 return -1;
 640
 641         rsp_max = 255;
 642         info = g_malloc0(rsp_max * sizeof(*info));
 643         flags = 0 /* | IREQ_CACHE_FLUSH */;
 644         inq_rc = hci_inquiry(dev_id, duration, rsp_max, NULL, &info, flags);
 645         if (inq_rc < 0)
 646                 perror("hci_inquiry");
 647         rsp_count = inq_rc;
 648
 649         for (idx = 0; idx < rsp_count; idx++) {
 650                 memset(addr, 0, sizeof(addr));
 651                 ba2str(&info[idx].bdaddr, addr);
 652                 memset(name, 0, sizeof(name));
 653                 if (hci_read_remote_name(sock, &info[idx].bdaddr, sizeof(name), name, 0) < 0)
 654                         snprintf(name, sizeof(name), "[unknown]");
 655                 if (desc->scan_cb)
 656                         desc->scan_cb(desc->scan_cb_data, addr, name);
 657         }
 658         g_free(info);
 659
 660         sr_bt_desc_close(desc);
 661
 662         return 0;
 663 }
 664
 665 /* }}} scan */
 666 /* {{{ connect/disconnect */
 667
 668 SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
 669 {
 670         struct sockaddr_l2 sl2;
 671         bdaddr_t mac;
 672         int s, ret;
 673         gint64 deadline;
 674
 675         if (!desc)
 676                 return -1;
 677         if (!desc->remote_addr[0])
 678                 return -1;
 679         sr_dbg("BLE connect, remote addr %s", desc->remote_addr);
 680
 681         s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, 0);
 682         if (s < 0) {
 683                 perror("socket create");
 684                 return s;
 685         }
 686         desc->fd = s;
 687
 688         memset(&sl2, 0, sizeof(sl2));
 689         sl2.l2_family = AF_BLUETOOTH;
 690         sl2.l2_psm = 0;
 691         if (desc->local_addr[0])
 692                 str2ba(desc->local_addr, &mac);
 693         else
 694                 mac = *BDADDR_ANY;
 695         memcpy(&sl2.l2_bdaddr, &mac, sizeof(sl2.l2_bdaddr));
 696         sl2.l2_cid = L2CAP_FC_CONNLESS;
 697         sl2.l2_bdaddr_type = BDADDR_LE_PUBLIC;
 698         ret = bind(s, (void *)&sl2, sizeof(sl2));
 699         if (ret < 0) {
 700                 perror("bind");
 701                 return ret;
 702         }
 703
 704         if (0) {
 705                 struct bt_security buf = {
 706                         .level = BT_SECURITY_LOW,
 707                         .key_size = 0,
 708                 };
 709                 ret = setsockopt(s, SOL_BLUETOOTH, BT_SECURITY, &buf, sizeof(buf));
 710                 if (ret < 0) {
 711                         perror("setsockopt");
 712                         return ret;
 713                 }
 714         }
 715
 716         deadline = g_get_monotonic_time();
 717         deadline += CONNECT_BLE_TIMEOUT * 1000 * 1000;
 718         str2ba(desc->remote_addr, &mac);
 719         memcpy(&sl2.l2_bdaddr, &mac, sizeof(sl2.l2_bdaddr));
 720         sl2.l2_bdaddr_type = BDADDR_LE_PUBLIC;
 721         ret = connect(s, (void *)&sl2, sizeof(sl2));
 722         /*
 723          * Cope with "in progress" condition. Keep polling the status
 724          * until connect() completes, then get the error by means of
 725          * getsockopt(). See the connect(2) manpage for details.
 726          */
 727         if (ret < 0 && errno == EINPROGRESS) {
 728                 struct pollfd fds[1];
 729                 uint32_t soerror;
 730                 socklen_t solen;
 731
 732                 /* TODO
 733                  * We seem to get here ("connect in progress") even when
 734                  * the specified peer is not around at all. Which results
 735                  * in extended periods of time where nothing happens, and
 736                  * an application timeout seems to be required.
 737                  */
 738                 sr_spew("in progress ...");
 739
 740                 do {
 741                         memset(fds, 0, sizeof(fds));
 742                         fds[0].fd = s;
 743                         fds[0].events = POLLOUT;
 744                         ret = poll(fds, ARRAY_SIZE(fds), -1);
 745                         if (ret < 0) {
 746                                 perror("poll(OUT)");
 747                                 return ret;
 748                         }
 749                         if (!ret)
 750                                 continue;
 751                         if (!(fds[0].revents & POLLOUT))
 752                                 continue;
 753                         if (g_get_monotonic_time() >= deadline) {
 754                                 sr_warn("Connect attempt timed out");
 755                                 return SR_ERR_IO;
 756                         }
 757                 } while (1);
 758                 memset(fds, 0, sizeof(fds));
 759                 fds[0].fd = s;
 760                 fds[0].events = POLLNVAL;
 761                 ret = poll(fds, 1, 0);
 762                 if (ret < 0) {
 763                         perror("poll(INVAL)");
 764                         return ret;
 765                 }
 766                 if (ret) {
 767                         /* socket fd is invalid(?) */
 768                         desc->fd = -1;
 769                         close(s);
 770                         return -1;
 771                 }
 772                 solen = sizeof(soerror);
 773                 ret = getsockopt(s, SOL_SOCKET, SO_ERROR, &soerror, &solen);
 774                 if (ret < 0) {
 775                         perror("getsockopt(SO_ERROR)");
 776                         return ret;
 777                 }
 778                 if (soerror) {
 779                         /* connect(2) failed, SO_ERROR has the error code. */
 780                         errno = soerror;
 781                         perror("connect(PROGRESS)");
 782                         return soerror;
 783                 }
 784
 785                 /*
 786                  * TODO Get the receive MTU here?
 787                  * getsockopt(SOL_BLUETOOTH, BT_RCVMTU, u16);
 788                  */
 789         }
 790         if (ret < 0) {
 791                 perror("connect");
 792                 return ret;
 793         }
 794
 795         return 0;
 796 }
 797
 798 SR_PRIV int sr_bt_connect_rfcomm(struct sr_bt_desc *desc)
 799 {
 800         struct sockaddr_rc addr;
 801         int i, fd, rc;
 802
 803         if (!desc)
 804                 return -1;
 805         if (!desc->remote_addr[0])
 806                 return -1;
 807         sr_dbg("RFCOMM connect, remote addr %s, channel %zu",
 808                 desc->remote_addr, desc->rfcomm_channel);
 809
 810         if (!desc->rfcomm_channel)
 811                 desc->rfcomm_channel = 1;
 812
 813         memset(&addr, 0, sizeof(addr));
 814         addr.rc_family = AF_BLUETOOTH;
 815         str2ba(desc->remote_addr, &addr.rc_bdaddr);
 816         addr.rc_channel = desc->rfcomm_channel;
 817
 818         /*
 819          * There are cases where connect returns EBUSY if we are re-connecting
 820          * to a device. Try multiple times to work around this issue.
 821          */
 822         for (i = 0; i < CONNECT_RFCOMM_TRIES; i++) {
 823                 fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 824                 if (fd < 0) {
 825                         perror("socket");
 826                         return -1;
 827                 }
 828
 829                 rc = connect(fd, (struct sockaddr *)&addr, sizeof(addr));
 830                 if (rc >= 0) {
 831                         sr_spew("connected");
 832                         desc->fd = fd;
 833                         return 0;
 834                 } else if (rc < 0 && errno == EBUSY) {
 835                         close(fd);
 836                         g_usleep(CONNECT_RFCOMM_RETRY_MS * 1000);
 837                 } else {
 838                         close(fd);
 839                         perror("connect");
 840                         return -2;
 841                 }
 842         }
 843
 844         sr_err("Connect failed, device busy.");
 845
 846         return -2;
 847 }
 848
 849 SR_PRIV void sr_bt_disconnect(struct sr_bt_desc *desc)
 850 {
 851         sr_dbg("BLE disconnect");
 852
 853         if (!desc)
 854                 return;
 855         sr_bt_desc_close(desc);
 856 }
 857
 858 static int sr_bt_check_socket_usable(struct sr_bt_desc *desc)
 859 {
 860         struct pollfd fds[1];
 861         int ret;
 862
 863         if (!desc)
 864                 return -1;
 865         if (desc->fd < 0)
 866                 return -1;
 867
 868         memset(fds, 0, sizeof(fds));
 869         fds[0].fd = desc->fd;
 870         fds[0].events = POLLERR | POLLHUP;
 871         ret = poll(fds, ARRAY_SIZE(fds), 0);
 872         if (ret < 0)
 873                 return ret;
 874         if (!ret)
 875                 return 0;
 876         if (fds[0].revents & POLLHUP)
 877                 return -1;
 878         if (fds[0].revents & POLLERR)
 879                 return -2;
 880         if (fds[0].revents & POLLNVAL)
 881                 return -3;
 882
 883         return 0;
 884 }
 885
 886 /* }}} connect/disconnect */
 887 /* {{{ indication/notification */
 888
 889 SR_PRIV int sr_bt_start_notify(struct sr_bt_desc *desc)
 890 {
 891         uint8_t buf[sizeof(desc->cccd_value)];
 892         ssize_t wrlen;
 893
 894         if (!desc)
 895                 return -1;
 896         sr_dbg("BLE start notify");
 897
 898         if (sr_bt_check_socket_usable(desc) < 0)
 899                 return -2;
 900
 901         bt_put_le16(desc->cccd_value, buf);
 902         wrlen = sr_bt_char_write_req(desc, desc->cccd_handle, buf, sizeof(buf));
 903         if (wrlen != sizeof(buf))
 904                 return -2;
 905
 906         return 0;
 907 }
 908
 909 SR_PRIV int sr_bt_check_notify(struct sr_bt_desc *desc)
 910 {
 911         uint8_t buf[1024];
 912         ssize_t rdlen;
 913         uint8_t packet_type;
 914         uint16_t packet_handle;
 915         uint8_t *packet_data;
 916         size_t packet_dlen;
 917
 918         if (!desc)
 919                 return -1;
 920
 921         if (sr_bt_check_socket_usable(desc) < 0)
 922                 return -2;
 923
 924         /* Get another message from the Bluetooth socket. */
 925         rdlen = sr_bt_read(desc, buf, sizeof(buf));
 926         if (rdlen < 0)
 927                 return -2;
 928         if (!rdlen)
 929                 return 0;
 930
 931         /* Get header fields and references to the payload data. */
 932         packet_type = 0x00;
 933         packet_handle = 0x0000;
 934         packet_data = NULL;
 935         packet_dlen = 0;
 936         if (rdlen >= 1)
 937                 packet_type = buf[0];
 938         if (rdlen >= 3) {
 939                 packet_handle = bt_get_le16(&buf[1]);
 940                 packet_data = &buf[3];
 941                 packet_dlen = rdlen - 3;
 942         }
 943
 944         /* Dispatch according to the message type. */
 945         switch (packet_type) {
 946         case BLE_ATT_ERROR_RESP:
 947                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "error response");
 948                 /* EMPTY */
 949                 break;
 950         case BLE_ATT_WRITE_RESP:
 951                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "write response");
 952                 /* EMPTY */
 953                 break;
 954         case BLE_ATT_HANDLE_INDICATION:
 955                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "handle indication");
 956                 sr_bt_write_type(desc, BLE_ATT_HANDLE_CONFIRMATION);
 957                 if (packet_handle != desc->read_handle)
 958                         return -4;
 959                 if (!packet_data)
 960                         return -4;
 961                 if (!desc->data_cb)
 962                         return 0;
 963                 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
 964         case BLE_ATT_HANDLE_NOTIFICATION:
 965                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "handle notification");
 966                 if (packet_handle != desc->read_handle)
 967                         return -4;
 968                 if (!packet_data)
 969                         return -4;
 970                 if (!desc->data_cb)
 971                         return 0;
 972                 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
 973         default:
 974                 sr_spew("unsupported type 0x%02x", packet_type);
 975                 return -3;
 976         }
 977
 978         return 0;
 979 }
 980
 981 /* }}} indication/notification */
 982 /* {{{ read/write */
 983
 984 SR_PRIV ssize_t sr_bt_write(struct sr_bt_desc *desc,
 985         const void *data, size_t len)
 986 {
 987         if (!desc)
 988                 return -1;
 989         if (desc->fd < 0)
 990                 return -1;
 991
 992         if (sr_bt_check_socket_usable(desc) < 0)
 993                 return -2;
 994
 995         /* Send TX data to the writable characteristics for BLE UART services. */
 996         if (desc->write_handle)
 997                 return sr_bt_char_write_req(desc, desc->write_handle, data, len);
 998
 999         /* Send raw TX data to the RFCOMM socket for BT Classic channels. */
1000         return write(desc->fd, data, len);
1001 }
1002
1003 static ssize_t sr_bt_write_type(struct sr_bt_desc *desc, uint8_t type)
1004 {
1005         ssize_t wrlen;
1006
1007         if (!desc)
1008                 return -1;
1009         if (desc->fd < 0)
1010                 return -1;
1011
1012         if (sr_bt_check_socket_usable(desc) < 0)
1013                 return -2;
1014
1015         wrlen = write(desc->fd, &type, sizeof(type));
1016         if (wrlen < 0)
1017                 return wrlen;
1018         if (wrlen < (ssize_t)sizeof(type))
1019                 return -1;
1020
1021         return 0;
1022 }
1023
1024 #if WITH_WRITE_TYPE_HANDLE
1025 static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
1026         uint8_t type, uint16_t handle)
1027 {
1028         return sr_bt_write_type_handle_bytes(desc, type, handle, NULL, 0);
1029 }
1030 #endif
1031
1032 static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
1033         uint8_t type, uint16_t handle, const uint8_t *data, size_t len)
1034 {
1035         uint8_t header[sizeof(uint8_t) + sizeof(uint16_t)];
1036         struct iovec iov[2] = {
1037                 { .iov_base = header, .iov_len = sizeof(header), },
1038                 { .iov_base = (void *)data, .iov_len = len, },
1039         };
1040         ssize_t wrlen;
1041
1042         if (!desc)
1043                 return -1;
1044         if (desc->fd < 0)
1045                 return -1;
1046
1047         if (sr_bt_check_socket_usable(desc) < 0)
1048                 return -2;
1049
1050         header[0] = type;
1051         bt_put_le16(handle, &header[1]);
1052
1053         if (data && len)
1054                 wrlen = writev(desc->fd, iov, ARRAY_SIZE(iov));
1055         else
1056                 wrlen = write(desc->fd, header, sizeof(header));
1057
1058         if (wrlen < 0)
1059                 return wrlen;
1060         if (wrlen < (ssize_t)sizeof(header))
1061                 return -1;
1062         wrlen -= sizeof(header);
1063
1064         return wrlen;
1065 }
1066
1067 /* Returns negative upon error, or returns the number of _payload_ bytes written. */
1068 static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
1069         uint16_t handle, const void *data, size_t len)
1070 {
1071         return sr_bt_write_type_handle_bytes(desc, BLE_ATT_WRITE_REQ,
1072                 handle, data, len);
1073 }
1074
1075 SR_PRIV ssize_t sr_bt_read(struct sr_bt_desc *desc, void *data, size_t len)
1076 {
1077         struct pollfd fds[1];
1078         int ret;
1079         ssize_t rdlen;
1080
1081         if (!desc)
1082                 return -1;
1083         if (desc->fd < 0)
1084                 return -1;
1085
1086         if (sr_bt_check_socket_usable(desc) < 0)
1087                 return -2;
1088
1089         memset(fds, 0, sizeof(fds));
1090         fds[0].fd = desc->fd;
1091         fds[0].events = POLLIN;
1092         ret = poll(fds, ARRAY_SIZE(fds), 0);
1093         if (ret < 0)
1094                 return ret;
1095         if (!ret)
1096                 return 0;
1097         if (!(fds[0].revents & POLLIN))
1098                 return 0;
1099
1100         rdlen = read(desc->fd, data, len);
1101
1102         return rdlen;
1103 }
1104
1105 /* }}} indication/notification */
1106
1107 #endif