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>
83 #include <sys/socket.h>
88 #include "libsigrok-internal.h"
91 #define LOG_PREFIX "bt-bluez"
94 #define CONNECT_BLE_TIMEOUT 20
95 #define STORE_MAC_REVERSE 1
96 #define ACCEPT_NONSEP_MAC 1
99 #define WITH_WRITE_TYPE_HANDLE 0
108 #if !defined HAVE_BT_PUT_LE16
109 static inline void bt_put_le16(uint16_t v, uint8_t *p)
111 p[0] = (v >> 0) & 0xff;
112 p[1] = (v >> 8) & 0xff;
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
160 static int sr_bt_mac_text_to_bytes(
const char *text, uint8_t *buf)
168 if (STORE_MAC_REVERSE)
171 while (len && endp && *endp) {
173 if (ACCEPT_NONSEP_MAC) {
175 numbuf[1] = endp[0] ? endp[1] :
'\0';
179 v = strtol(ACCEPT_NONSEP_MAC ? numbuf : text, &endp, 16);
182 if (*endp !=
':' && *endp !=
'-' && *endp !=
'\0')
184 if (v < 0 || v > 255)
186 if (STORE_MAC_REVERSE)
191 if (ACCEPT_NONSEP_MAC)
192 endp = (
char *)text + (endp - numbuf);
193 if (*endp ==
':' || *endp ==
'-')
198 sr_err(
"Failed to parse MAC, too few bytes in '%s'", text);
201 while (isspace(*endp))
204 sr_err(
"Failed to parse MAC, excess data in '%s'", text);
214 SR_PRIV const char *sr_bt_adapter_get_address(
size_t idx)
217 struct hci_dev_info info;
220 rc = hci_devinfo(idx, &info);
221 sr_spew(
"DIAG: hci_devinfo(%zu) => rc %d", idx, rc);
225 rc = ba2str(&info.bdaddr, addr);
226 sr_spew(
"DIAG: ba2str() => rc %d", rc);
230 return g_strdup(addr);
238 sr_bt_scan_cb scan_cb;
240 sr_bt_data_cb data_cb;
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;
252 struct hci_filter orig_filter;
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);
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);
268 SR_PRIV struct sr_bt_desc *sr_bt_desc_new(
void)
270 struct sr_bt_desc *desc;
272 desc = g_malloc0(
sizeof(*desc));
282 SR_PRIV void sr_bt_desc_free(
struct sr_bt_desc *desc)
287 sr_bt_desc_close(desc);
291 SR_PRIV int sr_bt_config_cb_scan(
struct sr_bt_desc *desc,
292 sr_bt_scan_cb cb,
void *cb_data)
298 desc->scan_cb_data = cb_data;
303 SR_PRIV int sr_bt_config_cb_data(
struct sr_bt_desc *desc,
304 sr_bt_data_cb cb,
void *cb_data)
310 desc->data_cb_data = cb_data;
315 SR_PRIV int sr_bt_config_addr_local(
struct sr_bt_desc *desc,
const char *addr)
323 if (!addr || !addr[0]) {
324 desc->local_addr[0] =
'\0';
328 rc = sr_bt_mac_text_to_bytes(addr, &mac_bytes.b[0]);
332 rc = ba2str(&mac_bytes, desc->local_addr);
339 SR_PRIV int sr_bt_config_addr_remote(
struct sr_bt_desc *desc,
const char *addr)
347 if (!addr || !addr[0]) {
348 desc->remote_addr[0] =
'\0';
352 rc = sr_bt_mac_text_to_bytes(addr, &mac_bytes.b[0]);
356 rc = ba2str(&mac_bytes, desc->remote_addr);
363 SR_PRIV int sr_bt_config_rfcomm(
struct sr_bt_desc *desc,
size_t channel)
368 desc->rfcomm_channel = channel;
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)
381 desc->read_handle = read_handle;
382 desc->write_handle = write_handle;
383 desc->cccd_handle = cccd_handle;
384 desc->cccd_value = cccd_value;
389 static int sr_bt_desc_open(
struct sr_bt_desc *desc,
int *id_ref)
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);
404 id = hci_get_route(NULL);
407 sr_err(
"devid failed");
414 sock = hci_open_dev(
id);
416 perror(
"open HCI socket");
424 static void sr_bt_desc_close(
struct sr_bt_desc *desc)
431 hci_close_dev(desc->fd);
440 #define EIR_NAME_COMPLETE 9
442 static int sr_bt_scan_prep(
struct sr_bt_desc *desc)
445 uint8_t type, owntype, filter;
446 uint16_t ival, window;
450 struct hci_filter scan_filter;
457 ival = htobs(0x0010);
458 window = htobs(0x0010);
462 rc = hci_le_set_scan_parameters(desc->fd,
463 type, ival, window, owntype, filter, timeout);
465 perror(
"set LE scan params");
472 rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
474 perror(
"set LE scan enable");
479 slen =
sizeof(desc->orig_filter);
480 rc = getsockopt(desc->fd, SOL_HCI, HCI_FILTER,
481 &desc->orig_filter, &slen);
483 perror(
"getsockopt(HCI_FILTER)");
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));
493 perror(
"setsockopt(HCI_FILTER)");
500 static int sr_bt_scan_post(
struct sr_bt_desc *desc)
510 rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
511 &desc->orig_filter,
sizeof(desc->orig_filter));
513 perror(
"setsockopt(HCI_FILTER)");
520 rc = hci_le_set_scan_enable(desc->fd, enable, dup, timeout);
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)
538 if (type == EIR_NAME_COMPLETE) {
539 ba2str(&info->bdaddr, addr);
540 name = g_strndup((
const char *)&data[1], dlen - 1);
542 scan_cb(cb_data, addr, name);
551 SR_PRIV int sr_bt_scan_le(
struct sr_bt_desc *desc,
int duration)
555 uint8_t buf[HCI_MAX_EVENT_SIZE];
556 ssize_t rdlen, rdpos;
557 evt_le_meta_event *meta;
558 le_advertising_info *info;
564 sr_dbg(
"BLE scan (LE)");
566 rc = sr_bt_desc_open(desc, NULL);
570 rc = sr_bt_scan_prep(desc);
574 deadline = time(NULL);
575 deadline += duration;
576 while (time(NULL) <= deadline) {
578 if (sr_bt_check_socket_usable(desc) < 0)
580 rdlen = sr_bt_read(desc, buf,
sizeof(buf));
587 if (rdlen < 1 + HCI_EVENT_HDR_SIZE)
589 meta = (
void *)&buf[1 + HCI_EVENT_HDR_SIZE];
590 rdlen -= 1 + HCI_EVENT_HDR_SIZE;
591 if (meta->subevent != EVT_LE_ADVERTISING_REPORT)
593 info = (
void *)&meta->data[1];
594 sr_spew(
"evt: type %d, len %d", info->evt_type, info->length);
599 while (rdpos < rdlen) {
600 datalen = info->data[rdpos];
601 dataptr = &info->data[1 + rdpos];
602 if (rdpos + 1 + datalen > info->length)
604 rdpos += 1 + datalen;
605 rc = sr_bt_scan_proc(desc,
606 desc->scan_cb, desc->scan_cb_data,
607 dataptr, datalen, info);
613 rc = sr_bt_scan_post(desc);
617 sr_bt_desc_close(desc);
622 SR_PRIV int sr_bt_scan_bt(
struct sr_bt_desc *desc,
int duration)
624 int dev_id, sock, rsp_max;
628 size_t rsp_count, idx;
634 sr_dbg(
"BLE scan (BT)");
636 sock = sr_bt_desc_open(desc, &dev_id);
641 info = g_malloc0(rsp_max *
sizeof(*info));
643 inq_rc = hci_inquiry(dev_id, duration, rsp_max, NULL, &info, flags);
645 perror(
"hci_inquiry");
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]");
655 desc->scan_cb(desc->scan_cb_data, addr, name);
659 sr_bt_desc_close(desc);
667 SR_PRIV int sr_bt_connect_ble(
struct sr_bt_desc *desc)
669 struct sockaddr_l2 sl2;
676 if (!desc->remote_addr[0])
678 sr_dbg(
"BLE connect, remote addr %s", desc->remote_addr);
680 s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, 0);
682 perror(
"socket create");
687 memset(&sl2, 0,
sizeof(sl2));
688 sl2.l2_family = AF_BLUETOOTH;
690 if (desc->local_addr[0])
691 str2ba(desc->local_addr, &mac);
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));
704 struct bt_security buf = {
705 .level = BT_SECURITY_LOW,
708 ret = setsockopt(s, SOL_BLUETOOTH, BT_SECURITY, &buf,
sizeof(buf));
710 perror(
"setsockopt");
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));
726 if (ret < 0 && errno == EINPROGRESS) {
727 struct pollfd fds[1];
737 sr_spew(
"in progress ...");
740 memset(fds, 0,
sizeof(fds));
742 fds[0].events = POLLOUT;
743 ret = poll(fds, ARRAY_SIZE(fds), -1);
750 if (!(fds[0].revents & POLLOUT))
752 if (g_get_monotonic_time() >= deadline) {
753 sr_warn(
"Connect attempt timed out");
757 memset(fds, 0,
sizeof(fds));
759 fds[0].events = POLLNVAL;
760 ret = poll(fds, 1, 0);
762 perror(
"poll(INVAL)");
771 solen =
sizeof(soerror);
772 ret = getsockopt(s, SOL_SOCKET, SO_ERROR, &soerror, &solen);
774 perror(
"getsockopt(SO_ERROR)");
780 perror(
"connect(PROGRESS)");
797 SR_PRIV int sr_bt_connect_rfcomm(
struct sr_bt_desc *desc)
799 struct sockaddr_rc addr;
804 if (!desc->remote_addr[0])
806 sr_dbg(
"RFCOMM connect, remote addr %s, channel %zu",
807 desc->remote_addr, desc->rfcomm_channel);
809 if (!desc->rfcomm_channel)
810 desc->rfcomm_channel = 1;
812 fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
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));
828 sr_spew(
"connected");
833 SR_PRIV void sr_bt_disconnect(
struct sr_bt_desc *desc)
835 sr_dbg(
"BLE disconnect");
839 sr_bt_desc_close(desc);
842 static int sr_bt_check_socket_usable(
struct sr_bt_desc *desc)
844 struct pollfd fds[1];
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);
860 if (fds[0].revents & POLLHUP)
862 if (fds[0].revents & POLLERR)
864 if (fds[0].revents & POLLNVAL)
873 SR_PRIV int sr_bt_start_notify(
struct sr_bt_desc *desc)
875 uint8_t buf[
sizeof(desc->cccd_value)];
880 sr_dbg(
"BLE start notify");
882 if (sr_bt_check_socket_usable(desc) < 0)
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))
893 SR_PRIV int sr_bt_check_notify(
struct sr_bt_desc *desc)
898 uint16_t packet_handle;
899 uint8_t *packet_data;
905 if (sr_bt_check_socket_usable(desc) < 0)
909 rdlen = sr_bt_read(desc, buf,
sizeof(buf));
917 packet_handle = 0x0000;
921 packet_type = buf[0];
923 packet_handle = bt_get_le16(&buf[1]);
924 packet_data = &buf[3];
925 packet_dlen = rdlen - 3;
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");
934 case BLE_ATT_WRITE_RESP:
935 sr_spew(
"read() len %zd, type 0x%02x (%s)", rdlen, buf[0],
"write response");
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)
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)
956 return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
958 sr_spew(
"unsupported type 0x%02x", packet_type);
968 SR_PRIV ssize_t sr_bt_write(
struct sr_bt_desc *desc,
969 const void *data,
size_t len)
976 if (sr_bt_check_socket_usable(desc) < 0)
980 if (desc->write_handle)
981 return sr_bt_char_write_req(desc, desc->write_handle, data, len);
984 return write(desc->fd, data, len);
987 static ssize_t sr_bt_write_type(
struct sr_bt_desc *desc, uint8_t type)
996 if (sr_bt_check_socket_usable(desc) < 0)
999 wrlen = write(desc->fd, &type,
sizeof(type));
1002 if (wrlen < (ssize_t)
sizeof(type))
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)
1012 return sr_bt_write_type_handle_bytes(desc, type, handle, NULL, 0);
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)
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, },
1031 if (sr_bt_check_socket_usable(desc) < 0)
1035 bt_put_le16(handle, &header[1]);
1038 wrlen = writev(desc->fd, iov, ARRAY_SIZE(iov));
1040 wrlen = write(desc->fd, header,
sizeof(header));
1044 if (wrlen < (ssize_t)
sizeof(header))
1046 wrlen -=
sizeof(header);
1052 static ssize_t sr_bt_char_write_req(
struct sr_bt_desc *desc,
1053 uint16_t handle,
const void *data,
size_t len)
1055 return sr_bt_write_type_handle_bytes(desc, BLE_ATT_WRITE_REQ,
1059 SR_PRIV ssize_t sr_bt_read(
struct sr_bt_desc *desc,
void *data,
size_t len)
1061 struct pollfd fds[1];
1070 if (sr_bt_check_socket_usable(desc) < 0)
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);
1081 if (!(fds[0].revents & POLLIN))
1084 rdlen = read(desc->fd, data, len);
The public libsigrok header file to be used by frontends.