New upstream version 2018.06.R1

17 files changed, 1969 insertions, 76 deletions

diff --git a/firmware/bluetooth_rxtx/Makefile b/firmware/bluetooth_rxtx/Makefile
index a2e1130..82d5434 100644
--- a/firmware/bluetooth_rxtx/Makefile
+++ b/firmware/bluetooth_rxtx/Makefile
@@ -12,8 +12,12 @@ SRC = $(TARGET).c \
 	ubertooth_cs.c \
 	ubertooth_clock.c \
 	ubertooth_dma.c \
+	le_phy.c \
+	queue.c \
 	cc2400_rangetest.c \
 	ego.c \
+	debug_uart.c \
+	tinyprintf.c \
 	$(LIBS_PATH)/usb_serial.c \
 	$(LIBS_PATH)/serial_fifo.c \
 	$(LIBS_PATH)/LPC17xx_Startup.c \
diff --git a/firmware/bluetooth_rxtx/bluetooth_le.c b/firmware/bluetooth_rxtx/bluetooth_le.c
index ec6843e..21894ce 100644
--- a/firmware/bluetooth_rxtx/bluetooth_le.c
+++ b/firmware/bluetooth_rxtx/bluetooth_le.c
@@ -21,6 +21,8 @@
 
 #include "bluetooth_le.h"
 
+#include <string.h>
+
 extern u8 le_channel_idx;
 extern u8 le_hop_amount;
 
@@ -31,9 +33,11 @@ u16 btle_next_hop(le_state_t *le)
 	return phys;
 }
 
