New upstream version 2018.08.R1

4 files changed, 141 insertions, 16 deletions

diff --git a/firmware/bluetooth_rxtx/bluetooth_le.h b/firmware/bluetooth_rxtx/bluetooth_le.h
index 1830416..ac1ec4d 100644
--- a/firmware/bluetooth_rxtx/bluetooth_le.h
+++ b/firmware/bluetooth_rxtx/bluetooth_le.h
@@ -59,7 +59,10 @@ typedef struct _le_state_t {
     u8 win_size_update;         // the new window size
     u16 win_offset_update;      // the new window offset
 
+    int do_follow;              // 1 if follow connections, 0 to only log advertising packets
+
     u8 target[6];               // target MAC for connection following (byte order reversed)
+    u8 target_mask[6];          // mask for MAC connection following (byte order reversed)
     int target_set;             // whether a target has been set (default: false)
     u32 last_packet;            // when was the last packet received
 } le_state_t;
diff --git a/firmware/bluetooth_rxtx/bluetooth_rxtx.c b/firmware/bluetooth_rxtx/bluetooth_rxtx.c
index 6982c01..3bfbec3 100644
--- a/firmware/bluetooth_rxtx/bluetooth_rxtx.c
+++ b/firmware/bluetooth_rxtx/bluetooth_rxtx.c
@@ -88,6 +88,7 @@ le_state_t le = {
 
 	.link_state = LINK_INACTIVE,
 	.conn_epoch = 0,
+	.do_follow = 1,
 	.target_set = 0,
 	.last_packet = 0,
 };
