libaltos: Add btle test program

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

2 files changed, 188 insertions, 1 deletions

diff --git a/libaltos/Makefile.am b/libaltos/Makefile.am
index 8f69c1ad..69fe7a57 100644
--- 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
index 00000000..0e325415
--- /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;
+}