[RFC 1/2] btproxy: Add support for creating pseudoterminal

[RFC 1/2] btproxy: Add support for creating pseudoterminal

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new virtual Bluetooth serial controller on /dev/pts/X which allows
to test serial attach with btattach and hciattach.
---
 tools/btproxy.c | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 65 insertions(+), 3 deletions(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 43de037..6d78876 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -629,6 +629,41 @@ static int open_unix(const char *path)
 	return fd;
 }
 
+static int open_pty(void)
+{
+	int fd;
+	char *pts;
+
+	fd = open("/dev/ptmx", O_RDWR | O_NOCTTY);
+	if (fd < 0) {
+		perror("Failed to open pseudoterminal master");
+		return -1;
+	}
+
+	if (grantpt(fd) < 0) {
+		perror("Failed to grant access to slave pty");
+		close(fd);
+		return -1;
+	}
+
+	if (unlockpt(fd) < 0) {
+		perror("Failed to unlock slave pty");
+		close(fd);
+		return -1;
+	}
+
+	pts = ptsname(fd);
+	if (!pts) {
+		perror("Failed to get slave pty");
+		close(fd);
+		return -1;
+	}
+
+	printf("New pts created: %s\n", pts);
+
+	return fd;
+}
+
 static int open_tcp(const char *address, unsigned int port)
 {
 	struct sockaddr_in addr;
@@ -728,6 +763,7 @@ static void usage(void)
 		"\t-c, --connect <address>     Connect to server\n"
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
+		"\t-P, --pty                   Use PTY\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -741,6 +777,7 @@ static const struct option main_options[] = {
 	{ "connect",  required_argument, NULL, 'c' },
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
+	{ "pty",      no_argument,       NULL, 'P' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -758,6 +795,7 @@ int main(int argc, char *argv[])
 	const char *unix_path = NULL;
 	unsigned short tcp_port = 0xb1ee;	/* 45550 */
 	bool use_redirect = false;
+	bool use_pty = false;
 	uint8_t type = HCI_BREDR;
 	const char *str;
 	sigset_t mask;
@@ -765,7 +803,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::p:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -789,6 +827,9 @@ int main(int argc, char *argv[])
 			else
 				unix_path = "/tmp/bt-server-bredr";
 			break;
+		case 'P':
+			use_pty = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -828,12 +869,13 @@ int main(int argc, char *argv[])
 		return EXIT_FAILURE;
 	}
 
-	if (unix_path && (server_address || use_redirect)) {
+	if (unix_path && (server_address || use_redirect || use_pty)) {
 		fprintf(stderr, "Invalid to specify TCP and Unix servers\n");
 		return EXIT_FAILURE;
 	}
 
-	if (connect_address && (unix_path || server_address || use_redirect)) {
+	if (connect_address && (unix_path || server_address || use_redirect ||
+								use_pty)) {
 		fprintf(stderr, "Invalid to specify client and server mode\n");
 		return EXIT_FAILURE;
 	}
@@ -876,6 +918,26 @@ int main(int argc, char *argv[])
 			close(host_fd);
 			return EXIT_FAILURE;
 		}
+	} else if (use_pty) {
+		int master_fd, dev_fd;
+
+		printf("Opening pseudoterminal\n");
+
+		master_fd = open_pty();
+		if (master_fd < 0)
+			return EXIT_FAILURE;
+
+		dev_fd = open_channel(hci_index);
+		if (dev_fd < 0) {
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
+
+		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+			close(dev_fd);
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
 	} else {
 		int server_fd;
 
-- 
2.5.0

[RFC 2/2] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete
---
 tools/btproxy.c | 541 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 535 insertions(+), 6 deletions(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 6d78876..c718e48 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -47,23 +47,22 @@
 #include "src/shared/util.h"
 #include "src/shared/mainloop.h"
 #include "src/shared/ecc.h"
+#include "src/shared/queue.h"
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
 #include "monitor/bt.h"
 
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
 
 #define BTPROTO_HCI	1
-struct sockaddr_hci {
-	sa_family_t	hci_family;
-	unsigned short	hci_dev;
-	unsigned short  hci_channel;
-};
 #define HCI_CHANNEL_USER	1
 
 static uint16_t hci_index = 0;
 static bool client_active = false;
 static bool debug_enabled = false;
 static bool emulate_ecc = false;
+static bool three_wire = false;
 
 static void hexdump_print(const char *str, void *user_data)
 {
@@ -287,6 +286,516 @@ static void host_read_destroy(void *user_data)
 		mainloop_remove_fd(proxy->dev_fd);
 }
 
+/* three-wire (H5) packet processing */
+
+#define H5_ACK_TIMEOUT		250
+
+#define HCI_3WIRE_ACK_PKT	0
+#define HCI_3WIRE_LINK_PKT	15
+
+/*
+ * Maximum Three-wire packet:
+ *     4 byte header + max value for 12-bit length + 2 bytes for CRC
+ */
+#define H5_MAX_LEN (4 + 0xfff + 2)
+
+/* Sliding window size */
+#define H5_TX_WIN_MAX		4
+
+#define SLIP_DELIMITER	0xc0
+#define SLIP_ESC	0xdb
+#define SLIP_ESC_DELIM	0xdc
+#define SLIP_ESC_ESC	0xdd
+
+#define H5_RX_ESC	1
+
+#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
+#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
+#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
+#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
+#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
+#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
+
+#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
+#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
+#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
+#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
+#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
+						((hdr)[2] |= (len) >> 4))
+
+static const uint8_t sync_req[] = { 0x01, 0x7e };
+static const uint8_t sync_rsp[] = { 0x02, 0x7d };
+static const uint8_t conf_req[] = { 0x03, 0xfc };
+static const uint8_t wakeup_req[] = { 0x05, 0xfa };
+static const uint8_t woken_req[] = { 0x06, 0xf9 };
+static const uint8_t sleep_req[] = { 0x07, 0x78 };
+
+static struct h5 {
+	size_t rx_pending;
+	uint8_t rx_buf[H5_MAX_LEN];
+	uint8_t *rx_ptr;
+	uint8_t flags;
+
+	uint8_t rx_ack;		/* Received last ack number */
+	uint8_t tx_seq;		/* Next seq number to send */
+	uint8_t tx_ack;		/* Next ack number to send */
+
+	uint8_t tx_win;
+
+	enum {
+		H5_UNINITIALIZED,
+		H5_INITIALIZED,
+		H5_ACTIVE,
+	} state;
+
+	int timer_id;
+
+	struct queue *tx_queue_unack;
+	struct queue *tx_queue_unrel;
+	struct queue *tx_queue_rel;
+
+	int (*rx_func)(struct proxy *proxy, uint8_t byte);
+} h5;
+
+struct h5_pkt {
+	uint16_t len;
+	uint8_t data[0];
+};
+
+static void h5_reset_rx(void);
+
+static void h5_reset_peer(void)
+{
+	h5.state = H5_UNINITIALIZED;
+
+	mainloop_remove_timeout(h5.timer_id);
+	h5.timer_id = -1;
+	h5_reset_rx();
+}
+
+static void h5_init(void)
+{
+	h5.tx_win = H5_TX_WIN_MAX;
+	h5_reset_peer();
+
+	h5.tx_queue_unack = queue_new();
+	h5.tx_queue_unrel = queue_new();
+	h5.tx_queue_rel = queue_new();
+}
+
+static bool h5_reliable_pkt(uint8_t type)
+{
+	switch (type) {
+	case HCI_ACLDATA_PKT:
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static uint8_t h5_slip_byte(uint8_t *pkt, uint8_t byte)
+{
+	const uint8_t esc_delim[] = { SLIP_ESC, SLIP_ESC_DELIM };
+	const uint8_t esc_esc[] = { SLIP_ESC, SLIP_ESC_ESC };
+
+	switch (byte) {
+	case SLIP_DELIMITER:
+		memcpy(pkt, &esc_delim, sizeof(esc_delim));
+		return sizeof(esc_delim);
+	case SLIP_ESC:
+		memcpy(pkt, &esc_esc, sizeof(esc_esc));
+		return sizeof(esc_esc);
+	default:
+		memcpy(pkt, &byte, sizeof(byte));
+		return sizeof(byte);
+	}
+}
+
+static void h5_print_header(const uint8_t *hdr, const char *str)
+{
+	if (H5_HDR_RELIABLE(hdr))
+		printf("%s REL: seq %u ack %u crc %u type %u len %u\n",
+				str, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
+				H5_HDR_CRC(hdr), H5_HDR_PKT_TYPE(hdr),
+				H5_HDR_LEN(hdr));
+	else
+		printf("%s UNREL: ack %u crc %u type %u len %u\n",
+				str, H5_HDR_ACK(hdr), H5_HDR_CRC(hdr),
+				H5_HDR_PKT_TYPE(hdr), H5_HDR_LEN(hdr));
+}
+
+static void h5_send(const uint8_t *payload, uint8_t pkt_type, int len)
+{
+	struct h5_pkt *pkt;
+	uint8_t *ptr;
+	uint8_t hdr[4];
+	int i;
+
+	memset(hdr, 0, sizeof(hdr));
+
+	H5_SET_ACK(hdr, h5.tx_ack);
+
+	if (h5_reliable_pkt(pkt_type)) {
+		H5_SET_RELIABLE(hdr);
+		H5_SET_SEQ(hdr, h5.tx_seq);
+		h5.tx_seq = (h5.tx_seq + 1) % 8;
+	}
+
+	H5_SET_TYPE(hdr, pkt_type);
+	H5_SET_LEN(hdr, len);
+
+	/* Calculate CRC */
+	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
+
+	if (debug_enabled)
+		h5_print_header(hdr, "TX: <");
+
+	/*
+	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
+	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
+	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
+	 * delimiters at start and end).
+	 */
+	pkt = malloc(sizeof(*pkt) + (len + 6) * 2 + 2);
+	if (!pkt)
+		return;
+
+	ptr = pkt->data;
+
+	*ptr++ = SLIP_DELIMITER;
+
+	for (i = 0; i < 4; i++)
+		ptr += h5_slip_byte(ptr, hdr[i]);
+
+	for (i = 0; i < len; i++)
+		ptr += h5_slip_byte(ptr, payload[i]);
+
+	*ptr++ = SLIP_DELIMITER;
+
+	pkt->len = ptr - pkt->data;
+
+	if (h5_reliable_pkt(pkt_type))
+		queue_push_tail(h5.tx_queue_rel, pkt);
+	else
+		queue_push_tail(h5.tx_queue_unrel, pkt);
+}
+
+static void h5_process_sig_pkt(void)
+{
+	uint8_t conf_rsp[3] = { 0x04, 0x7b };
+	const uint8_t *hdr = h5.rx_buf;
+	const uint8_t *payload = hdr + 4;
+
+	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
+		return;
+
+	if (H5_HDR_LEN(hdr) < 2)
+		return;
+
+	conf_rsp[2] = h5.tx_win & 0x07;
+
+	if (!memcmp(payload, sync_req, sizeof(sync_req))) {
+		if (h5.state == H5_ACTIVE)
+			h5_reset_peer();
+
+		h5_send(sync_rsp, H5_HDR_PKT_TYPE(hdr), sizeof(sync_rsp));
+		return;
+	} else if (!memcmp(payload, conf_req, 2)) {
+		h5_send(conf_rsp, H5_HDR_PKT_TYPE(hdr), sizeof(conf_rsp));
+		return;
+	}
+
+	exit(1);
+}
+
+static void h5_process_data(struct proxy *proxy, uint8_t pkt_type)
+{
+	uint8_t pkt[1 + h5.rx_ptr - 4 - h5.rx_buf];
+
+	pkt[0] = pkt_type;
+	memcpy(&pkt[1], h5.rx_buf + 4, sizeof(pkt) - 1);
+
+	host_write_packet(proxy, pkt, sizeof(pkt));
+}
+
+static void pkt_print(void *data, void *user_data)
+{
+	struct h5_pkt *pkt = data;
+
+	h5_print_header((uint8_t *)pkt + sizeof(*pkt) + 1, "unack pkt");
+}
+
+static void h5_process_unack_queue(void)
+{
+	struct h5_pkt *pkt;
+	uint8_t len = queue_length(h5.tx_queue_unack);
+	uint8_t seq = h5.tx_seq;
+
+	if (!len)
+		return;
+
+	printf("%s: rx_ack %u tx_ack %u tx_seq %u\n", __func__,
+					h5.rx_ack, h5.tx_ack, h5.tx_seq);
+
+	printf("%s: unack queue length %u\n", __func__, len);
+
+	queue_foreach(h5.tx_queue_unack, pkt_print, NULL);
+
+	/* Remove acked packets from unack queue */
+	while (len > 0) {
+		if (h5.rx_ack == seq)
+			break;
+
+		len--;
+		seq = (seq - 1) & 0x07;
+	}
+
+	if (seq != h5.rx_ack)
+		fprintf(stderr, "Acked wrong packet\n");
+
+	while (len--) {
+		printf("%s: Remove packet\n", __func__);
+
+		pkt = queue_pop_head(h5.tx_queue_unack);
+		if (pkt)
+			free(pkt);
+	}
+}
+
+static void h5_process_complete_pkt(struct proxy *proxy)
+{
+	const uint8_t *hdr = h5.rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		h5.tx_ack = (h5.tx_ack + 1) % 8;
+		/* TODO: Set ack req flag */
+	}
+
+	h5.rx_ack = H5_HDR_ACK(hdr);
+
+	h5_process_unack_queue();
+
+	switch (H5_HDR_PKT_TYPE(hdr)) {
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+	case HCI_ACLDATA_PKT:
+	case HCI_SCODATA_PKT:
+		/* Need to remove three-wire header */
+		h5_process_data(proxy, (H5_HDR_PKT_TYPE(hdr)));
+		break;
+	default:
+		/* Handle three-wire protocol */
+		h5_process_sig_pkt();
+	}
+
+	h5_reset_rx();
+}
+
+static int h5_crc(struct proxy *proxy, const uint8_t byte)
+{
+	h5_process_complete_pkt(proxy);
+
+	return 0;
+}
+
+static int h5_payload(struct proxy *proxy, const uint8_t byte)
+{
+	const uint8_t *hdr = h5.rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		/* TODO: process tx_ack */
+	}
+
+	if (H5_HDR_CRC(hdr)) {
+		h5.rx_func = h5_crc;
+		h5.rx_pending = 2;
+	} else {
+		h5_process_complete_pkt(proxy);
+	}
+
+	return 0;
+}
+
+static int h5_3wire_hdr(struct proxy *proxy, const uint8_t byte)
+{
+	const uint8_t *hdr = h5.rx_buf;
+
+	if (debug_enabled)
+		h5_print_header(hdr, "RX: >");
+
+	/* Add header checks here */
+
+	h5.rx_func = h5_payload;
+	h5.rx_pending = H5_HDR_LEN(hdr);
+
+	return 0;
+}
+
+static int h5_pkt_start(struct proxy *proxy, const uint8_t byte)
+{
+	if (byte == SLIP_DELIMITER)
+		return 1;
+
+	h5.rx_func = h5_3wire_hdr;
+	h5.rx_pending = 4;
+
+	return 0;
+}
+
+static int h5_delimeter(struct proxy *proxy, const uint8_t byte)
+{
+	if (byte == SLIP_DELIMITER)
+		h5.rx_func = h5_pkt_start;
+
+	return 1;
+}
+
+static void h5_reset_rx(void)
+{
+	h5.rx_pending = 0;
+	h5.flags = 0;
+
+	h5.rx_ptr = h5.rx_buf;
+
+	h5.rx_func = h5_delimeter;
+}
+
+static void h5_process_byte(const uint8_t byte)
+{
+	/* Mark escaped */
+	if (!(h5.flags & H5_RX_ESC) && byte == SLIP_ESC) {
+		h5.flags |= H5_RX_ESC;
+		return;
+	}
+
+	if (h5.flags & H5_RX_ESC) {
+		h5.flags &= ~H5_RX_ESC;
+
+		switch (byte) {
+		case SLIP_ESC_DELIM:
+			*h5.rx_ptr++ = SLIP_DELIMITER;
+			break;
+		case SLIP_ESC_ESC:
+			*h5.rx_ptr++ = SLIP_ESC;
+			break;
+		default:
+			fprintf(stderr, "Invalid esc byte %x\n", byte);
+			h5_reset_rx();
+			return;
+		}
+	} else {
+		*h5.rx_ptr++ = byte;
+	}
+
+	h5.rx_pending--;
+}
+
+static void h5_process_tx_queue(struct proxy *proxy);
+
+static void h5_timer_cb(int id, void *user_data)
+{
+	struct proxy *proxy = user_data;
+	struct h5_pkt *pkt;
+
+	printf("%s: Retransmitting %u packets\n", __func__,
+					queue_length(h5.tx_queue_unack));
+
+	while ((pkt = queue_peek_tail(h5.tx_queue_unack))) {
+		h5.tx_seq = (h5.tx_seq - 1) & 0x07;
+
+		/* Move pkt from unack to rel queue */
+		queue_remove(h5.tx_queue_unack, pkt);
+		queue_push_head(h5.tx_queue_rel, pkt);
+	}
+
+	h5_process_tx_queue(proxy);
+}
+
+static void h5_process_tx_queue(struct proxy *proxy)
+{
+	struct h5_pkt *pkt;
+
+	while ((pkt = queue_pop_head(h5.tx_queue_unrel))) {
+		if (!write_packet(proxy->host_fd, pkt->data, pkt->len, "H: ")) {
+			fprintf(stderr, "Write to Host descriptor failed\n");
+			mainloop_remove_fd(proxy->host_fd);
+		}
+
+		free(pkt);
+	}
+
+	if (queue_length(h5.tx_queue_unack) >= h5.tx_win) {
+		printf("Unacked queue too big\n");
+		return;
+	}
+
+	while ((pkt = queue_pop_head(h5.tx_queue_rel))) {
+		if (!write_packet(proxy->host_fd, pkt->data, pkt->len, "H: ")) {
+			fprintf(stderr, "Write to Host descriptor failed\n");
+			mainloop_remove_fd(proxy->host_fd);
+		}
+
+		queue_push_tail(h5.tx_queue_unack, pkt);
+		if (h5.timer_id >= 0) {
+			mainloop_modify_timeout(h5.timer_id, H5_ACK_TIMEOUT);
+			continue;
+		}
+
+		h5.timer_id = mainloop_add_timeout(H5_ACK_TIMEOUT, h5_timer_cb,
+								proxy, NULL);
+	}
+}
+
+static void host_write_3wire_packet(struct proxy *proxy, void *buf,
+				    uint16_t len)
+{
+	const uint8_t *ptr = buf;
+
+	while (len > 0) {
+		int processed;
+
+		if (h5.rx_pending > 0) {
+			if (*ptr == SLIP_DELIMITER) {
+				fprintf(stderr, "Too short packet\n");
+				h5_reset_rx();
+				continue;
+			}
+
+			h5_process_byte(*ptr);
+
+			ptr++;
+			len--;
+			continue;
+		}
+
+		processed = h5.rx_func(proxy, *ptr);
+		if (processed < 0) {
+			fprintf(stderr, "Error processing SLIP packet\n");
+			return;
+		}
+
+		ptr += processed;
+		len -= processed;
+
+		h5_process_tx_queue(proxy);
+	}
+
+	h5_process_tx_queue(proxy);
+}
+
+static void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf,
+								uint16_t len)
+{
+	uint8_t pkt_type = buf[0];
+
+	/* Get payload out of H4 packet */
+	h5_send(&buf[1], pkt_type, len - 1);
+
+	h5_process_tx_queue(proxy);
+}
+
 static void host_read_callback(int fd, uint32_t events, void *user_data)
 {
 	struct proxy *proxy = user_data;
@@ -351,6 +860,14 @@ process_packet:
 		sco_hdr = (void *) (proxy->host_buf + 1);
 		pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
 		break;
+	case SLIP_DELIMITER:
+		/* 2 bytes start/end packets + SLIP header and checksum */
+		if (proxy->host_len < 4 + 2)
+			return;
+
+		/* Later we shall check unslipped packet */
+		pktlen = proxy->host_len;
+		break;
 	case 0xff:
 		/* Notification packet from /dev/vhci - ignore */
 		proxy->host_len = 0;
@@ -367,6 +884,8 @@ process_packet:
 
 	if (emulate_ecc)
 		host_emulate_ecc(proxy, proxy->host_buf, pktlen);
+	else if (three_wire)
+		host_write_3wire_packet(proxy, proxy->host_buf, pktlen);
 	else
 		host_write_packet(proxy, proxy->host_buf, pktlen);
 
@@ -475,6 +994,8 @@ process_packet:
 
 	if (emulate_ecc)
 		dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
+	else if (three_wire)
+		dev_write_3wire_packet(proxy, proxy->dev_buf, pktlen);
 	else
 		dev_write_packet(proxy, proxy->dev_buf, pktlen);
 
@@ -764,6 +1285,7 @@ static void usage(void)
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
 		"\t-P, --pty                   Use PTY\n"
+		"\t-3, --3wire                 Use 3wire protocol\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -778,6 +1300,7 @@ static const struct option main_options[] = {
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
 	{ "pty",      no_argument,       NULL, 'P' },
+	{ "3wire",    no_argument,       NULL, '3' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -803,7 +1326,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -830,6 +1353,9 @@ int main(int argc, char *argv[])
 		case 'P':
 			use_pty = true;
 			break;
+		case '3':
+			three_wire = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -933,6 +1459,9 @@ int main(int argc, char *argv[])
 			return EXIT_FAILURE;
 		}
 
+		if (three_wire)
+			h5_init();
+
 		if (!setup_proxy(master_fd, false, dev_fd, true)) {
 			close(dev_fd);
 			close(master_fd);
-- 
2.5.0

Re: [RFC 2/2] btproxy: Add three-wire (h5) protocol initial support

[Marcel Holtmann]

Hi Andrei,

> With H5 support it is possible to create pts and attach to it using
> hciattach ot btattach with 3wire protocol. This is useful for testing
> and developing three-wire protocol.
> Implementation is based on kernel hci_h5.c H5 protocol.
> 
> Simple usage:
> To open virtual pts run:
> $ sudo tools/btproxy -d --pty -3
> Opening pseudoterminal
> New pts created: /dev/pts/2
> Opening user channel for hci0
> 
> Now attach to it using hciattach:
> $ sudo hciattach -n /dev/pts/2 3wire
> Device setup complete
> ---
> tools/btproxy.c | 541 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
> 1 file changed, 535 insertions(+), 6 deletions(-)
> 
> diff --git a/tools/btproxy.c b/tools/btproxy.c
> index 6d78876..c718e48 100644
> --- a/tools/btproxy.c
> +++ b/tools/btproxy.c
> @@ -47,23 +47,22 @@
> #include "src/shared/util.h"
> #include "src/shared/mainloop.h"
> #include "src/shared/ecc.h"
> +#include "src/shared/queue.h"
> +#include "lib/bluetooth.h"
> +#include "lib/hci.h"
> #include "monitor/bt.h"
> 
> #define HCI_BREDR	0x00
> #define HCI_AMP		0x01
> 
> #define BTPROTO_HCI	1
> -struct sockaddr_hci {
> -	sa_family_t	hci_family;
> -	unsigned short	hci_dev;
> -	unsigned short  hci_channel;
> -};
> #define HCI_CHANNEL_USER	1

please do not do this.

> static uint16_t hci_index = 0;
> static bool client_active = false;
> static bool debug_enabled = false;
> static bool emulate_ecc = false;
> +static bool three_wire = false;
> 
> static void hexdump_print(const char *str, void *user_data)
> {
> @@ -287,6 +286,516 @@ static void host_read_destroy(void *user_data)
> 		mainloop_remove_fd(proxy->dev_fd);
> }
> 
> +/* three-wire (H5) packet processing */
> +
> +#define H5_ACK_TIMEOUT		250
> +
> +#define HCI_3WIRE_ACK_PKT	0
> +#define HCI_3WIRE_LINK_PKT	15
> +
> +/*
> + * Maximum Three-wire packet:
> + *     4 byte header + max value for 12-bit length + 2 bytes for CRC
> + */
> +#define H5_MAX_LEN (4 + 0xfff + 2)
> +
> +/* Sliding window size */
> +#define H5_TX_WIN_MAX		4
> +
> +#define SLIP_DELIMITER	0xc0
> +#define SLIP_ESC	0xdb
> +#define SLIP_ESC_DELIM	0xdc
> +#define SLIP_ESC_ESC	0xdd
> +
> +#define H5_RX_ESC	1
> +
> +#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
> +#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
> +#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
> +#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
> +#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
> +#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
> +
> +#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
> +#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
> +#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
> +#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
> +#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
> +						((hdr)[2] |= (len) >> 4))
> +
> +static const uint8_t sync_req[] = { 0x01, 0x7e };
> +static const uint8_t sync_rsp[] = { 0x02, 0x7d };
> +static const uint8_t conf_req[] = { 0x03, 0xfc };
> +static const uint8_t wakeup_req[] = { 0x05, 0xfa };
> +static const uint8_t woken_req[] = { 0x06, 0xf9 };
> +static const uint8_t sleep_req[] = { 0x07, 0x78 };
> +
> +static struct h5 {
> +	size_t rx_pending;
> +	uint8_t rx_buf[H5_MAX_LEN];
> +	uint8_t *rx_ptr;
> +	uint8_t flags;
> +
> +	uint8_t rx_ack;		/* Received last ack number */
> +	uint8_t tx_seq;		/* Next seq number to send */
> +	uint8_t tx_ack;		/* Next ack number to send */
> +
> +	uint8_t tx_win;
> +
> +	enum {
> +		H5_UNINITIALIZED,
> +		H5_INITIALIZED,
> +		H5_ACTIVE,
> +	} state;
> +
> +	int timer_id;
> +
> +	struct queue *tx_queue_unack;
> +	struct queue *tx_queue_unrel;
> +	struct queue *tx_queue_rel;
> +
> +	int (*rx_func)(struct proxy *proxy, uint8_t byte);
> +} h5;

No global static variables please. That should be part of struct proxy instead. Global variables will cause problems if you ever try to start two instances.

> +
> +struct h5_pkt {
> +	uint16_t len;
> +	uint8_t data[0];
> +};

I think using get_unaligned_le16 to get the length would be a lot simpler.

> +
> +static void h5_reset_rx(void);
> +
> +static void h5_reset_peer(void)
> +{
> +	h5.state = H5_UNINITIALIZED;
> +
> +	mainloop_remove_timeout(h5.timer_id);
> +	h5.timer_id = -1;
> +	h5_reset_rx();
> +}

With that all said, I think we might want to create a tools/h5.[ch] to abstract this properly into a separate helper library.

Regards

Marcel

Re: [RFC 2/2] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

Hi Marcel,

On Thu, Nov 26, 2015 at 07:41:26AM -0800, Marcel Holtmann wrote:
> Hi Andrei,
> 
> > With H5 support it is possible to create pts and attach to it using
> > hciattach ot btattach with 3wire protocol. This is useful for testing
> > and developing three-wire protocol.
> > Implementation is based on kernel hci_h5.c H5 protocol.
> > 
> > Simple usage:
> > To open virtual pts run:
> > $ sudo tools/btproxy -d --pty -3
> > Opening pseudoterminal
> > New pts created: /dev/pts/2
> > Opening user channel for hci0
> > 
> > Now attach to it using hciattach:
> > $ sudo hciattach -n /dev/pts/2 3wire
> > Device setup complete
> > ---
> > tools/btproxy.c | 541 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
> > 1 file changed, 535 insertions(+), 6 deletions(-)
> > 
> > diff --git a/tools/btproxy.c b/tools/btproxy.c
> > index 6d78876..c718e48 100644
> > --- a/tools/btproxy.c
> > +++ b/tools/btproxy.c
> > @@ -47,23 +47,22 @@
> > #include "src/shared/util.h"
> > #include "src/shared/mainloop.h"
> > #include "src/shared/ecc.h"
> > +#include "src/shared/queue.h"
> > +#include "lib/bluetooth.h"
> > +#include "lib/hci.h"
> > #include "monitor/bt.h"
> > 
> > #define HCI_BREDR	0x00
> > #define HCI_AMP		0x01
> > 
> > #define BTPROTO_HCI	1
> > -struct sockaddr_hci {
> > -	sa_family_t	hci_family;
> > -	unsigned short	hci_dev;
> > -	unsigned short  hci_channel;
> > -};
> > #define HCI_CHANNEL_USER	1
> 
> please do not do this.

Why? I added #include "lib/hci.h" where this structure defined, why do
we need redefined it again? It was defined 3 times.

> > static uint16_t hci_index = 0;
> > static bool client_active = false;
> > static bool debug_enabled = false;
> > static bool emulate_ecc = false;
> > +static bool three_wire = false;
> > 
> > static void hexdump_print(const char *str, void *user_data)
> > {
> > @@ -287,6 +286,516 @@ static void host_read_destroy(void *user_data)
> > 		mainloop_remove_fd(proxy->dev_fd);
> > }
> > 
> > +/* three-wire (H5) packet processing */
> > +
> > +#define H5_ACK_TIMEOUT		250
> > +
> > +#define HCI_3WIRE_ACK_PKT	0
> > +#define HCI_3WIRE_LINK_PKT	15
> > +
> > +/*
> > + * Maximum Three-wire packet:
> > + *     4 byte header + max value for 12-bit length + 2 bytes for CRC
> > + */
> > +#define H5_MAX_LEN (4 + 0xfff + 2)
> > +
> > +/* Sliding window size */
> > +#define H5_TX_WIN_MAX		4
> > +
> > +#define SLIP_DELIMITER	0xc0
> > +#define SLIP_ESC	0xdb
> > +#define SLIP_ESC_DELIM	0xdc
> > +#define SLIP_ESC_ESC	0xdd
> > +
> > +#define H5_RX_ESC	1
> > +
> > +#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
> > +#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
> > +#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
> > +#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
> > +#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
> > +#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
> > +
> > +#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
> > +#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
> > +#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
> > +#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
> > +#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
> > +						((hdr)[2] |= (len) >> 4))
> > +
> > +static const uint8_t sync_req[] = { 0x01, 0x7e };
> > +static const uint8_t sync_rsp[] = { 0x02, 0x7d };
> > +static const uint8_t conf_req[] = { 0x03, 0xfc };
> > +static const uint8_t wakeup_req[] = { 0x05, 0xfa };
> > +static const uint8_t woken_req[] = { 0x06, 0xf9 };
> > +static const uint8_t sleep_req[] = { 0x07, 0x78 };
> > +
> > +static struct h5 {
> > +	size_t rx_pending;
> > +	uint8_t rx_buf[H5_MAX_LEN];
> > +	uint8_t *rx_ptr;
> > +	uint8_t flags;
> > +
> > +	uint8_t rx_ack;		/* Received last ack number */
> > +	uint8_t tx_seq;		/* Next seq number to send */
> > +	uint8_t tx_ack;		/* Next ack number to send */
> > +
> > +	uint8_t tx_win;
> > +
> > +	enum {
> > +		H5_UNINITIALIZED,
> > +		H5_INITIALIZED,
> > +		H5_ACTIVE,
> > +	} state;
> > +
> > +	int timer_id;
> > +
> > +	struct queue *tx_queue_unack;
> > +	struct queue *tx_queue_unrel;
> > +	struct queue *tx_queue_rel;
> > +
> > +	int (*rx_func)(struct proxy *proxy, uint8_t byte);
> > +} h5;
> 
> No global static variables please. That should be part of struct proxy instead. Global variables will cause problems if you ever try to start two instances.

OK, I will move it there.

> 
> > +
> > +struct h5_pkt {
> > +	uint16_t len;
> > +	uint8_t data[0];
> > +};
> 
> I think using get_unaligned_le16 to get the length would be a lot simpler.

Sorry, do not get this, how can we use get_unaligned_le16() ?

> 
> > +
> > +static void h5_reset_rx(void);
> > +
> > +static void h5_reset_peer(void)
> > +{
> > +	h5.state = H5_UNINITIALIZED;
> > +
> > +	mainloop_remove_timeout(h5.timer_id);
> > +	h5.timer_id = -1;
> > +	h5_reset_rx();
> > +}
> 
> With that all said, I think we might want to create a tools/h5.[ch] to abstract this properly into a separate helper library.

OK, I will move the code to h5.[ch]

Best regards 
Andrei Emeltchenko 

Re: [RFC 2/2] btproxy: Add three-wire (h5) protocol initial support

[Marcel Holtmann]

Hi Andrei,

>>> With H5 support it is possible to create pts and attach to it using
>>> hciattach ot btattach with 3wire protocol. This is useful for testing
>>> and developing three-wire protocol.
>>> Implementation is based on kernel hci_h5.c H5 protocol.
>>> 
>>> Simple usage:
>>> To open virtual pts run:
>>> $ sudo tools/btproxy -d --pty -3
>>> Opening pseudoterminal
>>> New pts created: /dev/pts/2
>>> Opening user channel for hci0
>>> 
>>> Now attach to it using hciattach:
>>> $ sudo hciattach -n /dev/pts/2 3wire
>>> Device setup complete
>>> ---
>>> tools/btproxy.c | 541 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
>>> 1 file changed, 535 insertions(+), 6 deletions(-)
>>> 
>>> diff --git a/tools/btproxy.c b/tools/btproxy.c
>>> index 6d78876..c718e48 100644
>>> --- a/tools/btproxy.c
>>> +++ b/tools/btproxy.c
>>> @@ -47,23 +47,22 @@
>>> #include "src/shared/util.h"
>>> #include "src/shared/mainloop.h"
>>> #include "src/shared/ecc.h"
>>> +#include "src/shared/queue.h"
>>> +#include "lib/bluetooth.h"
>>> +#include "lib/hci.h"
>>> #include "monitor/bt.h"
>>> 
>>> #define HCI_BREDR	0x00
>>> #define HCI_AMP		0x01
>>> 
>>> #define BTPROTO_HCI	1
>>> -struct sockaddr_hci {
>>> -	sa_family_t	hci_family;
>>> -	unsigned short	hci_dev;
>>> -	unsigned short  hci_channel;
>>> -};
>>> #define HCI_CHANNEL_USER	1
>> 
>> please do not do this.
> 
> Why? I added #include "lib/hci.h" where this structure defined, why do
> we need redefined it again? It was defined 3 times.

we want to get rid of using libbluetooth over and over again. We need to start somewhere. And I have no idea on what you would gain from including lib/hci.h in this case.

>>> static uint16_t hci_index = 0;
>>> static bool client_active = false;
>>> static bool debug_enabled = false;
>>> static bool emulate_ecc = false;
>>> +static bool three_wire = false;
>>> 
>>> static void hexdump_print(const char *str, void *user_data)
>>> {
>>> @@ -287,6 +286,516 @@ static void host_read_destroy(void *user_data)
>>> 		mainloop_remove_fd(proxy->dev_fd);
>>> }
>>> 
>>> +/* three-wire (H5) packet processing */
>>> +
>>> +#define H5_ACK_TIMEOUT		250
>>> +
>>> +#define HCI_3WIRE_ACK_PKT	0
>>> +#define HCI_3WIRE_LINK_PKT	15
>>> +
>>> +/*
>>> + * Maximum Three-wire packet:
>>> + *     4 byte header + max value for 12-bit length + 2 bytes for CRC
>>> + */
>>> +#define H5_MAX_LEN (4 + 0xfff + 2)
>>> +
>>> +/* Sliding window size */
>>> +#define H5_TX_WIN_MAX		4
>>> +
>>> +#define SLIP_DELIMITER	0xc0
>>> +#define SLIP_ESC	0xdb
>>> +#define SLIP_ESC_DELIM	0xdc
>>> +#define SLIP_ESC_ESC	0xdd
>>> +
>>> +#define H5_RX_ESC	1
>>> +
>>> +#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
>>> +#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
>>> +#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
>>> +#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
>>> +#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
>>> +#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
>>> +
>>> +#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
>>> +#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
>>> +#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
>>> +#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
>>> +#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
>>> +						((hdr)[2] |= (len) >> 4))
>>> +
>>> +static const uint8_t sync_req[] = { 0x01, 0x7e };
>>> +static const uint8_t sync_rsp[] = { 0x02, 0x7d };
>>> +static const uint8_t conf_req[] = { 0x03, 0xfc };
>>> +static const uint8_t wakeup_req[] = { 0x05, 0xfa };
>>> +static const uint8_t woken_req[] = { 0x06, 0xf9 };
>>> +static const uint8_t sleep_req[] = { 0x07, 0x78 };
>>> +
>>> +static struct h5 {
>>> +	size_t rx_pending;
>>> +	uint8_t rx_buf[H5_MAX_LEN];
>>> +	uint8_t *rx_ptr;
>>> +	uint8_t flags;
>>> +
>>> +	uint8_t rx_ack;		/* Received last ack number */
>>> +	uint8_t tx_seq;		/* Next seq number to send */
>>> +	uint8_t tx_ack;		/* Next ack number to send */
>>> +
>>> +	uint8_t tx_win;
>>> +
>>> +	enum {
>>> +		H5_UNINITIALIZED,
>>> +		H5_INITIALIZED,
>>> +		H5_ACTIVE,
>>> +	} state;
>>> +
>>> +	int timer_id;
>>> +
>>> +	struct queue *tx_queue_unack;
>>> +	struct queue *tx_queue_unrel;
>>> +	struct queue *tx_queue_rel;
>>> +
>>> +	int (*rx_func)(struct proxy *proxy, uint8_t byte);
>>> +} h5;
>> 
>> No global static variables please. That should be part of struct proxy instead. Global variables will cause problems if you ever try to start two instances.
> 
> OK, I will move it there.
> 
>> 
>>> +
>>> +struct h5_pkt {
>>> +	uint16_t len;
>>> +	uint8_t data[0];
>>> +};
>> 
>> I think using get_unaligned_le16 to get the length would be a lot simpler.
> 
> Sorry, do not get this, how can we use get_unaligned_le16() ?

len = get_unaligned_le16(buf)
data = buf + 2

Regards

Marcel

Re: [RFC 2/2] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

Hi Marcel,

On Fri, Nov 27, 2015 at 04:25:30AM -0800, Marcel Holtmann wrote:
> >>> 
> >>> #define BTPROTO_HCI	1
> >>> -struct sockaddr_hci {
> >>> -	sa_family_t	hci_family;
> >>> -	unsigned short	hci_dev;
> >>> -	unsigned short  hci_channel;
> >>> -};
> >>> #define HCI_CHANNEL_USER	1
> >> 
> >> please do not do this.
> > 
> > Why? I added #include "lib/hci.h" where this structure defined, why do
> > we need redefined it again? It was defined 3 times.
> 
> we want to get rid of using libbluetooth over and over again. We need to start somewhere. And I have no idea on what you would gain from including lib/hci.h in this case.

I get packet types definitions from lib/hci.h. But since this would be
moved to h5.c this is not important anymore. I'll remove this chunk.

> >> 
> >>> +
> >>> +struct h5_pkt {
> >>> +	uint16_t len;
> >>> +	uint8_t data[0];
> >>> +};
> >> 
> >> I think using get_unaligned_le16 to get the length would be a lot simpler.
> > 
> > Sorry, do not get this, how can we use get_unaligned_le16() ?
> 
> len = get_unaligned_le16(buf)

This is just my packet, no need for le16 here. I can use of course just
uint8_t array and use first two bytes for length.

Best regards 
Andrei Emeltchenko 

> data = buf + 2
> 
> Regards
> 
> Marcel
> 

[PATCHv1 0/2] Three-wire (H5) support in btproxy

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Changes:
	* v1: Split code to h5.[ch], remove global vars, add callbacks
	* RFC: Initial code

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete

Andrei Emeltchenko (2):
  btproxy: Add support for creating pseudoterminal
  btproxy: Add three-wire (h5) protocol initial support

 Makefile.tools  |   2 +-
 tools/btproxy.c | 111 +++++++++++-
 tools/h5.c      | 553 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tools/h5.h      |  71 ++++++++
 4 files changed, 730 insertions(+), 7 deletions(-)
 create mode 100644 tools/h5.c
 create mode 100644 tools/h5.h

-- 
2.5.0

[PATCHv1 1/2] btproxy: Add support for creating pseudoterminal

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new virtual Bluetooth serial controller on /dev/pts/X which allows
to test serial attach with btattach and hciattach.
---
 tools/btproxy.c | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 65 insertions(+), 3 deletions(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 43de037..6d78876 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -629,6 +629,41 @@ static int open_unix(const char *path)
 	return fd;
 }
 
+static int open_pty(void)
+{
+	int fd;
+	char *pts;
+
+	fd = open("/dev/ptmx", O_RDWR | O_NOCTTY);
+	if (fd < 0) {
+		perror("Failed to open pseudoterminal master");
+		return -1;
+	}
+
+	if (grantpt(fd) < 0) {
+		perror("Failed to grant access to slave pty");
+		close(fd);
+		return -1;
+	}
+
+	if (unlockpt(fd) < 0) {
+		perror("Failed to unlock slave pty");
+		close(fd);
+		return -1;
+	}
+
+	pts = ptsname(fd);
+	if (!pts) {
+		perror("Failed to get slave pty");
+		close(fd);
+		return -1;
+	}
+
+	printf("New pts created: %s\n", pts);
+
+	return fd;
+}
+
 static int open_tcp(const char *address, unsigned int port)
 {
 	struct sockaddr_in addr;
@@ -728,6 +763,7 @@ static void usage(void)
 		"\t-c, --connect <address>     Connect to server\n"
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
+		"\t-P, --pty                   Use PTY\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -741,6 +777,7 @@ static const struct option main_options[] = {
 	{ "connect",  required_argument, NULL, 'c' },
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
+	{ "pty",      no_argument,       NULL, 'P' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -758,6 +795,7 @@ int main(int argc, char *argv[])
 	const char *unix_path = NULL;
 	unsigned short tcp_port = 0xb1ee;	/* 45550 */
 	bool use_redirect = false;
+	bool use_pty = false;
 	uint8_t type = HCI_BREDR;
 	const char *str;
 	sigset_t mask;
@@ -765,7 +803,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::p:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -789,6 +827,9 @@ int main(int argc, char *argv[])
 			else
 				unix_path = "/tmp/bt-server-bredr";
 			break;
+		case 'P':
+			use_pty = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -828,12 +869,13 @@ int main(int argc, char *argv[])
 		return EXIT_FAILURE;
 	}
 
-	if (unix_path && (server_address || use_redirect)) {
+	if (unix_path && (server_address || use_redirect || use_pty)) {
 		fprintf(stderr, "Invalid to specify TCP and Unix servers\n");
 		return EXIT_FAILURE;
 	}
 
-	if (connect_address && (unix_path || server_address || use_redirect)) {
+	if (connect_address && (unix_path || server_address || use_redirect ||
+								use_pty)) {
 		fprintf(stderr, "Invalid to specify client and server mode\n");
 		return EXIT_FAILURE;
 	}
@@ -876,6 +918,26 @@ int main(int argc, char *argv[])
 			close(host_fd);
 			return EXIT_FAILURE;
 		}
+	} else if (use_pty) {
+		int master_fd, dev_fd;
+
+		printf("Opening pseudoterminal\n");
+
+		master_fd = open_pty();
+		if (master_fd < 0)
+			return EXIT_FAILURE;
+
+		dev_fd = open_channel(hci_index);
+		if (dev_fd < 0) {
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
+
+		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+			close(dev_fd);
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
 	} else {
 		int server_fd;
 
-- 
2.5.0

[PATCHv1 2/2] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete
---
 Makefile.tools  |   2 +-
 tools/btproxy.c |  47 ++++-
 tools/h5.c      | 553 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tools/h5.h      |  71 ++++++++
 4 files changed, 667 insertions(+), 6 deletions(-)
 create mode 100644 tools/h5.c
 create mode 100644 tools/h5.h

diff --git a/Makefile.tools b/Makefile.tools
index 6ebbe9f..275603a 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -284,7 +284,7 @@ tools_btattach_LDADD = src/libshared-mainloop.la
 tools_btsnoop_SOURCES = tools/btsnoop.c
 tools_btsnoop_LDADD = src/libshared-mainloop.la
 
-tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h
+tools_btproxy_SOURCES = tools/btproxy.c tools/h5.c tools/h5.h monitor/bt.h
 tools_btproxy_LDADD = src/libshared-mainloop.la
 
 tools_btiotest_SOURCES = tools/btiotest.c btio/btio.h btio/btio.c
diff --git a/tools/btproxy.c b/tools/btproxy.c
index 6d78876..f4879da 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -48,6 +48,7 @@
 #include "src/shared/mainloop.h"
 #include "src/shared/ecc.h"
 #include "monitor/bt.h"
+#include "h5.h"
 
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
@@ -64,6 +65,7 @@ static uint16_t hci_index = 0;
 static bool client_active = false;
 static bool debug_enabled = false;
 static bool emulate_ecc = false;
+static bool three_wire = false;
 
 static void hexdump_print(const char *str, void *user_data)
 {
@@ -86,8 +88,21 @@ struct proxy {
 	/* ECC emulation */
 	uint8_t event_mask[8];
 	uint8_t local_sk256[32];
+
+	/* H5 protocol */
+	struct h5 *h5;
 };
 
+void proxy_set_h5(struct proxy *proxy, struct h5 *h5)
+{
+	proxy->h5 = h5;
+}
+
+struct h5 *proxy_get_h5(struct proxy *proxy)
+{
+	return proxy->h5;
+}
+
 static bool write_packet(int fd, const void *data, size_t size,
 							void *user_data)
 {
@@ -351,6 +366,14 @@ process_packet:
 		sco_hdr = (void *) (proxy->host_buf + 1);
 		pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
 		break;
+	case SLIP_DELIMITER:
+		/* 2 bytes start/end packets + SLIP header and checksum */
+		if (proxy->host_len < 4 + 2)
+			return;
+
+		/* Later we shall check unslipped packet */
+		pktlen = proxy->host_len;
+		break;
 	case 0xff:
 		/* Notification packet from /dev/vhci - ignore */
 		proxy->host_len = 0;
@@ -367,6 +390,8 @@ process_packet:
 
 	if (emulate_ecc)
 		host_emulate_ecc(proxy, proxy->host_buf, pktlen);
+	else if (three_wire)
+		host_write_3wire_packet(proxy, proxy->host_buf, pktlen);
 	else
 		host_write_packet(proxy, proxy->host_buf, pktlen);
 
@@ -475,6 +500,8 @@ process_packet:
 
 	if (emulate_ecc)
 		dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
+	else if (three_wire)
+		dev_write_3wire_packet(proxy, proxy->dev_buf, pktlen);
 	else
 		dev_write_packet(proxy, proxy->dev_buf, pktlen);
 
@@ -488,14 +515,14 @@ process_packet:
 	proxy->dev_len = 0;
 }
 
-static bool setup_proxy(int host_fd, bool host_shutdown,
+static struct proxy *setup_proxy(int host_fd, bool host_shutdown,
 						int dev_fd, bool dev_shutdown)
 {
 	struct proxy *proxy;
 
 	proxy = new0(struct proxy, 1);
 	if (!proxy)
-		return false;
+		return NULL;
 
 	if (emulate_ecc)
 		printf("Enabling ECC emulation\n");
@@ -512,7 +539,7 @@ static bool setup_proxy(int host_fd, bool host_shutdown,
 	mainloop_add_fd(proxy->dev_fd, EPOLLIN | EPOLLRDHUP,
 				dev_read_callback, proxy, dev_read_destroy);
 
-	return true;
+	return proxy;
 }
 
 static int open_channel(uint16_t index)
@@ -764,6 +791,7 @@ static void usage(void)
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
 		"\t-P, --pty                   Use PTY\n"
+		"\t-3, --3wire                 Use 3wire protocol\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -778,6 +806,7 @@ static const struct option main_options[] = {
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
 	{ "pty",      no_argument,       NULL, 'P' },
+	{ "3wire",    no_argument,       NULL, '3' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -803,7 +832,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -830,6 +859,9 @@ int main(int argc, char *argv[])
 		case 'P':
 			use_pty = true;
 			break;
+		case '3':
+			three_wire = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -920,6 +952,7 @@ int main(int argc, char *argv[])
 		}
 	} else if (use_pty) {
 		int master_fd, dev_fd;
+		struct proxy *proxy;
 
 		printf("Opening pseudoterminal\n");
 
@@ -933,11 +966,15 @@ int main(int argc, char *argv[])
 			return EXIT_FAILURE;
 		}
 
-		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+		proxy = setup_proxy(master_fd, false, dev_fd, true);
+		if (!proxy) {
 			close(dev_fd);
 			close(master_fd);
 			return EXIT_FAILURE;
 		}
+
+		if (three_wire)
+			h5_init(proxy, host_write_packet, dev_write_packet);
 	} else {
 		int server_fd;
 
diff --git a/tools/h5.c b/tools/h5.c
new file mode 100644
index 0000000..15a42be
--- /dev/null
+++ b/tools/h5.c
@@ -0,0 +1,553 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+
+#include "src/shared/mainloop.h"
+#include "src/shared/queue.h"
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "h5.h"
+
+static const uint8_t sync_req[] = { 0x01, 0x7e };
+static const uint8_t sync_rsp[] = { 0x02, 0x7d };
+static const uint8_t conf_req[] = { 0x03, 0xfc };
+static const uint8_t wakeup_req[] = { 0x05, 0xfa };
+static const uint8_t woken_req[] = { 0x06, 0xf9 };
+static const uint8_t sleep_req[] = { 0x07, 0x78 };
+
+struct h5 {
+	size_t rx_pending;
+	uint8_t rx_buf[H5_MAX_LEN];
+	uint8_t *rx_ptr;
+	uint8_t flags;
+
+	uint8_t rx_ack;		/* Received last ack number */
+	uint8_t tx_seq;		/* Next seq number to send */
+	uint8_t tx_ack;		/* Next ack number to send */
+
+	uint8_t tx_win;
+
+	enum {
+		H5_UNINITIALIZED,
+		H5_INITIALIZED,
+		H5_ACTIVE,
+	} state;
+
+	int timer_id;
+
+	struct queue *tx_queue_unack;
+	struct queue *tx_queue_unrel;
+	struct queue *tx_queue_rel;
+
+	int (*rx_func)(struct proxy *proxy, uint8_t byte);
+
+	void (*host_write)(struct proxy *proxy, void *buf, uint16_t len);
+	void (*dev_write)(struct proxy *proxy, void *buf, uint16_t len);
+};
+
+struct h5_pkt {
+	uint16_t len;
+	uint8_t data[0];
+};
+
+static void h5_reset_rx(struct h5 *h5);
+
+static void h5_reset_peer(struct h5 *h5)
+{
+	h5->state = H5_UNINITIALIZED;
+
+	mainloop_remove_timeout(h5->timer_id);
+	h5->timer_id = -1;
+	h5_reset_rx(h5);
+}
+
+bool h5_init(struct proxy *proxy,
+		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
+		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len))
+{
+	struct h5 *h5;
+
+	h5 = malloc(sizeof(*h5));
+	if (!h5)
+		return false;
+
+	h5->tx_win = H5_TX_WIN_MAX;
+	h5_reset_peer(h5);
+
+	h5->tx_queue_unack = queue_new();
+	h5->tx_queue_unrel = queue_new();
+	h5->tx_queue_rel = queue_new();
+
+	h5->host_write = host_w;
+	h5->dev_write = dev_w;
+
+	proxy_set_h5(proxy, h5);
+
+	return true;
+}
+
+void h5_clean(struct h5 *h5)
+{
+	mainloop_remove_timeout(h5->timer_id);
+
+	queue_remove_all(h5->tx_queue_unack, NULL, NULL, free);
+	queue_remove_all(h5->tx_queue_unrel, NULL, NULL, free);
+	queue_remove_all(h5->tx_queue_rel, NULL, NULL, free);
+
+	free(h5);
+}
+
+static bool h5_reliable_pkt(uint8_t type)
+{
+	switch (type) {
+	case HCI_ACLDATA_PKT:
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static uint8_t h5_slip_byte(uint8_t *pkt, uint8_t byte)
+{
+	const uint8_t esc_delim[] = { SLIP_ESC, SLIP_ESC_DELIM };
+	const uint8_t esc_esc[] = { SLIP_ESC, SLIP_ESC_ESC };
+
+	switch (byte) {
+	case SLIP_DELIMITER:
+		memcpy(pkt, &esc_delim, sizeof(esc_delim));
+		return sizeof(esc_delim);
+	case SLIP_ESC:
+		memcpy(pkt, &esc_esc, sizeof(esc_esc));
+		return sizeof(esc_esc);
+	default:
+		memcpy(pkt, &byte, sizeof(byte));
+		return sizeof(byte);
+	}
+}
+
+static void h5_print_header(const uint8_t *hdr, const char *str)
+{
+	if (H5_HDR_RELIABLE(hdr))
+		printf("%s REL: seq %u ack %u crc %u type %u len %u\n",
+				str, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
+				H5_HDR_CRC(hdr), H5_HDR_PKT_TYPE(hdr),
+				H5_HDR_LEN(hdr));
+	else
+		printf("%s UNREL: ack %u crc %u type %u len %u\n",
+				str, H5_HDR_ACK(hdr), H5_HDR_CRC(hdr),
+				H5_HDR_PKT_TYPE(hdr), H5_HDR_LEN(hdr));
+}
+
+static void h5_send(struct proxy *proxy, const uint8_t *payload,
+						uint8_t pkt_type, int len)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+	uint8_t *ptr;
+	uint8_t hdr[4];
+	int i;
+
+	memset(hdr, 0, sizeof(hdr));
+
+	H5_SET_ACK(hdr, h5->tx_ack);
+
+	if (h5_reliable_pkt(pkt_type)) {
+		H5_SET_RELIABLE(hdr);
+		H5_SET_SEQ(hdr, h5->tx_seq);
+		h5->tx_seq = (h5->tx_seq + 1) % 8;
+	}
+
+	H5_SET_TYPE(hdr, pkt_type);
+	H5_SET_LEN(hdr, len);
+
+	/* Calculate CRC */
+	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
+
+	h5_print_header(hdr, "TX: <");
+
+	/*
+	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
+	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
+	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
+	 * delimiters at start and end).
+	 */
+	pkt = malloc(sizeof(*pkt) + (len + 6) * 2 + 2);
+	if (!pkt)
+		return;
+
+	ptr = pkt->data;
+
+	*ptr++ = SLIP_DELIMITER;
+
+	for (i = 0; i < 4; i++)
+		ptr += h5_slip_byte(ptr, hdr[i]);
+
+	for (i = 0; i < len; i++)
+		ptr += h5_slip_byte(ptr, payload[i]);
+
+	*ptr++ = SLIP_DELIMITER;
+
+	pkt->len = ptr - pkt->data;
+
+	if (h5_reliable_pkt(pkt_type))
+		queue_push_tail(h5->tx_queue_rel, pkt);
+	else
+		queue_push_tail(h5->tx_queue_unrel, pkt);
+}
+
+static void h5_process_sig_pkt(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t conf_rsp[3] = { 0x04, 0x7b };
+	const uint8_t *hdr = h5->rx_buf;
+	const uint8_t *payload = hdr + 4;
+
+	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
+		return;
+
+	if (H5_HDR_LEN(hdr) < 2)
+		return;
+
+	conf_rsp[2] = h5->tx_win & 0x07;
+
+	if (!memcmp(payload, sync_req, sizeof(sync_req))) {
+		if (h5->state == H5_ACTIVE)
+			h5_reset_peer(h5);
+
+		h5_send(proxy, sync_rsp, H5_HDR_PKT_TYPE(hdr),
+							sizeof(sync_rsp));
+		return;
+	} else if (!memcmp(payload, conf_req, 2)) {
+		h5_send(proxy, conf_rsp, H5_HDR_PKT_TYPE(hdr),
+							sizeof(conf_rsp));
+		return;
+	}
+
+	exit(1);
+}
+
+static void h5_process_data(struct proxy *proxy, uint8_t pkt_type)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t pkt[1 + h5->rx_ptr - 4 - h5->rx_buf];
+
+	pkt[0] = pkt_type;
+	memcpy(&pkt[1], h5->rx_buf + 4, sizeof(pkt) - 1);
+
+	h5->host_write(proxy, pkt, sizeof(pkt));
+}
+
+static void pkt_print(void *data, void *user_data)
+{
+	struct h5_pkt *pkt = data;
+
+	h5_print_header((uint8_t *)pkt + sizeof(*pkt) + 1, "unack pkt");
+}
+
+static void h5_process_unack_queue(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t len = queue_length(h5->tx_queue_unack);
+	uint8_t seq = h5->tx_seq;
+	struct h5_pkt *pkt;
+
+	if (!len)
+		return;
+
+	printf("%s: rx_ack %u tx_ack %u tx_seq %u\n", __func__,
+					h5->rx_ack, h5->tx_ack, h5->tx_seq);
+
+	printf("%s: unack queue length %u\n", __func__, len);
+
+	queue_foreach(h5->tx_queue_unack, pkt_print, NULL);
+
+	/* Remove acked packets from unack queue */
+	while (len > 0) {
+		if (h5->rx_ack == seq)
+			break;
+
+		len--;
+		seq = (seq - 1) & 0x07;
+	}
+
+	if (seq != h5->rx_ack)
+		fprintf(stderr, "Acked wrong packet\n");
+
+	while (len--) {
+		printf("%s: Remove packet\n", __func__);
+
+		pkt = queue_pop_head(h5->tx_queue_unack);
+		if (pkt)
+			free(pkt);
+	}
+}
+
+static void h5_process_complete_pkt(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		h5->tx_ack = (h5->tx_ack + 1) % 8;
+		/* TODO: Set ack req flag */
+	}
+
+	h5->rx_ack = H5_HDR_ACK(hdr);
+
+	h5_process_unack_queue(proxy);
+
+	switch (H5_HDR_PKT_TYPE(hdr)) {
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+	case HCI_ACLDATA_PKT:
+	case HCI_SCODATA_PKT:
+		/* Need to remove three-wire header */
+		h5_process_data(proxy, (H5_HDR_PKT_TYPE(hdr)));
+		break;
+	default:
+		/* Handle three-wire protocol */
+		h5_process_sig_pkt(proxy);
+	}
+
+	h5_reset_rx(h5);
+}
+
+static int h5_crc(struct proxy *proxy, const uint8_t byte)
+{
+	h5_process_complete_pkt(proxy);
+
+	return 0;
+}
+
+static int h5_payload(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		/* TODO: process tx_ack */
+	}
+
+	if (H5_HDR_CRC(hdr)) {
+		h5->rx_func = h5_crc;
+		h5->rx_pending = 2;
+	} else {
+		h5_process_complete_pkt(proxy);
+	}
+
+	return 0;
+}
+
+static int h5_3wire_hdr(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	h5_print_header(hdr, "RX: >");
+
+	/* Add header checks here */
+
+	h5->rx_func = h5_payload;
+	h5->rx_pending = H5_HDR_LEN(hdr);
+
+	return 0;
+}
+
+static int h5_pkt_start(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	if (byte == SLIP_DELIMITER)
+		return 1;
+
+	h5->rx_func = h5_3wire_hdr;
+	h5->rx_pending = 4;
+
+	return 0;
+}
+
+static int h5_delimeter(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	if (byte == SLIP_DELIMITER)
+		h5->rx_func = h5_pkt_start;
+
+	return 1;
+}
+
+static void h5_reset_rx(struct h5 *h5)
+{
+	h5->rx_pending = 0;
+	h5->flags = 0;
+
+	h5->rx_ptr = h5->rx_buf;
+
+	h5->rx_func = h5_delimeter;
+}
+
+static void h5_process_byte(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	/* Mark escaped */
+	if (!(h5->flags & H5_RX_ESC) && byte == SLIP_ESC) {
+		h5->flags |= H5_RX_ESC;
+		return;
+	}
+
+	if (h5->flags & H5_RX_ESC) {
+		h5->flags &= ~H5_RX_ESC;
+
+		switch (byte) {
+		case SLIP_ESC_DELIM:
+			*h5->rx_ptr++ = SLIP_DELIMITER;
+			break;
+		case SLIP_ESC_ESC:
+			*h5->rx_ptr++ = SLIP_ESC;
+			break;
+		default:
+			fprintf(stderr, "Invalid esc byte %x\n", byte);
+			h5_reset_rx(h5);
+			return;
+		}
+	} else {
+		*h5->rx_ptr++ = byte;
+	}
+
+	h5->rx_pending--;
+}
+
+static void h5_process_tx_queue(struct proxy *proxy);
+
+static void h5_timer_cb(int id, void *user_data)
+{
+	struct proxy *proxy = user_data;
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+
+	printf("%s: Retransmitting %u packets\n", __func__,
+					queue_length(h5->tx_queue_unack));
+
+	while ((pkt = queue_peek_tail(h5->tx_queue_unack))) {
+		h5->tx_seq = (h5->tx_seq - 1) & 0x07;
+
+		/* Move pkt from unack to rel queue */
+		queue_remove(h5->tx_queue_unack, pkt);
+		queue_push_head(h5->tx_queue_rel, pkt);
+	}
+
+	h5_process_tx_queue(proxy);
+}
+
+static void h5_process_tx_queue(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+
+	while ((pkt = queue_pop_head(h5->tx_queue_unrel))) {
+		h5->dev_write(proxy, pkt->data, pkt->len);
+
+		free(pkt);
+	}
+
+	if (queue_length(h5->tx_queue_unack) >= h5->tx_win) {
+		printf("Unacked queue too big\n");
+		return;
+	}
+
+	while ((pkt = queue_pop_head(h5->tx_queue_rel))) {
+		h5->dev_write(proxy, pkt->data, pkt->len);
+		/*
+		if (!write_packet(proxy->host_fd, pkt->data, pkt->len, "H: ")) {
+			fprintf(stderr, "Write to Host descriptor failed\n");
+			mainloop_remove_fd(proxy->host_fd);
+		}
+		*/
+
+		queue_push_tail(h5->tx_queue_unack, pkt);
+		if (h5->timer_id >= 0) {
+			mainloop_modify_timeout(h5->timer_id, H5_ACK_TIMEOUT);
+			continue;
+		}
+
+		h5->timer_id = mainloop_add_timeout(H5_ACK_TIMEOUT, h5_timer_cb,
+								proxy, NULL);
+	}
+}
+
+void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *ptr = buf;
+
+	while (len > 0) {
+		int processed;
+
+		if (h5->rx_pending > 0) {
+			if (*ptr == SLIP_DELIMITER) {
+				fprintf(stderr, "Too short packet\n");
+				h5_reset_rx(h5);
+				continue;
+			}
+
+			h5_process_byte(proxy, *ptr);
+
+			ptr++;
+			len--;
+			continue;
+		}
+
+		processed = h5->rx_func(proxy, *ptr);
+		if (processed < 0) {
+			fprintf(stderr, "Error processing SLIP packet\n");
+			return;
+		}
+
+		ptr += processed;
+		len -= processed;
+
+		h5_process_tx_queue(proxy);
+	}
+
+	h5_process_tx_queue(proxy);
+}
+
+void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len)
+{
+	uint8_t pkt_type = buf[0];
+
+	/* Get payload out of H4 packet */
+	h5_send(proxy, &buf[1], pkt_type, len - 1);
+
+	h5_process_tx_queue(proxy);
+}
+
+
diff --git a/tools/h5.h b/tools/h5.h
new file mode 100644
index 0000000..cd88e30
--- /dev/null
+++ b/tools/h5.h
@@ -0,0 +1,71 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#define H5_ACK_TIMEOUT		250
+
+#define HCI_3WIRE_ACK_PKT	0
+#define HCI_3WIRE_LINK_PKT	15
+
+/*
+ * Maximum Three-wire packet:
+ *     4 byte header + max value for 12-bit length + 2 bytes for CRC
+ */
+#define H5_MAX_LEN (4 + 0xfff + 2)
+
+/* Sliding window size */
+#define H5_TX_WIN_MAX		4
+
+#define SLIP_DELIMITER	0xc0
+#define SLIP_ESC	0xdb
+#define SLIP_ESC_DELIM	0xdc
+#define SLIP_ESC_ESC	0xdd
+
+#define H5_RX_ESC	1
+
+#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
+#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
+#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
+#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
+#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
+#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
+
+#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
+#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
+#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
+#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
+#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
+						((hdr)[2] |= (len) >> 4))
+
+struct proxy;
+struct h5;
+
+bool h5_init(struct proxy *proxy,
+		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
+		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len));
+void h5_clean(struct h5 *h5);
+
+void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len);
+void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len);
+
+void proxy_set_h5(struct proxy *proxy, struct h5 *h5);
+struct h5 *proxy_get_h5(struct proxy *proxy);
-- 
2.5.0

