src/bt/bt_bluez.c

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