e_sys - fix l2ping code to work and actually properly ping

it wasn't working. first response may not be a 200 ident so keep
looking for them. also send a bit more than 1 byte to be sure, and
chekc the response is what we sent to be sure. also enforce a timeout
(10sec here) where we give up so it doesn't hang possibly forever.
all in all l2ping in e_sys works again. now. in the process i added a
timeout param too.

@fix

src/bin/e_sys_l2ping.c

@@ -4,97 +4,170 @@
 
 #ifdef HAVE_BLUETOOTH
 #include <unistd.h>
-
 #include <bluetooth/bluetooth.h>
 #include <bluetooth/l2cap.h>
+#include <sys/select.h>
+#include <fcntl.h>
 #endif
 
+#define MAX_SZ 500
+
 double
-e_sys_l2ping(const char *bluetooth_mac)
+e_sys_l2ping(const char *bluetooth_mac, int timeout_ms)
 {
 #ifdef HAVE_BLUETOOTH
-   char send_buf[L2CAP_CMD_HDR_SIZE + 1];
+   char send_buf[L2CAP_CMD_HDR_SIZE + MAX_SZ];
-   char recv_buf[L2CAP_CMD_HDR_SIZE + 1];
+   char recv_buf[L2CAP_CMD_HDR_SIZE + MAX_SZ];
    char tmp[18];
+   bdaddr_t bdaddr;
    l2cap_cmd_hdr *send_cmd;
    l2cap_cmd_hdr *recv_cmd;
    struct sockaddr_l2 addr;
    socklen_t optlen;
    double start;
-   int fd;
+   int fd, err, size, i;
+   fd_set rfds, wfds, exfds;
+   struct timeval tv;
 
-   /* Create socket */
+   // Create socket
-   fd = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
+   fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_NONBLOCK, BTPROTO_L2CAP);
-   if (fd < 0) {
+   if (fd < 0)
-      perror("Can't create socket");
-      return -1;
-   }
-
-   /* Bind to local address */
-   memset(&addr, 0, sizeof(addr));
-   addr.l2_family = AF_BLUETOOTH;
-   bacpy(&addr.l2_bdaddr, BDADDR_ANY);
-
-   if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
      {
-        perror("Can't bind socket");
+        perror("Can't create socket");
-	return -1;
+        return -1;
      }
 
-   /* Connect to remote device */
+   // Bind to local address
+   memset(&addr, 0, sizeof(addr));
+   addr.l2_family = AF_BLUETOOTH;
+   bacpy(&bdaddr, BDADDR_ANY);
+   bacpy(&addr.l2_bdaddr, &bdaddr);
+   if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0)
+     {
+        perror("Can't bind socket");
+        return -1;
+     }
+
+   fcntl(fd, F_SETFL, O_NONBLOCK);
+
+   // Connect to remote device
    memset(&addr, 0, sizeof(addr));
    addr.l2_family = AF_BLUETOOTH;
    str2ba(bluetooth_mac, &addr.l2_bdaddr);
+   connect(fd, (struct sockaddr *)&addr, sizeof(addr));
 
-   if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
+   FD_ZERO(&rfds);
-     {
+   FD_ZERO(&wfds);
-        perror("Can't connect");
+   FD_ZERO(&exfds);
-	return -1;
+   FD_SET(fd, &wfds);
-     }
+   tv.tv_sec = timeout_ms / 1000;
-
+   tv.tv_usec = (timeout_ms % 1000) * 1000;
    start = ecore_time_get();
+   err = select(fd + 1, &rfds, &wfds, &exfds, &tv);
+   if (err == 0)
+     {
+        fprintf(stderr, "Connect timeout %s\n", bluetooth_mac);
+        return -1;
+     }
+   // adjust timeout by how long we waited to connect
+   timeout_ms -= (ecore_time_get() - start) * 1000;
+   if (timeout_ms < 1) timeout_ms = 1;
 
-   /* Get local address */
+   // Get local address
    memset(&addr, 0, sizeof(addr));
    optlen = sizeof(addr);
-
+   if (getsockname(fd, (struct sockaddr *)&addr, &optlen) < 0)
-   if (getsockname(fd, (struct sockaddr *) &addr, &optlen) < 0)
      {
         perror("Can't get local address");
-	return -1;
+        return -1;
      }
 
    ba2str(&addr.l2_bdaddr, tmp);
 
-   send_cmd = (l2cap_cmd_hdr *) send_buf;
+   size = 44; // use std 44 byte ping size, but no more than MAX_SZ
-   send_cmd->ident = 200;
-   send_cmd->len = htobs(1);
-   send_cmd->code = L2CAP_ECHO_REQ;
-   send_buf[L2CAP_CMD_HDR_SIZE] = 'A';
 
