src/bt/bt_bluez.c

   1 /*
   2  * This file is part of the sigrok project.
   3  *
   4  * Copyright (C) 2018-2019 Gerhard Sittig <gerhard.sittig@gmx.net>
   5  *
   6  * This program is free software: you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation, either version 3 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 /*
  21  * Scan support for Bluetooth LE devices is modelled after the MIT licensed
  22  * https://github.com/carsonmcdonald/bluez-experiments experiments/scantest.c
  23  * example source code which is:
  24  *
  25  * The MIT License (MIT)
  26  *
  27  * Copyright (c) 2013 Carson McDonald
  28  *
  29  * Permission is hereby granted, free of charge, to any person obtaining a copy of
  30  * this software and associated documentation files (the "Software"), to deal in
  31  * the Software without restriction, including without limitation the rights to
  32  * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
  33  * the Software, and to permit persons to whom the Software is furnished to do so,
  34  * subject to the following conditions:
  35  *
  36  * The above copyright notice and this permission notice shall be included in all
  37  * copies or substantial portions of the Software.
  38  *
  39  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  40  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
  41  * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
  42  * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
  43  * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  44  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  45  */
  46
  47 /*
  48  * This file implements an internal platform agnostic API of libsigrok
  49  * for Bluetooth communication, as well as the first implementation on a
  50  * specific platform which is based on the BlueZ library and got tested
  51  * on Linux.
  52  *
  53  * TODO
  54  * - Separate the "common" from the "bluez specific" parts. The current
  55  *   implementation uses the fact that HAVE_BLUETOOTH exclusively depends
  56  *   on HAVE_LIBBLUEZ, and thus both are identical.
  57  * - Add missing features to the Linux platform support: Scan without
  58  *   root privileges, UUID to handle translation.
  59  * - Add support for other platforms.
  60  */
  61
  62 #include "config.h"
  63
  64 /* Unconditionally compile the source, optionally end up empty. */
  65 #ifdef HAVE_BLUETOOTH
  66
  67 #ifdef HAVE_LIBBLUEZ
  68 #include <bluetooth/bluetooth.h>
  69 #include <bluetooth/hci.h>
  70 #include <bluetooth/hci_lib.h>
  71 #include <bluetooth/l2cap.h>
  72 #include <bluetooth/rfcomm.h>
  73 #endif
  74 #include <ctype.h>
  75 #include <errno.h>
  76 #include <glib.h>
  77 #include <poll.h>
  78 #include <stdarg.h>
  79 #include <stdio.h>
  80 #include <stdlib.h>
  81 #include <string.h>
  82 #include <sys/uio.h>
  83 #include <sys/socket.h>
  84 #include <time.h>
  85 #include <unistd.h>
  86
  87 #include <libsigrok/libsigrok.h>
  88 #include "libsigrok-internal.h"
  89
  90 #define LOG_PREFIX "bt-bluez"
  91
  92 #define CONNECT_BLE_TIMEOUT     20      /* Connect timeout in seconds. */
  93 #define STORE_MAC_REVERSE       1
  94 #define ACCEPT_NONSEP_MAC       1
  95
  96 /* 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_spew("DIAG: hci_devinfo(%zu) => rc %d", idx, rc);
 220         if (rc < 0)
 221                 return NULL;
 222
 223         rc = ba2str(&info.bdaddr, addr);
 224         sr_spew("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         if (desc->local_addr[0]) {
 397                 id = hci_devid(desc->local_addr);
 398         } else if (desc->remote_addr[0]) {
 399                 str2ba(desc->remote_addr, &mac);
 400                 id = hci_get_route(&mac);
 401         } else {
 402                 id = hci_get_route(NULL);
 403         }
 404         if (id < 0) {
 405                 sr_err("devid failed");
 406                 return -1;
 407         }
 408         desc->devid = id;
 409         if (id_ref)
 410                 *id_ref = id;
 411
 412         sock = hci_open_dev(id);
 413         if (sock < 0) {
 414                 perror("open HCI socket");
 415                 return -1;
 416         }
 417         desc->fd = sock;
 418
 419         return sock;
 420 }
 421
 422 static void sr_bt_desc_close(struct sr_bt_desc *desc)
 423 {
 424         if (!desc)
 425                 return;
 426
 427         sr_dbg("BLE close");
 428         if (desc->fd >= 0) {
 429                 hci_close_dev(desc->fd);
 430                 desc->fd = -1;
 431         }
 432         desc->devid = -1;
 433 }
 434
 435 /* }}} descriptor */
 436 /* {{{ scan */
 437
 438 #define EIR_NAME_COMPLETE       9
 439
 440 static int sr_bt_scan_prep(struct sr_bt_desc *desc)
 441 {
 442         int rc;
 443         uint8_t type, owntype, filter;
 444         uint16_t ival, window;
 445         int timeout;
 446         uint8_t enable, dup;
 447         socklen_t slen;
 448         struct hci_filter scan_filter;
 449
 450         if (!desc)
 451                 return -1;
 452
 453         /* TODO Replace magic values with symbolic identifiers. */
 454         type = 0x01;    /* LE public? */
 455         ival = htobs(0x0010);
 456         window = htobs(0x0010);
 457         owntype = 0x00; /* any? */
 458         filter = 0x00;
 459         timeout = 1000;
 460         rc = hci_le_set_scan_parameters(desc->fd,
 461                 type, ival, window, owntype, filter, timeout);
 462         if (rc < 0) {
 463                 perror("set LE scan params");
 464                 return -1;
 465         }
 466
 467         enable = 1;
 468         dup = 1;
 469         timeout = 1000;
 470         rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
 471         if (rc < 0) {
 472                 perror("set LE scan enable");
 473                 return -1;
 474         }
 475
 476         /* Save the current filter. For later restoration. */
 477         slen = sizeof(desc->orig_filter);
 478         rc = getsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 479                 &desc->orig_filter, &slen);
 480         if (rc < 0) {
 481                 perror("getsockopt(HCI_FILTER)");
 482                 return -1;
 483         }
 484
 485         hci_filter_clear(&scan_filter);
 486         hci_filter_set_ptype(HCI_EVENT_PKT, &scan_filter);
 487         hci_filter_set_event(EVT_LE_META_EVENT, &scan_filter);
 488         rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 489                 &scan_filter, sizeof(scan_filter));
 490         if (rc < 0) {
 491                 perror("setsockopt(HCI_FILTER)");
 492                 return -1;
 493         }
 494
 495         return 0;
 496 }
 497
 498 static int sr_bt_scan_post(struct sr_bt_desc *desc)
 499 {
 500         int rc;
 501         uint8_t enable, dup;
 502         int timeout;
 503
 504         if (!desc)
 505                 return -1;
 506
 507         /* Restore previous HCI filter. */
 508         rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
 509                 &desc->orig_filter, sizeof(desc->orig_filter));
 510         if (rc < 0) {
 511                 perror("setsockopt(HCI_FILTER)");
 512                 return -1;
 513         }
 514
 515         enable = 0;
 516         dup = 1;
 517         timeout = 1000;
 518         rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
 519         if (rc < 0)
 520                 return -1;
 521
 522         return 0;
 523 }
 524
 525 static int sr_bt_scan_proc(struct sr_bt_desc *desc,
 526         sr_bt_scan_cb scan_cb, void *cb_data,
 527         uint8_t *data, size_t dlen, le_advertising_info *info)
 528 {
 529         uint8_t type;
 530         char addr[20];
 531         const char *name;
 532
 533         (void)desc;
 534
 535         type = data[0];
 536         if (type == EIR_NAME_COMPLETE) {
 537                 ba2str(&info->bdaddr, addr);
 538                 name = g_strndup((const char *)&data[1], dlen - 1);
 539                 if (scan_cb)
 540                         scan_cb(cb_data, addr, name);
 541                 free((void *)name);
 542                 return 0;
 543         }
 544
 545         /* Unknown or unsupported type, ignore silently. */
 546         return 0;
 547 }
 548
 549 SR_PRIV int sr_bt_scan_le(struct sr_bt_desc *desc, int duration)
 550 {
 551         int rc;
 552         time_t deadline;
 553         uint8_t buf[HCI_MAX_EVENT_SIZE];
 554         ssize_t rdlen, rdpos;
 555         evt_le_meta_event *meta;
 556         le_advertising_info *info;
 557         uint8_t *dataptr;
 558         size_t datalen;
 559
 560         if (!desc)
 561                 return -1;
 562         sr_dbg("BLE scan (LE)");
 563
 564         rc = sr_bt_desc_open(desc, NULL);
 565         if (rc < 0)
 566                 return -1;
 567
 568         rc = sr_bt_scan_prep(desc);
 569         if (rc < 0)
 570                 return -1;
 571
 572         deadline = time(NULL);
 573         deadline += duration;
 574         while (time(NULL) <= deadline) {
 575
 576                 if (sr_bt_check_socket_usable(desc) < 0)
 577                         break;
 578                 rdlen = sr_bt_read(desc, buf, sizeof(buf));
 579                 if (rdlen < 0)
 580                         break;
 581                 if (!rdlen) {
 582                         g_usleep(50000);
 583                         continue;
 584                 }
 585                 if (rdlen < 1 + HCI_EVENT_HDR_SIZE)
 586                         continue;
 587                 meta = (void *)&buf[1 + HCI_EVENT_HDR_SIZE];
 588                 rdlen -= 1 + HCI_EVENT_HDR_SIZE;
 589                 if (meta->subevent != EVT_LE_ADVERTISING_REPORT)
 590                         continue;
 591                 info = (void *)&meta->data[1];
 592                 sr_spew("evt: type %d, len %d", info->evt_type, info->length);
 593                 if (!info->length)
 594                         continue;
 595
 596                 rdpos = 0;
 597                 while (rdpos < rdlen) {
 598                         datalen = info->data[rdpos];
 599                         dataptr = &info->data[1 + rdpos];
 600                         if (rdpos + 1 + datalen > info->length)
 601                                 break;
 602                         rdpos += 1 + datalen;
 603                         rc = sr_bt_scan_proc(desc,
 604                                 desc->scan_cb, desc->scan_cb_data,
 605                                 dataptr, datalen, info);
 606                         if (rc < 0)
 607                                 break;
 608                 }
 609         }
 610
 611         rc = sr_bt_scan_post(desc);
 612         if (rc < 0)
 613                 return -1;
 614
 615         sr_bt_desc_close(desc);
 616
 617         return 0;
 618 }
 619
 620 SR_PRIV int sr_bt_scan_bt(struct sr_bt_desc *desc, int duration)
 621 {
 622         int dev_id, sock, rsp_max;
 623         long flags;
 624         inquiry_info *info;
 625         int inq_rc;
 626         size_t rsp_count, idx;
 627         char addr[20];
 628         char name[256];
 629
 630         if (!desc)
 631                 return -1;
 632         sr_dbg("BLE scan (BT)");
 633
 634         sock = sr_bt_desc_open(desc, &dev_id);
 635         if (sock < 0)
 636                 return -1;
 637
 638         rsp_max = 255;
 639         info = g_malloc0(rsp_max * sizeof(*info));
 640         flags = 0 /* | IREQ_CACHE_FLUSH */;
 641         inq_rc = hci_inquiry(dev_id, duration, rsp_max, NULL, &info, flags);
 642         if (inq_rc < 0)
 643                 perror("hci_inquiry");
 644         rsp_count = inq_rc;
 645
 646         for (idx = 0; idx < rsp_count; idx++) {
 647                 memset(addr, 0, sizeof(addr));
 648                 ba2str(&info[idx].bdaddr, addr);
 649                 memset(name, 0, sizeof(name));
 650                 if (hci_read_remote_name(sock, &info[idx].bdaddr, sizeof(name), name, 0) < 0)
 651                         snprintf(name, sizeof(name), "[unknown]");
 652                 if (desc->scan_cb)
 653                         desc->scan_cb(desc->scan_cb_data, addr, name);
 654         }
 655         g_free(info);
 656
 657         sr_bt_desc_close(desc);
 658
 659         return 0;
 660 }
 661
 662 /* }}} scan */
 663 /* {{{ connect/disconnect */
 664
 665 SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
 666 {
 667         struct sockaddr_l2 sl2;
 668         bdaddr_t mac;
 669         int s, ret;
 670         gint64 deadline;
 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         deadline = g_get_monotonic_time();
 714         deadline += CONNECT_BLE_TIMEOUT * 1000 * 1000;
 715         str2ba(desc->remote_addr, &mac);
 716         memcpy(&sl2.l2_bdaddr, &mac, sizeof(sl2.l2_bdaddr));
 717         sl2.l2_bdaddr_type = BDADDR_LE_PUBLIC;
 718         ret = connect(s, (void *)&sl2, sizeof(sl2));
 719         /*
 720          * Cope with "in progress" condition. Keep polling the status
 721          * until connect() completes, then get the error by means of
 722          * getsockopt(). See the connect(2) manpage for details.
 723          */
 724         if (ret < 0 && errno == EINPROGRESS) {
 725                 struct pollfd fds[1];
 726                 uint32_t soerror;
 727                 socklen_t solen;
 728
 729                 /* TODO
 730                  * We seem to get here ("connect in progress") even when
 731                  * the specified peer is not around at all. Which results
 732                  * in extended periods of time where nothing happens, and
 733                  * an application timeout seems to be required.
 734                  */
 735                 sr_spew("in progress ...");
 736
 737                 do {
 738                         memset(fds, 0, sizeof(fds));
 739                         fds[0].fd = s;
 740                         fds[0].events = POLLOUT;
 741                         ret = poll(fds, ARRAY_SIZE(fds), -1);
 742                         if (ret < 0) {
 743                                 perror("poll(OUT)");
 744                                 return ret;
 745                         }
 746                         if (!ret)
 747                                 continue;
 748                         if (!(fds[0].revents & POLLOUT))
 749                                 continue;
 750                         if (g_get_monotonic_time() >= deadline) {
 751                                 sr_warn("Connect attempt timed out");
 752                                 return SR_ERR_IO;
 753                         }
 754                 } while (1);
 755                 memset(fds, 0, sizeof(fds));
 756                 fds[0].fd = s;
 757                 fds[0].events = POLLNVAL;
 758                 ret = poll(fds, 1, 0);
 759                 if (ret < 0) {
 760                         perror("poll(INVAL)");
 761                         return ret;
 762                 }
 763                 if (ret) {
 764                         /* socket fd is invalid(?) */
 765                         desc->fd = -1;
 766                         close(s);
 767                         return -1;
 768                 }
 769                 solen = sizeof(soerror);
 770                 ret = getsockopt(s, SOL_SOCKET, SO_ERROR, &soerror, &solen);
 771                 if (ret < 0) {
 772                         perror("getsockopt(SO_ERROR)");
 773                         return ret;
 774                 }
 775                 if (soerror) {
 776                         /* connect(2) failed, SO_ERROR has the error code. */
 777                         errno = soerror;
 778                         perror("connect(PROGRESS)");
 779                         return soerror;
 780                 }
 781
 782                 /*
 783                  * TODO Get the receive MTU here?
 784                  * getsockopt(SOL_BLUETOOTH, BT_RCVMTU, u16);
 785                  */
 786         }
 787         if (ret < 0) {
 788                 perror("connect");
 789                 return ret;
 790         }
 791
 792         return 0;
 793 }
 794
 795 SR_PRIV int sr_bt_connect_rfcomm(struct sr_bt_desc *desc)
 796 {
 797         struct sockaddr_rc addr;
 798         int fd, rc;
 799
 800         if (!desc)
 801                 return -1;
 802         if (!desc->remote_addr[0])
 803                 return -1;
 804         sr_dbg("RFCOMM connect, remote addr %s, channel %zu",
 805                 desc->remote_addr, desc->rfcomm_channel);
 806
 807         if (!desc->rfcomm_channel)
 808                 desc->rfcomm_channel = 1;
 809
 810         fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 811         if (fd < 0) {
 812                 perror("socket");
 813                 return -1;
 814         }
 815         desc->fd = fd;
 816
 817         memset(&addr, 0, sizeof(addr));
 818         addr.rc_family = AF_BLUETOOTH;
 819         str2ba(desc->remote_addr, &addr.rc_bdaddr);
 820         addr.rc_channel = desc->rfcomm_channel;
 821         rc = connect(fd, (struct sockaddr *)&addr, sizeof(addr));
 822         if (rc < 0) {
 823                 perror("connect");
 824                 return -2;
 825         }
 826         sr_spew("connected");
 827
 828         return 0;
 829 }
 830
 831 SR_PRIV void sr_bt_disconnect(struct sr_bt_desc *desc)
 832 {
 833         sr_dbg("BLE disconnect");
 834
 835         if (!desc)
 836                 return;
 837         sr_bt_desc_close(desc);
 838 }
 839
 840 static int sr_bt_check_socket_usable(struct sr_bt_desc *desc)
 841 {
 842         struct pollfd fds[1];
 843         int ret;
 844
 845         if (!desc)
 846                 return -1;
 847         if (desc->fd < 0)
 848                 return -1;
 849
 850         memset(fds, 0, sizeof(fds));
 851         fds[0].fd = desc->fd;
 852         fds[0].events = POLLERR | POLLHUP;
 853         ret = poll(fds, ARRAY_SIZE(fds), 0);
 854         if (ret < 0)
 855                 return ret;
 856         if (!ret)
 857                 return 0;
 858         if (fds[0].revents & POLLHUP)
 859                 return -1;
 860         if (fds[0].revents & POLLERR)
 861                 return -2;
 862         if (fds[0].revents & POLLNVAL)
 863                 return -3;
 864
 865         return 0;
 866 }
 867
 868 /* }}} connect/disconnect */
 869 /* {{{ indication/notification */
 870
 871 SR_PRIV int sr_bt_start_notify(struct sr_bt_desc *desc)
 872 {
 873         uint8_t buf[sizeof(desc->cccd_value)];
 874         ssize_t wrlen;
 875
 876         if (!desc)
 877                 return -1;
 878         sr_dbg("BLE start notify");
 879
 880         if (sr_bt_check_socket_usable(desc) < 0)
 881                 return -2;
 882
 883         bt_put_le16(desc->cccd_value, buf);
 884         wrlen = sr_bt_char_write_req(desc, desc->cccd_handle, buf, sizeof(buf));
 885         if (wrlen != sizeof(buf))
 886                 return -2;
 887
 888         return 0;
 889 }
 890
 891 SR_PRIV int sr_bt_check_notify(struct sr_bt_desc *desc)
 892 {
 893         uint8_t buf[1024];
 894         ssize_t rdlen;
 895         uint8_t packet_type;
 896         uint16_t packet_handle;
 897         uint8_t *packet_data;
 898         size_t packet_dlen;
 899
 900         if (!desc)
 901                 return -1;
 902
 903         if (sr_bt_check_socket_usable(desc) < 0)
 904                 return -2;
 905
 906         /* Get another message from the Bluetooth socket. */
 907         rdlen = sr_bt_read(desc, buf, sizeof(buf));
 908         if (rdlen < 0)
 909                 return -2;
 910         if (!rdlen)
 911                 return 0;
 912
 913         /* Get header fields and references to the payload data. */
 914         packet_type = 0x00;
 915         packet_handle = 0x0000;
 916         packet_data = NULL;
 917         packet_dlen = 0;
 918         if (rdlen >= 1)
 919                 packet_type = buf[0];
 920         if (rdlen >= 3) {
 921                 packet_handle = bt_get_le16(&buf[1]);
 922                 packet_data = &buf[3];
 923                 packet_dlen = rdlen - 3;
 924         }
 925
 926         /* Dispatch according to the message type. */
 927         switch (packet_type) {
 928         case BLE_ATT_ERROR_RESP:
 929                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "error response");
 930                 /* EMPTY */
 931                 break;
 932         case BLE_ATT_WRITE_RESP:
 933                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "write response");
 934                 /* EMPTY */
 935                 break;
 936         case BLE_ATT_HANDLE_INDICATION:
 937                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "handle indication");
 938                 sr_bt_write_type(desc, BLE_ATT_HANDLE_CONFIRMATION);
 939                 if (packet_handle != desc->read_handle)
 940                         return -4;
 941                 if (!packet_data)
 942                         return -4;
 943                 if (!desc->data_cb)
 944                         return 0;
 945                 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
 946         case BLE_ATT_HANDLE_NOTIFICATION:
 947                 sr_spew("read() len %zd, type 0x%02x (%s)", rdlen, buf[0], "handle notification");
 948                 if (packet_handle != desc->read_handle)
 949                         return -4;
 950                 if (!packet_data)
 951                         return -4;
 952                 if (!desc->data_cb)
 953                         return 0;
 954                 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
 955         default:
 956                 sr_spew("unsupported type 0x%02x", packet_type);
 957                 return -3;
 958         }
 959
 960         return 0;
 961 }
 962
 963 /* }}} indication/notification */
 964 /* {{{ read/write */
 965
 966 SR_PRIV ssize_t sr_bt_write(struct sr_bt_desc *desc,
 967         const void *data, size_t len)
 968 {
 969         if (!desc)
 970                 return -1;
 971         if (desc->fd < 0)
 972                 return -1;
 973
 974         if (sr_bt_check_socket_usable(desc) < 0)
 975                 return -2;
 976
 977         /* Send TX data to the writable characteristics for BLE UART services. */
 978         if (desc->write_handle)
 979                 return sr_bt_char_write_req(desc, desc->write_handle, data, len);
 980
 981         /* Send raw TX data to the RFCOMM socket for BT Classic channels. */
 982         return write(desc->fd, data, len);
 983 }
 984
 985 static ssize_t sr_bt_write_type(struct sr_bt_desc *desc, uint8_t type)
 986 {
 987         ssize_t wrlen;
 988
 989         if (!desc)
 990                 return -1;
 991         if (desc->fd < 0)
 992                 return -1;
 993
 994         if (sr_bt_check_socket_usable(desc) < 0)
 995                 return -2;
 996
 997         wrlen = write(desc->fd, &type, sizeof(type));
 998         if (wrlen < 0)
 999                 return wrlen;
1000         if (wrlen < (ssize_t)sizeof(type))
1001                 return -1;
1002
1003         return 0;
1004 }
1005
1006 #if WITH_WRITE_TYPE_HANDLE
1007 static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
1008         uint8_t type, uint16_t handle)
1009 {
1010         return sr_bt_write_type_handle_bytes(desc, type, handle, NULL, 0);
1011 }
1012 #endif
1013
1014 static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
1015         uint8_t type, uint16_t handle, const uint8_t *data, size_t len)
1016 {
1017         uint8_t header[sizeof(uint8_t) + sizeof(uint16_t)];
1018         struct iovec iov[2] = {
1019                 { .iov_base = header, .iov_len = sizeof(header), },
1020                 { .iov_base = (void *)data, .iov_len = len, },
1021         };
1022         ssize_t wrlen;
1023
1024         if (!desc)
1025                 return -1;
1026         if (desc->fd < 0)
1027                 return -1;
1028
1029         if (sr_bt_check_socket_usable(desc) < 0)
1030                 return -2;
1031
1032         header[0] = type;
1033         bt_put_le16(handle, &header[1]);
1034
1035         if (data && len)
1036                 wrlen = writev(desc->fd, iov, ARRAY_SIZE(iov));
1037         else
1038                 wrlen = write(desc->fd, header, sizeof(header));
1039
1040         if (wrlen < 0)
1041                 return wrlen;
1042         if (wrlen < (ssize_t)sizeof(header))
1043                 return -1;
1044         wrlen -= sizeof(header);
1045
1046         return wrlen;
1047 }
1048
1049 /* Returns negative upon error, or returns the number of _payload_ bytes written. */
1050 static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
1051         uint16_t handle, const void *data, size_t len)
1052 {
1053         return sr_bt_write_type_handle_bytes(desc, BLE_ATT_WRITE_REQ,
1054                 handle, data, len);
1055 }
1056
1057 SR_PRIV ssize_t sr_bt_read(struct sr_bt_desc *desc, void *data, size_t len)
1058 {
1059         struct pollfd fds[1];
1060         int ret;
1061         ssize_t rdlen;
1062
1063         if (!desc)
1064                 return -1;
1065         if (desc->fd < 0)
1066                 return -1;
1067
1068         if (sr_bt_check_socket_usable(desc) < 0)
1069                 return -2;
1070
1071         memset(fds, 0, sizeof(fds));
1072         fds[0].fd = desc->fd;
1073         fds[0].events = POLLIN;
1074         ret = poll(fds, ARRAY_SIZE(fds), 0);
1075         if (ret < 0)
1076                 return ret;
1077         if (!ret)
1078                 return 0;
1079         if (!(fds[0].revents & POLLIN))
1080                 return 0;
1081
1082         rdlen = read(desc->fd, data, len);
1083
1084         return rdlen;
1085 }
1086
1087 /* }}} indication/notification */
1088
1089 #endif