Re: [PATCHv1 2/2] btproxy: Add three-wire (h5) protocol initial support

[Marcel Holtmann]

Hi Andrei,

> With H5 support it is possible to create pts and attach to it using
> hciattach ot btattach with 3wire protocol. This is useful for testing
> and developing three-wire protocol.
> Implementation is based on kernel hci_h5.c H5 protocol.
> 
> Simple usage:
> To open virtual pts run:
> $ sudo tools/btproxy -d --pty -3
> Opening pseudoterminal
> New pts created: /dev/pts/2
> Opening user channel for hci0
> 
> Now attach to it using hciattach:
> $ sudo hciattach -n /dev/pts/2 3wire
> Device setup complete
> ---
> Makefile.tools  |   2 +-
> tools/btproxy.c |  47 ++++-
> tools/h5.c      | 553 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
> tools/h5.h      |  71 ++++++++
> 4 files changed, 667 insertions(+), 6 deletions(-)
> create mode 100644 tools/h5.c
> create mode 100644 tools/h5.h
> 
> diff --git a/Makefile.tools b/Makefile.tools
> index 6ebbe9f..275603a 100644
> --- a/Makefile.tools
> +++ b/Makefile.tools
> @@ -284,7 +284,7 @@ tools_btattach_LDADD = src/libshared-mainloop.la
> tools_btsnoop_SOURCES = tools/btsnoop.c
> tools_btsnoop_LDADD = src/libshared-mainloop.la
> 
> -tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h
> +tools_btproxy_SOURCES = tools/btproxy.c tools/h5.c tools/h5.h monitor/bt.h