-   if (send(fd, send_buf, L2CAP_CMD_HDR_SIZE + 1, 0) <= 0)
+   send_cmd = (l2cap_cmd_hdr *)send_buf;
+   send_cmd->ident = 200;
+   send_cmd->len = htobs(size);
+   send_cmd->code = L2CAP_ECHO_REQ;
+   // init buffer with some content
+   for (i = 0; i < size; i++)
+     {
+        // ABCDEF....WXYZABCEF... up to "size" chars
+        send_buf[L2CAP_CMD_HDR_SIZE + i] = 'A' + (i % 26);
+     }
+   // get our time just before a send
+   start = ecore_time_get();
+   // send the ping
+   if (send(fd, send_buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0)
      {
         perror("Send failed");
-	return -1;
+        return -1;
      }
 
-   if (recv(fd, recv_buf, L2CAP_CMD_HDR_SIZE + 1, 0) < 0)
+   do
      {
-        perror("Recv failed");
+        FD_ZERO(&rfds);
-	return -1;
+        FD_ZERO(&wfds);
-     }
+        FD_ZERO(&exfds);
+        FD_SET(fd, &rfds);
+        tv.tv_sec = timeout_ms / 1000;
+        tv.tv_usec = (timeout_ms % 1000) * 1000;
+        err = select(fd + 1, &rfds, &wfds, &exfds, &tv);
+        if (err < 0)
+          {
+             perror("Select failed");
+             return -1;
+          }
+        else if (err == 0)
+          {
+             fprintf(stderr, "Select timeout %s\n", bluetooth_mac);
+             return -1;
+          }
+        err = recv(fd, recv_buf, L2CAP_CMD_HDR_SIZE + size, 0);
+        if (err == 0)
+          {
+             fprintf(stderr, "Disconnect %s\n", bluetooth_mac);
+             return -1;
+          }
+        else if (err < 0)
+          {
+             perror("Recv failed");
+             return -1;
+          }
 
-   recv_cmd = (l2cap_cmd_hdr *) recv_buf;
+        recv_cmd = (l2cap_cmd_hdr *)recv_buf;
-   recv_cmd->len = btohs(recv_cmd->len);
+        recv_cmd->len = btohs(recv_cmd->len);
-   if (recv_cmd->ident != 200)
+        // we only want the 200 ident response packets
-     return -1; /* Wrong packet */
+        if (recv_cmd->ident != 200) continue;
+        if (recv_cmd->code == L2CAP_COMMAND_REJ)
+          {
+             fprintf(stderr, "Peer %s doesn't do echo\n", bluetooth_mac);
+             return -1;
+          }
+        if (recv_cmd->len != size)
+          {
+             fprintf(stderr, "Size %i echo for %s does not match %i\n",
+                     recv_cmd->len, bluetooth_mac, size);
+             return -1;
+          }
+        if (memcmp(send_buf + L2CAP_CMD_HDR_SIZE,
+                   recv_buf + L2CAP_CMD_HDR_SIZE, size) != 0)
+          {
+             fprintf(stderr, "Echo response for %s data does not match sent data\n", bluetooth_mac);
+             return -1;
+          }
+        fprintf(stderr, "Device %s responded\n", bluetooth_mac);
+        break;
+     }
+   while (1);
 
    close(fd);
-
+   // time it took to send and get our response
    return ecore_time_get() - start;
 #else
    (void) bluetooth_mac;
-   fprintf(stderr, "e_sys_l2ping nop\n");
+   fprintf(stderr, "e_sys_l2ping nop %s\n", bluetooth_mac);
    return -1;
 #endif
 }

src/bin/e_sys_main.c

@@ -31,7 +31,7 @@
 extern char **environ;
 #endif
 
-double e_sys_l2ping(const char *bluetooth_mac);
+double e_sys_l2ping(const char *bluetooth_mac, int timeout_ms);
 
 /* local subsystem functions */
 #ifdef HAVE_EEZE_MOUNT
@@ -61,7 +61,7 @@ main(int argc,
    int i, gn;
    int test = 0;
    char *action = NULL, *cmd;
-   char *output = NULL;
+   char *output = NULL, *param = NULL;
 #ifdef HAVE_EEZE_MOUNT
    Eina_Bool mnt = EINA_FALSE;
    const char *act = NULL;
@@ -93,6 +93,7 @@ main(int argc,
           {
              action = argv[1];
              output = argv[2];
+             if (argc >= 4) param = argv[3];
           }
         else if (!strcmp(argv[1], "rfkill-unblock"))
           {
@@ -130,7 +131,6 @@ main(int argc,
         exit(1);
      }
    if (!action) exit(1);
-   fprintf(stderr, "action %s %i\n", action, argc);
 
    eina_init();
 
@@ -175,14 +175,18 @@ main(int argc,
    if (!test && !strcmp(action, "l2ping"))
      {
         char tmp[128];
-	double latency;
+        double latency;
+        int timeout = 10 * 1000;
 
-	latency = e_sys_l2ping(output);
+        if (param) timeout = atoi(param);
+        if (timeout < 1) timeout = 1;// min timeout 1ms
+        else if (timeout > (60 * 60 * 1000)) timeout = 60 * 60 * 1000;// max 1h
+        latency = e_sys_l2ping(output, timeout);
 
-	eina_convert_dtoa(latency, tmp);
+        eina_convert_dtoa(latency, tmp);
-	fputs(tmp, stdout);
+        fputs(tmp, stdout);
 
-	return (latency < 0) ? 1 : 0;
+        return (latency < 0) ? 1 : 0;
      }
    /* sanitize environment */
 #ifdef HAVE_UNSETENV