@@ -538,6 +539,7 @@ static int vendor_request_handler(uint8_t request, uint16_t* request_params, uin
 		break;
 
 	case UBERTOOTH_BTLE_SNIFFING:
+		le.do_follow = request_params[0];
 		*data_len = 0;
 
 		do_hop = 0;
@@ -647,13 +649,41 @@ static int vendor_request_handler(uint8_t request, uint16_t* request_params, uin
 	case UBERTOOTH_BTLE_SET_TARGET:
 		// Addresses appear in packets in reverse-octet order.
 		// Store the target address in reverse order so that we can do a simple memcmp later
-		le.target[0] = data[5];
-		le.target[1] = data[4];
-		le.target[2] = data[3];
-		le.target[3] = data[2];
-		le.target[4] = data[1];
-		le.target[5] = data[0];
-		le.target_set = 1;
+		if (data[6] > 48) {
+			return 1; // invalid mask
+		}
+		else if (data[6] == 0) {
+			le.target_set = 0;
+			memset(le.target, 0, 6);
+			memset(le.target_mask, 0, 6);
+		} else {
+			unsigned last;
+
+			for (i = 0; i < 6; ++i)
+				le.target[i] = data[5-i];
+
+			// compute mask
+			memset(le.target_mask, 0, 6);
+			for (i = 5; data[6] > 8; --i, data[6] -= 8) {
+				le.target_mask[i] = 0xff;
+			}
+			last = i;
+
+			if (data[6] > 0) {
+				uint8_t final_byte = 0;
+				for (i = 0; i < data[6]; ++i) {
+					final_byte >>= 1;
+					final_byte |= 0b10000000;
+				}
+				le.target_mask[last] = final_byte;
+			}
+
+			// in case the user specifies a bad mask
+			for (i = 0; i < 5; ++i)
+				le.target[i] &= le.target_mask[i];
+
+			le.target_set = 1;
+		}
 		break;
 
 	case UBERTOOTH_CANCEL_FOLLOW:
diff --git a/firmware/bluetooth_rxtx/le_phy.c b/firmware/bluetooth_rxtx/le_phy.c
index 2f4782d..caafbde 100644
--- a/firmware/bluetooth_rxtx/le_phy.c
+++ b/firmware/bluetooth_rxtx/le_phy.c
@@ -112,6 +112,11 @@ typedef struct _le_conn_t {
 
 	uint16_t conn_event_counter;
 
+	int      conn_update_pending;
+	uint32_t conn_update_pending_interval;
+	uint32_t conn_update_pending_supervision_timeout;
+	uint16_t conn_update_instant;
+
 	int      channel_map_update_pending;
 	uint16_t channel_map_update_instant;
 	le_channel_remapping_t pending_remapping;
@@ -202,8 +207,9 @@ static void reset_conn_event(void) {
 // 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)
+// 2) increment connection event counter
+// 3) check if supervision timeout is exceeded
+// 4) 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
@@ -558,11 +564,45 @@ static void timer1_cancel_fs_lock(void) {
 }
 
 void TIMER1_IRQHandler(void) {
+	// MR0: connection events
 	if (T1IR & TIR_MR0_Interrupt) {
 		// ack the interrupt
 		T1IR = TIR_MR0_Interrupt;
 
-		// channel map update, can be interleaved with connection update
+		// connection update procedure
+		if (conn.conn_update_pending &&
+				conn.conn_event_counter == conn.conn_update_instant) {
+
+			// on the first past through, handle the transmit window
+			// offset. if there's no offset, skip down to else block
+			if (!conn_event.opened && conn.win_offset > 0) {
+				timer1_set_match(conn.last_anchor + conn.conn_interval +
+						conn.win_offset - RX_WARMUP_TIME);
+				conn_event.opened = 1;
+			}
+
+			// after the transmit window offset, or if there is no
+			// transmit window, set a packet timeout and change the
+			// channel
+			else { // conn_event.opened || conn.win_offset == 0
+				conn_event.opened = 1;
+
+				// this is like a new connection, so set all values
+				// accordingly
+				conn.anchor_set = 0;
+				conn.conn_interval = conn.conn_update_pending_interval;
+				conn.supervision_timeout = conn.conn_update_pending_supervision_timeout;
+				conn.conn_update_pending = 0;
+
+				// timeout after conn window + max packet length
+				timer1_set_match(conn.last_anchor + conn.conn_interval +
+						conn.win_offset + conn.win_size + USEC(2120));
+				change_channel();
+			}
+			return;
+		}
+
+		// channel map update
 		if (conn.channel_map_update_pending &&
 				conn.conn_event_counter == conn.channel_map_update_instant) {
 			conn.remapping = conn.pending_remapping;
@@ -577,9 +617,25 @@ void TIMER1_IRQHandler(void) {
 			change_channel();
 		}
 
-		// timeout: close connection event and set timer for next hop
+		// regular connection event, plus timeout from connection updates
+		// FIXME connection update timeouts and initial connection
+		// timeouts need to be handled differently: they should have a
+		// full window until the packets from the new connection are
+		// captured and a new anchor is set.
 		else {
-			finish_conn_event();
+			// 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();
+			}
 		}
 	}
 
@@ -687,6 +743,9 @@ static void le_connect_handler(le_rx_t *buf) {
 	uint32_t aa, crc_init;
 	uint32_t win_size, max_win_size;
 
+	if (!le.do_follow)
+		return;
+
 	if (buf->size != 2 + 6 + 6 + 22 + 3)
 		return;
 
@@ -747,6 +806,24 @@ err_out:
 	reset_conn();
 }
 
+static void connection_update_handler(le_rx_t *buf) {
+	conn.win_size            = extract_field(buf, 3, 1);
+	conn.win_offset          = extract_field(buf, 4, 2);
+	conn.conn_update_pending_interval = extract_field(buf, 6, 2);
+	conn.conn_update_pending_supervision_timeout = extract_field(buf, 10, 2);
+	conn.conn_update_instant = extract_field(buf, 12, 2);
+
+	// TODO check for invalid values. XXX what do we even do in that
+	// case? we will probably drop the connection, but at least it's on
+	// our own terms and not some impossibly long supervision timeout.
+	conn.win_size   *= USEC(1250);
+	conn.win_offset *= USEC(1250);
+	conn.conn_update_pending_interval *= USEC(1250);
+	conn.conn_update_pending_supervision_timeout *= MSEC(10);
+
+	conn.conn_update_pending = 1;
+}
+
 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);
@@ -759,6 +836,7 @@ static void packet_handler(le_rx_t *buf) {
 		switch (buf->data[0] & 0xf) {
 			// CONNECT_REQ
 			case 0x05:
+				// TODO validate length
 				le_connect_handler(buf);
 				break;
 		}
@@ -769,6 +847,12 @@ static void packet_handler(le_rx_t *buf) {
 		// LL control PDU
 		if ((buf->data[0] & 0b11) == 0b11 && buf->data[1] > 0) {
 			switch (buf->data[2]) {
+				// LE_CONNECTION_UPDATE_REQ -- update connection parameters
+				case 0x0:
+					if (buf->data[1] == 12)
+						connection_update_handler(buf);
+					break;
+
 				// LE_CHANNEL_MAP_REQ -- update channel map
 				case 0x1:
 					if (buf->data[1] == 8)
@@ -777,7 +861,15 @@ static void packet_handler(le_rx_t *buf) {
 			}
 		}
 	}
+}
 
+// compare a BD addr against target with mask
+static int bd_addr_cmp(uint8_t *bd_addr) {
+	unsigned i;
+	for (i = 0; i < 6; ++i)
+		if ((bd_addr[i] & le.target_mask[i]) != le.target[i])
+			return 0;
+	return 1;
 }
 
 static int filter_match(le_rx_t *buf) {
@@ -797,7 +889,7 @@ static int filter_match(le_rx_t *buf) {
 			// header + one address
 			if (buf->size < 2 + 6)
 				return 0;
-			return memcmp(&buf->data[2], le.target, 6) == 0;
+			return bd_addr_cmp(&buf->data[2]);
 			break;
 
 		// ADV_DIRECT_IND, SCAN_REQ, CONNECT_REQ
@@ -807,8 +899,8 @@ static int filter_match(le_rx_t *buf) {
 			// 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;
+			return bd_addr_cmp(&buf->data[2]) ||
+				   bd_addr_cmp(&buf->data[8]);
 			break;
 
 		default:
diff --git a/firmware/bluetooth_rxtx/ubertooth_usb.c b/firmware/bluetooth_rxtx/ubertooth_usb.c
index 939aaa0..cc27a18 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(0x0103),		// bcdDevice
+	LE_WORD(0x0105),		// bcdDevice
 	0x01,              		// iManufacturer
 	0x02,              		// iProduct
 	0x03,              		// iSerialNumber