generally .h files before .c files. And I would do this one like this:

tools_btproxy_SOURCES = tools/btproxy.c monitor.h \
					tools/h5.h tools/h5.c


> tools_btproxy_LDADD = src/libshared-mainloop.la
> 
> tools_btiotest_SOURCES = tools/btiotest.c btio/btio.h btio/btio.c
> diff --git a/tools/btproxy.c b/tools/btproxy.c
> index 6d78876..f4879da 100644
> --- a/tools/btproxy.c
> +++ b/tools/btproxy.c
> @@ -48,6 +48,7 @@
> #include "src/shared/mainloop.h"
> #include "src/shared/ecc.h"
> #include "monitor/bt.h"
> +#include "h5.h"
> 
> #define HCI_BREDR	0x00
> #define HCI_AMP		0x01
> @@ -64,6 +65,7 @@ static uint16_t hci_index = 0;
> static bool client_active = false;
> static bool debug_enabled = false;
> static bool emulate_ecc = false;
> +static bool three_wire = false;
> 
> static void hexdump_print(const char *str, void *user_data)
> {
> @@ -86,8 +88,21 @@ struct proxy {
> 	/* ECC emulation */
> 	uint8_t event_mask[8];
> 	uint8_t local_sk256[32];
> +
> +	/* H5 protocol */
> +	struct h5 *h5;
> };
> 
> +void proxy_set_h5(struct proxy *proxy, struct h5 *h5)
> +{
> +	proxy->h5 = h5;
> +}
> +
> +struct h5 *proxy_get_h5(struct proxy *proxy)
> +{
> +	return proxy->h5;
> +}
> +
> static bool write_packet(int fd, const void *data, size_t size,
> 							void *user_data)
> {
> @@ -351,6 +366,14 @@ process_packet:
> 		sco_hdr = (void *) (proxy->host_buf + 1);
> 		pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
> 		break;
> +	case SLIP_DELIMITER:
> +		/* 2 bytes start/end packets + SLIP header and checksum */
> +		if (proxy->host_len < 4 + 2)
> +			return;
> +
> +		/* Later we shall check unslipped packet */
> +		pktlen = proxy->host_len;
> +		break;

This is something I do not like much. It is not really an H:4 packet at all. This is intermixing the H:4 part with the H:5 part and I not convinced that is a good idea.

> 	case 0xff:
> 		/* Notification packet from /dev/vhci - ignore */
> 		proxy->host_len = 0;
> @@ -367,6 +390,8 @@ process_packet:
> 
> 	if (emulate_ecc)
> 		host_emulate_ecc(proxy, proxy->host_buf, pktlen);
> +	else if (three_wire)
> +		host_write_3wire_packet(proxy, proxy->host_buf, pktlen);
> 	else
> 		host_write_packet(proxy, proxy->host_buf, pktlen);

Is this really the best idea? We might need some extra abstraction for the transport to allow for better HCI vs transport abstraction.

> 
> @@ -475,6 +500,8 @@ process_packet:
> 
> 	if (emulate_ecc)
> 		dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
> +	else if (three_wire)
> +		dev_write_3wire_packet(proxy, proxy->dev_buf, pktlen);
> 	else
> 		dev_write_packet(proxy, proxy->dev_buf, pktlen);
> 
> @@ -488,14 +515,14 @@ process_packet:
> 	proxy->dev_len = 0;
> }
> 
> -static bool setup_proxy(int host_fd, bool host_shutdown,
> +static struct proxy *setup_proxy(int host_fd, bool host_shutdown,
> 						int dev_fd, bool dev_shutdown)
> {
> 	struct proxy *proxy;
> 
> 	proxy = new0(struct proxy, 1);
> 	if (!proxy)
> -		return false;
> +		return NULL;
> 
> 	if (emulate_ecc)
> 		printf("Enabling ECC emulation\n");
> @@ -512,7 +539,7 @@ static bool setup_proxy(int host_fd, bool host_shutdown,
> 	mainloop_add_fd(proxy->dev_fd, EPOLLIN | EPOLLRDHUP,
> 				dev_read_callback, proxy, dev_read_destroy);
> 
> -	return true;
> +	return proxy;
> }
> 
> static int open_channel(uint16_t index)
> @@ -764,6 +791,7 @@ static void usage(void)
> 		"\t-l, --listen [address]      Use TCP server\n"
> 		"\t-u, --unix [path]           Use Unix server\n"
> 		"\t-P, --pty                   Use PTY\n"
> +		"\t-3, --3wire                 Use 3wire protocol\n"
> 		"\t-p, --port <port>           Use specified TCP port\n"
> 		"\t-i, --index <num>           Use specified controller\n"
> 		"\t-a, --amp                   Create AMP controller\n"
> @@ -778,6 +806,7 @@ static const struct option main_options[] = {
> 	{ "listen",   optional_argument, NULL, 'l' },
> 	{ "unix",     optional_argument, NULL, 'u' },
> 	{ "pty",      no_argument,       NULL, 'P' },
> +	{ "3wire",    no_argument,       NULL, '3' },
> 	{ "port",     required_argument, NULL, 'p' },
> 	{ "index",    required_argument, NULL, 'i' },
> 	{ "amp",      no_argument,       NULL, 'a' },
> @@ -803,7 +832,7 @@ int main(int argc, char *argv[])
> 	for (;;) {
> 		int opt;
> 
> -		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
> +		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
> 						main_options, NULL);
> 		if (opt < 0)
> 			break;
> @@ -830,6 +859,9 @@ int main(int argc, char *argv[])
> 		case 'P':
> 			use_pty = true;
> 			break;
> +		case '3':
> +			three_wire = true;
> +			break;
> 		case 'p':
> 			tcp_port = atoi(optarg);
> 			break;
> @@ -920,6 +952,7 @@ int main(int argc, char *argv[])
> 		}
> 	} else if (use_pty) {
> 		int master_fd, dev_fd;
> +		struct proxy *proxy;
> 
> 		printf("Opening pseudoterminal\n");
> 
> @@ -933,11 +966,15 @@ int main(int argc, char *argv[])
> 			return EXIT_FAILURE;
> 		}
> 
> -		if (!setup_proxy(master_fd, false, dev_fd, true)) {
> +		proxy = setup_proxy(master_fd, false, dev_fd, true);
> +		if (!proxy) {
> 			close(dev_fd);
> 			close(master_fd);
> 			return EXIT_FAILURE;
> 		}
> +
> +		if (three_wire)
> +			h5_init(proxy, host_write_packet, dev_write_packet);

I wonder if it would not be better to leave setup_proxy as is and instead create a new setup_proxy_h5.

> 	} else {
> 		int server_fd;
> 
> diff --git a/tools/h5.c b/tools/h5.c
> new file mode 100644
> index 0000000..15a42be
> --- /dev/null
> +++ b/tools/h5.c
> @@ -0,0 +1,553 @@
> +/*
> + *
> + *  BlueZ - Bluetooth protocol stack for Linux
> + *
> + *  Copyright (C) 2015  Intel Corporation
> + *
> + *
> + *  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.
> + *
> + *  You should have received a copy of the GNU General Public License
> + *  along with this program; if not, write to the Free Software
> + *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
> + *
> + */
> +
> +#include <stdint.h>
> +#include <stdlib.h>
> +#include <stdbool.h>
> +#include <unistd.h>
> +
> +#include "src/shared/mainloop.h"
> +#include "src/shared/queue.h"
> +#include "lib/bluetooth.h"
> +#include "lib/hci.h"
> +
> +#include "h5.h"
> +
> +static const uint8_t sync_req[] = { 0x01, 0x7e };
> +static const uint8_t sync_rsp[] = { 0x02, 0x7d };
> +static const uint8_t conf_req[] = { 0x03, 0xfc };
> +static const uint8_t wakeup_req[] = { 0x05, 0xfa };
> +static const uint8_t woken_req[] = { 0x06, 0xf9 };
> +static const uint8_t sleep_req[] = { 0x07, 0x78 };
> +
> +struct h5 {
> +	size_t rx_pending;
> +	uint8_t rx_buf[H5_MAX_LEN];
> +	uint8_t *rx_ptr;
> +	uint8_t flags;
> +
> +	uint8_t rx_ack;		/* Received last ack number */
> +	uint8_t tx_seq;		/* Next seq number to send */
> +	uint8_t tx_ack;		/* Next ack number to send */
> +
> +	uint8_t tx_win;
> +
> +	enum {
> +		H5_UNINITIALIZED,
> +		H5_INITIALIZED,
> +		H5_ACTIVE,
> +	} state;
> +
> +	int timer_id;
> +
> +	struct queue *tx_queue_unack;
> +	struct queue *tx_queue_unrel;
> +	struct queue *tx_queue_rel;
> +
> +	int (*rx_func)(struct proxy *proxy, uint8_t byte);
> +
> +	void (*host_write)(struct proxy *proxy, void *buf, uint16_t len);
> +	void (*dev_write)(struct proxy *proxy, void *buf, uint16_t len);
> +};
> +
> +struct h5_pkt {
> +	uint16_t len;
> +	uint8_t data[0];
> +};
> +
> +static void h5_reset_rx(struct h5 *h5);
> +
> +static void h5_reset_peer(struct h5 *h5)
> +{
> +	h5->state = H5_UNINITIALIZED;
> +
> +	mainloop_remove_timeout(h5->timer_id);

Actually using src/shared/timeout.h would be preferred. It has an implementation that uses mainloop, but is a lot more generic and its us eventually easier move towards ELL.

> +	h5->timer_id = -1;
> +	h5_reset_rx(h5);
> +}
> +
> +bool h5_init(struct proxy *proxy,
> +		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
> +		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len))
> +{
> +	struct h5 *h5;
> +
> +	h5 = malloc(sizeof(*h5));
> +	if (!h5)
> +		return false;
> +
> +	h5->tx_win = H5_TX_WIN_MAX;
> +	h5_reset_peer(h5);
> +
> +	h5->tx_queue_unack = queue_new();
> +	h5->tx_queue_unrel = queue_new();
> +	h5->tx_queue_rel = queue_new();
> +
> +	h5->host_write = host_w;
> +	h5->dev_write = dev_w;
> +
> +	proxy_set_h5(proxy, h5);
> +
> +	return true;
> +}
> +
> +void h5_clean(struct h5 *h5)
> +{
> +	mainloop_remove_timeout(h5->timer_id);
> +
> +	queue_remove_all(h5->tx_queue_unack, NULL, NULL, free);
> +	queue_remove_all(h5->tx_queue_unrel, NULL, NULL, free);
> +	queue_remove_all(h5->tx_queue_rel, NULL, NULL, free);
> +
> +	free(h5);
> +}
> +
> +static bool h5_reliable_pkt(uint8_t type)
> +{
> +	switch (type) {
> +	case HCI_ACLDATA_PKT:
> +	case HCI_COMMAND_PKT:
> +	case HCI_EVENT_PKT:
> +		return true;
> +	default:
> +		return false;
> +	}
> +}
> +
> +static uint8_t h5_slip_byte(uint8_t *pkt, uint8_t byte)
> +{
> +	const uint8_t esc_delim[] = { SLIP_ESC, SLIP_ESC_DELIM };
> +	const uint8_t esc_esc[] = { SLIP_ESC, SLIP_ESC_ESC };
> +
> +	switch (byte) {
> +	case SLIP_DELIMITER:
> +		memcpy(pkt, &esc_delim, sizeof(esc_delim));
> +		return sizeof(esc_delim);
> +	case SLIP_ESC:
> +		memcpy(pkt, &esc_esc, sizeof(esc_esc));
> +		return sizeof(esc_esc);
> +	default:
> +		memcpy(pkt, &byte, sizeof(byte));
> +		return sizeof(byte);
> +	}
> +}
> +
> +static void h5_print_header(const uint8_t *hdr, const char *str)
> +{
> +	if (H5_HDR_RELIABLE(hdr))
> +		printf("%s REL: seq %u ack %u crc %u type %u len %u\n",
> +				str, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
> +				H5_HDR_CRC(hdr), H5_HDR_PKT_TYPE(hdr),
> +				H5_HDR_LEN(hdr));
> +	else
> +		printf("%s UNREL: ack %u crc %u type %u len %u\n",
> +				str, H5_HDR_ACK(hdr), H5_HDR_CRC(hdr),
> +				H5_HDR_PKT_TYPE(hdr), H5_HDR_LEN(hdr));
> +}
> +
> +static void h5_send(struct proxy *proxy, const uint8_t *payload,
> +						uint8_t pkt_type, int len)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	struct h5_pkt *pkt;
> +	uint8_t *ptr;
> +	uint8_t hdr[4];
> +	int i;
> +
> +	memset(hdr, 0, sizeof(hdr));
> +
> +	H5_SET_ACK(hdr, h5->tx_ack);
> +
> +	if (h5_reliable_pkt(pkt_type)) {
> +		H5_SET_RELIABLE(hdr);
> +		H5_SET_SEQ(hdr, h5->tx_seq);
> +		h5->tx_seq = (h5->tx_seq + 1) % 8;
> +	}
> +
> +	H5_SET_TYPE(hdr, pkt_type);
> +	H5_SET_LEN(hdr, len);
> +
> +	/* Calculate CRC */
> +	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
> +
> +	h5_print_header(hdr, "TX: <");
> +
> +	/*
> +	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
> +	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
> +	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
> +	 * delimiters at start and end).
> +	 */
> +	pkt = malloc(sizeof(*pkt) + (len + 6) * 2 + 2);
> +	if (!pkt)
> +		return;
> +
> +	ptr = pkt->data;
> +
> +	*ptr++ = SLIP_DELIMITER;
> +
> +	for (i = 0; i < 4; i++)
> +		ptr += h5_slip_byte(ptr, hdr[i]);
> +
> +	for (i = 0; i < len; i++)
> +		ptr += h5_slip_byte(ptr, payload[i]);
> +
> +	*ptr++ = SLIP_DELIMITER;
> +
> +	pkt->len = ptr - pkt->data;
> +
> +	if (h5_reliable_pkt(pkt_type))
> +		queue_push_tail(h5->tx_queue_rel, pkt);
> +	else
> +		queue_push_tail(h5->tx_queue_unrel, pkt);
> +}
> +
> +static void h5_process_sig_pkt(struct proxy *proxy)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	uint8_t conf_rsp[3] = { 0x04, 0x7b };
> +	const uint8_t *hdr = h5->rx_buf;
> +	const uint8_t *payload = hdr + 4;
> +
> +	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
> +		return;
> +
> +	if (H5_HDR_LEN(hdr) < 2)
> +		return;
> +
> +	conf_rsp[2] = h5->tx_win & 0x07;
> +
> +	if (!memcmp(payload, sync_req, sizeof(sync_req))) {
> +		if (h5->state == H5_ACTIVE)
> +			h5_reset_peer(h5);
> +
> +		h5_send(proxy, sync_rsp, H5_HDR_PKT_TYPE(hdr),
> +							sizeof(sync_rsp));
> +		return;
> +	} else if (!memcmp(payload, conf_req, 2)) {
> +		h5_send(proxy, conf_rsp, H5_HDR_PKT_TYPE(hdr),
> +							sizeof(conf_rsp));
> +		return;
> +	}
> +
> +	exit(1);
> +}
> +
> +static void h5_process_data(struct proxy *proxy, uint8_t pkt_type)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	uint8_t pkt[1 + h5->rx_ptr - 4 - h5->rx_buf];
> +
> +	pkt[0] = pkt_type;
> +	memcpy(&pkt[1], h5->rx_buf + 4, sizeof(pkt) - 1);
> +
> +	h5->host_write(proxy, pkt, sizeof(pkt));
> +}
> +
> +static void pkt_print(void *data, void *user_data)
> +{
> +	struct h5_pkt *pkt = data;
> +
> +	h5_print_header((uint8_t *)pkt + sizeof(*pkt) + 1, "unack pkt");
> +}
> +
> +static void h5_process_unack_queue(struct proxy *proxy)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	uint8_t len = queue_length(h5->tx_queue_unack);
> +	uint8_t seq = h5->tx_seq;
> +	struct h5_pkt *pkt;
> +
> +	if (!len)
> +		return;
> +
> +	printf("%s: rx_ack %u tx_ack %u tx_seq %u\n", __func__,
> +					h5->rx_ack, h5->tx_ack, h5->tx_seq);
> +
> +	printf("%s: unack queue length %u\n", __func__, len);
> +
> +	queue_foreach(h5->tx_queue_unack, pkt_print, NULL);
> +
> +	/* Remove acked packets from unack queue */
> +	while (len > 0) {
> +		if (h5->rx_ack == seq)
> +			break;
> +
> +		len--;
> +		seq = (seq - 1) & 0x07;
> +	}
> +
> +	if (seq != h5->rx_ack)
> +		fprintf(stderr, "Acked wrong packet\n");
> +
> +	while (len--) {
> +		printf("%s: Remove packet\n", __func__);
> +
> +		pkt = queue_pop_head(h5->tx_queue_unack);
> +		if (pkt)
> +			free(pkt);
> +	}
> +}
> +
> +static void h5_process_complete_pkt(struct proxy *proxy)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	const uint8_t *hdr = h5->rx_buf;
> +
> +	if (H5_HDR_RELIABLE(hdr)) {
> +		h5->tx_ack = (h5->tx_ack + 1) % 8;
> +		/* TODO: Set ack req flag */
> +	}
> +
> +	h5->rx_ack = H5_HDR_ACK(hdr);
> +
> +	h5_process_unack_queue(proxy);
> +
> +	switch (H5_HDR_PKT_TYPE(hdr)) {
> +	case HCI_COMMAND_PKT:
> +	case HCI_EVENT_PKT:
> +	case HCI_ACLDATA_PKT:
> +	case HCI_SCODATA_PKT:
> +		/* Need to remove three-wire header */
> +		h5_process_data(proxy, (H5_HDR_PKT_TYPE(hdr)));
> +		break;
> +	default:
> +		/* Handle three-wire protocol */
> +		h5_process_sig_pkt(proxy);
> +	}
> +
> +	h5_reset_rx(h5);
> +}
> +
> +static int h5_crc(struct proxy *proxy, const uint8_t byte)
> +{
> +	h5_process_complete_pkt(proxy);
> +
> +	return 0;
> +}
> +
> +static int h5_payload(struct proxy *proxy, const uint8_t byte)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	const uint8_t *hdr = h5->rx_buf;
> +
> +	if (H5_HDR_RELIABLE(hdr)) {
> +		/* TODO: process tx_ack */
> +	}
> +
> +	if (H5_HDR_CRC(hdr)) {
> +		h5->rx_func = h5_crc;
> +		h5->rx_pending = 2;
> +	} else {
> +		h5_process_complete_pkt(proxy);
> +	}
> +
> +	return 0;
> +}
> +
> +static int h5_3wire_hdr(struct proxy *proxy, const uint8_t byte)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	const uint8_t *hdr = h5->rx_buf;
> +
> +	h5_print_header(hdr, "RX: >");
> +
> +	/* Add header checks here */
> +
> +	h5->rx_func = h5_payload;
> +	h5->rx_pending = H5_HDR_LEN(hdr);
> +
> +	return 0;
> +}
> +
> +static int h5_pkt_start(struct proxy *proxy, const uint8_t byte)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +
> +	if (byte == SLIP_DELIMITER)
> +		return 1;
> +
> +	h5->rx_func = h5_3wire_hdr;
> +	h5->rx_pending = 4;
> +
> +	return 0;
> +}
> +
> +static int h5_delimeter(struct proxy *proxy, const uint8_t byte)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +
> +	if (byte == SLIP_DELIMITER)
> +		h5->rx_func = h5_pkt_start;
> +
> +	return 1;
> +}
> +
> +static void h5_reset_rx(struct h5 *h5)
> +{
> +	h5->rx_pending = 0;
> +	h5->flags = 0;
> +
> +	h5->rx_ptr = h5->rx_buf;
> +
> +	h5->rx_func = h5_delimeter;
> +}
> +
> +static void h5_process_byte(struct proxy *proxy, const uint8_t byte)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +
> +	/* Mark escaped */
> +	if (!(h5->flags & H5_RX_ESC) && byte == SLIP_ESC) {
> +		h5->flags |= H5_RX_ESC;
> +		return;
> +	}
> +
> +	if (h5->flags & H5_RX_ESC) {
> +		h5->flags &= ~H5_RX_ESC;
> +
> +		switch (byte) {
> +		case SLIP_ESC_DELIM:
> +			*h5->rx_ptr++ = SLIP_DELIMITER;
> +			break;
> +		case SLIP_ESC_ESC:
> +			*h5->rx_ptr++ = SLIP_ESC;
> +			break;
> +		default:
> +			fprintf(stderr, "Invalid esc byte %x\n", byte);
> +			h5_reset_rx(h5);
> +			return;
> +		}
> +	} else {
> +		*h5->rx_ptr++ = byte;
> +	}
> +
> +	h5->rx_pending--;
> +}
> +
> +static void h5_process_tx_queue(struct proxy *proxy);

