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