-u8 btle_channel_index(u8 channel) {
-	u8 idx;
-	channel /= 2;
+// calculate channel index from physical channel
+// channel is in range [2402, 2480]
+uint8_t btle_channel_index(uint16_t channel) {
+	uint8_t idx;
+	channel = (channel - 2402) / 2;
 	if (channel == 0)
 		idx = 37;
 	else if (channel < 12)
@@ -176,3 +180,63 @@ u32 btle_crcgen_lut(u32 crc_init, u8 *data, int len) {
 	}
 	return state;
 }
+
+/*
+ * Dewhiten and reverse the bit order of a buffer in place.
+ * Channel is a physical channel in the range [2402, 2480]
+ * TODO convert this to use whitening word
+ */
+void le_dewhiten(uint8_t *data, unsigned size, unsigned channel) {
+	unsigned i, j, bit;
+	unsigned idx = whitening_index[btle_channel_index(channel)];
+
+	for (i = 0; i < size; ++i) {
+		uint8_t out = 0;
+		for (j = 0; j < 8; ++j) {
+			bit = (data[i] >> (7-j)) & 1;
+			bit ^= whitening[idx];
+			idx = (idx + 1) % sizeof(whitening);
+			out |= bit << j;
+		}
+		data[i] = out;
+	}
+}
+
+/*
+ * Parse a channel map and populate the le_channel_remapping_t struct.
+ */
+void le_parse_channel_map(uint8_t *channel_map, le_channel_remapping_t *remapping) {
+	unsigned i, j, byte;
+	unsigned idx = 0;
+
+	memset(remapping, 0, sizeof(*remapping));
+
+	for (i = 0; i < 5; ++i) {
+		byte = channel_map[i];
+		for (j = 0; j < 8; ++j) {
+			if (byte & 1) {
+				remapping->channel_in_use[idx] = 1;
+				remapping->remapping_index[remapping->total_channels] = idx;
+				++remapping->total_channels;
+			} else {
+				remapping->channel_in_use[idx] = 0;
+			}
+
+			byte >>= 1;
+
+			++idx;
+			if (idx == 37)
+				break;
+		}
+	}
+}
+
+/*
+ * Map a channel index to a used index given a remapping struct.
+ */
+uint8_t le_map_channel(uint8_t channel_idx, le_channel_remapping_t *remapping) {
+	if (remapping->channel_in_use[channel_idx])
+		return channel_idx;
+	else
+		return remapping->remapping_index[channel_idx % remapping->total_channels];
+}
diff --git a/firmware/bluetooth_rxtx/bluetooth_le.h b/firmware/bluetooth_rxtx/bluetooth_le.h
index ae00514..1830416 100644
--- a/firmware/bluetooth_rxtx/bluetooth_le.h
+++ b/firmware/bluetooth_rxtx/bluetooth_le.h
@@ -64,6 +64,13 @@ typedef struct _le_state_t {
     u32 last_packet;            // when was the last packet received
 } le_state_t;
 
+// for handling LE channel maps
+typedef struct _le_channel_remapping_t {
+	int         channel_in_use[37];
+	uint8_t     remapping_index[37]; // array of channel indices
+	unsigned    total_channels;
+} le_channel_remapping_t;
+
 static const u8 whitening[] = {
     1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 1, 1, 1,
     1, 0, 0, 1, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0,
@@ -86,11 +93,14 @@ static const u8 hop_interval_lut[] = {
 };
 
 u16 btle_next_hop(le_state_t *le);
-u8 btle_channel_index(u8 channel);
+uint8_t btle_channel_index(uint16_t channel);
 u16 btle_channel_index_to_phys(u8 idx);
 u32 btle_calc_crc(u32 crc_init, u8 *data, int len);
 u32 btle_reverse_crc(u32 crc, u8 *data, int len);
 u32 btle_crcgen_lut(u32 crc_init, u8 *data, int len);
+void le_dewhiten(uint8_t *data, unsigned size, unsigned channel);
+void le_parse_channel_map(uint8_t *channel_map, le_channel_remapping_t *remapping);
+uint8_t le_map_channel(uint8_t channel_idx, le_channel_remapping_t *remapping);
 
 static const u32 whitening_word[40][12] = {
 	{ 0xc3bcb240, 0x5f4a371f, 0x9a9cf685, 0x44c5d6c1, 0xe1de5920, 0xafa51b8f,
diff --git a/firmware/bluetooth_rxtx/bluetooth_rxtx.c b/firmware/bluetooth_rxtx/bluetooth_rxtx.c
index 4ba7d69..6982c01 100644
--- a/firmware/bluetooth_rxtx/bluetooth_rxtx.c
+++ b/firmware/bluetooth_rxtx/bluetooth_rxtx.c
@@ -33,6 +33,7 @@
 #include "bluetooth_le.h"
 #include "cc2400_rangetest.h"
 #include "ego.h"
+#include "debug_uart.h"
 
 #define MIN(x,y)	((x)<(y)?(x):(y))
 #define MAX(x,y)	((x)>(y)?(x):(y))
@@ -49,6 +50,7 @@ volatile uint16_t hop_direct_channel = 0;      // for hopping directly to a chan
 volatile uint16_t hop_timeout = 158;
 volatile uint16_t requested_channel = 0;
 volatile uint16_t le_adv_channel = 2402;
+volatile int      cancel_follow = 0;
 
 /* bulk USB stuff */
 volatile uint8_t  idle_buf_clkn_high = 0;
@@ -542,7 +544,7 @@ static int vendor_request_handler(uint8_t request, uint16_t* request_params, uin
 		hop_mode = HOP_BTLE;
 		requested_mode = MODE_BT_FOLLOW_LE;
 
-		queue_init();
+		usb_queue_init();
 		cs_threshold_calc_and_set(channel);
 		break;
 
@@ -596,7 +598,7 @@ static int vendor_request_handler(uint8_t request, uint16_t* request_params, uin
 		hop_mode = HOP_NONE;
 		requested_mode = MODE_BT_PROMISC_LE;
 
-		queue_init();
+		usb_queue_init();
 		cs_threshold_calc_and_set(channel);
 		break;
 
@@ -654,6 +656,11 @@ static int vendor_request_handler(uint8_t request, uint16_t* request_params, uin
 		le.target_set = 1;
 		break;
 
+	case UBERTOOTH_CANCEL_FOLLOW:
+		// cancel following an active connection
+		cancel_follow = 1;
+		break;
+
 #ifdef TX_ENABLE
 	case UBERTOOTH_JAM_MODE:
 		jam_mode = request_params[0];
@@ -779,7 +786,32 @@ static void msleep(uint32_t millis)
 	}
 }
 
-void DMA_IRQHandler()
+void legacy_DMA_IRQHandler();
+void le_DMA_IRQHandler();
+void DMA_IRQHandler(void) {
+	if (mode == MODE_BT_FOLLOW_LE)
+		le_DMA_IRQHandler();
+	else
+		legacy_DMA_IRQHandler();
+
+	// DMA channel 7: debug UART
+	if (DMACIntStat & (1 << 7)) {
+		// TC -- DMA completed, unset flag so another printf can occur
+		if (DMACIntTCStat & (1 << 7)) {
+			DMACIntTCClear = (1 << 7);
+			debug_dma_active = 0;
+		}
+		// error -- blow up
+		if (DMACIntErrStat & (1 << 7)) {
+			DMACIntErrClr = (1 << 7);
+			// FIXME do something better here
+			USRLED_SET;
+			while (1) { }
+		}
+	}
+}
+
+void legacy_DMA_IRQHandler()
 {
 	if ( mode == MODE_RX_SYMBOLS
 	   || mode == MODE_BT_FOLLOW
@@ -1056,7 +1088,7 @@ void le_transmit(u32 aa, u8 len, u8 *data)
 	}
 
 	// whiten the data and copy it into the txbuf
-	int idx = whitening_index[btle_channel_index(channel-2402)];
+	int idx = whitening_index[btle_channel_index(channel)];
 	for (i = 0; i < len; ++i) {
 		byte = data[i];
 		txbuf[i+4] = 0;
@@ -1241,9 +1273,9 @@ void bt_stream_rx()
 
 	RXLED_CLR;
 
-	queue_init();
+	usb_queue_init();
 	dio_ssp_init();
-	dma_init();
+	dma_init_rx_symbols();
 	dio_ssp_start();
 
 	cc2400_rx();
@@ -1324,10 +1356,6 @@ void bt_stream_rx()
 		rx_err = 0;
 	}
 
-	/* This call is a nop so far. Since bt_rx_stream() starts the
-	 * stream, it makes sense that it would stop it. TODO - how
-	 * should setup/teardown be handled? Should every new mode be
-	 * starting from scratch? */
 	dio_ssp_stop();
 	cs_trigger_disable();
 }
@@ -1502,9 +1530,9 @@ void bt_generic_le(u8 active_mode)
 
 	RXLED_CLR;
 
-	queue_init();
+	usb_queue_init();
 	dio_ssp_init();
-	dma_init();
+	dma_init_rx_symbols();
 	dio_ssp_start();
 	cc2400_rx();
 
@@ -1632,7 +1660,7 @@ void bt_le_sync(u8 active_mode)
 
 	RXLED_CLR;
 
-	queue_init();
+	usb_queue_init();
 	dio_ssp_init();
 	dma_init_le();
 	dio_ssp_start();
@@ -1689,7 +1717,7 @@ void bt_le_sync(u8 active_mode)
 		u8 *p = (u8 *)packet;
 		packet[0] = le.access_address;
 
-		const uint32_t *whit = whitening_word[btle_channel_index(channel-2402)];
+		const uint32_t *whit = whitening_word[btle_channel_index(channel)];
 		for (i = 0; i < 4; i+= 4) {
 			uint32_t v = rxbuf1[i+0] << 24
 					   | rxbuf1[i+1] << 16
@@ -1837,7 +1865,7 @@ cleanup:
  * follows a known AA around */
 int cb_follow_le() {
 	int i, j, k;
-	int idx = whitening_index[btle_channel_index(channel-2402)];
+	int idx = whitening_index[btle_channel_index(channel)];
 
 	u32 access_address = 0;
 	for (i = 0; i < 31; ++i) {
@@ -1998,18 +2026,15 @@ void connection_follow_cb(u8 *packet) {
 	}
 }
 
+void le_phy_main(void);
 void bt_follow_le() {
+	le_phy_main();
+
+	/* old method
 	reset_le();
 	packet_cb = connection_follow_cb;
 	bt_le_sync(MODE_BT_FOLLOW_LE);
-
-	/* old non-sync mode
-	data_cb = cb_follow_le;
-	packet_cb = connection_follow_cb;
-	bt_generic_le(MODE_BT_FOLLOW_LE);
 	*/
-
-	mode = MODE_IDLE;
 }
 
 // issue state change message
@@ -2160,7 +2185,7 @@ int cb_le_promisc(char *unpacked) {
 	};
 
 	for (i = 0; i < 4; ++i) {
-		idx = whitening_index[btle_channel_index(channel-2402)];
+		idx = whitening_index[btle_channel_index(channel)];
 
 		// whiten the desired data
 		for (j = 0; j < (int)sizeof(desired[i]); ++j) {
@@ -2196,7 +2221,7 @@ int cb_le_promisc(char *unpacked) {
 			continue;
 
 		// found a match! unwhiten it and send it home
-		idx = whitening_index[btle_channel_index(channel-2402)];
+		idx = whitening_index[btle_channel_index(channel)];
 		for (j = 0; j < 4+3+3; ++j) {
 			u8 byte = 0;
 			for (k = 0; k < 8; k++) {
@@ -2322,7 +2347,7 @@ void rx_generic_sync(void) {
 	buf[2] = (reg_val >> 8) & 0xFF;
 	buf[3] = reg_val & 0xFF;
 
-	queue_init();
+	usb_queue_init();
 	clkn_start();
 
 	while (!(cc2400_status() & XOSC16M_STABLE));
@@ -2411,7 +2436,7 @@ void specan()
 
 	RXLED_SET;
 
-	queue_init();
+	usb_queue_init();
 	clkn_start();
 
 #ifdef UBERTOOTH_ONE
@@ -2526,6 +2551,10 @@ int main()
 	clkn_init();
 	ubertooth_usb_init(vendor_request_handler);
 	cc2400_idle();
+	dma_poweron();
+
+	debug_uart_init(0);
+	debug_printf("\n\n****UBERTOOTH BOOT****\n%s\n", compile_info);
 
 	while (1) {
 		handle_usb(clkn);
@@ -2553,6 +2582,7 @@ int main()
 					bt_stream_rx();
 					break;
 				case MODE_BT_FOLLOW_LE:
+					mode = MODE_BT_FOLLOW_LE;
 					bt_follow_le();
 					break;
 				case MODE_BT_PROMISC_LE:
diff --git a/firmware/bluetooth_rxtx/debug_uart.c b/firmware/bluetooth_rxtx/debug_uart.c
new file mode 100644
index 0000000..49032bd
--- /dev/null
+++ b/firmware/bluetooth_rxtx/debug_uart.c
@@ -0,0 +1,101 @@
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "ubertooth.h"
+#include "tinyprintf.h"
+
+int debug_dma_active = 0;
+char debug_buffer[256];
+
+void debug_uart_init(int flow_control) {
+	// power on UART1 peripheral
+	PCONP |= PCONP_PCUART1;
+
+	// 8N1, enable access to divisor latches
+	U1LCR = 0b10000011;
+
+	// divisor: 11, fractional: 3/13. final baud: 115,411
+	U1DLL = 11;
+	U1DLM = 0;
+	U1FDR = (3 << 0) | (13 << 4);
+
+	// block access to divisor latches
+	U1LCR &= ~0b10000000;
+
+	// enable auto RTS/CTS
+	if (flow_control)
+		U1MCR = 0b11000000;
+	else
+		U1MCR = 0;
+
+	// enable FIFO and DMA
+	U1FCR = 0b1001;
+
+	// set P0.15 as TXD1, with pullup
+	PINSEL0  = (PINSEL0  & ~(0b11 << 30)) | (0b01 << 30);
+	PINMODE0 = (PINMODE0 & ~(0b11 << 30)) | (0b00 << 30);
+
+	// set P0.16 as RXD1, with pullup
+	PINSEL1  = (PINSEL1  & ~(0b11 <<  0)) | (0b01 <<  0);
+	PINMODE1 = (PINMODE1 & ~(0b11 <<  0)) | (0b00 <<  0);
+
+	if (flow_control) {
+		// set P0.17 as CTS1, no pullup/down
+		PINSEL1  = (PINSEL1  & ~(0b11 <<  2)) | (0b01 <<  2);
+		PINMODE1 = (PINMODE1 & ~(0b11 <<  2)) | (0b10 <<  2);
+
+		// set P0.22 as RTS1, no pullup/down
+		PINSEL1  = (PINSEL1  & ~(0b11 << 12)) | (0b01 << 12);
+		PINMODE1 = (PINMODE1 & ~(0b11 << 12)) | (0b10 << 12);
+	}
+}
+
+// synchronously write a string to debug UART
+// does not start any DMA
+void debug_write(const char *str) {
+	unsigned i;
+
+	for (i = 0; str[i]; ++i) {
+		while ((U1LSR & U1LSR_THRE) == 0)
+			;
+		U1THR = str[i];
+	}
+}
+
+static void debug_send_dma(size_t size) {
+	DMACC7SrcAddr = (uint32_t)debug_buffer;
+	DMACC7DestAddr = (uint32_t)&U1THR;
+	DMACC7LLI = 0;
+	DMACC7Control =
+			(size & 0xfff)   | // transfer size
+			(0b000 << 12)    | // source burst: 1 byte
+			(0b000 << 15)    | // dest burst: 1 byte
+			DMACCxControl_SI | // source increment
+			DMACCxControl_I  ; // terminal count interrupt enable
+	DMACC7Config =
+			(10 << 6)         | // UART1 TX
+			(0b001 << 11)     | // memory to peripheral
+			DMACCxConfig_IE   | // allow error interrupts
+			DMACCxConfig_ITC  ; // allow terminal count interrupts
+
+	DMACC7Config |= 1;
+}
+
+void debug_printf(char *fmt, ...) {
+	va_list ap;
+	void *ret;
+
+	// TODO warn user?
+	if (debug_dma_active)
+		return;
+	debug_dma_active = 1;
+
+	va_start(ap, fmt);
+	tfp_vsnprintf(debug_buffer, sizeof(debug_buffer) - 1, fmt, ap);
+	va_end(ap);
+	debug_buffer[sizeof(debug_buffer) - 1] = 0;
+
+	size_t len = strlen(debug_buffer);
+	debug_send_dma(len);
+}
diff --git a/firmware/bluetooth_rxtx/debug_uart.h b/firmware/bluetooth_rxtx/debug_uart.h
new file mode 100644
index 0000000..a8ccb84
--- /dev/null
+++ b/firmware/bluetooth_rxtx/debug_uart.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright 2017 Mike Ryan
+ *
+ * This file is part of Project Ubertooth and is released under the
+ * terms of the GPL. Refer to COPYING for more information.
+ */
+
+#ifndef __DEBUG_UART_H__
+#define __DEBUG_UART_H__
+
+extern int debug_dma_active;
+
+// initialize debug UART
+void debug_uart_init(int flow_control);
+
+// write a string to UART synchronously without DMA
+// you probably don't want this
+void debug_write(char *fmt);
+
+// printf to debug UART -- this is the one you want!
+void debug_printf(char *fmt, ...);
+
+#endif
diff --git a/firmware/bluetooth_rxtx/le_phy.c b/firmware/bluetooth_rxtx/le_phy.c
new file mode 100644
index 0000000..2f4782d
--- /dev/null
+++ b/firmware/bluetooth_rxtx/le_phy.c
@@ -0,0 +1,869 @@
+/*
+ * Copyright 2017 Mike Ryan
+ *
+ * This file is part of Project Ubertooth and is released under the
+ * terms of the GPL. Refer to COPYING for more information.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "ubertooth.h"
+#include "ubertooth_clock.h"
+#include "ubertooth_dma.h"
+#include "ubertooth_usb.h"
+#include "bluetooth_le.h"
+#include "queue.h"
+
+// current time, from timer1
+#define NOW T1TC
+#define USEC(X) ((X)*10)
+#define MSEC(X) ((X)*10000)
+#define  SEC(X) ((X)*10000000)
+#define PACKET_DURATION(X) (USEC(40 + (X)->size * 8))
+
+#define ADVERTISING_AA (0x8e89bed6)
+
+///////////////////////
+// time constants
+
+// time for the radio to warmup + some timing slack
+#define RX_WARMUP_TIME USEC(300)
+
+// max inter-frame space between packets in a connection event
+#define IFS_TIMEOUT USEC(300)
+
+// observed connection anchor must be within ANCHOR_EPSILON of
+// calculated anchor
+#define ANCHOR_EPSILON USEC(3)
+
+
+//////////////////////
+// global state
+
+extern le_state_t le; // FIXME - refactor this struct
+volatile uint16_t rf_channel;
+uint8_t le_dma_dest[2];
+
+extern volatile uint8_t mode;
+extern volatile uint8_t requested_mode;
+extern volatile uint16_t le_adv_channel;
+extern volatile int cancel_follow;
+
+////////////////////
+// buffers
+
+// packet buffers live in a pool. a minimum of one buffer is always
+// being used, either waiting to receive or actively receiving a packet
+// (current_rxbuf). once a packet is received, it is placed into the
+// packet queue. the main loop pulls packets from this queue and
+// processes them, and then returns the buffers back to the pool by
+// calling buffer_release()
+
+#define LE_BUFFER_POOL_SIZE 4
+typedef struct _le_rx_t {
+	uint8_t data[2 + 255 + 3];  // header + PDU + CRC
+	unsigned size;              // total data length (known after header rx)
+	unsigned pos;               // current input byte offset
+	uint32_t timestamp;         // timestamp taken after first byte rx
+	unsigned channel;           // physical channel
+	uint32_t access_address;    // access address
+	int available;              // 1 if available, 0 in use
+	int8_t rssi_min, rssi_max;  // min and max RSSI observed values
+	int rssi_sum;               // running sum of all RSSI values
+} le_rx_t;
+
+// pool of all buffers
+static le_rx_t le_buffer_pool[LE_BUFFER_POOL_SIZE];
+
+// buffer waiting for or actively receiving packet
+static le_rx_t *current_rxbuf = NULL;
+
+// received packets, waiting to be processed
+queue_t packet_queue;
+
+
+/////////////////////
+// connections
+
+// this system is architected so that following multiple connections may
+// be possible in the future. all connection state lives in an le_conn_t
+// struct. at present only one such structure exists. refer to
+// connection event below for how anchors are handled.
+
+typedef struct _le_conn_t {
+	uint32_t access_address;
+	uint32_t crc_init;
+	uint32_t crc_init_reversed;
+
+	uint8_t  channel_idx;
+	uint8_t  hop_increment;
+	uint32_t conn_interval; // in units of 100 ns
+	uint32_t supervision_timeout; // in units of 100 ns
+
+	uint8_t  win_size;
+	uint32_t win_offset; // in units of 100 ns
+
+	le_channel_remapping_t remapping;
+
+	uint32_t last_anchor;
+	int      anchor_set;
+	uint32_t last_packet_ts; // used to check supervision timeout
+
+	uint16_t conn_event_counter;
+
+	int      channel_map_update_pending;
+	uint16_t channel_map_update_instant;
+	le_channel_remapping_t pending_remapping;
+} le_conn_t;
+le_conn_t conn = { 0, };
+
+// every connection event is tracked using this global le_conn_event_t
+// structure named conn_event. when a packet is observed, anchor is set.
+// the event may close due to receiving two packets, or if a timeout
+// occurs. in both cases, finish_conn_event() is called, which updates
+// the active connection's anchor. opened is set to 1 once the radio is
+// tuned to the data channel for the connection event.
+typedef struct _le_conn_event_t {
+	uint32_t anchor;
+	unsigned num_packets;
+	int opened;
+} le_conn_event_t;
+le_conn_event_t conn_event;
+
+static void reset_conn(void) {
+	memset(&conn, 0, sizeof(conn));
+	conn.access_address = ADVERTISING_AA;
+}
+
+
+//////////////////////
+// code
+
+// pre-declarations for utility stuff
+static void timer1_start(void);
+static void timer1_stop(void);
+static void timer1_set_match(uint32_t match);
+static void timer1_clear_match(void);
+static void timer1_wait_fs_lock(void);
+static void timer1_cancel_fs_lock(void);
+static void blink(int tx, int rx, int usr);
+static void le_dma_init(void);
+static void le_cc2400_strobe_rx(void);
+static void change_channel(void);
+static uint8_t dewhiten_length(unsigned channel, uint8_t data);
+
+// resets the state of all available buffers
+static void buffers_init(void) {
+	int i;
+
+	for (i = 0; i < LE_BUFFER_POOL_SIZE; ++i)
+		le_buffer_pool[i].available = 1;
+}
+
+// clear a buffer for new data
+static void buffer_clear(le_rx_t *buf) {
+	buf->pos = 0;
+	buf->size = 0;
+	memset(buf->data, 0, sizeof(buf->data));
+	buf->rssi_min = INT8_MAX;
+	buf->rssi_max = INT8_MIN;
+	buf->rssi_sum = 0;
+}
+
+// get a packet buffer
+// returns a pointer to a buffer if available
+// returns NULL otherwise
+static le_rx_t *buffer_get(void) {
+	int i;
+
+	for (i = 0; i < LE_BUFFER_POOL_SIZE; ++i) {
+		if (le_buffer_pool[i].available) {
+			le_buffer_pool[i].available = 0;
+			buffer_clear(&le_buffer_pool[i]);
+			return &le_buffer_pool[i];
+		}
+	}
+
+	return NULL;
+}
+
+// release a buffer back to the pool
+static void buffer_release(le_rx_t *buffer) {
+	buffer->available = 1;
+}
+
+// clear a connection event
+static void reset_conn_event(void) {
+	conn_event.num_packets = 0;
+	conn_event.opened = 0;
+}
+
+// finish a connection event
+//
+// 1) update the anchor point (see details below)
+// 2) check if supervision timeout is exceeded
+// 2) setup radio for next packet (data or adv if timeout exceeded)
+//
+// anchor update logic can be summarized thusly:
+// 1) if we received two packets, set the connection anchor to the
+//    observed value
+// 2) if we received one packet, see if it's within ANCHOR_EPISLON
+//    microseconds if the expected anchor time. if so, it's the master
+//    and we can update the anchor
+// 3) if the single packet is a slave or we received zero packets,
+//    update the anchor to the estimated value
+//
+// FIXME this code does not properly handle the case where the initial
+// connection transmit window has no received packets
+static void finish_conn_event(void) {
+	uint32_t last_anchor = 0;
+	int last_anchor_set = 0;
+
+	// two packets -- update anchor
+	if (conn_event.num_packets == 2) {
+		last_anchor = conn_event.anchor;
+		last_anchor_set = 1;
+	}
+
+	// if there's one packet, we need to find out if it was the master
+	else if (conn_event.num_packets == 1 && conn.anchor_set) {
+		// calculate the difference between the estimated and observed anchor
+		uint32_t estimated_anchor = conn.last_anchor + conn.conn_interval;
+		uint32_t delta = estimated_anchor - conn_event.anchor;
+		// see whether the observed anchor is within 3 us of the estimate
+		delta += ANCHOR_EPSILON;
+		if (delta < 2 * ANCHOR_EPSILON) {
+			last_anchor = conn_event.anchor;
+			last_anchor_set = 1;
+		}
+	}
+
+	// if we observed a new anchor, set it
+	if (last_anchor_set) {
+		conn.last_anchor = last_anchor;
+		conn.anchor_set = 1;
+	}
+
+	// without a new anchor, estimate the next anchor
+	else if (conn.anchor_set) {
+		conn.last_anchor += conn.conn_interval;
+	}
+
+	else {
+		// FIXME this is totally broken if we receive the slave's packet first
+		conn.last_anchor = conn_event.anchor;
+		conn.last_packet_ts = NOW; // FIXME gross hack
+	}
+
+	// update last packet for supervision timeout
+	if (conn_event.num_packets > 0) {
+		conn.last_packet_ts = NOW;
+	}
+
+	reset_conn_event();
+
+	// increment connection event counter
+	++conn.conn_event_counter;
+
+	// supervision timeout reached - switch back to advertising
+	if (NOW - conn.last_packet_ts > conn.supervision_timeout) {
+		reset_conn();
+		change_channel();
+	}
+
+	// FIXME - hack to cancel following a connection
+	else if (cancel_follow) {
+		cancel_follow = 0;
+		reset_conn();
+		change_channel();
+	}
+
+	// supervision timeout not reached - hop to next channel
+	else {
+		timer1_set_match(conn.last_anchor + conn.conn_interval - RX_WARMUP_TIME);
+	}
+}
+
+// DMA handler
+// called once per byte. handles all incoming data, but only minimally
+// processes received data. at the end of a packet, it enqueues the
+// received packet, fetches a new buffer, and restarts RX.
+void le_DMA_IRQHandler(void) {
+	unsigned pos;
+	int8_t rssi;
+	uint32_t timestamp = NOW; // sampled early for most accurate measurement
+
+	// channel 0
+	if (DMACIntStat & (1 << 0)) {
+		// terminal count - byte received
+		if (DMACIntTCStat & (1 << 0)) {
+			DMACIntTCClear = (1 << 0);
+
+			// poll RSSI
+			rssi = (int8_t)(cc2400_get(RSSI) >> 8);
+			current_rxbuf->rssi_sum += rssi;
+			if (rssi < current_rxbuf->rssi_min) current_rxbuf->rssi_min = rssi;
+			if (rssi > current_rxbuf->rssi_max) current_rxbuf->rssi_max = rssi;
+
+			// grab byte from DMA buffer
+			pos = current_rxbuf->pos;
+			current_rxbuf->data[pos] = le_dma_dest[pos & 1]; // dirty hack
+			pos += 1;
+			current_rxbuf->pos = pos;
+
+			if (pos == 1) {
+				current_rxbuf->timestamp = timestamp - USEC(8 + 32); // packet starts at preamble
+				current_rxbuf->channel = rf_channel;
+				current_rxbuf->access_address = conn.access_address;
+
+				// data packet received: cancel timeout
+				// new timeout or hop timer will be set at end of packet RX
+				if (btle_channel_index(rf_channel) < 37) {
+					timer1_clear_match();
+				}
+			}
+
+			// get length from header
+			if (pos == 2) {
+				uint8_t length = dewhiten_length(current_rxbuf->channel, current_rxbuf->data[1]);
+				current_rxbuf->size = length + 2 + 3; // two bytes for header and three for CRC
+			}
+
+			// finished packet - state transition
+			if (pos > 2 && pos >= current_rxbuf->size) {
+				// stop the CC2400 before flushing SSP
+				cc2400_strobe(SFSON);
+
+				// stop DMA on this channel and flush SSP
+				DMACC0Config = 0;
+				DMACIntTCClear = (1 << 0); // if we don't clear a second time, data is corrupt
+
+				DIO_SSP_DMACR &= ~SSPDMACR_RXDMAE;
+				while (SSP1SR & SSPSR_RNE) {
+					uint8_t tmp = (uint8_t)DIO_SSP_DR;
+				}
+
+				// TODO error transition on queue_insert
+				queue_insert(&packet_queue, current_rxbuf);
+
+				// track connection events
+				if (btle_channel_index(rf_channel) < 37) {
+					++conn_event.num_packets;
+
+					// first packet: set connection anchor
+					if (conn_event.num_packets == 1) {
+						conn_event.anchor = current_rxbuf->timestamp;
+						timer1_set_match(NOW + IFS_TIMEOUT); // set a timeout for next packet
+					}
+
+					// second packet: close connection event, and set hop timer
+					else if (conn_event.num_packets == 2) {
+						cc2400_strobe(SRFOFF);
+						current_rxbuf = buffer_get();
+						finish_conn_event();
+						return;
+					}
+				}
+
+				// get a new packet
+				// TODO handle error transition
+				current_rxbuf = buffer_get();
+
+				// restart DMA and SSP
+				le_dma_init();
+				dio_ssp_start();
+
+				// wait for FS_LOCK in background
+				timer1_wait_fs_lock();
+			}
+		}
+
+		// error - transition to error state
+		if (DMACIntErrStat & (1 << 0)) {
+			// TODO error state transition
+			DMACIntErrClr = (1 << 0);
+		}
+	}
+}
+
+static void le_dma_init(void) {
+	int i;
+
+	// DMA linked list items
+	typedef struct {
+		uint32_t src;
+		uint32_t dest;
+		uint32_t next_lli;
+		uint32_t control;
+	} dma_lli;
+	static dma_lli le_dma_lli[2];
+
+	for (i = 0; i < 2; ++i) {
+		le_dma_lli[i].src = (uint32_t)&(DIO_SSP_DR);
+		le_dma_lli[i].dest = (uint32_t)&le_dma_dest[i];
+		le_dma_lli[i].next_lli = (uint32_t)&le_dma_lli[1-i]; // two elements pointing back at each other
+		le_dma_lli[i].control = 1 |
+				(0 << 12) |        // source burst size = 1
+				(0 << 15) |        // destination burst size = 1
+				(0 << 18) |        // source width 8 bits
+				(0 << 21) |        // destination width 8 bits
+				DMACCxControl_I;   // terminal count interrupt enable
+	}
+
+	// configure DMA channel 0
+	DMACC0SrcAddr = le_dma_lli[0].src;
+	DMACC0DestAddr = le_dma_lli[0].dest;
+	DMACC0LLI = le_dma_lli[0].next_lli;
+	DMACC0Control = le_dma_lli[0].control;
+	DMACC0Config =
+			DIO_SSP_SRC |
+			(0x2 << 11) |     // peripheral to memory
+			DMACCxConfig_IE | // allow error interrupts
+			DMACCxConfig_ITC; // allow terminal count interrupts
+}
+
+// initalize USB, SSP, and DMA
+static void le_sys_init(void) {
+	usb_queue_init(); // USB FIFO FIXME replace with safer queue
+	dio_ssp_init();   // init SSP and raise !CS (self-routed GPIO)
+	le_dma_init();    // prepare DMA + interrupts
+	dio_ssp_start();  // enable SSP + DMA
+}
+
+// initialize RF and strobe FSON
+static void le_cc2400_init_rf(void) {
+	u16 grmdm, mdmctrl;
+	uint32_t sync = rbit(conn.access_address);
+
+	mdmctrl = 0x0040; // 250 kHz frequency deviation
+	grmdm = 0x44E1; // un-buffered mode, packet w/ sync word detection
+	// 0 10 00 1 001 11 0 00 0 1
+	//   |  |  | |   |  +--------> CRC off
+	//   |  |  | |   +-----------> sync word: 32 MSB bits of SYNC_WORD
+	//   |  |  | +---------------> 1 preamble byte of 01010101
+	//   |  |  +-----------------> packet mode
+	//   |  +--------------------> un-buffered mode
+	//   +-----------------------> sync error bits: 2
+
+	cc2400_set(MANAND,  0x7ffe);
+	cc2400_set(LMTST,   0x2b22);
+
+	cc2400_set(MDMTST0, 0x124b);
+	// 1      2      4b
+	// 00 0 1 0 0 10 01001011
+	//    | | | | |  +---------> AFC_DELTA = ??
+	//    | | | | +------------> AFC settling = 4 pairs (8 bit preamble)
+	//    | | | +--------------> no AFC adjust on packet
+	//    | | +----------------> do not invert data
+	//    | +------------------> TX IF freq 1 0Hz
+	//    +--------------------> PRNG off
+	//
+	// ref: CC2400 datasheet page 67
+	// AFC settling explained page 41/42
+
+	cc2400_set(GRMDM,   grmdm);
+
+	cc2400_set(SYNCL,   sync & 0xffff);
+	cc2400_set(SYNCH,   (sync >> 16) & 0xffff);
+
+	cc2400_set(FSDIV,   rf_channel - 1); // 1 MHz IF
+	cc2400_set(MDMCTRL, mdmctrl);
+
+	// XOSC16M should always be stable, but leave this test anyway
+	while (!(cc2400_status() & XOSC16M_STABLE));
+
+	// wait for FS_LOCK in background
+	cc2400_strobe(SFSON);
+	timer1_wait_fs_lock();
+}
+
+// strobe RX and enable PA
+static void le_cc2400_strobe_rx(void) {
+	cc2400_strobe(SRX);
+#ifdef UBERTOOTH_ONE
+	PAEN_SET;
+	HGM_SET;
+#endif
+}
+
+// change channel and init rx
+static void change_channel(void) {
+	uint8_t channel_idx = 0;
+
+	cc2400_strobe(SRFOFF);
+
+	// stop DMA and flush SSP
+	DIO_SSP_DMACR &= ~SSPDMACR_RXDMAE;
+	while (SSP1SR & SSPSR_RNE) {
+		uint8_t tmp = (uint8_t)DIO_SSP_DR;
+	}
+
+	buffer_clear(current_rxbuf);
+	le_dma_init();
+	dio_ssp_start();
+
+	if (conn.access_address == ADVERTISING_AA) {
+		// FIXME
+		switch (le_adv_channel) {
+			case 2402: channel_idx = 37; break;
+			case 2426: channel_idx = 38; break;
+			case 2480: channel_idx = 39; break;
+			default:   channel_idx = 37; break;
+		}
+	} else {
+		conn.channel_idx = (conn.channel_idx + conn.hop_increment) % 37;
+		channel_idx = le_map_channel(conn.channel_idx, &conn.remapping);
+	}
+
+	rf_channel = btle_channel_index_to_phys(channel_idx);
+	le_cc2400_init_rf();
+}
+
+///////
+// timer stuff
+
+static void timer1_start(void) {
+	T1TCR = TCR_Counter_Reset;
+	T1PR = 4; // 100 ns
+	T1TCR = TCR_Counter_Enable;
+
+	// set up interrupt handler
+	ISER0 = ISER0_ISE_TIMER1;
+}
+
+static void timer1_stop(void) {
+	T1TCR = TCR_Counter_Reset;
+
+	// clear interrupt handler
+	ICER0 = ICER0_ICE_TIMER1;
+}
+
+static void timer1_set_match(uint32_t match) {
+	T1MR0 = match;
+	T1MCR |= TMCR_MR0I;
+}
+
+static void timer1_clear_match(void) {
+	T1MCR &= ~TMCR_MR0I;
+}
+
+static void timer1_wait_fs_lock(void) {
+	T1MR2 = NOW + USEC(3);
+	T1MCR |= TMCR_MR2I;
+}
+
+static void timer1_cancel_fs_lock(void) {
+	T1MCR &= ~TMCR_MR2I;
+}
+
+void TIMER1_IRQHandler(void) {
+	if (T1IR & TIR_MR0_Interrupt) {
+		// ack the interrupt
+		T1IR = TIR_MR0_Interrupt;
+
+		// channel map update, can be interleaved with connection update
+		if (conn.channel_map_update_pending &&
+				conn.conn_event_counter == conn.channel_map_update_instant) {
+			conn.remapping = conn.pending_remapping;
+			conn.channel_map_update_pending = 0;
+		}
+
+		// new connection event: set timeout and change channel
+		if (!conn_event.opened) {
+			conn_event.opened = 1;
+			// timeout is max packet length + warmup time (slack)
+			timer1_set_match(NOW + USEC(2120) + RX_WARMUP_TIME);
+			change_channel();
+		}
+
+		// timeout: close connection event and set timer for next hop
+		else {
+			finish_conn_event();
+		}
+	}
+
+	// LEDs
+	if (T1IR & TIR_MR1_Interrupt) {
+		T1IR = TIR_MR1_Interrupt;
+		T1MCR &= ~TMCR_MR1I;
+
+		TXLED_CLR;
+		RXLED_CLR;
+		USRLED_CLR;
+	}
+
+	// check FS_LOCK
+	if (T1IR & TIR_MR2_Interrupt) {
+		T1IR = TIR_MR2_Interrupt;
+
+		// if FS is locked, strobe RX and clear interrupt
+		if (cc2400_status() & FS_LOCK) {
+			le_cc2400_strobe_rx();
+			T1MCR &= ~TMCR_MR2I;
+		}
+
+		// if FS is not locked, check again in 3 us
+		else {
+			timer1_wait_fs_lock();
+		}
+	}
+}
+
+static void blink(int tx, int rx, int usr) {
+	if (tx)
+		TXLED_SET;
+	if (rx)
+		RXLED_SET;
+	if (usr)
+		USRLED_SET;
+
+	// blink for 10 ms
+	T1MR1 = NOW + MSEC(10);
+	T1MCR |= TMCR_MR1I;
+}
+
+// helper function to dewhiten length from whitened data (only used
+// during DMA)
+static uint8_t dewhiten_length(unsigned channel, uint8_t data) {
+	unsigned int i, bit;
+	int idx = whitening_index[btle_channel_index(channel)];
+	uint8_t out = 0;
+
+	// length is second byte of packet
+	idx = (idx + 8) % sizeof(whitening);
+
+	for (i = 0; i < 8; ++i) {
+		bit = (data >> (7-i)) & 1;
+		bit ^= whitening[idx];
+		idx = (idx + 1) % sizeof(whitening);
+		out |= bit << i;
+	}
+
+	return out;
+}
+
+// enqueue a packet for USB
+// FIXME this is cribbed from existing code, but does not have enough
+// room for larger LE packets
+static int usb_enqueue_le(le_rx_t *packet) {
+	usb_pkt_rx* f = usb_enqueue();
+
+	// fail if queue is full
+	if (f == NULL) {
+		return 0;
+	}
+
+	f->pkt_type = LE_PACKET;
+
+	f->clkn_high = 0;
+	f->clk100ns = packet->timestamp;
+
+	f->channel = (uint8_t)((packet->channel - 2402) & 0xff);
+	f->rssi_avg = packet->rssi_sum / packet->size;
+	f->rssi_min = packet->rssi_min;
+	f->rssi_max = packet->rssi_max;
+	f->rssi_count = 0;
+
+	memcpy(f->data, &packet->access_address, 4);
+	memcpy(f->data+4, packet->data, DMA_SIZE-4);
+
+	f->status = 0;
+
+	return 1;
+}
+
+static unsigned extract_field(le_rx_t *buf, size_t offset, unsigned size) {
+	unsigned i, ret = 0;
+
+	// this could just be replaced by memcpy... right?
+	for (i = 0; i < size; ++i)
+		ret |= buf->data[offset + i] << (i*8);
+
+	return ret;
+}
+
+static void le_connect_handler(le_rx_t *buf) {
+	uint32_t aa, crc_init;
+	uint32_t win_size, max_win_size;
+
+	if (buf->size != 2 + 6 + 6 + 22 + 3)
+		return;
+
+	// FIXME ugly hack
+	if (cancel_follow)
+		cancel_follow = 0;
+
+	conn.access_address     = extract_field(buf, 14, 4);
+	conn.crc_init           = extract_field(buf, 18, 3);
+	conn.crc_init_reversed  = rbit(conn.crc_init);
+	conn.win_size           = extract_field(buf, 21, 1);
+	conn.win_offset         = extract_field(buf, 22, 2);
+	conn.conn_interval      = extract_field(buf, 24, 2);
+	conn.supervision_timeout = extract_field(buf, 28, 2);
+	conn.hop_increment      = extract_field(buf, 35, 1) & 0x1f;
+
+	if (conn.conn_interval < 6 || conn.conn_interval > 3200) {
+		goto err_out;
+	} else {
+		conn.conn_interval *= USEC(1250);
+	}
+
+	// window offset is in range [0, conn_interval]
+	conn.win_offset *= USEC(1250);
+	if (conn.win_offset > conn.conn_interval)
+		goto err_out;
+
+	// win size is in range [1.25 ms, MIN(10 ms, conn_interval - 1.25 ms)]
+	win_size = conn.win_size * USEC(1250);
+	max_win_size = conn.conn_interval - USEC(1250);
+	if (max_win_size > MSEC(10))
+		max_win_size = MSEC(10);
+	if (win_size < USEC(1250) || win_size > max_win_size)
+		goto err_out;
+
+	// The connSupervisionTimeout shall be a multiple of 10 ms in the
+	// range of 100 ms to 32.0 s and it shall be larger than (1 +
+	// connSlaveLatency) * connInterval * 2
+	conn.supervision_timeout *= MSEC(10);
+	if (conn.supervision_timeout < MSEC(100) || conn.supervision_timeout > SEC(32))
+		goto err_out;
+	// TODO handle slave latency
+
+	le_parse_channel_map(&buf->data[30], &conn.remapping);
+	if (conn.remapping.total_channels == 0)
+		goto err_out;
+
+	// cancel RX on advertising channel
+	timer1_cancel_fs_lock();
+
+	reset_conn_event();
+	timer1_set_match(buf->timestamp + PACKET_DURATION(buf) +
+			conn.win_offset + USEC(1250) - RX_WARMUP_TIME);
+	return;
+
+	// error condition: reset conn and return
+err_out:
+	reset_conn();
+}
+
+static void channel_map_update_handler(le_rx_t *buf) {
+	conn.channel_map_update_pending = 1;
+	conn.channel_map_update_instant = extract_field(buf, 8, 2);
+	le_parse_channel_map(&buf->data[3], &conn.pending_remapping);
+}
+
+static void packet_handler(le_rx_t *buf) {
+	// advertising packet
+	if (btle_channel_index(buf->channel) >= 37) {
+		switch (buf->data[0] & 0xf) {
+			// CONNECT_REQ
+			case 0x05:
+				le_connect_handler(buf);
+				break;
+		}
+	}
+
+	// data packet
+	else {
+		// LL control PDU
+		if ((buf->data[0] & 0b11) == 0b11 && buf->data[1] > 0) {
+			switch (buf->data[2]) {
+				// LE_CHANNEL_MAP_REQ -- update channel map
+				case 0x1:
+					if (buf->data[1] == 8)
+						channel_map_update_handler(buf);
+					break;
+			}
+		}
+	}
+
+}
+
+static int filter_match(le_rx_t *buf) {
+	if (!le.target_set)
+		return 1;
+
+	// allow all data channel packets
+	if (btle_channel_index(buf->channel) < 37)
+		return 1;
+
+	switch (buf->data[0] & 0xf) {
+		// ADV_IND, ADV_NONCONN_IND, ADV_SCAN_IND, SCAN_RSP
+		case 0x00:
+		case 0x02:
+		case 0x06:
+		case 0x04:
+			// header + one address
+			if (buf->size < 2 + 6)
+				return 0;
+			return memcmp(&buf->data[2], le.target, 6) == 0;
+			break;
+
+		// ADV_DIRECT_IND, SCAN_REQ, CONNECT_REQ
+		case 0x01:
+		case 0x03:
+		case 0x05:
+			// header + two addresses
+			if (buf->size < 2 + 6 + 6)
+				return 0;
+			return memcmp(&buf->data[2], le.target, 6) == 0 ||
+				   memcmp(&buf->data[8], le.target, 6) == 0;
+			break;
+
+		default:
+			break;
+	}
+
+	return 0;
+}
+
+void le_phy_main(void) {
+	// disable USB interrupts -- we poll them below
+	// n.b., they should not be enabled but let's be careful
+	ICER0 = ICER0_ICE_USB;
+	// disable clkn and timer0
+	clkn_disable();
+
+	buffers_init();
+	queue_init(&packet_queue);
+	timer1_start();
+
+	current_rxbuf = buffer_get();
+	rf_channel = le_adv_channel; // FIXME
+	conn.access_address = ADVERTISING_AA;
+	le_sys_init();
+	le_cc2400_init_rf();
+
+	cancel_follow = 0;
+
+	while (requested_mode == MODE_BT_FOLLOW_LE) {
+		le_rx_t *packet = NULL;
+		if (queue_remove(&packet_queue, (void **)&packet)) {
+			le_dewhiten(packet->data, packet->size, packet->channel);
+
+			if (filter_match(packet)) {
+				blink(0, 1, 0); // RX LED
+				usb_enqueue_le(packet);
+				packet_handler(packet);
+			}
+
+			buffer_release(packet);
+		}
+
+		// polled USB handling
+		handle_usb(0);
+
+		// XXX maybe LED light show?
+	}
+
+	timer1_stop();
+
+	// reset state
+	RXLED_CLR;
+	TXLED_CLR;
+	USRLED_CLR;
+	clkn_init();
+
+	// TODO kill CC2400
+}
diff --git a/firmware/bluetooth_rxtx/queue.c b/firmware/bluetooth_rxtx/queue.c
new file mode 100644
index 0000000..9b77e91
--- /dev/null
+++ b/firmware/bluetooth_rxtx/queue.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2017 Mike Ryan
+ *
+ * This file is part of Project Ubertooth and is released under the
+ * terms of the GPL. Refer to COPYING for more information.
+ */
+
+#include "queue.h"
+
+// queue implementation is based heavily on Koopman's "Better Embedded
+// Systems Software" section 20.3.3.1 pg 209
+
+void queue_init(queue_t *f) {
+	f->head = 0;
+	f->tail = 0;
+}
+
+// insert
+int queue_insert(queue_t *f, void *x) {
+	unsigned newtail;
+	// access next free element
+	newtail = f->tail + 1;
+
+	// wrap around to beginning if needed
+	if (newtail >= FIFOSIZE) { newtail = 0; }
+
+	// if head and tail are equal, queue is full
+	if (newtail == f->head) { return 0; }
+
+	// write data before updating pointer
+	f->data[newtail] = x;
+	f->tail = newtail;
+
+	return 1;
+}
+
+// TODO remove
+int queue_remove(queue_t *f, void **x) {
+	unsigned newhead;
+
+	if (f->head == f->tail) { return 0; }
+
+	newhead = f->head + 1;
+
+	if (newhead >= FIFOSIZE) { newhead = 0; }
+
+	*x = f->data[newhead];
+	f->head = newhead;
+
+	return 1;
+}
diff --git a/firmware/bluetooth_rxtx/queue.h b/firmware/bluetooth_rxtx/queue.h
new file mode 100644
index 0000000..88804eb
--- /dev/null
+++ b/firmware/bluetooth_rxtx/queue.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright 2017 Mike Ryan
+ *
+ * This file is part of Project Ubertooth and is released under the
+ * terms of the GPL. Refer to COPYING for more information.
+ */
+
+#ifndef __QUEUE_H__
+#define __QUEUE_H__
+
+#define FIFOSIZE 10
+
+typedef struct _queue_t {
+	void *data[FIFOSIZE];
+	unsigned head, tail;
+} queue_t;
+
+void queue_init(queue_t *f);
+int queue_insert(queue_t *f, void *x);
+int queue_remove(queue_t *f, void **x);
+
+#endif /* __QUEUE_H__ */
diff --git a/firmware/bluetooth_rxtx/tinyprintf.c b/firmware/bluetooth_rxtx/tinyprintf.c
new file mode 100644
index 0000000..ef5e5c9
--- /dev/null
+++ b/firmware/bluetooth_rxtx/tinyprintf.c
@@ -0,0 +1,524 @@
+/*
+File: tinyprintf.c
+
+Copyright (C) 2004  Kustaa Nyholm
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library 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
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+*/
+
+#include "tinyprintf.h"
+
+
+/*
+ * Configuration
+ */
+
+/* Enable long int support */
+#define PRINTF_LONG_SUPPORT
+
+/* Enable long long int support (implies long int support) */
+#define PRINTF_LONG_LONG_SUPPORT
+
+/* Enable %z (size_t) support */
+#define PRINTF_SIZE_T_SUPPORT
+
+/*
+ * Configuration adjustments
+ */
+#ifdef PRINTF_SIZE_T_SUPPORT
+#include <sys/types.h>
+#endif
+
+#ifdef PRINTF_LONG_LONG_SUPPORT
+# define PRINTF_LONG_SUPPORT
+#endif
+
+/* __SIZEOF_<type>__ defined at least by gcc */
+#ifdef __SIZEOF_POINTER__
+# define SIZEOF_POINTER __SIZEOF_POINTER__
+#endif
+#ifdef __SIZEOF_LONG_LONG__
+# define SIZEOF_LONG_LONG __SIZEOF_LONG_LONG__
+#endif
+#ifdef __SIZEOF_LONG__
+# define SIZEOF_LONG __SIZEOF_LONG__
+#endif
+#ifdef __SIZEOF_INT__
+# define SIZEOF_INT __SIZEOF_INT__
+#endif
+
+#ifdef __GNUC__
+# define _TFP_GCC_NO_INLINE_  __attribute__ ((noinline))
+#else
+# define _TFP_GCC_NO_INLINE_
+#endif
+
+/*
+ * Implementation
+ */
+struct param {
+    char lz:1;          /**<  Leading zeros */
+    char alt:1;         /**<  alternate form */
+    char uc:1;          /**<  Upper case (for base16 only) */
+    char align_left:1;  /**<  0 == align right (default), 1 == align left */
+    unsigned int width; /**<  field width */
+    char sign;          /**<  The sign to display (if any) */
+    unsigned int base;  /**<  number base (e.g.: 8, 10, 16) */
+    char *bf;           /**<  Buffer to output */
+};
+
+
+#ifdef PRINTF_LONG_LONG_SUPPORT
+static void _TFP_GCC_NO_INLINE_ ulli2a(
+    unsigned long long int num, struct param *p)
+{
+    int n = 0;
+    unsigned long long int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void lli2a(long long int num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    ulli2a(num, p);
+}
+#endif
+
+#ifdef PRINTF_LONG_SUPPORT
+static void uli2a(unsigned long int num, struct param *p)
+{
+    int n = 0;
+    unsigned long int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void li2a(long num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    uli2a(num, p);
+}
+#endif
+
+static void ui2a(unsigned int num, struct param *p)
+{
+    int n = 0;
+    unsigned int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void i2a(int num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    ui2a(num, p);
+}
+
+static int a2d(char ch)
+{
+    if (ch >= '0' && ch <= '9')
+        return ch - '0';
+    else if (ch >= 'a' && ch <= 'f')
+        return ch - 'a' + 10;
+    else if (ch >= 'A' && ch <= 'F')
+        return ch - 'A' + 10;
+    else
+        return -1;
+}
+
+static char a2u(char ch, const char **src, int base, unsigned int *nump)
+{
+    const char *p = *src;
+    unsigned int num = 0;
+    int digit;
+    while ((digit = a2d(ch)) >= 0) {
+        if (digit > base)
+            break;
+        num = num * base + digit;
+        ch = *p++;
+    }
+    *src = p;
+    *nump = num;
+    return ch;
+}
+
+static void putchw(void *putp, putcf putf, struct param *p)
+{
+    char ch;
+    int n = p->width;
+    char *bf = p->bf;
+
+    /* Number of filling characters */
+    while (*bf++ && n > 0)
+        n--;
+    if (p->sign)
+        n--;
+    if (p->alt && p->base == 16)
+        n -= 2;
+    else if (p->alt && p->base == 8)
+        n--;
+
+    /* Fill with space to align to the right, before alternate or sign */
+    if (!p->lz && !p->align_left) {
+        while (n-- > 0)
+            putf(putp, ' ');
+    }
+
+    /* print sign */
+    if (p->sign)
+        putf(putp, p->sign);
+
+    /* Alternate */
+    if (p->alt && p->base == 16) {
+        putf(putp, '0');
+        putf(putp, (p->uc ? 'X' : 'x'));
+    } else if (p->alt && p->base == 8) {
+        putf(putp, '0');
+    }
+
+    /* Fill with zeros, after alternate or sign */
+    if (p->lz) {
+        while (n-- > 0)
+            putf(putp, '0');
+    }
+
+    /* Put actual buffer */
+    bf = p->bf;
+    while ((ch = *bf++))
+        putf(putp, ch);
+
+    /* Fill with space to align to the left, after string */
+    if (!p->lz && p->align_left) {
+        while (n-- > 0)
+            putf(putp, ' ');
+    }
+}
+
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
+{
+    struct param p;
+#ifdef PRINTF_LONG_SUPPORT
+    char bf[23];  /* long = 64b on some architectures */
+#else
+    char bf[12];  /* int = 32b on some architectures */
+#endif
+    char ch;
+    p.bf = bf;
+
+    while ((ch = *(fmt++))) {
+        if (ch != '%') {
+			// Ubertooth-specific hack: turn \n into \r\n
+			if (ch == '\n')
+				putf(putp, '\r');
+            putf(putp, ch);
+        } else {
+#ifdef PRINTF_LONG_SUPPORT
+            char lng = 0;  /* 1 for long, 2 for long long */
+#endif
+            /* Init parameter struct */
+            p.lz = 0;
+            p.alt = 0;
+            p.width = 0;
+            p.align_left = 0;
+            p.sign = 0;
+
+            /* Flags */
+            while ((ch = *(fmt++))) {
+                switch (ch) {
+                case '-':
+                    p.align_left = 1;
+                    continue;
+                case '0':
+                    p.lz = 1;
+                    continue;
+                case '#':
+                    p.alt = 1;
+                    continue;
+                default:
+                    break;
+                }
+                break;
+            }
+
+            /* Width */
+            if (ch >= '0' && ch <= '9') {
+                ch = a2u(ch, &fmt, 10, &(p.width));
+            }
+
+            /* We accept 'x.y' format but don't support it completely:
+             * we ignore the 'y' digit => this ignores 0-fill
+             * size and makes it == width (ie. 'x') */
+            if (ch == '.') {
+              p.lz = 1;  /* zero-padding */
+              /* ignore actual 0-fill size: */
+              do {
+                ch = *(fmt++);
+              } while ((ch >= '0') && (ch <= '9'));
+            }
+
+#ifdef PRINTF_SIZE_T_SUPPORT
+# ifdef PRINTF_LONG_SUPPORT
+            if (ch == 'z') {
+                ch = *(fmt++);
+                if (sizeof(size_t) == sizeof(unsigned long int))
+                    lng = 1;
+#  ifdef PRINTF_LONG_LONG_SUPPORT
+                else if (sizeof(size_t) == sizeof(unsigned long long int))
+                    lng = 2;
+#  endif
+            } else
+# endif
+#endif
+
+#ifdef PRINTF_LONG_SUPPORT
+            if (ch == 'l') {
+                ch = *(fmt++);
+                lng = 1;
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (ch == 'l') {
+                  ch = *(fmt++);
+                  lng = 2;
+                }
+#endif
+            }
+#endif
+            switch (ch) {
+            case 0:
+                goto abort;
+            case 'u':
+                p.base = 10;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    ulli2a(va_arg(va, unsigned long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    uli2a(va_arg(va, unsigned long int), &p);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'd':
+            case 'i':
+                p.base = 10;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    lli2a(va_arg(va, long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    li2a(va_arg(va, long int), &p);
+                else
+#endif
+                    i2a(va_arg(va, int), &p);
+                putchw(putp, putf, &p);
+                break;
+#ifdef SIZEOF_POINTER
+            case 'p':
+                p.alt = 1;
+# if defined(SIZEOF_INT) && SIZEOF_POINTER <= SIZEOF_INT
+                lng = 0;
+# elif defined(SIZEOF_LONG) && SIZEOF_POINTER <= SIZEOF_LONG
+                lng = 1;
+# elif defined(SIZEOF_LONG_LONG) && SIZEOF_POINTER <= SIZEOF_LONG_LONG
+                lng = 2;
+# endif
+#endif
+            case 'x':
+            case 'X':
+                p.base = 16;
+                p.uc = (ch == 'X')?1:0;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    ulli2a(va_arg(va, unsigned long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    uli2a(va_arg(va, unsigned long int), &p);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'o':
+                p.base = 8;
+                ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'c':
+                putf(putp, (char)(va_arg(va, int)));
+                break;
+            case 's':
+                p.bf = va_arg(va, char *);
+                putchw(putp, putf, &p);
+                p.bf = bf;
+                break;
+            case '%':
+                putf(putp, ch);
+            default:
+                break;
+            }
+        }
+    }
+ abort:;
+}
+
+#if TINYPRINTF_DEFINE_TFP_PRINTF
+static putcf stdout_putf;
+static void *stdout_putp;
+
+void init_printf(void *putp, putcf putf)
+{
+    stdout_putf = putf;
+    stdout_putp = putp;
+}
+
+void tfp_printf(char *fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    tfp_format(stdout_putp, stdout_putf, fmt, va);
+    va_end(va);
+}
+#endif
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+struct _vsnprintf_putcf_data
+{
+  size_t dest_capacity;
+  char *dest;
+  size_t num_chars;
+};
+
+static void _vsnprintf_putcf(void *p, char c)
+{
+  struct _vsnprintf_putcf_data *data = (struct _vsnprintf_putcf_data*)p;
+  if (data->num_chars < data->dest_capacity)
+    data->dest[data->num_chars] = c;
+  data->num_chars ++;
+}
+
+int tfp_vsnprintf(char *str, size_t size, const char *format, va_list ap)
+{
+  struct _vsnprintf_putcf_data data;
+
+  if (size < 1)
+    return 0;
+
+  data.dest = str;
+  data.dest_capacity = size-1;
+  data.num_chars = 0;
+  tfp_format(&data, _vsnprintf_putcf, format, ap);
+
+  if (data.num_chars < data.dest_capacity)
+    data.dest[data.num_chars] = '\0';
+  else
+    data.dest[data.dest_capacity] = '\0';
+
+  return data.num_chars;
+}
+
+int tfp_snprintf(char *str, size_t size, const char *format, ...)
+{
+  va_list ap;
+  int retval;
+
+  va_start(ap, format);
+  retval = tfp_vsnprintf(str, size, format, ap);
+  va_end(ap);
+  return retval;
+}
+
+struct _vsprintf_putcf_data
+{
+  char *dest;
+  size_t num_chars;
+};
+
+static void _vsprintf_putcf(void *p, char c)
+{
+  struct _vsprintf_putcf_data *data = (struct _vsprintf_putcf_data*)p;
+  data->dest[data->num_chars++] = c;
+}
+
+int tfp_vsprintf(char *str, const char *format, va_list ap)
+{
+  struct _vsprintf_putcf_data data;
+  data.dest = str;
+  data.num_chars = 0;
+  tfp_format(&data, _vsprintf_putcf, format, ap);
+  data.dest[data.num_chars] = '\0';
+  return data.num_chars;
+}
+
+int tfp_sprintf(char *str, const char *format, ...)
+{
+  va_list ap;
+  int retval;
+
+  va_start(ap, format);
+  retval = tfp_vsprintf(str, format, ap);
+  va_end(ap);
+  return retval;
+}
+#endif
diff --git a/firmware/bluetooth_rxtx/tinyprintf.h b/firmware/bluetooth_rxtx/tinyprintf.h
new file mode 100644
index 0000000..a769f4a
--- /dev/null
+++ b/firmware/bluetooth_rxtx/tinyprintf.h
@@ -0,0 +1,186 @@
+/*
+File: tinyprintf.h
+
+Copyright (C) 2004  Kustaa Nyholm
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library 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
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+This library is really just two files: 'tinyprintf.h' and 'tinyprintf.c'.
+
+They provide a simple and small (+400 loc) printf functionality to
+be used in embedded systems.
+
+I've found them so useful in debugging that I do not bother with a
+debugger at all.
+
+They are distributed in source form, so to use them, just compile them
+into your project.
+
+Two printf variants are provided: printf and the 'sprintf' family of
+functions ('snprintf', 'sprintf', 'vsnprintf', 'vsprintf').
+
+The formats supported by this implementation are:
+'c' 'd' 'i' 'o' 'p' 'u' 's' 'x' 'X'.
+
+Zero padding and field width are also supported.
+
+If the library is compiled with 'PRINTF_SUPPORT_LONG' defined, then
+the long specifier is also supported. Note that this will pull in some
+long math routines (pun intended!) and thus make your executable
+noticeably longer. Likewise with 'PRINTF_LONG_LONG_SUPPORT' for the
+long long specifier, and with 'PRINTF_SIZE_T_SUPPORT' for the size_t
+specifier.
+
+The memory footprint of course depends on the target CPU, compiler and
+compiler options, but a rough guesstimate (based on a H8S target) is about
+1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space.
+Not too bad. Your mileage may vary. By hacking the source code you can
+get rid of some hundred bytes, I'm sure, but personally I feel the balance of
+functionality and flexibility versus  code size is close to optimal for
+many embedded systems.
+
+To use the printf, you need to supply your own character output function,
+something like :
+
+void putc ( void* p, char c)
+{
+    while (!SERIAL_PORT_EMPTY) ;
+    SERIAL_PORT_TX_REGISTER = c;
+}
+
+Before you can call printf, you need to initialize it to use your
+character output function with something like:
+
+init_printf(NULL,putc);
+
+Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc',
+the NULL (or any pointer) you pass into the 'init_printf' will eventually be
+passed to your 'putc' routine. This allows you to pass some storage space (or
+anything really) to the character output function, if necessary.
+This is not often needed but it was implemented like that because it made
+implementing the sprintf function so neat (look at the source code).
+
+The code is re-entrant, except for the 'init_printf' function, so it is safe
+to call it from interrupts too, although this may result in mixed output.
+If you rely on re-entrancy, take care that your 'putc' function is re-entrant!
+
+The printf and sprintf functions are actually macros that translate to
+'tfp_printf' and 'tfp_sprintf' when 'TINYPRINTF_OVERRIDE_LIBC' is set
+(default). Setting it to 0 makes it possible to use them along with
+'stdio.h' printf's in a single source file. When
+'TINYPRINTF_OVERRIDE_LIBC' is set, please note that printf/sprintf are
+not function-like macros, so if you have variables or struct members
+with these names, things will explode in your face.  Without variadic
+macros this is the best we can do to wrap these function. If it is a
+problem, just give up the macros and use the functions directly, or
+rename them.
+
+It is also possible to avoid defining tfp_printf and/or tfp_sprintf by
+clearing 'TINYPRINTF_DEFINE_TFP_PRINTF' and/or
+'TINYPRINTF_DEFINE_TFP_SPRINTF' to 0. This allows for example to
+export only tfp_format, which is at the core of all the other
+functions.
+
+For further details see source code.
+
+regs Kusti, 23.10.2004
+*/
+
+#ifndef __TFP_PRINTF__
+#define __TFP_PRINTF__
+
+#include <stdarg.h>
+
+/* Global configuration */
+
+/* Set this to 0 if you do not want to provide tfp_printf */
+#ifndef TINYPRINTF_DEFINE_TFP_PRINTF
+# define TINYPRINTF_DEFINE_TFP_PRINTF 1
+#endif
+
+/* Set this to 0 if you do not want to provide
+   tfp_sprintf/snprintf/vsprintf/vsnprintf */
+#ifndef TINYPRINTF_DEFINE_TFP_SPRINTF
+# define TINYPRINTF_DEFINE_TFP_SPRINTF 1
+#endif
+
+/* Set this to 0 if you do not want tfp_printf and
+   tfp_{vsn,sn,vs,s}printf to be also available as
+   printf/{vsn,sn,vs,s}printf */
+#ifndef TINYPRINTF_OVERRIDE_LIBC
+# define TINYPRINTF_OVERRIDE_LIBC 1
+#endif
+
+/* Optional external types dependencies */
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+# include <sys/types.h>  /* size_t */
+#endif
+
+/* Declarations */
+
+#ifdef __GNUC__
+# define _TFP_SPECIFY_PRINTF_FMT(fmt_idx,arg1_idx) \
+    __attribute__((format (printf, fmt_idx, arg1_idx)))
+#else
+# define _TFP_SPECIFY_PRINTF_FMT(fmt_idx,arg1_idx)
+#endif
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+typedef void (*putcf) (void *, char);
+
+/*
+   'tfp_format' really is the central function for all tinyprintf. For
+   each output character after formatting, the 'putf' callback is
+   called with 2 args:
+     - an arbitrary void* 'putp' param defined by the user and
+       passed unmodified from 'tfp_format',
+     - the character.
+   The 'tfp_printf' and 'tfp_sprintf' functions simply define their own
+   callback and pass to it the right 'putp' it is expecting.
+*/
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va);
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+int tfp_vsnprintf(char *str, size_t size, const char *fmt, va_list ap);
+int tfp_snprintf(char *str, size_t size, const char *fmt, ...) \
+     _TFP_SPECIFY_PRINTF_FMT(3, 4);
+int tfp_vsprintf(char *str, const char *fmt, va_list ap);
+int tfp_sprintf(char *str, const char *fmt, ...) \
+    _TFP_SPECIFY_PRINTF_FMT(2, 3);
+# if TINYPRINTF_OVERRIDE_LIBC
+#  define vsnprintf tfp_vsnprintf
+#  define snprintf tfp_snprintf
+#  define vsprintf tfp_vsprintf
+#  define sprintf tfp_sprintf
+# endif
+#endif
+
+#if TINYPRINTF_DEFINE_TFP_PRINTF
+void init_printf(void *putp, putcf putf);
+void tfp_printf(char *fmt, ...) _TFP_SPECIFY_PRINTF_FMT(1, 2);
+# if TINYPRINTF_OVERRIDE_LIBC
+#  define printf tfp_printf
+# endif
+#endif
+
+#ifdef  __cplusplus
+}
+#endif
+
+#endif
diff --git a/firmware/bluetooth_rxtx/ubertooth_clock.c b/firmware/bluetooth_rxtx/ubertooth_clock.c
index 1da3d92..e7b0c01 100644
--- a/firmware/bluetooth_rxtx/ubertooth_clock.c
+++ b/firmware/bluetooth_rxtx/ubertooth_clock.c
@@ -74,3 +74,9 @@ void clkn_init()
 	T0MCR = TMCR_MR0R | TMCR_MR0I;
 	ISER0 = ISER0_ISE_TIMER0;
 }
+
+// totally disable clkn and timer0
+void clkn_disable(void) {
+	clkn_stop();
+	ICER0 = ICER0_ICE_TIMER0;
+}
diff --git a/firmware/bluetooth_rxtx/ubertooth_clock.h b/firmware/bluetooth_rxtx/ubertooth_clock.h
index 09ee426..833899b 100644
--- a/firmware/bluetooth_rxtx/ubertooth_clock.h
+++ b/firmware/bluetooth_rxtx/ubertooth_clock.h
@@ -51,5 +51,6 @@ volatile uint32_t clkn_next_drift_fix;
 void clkn_stop();
 void clkn_start();
 void clkn_init();
+void clkn_disable();
 
 #endif
diff --git a/firmware/bluetooth_rxtx/ubertooth_dma.c b/firmware/bluetooth_rxtx/ubertooth_dma.c
index ea79d05..16ae2c7 100644
--- a/firmware/bluetooth_rxtx/ubertooth_dma.c
+++ b/firmware/bluetooth_rxtx/ubertooth_dma.c
@@ -35,20 +35,12 @@ dma_lli rx_dma_lli2;
 dma_lli le_dma_lli[11]; // 11 x 4 bytes
 
 
-static void dma_enable(void)
-{
-	/* enable DMA */
-	DMACC0Config |= DMACCxConfig_E;
-	ISER0 = ISER0_ISE_DMA;
-}
-
-static void dma_disable(void)
-{
-	// disable DMA engine:
-	// refer to UM10360 LPC17xx User Manual Ch 31 Sec 31.6.1, PDF page 607
+void dma_poweron(void) {
+	// refer to UM10360 LPC17xx User Manual Ch 31 Sec 31.6.1, PDF page 616
+	PCONP |= PCONP_PCGPDMA;
 
-	// disable DMA interrupts
-	ICER0 = ICER0_ICE_DMA;
+	// enable DMA interrupts
+	ISER0 = ISER0_ISE_DMA;
 
 	// disable active channels
 	DMACC0Config = 0;
@@ -62,25 +54,29 @@ static void dma_disable(void)
 	DMACIntTCClear = 0xFF;
 	DMACIntErrClr = 0xFF;
 
+	/* enable DMA globally */
+	DMACConfig = DMACConfig_E;
+	while (!(DMACConfig & DMACConfig_E));
+}
+
+void dma_poweroff(void) {
 	// Disable the DMA controller by writing 0 to the DMA Enable bit in the DMACConfig
 	// register.
 	DMACConfig &= ~DMACConfig_E;
 	while (DMACConfig & DMACConfig_E);
 
-	/* reset interrupt counters */
-	rx_tc = 0;
-	rx_err = 0;
+	ICER0 = ICER0_ICE_DMA;
 
-	active_rxbuf = &rxbuf1[0];
-	idle_rxbuf = &rxbuf2[0];
+	PCONP &= ~PCONP_PCGPDMA;
 }
 
-void dma_init()
-{
-	/* power up GPDMA controller */
-	PCONP |= PCONP_PCGPDMA;
+void dma_clear_interrupts(unsigned channel) {
+	DMACIntTCClear = 1 << channel;
+	DMACIntErrClr  = 1 << channel;
+}
 
-	dma_disable();
+void dma_init_rx_symbols(void) {
+	dma_clear_interrupts(0);
 
 	/* DMA linked lists */
 	rx_dma_lli1.src = (uint32_t)&(DIO_SSP_DR);
@@ -105,11 +101,7 @@ void dma_init()
 			DMACCxControl_DI | /* destination increment */
 			DMACCxControl_I;   /* terminal count interrupt enable */
 
-	/* enable DMA globally */
-	DMACConfig = DMACConfig_E;
-	while (!(DMACConfig & DMACConfig_E));
-
-	/* configure DMA channel 1 */
+	/* configure DMA channel 0 */
 	DMACC0SrcAddr = rx_dma_lli1.src;
 	DMACC0DestAddr = rx_dma_lli1.dest;
 	DMACC0LLI = rx_dma_lli1.next_lli;
@@ -118,20 +110,21 @@ void dma_init()
 	               | (0x2 << 11)       /* peripheral to memory */
 	               | DMACCxConfig_IE   /* allow error interrupts */
 	               | DMACCxConfig_ITC; /* allow terminal count interrupts */
-}
 
-void dma_init_le()
-{
-	int i;
+	rx_tc = 0;
+	rx_err = 0;
 
-	/* power up GPDMA controller */
-	PCONP |= PCONP_PCGPDMA;
+	active_rxbuf = &rxbuf1[0];
+	idle_rxbuf = &rxbuf2[0];
 
-	dma_disable();
+	// enable channel
+	DMACC0Config |= 1;
+}
 
-	/* enable DMA globally */
-	DMACConfig = DMACConfig_E;
-	while (!(DMACConfig & DMACConfig_E));
+void dma_init_le(void) {
+	int i;
+
+	dma_clear_interrupts(0);
 
 	for (i = 0; i < 11; ++i) {
 		le_dma_lli[i].src = (uint32_t)&(DIO_SSP_DR);
@@ -158,7 +151,6 @@ void dma_init_le()
 			DMACCxConfig_ITC; /* allow terminal count interrupts */
 }
 
-
 void dio_ssp_start()
 {
 	/* make sure the (active low) slave select signal is not active */
@@ -168,7 +160,8 @@ void dio_ssp_start()
 	DIO_SSP_DMACR |= SSPDMACR_RXDMAE;
 	DIO_SSP_CR1 |= SSPCR1_SSE;
 
-	dma_enable();
+	// enable channel
+	DMACC0Config |= 1;
 
 	/* activate slave select pin */
 	DIO_SSEL_CLR;
@@ -179,9 +172,16 @@ void dio_ssp_stop()
 	// disable CC2400's output (active low)
 	DIO_SSEL_SET;
 
-	// disable DMA on SSP; disable SSP
+	// disable DMA on SSP; disable SSP ; disable DMA channel
 	DIO_SSP_DMACR &= ~SSPDMACR_RXDMAE;
 	DIO_SSP_CR1 &= ~SSPCR1_SSE;
+	DMACC0Config = 0;
+	dma_clear_interrupts(0);
 
-	dma_disable();
+	// TODO flush SSP
+	/*
+	while (SSP1SR & SSPSR_RNE) {
+		u8 tmp = (u8)DIO_SSP_DR;
+	}
+	*/
 }
diff --git a/firmware/bluetooth_rxtx/ubertooth_dma.h b/firmware/bluetooth_rxtx/ubertooth_dma.h
index 3a4c76b..dfb969d 100644
--- a/firmware/bluetooth_rxtx/ubertooth_dma.h
+++ b/firmware/bluetooth_rxtx/ubertooth_dma.h
@@ -39,7 +39,9 @@ volatile uint8_t* volatile idle_rxbuf;
 volatile uint32_t rx_tc;
 volatile uint32_t rx_err;
 
-void dma_init();
+void dma_poweron();
+void dma_poweroff();
+void dma_init_rx_symbols();
 void dma_init_le();
 void dio_ssp_start();
 void dio_ssp_stop();
diff --git a/firmware/bluetooth_rxtx/ubertooth_usb.c b/firmware/bluetooth_rxtx/ubertooth_usb.c
index 0aef55e..939aaa0 100644
--- a/firmware/bluetooth_rxtx/ubertooth_usb.c
+++ b/firmware/bluetooth_rxtx/ubertooth_usb.c
@@ -91,7 +91,7 @@ static u8 abDescriptors[] = {
 	MAX_PACKET_SIZE0,  		// bMaxPacketSize
 	LE_WORD(ID_VENDOR),		// idVendor
 	LE_WORD(ID_PRODUCT),		// idProduct
-	LE_WORD(0x0102),		// bcdDevice
+	LE_WORD(0x0103),		// bcdDevice
 	0x01,              		// iManufacturer
 	0x02,              		// iProduct
 	0x03,              		// iSerialNumber
@@ -272,7 +272,7 @@ usb_pkt_rx fifo[128];
 volatile u32 head = 0;
 volatile u32 tail = 0;
 
-void queue_init(void)
+void usb_queue_init(void)
 {
 	head = 0;
 	tail = 0;
diff --git a/firmware/bluetooth_rxtx/ubertooth_usb.h b/firmware/bluetooth_rxtx/ubertooth_usb.h
index 6d34b24..00c1082 100644
--- a/firmware/bluetooth_rxtx/ubertooth_usb.h
+++ b/firmware/bluetooth_rxtx/ubertooth_usb.h
@@ -56,7 +56,7 @@
 typedef int (VendorRequestHandler)(u8 request, u16 *request_params, u8 *data, int *data_len);
 
 int ubertooth_usb_init(VendorRequestHandler *vendor_req_handler);
-void queue_init();
+void usb_queue_init();
 usb_pkt_rx *usb_enqueue();
 usb_pkt_rx *dequeue();
 void handle_usb(u32 clkn);