It would be great if we can reorder the code so that no forward declarations are needed.

> +
> +static void h5_timer_cb(int id, void *user_data)
> +{
> +	struct proxy *proxy = user_data;
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	struct h5_pkt *pkt;
> +
> +	printf("%s: Retransmitting %u packets\n", __func__,
> +					queue_length(h5->tx_queue_unack));
> +
> +	while ((pkt = queue_peek_tail(h5->tx_queue_unack))) {
> +		h5->tx_seq = (h5->tx_seq - 1) & 0x07;
> +
> +		/* Move pkt from unack to rel queue */
> +		queue_remove(h5->tx_queue_unack, pkt);
> +		queue_push_head(h5->tx_queue_rel, pkt);
> +	}
> +
> +	h5_process_tx_queue(proxy);
> +}
> +
> +static void h5_process_tx_queue(struct proxy *proxy)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	struct h5_pkt *pkt;
> +
> +	while ((pkt = queue_pop_head(h5->tx_queue_unrel))) {
> +		h5->dev_write(proxy, pkt->data, pkt->len);
> +
> +		free(pkt);
> +	}
> +
> +	if (queue_length(h5->tx_queue_unack) >= h5->tx_win) {
> +		printf("Unacked queue too big\n");
> +		return;
> +	}
> +
> +	while ((pkt = queue_pop_head(h5->tx_queue_rel))) {
> +		h5->dev_write(proxy, pkt->data, pkt->len);
> +		/*
> +		if (!write_packet(proxy->host_fd, pkt->data, pkt->len, "H: ")) {
> +			fprintf(stderr, "Write to Host descriptor failed\n");
> +			mainloop_remove_fd(proxy->host_fd);
> +		}
> +		*/
> +
> +		queue_push_tail(h5->tx_queue_unack, pkt);
> +		if (h5->timer_id >= 0) {
> +			mainloop_modify_timeout(h5->timer_id, H5_ACK_TIMEOUT);
> +			continue;
> +		}
> +
> +		h5->timer_id = mainloop_add_timeout(H5_ACK_TIMEOUT, h5_timer_cb,
> +								proxy, NULL);
> +	}
> +}
> +
> +void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len)
> +{
> +	struct h5 *h5 = proxy_get_h5(proxy);
> +	const uint8_t *ptr = buf;
> +
> +	while (len > 0) {
> +		int processed;
> +
> +		if (h5->rx_pending > 0) {
> +			if (*ptr == SLIP_DELIMITER) {
> +				fprintf(stderr, "Too short packet\n");
> +				h5_reset_rx(h5);
> +				continue;
> +			}
> +
> +			h5_process_byte(proxy, *ptr);
> +
> +			ptr++;
> +			len--;
> +			continue;
> +		}
> +
> +		processed = h5->rx_func(proxy, *ptr);
> +		if (processed < 0) {
> +			fprintf(stderr, "Error processing SLIP packet\n");
> +			return;
> +		}
> +
> +		ptr += processed;
> +		len -= processed;
> +
> +		h5_process_tx_queue(proxy);
> +	}
> +
> +	h5_process_tx_queue(proxy);
> +}
> +
> +void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len)
> +{
> +	uint8_t pkt_type = buf[0];
> +
> +	/* Get payload out of H4 packet */
> +	h5_send(proxy, &buf[1], pkt_type, len - 1);
> +
> +	h5_process_tx_queue(proxy);
> +}
> +
> +
> diff --git a/tools/h5.h b/tools/h5.h
> new file mode 100644
> index 0000000..cd88e30
> --- /dev/null
> +++ b/tools/h5.h
> @@ -0,0 +1,71 @@
> +/*
> + *
> + *  BlueZ - Bluetooth protocol stack for Linux
> + *
> + *  Copyright (C) 2015  Intel Corporation
> + *
> + *
> + *  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.
> + *
> + *  You should have received a copy of the GNU General Public License
> + *  along with this program; if not, write to the Free Software
> + *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
> + *
> + */
> +
> +#define H5_ACK_TIMEOUT		250
> +
> +#define HCI_3WIRE_ACK_PKT	0
> +#define HCI_3WIRE_LINK_PKT	15
> +
> +/*
> + * Maximum Three-wire packet:
> + *     4 byte header + max value for 12-bit length + 2 bytes for CRC
> + */
> +#define H5_MAX_LEN (4 + 0xfff + 2)
> +
> +/* Sliding window size */
> +#define H5_TX_WIN_MAX		4
> +
> +#define SLIP_DELIMITER	0xc0
> +#define SLIP_ESC	0xdb
> +#define SLIP_ESC_DELIM	0xdc
> +#define SLIP_ESC_ESC	0xdd
> +
> +#define H5_RX_ESC	1
> +
> +#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
> +#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
> +#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
> +#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
> +#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
> +#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
> +
> +#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
> +#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
> +#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
> +#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
> +#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
> +						((hdr)[2] |= (len) >> 4))

