src/bt/bt_bluez.c

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