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