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