#include <ctype.h>
#include <errno.h>
#include <glib.h>
+#include <inttypes.h>
#include <poll.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include <sys/uio.h>
#include <sys/socket.h>
#include <time.h>
#include <unistd.h>
#include <libsigrok/libsigrok.h>
#include "libsigrok-internal.h"
-/** @cond PRIVATE */
#define LOG_PREFIX "bt-bluez"
-/** @endcond */
+#define CONNECT_BLE_TIMEOUT 20 /* Connect timeout in seconds. */
#define STORE_MAC_REVERSE 1
#define ACCEPT_NONSEP_MAC 1
-/* Silence warning about (currently) unused routine. */
-#define WITH_WRITE_TYPE_HANDLE 0
+#define CONNECT_RFCOMM_TRIES 3
+#define CONNECT_RFCOMM_RETRY_MS 100
/* {{{ compat decls */
/*
* the header doesn't.
*/
-#if !defined HAVE_BT_PUT_LE16
-static inline void bt_put_le16(uint16_t v, uint8_t *p)
-{
- p[0] = (v >> 0) & 0xff;
- p[1] = (v >> 8) & 0xff;
-}
-#endif
-
/* }}} compat decls */
/* {{{ Linux socket specific decls */
char addr[20];
rc = hci_devinfo(idx, &info);
- sr_dbg("DIAG: hci_devinfo(%zu) => rc %d", idx, rc);
+ sr_spew("DIAG: hci_devinfo(%zu) => rc %d", idx, rc);
if (rc < 0)
return NULL;
rc = ba2str(&info.bdaddr, addr);
- sr_dbg("DIAG: ba2str() => rc %d", rc);
+ sr_spew("DIAG: ba2str() => rc %d", rc);
if (rc < 0)
return NULL;
uint16_t write_handle;
uint16_t cccd_handle;
uint16_t cccd_value;
+ uint16_t ble_mtu;
/* Internal state. */
int devid;
int fd;
static void sr_bt_desc_close(struct sr_bt_desc *desc);
static int sr_bt_check_socket_usable(struct sr_bt_desc *desc);
static ssize_t sr_bt_write_type(struct sr_bt_desc *desc, uint8_t type);
-#if WITH_WRITE_TYPE_HANDLE
static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
uint8_t type, uint16_t handle);
-#endif
static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
uint8_t type, uint16_t handle, const uint8_t *data, size_t len);
static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
SR_PRIV int sr_bt_config_notify(struct sr_bt_desc *desc,
uint16_t read_handle, uint16_t write_handle,
- uint16_t cccd_handle, uint16_t cccd_value)
+ uint16_t cccd_handle, uint16_t cccd_value,
+ uint16_t ble_mtu)
{
if (!desc)
desc->write_handle = write_handle;
desc->cccd_handle = cccd_handle;
desc->cccd_value = cccd_value;
+ desc->ble_mtu = ble_mtu;
return 0;
}
return -1;
sr_dbg("BLE open");
- sr_spew("get devid");
if (desc->local_addr[0]) {
id = hci_devid(desc->local_addr);
} else if (desc->remote_addr[0]) {
id = hci_get_route(NULL);
}
if (id < 0) {
- sr_spew("devid failed");
+ sr_err("devid failed");
return -1;
}
desc->devid = id;
if (id_ref)
*id_ref = id;
- sr_spew("open HCI socket");
sock = hci_open_dev(id);
if (sock < 0) {
perror("open HCI socket");
if (!desc)
return -1;
- sr_dbg("BLE scan prep");
/* TODO Replace magic values with symbolic identifiers. */
- sr_spew("set LE scan params");
type = 0x01; /* LE public? */
ival = htobs(0x0010);
window = htobs(0x0010);
return -1;
}
- sr_spew("set LE scan enable");
enable = 1;
dup = 1;
timeout = 1000;
}
/* Save the current filter. For later restoration. */
- sr_spew("get HCI filter");
slen = sizeof(desc->orig_filter);
rc = getsockopt(desc->fd, SOL_HCI, HCI_FILTER,
&desc->orig_filter, &slen);
return -1;
}
- sr_spew("set HCI filter");
hci_filter_clear(&scan_filter);
hci_filter_set_ptype(HCI_EVENT_PKT, &scan_filter);
hci_filter_set_event(EVT_LE_META_EVENT, &scan_filter);
if (!desc)
return -1;
- sr_dbg("BLE scan post");
/* Restore previous HCI filter. */
- sr_spew("set HCI filter");
rc = setsockopt(desc->fd, SOL_HCI, HCI_FILTER,
&desc->orig_filter, sizeof(desc->orig_filter));
if (rc < 0) {
return -1;
}
- sr_spew("set LE scan enable");
enable = 0;
dup = 1;
timeout = 1000;
return -1;
sr_dbg("BLE scan (LE)");
- sr_spew("desc open");
rc = sr_bt_desc_open(desc, NULL);
if (rc < 0)
return -1;
- sr_spew("scan prep");
rc = sr_bt_scan_prep(desc);
if (rc < 0)
return -1;
- sr_spew("scan loop");
deadline = time(NULL);
deadline += duration;
while (time(NULL) <= deadline) {
if (rdlen < 0)
break;
if (!rdlen) {
- sr_spew("usleep() start");
g_usleep(50000);
- sr_spew("usleep() done");
continue;
}
if (rdlen < 1 + HCI_EVENT_HDR_SIZE)
}
}
- sr_spew("scan post");
rc = sr_bt_scan_post(desc);
if (rc < 0)
return -1;
struct sockaddr_l2 sl2;
bdaddr_t mac;
int s, ret;
+ gint64 deadline;
if (!desc)
return -1;
return -1;
sr_dbg("BLE connect, remote addr %s", desc->remote_addr);
- sr_spew("socket()");
s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, 0);
if (s < 0) {
perror("socket create");
}
desc->fd = s;
- sr_spew("bind()");
memset(&sl2, 0, sizeof(sl2));
sl2.l2_family = AF_BLUETOOTH;
sl2.l2_psm = 0;
.level = BT_SECURITY_LOW,
.key_size = 0,
};
- sr_spew("security");
ret = setsockopt(s, SOL_BLUETOOTH, BT_SECURITY, &buf, sizeof(buf));
if (ret < 0) {
perror("setsockopt");
}
}
- sr_spew("connect()");
+ deadline = g_get_monotonic_time();
+ deadline += CONNECT_BLE_TIMEOUT * 1000 * 1000;
str2ba(desc->remote_addr, &mac);
memcpy(&sl2.l2_bdaddr, &mac, sizeof(sl2.l2_bdaddr));
sl2.l2_bdaddr_type = BDADDR_LE_PUBLIC;
sr_spew("in progress ...");
do {
- sr_spew("poll(OUT)");
memset(fds, 0, sizeof(fds));
fds[0].fd = s;
fds[0].events = POLLOUT;
continue;
if (!(fds[0].revents & POLLOUT))
continue;
+ if (g_get_monotonic_time() >= deadline) {
+ sr_warn("Connect attempt timed out");
+ return SR_ERR_IO;
+ }
} while (1);
- sr_spew("poll(INVAL)");
memset(fds, 0, sizeof(fds));
fds[0].fd = s;
fds[0].events = POLLNVAL;
close(s);
return -1;
}
- sr_spew("getsocktop(SO_ERROR)");
solen = sizeof(soerror);
ret = getsockopt(s, SOL_SOCKET, SO_ERROR, &soerror, &solen);
if (ret < 0) {
* getsockopt(SOL_BLUETOOTH, BT_RCVMTU, u16);
*/
}
- sr_spew("connect() => rc %d, fd %d", ret, desc->fd);
if (ret < 0) {
perror("connect");
return ret;
SR_PRIV int sr_bt_connect_rfcomm(struct sr_bt_desc *desc)
{
struct sockaddr_rc addr;
- int fd, rc;
+ int i, fd, rc;
if (!desc)
return -1;
if (!desc->rfcomm_channel)
desc->rfcomm_channel = 1;
- sr_spew("socket()");
- fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
- if (fd < 0) {
- perror("socket");
- return -1;
- }
- desc->fd = fd;
-
- sr_spew("connect()");
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
str2ba(desc->remote_addr, &addr.rc_bdaddr);
addr.rc_channel = desc->rfcomm_channel;
- rc = connect(fd, (struct sockaddr *)&addr, sizeof(addr));
- if (rc < 0) {
- perror("connect");
- return -2;
+
+ /*
+ * There are cases where connect returns EBUSY if we are re-connecting
+ * to a device. Try multiple times to work around this issue.
+ */
+ for (i = 0; i < CONNECT_RFCOMM_TRIES; i++) {
+ fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+ if (fd < 0) {
+ perror("socket");
+ return -1;
+ }
+
+ rc = connect(fd, (struct sockaddr *)&addr, sizeof(addr));
+ if (rc >= 0) {
+ sr_spew("connected");
+ desc->fd = fd;
+ return 0;
+ } else if (rc < 0 && errno == EBUSY) {
+ close(fd);
+ g_usleep(CONNECT_RFCOMM_RETRY_MS * 1000);
+ } else {
+ close(fd);
+ perror("connect");
+ return -2;
+ }
}
- sr_spew("connected");
- return 0;
+ sr_err("Connect failed, device busy.");
+
+ return -2;
}
SR_PRIV void sr_bt_disconnect(struct sr_bt_desc *desc)
struct pollfd fds[1];
int ret;
- sr_spew("socket usability check");
if (!desc)
return -1;
if (desc->fd < 0)
if (sr_bt_check_socket_usable(desc) < 0)
return -2;
- sr_spew("write()");
- bt_put_le16(desc->cccd_value, buf);
+ write_u16le(buf, desc->cccd_value);
wrlen = sr_bt_char_write_req(desc, desc->cccd_handle, buf, sizeof(buf));
if (wrlen != sizeof(buf))
return -2;
{
uint8_t buf[1024];
ssize_t rdlen;
+ const uint8_t *bufptr;
+ size_t buflen;
uint8_t packet_type;
uint16_t packet_handle;
uint8_t *packet_data;
size_t packet_dlen;
+ const char *type_text;
+ int ret;
+ uint16_t mtu;
- sr_dbg("BLE check notify");
if (!desc)
return -1;
if (sr_bt_check_socket_usable(desc) < 0)
return -2;
- /* Get another message from the Bluetooth socket. */
- sr_spew("read() non-blocking");
+ /*
+ * Get another message from the Bluetooth socket.
+ *
+ * TODO Can we assume that every "message" comes in a separate
+ * read(2) call, or can data combine at the caller's? Need we
+ * loop over the received content until all was consumed?
+ */
rdlen = sr_bt_read(desc, buf, sizeof(buf));
- sr_spew("read() => %zd", rdlen);
- if (rdlen < 0)
+ if (rdlen < 0) {
+ sr_dbg("check notifiy, read error, %zd", rdlen);
return -2;
- if (!rdlen)
+ }
+ if (!rdlen) {
+ if (0) sr_spew("check notifiy, empty read");
return 0;
- sr_spew("read() len %zd, type 0x%02x", rdlen, buf[0]);
+ }
+ bufptr = &buf[0];
+ buflen = (size_t)rdlen;
+ if (sr_log_loglevel_get() >= SR_LOG_SPEW) {
+ GString *txt;
+ txt = sr_hexdump_new(bufptr, buflen);
+ sr_spew("check notifiy, read succes, length %zd, data: %s",
+ rdlen, txt->str);
+ sr_hexdump_free(txt);
+ }
- /* Get header fields and references to the payload data. */
- packet_type = 0x00;
+ /*
+ * Get header fields and references to the payload data. Notice
+ * that the first 16bit item after the packet type often is the
+ * handle, but need not always be. That is why the read position
+ * is kept, so that individual packet type handlers can either
+ * read _their_ layout strictly sequentially, or can conveniently
+ * access what a common preparation step has provided to them.
+ */
packet_handle = 0x0000;
packet_data = NULL;
packet_dlen = 0;
- if (rdlen >= 1)
- packet_type = buf[0];
- if (rdlen >= 3) {
- packet_handle = bt_get_le16(&buf[1]);
- packet_data = &buf[3];
- packet_dlen = rdlen - 3;
+ packet_type = read_u8_inc_len(&bufptr, &buflen);
+ if (buflen >= sizeof(uint16_t)) {
+ packet_handle = read_u16le(bufptr);
+ packet_data = (void *)&bufptr[sizeof(uint16_t)];
+ packet_dlen = buflen - sizeof(uint16_t);
+ if (!packet_dlen)
+ packet_data = NULL;
}
+ if (0) sr_spew("check notifiy, prep, hdl %" PRIu16 ", data %p len %zu",
+ packet_handle, packet_data, packet_dlen);
/* Dispatch according to the message type. */
switch (packet_type) {
+ case BLE_ATT_EXCHANGE_MTU_REQ:
+ type_text = "MTU exchange request";
+ if (buflen < sizeof(uint16_t)) {
+ sr_dbg("%s, invalid (size)", type_text);
+ break;
+ }
+ mtu = read_u16le_inc_len(&bufptr, &buflen);
+ sr_dbg("%s, peripheral value %" PRIu16, type_text, mtu);
+ if (desc->ble_mtu) {
+ mtu = desc->ble_mtu;
+ sr_dbg("%s, central value %" PRIu16, type_text, mtu);
+ sr_bt_write_type_handle(desc,
+ BLE_ATT_EXCHANGE_MTU_RESP, mtu);
+ break;
+ }
+ sr_warn("Unhandled BLE %s.", type_text);
+ break;
case BLE_ATT_ERROR_RESP:
- sr_spew("error response");
+ type_text = "error response";
+ if (!buflen) {
+ sr_dbg("%s, no payload", type_text);
+ break;
+ }
/* EMPTY */
+ sr_dbg("%s, not handled here", type_text);
break;
case BLE_ATT_WRITE_RESP:
- sr_spew("write response");
- /* EMPTY */
+ type_text = "write response";
+ sr_dbg("%s, note taken", type_text);
break;
case BLE_ATT_HANDLE_INDICATION:
- sr_spew("handle indication");
+ type_text = "handle indication";
+ sr_dbg("%s, data len %zu", type_text, packet_dlen);
sr_bt_write_type(desc, BLE_ATT_HANDLE_CONFIRMATION);
+ sr_spew("%s, confirmation sent", type_text);
if (packet_handle != desc->read_handle)
return -4;
- if (!packet_data)
- return -4;
if (!desc->data_cb)
return 0;
- return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
+ ret = desc->data_cb(desc->data_cb_data,
+ packet_data, packet_dlen);
+ sr_spew("%s, data cb ret %d", type_text, ret);
+ return ret;
case BLE_ATT_HANDLE_NOTIFICATION:
- sr_spew("handle notification");
+ type_text = "handle notification";
+ sr_dbg("%s, data len %zu", type_text, packet_dlen);
if (packet_handle != desc->read_handle)
return -4;
- if (!packet_data)
- return -4;
if (!desc->data_cb)
return 0;
- return desc->data_cb(desc->data_cb_data, packet_data, packet_dlen);
+ ret = desc->data_cb(desc->data_cb_data,
+ packet_data, packet_dlen);
+ sr_spew("%s, data cb ret %d", type_text, ret);
+ return ret;
default:
- sr_spew("unsupported type 0x%02x", packet_type);
+ sr_dbg("unhandled type 0x%02x, len %zu",
+ packet_type, buflen);
return -3;
}
SR_PRIV ssize_t sr_bt_write(struct sr_bt_desc *desc,
const void *data, size_t len)
{
- sr_dbg("BLE write (raw)");
if (!desc)
return -1;
if (desc->fd < 0)
{
ssize_t wrlen;
- sr_dbg("BLE write (type)");
if (!desc)
return -1;
if (desc->fd < 0)
return 0;
}
-#if WITH_WRITE_TYPE_HANDLE
static ssize_t sr_bt_write_type_handle(struct sr_bt_desc *desc,
uint8_t type, uint16_t handle)
{
- sr_dbg("BLE write (type, handle)");
return sr_bt_write_type_handle_bytes(desc, type, handle, NULL, 0);
}
-#endif
static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
uint8_t type, uint16_t handle, const uint8_t *data, size_t len)
};
ssize_t wrlen;
- sr_dbg("BLE write (type, handle, data)");
if (!desc)
return -1;
if (desc->fd < 0)
return -2;
header[0] = type;
- bt_put_le16(handle, &header[1]);
+ write_u16le(&header[1], handle);
if (data && len)
wrlen = writev(desc->fd, iov, ARRAY_SIZE(iov));
static ssize_t sr_bt_char_write_req(struct sr_bt_desc *desc,
uint16_t handle, const void *data, size_t len)
{
- sr_dbg("BLE write-char req");
-
return sr_bt_write_type_handle_bytes(desc, BLE_ATT_WRITE_REQ,
handle, data, len);
}
int ret;
ssize_t rdlen;
- sr_dbg("BLE read (non-blocking)");
if (!desc)
return -1;
if (desc->fd < 0)
if (sr_bt_check_socket_usable(desc) < 0)
return -2;
- sr_spew("poll(POLLIN)");
memset(fds, 0, sizeof(fds));
fds[0].fd = desc->fd;
fds[0].events = POLLIN;
ret = poll(fds, ARRAY_SIZE(fds), 0);
- sr_spew("poll(%d, POLLIN) => 0x%x", desc->fd, fds[0].revents);
if (ret < 0)
return ret;
if (!ret)
if (!(fds[0].revents & POLLIN))
return 0;
- sr_spew("read()");
rdlen = read(desc->fd, data, len);
- sr_spew("read() => %zd", rdlen);
return rdlen;
}
projects / libsigrok.git / blobdiff