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