device: introduce common helpers for channel group allocation

[libsigrok.git] / src / bt / bt_bluez.c

diff --git a/src/bt/bt_bluez.c b/src/bt/bt_bluez.c
index 8df950fb2e13ea762e2fc368d7e0e940e529ef6d..d0a3b7c847605ca2d427fb8e93f9e36eea02ded0 100644 (file)
--- a/src/bt/bt_bluez.c
+++ b/src/bt/bt_bluez.c
@@ -87,13 +87,15 @@
 #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
 
+#define CONNECT_RFCOMM_TRIES   3
+#define CONNECT_RFCOMM_RETRY_MS        100
+
 /* Silence warning about (currently) unused routine. */
 #define WITH_WRITE_TYPE_HANDLE 0
 
@@ -104,14 +106,6 @@
  * 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 */
 
@@ -217,12 +211,12 @@ SR_PRIV const char *sr_bt_adapter_get_address(size_t idx)
        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;
 
@@ -403,7 +397,7 @@ static int sr_bt_desc_open(struct sr_bt_desc *desc, int *id_ref)
                id = hci_get_route(NULL);
        }
        if (id < 0) {
-               sr_spew("devid failed");
+               sr_err("devid failed");
                return -1;
        }
        desc->devid = id;
@@ -668,6 +662,7 @@ SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
        struct sockaddr_l2 sl2;
        bdaddr_t mac;
        int s, ret;
+       gint64 deadline;
 
        if (!desc)
                return -1;
@@ -710,6 +705,8 @@ SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
                }
        }
 
+       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;
@@ -745,6 +742,10 @@ SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
                                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);
                memset(fds, 0, sizeof(fds));
                fds[0].fd = s;
@@ -789,7 +790,7 @@ SR_PRIV int sr_bt_connect_ble(struct sr_bt_desc *desc)
 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;
@@ -801,25 +802,40 @@ SR_PRIV int sr_bt_connect_rfcomm(struct sr_bt_desc *desc)
        if (!desc->rfcomm_channel)
                desc->rfcomm_channel = 1;
 
-       fd = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
-       if (fd < 0) {
-               perror("socket");
-               return -1;
-       }
-       desc->fd = fd;
-
        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)
@@ -874,7 +890,7 @@ SR_PRIV int sr_bt_start_notify(struct sr_bt_desc *desc)
        if (sr_bt_check_socket_usable(desc) < 0)
                return -2;
 
-       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;
@@ -1024,7 +1040,7 @@ static ssize_t sr_bt_write_type_handle_bytes(struct sr_bt_desc *desc,
                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));