Move all of these to the .c file. They do not belong in the header.

> +
> +struct proxy;
> +struct h5;
> +
> +bool h5_init(struct proxy *proxy,
> +		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
> +		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len));
> +void h5_clean(struct h5 *h5);
> +
> +void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len);
> +void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len);
> +
> +void proxy_set_h5(struct proxy *proxy, struct h5 *h5);
> +struct h5 *proxy_get_h5(struct proxy *proxy);

I would really prefer if we can have the struct h5 be an abstraction that works for more than just struct proxy. Please try to make this more generic. The more generic it is the more useful it can become.

Regards

Marcel

Re: [PATCHv1 2/2] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

Hi Marcel,

On Fri, Nov 27, 2015 at 06:56:07AM -0800, Marcel Holtmann wrote:
> Hi Andrei,
> 
> > With H5 support it is possible to create pts and attach to it using
> > hciattach ot btattach with 3wire protocol. This is useful for testing
> > and developing three-wire protocol.
> > Implementation is based on kernel hci_h5.c H5 protocol.
> > 
> > Simple usage:
> > To open virtual pts run:
> > $ sudo tools/btproxy -d --pty -3
> > Opening pseudoterminal
> > New pts created: /dev/pts/2
> > Opening user channel for hci0
> > 
> > Now attach to it using hciattach:
> > $ sudo hciattach -n /dev/pts/2 3wire
> > Device setup complete
> > ---
> > Makefile.tools  |   2 +-
> > tools/btproxy.c |  47 ++++-
> > tools/h5.c      | 553 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
> > tools/h5.h      |  71 ++++++++
> > 4 files changed, 667 insertions(+), 6 deletions(-)
> > create mode 100644 tools/h5.c
> > create mode 100644 tools/h5.h
> > 
> > diff --git a/Makefile.tools b/Makefile.tools
> > index 6ebbe9f..275603a 100644
> > --- a/Makefile.tools
> > +++ b/Makefile.tools
> > @@ -284,7 +284,7 @@ tools_btattach_LDADD = src/libshared-mainloop.la
> > tools_btsnoop_SOURCES = tools/btsnoop.c
> > tools_btsnoop_LDADD = src/libshared-mainloop.la
> > 
> > -tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h
> > +tools_btproxy_SOURCES = tools/btproxy.c tools/h5.c tools/h5.h monitor/bt.h
> 
> generally .h files before .c files. And I would do this one like this:
> 
> tools_btproxy_SOURCES = tools/btproxy.c monitor.h \
> 					tools/h5.h tools/h5.c

OK

> > tools_btproxy_LDADD = src/libshared-mainloop.la
> > 
> > tools_btiotest_SOURCES = tools/btiotest.c btio/btio.h btio/btio.c
> > diff --git a/tools/btproxy.c b/tools/btproxy.c
> > index 6d78876..f4879da 100644
> > --- a/tools/btproxy.c
> > +++ b/tools/btproxy.c
> > @@ -48,6 +48,7 @@
> > #include "src/shared/mainloop.h"
> > #include "src/shared/ecc.h"
> > #include "monitor/bt.h"
> > +#include "h5.h"
> > 
> > #define HCI_BREDR	0x00
> > #define HCI_AMP		0x01
> > @@ -64,6 +65,7 @@ static uint16_t hci_index = 0;
> > static bool client_active = false;
> > static bool debug_enabled = false;
> > static bool emulate_ecc = false;
> > +static bool three_wire = false;
> > 
> > static void hexdump_print(const char *str, void *user_data)
> > {
> > @@ -86,8 +88,21 @@ struct proxy {
> > 	/* ECC emulation */
> > 	uint8_t event_mask[8];
> > 	uint8_t local_sk256[32];
> > +
> > +	/* H5 protocol */
> > +	struct h5 *h5;
> > };
> > 
> > +void proxy_set_h5(struct proxy *proxy, struct h5 *h5)
> > +{
> > +	proxy->h5 = h5;
> > +}
> > +
> > +struct h5 *proxy_get_h5(struct proxy *proxy)
> > +{
> > +	return proxy->h5;
> > +}
> > +
> > static bool write_packet(int fd, const void *data, size_t size,
> > 							void *user_data)
> > {
> > @@ -351,6 +366,14 @@ process_packet:
> > 		sco_hdr = (void *) (proxy->host_buf + 1);
> > 		pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
> > 		break;
> > +	case SLIP_DELIMITER:
> > +		/* 2 bytes start/end packets + SLIP header and checksum */
> > +		if (proxy->host_len < 4 + 2)
> > +			return;
> > +
> > +		/* Later we shall check unslipped packet */
> > +		pktlen = proxy->host_len;
> > +		break;
> 
> This is something I do not like much. It is not really an H:4 packet at all. This is intermixing the H:4 part with the H:5 part and I not convinced that is a good idea.

I moved the code right after read() in the next patch series.

> 
> > 	case 0xff:
> > 		/* Notification packet from /dev/vhci - ignore */
> > 		proxy->host_len = 0;
> > @@ -367,6 +390,8 @@ process_packet:
> > 
> > 	if (emulate_ecc)
> > 		host_emulate_ecc(proxy, proxy->host_buf, pktlen);
> > +	else if (three_wire)
> > +		host_write_3wire_packet(proxy, proxy->host_buf, pktlen);
> > 	else
> > 		host_write_packet(proxy, proxy->host_buf, pktlen);
> 
> Is this really the best idea? We might need some extra abstraction for the transport to allow for better HCI vs transport abstraction.

the code should be moved out from here

> 
> > 
> > @@ -475,6 +500,8 @@ process_packet:
> > 
> > 	if (emulate_ecc)
> > 		dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
> > +	else if (three_wire)
> > +		dev_write_3wire_packet(proxy, proxy->dev_buf, pktlen);
> > 	else
> > 		dev_write_packet(proxy, proxy->dev_buf, pktlen);
> > 
> > @@ -488,14 +515,14 @@ process_packet:
> > 	proxy->dev_len = 0;
> > }
> > 
> > -static bool setup_proxy(int host_fd, bool host_shutdown,
> > +static struct proxy *setup_proxy(int host_fd, bool host_shutdown,
> > 						int dev_fd, bool dev_shutdown)
> > {
> > 	struct proxy *proxy;
> > 
> > 	proxy = new0(struct proxy, 1);
> > 	if (!proxy)
> > -		return false;
> > +		return NULL;
> > 
> > 	if (emulate_ecc)
> > 		printf("Enabling ECC emulation\n");
> > @@ -512,7 +539,7 @@ static bool setup_proxy(int host_fd, bool host_shutdown,
> > 	mainloop_add_fd(proxy->dev_fd, EPOLLIN | EPOLLRDHUP,
> > 				dev_read_callback, proxy, dev_read_destroy);
> > 
> > -	return true;
> > +	return proxy;
> > }
> > 
> > static int open_channel(uint16_t index)
> > @@ -764,6 +791,7 @@ static void usage(void)
> > 		"\t-l, --listen [address]      Use TCP server\n"
> > 		"\t-u, --unix [path]           Use Unix server\n"
> > 		"\t-P, --pty                   Use PTY\n"
> > +		"\t-3, --3wire                 Use 3wire protocol\n"
> > 		"\t-p, --port <port>           Use specified TCP port\n"
> > 		"\t-i, --index <num>           Use specified controller\n"
> > 		"\t-a, --amp                   Create AMP controller\n"
> > @@ -778,6 +806,7 @@ static const struct option main_options[] = {
> > 	{ "listen",   optional_argument, NULL, 'l' },
> > 	{ "unix",     optional_argument, NULL, 'u' },
> > 	{ "pty",      no_argument,       NULL, 'P' },
> > +	{ "3wire",    no_argument,       NULL, '3' },
> > 	{ "port",     required_argument, NULL, 'p' },
> > 	{ "index",    required_argument, NULL, 'i' },
> > 	{ "amp",      no_argument,       NULL, 'a' },
> > @@ -803,7 +832,7 @@ int main(int argc, char *argv[])
> > 	for (;;) {
> > 		int opt;
> > 
> > -		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
> > +		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
> > 						main_options, NULL);
> > 		if (opt < 0)
> > 			break;
> > @@ -830,6 +859,9 @@ int main(int argc, char *argv[])
> > 		case 'P':
> > 			use_pty = true;
> > 			break;
> > +		case '3':
> > +			three_wire = true;
> > +			break;
> > 		case 'p':
> > 			tcp_port = atoi(optarg);
> > 			break;
> > @@ -920,6 +952,7 @@ int main(int argc, char *argv[])
> > 		}
> > 	} else if (use_pty) {
> > 		int master_fd, dev_fd;
> > +		struct proxy *proxy;
> > 
> > 		printf("Opening pseudoterminal\n");
> > 
> > @@ -933,11 +966,15 @@ int main(int argc, char *argv[])
> > 			return EXIT_FAILURE;
> > 		}
> > 
> > -		if (!setup_proxy(master_fd, false, dev_fd, true)) {
> > +		proxy = setup_proxy(master_fd, false, dev_fd, true);
> > +		if (!proxy) {
> > 			close(dev_fd);
> > 			close(master_fd);
> > 			return EXIT_FAILURE;
> > 		}
> > +
> > +		if (three_wire)
> > +			h5_init(proxy, host_write_packet, dev_write_packet);
> 
> I wonder if it would not be better to leave setup_proxy as is and instead create a new setup_proxy_h5.

Done.

> > 	} else {
> > 		int server_fd;
> > 
> > diff --git a/tools/h5.c b/tools/h5.c
> > new file mode 100644
> > index 0000000..15a42be
> > --- /dev/null
> > +++ b/tools/h5.c
> > @@ -0,0 +1,553 @@
> > +/*
> > + *
> > + *  BlueZ - Bluetooth protocol stack for Linux
> > + *
> > + *  Copyright (C) 2015  Intel Corporation
> > + *
> > + *
> > + *  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.
> > + *
> > + *  You should have received a copy of the GNU General Public License
> > + *  along with this program; if not, write to the Free Software
> > + *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
> > + *
> > + */
> > +
> > +#include <stdint.h>
> > +#include <stdlib.h>
> > +#include <stdbool.h>
> > +#include <unistd.h>
> > +
> > +#include "src/shared/mainloop.h"
> > +#include "src/shared/queue.h"
> > +#include "lib/bluetooth.h"
> > +#include "lib/hci.h"
> > +
> > +#include "h5.h"
> > +
> > +static const uint8_t sync_req[] = { 0x01, 0x7e };
> > +static const uint8_t sync_rsp[] = { 0x02, 0x7d };
> > +static const uint8_t conf_req[] = { 0x03, 0xfc };
> > +static const uint8_t wakeup_req[] = { 0x05, 0xfa };
> > +static const uint8_t woken_req[] = { 0x06, 0xf9 };
> > +static const uint8_t sleep_req[] = { 0x07, 0x78 };
> > +
> > +struct h5 {
> > +	size_t rx_pending;
> > +	uint8_t rx_buf[H5_MAX_LEN];
> > +	uint8_t *rx_ptr;
> > +	uint8_t flags;
> > +
> > +	uint8_t rx_ack;		/* Received last ack number */
> > +	uint8_t tx_seq;		/* Next seq number to send */
> > +	uint8_t tx_ack;		/* Next ack number to send */
> > +
> > +	uint8_t tx_win;
> > +
> > +	enum {
> > +		H5_UNINITIALIZED,
> > +		H5_INITIALIZED,
> > +		H5_ACTIVE,
> > +	} state;
> > +
> > +	int timer_id;
> > +
> > +	struct queue *tx_queue_unack;
> > +	struct queue *tx_queue_unrel;
> > +	struct queue *tx_queue_rel;
> > +
> > +	int (*rx_func)(struct proxy *proxy, uint8_t byte);
> > +
> > +	void (*host_write)(struct proxy *proxy, void *buf, uint16_t len);
> > +	void (*dev_write)(struct proxy *proxy, void *buf, uint16_t len);
> > +};
> > +
> > +struct h5_pkt {
> > +	uint16_t len;
> > +	uint8_t data[0];
> > +};
> > +
> > +static void h5_reset_rx(struct h5 *h5);
> > +
> > +static void h5_reset_peer(struct h5 *h5)
> > +{
> > +	h5->state = H5_UNINITIALIZED;
> > +
> > +	mainloop_remove_timeout(h5->timer_id);
> 
> Actually using src/shared/timeout.h would be preferred. It has an implementation that uses mainloop, but is a lot more generic and its us eventually easier move towards ELL.

OK

> 
> > +	h5->timer_id = -1;
> > +	h5_reset_rx(h5);
> > +}
> > +
> > +bool h5_init(struct proxy *proxy,
> > +		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
> > +		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len))
> > +{
> > +	struct h5 *h5;
> > +
> > +	h5 = malloc(sizeof(*h5));
> > +	if (!h5)
> > +		return false;
> > +
> > +	h5->tx_win = H5_TX_WIN_MAX;
> > +	h5_reset_peer(h5);
> > +
> > +	h5->tx_queue_unack = queue_new();
> > +	h5->tx_queue_unrel = queue_new();
> > +	h5->tx_queue_rel = queue_new();
> > +
> > +	h5->host_write = host_w;
> > +	h5->dev_write = dev_w;
> > +
> > +	proxy_set_h5(proxy, h5);
> > +
> > +	return true;
> > +}
> > +
> > +void h5_clean(struct h5 *h5)
> > +{
> > +	mainloop_remove_timeout(h5->timer_id);
> > +
> > +	queue_remove_all(h5->tx_queue_unack, NULL, NULL, free);
> > +	queue_remove_all(h5->tx_queue_unrel, NULL, NULL, free);
> > +	queue_remove_all(h5->tx_queue_rel, NULL, NULL, free);
> > +
> > +	free(h5);
> > +}
> > +
> > +static bool h5_reliable_pkt(uint8_t type)
> > +{
> > +	switch (type) {
> > +	case HCI_ACLDATA_PKT:
> > +	case HCI_COMMAND_PKT:
> > +	case HCI_EVENT_PKT:
> > +		return true;
> > +	default:
> > +		return false;
> > +	}
> > +}
> > +
> > +static uint8_t h5_slip_byte(uint8_t *pkt, uint8_t byte)
> > +{
> > +	const uint8_t esc_delim[] = { SLIP_ESC, SLIP_ESC_DELIM };
> > +	const uint8_t esc_esc[] = { SLIP_ESC, SLIP_ESC_ESC };
> > +
> > +	switch (byte) {
> > +	case SLIP_DELIMITER:
> > +		memcpy(pkt, &esc_delim, sizeof(esc_delim));
> > +		return sizeof(esc_delim);
> > +	case SLIP_ESC:
> > +		memcpy(pkt, &esc_esc, sizeof(esc_esc));
> > +		return sizeof(esc_esc);
> > +	default:
> > +		memcpy(pkt, &byte, sizeof(byte));
> > +		return sizeof(byte);
> > +	}
> > +}
> > +
> > +static void h5_print_header(const uint8_t *hdr, const char *str)
> > +{
> > +	if (H5_HDR_RELIABLE(hdr))
> > +		printf("%s REL: seq %u ack %u crc %u type %u len %u\n",
> > +				str, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
> > +				H5_HDR_CRC(hdr), H5_HDR_PKT_TYPE(hdr),
> > +				H5_HDR_LEN(hdr));
> > +	else
> > +		printf("%s UNREL: ack %u crc %u type %u len %u\n",
> > +				str, H5_HDR_ACK(hdr), H5_HDR_CRC(hdr),
> > +				H5_HDR_PKT_TYPE(hdr), H5_HDR_LEN(hdr));
> > +}
> > +
> > +static void h5_send(struct proxy *proxy, const uint8_t *payload,
> > +						uint8_t pkt_type, int len)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	struct h5_pkt *pkt;
> > +	uint8_t *ptr;
> > +	uint8_t hdr[4];
> > +	int i;
> > +
> > +	memset(hdr, 0, sizeof(hdr));
> > +
> > +	H5_SET_ACK(hdr, h5->tx_ack);
> > +
> > +	if (h5_reliable_pkt(pkt_type)) {
> > +		H5_SET_RELIABLE(hdr);
> > +		H5_SET_SEQ(hdr, h5->tx_seq);
> > +		h5->tx_seq = (h5->tx_seq + 1) % 8;
> > +	}
> > +
> > +	H5_SET_TYPE(hdr, pkt_type);
> > +	H5_SET_LEN(hdr, len);
> > +
> > +	/* Calculate CRC */
> > +	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
> > +
> > +	h5_print_header(hdr, "TX: <");
> > +
> > +	/*
> > +	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
> > +	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
> > +	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
> > +	 * delimiters at start and end).
> > +	 */
> > +	pkt = malloc(sizeof(*pkt) + (len + 6) * 2 + 2);
> > +	if (!pkt)
> > +		return;
> > +
> > +	ptr = pkt->data;
> > +
> > +	*ptr++ = SLIP_DELIMITER;
> > +
> > +	for (i = 0; i < 4; i++)
> > +		ptr += h5_slip_byte(ptr, hdr[i]);
> > +
> > +	for (i = 0; i < len; i++)
> > +		ptr += h5_slip_byte(ptr, payload[i]);
> > +
> > +	*ptr++ = SLIP_DELIMITER;
> > +
> > +	pkt->len = ptr - pkt->data;
> > +
> > +	if (h5_reliable_pkt(pkt_type))
> > +		queue_push_tail(h5->tx_queue_rel, pkt);
> > +	else
> > +		queue_push_tail(h5->tx_queue_unrel, pkt);
> > +}
> > +
> > +static void h5_process_sig_pkt(struct proxy *proxy)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	uint8_t conf_rsp[3] = { 0x04, 0x7b };
> > +	const uint8_t *hdr = h5->rx_buf;
> > +	const uint8_t *payload = hdr + 4;
> > +
> > +	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
> > +		return;
> > +
> > +	if (H5_HDR_LEN(hdr) < 2)
> > +		return;
> > +
> > +	conf_rsp[2] = h5->tx_win & 0x07;
> > +
> > +	if (!memcmp(payload, sync_req, sizeof(sync_req))) {
> > +		if (h5->state == H5_ACTIVE)
> > +			h5_reset_peer(h5);
> > +
> > +		h5_send(proxy, sync_rsp, H5_HDR_PKT_TYPE(hdr),
> > +							sizeof(sync_rsp));
> > +		return;
> > +	} else if (!memcmp(payload, conf_req, 2)) {
> > +		h5_send(proxy, conf_rsp, H5_HDR_PKT_TYPE(hdr),
> > +							sizeof(conf_rsp));
> > +		return;
> > +	}
> > +
> > +	exit(1);
> > +}
> > +
> > +static void h5_process_data(struct proxy *proxy, uint8_t pkt_type)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	uint8_t pkt[1 + h5->rx_ptr - 4 - h5->rx_buf];
> > +
> > +	pkt[0] = pkt_type;
> > +	memcpy(&pkt[1], h5->rx_buf + 4, sizeof(pkt) - 1);
> > +
> > +	h5->host_write(proxy, pkt, sizeof(pkt));
> > +}
> > +
> > +static void pkt_print(void *data, void *user_data)
> > +{
> > +	struct h5_pkt *pkt = data;
> > +
> > +	h5_print_header((uint8_t *)pkt + sizeof(*pkt) + 1, "unack pkt");
> > +}
> > +
> > +static void h5_process_unack_queue(struct proxy *proxy)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	uint8_t len = queue_length(h5->tx_queue_unack);
> > +	uint8_t seq = h5->tx_seq;
> > +	struct h5_pkt *pkt;
> > +
> > +	if (!len)
> > +		return;
> > +
> > +	printf("%s: rx_ack %u tx_ack %u tx_seq %u\n", __func__,
> > +					h5->rx_ack, h5->tx_ack, h5->tx_seq);
> > +
> > +	printf("%s: unack queue length %u\n", __func__, len);
> > +
> > +	queue_foreach(h5->tx_queue_unack, pkt_print, NULL);
> > +
> > +	/* Remove acked packets from unack queue */
> > +	while (len > 0) {
> > +		if (h5->rx_ack == seq)
> > +			break;
> > +
> > +		len--;
> > +		seq = (seq - 1) & 0x07;
> > +	}
> > +
> > +	if (seq != h5->rx_ack)
> > +		fprintf(stderr, "Acked wrong packet\n");
> > +
> > +	while (len--) {
> > +		printf("%s: Remove packet\n", __func__);
> > +
> > +		pkt = queue_pop_head(h5->tx_queue_unack);
> > +		if (pkt)
> > +			free(pkt);
> > +	}
> > +}
> > +
> > +static void h5_process_complete_pkt(struct proxy *proxy)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	const uint8_t *hdr = h5->rx_buf;
> > +
> > +	if (H5_HDR_RELIABLE(hdr)) {
> > +		h5->tx_ack = (h5->tx_ack + 1) % 8;
> > +		/* TODO: Set ack req flag */
> > +	}
> > +
> > +	h5->rx_ack = H5_HDR_ACK(hdr);
> > +
> > +	h5_process_unack_queue(proxy);
> > +
> > +	switch (H5_HDR_PKT_TYPE(hdr)) {
> > +	case HCI_COMMAND_PKT:
> > +	case HCI_EVENT_PKT:
> > +	case HCI_ACLDATA_PKT:
> > +	case HCI_SCODATA_PKT:
> > +		/* Need to remove three-wire header */
> > +		h5_process_data(proxy, (H5_HDR_PKT_TYPE(hdr)));
> > +		break;
> > +	default:
> > +		/* Handle three-wire protocol */
> > +		h5_process_sig_pkt(proxy);
> > +	}
> > +
> > +	h5_reset_rx(h5);
> > +}
> > +
> > +static int h5_crc(struct proxy *proxy, const uint8_t byte)
> > +{
> > +	h5_process_complete_pkt(proxy);
> > +
> > +	return 0;
> > +}
> > +
> > +static int h5_payload(struct proxy *proxy, const uint8_t byte)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	const uint8_t *hdr = h5->rx_buf;
> > +
> > +	if (H5_HDR_RELIABLE(hdr)) {
> > +		/* TODO: process tx_ack */
> > +	}
> > +
> > +	if (H5_HDR_CRC(hdr)) {
> > +		h5->rx_func = h5_crc;
> > +		h5->rx_pending = 2;
> > +	} else {
> > +		h5_process_complete_pkt(proxy);
> > +	}
> > +
> > +	return 0;
> > +}
> > +
> > +static int h5_3wire_hdr(struct proxy *proxy, const uint8_t byte)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	const uint8_t *hdr = h5->rx_buf;
> > +
> > +	h5_print_header(hdr, "RX: >");
> > +
> > +	/* Add header checks here */
> > +
> > +	h5->rx_func = h5_payload;
> > +	h5->rx_pending = H5_HDR_LEN(hdr);
> > +
> > +	return 0;
> > +}
> > +
> > +static int h5_pkt_start(struct proxy *proxy, const uint8_t byte)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +
> > +	if (byte == SLIP_DELIMITER)
> > +		return 1;
> > +
> > +	h5->rx_func = h5_3wire_hdr;
> > +	h5->rx_pending = 4;
> > +
> > +	return 0;
> > +}
> > +
> > +static int h5_delimeter(struct proxy *proxy, const uint8_t byte)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +
> > +	if (byte == SLIP_DELIMITER)
> > +		h5->rx_func = h5_pkt_start;
> > +
> > +	return 1;
> > +}
> > +
> > +static void h5_reset_rx(struct h5 *h5)
> > +{
> > +	h5->rx_pending = 0;
> > +	h5->flags = 0;
> > +
> > +	h5->rx_ptr = h5->rx_buf;
> > +
> > +	h5->rx_func = h5_delimeter;
> > +}
> > +
> > +static void h5_process_byte(struct proxy *proxy, const uint8_t byte)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +
> > +	/* Mark escaped */
> > +	if (!(h5->flags & H5_RX_ESC) && byte == SLIP_ESC) {
> > +		h5->flags |= H5_RX_ESC;
> > +		return;
> > +	}
> > +
> > +	if (h5->flags & H5_RX_ESC) {
> > +		h5->flags &= ~H5_RX_ESC;
> > +
> > +		switch (byte) {
> > +		case SLIP_ESC_DELIM:
> > +			*h5->rx_ptr++ = SLIP_DELIMITER;
> > +			break;
> > +		case SLIP_ESC_ESC:
> > +			*h5->rx_ptr++ = SLIP_ESC;
> > +			break;
> > +		default:
> > +			fprintf(stderr, "Invalid esc byte %x\n", byte);
> > +			h5_reset_rx(h5);
> > +			return;
> > +		}
> > +	} else {
> > +		*h5->rx_ptr++ = byte;
> > +	}
> > +
> > +	h5->rx_pending--;
> > +}
> > +
> > +static void h5_process_tx_queue(struct proxy *proxy);
> 
> It would be great if we can reorder the code so that no forward declarations are needed.

