libaltos: Add btle test program

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/libaltos/Makefile.am b/libaltos/Makefile.am
index 8f69c1ad9661df9b95290c007d89467a8ca8ec9a..69fe7a5716a96d002eb94cb4854dc7f0aeb6d4b2 100644 (file)
--- a/libaltos/Makefile.am
+++ b/libaltos/Makefile.am
@@ -24,11 +24,15 @@ WINDOWS_SRC=\
 WINDOWS_H=\
        libaltos.h
 
-noinst_PROGRAMS=cjnitest
+noinst_PROGRAMS=cjnitest btletest
 
 cjnitest_SOURCES=cjnitest.c
 cjnitest_LDADD=libaltos.la
 
+btletest_SOURCES=btletest.c
+
+btletest_LDADD=-lbluetooth
+
 if MULTI_ARCH
 altoslib_LTLIBRARIES+=libaltos32.la libaltos64.la
 

diff --git a/libaltos/btletest.c b/libaltos/btletest.c
new file mode 100644 (file)
index 0000000..0e32541
--- /dev/null
+++ b/libaltos/btletest.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright © 2017 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <errno.h>
+#include <unistd.h>
+#include <ctype.h>
+#include <dirent.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/rfcomm.h>
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+#include <bluetooth/l2cap.h>
+#include <poll.h>
+
+#define ATT_OP_MTU_REQ         0x02
+#define ATT_OP_MTU_RESP                0x03
+#define ATT_OP_WRITE_CMD       0x52
+#define ATT_OP_HANDLE_NOTIFY   0x1b
+#define CID_ATT                        0x0004
+#define TX_ENDPOINT            0x003a
+#define RX_ENDPOINT            0x0037
+#define RX_NOTIFY              0x0038
+
+int
+main(int argc, char **argv)
+{
+       int sk;
+       int psm;
+       struct sockaddr_l2 src_addr = { 0 };
+       struct sockaddr_l2 dst_addr = { 0 };
+       char buf[1024];
+       struct pollfd   fd[2];
+       int n, i;
+       char *btaddr;
+       int     mtu;
+
+       btaddr = argc > 1 ? argv[1] : "D8:80:39:F3:4E:A5";
+
+       sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+       if (sk < 0) {
+               perror("socket");
+               exit(1);
+       }
+
+       src_addr.l2_family = AF_BLUETOOTH;
+       /* Leave src_addr.l2_bdaddr all zeros */
+       src_addr.l2_cid = htobs(CID_ATT);
+       src_addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;
+       if (bind(sk, (struct sockaddr *) &src_addr, sizeof (src_addr)) < 0) {
+               perror("bind");
+               exit(1);
+       }
+
+       dst_addr.l2_family = AF_BLUETOOTH;
+       str2ba(btaddr, &dst_addr.l2_bdaddr);
+       dst_addr.l2_cid = htobs(CID_ATT);
+       dst_addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;
+
+       if (connect(sk, (struct sockaddr *) &dst_addr, sizeof(dst_addr)) < 0) {
+               perror("connect");
+               exit(1);
+       }
+
+       buf[0] = ATT_OP_MTU_REQ;
+       buf[1] = sizeof(buf) & 0xff;
+       buf[2] = sizeof(buf) >> 8;
+       n = write(sk, buf, 3);
+       if (n != 3) {
+               perror("write mtu\n");
+               exit(1);
+       }
+
+       fd[0].fd = sk;
+       fd[0].events = POLLIN;
+       for (;;) {
+               n = poll(fd, 1, 3000);
+               if (n <= 0) {
+                       printf("timeout waiting for MTU response\n");
+                       exit(1);
+               }
+               if (fd[0].revents & POLLIN) {
+                       n = read(sk, buf, sizeof(buf));
+                       printf("read %d\n", n);
+                       for (i = 0; i < n; i++)
+                               printf("%02x\n", buf[i]);
+                       if (buf[0] == ATT_OP_MTU_RESP) {
+                               mtu = (buf[1] & 0xff) | ((buf[2] & 0xff) << 8);
+                               break;
+                       }
+               }
+       }
+       printf("mtu %d\n", mtu);
+
+       buf[0] = ATT_OP_WRITE_CMD;
+       buf[1] = RX_NOTIFY & 0xff;
+       buf[2] = RX_NOTIFY >> 8;
+       buf[3] = 1;
+       n = write(sk, buf, 4);
+       if (n != 4) {
+               perror("write notify");
+               exit(1);
+       }
+
+       fd[0].fd = 0;
+       fd[0].events = POLLIN;
+       fd[1].fd = sk;
+       fd[1].events = POLLIN;
+
+       for (;;) {
+               n = poll(fd, 2, -1);
+               if (n == 0)
+                       continue;
+               if (fd[0].revents & POLLIN) {
+                       char    *b;
+                       n = read(0, buf+3, sizeof(buf)-3);
+                       if (n < 0) {
+                               perror("read stdin");
+                               exit(1);
+                       }
+                       if (n == 0)
+                               break;
+
+                       b = buf;
+                       while (n > 0) {
+                               int this = n;
+                               if (this > mtu - 3)
+                                       this = mtu - 3;
+
+                               b[0] = ATT_OP_WRITE_CMD;
+                               b[1] = TX_ENDPOINT;
+                               b[2] = TX_ENDPOINT >> 8;
+                               if (write(sk, b, this + 3) != this + 3) {
+                                       perror("write sk");
+                                       exit(1);
+                               }
+                               b += this;
+                               n -= this;
+                       }
+               }
+               if (fd[1].revents & POLLIN) {
+                       uint16_t        ch;
+
+                       n = read(sk, buf, sizeof(buf));
+                       if (n < 0) {
+                               perror("read sk");
+                               exit(1);
+                       }
+                       if (n == 0)
+                               continue;
+                       ch = buf[1] | (buf[2] << 8);
+                       switch (buf[0]) {
+                       case ATT_OP_HANDLE_NOTIFY:
+                               if (ch == RX_ENDPOINT)
+                                       write(1, buf+3, n-3);
+                               break;
+                       }
+               }
+               if (fd[1].revents & (POLLERR|POLLHUP))
+                       break;
+       }
+       close(sk);
+
+       return 0;
+}