diff --git a/src/bin/e_sys_l2ping.c b/src/bin/e_sys_l2ping.c index 250f723d6..731ea386a 100644 --- a/src/bin/e_sys_l2ping.c +++ b/src/bin/e_sys_l2ping.c @@ -4,97 +4,170 @@ #ifdef HAVE_BLUETOOTH #include - #include #include +#include +#include #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 recv_buf[L2CAP_CMD_HDR_SIZE + 1]; + char send_buf[L2CAP_CMD_HDR_SIZE + MAX_SZ]; + 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 */ - fd = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); - 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) + // Create socket + fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_NONBLOCK, BTPROTO_L2CAP); + if (fd < 0) { - perror("Can't bind socket"); - return -1; + perror("Can't create socket"); + 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) - { - perror("Can't connect"); - return -1; - } - + FD_ZERO(&rfds); + FD_ZERO(&wfds); + FD_ZERO(&exfds); + 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; - send_cmd->ident = 200; - send_cmd->len = htobs(1); - send_cmd->code = L2CAP_ECHO_REQ; - send_buf[L2CAP_CMD_HDR_SIZE] = 'A'; + size = 44; // use std 44 byte ping size, but no more than MAX_SZ - 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"); - return -1; - } + FD_ZERO(&rfds); + 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->len = btohs(recv_cmd->len); - if (recv_cmd->ident != 200) - return -1; /* Wrong packet */ + recv_cmd = (l2cap_cmd_hdr *)recv_buf; + recv_cmd->len = btohs(recv_cmd->len); + // we only want the 200 ident response packets + 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 } diff --git a/src/bin/e_sys_main.c b/src/bin/e_sys_main.c index fee9ae97a..509d9b117 100644 --- a/src/bin/e_sys_main.c +++ b/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); - fputs(tmp, stdout); + eina_convert_dtoa(latency, tmp); + fputs(tmp, stdout); - return (latency < 0) ? 1 : 0; + return (latency < 0) ? 1 : 0; } /* sanitize environment */ #ifdef HAVE_UNSETENV