It is not possible in this case.

> 
> > +
> > +static void h5_timer_cb(int id, void *user_data)
> > +{
> > +	struct proxy *proxy = user_data;
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	struct h5_pkt *pkt;
> > +
> > +	printf("%s: Retransmitting %u packets\n", __func__,
> > +					queue_length(h5->tx_queue_unack));
> > +
> > +	while ((pkt = queue_peek_tail(h5->tx_queue_unack))) {
> > +		h5->tx_seq = (h5->tx_seq - 1) & 0x07;
> > +
> > +		/* Move pkt from unack to rel queue */
> > +		queue_remove(h5->tx_queue_unack, pkt);
> > +		queue_push_head(h5->tx_queue_rel, pkt);
> > +	}
> > +
> > +	h5_process_tx_queue(proxy);
> > +}
> > +
> > +static void h5_process_tx_queue(struct proxy *proxy)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	struct h5_pkt *pkt;
> > +
> > +	while ((pkt = queue_pop_head(h5->tx_queue_unrel))) {
> > +		h5->dev_write(proxy, pkt->data, pkt->len);
> > +
> > +		free(pkt);
> > +	}
> > +
> > +	if (queue_length(h5->tx_queue_unack) >= h5->tx_win) {
> > +		printf("Unacked queue too big\n");
> > +		return;
> > +	}
> > +
> > +	while ((pkt = queue_pop_head(h5->tx_queue_rel))) {
> > +		h5->dev_write(proxy, pkt->data, pkt->len);
> > +		/*
> > +		if (!write_packet(proxy->host_fd, pkt->data, pkt->len, "H: ")) {
> > +			fprintf(stderr, "Write to Host descriptor failed\n");
> > +			mainloop_remove_fd(proxy->host_fd);
> > +		}
> > +		*/
> > +
> > +		queue_push_tail(h5->tx_queue_unack, pkt);
> > +		if (h5->timer_id >= 0) {
> > +			mainloop_modify_timeout(h5->timer_id, H5_ACK_TIMEOUT);
> > +			continue;
> > +		}
> > +
> > +		h5->timer_id = mainloop_add_timeout(H5_ACK_TIMEOUT, h5_timer_cb,
> > +								proxy, NULL);
> > +	}
> > +}
> > +
> > +void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len)
> > +{
> > +	struct h5 *h5 = proxy_get_h5(proxy);
> > +	const uint8_t *ptr = buf;
> > +
> > +	while (len > 0) {
> > +		int processed;
> > +
> > +		if (h5->rx_pending > 0) {
> > +			if (*ptr == SLIP_DELIMITER) {
> > +				fprintf(stderr, "Too short packet\n");
> > +				h5_reset_rx(h5);
> > +				continue;
> > +			}
> > +
> > +			h5_process_byte(proxy, *ptr);
> > +
> > +			ptr++;
> > +			len--;
> > +			continue;
> > +		}
> > +
> > +		processed = h5->rx_func(proxy, *ptr);
> > +		if (processed < 0) {
> > +			fprintf(stderr, "Error processing SLIP packet\n");
> > +			return;
> > +		}
> > +
> > +		ptr += processed;
> > +		len -= processed;
> > +
> > +		h5_process_tx_queue(proxy);
> > +	}
> > +
> > +	h5_process_tx_queue(proxy);
> > +}
> > +
> > +void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len)
> > +{
> > +	uint8_t pkt_type = buf[0];
> > +
> > +	/* Get payload out of H4 packet */
> > +	h5_send(proxy, &buf[1], pkt_type, len - 1);
> > +
> > +	h5_process_tx_queue(proxy);
> > +}
> > +
> > +
> > diff --git a/tools/h5.h b/tools/h5.h
> > new file mode 100644
> > index 0000000..cd88e30
> > --- /dev/null
> > +++ b/tools/h5.h
> > @@ -0,0 +1,71 @@
> > +/*
> > + *
> > + *  BlueZ - Bluetooth protocol stack for Linux
> > + *
> > + *  Copyright (C) 2015  Intel Corporation
> > + *
> > + *
> > + *  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.
> > + *
> > + *  You should have received a copy of the GNU General Public License
> > + *  along with this program; if not, write to the Free Software
> > + *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
> > + *
> > + */
> > +
> > +#define H5_ACK_TIMEOUT		250
> > +
> > +#define HCI_3WIRE_ACK_PKT	0
> > +#define HCI_3WIRE_LINK_PKT	15
> > +
> > +/*
> > + * Maximum Three-wire packet:
> > + *     4 byte header + max value for 12-bit length + 2 bytes for CRC
> > + */
> > +#define H5_MAX_LEN (4 + 0xfff + 2)
> > +
> > +/* Sliding window size */
> > +#define H5_TX_WIN_MAX		4
> > +
> > +#define SLIP_DELIMITER	0xc0
> > +#define SLIP_ESC	0xdb
> > +#define SLIP_ESC_DELIM	0xdc
> > +#define SLIP_ESC_ESC	0xdd
> > +
> > +#define H5_RX_ESC	1
> > +
> > +#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
> > +#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
> > +#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
> > +#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
> > +#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
> > +#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
> > +
> > +#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
> > +#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
> > +#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
> > +#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
> > +#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
> > +						((hdr)[2] |= (len) >> 4))
> 
> Move all of these to the .c file. They do not belong in the header.

OK

> 
> > +
> > +struct proxy;
> > +struct h5;
> > +
> > +bool h5_init(struct proxy *proxy,
> > +		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
> > +		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len));
> > +void h5_clean(struct h5 *h5);
> > +
> > +void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len);
> > +void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len);
> > +
> > +void proxy_set_h5(struct proxy *proxy, struct h5 *h5);
> > +struct h5 *proxy_get_h5(struct proxy *proxy);
> 
> I would really prefer if we can have the struct h5 be an abstraction that works for more than just struct proxy. Please try to make this more generic. The more generic it is the more useful it can become.

Do you mean here that struct proxy shall have union with struct h5 be
part of this union? Then we might add some other structures to this
union.

Best regards 
Andrei Emeltchenko 

[PATCHv2 0/2] Three-wire (H5) support in btproxy

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Changes:
	* v2: Fix comments from review, added h5 debug
	* v1: Split code to h5.[ch], remove global vars, add callbacks
	* RFC: Initial code

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete

Andrei Emeltchenko (2):
  btproxy: Add support for creating pseudoterminal
  btproxy: Add three-wire (h5) protocol initial support

 Makefile.tools  |   3 +-
 tools/btproxy.c | 121 +++++++++++-
 tools/h5.c      | 596 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tools/h5.h      |  36 ++++
 4 files changed, 752 insertions(+), 4 deletions(-)
 create mode 100644 tools/h5.c
 create mode 100644 tools/h5.h

-- 
2.5.0

[PATCHv2 1/2] btproxy: Add support for creating pseudoterminal

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new virtual Bluetooth serial controller on /dev/pts/X which allows
to test serial attach with btattach and hciattach.
---
 tools/btproxy.c | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 65 insertions(+), 3 deletions(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 43de037..6d78876 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -629,6 +629,41 @@ static int open_unix(const char *path)
 	return fd;
 }
 
+static int open_pty(void)
+{
+	int fd;
+	char *pts;
+
+	fd = open("/dev/ptmx", O_RDWR | O_NOCTTY);
+	if (fd < 0) {
+		perror("Failed to open pseudoterminal master");
+		return -1;
+	}
+
+	if (grantpt(fd) < 0) {
+		perror("Failed to grant access to slave pty");
+		close(fd);
+		return -1;
+	}
+
+	if (unlockpt(fd) < 0) {
+		perror("Failed to unlock slave pty");
+		close(fd);
+		return -1;
+	}
+
+	pts = ptsname(fd);
+	if (!pts) {
+		perror("Failed to get slave pty");
+		close(fd);
+		return -1;
+	}
+
+	printf("New pts created: %s\n", pts);
+
+	return fd;
+}
+
 static int open_tcp(const char *address, unsigned int port)
 {
 	struct sockaddr_in addr;
@@ -728,6 +763,7 @@ static void usage(void)
 		"\t-c, --connect <address>     Connect to server\n"
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
+		"\t-P, --pty                   Use PTY\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -741,6 +777,7 @@ static const struct option main_options[] = {
 	{ "connect",  required_argument, NULL, 'c' },
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
+	{ "pty",      no_argument,       NULL, 'P' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -758,6 +795,7 @@ int main(int argc, char *argv[])
 	const char *unix_path = NULL;
 	unsigned short tcp_port = 0xb1ee;	/* 45550 */
 	bool use_redirect = false;
+	bool use_pty = false;
 	uint8_t type = HCI_BREDR;
 	const char *str;
 	sigset_t mask;
@@ -765,7 +803,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::p:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -789,6 +827,9 @@ int main(int argc, char *argv[])
 			else
 				unix_path = "/tmp/bt-server-bredr";
 			break;
+		case 'P':
+			use_pty = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -828,12 +869,13 @@ int main(int argc, char *argv[])
 		return EXIT_FAILURE;
 	}
 
-	if (unix_path && (server_address || use_redirect)) {
+	if (unix_path && (server_address || use_redirect || use_pty)) {
 		fprintf(stderr, "Invalid to specify TCP and Unix servers\n");
 		return EXIT_FAILURE;
 	}
 
-	if (connect_address && (unix_path || server_address || use_redirect)) {
+	if (connect_address && (unix_path || server_address || use_redirect ||
+								use_pty)) {
 		fprintf(stderr, "Invalid to specify client and server mode\n");
 		return EXIT_FAILURE;
 	}
@@ -876,6 +918,26 @@ int main(int argc, char *argv[])
 			close(host_fd);
 			return EXIT_FAILURE;
 		}
+	} else if (use_pty) {
+		int master_fd, dev_fd;
+
+		printf("Opening pseudoterminal\n");
+
+		master_fd = open_pty();
+		if (master_fd < 0)
+			return EXIT_FAILURE;
+
+		dev_fd = open_channel(hci_index);
+		if (dev_fd < 0) {
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
+
+		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+			close(dev_fd);
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
 	} else {
 		int server_fd;
 
-- 
2.5.0

[PATCHv2 2/2] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete
---
 Makefile.tools  |   3 +-
 tools/btproxy.c |  57 +++++-
 tools/h5.c      | 596 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tools/h5.h      |  36 ++++
 4 files changed, 689 insertions(+), 3 deletions(-)
 create mode 100644 tools/h5.c
 create mode 100644 tools/h5.h

diff --git a/Makefile.tools b/Makefile.tools
index 6ebbe9f..cc95447 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -284,7 +284,8 @@ tools_btattach_LDADD = src/libshared-mainloop.la
 tools_btsnoop_SOURCES = tools/btsnoop.c
 tools_btsnoop_LDADD = src/libshared-mainloop.la
 
-tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h
+tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h \
+				 tools/h5.c tools/h5.h
 tools_btproxy_LDADD = src/libshared-mainloop.la
 
 tools_btiotest_SOURCES = tools/btiotest.c btio/btio.h btio/btio.c
diff --git a/tools/btproxy.c b/tools/btproxy.c
index 6d78876..1a3eb5e 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -48,6 +48,7 @@
 #include "src/shared/mainloop.h"
 #include "src/shared/ecc.h"
 #include "monitor/bt.h"
+#include "h5.h"
 
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
@@ -64,6 +65,7 @@ static uint16_t hci_index = 0;
 static bool client_active = false;
 static bool debug_enabled = false;
 static bool emulate_ecc = false;
+static bool three_wire = false;
 
 static void hexdump_print(const char *str, void *user_data)
 {
@@ -86,8 +88,21 @@ struct proxy {
 	/* ECC emulation */
 	uint8_t event_mask[8];
 	uint8_t local_sk256[32];
+
+	/* H5 protocol */
+	struct h5 *h5;
 };
 
+void proxy_set_h5(struct proxy *proxy, struct h5 *h5)
+{
+	proxy->h5 = h5;
+}
+
+struct h5 *proxy_get_h5(struct proxy *proxy)
+{
+	return proxy->h5;
+}
+
 static bool write_packet(int fd, const void *data, size_t size,
 							void *user_data)
 {
@@ -323,6 +338,9 @@ static void host_read_callback(int fd, uint32_t events, void *user_data)
 		util_hexdump('>', proxy->host_buf + proxy->host_len, len,
 						hexdump_print, "H: ");
 
+	if (three_wire)
+		return host_write_3wire_packet(proxy, proxy->host_buf, len);
+
 	proxy->host_len += len;
 
 process_packet:
@@ -475,6 +493,8 @@ process_packet:
 
 	if (emulate_ecc)
 		dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
+	else if (three_wire)
+		dev_write_3wire_packet(proxy, proxy->dev_buf, pktlen);
 	else
 		dev_write_packet(proxy, proxy->dev_buf, pktlen);
 
@@ -515,6 +535,34 @@ static bool setup_proxy(int host_fd, bool host_shutdown,
 	return true;
 }
 
+static bool setup_proxy_h5(int host_fd, bool host_shutdown,
+						int dev_fd, bool dev_shutdown)
+{
+	struct proxy *proxy;
+
+	proxy = new0(struct proxy, 1);
+	if (!proxy)
+		return false;
+
+	proxy->host_fd = host_fd;
+	proxy->host_shutdown = host_shutdown;
+
+	proxy->dev_fd = dev_fd;
+	proxy->dev_shutdown = dev_shutdown;
+
+	mainloop_add_fd(proxy->host_fd, EPOLLIN | EPOLLRDHUP,
+				host_read_callback, proxy, host_read_destroy);
+
+	mainloop_add_fd(proxy->dev_fd, EPOLLIN | EPOLLRDHUP,
+				dev_read_callback, proxy, dev_read_destroy);
+
+	if (three_wire)
+		h5_init(proxy, debug_enabled, host_write_packet,
+							dev_write_packet);
+
+	return true;
+}
+
 static int open_channel(uint16_t index)
 {
 	struct sockaddr_hci addr;
@@ -764,6 +812,7 @@ static void usage(void)
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
 		"\t-P, --pty                   Use PTY\n"
+		"\t-3, --3wire                 Use 3wire protocol\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -778,6 +827,7 @@ static const struct option main_options[] = {
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
 	{ "pty",      no_argument,       NULL, 'P' },
+	{ "3wire",    no_argument,       NULL, '3' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -803,7 +853,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -830,6 +880,9 @@ int main(int argc, char *argv[])
 		case 'P':
 			use_pty = true;
 			break;
+		case '3':
+			three_wire = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -933,7 +986,7 @@ int main(int argc, char *argv[])
 			return EXIT_FAILURE;
 		}
 
-		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+		if (!setup_proxy_h5(master_fd, false, dev_fd, true)) {
 			close(dev_fd);
 			close(master_fd);
 			return EXIT_FAILURE;
diff --git a/tools/h5.c b/tools/h5.c
new file mode 100644
index 0000000..53b6245
--- /dev/null
+++ b/tools/h5.c
@@ -0,0 +1,596 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+
+#include "src/shared/mainloop.h"
+#include "src/shared/queue.h"
+#include "src/shared/timeout.h"
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "h5.h"
+
+#define H5_ACK_TIMEOUT		250
+
+#define HCI_3WIRE_ACK_PKT	0
+#define HCI_3WIRE_LINK_PKT	15
+
+/*
+ * Maximum Three-wire packet:
+ *     4 byte header + max value for 12-bit length + 2 bytes for CRC
+ */
+#define H5_MAX_LEN (4 + 0xfff + 2)
+
+/* Sliding window size */
+#define H5_TX_WIN_MAX		4
+
+#define SLIP_DELIMITER	0xc0
+#define SLIP_ESC	0xdb
+#define SLIP_ESC_DELIM	0xdc
+#define SLIP_ESC_ESC	0xdd
+
+#define H5_RX_ESC	1
+
+#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
+#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
+#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
+#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
+#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
+#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
+
+#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
+#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
+#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
+#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
+#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
+						((hdr)[2] |= (len) >> 4))
+
+static const uint8_t sync_req[] = { 0x01, 0x7e };
+static const uint8_t sync_rsp[] = { 0x02, 0x7d };
+static const uint8_t conf_req[] = { 0x03, 0xfc };
+static const uint8_t wakeup_req[] = { 0x05, 0xfa };
+static const uint8_t woken_req[] = { 0x06, 0xf9 };
+static const uint8_t sleep_req[] = { 0x07, 0x78 };
+
+static bool debug_enabled = false;
+
+struct h5 {
+	size_t rx_pending;
+	uint8_t rx_buf[H5_MAX_LEN];
+	uint8_t *rx_ptr;
+	uint8_t flags;
+
+	uint8_t rx_ack;		/* Received last ack number */
+	uint8_t tx_seq;		/* Next seq number to send */
+	uint8_t tx_ack;		/* Next ack number to send */
+
+	uint8_t tx_win;
+
+	enum {
+		H5_UNINITIALIZED,
+		H5_INITIALIZED,
+		H5_ACTIVE,
+	} state;
+
+	unsigned int timeout_id;
+
+	struct queue *tx_queue_unack;
+	struct queue *tx_queue_unrel;
+	struct queue *tx_queue_rel;
+
+	int (*rx_func)(struct proxy *proxy, uint8_t byte);
+
+	void (*host_write)(struct proxy *proxy, void *buf, uint16_t len);
+	void (*dev_write)(struct proxy *proxy, void *buf, uint16_t len);
+};
+
+struct h5_pkt {
+	uint16_t len;
+	uint8_t data[0];
+};
+
+static void h5_reset_rx(struct h5 *h5);
+
+static void h5_reset_peer(struct h5 *h5)
+{
+	h5->state = H5_UNINITIALIZED;
+
+	timeout_remove(h5->timeout_id);
+	h5->timeout_id = 0;
+
+	h5_reset_rx(h5);
+}
+
+bool h5_init(struct proxy *proxy, bool debug,
+		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
+		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len))
+{
+	struct h5 *h5;
+
+	debug_enabled = debug;
+
+	h5 = malloc(sizeof(*h5));
+	if (!h5)
+		return false;
+
+	h5->tx_win = H5_TX_WIN_MAX;
+	h5_reset_peer(h5);
+
+	h5->tx_queue_unack = queue_new();
+	h5->tx_queue_unrel = queue_new();
+	h5->tx_queue_rel = queue_new();
+
+	h5->host_write = host_w;
+	h5->dev_write = dev_w;
+
+	proxy_set_h5(proxy, h5);
+
+	return true;
+}
+
+void h5_clean(struct h5 *h5)
+{
+	timeout_remove(h5->timeout_id);
+	h5->timeout_id = 0;
+
+	queue_remove_all(h5->tx_queue_unack, NULL, NULL, free);
+	queue_remove_all(h5->tx_queue_unrel, NULL, NULL, free);
+	queue_remove_all(h5->tx_queue_rel, NULL, NULL, free);
+
+	free(h5);
+}
+
+static bool h5_reliable_pkt(uint8_t type)
+{
+	switch (type) {
+	case HCI_ACLDATA_PKT:
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static uint8_t h5_slip_byte(uint8_t *pkt, uint8_t byte)
+{
+	const uint8_t esc_delim[] = { SLIP_ESC, SLIP_ESC_DELIM };
+	const uint8_t esc_esc[] = { SLIP_ESC, SLIP_ESC_ESC };
+
+	switch (byte) {
+	case SLIP_DELIMITER:
+		memcpy(pkt, &esc_delim, sizeof(esc_delim));
+		return sizeof(esc_delim);
+	case SLIP_ESC:
+		memcpy(pkt, &esc_esc, sizeof(esc_esc));
+		return sizeof(esc_esc);
+	default:
+		memcpy(pkt, &byte, sizeof(byte));
+		return sizeof(byte);
+	}
+}
+
+static void h5_print_header(const uint8_t *hdr, const char *str)
+{
+	if (H5_HDR_RELIABLE(hdr))
+		printf("%s REL: seq %u ack %u crc %u type %u len %u\n",
+				str, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
+				H5_HDR_CRC(hdr), H5_HDR_PKT_TYPE(hdr),
+				H5_HDR_LEN(hdr));
+	else
+		printf("%s UNREL: ack %u crc %u type %u len %u\n",
+				str, H5_HDR_ACK(hdr), H5_HDR_CRC(hdr),
+				H5_HDR_PKT_TYPE(hdr), H5_HDR_LEN(hdr));
+}
+
+static void h5_send(struct proxy *proxy, const uint8_t *payload,
+						uint8_t pkt_type, int len)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+	uint8_t *ptr;
+	uint8_t hdr[4];
+	int i;
+
+	memset(hdr, 0, sizeof(hdr));
+
+	H5_SET_ACK(hdr, h5->tx_ack);
+
+	if (h5_reliable_pkt(pkt_type)) {
+		H5_SET_RELIABLE(hdr);
+		H5_SET_SEQ(hdr, h5->tx_seq);
+		h5->tx_seq = (h5->tx_seq + 1) % 8;
+	}
+
+	H5_SET_TYPE(hdr, pkt_type);
+	H5_SET_LEN(hdr, len);
+
+	/* Calculate CRC */
+	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
+
+	if (debug_enabled)
+		h5_print_header(hdr, "TX: <");
+
+	/*
+	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
+	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
+	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
+	 * delimiters at start and end).
+	 */
+	pkt = malloc(sizeof(*pkt) + (len + 6) * 2 + 2);
+	if (!pkt)
+		return;
+
+	ptr = pkt->data;
+
+	*ptr++ = SLIP_DELIMITER;
+
+	for (i = 0; i < 4; i++)
+		ptr += h5_slip_byte(ptr, hdr[i]);
+
+	for (i = 0; i < len; i++)
+		ptr += h5_slip_byte(ptr, payload[i]);
+
+	*ptr++ = SLIP_DELIMITER;
+
+	pkt->len = ptr - pkt->data;
+
+	if (h5_reliable_pkt(pkt_type))
+		queue_push_tail(h5->tx_queue_rel, pkt);
+	else
+		queue_push_tail(h5->tx_queue_unrel, pkt);
+}
+
+static void h5_process_sig_pkt(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t conf_rsp[3] = { 0x04, 0x7b };
+	const uint8_t *hdr = h5->rx_buf;
+	const uint8_t *payload = hdr + 4;
+
+	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
+		return;
+
+	if (H5_HDR_LEN(hdr) < 2)
+		return;
+
+	conf_rsp[2] = h5->tx_win & 0x07;
+
+	if (!memcmp(payload, sync_req, sizeof(sync_req))) {
+		if (h5->state == H5_ACTIVE)
+			h5_reset_peer(h5);
+
+		h5_send(proxy, sync_rsp, H5_HDR_PKT_TYPE(hdr),
+							sizeof(sync_rsp));
+		return;
+	} else if (!memcmp(payload, conf_req, 2)) {
+		h5_send(proxy, conf_rsp, H5_HDR_PKT_TYPE(hdr),
+							sizeof(conf_rsp));
+		return;
+	}
+
+	exit(1);
+}
+
+static void h5_process_data(struct proxy *proxy, uint8_t pkt_type)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t pkt[1 + h5->rx_ptr - 4 - h5->rx_buf];
+
+	pkt[0] = pkt_type;
+	memcpy(&pkt[1], h5->rx_buf + 4, sizeof(pkt) - 1);
+
+	h5->host_write(proxy, pkt, sizeof(pkt));
+}
+
+static void pkt_print(void *data, void *user_data)
+{
+	struct h5_pkt *pkt = data;
+
+	h5_print_header((uint8_t *)pkt + sizeof(*pkt) + 1, "unack pkt");
+}
+
+static void h5_process_unack_queue(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t len = queue_length(h5->tx_queue_unack);
+	uint8_t seq = h5->tx_seq;
+	struct h5_pkt *pkt;
+
+	if (!len)
+		return;
+
+	if (debug_enabled) {
+		printf("%s: rx_ack %u tx_ack %u tx_seq %u\n", __func__,
+					h5->rx_ack, h5->tx_ack, h5->tx_seq);
+
+		printf("%s: unack queue length %u\n", __func__, len);
+
+	}
+
+	/* Remove acked packets from unack queue */
+	while (len > 0) {
+		if (h5->rx_ack == seq)
+			break;
+
+		len--;
+		seq = (seq - 1) & 0x07;
+	}
+
+	if (seq != h5->rx_ack)
+		fprintf(stderr, "Acked wrong packet\n");
+
+	while (len--) {
+		pkt = queue_pop_head(h5->tx_queue_unack);
+		if (pkt) {
+			/* FIXME: print unslipped header */
+			if (debug_enabled)
+				pkt_print(pkt, NULL);
+
+			free(pkt);
+		}
+	}
+}
+
+static void h5_process_complete_pkt(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		h5->tx_ack = (h5->tx_ack + 1) % 8;
+		/* TODO: Set ack req flag */
+	}
+
+	h5->rx_ack = H5_HDR_ACK(hdr);
+
+	h5_process_unack_queue(proxy);
+
+	switch (H5_HDR_PKT_TYPE(hdr)) {
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+	case HCI_ACLDATA_PKT:
+	case HCI_SCODATA_PKT:
+		/* Need to remove three-wire header */
+		h5_process_data(proxy, (H5_HDR_PKT_TYPE(hdr)));
+		break;
+	default:
+		/* Handle three-wire protocol */
+		h5_process_sig_pkt(proxy);
+	}
+
+	h5_reset_rx(h5);
+}
+
+static int h5_crc(struct proxy *proxy, const uint8_t byte)
+{
+	h5_process_complete_pkt(proxy);
+
+	return 0;
+}
+
+static int h5_payload(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		/* TODO: process tx_ack */
+	}
+
+	if (H5_HDR_CRC(hdr)) {
+		h5->rx_func = h5_crc;
+		h5->rx_pending = 2;
+	} else {
+		h5_process_complete_pkt(proxy);
+	}
+
+	return 0;
+}
+
+static int h5_3wire_hdr(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	h5_print_header(hdr, "RX: >");
+
+	/* Add header checks here */
+
+	h5->rx_func = h5_payload;
+	h5->rx_pending = H5_HDR_LEN(hdr);
+
+	return 0;
+}
+
+static int h5_pkt_start(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	if (byte == SLIP_DELIMITER)
+		return 1;
+
+	h5->rx_func = h5_3wire_hdr;
+	h5->rx_pending = 4;
+
+	return 0;
+}
+
+static int h5_delimeter(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	if (byte == SLIP_DELIMITER)
+		h5->rx_func = h5_pkt_start;
+
+	return 1;
+}
+
+static void h5_reset_rx(struct h5 *h5)
+{
+	h5->rx_pending = 0;
+	h5->flags = 0;
+
+	h5->rx_ptr = h5->rx_buf;
+
+	h5->rx_func = h5_delimeter;
+}
+
+static void h5_process_byte(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	/* Mark escaped */
+	if (!(h5->flags & H5_RX_ESC) && byte == SLIP_ESC) {
+		h5->flags |= H5_RX_ESC;
+		return;
+	}
+
+	if (h5->flags & H5_RX_ESC) {
+		h5->flags &= ~H5_RX_ESC;
+
+		switch (byte) {
+		case SLIP_ESC_DELIM:
+			*h5->rx_ptr++ = SLIP_DELIMITER;
+			break;
+		case SLIP_ESC_ESC:
+			*h5->rx_ptr++ = SLIP_ESC;
+			break;
+		default:
+			fprintf(stderr, "Invalid esc byte %x\n", byte);
+			h5_reset_rx(h5);
+			return;
+		}
+	} else {
+		*h5->rx_ptr++ = byte;
+	}
+
+	h5->rx_pending--;
+}
+
+static void h5_process_tx_queue(struct proxy *proxy);
+
+static bool h5_timeout_cb(void *user_data)
+{
+	struct proxy *proxy = user_data;
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+
+	if (debug_enabled)
+		printf("%s: Retransmitting %u packets\n", __func__,
+					queue_length(h5->tx_queue_unack));
+
+	while ((pkt = queue_peek_tail(h5->tx_queue_unack))) {
+		h5->tx_seq = (h5->tx_seq - 1) & 0x07;
+
+		/* Move pkt from unack to rel queue */
+		queue_remove(h5->tx_queue_unack, pkt);
+		queue_push_head(h5->tx_queue_rel, pkt);
+	}
+
+	h5_process_tx_queue(proxy);
+
+	return false;
+}
+
+static void h5_process_tx_queue(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+
+	while ((pkt = queue_pop_head(h5->tx_queue_unrel))) {
+		h5->dev_write(proxy, pkt->data, pkt->len);
+
+		free(pkt);
+	}
+
+	if (queue_length(h5->tx_queue_unack) >= h5->tx_win) {
+		printf("Unacked queue too big\n");
+		return;
+	}
+
+	while ((pkt = queue_pop_head(h5->tx_queue_rel))) {
+		h5->dev_write(proxy, pkt->data, pkt->len);
+
+		queue_push_tail(h5->tx_queue_unack, pkt);
+
+		if (h5->timeout_id > 0)
+			timeout_remove(h5->timeout_id);
+
+		h5->timeout_id = timeout_add(H5_ACK_TIMEOUT, h5_timeout_cb,
+								proxy, NULL);
+	}
+}
+
+void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *ptr = buf;
+
+	while (len > 0) {
+		int processed;
+
+		if (h5->rx_pending > 0) {
+			if (*ptr == SLIP_DELIMITER) {
+				fprintf(stderr, "Too short packet\n");
+				h5_reset_rx(h5);
+				continue;
+			}
+
+			h5_process_byte(proxy, *ptr);
+
+			ptr++;
+			len--;
+			continue;
+		}
+
+		processed = h5->rx_func(proxy, *ptr);
+		if (processed < 0) {
+			fprintf(stderr, "Error processing SLIP packet\n");
+			return;
+		}
+
+		ptr += processed;
+		len -= processed;
+
+		h5_process_tx_queue(proxy);
+	}
+
+	h5_process_tx_queue(proxy);
+}
+
+void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len)
+{
+	uint8_t pkt_type = buf[0];
+
+	/* Get payload out of H4 packet */
+	h5_send(proxy, &buf[1], pkt_type, len - 1);
+
+	h5_process_tx_queue(proxy);
+}
+
+
diff --git a/tools/h5.h b/tools/h5.h
new file mode 100644
index 0000000..9c661fa
--- /dev/null
+++ b/tools/h5.h
@@ -0,0 +1,36 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+struct proxy;
+struct h5;
+
+bool h5_init(struct proxy *proxy, bool debug,
+		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
+		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len));
+void h5_clean(struct h5 *h5);
+
+void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len);
+void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len);
+
+void proxy_set_h5(struct proxy *proxy, struct h5 *h5);
+struct h5 *proxy_get_h5(struct proxy *proxy);
-- 
2.5.0

[PATCHv3 0/5] Three-wire (H5) support in btproxy

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Changes:
	* v3: Fixed bugs, added emulation of received packet loss.
	* v2: Fix comments from review, added h5 debug
	* v1: Split code to h5.[ch], remove global vars, add callbacks
	* RFC: Initial code

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete

Andrei Emeltchenko (5):
  btproxy: Add support for creating pseudoterminal
  btproxy: Add three-wire (h5) protocol initial support
  btproxy: Fix possible string overflow
  btproxy: Simplify Bluetooth proxy
  btproxy: h5: Emulate packet loss

 Makefile.tools  |   3 +-
 tools/btproxy.c | 138 ++++++++++++-
 tools/h5.c      | 605 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tools/h5.h      |  36 ++++
 4 files changed, 777 insertions(+), 5 deletions(-)
 create mode 100644 tools/h5.c
 create mode 100644 tools/h5.h

-- 
2.5.0

[PATCHv3 1/5] btproxy: Add support for creating pseudoterminal

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new virtual Bluetooth serial controller on /dev/pts/X which allows
to test serial attach with btattach and hciattach.
---
 tools/btproxy.c | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 65 insertions(+), 3 deletions(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 43de037..6d78876 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -629,6 +629,41 @@ static int open_unix(const char *path)
 	return fd;
 }
 
+static int open_pty(void)
+{
+	int fd;
+	char *pts;
+
+	fd = open("/dev/ptmx", O_RDWR | O_NOCTTY);
+	if (fd < 0) {
+		perror("Failed to open pseudoterminal master");
+		return -1;
+	}
+
+	if (grantpt(fd) < 0) {
+		perror("Failed to grant access to slave pty");
+		close(fd);
+		return -1;
+	}
+
+	if (unlockpt(fd) < 0) {
+		perror("Failed to unlock slave pty");
+		close(fd);
+		return -1;
+	}
+
+	pts = ptsname(fd);
+	if (!pts) {
+		perror("Failed to get slave pty");
+		close(fd);
+		return -1;
+	}
+
+	printf("New pts created: %s\n", pts);
+
+	return fd;
+}
+
 static int open_tcp(const char *address, unsigned int port)
 {
 	struct sockaddr_in addr;
@@ -728,6 +763,7 @@ static void usage(void)
 		"\t-c, --connect <address>     Connect to server\n"
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
+		"\t-P, --pty                   Use PTY\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -741,6 +777,7 @@ static const struct option main_options[] = {
 	{ "connect",  required_argument, NULL, 'c' },
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
+	{ "pty",      no_argument,       NULL, 'P' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -758,6 +795,7 @@ int main(int argc, char *argv[])
 	const char *unix_path = NULL;
 	unsigned short tcp_port = 0xb1ee;	/* 45550 */
 	bool use_redirect = false;
+	bool use_pty = false;
 	uint8_t type = HCI_BREDR;
 	const char *str;
 	sigset_t mask;
@@ -765,7 +803,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::p:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -789,6 +827,9 @@ int main(int argc, char *argv[])
 			else
 				unix_path = "/tmp/bt-server-bredr";
 			break;
+		case 'P':
+			use_pty = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -828,12 +869,13 @@ int main(int argc, char *argv[])
 		return EXIT_FAILURE;
 	}
 
-	if (unix_path && (server_address || use_redirect)) {
+	if (unix_path && (server_address || use_redirect || use_pty)) {
 		fprintf(stderr, "Invalid to specify TCP and Unix servers\n");
 		return EXIT_FAILURE;
 	}
 
-	if (connect_address && (unix_path || server_address || use_redirect)) {
+	if (connect_address && (unix_path || server_address || use_redirect ||
+								use_pty)) {
 		fprintf(stderr, "Invalid to specify client and server mode\n");
 		return EXIT_FAILURE;
 	}
@@ -876,6 +918,26 @@ int main(int argc, char *argv[])
 			close(host_fd);
 			return EXIT_FAILURE;
 		}
+	} else if (use_pty) {
+		int master_fd, dev_fd;
+
+		printf("Opening pseudoterminal\n");
+
+		master_fd = open_pty();
+		if (master_fd < 0)
+			return EXIT_FAILURE;
+
+		dev_fd = open_channel(hci_index);
+		if (dev_fd < 0) {
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
+
+		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+			close(dev_fd);
+			close(master_fd);
+			return EXIT_FAILURE;
+		}
 	} else {
 		int server_fd;
 
-- 
2.5.0

[PATCHv3 2/5] btproxy: Add three-wire (h5) protocol initial support

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

With H5 support it is possible to create pts and attach to it using
hciattach ot btattach with 3wire protocol. This is useful for testing
and developing three-wire protocol.
Implementation is based on kernel hci_h5.c H5 protocol.

Simple usage:
To open virtual pts run:
$ sudo tools/btproxy -d --pty -3
Opening pseudoterminal
New pts created: /dev/pts/2
Opening user channel for hci0

Now attach to it using hciattach:
$ sudo hciattach -n /dev/pts/2 3wire
Device setup complete
---
 Makefile.tools  |   3 +-
 tools/btproxy.c |  57 +++++-
 tools/h5.c      | 596 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tools/h5.h      |  36 ++++
 4 files changed, 689 insertions(+), 3 deletions(-)
 create mode 100644 tools/h5.c
 create mode 100644 tools/h5.h

diff --git a/Makefile.tools b/Makefile.tools
index e79b53b..7b3974b 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -288,7 +288,8 @@ tools_btattach_LDADD = src/libshared-mainloop.la
 tools_btsnoop_SOURCES = tools/btsnoop.c
 tools_btsnoop_LDADD = src/libshared-mainloop.la
 
-tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h
+tools_btproxy_SOURCES = tools/btproxy.c monitor/bt.h \
+				 tools/h5.c tools/h5.h
 tools_btproxy_LDADD = src/libshared-mainloop.la
 
 tools_btiotest_SOURCES = tools/btiotest.c btio/btio.h btio/btio.c
diff --git a/tools/btproxy.c b/tools/btproxy.c
index 6d78876..1a3eb5e 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -48,6 +48,7 @@
 #include "src/shared/mainloop.h"
 #include "src/shared/ecc.h"
 #include "monitor/bt.h"
+#include "h5.h"
 
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
@@ -64,6 +65,7 @@ static uint16_t hci_index = 0;
 static bool client_active = false;
 static bool debug_enabled = false;
 static bool emulate_ecc = false;
+static bool three_wire = false;
 
 static void hexdump_print(const char *str, void *user_data)
 {
@@ -86,8 +88,21 @@ struct proxy {
 	/* ECC emulation */
 	uint8_t event_mask[8];
 	uint8_t local_sk256[32];
+
+	/* H5 protocol */
+	struct h5 *h5;
 };
 
+void proxy_set_h5(struct proxy *proxy, struct h5 *h5)
+{
+	proxy->h5 = h5;
+}
+
+struct h5 *proxy_get_h5(struct proxy *proxy)
+{
+	return proxy->h5;
+}
+
 static bool write_packet(int fd, const void *data, size_t size,
 							void *user_data)
 {
@@ -323,6 +338,9 @@ static void host_read_callback(int fd, uint32_t events, void *user_data)
 		util_hexdump('>', proxy->host_buf + proxy->host_len, len,
 						hexdump_print, "H: ");
 
+	if (three_wire)
+		return host_write_3wire_packet(proxy, proxy->host_buf, len);
+
 	proxy->host_len += len;
 
 process_packet:
@@ -475,6 +493,8 @@ process_packet:
 
 	if (emulate_ecc)
 		dev_emulate_ecc(proxy, proxy->dev_buf, pktlen);
+	else if (three_wire)
+		dev_write_3wire_packet(proxy, proxy->dev_buf, pktlen);
 	else
 		dev_write_packet(proxy, proxy->dev_buf, pktlen);
 
@@ -515,6 +535,34 @@ static bool setup_proxy(int host_fd, bool host_shutdown,
 	return true;
 }
 
+static bool setup_proxy_h5(int host_fd, bool host_shutdown,
+						int dev_fd, bool dev_shutdown)
+{
+	struct proxy *proxy;
+
+	proxy = new0(struct proxy, 1);
+	if (!proxy)
+		return false;
+
+	proxy->host_fd = host_fd;
+	proxy->host_shutdown = host_shutdown;
+
+	proxy->dev_fd = dev_fd;
+	proxy->dev_shutdown = dev_shutdown;
+
+	mainloop_add_fd(proxy->host_fd, EPOLLIN | EPOLLRDHUP,
+				host_read_callback, proxy, host_read_destroy);
+
+	mainloop_add_fd(proxy->dev_fd, EPOLLIN | EPOLLRDHUP,
+				dev_read_callback, proxy, dev_read_destroy);
+
+	if (three_wire)
+		h5_init(proxy, debug_enabled, host_write_packet,
+							dev_write_packet);
+
+	return true;
+}
+
 static int open_channel(uint16_t index)
 {
 	struct sockaddr_hci addr;
@@ -764,6 +812,7 @@ static void usage(void)
 		"\t-l, --listen [address]      Use TCP server\n"
 		"\t-u, --unix [path]           Use Unix server\n"
 		"\t-P, --pty                   Use PTY\n"
+		"\t-3, --3wire                 Use 3wire protocol\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -778,6 +827,7 @@ static const struct option main_options[] = {
 	{ "listen",   optional_argument, NULL, 'l' },
 	{ "unix",     optional_argument, NULL, 'u' },
 	{ "pty",      no_argument,       NULL, 'P' },
+	{ "3wire",    no_argument,       NULL, '3' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -803,7 +853,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::Pp:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -830,6 +880,9 @@ int main(int argc, char *argv[])
 		case 'P':
 			use_pty = true;
 			break;
+		case '3':
+			three_wire = true;
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
@@ -933,7 +986,7 @@ int main(int argc, char *argv[])
 			return EXIT_FAILURE;
 		}
 
-		if (!setup_proxy(master_fd, false, dev_fd, true)) {
+		if (!setup_proxy_h5(master_fd, false, dev_fd, true)) {
 			close(dev_fd);
 			close(master_fd);
 			return EXIT_FAILURE;
diff --git a/tools/h5.c b/tools/h5.c
new file mode 100644
index 0000000..03a58a0
--- /dev/null
+++ b/tools/h5.c
@@ -0,0 +1,596 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+
+#include "src/shared/mainloop.h"
+#include "src/shared/queue.h"
+#include "src/shared/timeout.h"
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "h5.h"
+
+#define H5_ACK_TIMEOUT		1500
+
+#define HCI_3WIRE_ACK_PKT	0
+#define HCI_3WIRE_LINK_PKT	15
+
+/*
+ * Maximum Three-wire packet:
+ *     4 byte header + max value for 12-bit length + 2 bytes for CRC
+ */
+#define H5_MAX_LEN (4 + 0xfff + 2)
+
+/* Sliding window size */
+#define H5_TX_WIN_MAX		4
+
+#define SLIP_DELIMITER	0xc0
+#define SLIP_ESC	0xdb
+#define SLIP_ESC_DELIM	0xdc
+#define SLIP_ESC_ESC	0xdd
+
+#define H5_RX_ESC	1
+
+#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
+#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
+#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
+#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
+#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
+#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
+
+#define H5_SET_SEQ(hdr, seq)	((hdr)[0] |= (seq))
+#define H5_SET_ACK(hdr, ack)	((hdr)[0] |= (ack) << 3)
+#define H5_SET_RELIABLE(hdr)	((hdr)[0] |= 1 << 7)
+#define H5_SET_TYPE(hdr, type)	((hdr)[1] |= type)
+#define H5_SET_LEN(hdr, len)	(((hdr)[1] |= ((len) & 0x0f) << 4), \
+						((hdr)[2] |= (len) >> 4))
+
+static const uint8_t sync_req[] = { 0x01, 0x7e };
+static const uint8_t sync_rsp[] = { 0x02, 0x7d };
+static const uint8_t conf_req[] = { 0x03, 0xfc };
+static const uint8_t wakeup_req[] = { 0x05, 0xfa };
+static const uint8_t woken_req[] = { 0x06, 0xf9 };
+static const uint8_t sleep_req[] = { 0x07, 0x78 };
+
+static bool debug_enabled = false;
+
+struct h5 {
+	size_t rx_pending;
+	uint8_t rx_buf[H5_MAX_LEN];
+	uint8_t *rx_ptr;
+	uint8_t flags;
+
+	uint8_t rx_ack;		/* Received last ack number */
+	uint8_t tx_seq;		/* Next seq number to send */
+	uint8_t tx_ack;		/* Next ack number to send */
+
+	uint8_t tx_win;
+
+	enum {
+		H5_UNINITIALIZED,
+		H5_INITIALIZED,
+		H5_ACTIVE,
+	} state;
+
+	unsigned int timeout_id;
+
+	struct queue *tx_queue_unack;
+	struct queue *tx_queue_unrel;
+	struct queue *tx_queue_rel;
+
+	int (*rx_func)(struct proxy *proxy, uint8_t byte);
+
+	void (*host_write)(struct proxy *proxy, void *buf, uint16_t len);
+	void (*dev_write)(struct proxy *proxy, void *buf, uint16_t len);
+};
+
+struct h5_pkt {
+	uint16_t len;
+	uint8_t data[0];
+};
+
+static void h5_reset_rx(struct h5 *h5);
+
+static void h5_reset_peer(struct h5 *h5)
+{
+	h5->state = H5_UNINITIALIZED;
+
+	timeout_remove(h5->timeout_id);
+	h5->timeout_id = 0;
+
+	h5_reset_rx(h5);
+}
+
+bool h5_init(struct proxy *proxy, bool debug,
+		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
+		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len))
+{
+	struct h5 *h5;
+
+	debug_enabled = debug;
+
+	h5 = malloc(sizeof(*h5));
+	if (!h5)
+		return false;
+
+	h5->tx_win = H5_TX_WIN_MAX;
+	h5_reset_peer(h5);
+
+	h5->tx_queue_unack = queue_new();
+	h5->tx_queue_unrel = queue_new();
+	h5->tx_queue_rel = queue_new();
+
+	h5->host_write = host_w;
+	h5->dev_write = dev_w;
+
+	proxy_set_h5(proxy, h5);
+
+	return true;
+}
+
+void h5_clean(struct h5 *h5)
+{
+	timeout_remove(h5->timeout_id);
+	h5->timeout_id = 0;
+
+	queue_remove_all(h5->tx_queue_unack, NULL, NULL, free);
+	queue_remove_all(h5->tx_queue_unrel, NULL, NULL, free);
+	queue_remove_all(h5->tx_queue_rel, NULL, NULL, free);
+
+	free(h5);
+}
+
+static bool h5_reliable_pkt(uint8_t type)
+{
+	switch (type) {
+	case HCI_ACLDATA_PKT:
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static uint8_t h5_slip_byte(uint8_t *pkt, uint8_t byte)
+{
+	const uint8_t esc_delim[] = { SLIP_ESC, SLIP_ESC_DELIM };
+	const uint8_t esc_esc[] = { SLIP_ESC, SLIP_ESC_ESC };
+
+	switch (byte) {
+	case SLIP_DELIMITER:
+		memcpy(pkt, &esc_delim, sizeof(esc_delim));
+		return sizeof(esc_delim);
+	case SLIP_ESC:
+		memcpy(pkt, &esc_esc, sizeof(esc_esc));
+		return sizeof(esc_esc);
+	default:
+		memcpy(pkt, &byte, sizeof(byte));
+		return sizeof(byte);
+	}
+}
+
+static void h5_print_header(const uint8_t *hdr, const char *str)
+{
+	if (H5_HDR_RELIABLE(hdr))
+		printf("%s REL: seq %u ack %u crc %u type %u len %u\n",
+				str, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
+				H5_HDR_CRC(hdr), H5_HDR_PKT_TYPE(hdr),
+				H5_HDR_LEN(hdr));
+	else
+		printf("%s UNREL: ack %u crc %u type %u len %u\n",
+				str, H5_HDR_ACK(hdr), H5_HDR_CRC(hdr),
+				H5_HDR_PKT_TYPE(hdr), H5_HDR_LEN(hdr));
+}
+
+static void h5_send(struct proxy *proxy, const uint8_t *payload,
+						uint8_t pkt_type, int len)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+	uint8_t *ptr;
+	uint8_t hdr[4];
+	int i;
+
+	if (debug_enabled)
+		printf("%s: rx_ack %u tx_ack %u tx_seq %u\n",
+			__func__, h5->rx_ack, h5->tx_ack, h5->tx_seq);
+
+	memset(hdr, 0, sizeof(hdr));
+
+	H5_SET_ACK(hdr, h5->tx_ack);
+
+	if (h5_reliable_pkt(pkt_type)) {
+		H5_SET_RELIABLE(hdr);
+		H5_SET_SEQ(hdr, h5->tx_seq);
+		h5->tx_seq = (h5->tx_seq + 1) % 8;
+	}
+
+	H5_SET_TYPE(hdr, pkt_type);
+	H5_SET_LEN(hdr, len);
+
+	/* Calculate CRC */
+	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
+
+	if (debug_enabled)
+		h5_print_header(hdr, "TX: <");
+
+	/*
+	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
+	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
+	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
+	 * delimiters at start and end).
+	 */
+	pkt = malloc(sizeof(*pkt) + (len + 6) * 2 + 2);
+	if (!pkt)
+		return;
+
+	ptr = pkt->data;
+
+	*ptr++ = SLIP_DELIMITER;
+
+	for (i = 0; i < 4; i++)
+		ptr += h5_slip_byte(ptr, hdr[i]);
+
+	for (i = 0; i < len; i++)
+		ptr += h5_slip_byte(ptr, payload[i]);
+
+	*ptr++ = SLIP_DELIMITER;
+
+	pkt->len = ptr - pkt->data;
+
+	if (h5_reliable_pkt(pkt_type))
+		queue_push_tail(h5->tx_queue_rel, pkt);
+	else
+		queue_push_tail(h5->tx_queue_unrel, pkt);
+}
+
+static void h5_process_sig_pkt(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t conf_rsp[3] = { 0x04, 0x7b };
+	const uint8_t *hdr = h5->rx_buf;
+	const uint8_t *payload = hdr + 4;
+
+	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
+		return;
+
+	if (H5_HDR_LEN(hdr) < 2)
+		return;
+
+	conf_rsp[2] = h5->tx_win & 0x07;
+
+	if (!memcmp(payload, sync_req, sizeof(sync_req))) {
+		if (h5->state == H5_ACTIVE)
+			h5_reset_peer(h5);
+
+		h5_send(proxy, sync_rsp, H5_HDR_PKT_TYPE(hdr),
+							sizeof(sync_rsp));
+		return;
+	} else if (!memcmp(payload, conf_req, 2)) {
+		h5_send(proxy, conf_rsp, H5_HDR_PKT_TYPE(hdr),
+							sizeof(conf_rsp));
+		return;
+	}
+
+	exit(1);
+}
+
+static void h5_process_data(struct proxy *proxy, uint8_t pkt_type)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t pkt[1 + h5->rx_ptr - 4 - h5->rx_buf];
+
+	pkt[0] = pkt_type;
+	memcpy(&pkt[1], h5->rx_buf + 4, sizeof(pkt) - 1);
+
+	h5->host_write(proxy, pkt, sizeof(pkt));
+}
+
+static void pkt_print(void *data, void *user_data)
+{
+	struct h5_pkt *pkt = data;
+
+	h5_print_header((uint8_t *)pkt + sizeof(*pkt) + 1, "freeing unack pkt");
+}
+
+static void h5_process_unack_queue(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	uint8_t len = queue_length(h5->tx_queue_unack);
+	uint8_t seq = h5->tx_seq;
+	struct h5_pkt *pkt;
+
+	if (!len)
+		return;
+
+	if (debug_enabled)
+		printf("%s: rx_ack %u tx_ack %u tx_seq %u queue len %u\n",
+			__func__, h5->rx_ack, h5->tx_ack, h5->tx_seq, len);
+
+	/* Remove acked packets from unack queue */
+	while (len > 0) {
+		if (h5->rx_ack == seq)
+			break;
+
+		len--;
+		seq = (seq - 1) & 0x07;
+	}
+
+	if (seq != h5->rx_ack)
+		fprintf(stderr, "Acked wrong packet\n");
+
+	while (len--) {
+		pkt = queue_pop_head(h5->tx_queue_unack);
+		if (pkt) {
+			/* FIXME: print unslipped header */
+			if (debug_enabled)
+				pkt_print(pkt, NULL);
+
+			free(pkt);
+		}
+	}
+}
+
+static void h5_process_tx_queue(struct proxy *proxy);
+
+static void h5_process_complete_pkt(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		h5->tx_ack = (h5->tx_ack + 1) % 8;
+		/* Need to ack reliable packet */
+	}
+
+	h5->rx_ack = H5_HDR_ACK(hdr);
+
+	h5_process_unack_queue(proxy);
+
+	switch (H5_HDR_PKT_TYPE(hdr)) {
+	case HCI_COMMAND_PKT:
+	case HCI_EVENT_PKT:
+	case HCI_ACLDATA_PKT:
+	case HCI_SCODATA_PKT:
+		/* Need to remove three-wire header */
+		h5_process_data(proxy, (H5_HDR_PKT_TYPE(hdr)));
+		break;
+	default:
+		/* Handle three-wire protocol */
+		h5_process_sig_pkt(proxy);
+	}
+
+	h5_process_tx_queue(proxy);
+
+	h5_reset_rx(h5);
+}
+
+static int h5_crc(struct proxy *proxy, const uint8_t byte)
+{
+	h5_process_complete_pkt(proxy);
+
+	return 0;
+}
+
+static int h5_payload(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		/* TODO: process tx_ack */
+	}
+
+	if (H5_HDR_CRC(hdr)) {
+		h5->rx_func = h5_crc;
+		h5->rx_pending = 2;
+	} else {
+		h5_process_complete_pkt(proxy);
+	}
+
+	return 0;
+}
+
+static int h5_3wire_hdr(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *hdr = h5->rx_buf;
+
+	h5_print_header(hdr, "RX: >");
+
+	/* Add header checks here */
+
+	h5->rx_func = h5_payload;
+	h5->rx_pending = H5_HDR_LEN(hdr);
+
+	return 0;
+}
+
+static int h5_pkt_start(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	if (byte == SLIP_DELIMITER)
+		return 1;
+
+	h5->rx_func = h5_3wire_hdr;
+	h5->rx_pending = 4;
+
+	return 0;
+}
+
+static int h5_delimeter(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	if (byte == SLIP_DELIMITER)
+		h5->rx_func = h5_pkt_start;
+
+	return 1;
+}
+
+static void h5_reset_rx(struct h5 *h5)
+{
+	h5->rx_pending = 0;
+	h5->flags = 0;
+
+	h5->rx_ptr = h5->rx_buf;
+
+	h5->rx_func = h5_delimeter;
+}
+
+static void h5_process_byte(struct proxy *proxy, const uint8_t byte)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+
+	/* Mark escaped */
+	if (!(h5->flags & H5_RX_ESC) && byte == SLIP_ESC) {
+		h5->flags |= H5_RX_ESC;
+		return;
+	}
+
+	if (h5->flags & H5_RX_ESC) {
+		h5->flags &= ~H5_RX_ESC;
+
+		switch (byte) {
+		case SLIP_ESC_DELIM:
+			*h5->rx_ptr++ = SLIP_DELIMITER;
+			break;
+		case SLIP_ESC_ESC:
+			*h5->rx_ptr++ = SLIP_ESC;
+			break;
+		default:
+			fprintf(stderr, "Invalid esc byte %x\n", byte);
+			h5_reset_rx(h5);
+			return;
+		}
+	} else {
+		*h5->rx_ptr++ = byte;
+	}
+
+	h5->rx_pending--;
+}
+
+static bool h5_timeout_cb(void *user_data)
+{
+	struct proxy *proxy = user_data;
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+
+	if (debug_enabled)
+		printf("%s: Retransmitting %u packet(s)\n", __func__,
+			queue_length(h5->tx_queue_unack));
+
+	if (!queue_length(h5->tx_queue_unack)) {
+		return false;
+	}
+
+	while ((pkt = queue_peek_tail(h5->tx_queue_unack))) {
+		/* Move pkt from unack to rel queue */
+		queue_remove(h5->tx_queue_unack, pkt);
+		queue_push_head(h5->tx_queue_rel, pkt);
+	}
+
+	h5_process_tx_queue(proxy);
+
+	return false;
+}
+
+static void h5_process_tx_queue(struct proxy *proxy)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	struct h5_pkt *pkt;
+
+	while ((pkt = queue_pop_head(h5->tx_queue_unrel))) {
+		h5->dev_write(proxy, pkt->data, pkt->len);
+
+		free(pkt);
+	}
+
+	if (queue_length(h5->tx_queue_unack) >= h5->tx_win) {
+		printf("Unacked queue too big\n");
+		return;
+	}
+
+	while ((pkt = queue_pop_head(h5->tx_queue_rel))) {
+		h5->dev_write(proxy, pkt->data, pkt->len);
+
+		queue_push_tail(h5->tx_queue_unack, pkt);
+
+		if (h5->timeout_id > 0)
+			timeout_remove(h5->timeout_id);
+
+		h5->timeout_id = timeout_add(H5_ACK_TIMEOUT, h5_timeout_cb,
+								proxy, NULL);
+	}
+}
+
+void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len)
+{
+	struct h5 *h5 = proxy_get_h5(proxy);
+	const uint8_t *ptr = buf;
+
+	while (len > 0) {
+		int processed;
+
+		if (h5->rx_pending > 0) {
+			if (*ptr == SLIP_DELIMITER) {
+				fprintf(stderr, "Too short packet\n");
+				h5_reset_rx(h5);
+				continue;
+			}
+
+			h5_process_byte(proxy, *ptr);
+
+			ptr++;
+			len--;
+			continue;
+		}
+
+		processed = h5->rx_func(proxy, *ptr);
+		if (processed < 0) {
+			fprintf(stderr, "Error processing SLIP packet\n");
+			return;
+		}
+
+		ptr += processed;
+		len -= processed;
+	}
+}
+
+void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len)
+{
+	uint8_t pkt_type = buf[0];
+
+	/* Get payload out of H4 packet */
+	h5_send(proxy, &buf[1], pkt_type, len - 1);
+
+	h5_process_tx_queue(proxy);
+}
+
+
diff --git a/tools/h5.h b/tools/h5.h
new file mode 100644
index 0000000..9c661fa
--- /dev/null
+++ b/tools/h5.h
@@ -0,0 +1,36 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2015  Intel Corporation
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+struct proxy;
+struct h5;
+
+bool h5_init(struct proxy *proxy, bool debug,
+		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
+		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len));
+void h5_clean(struct h5 *h5);
+
+void host_write_3wire_packet(struct proxy *proxy, void *buf, uint16_t len);
+void dev_write_3wire_packet(struct proxy *proxy, uint8_t *buf, uint16_t len);
+
+void proxy_set_h5(struct proxy *proxy, struct h5 *h5);
+struct h5 *proxy_get_h5(struct proxy *proxy);
-- 
2.5.0

[PATCHv3 3/5] btproxy: Fix possible string overflow

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Follow common practice when copying to fixed size buffer.
---
 tools/btproxy.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 1a3eb5e..ee4dadd 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -657,7 +657,7 @@ static int open_unix(const char *path)
 
 	memset(&addr, 0, sizeof(addr));
 	addr.sun_family = AF_UNIX;
-	strcpy(addr.sun_path, path);
+	strncpy(addr.sun_path, path, sizeof(addr.sun_path));
 
 	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 		perror("Failed to bind Unix server socket");
-- 
2.5.0

[PATCHv3 4/5] btproxy: Simplify Bluetooth proxy

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

At the moment every time we use btproxy we have to down Bluetooth
device. This is very unhandy especially when connection to proxy ends
and other connections always fail. Add HCIDEVDOWN ioctl before binding
to user channel.
---
 tools/btproxy.c | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index ee4dadd..02ae53d 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -39,6 +39,7 @@
 #include <termios.h>
 #include <sys/stat.h>
 #include <sys/socket.h>
+#include <sys/ioctl.h>
 #include <sys/un.h>
 
 #include <netdb.h>
@@ -50,6 +51,8 @@
 #include "monitor/bt.h"
 #include "h5.h"
 
+#define HCIDEVDOWN	_IOW('H', 202, int)
+
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
 
@@ -576,6 +579,12 @@ static int open_channel(uint16_t index)
 		return -1;
 	}
 
+	if (ioctl(fd, HCIDEVDOWN, hci_index) < 0) {
+		close(fd);
+		perror("Failed to down hci device");
+		return -1;
+	}
+
 	memset(&addr, 0, sizeof(addr));
 	addr.hci_family = AF_BLUETOOTH;
 	addr.hci_dev = index;
-- 
2.5.0

[PATCHv3 5/5] btproxy: h5: Emulate packet loss

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Emulating packet loss helps testing H:5 Bluetooth drivers. When full
H:5 packet with specified number received we drop it forcing remote
side to re-send packet.
---
 tools/btproxy.c | 12 +++++++++---
 tools/h5.c      | 11 ++++++++++-
 tools/h5.h      |  2 +-
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/tools/btproxy.c b/tools/btproxy.c
index 02ae53d..92bf51f 100644
--- a/tools/btproxy.c
+++ b/tools/btproxy.c
@@ -69,6 +69,7 @@ static bool client_active = false;
 static bool debug_enabled = false;
 static bool emulate_ecc = false;
 static bool three_wire = false;
+static int emulate_lose_num = -1;
 
 static void hexdump_print(const char *str, void *user_data)
 {
@@ -560,8 +561,8 @@ static bool setup_proxy_h5(int host_fd, bool host_shutdown,
 				dev_read_callback, proxy, dev_read_destroy);
 
 	if (three_wire)
-		h5_init(proxy, debug_enabled, host_write_packet,
-							dev_write_packet);
+		h5_init(proxy, debug_enabled, emulate_lose_num,
+					host_write_packet, dev_write_packet);
 
 	return true;
 }
@@ -822,6 +823,7 @@ static void usage(void)
 		"\t-u, --unix [path]           Use Unix server\n"
 		"\t-P, --pty                   Use PTY\n"
 		"\t-3, --3wire                 Use 3wire protocol\n"
+		"\t-L, --lose <num>            Lose specified byte number\n"
 		"\t-p, --port <port>           Use specified TCP port\n"
 		"\t-i, --index <num>           Use specified controller\n"
 		"\t-a, --amp                   Create AMP controller\n"
@@ -837,6 +839,7 @@ static const struct option main_options[] = {
 	{ "unix",     optional_argument, NULL, 'u' },
 	{ "pty",      no_argument,       NULL, 'P' },
 	{ "3wire",    no_argument,       NULL, '3' },
+	{ "lose",     required_argument, NULL, 'L' },
 	{ "port",     required_argument, NULL, 'p' },
 	{ "index",    required_argument, NULL, 'i' },
 	{ "amp",      no_argument,       NULL, 'a' },
@@ -862,7 +865,7 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "rc:l::u::P3p:i:aedvh",
+		opt = getopt_long(argc, argv, "rc:l::u::P3L:p:i:aedvh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
@@ -892,6 +895,9 @@ int main(int argc, char *argv[])
 		case '3':
 			three_wire = true;
 			break;
+		case 'L':
+			emulate_lose_num = atoi(optarg);
+			break;
 		case 'p':
 			tcp_port = atoi(optarg);
 			break;
diff --git a/tools/h5.c b/tools/h5.c
index 03a58a0..7602efe 100644
--- a/tools/h5.c
+++ b/tools/h5.c
@@ -77,6 +77,7 @@ static const uint8_t woken_req[] = { 0x06, 0xf9 };
 static const uint8_t sleep_req[] = { 0x07, 0x78 };
 
 static bool debug_enabled = false;
+static int emulate_lose_num = -1;
 
 struct h5 {
 	size_t rx_pending;
@@ -125,13 +126,14 @@ static void h5_reset_peer(struct h5 *h5)
 	h5_reset_rx(h5);
 }
 
-bool h5_init(struct proxy *proxy, bool debug,
+bool h5_init(struct proxy *proxy, bool debug, int lose,
 		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
 		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len))
 {
 	struct h5 *h5;
 
 	debug_enabled = debug;
+	emulate_lose_num = lose;
 
 	h5 = malloc(sizeof(*h5));
 	if (!h5)
@@ -362,6 +364,13 @@ static void h5_process_complete_pkt(struct proxy *proxy)
 {
 	struct h5 *h5 = proxy_get_h5(proxy);
 	const uint8_t *hdr = h5->rx_buf;
+	static int drop_num;
+
+	if (drop_num++ == emulate_lose_num) {
+		printf("Emulate packet loss, num: %d\n", drop_num);
+		h5_reset_rx(h5);
+		return;
+	}
 
 	if (H5_HDR_RELIABLE(hdr)) {
 		h5->tx_ack = (h5->tx_ack + 1) % 8;
diff --git a/tools/h5.h b/tools/h5.h
index 9c661fa..4f46628 100644
--- a/tools/h5.h
+++ b/tools/h5.h
@@ -24,7 +24,7 @@
 struct proxy;
 struct h5;
 
-bool h5_init(struct proxy *proxy, bool debug,
+bool h5_init(struct proxy *proxy, bool debug, int lose,
 		void (*host_w)(struct proxy *proxy, void *buf, uint16_t len),
 		void (*dev_w)(struct proxy *proxy, void *buf, uint16_t len));
 void h5_clean(struct h5 *h5);
-- 
2.5.0