add ubertooth-afh host tool for afh channel map detection

16 files changed, 487 insertions, 84 deletions

diff --git a/firmware/bluetooth_rxtx/bluetooth.c b/firmware/bluetooth_rxtx/bluetooth.c
index 915dd32..44a58d3 100644
--- a/firmware/bluetooth_rxtx/bluetooth.c
+++ b/firmware/bluetooth_rxtx/bluetooth.c
@@ -27,7 +27,6 @@ u16 d1;
 /* frequency register bank */
 u8 bank[CHANNELS];
 u8 afh_bank[CHANNELS];
-u8 used_channels;
 
 /* count the number of 1 bits in a uint64_t */
 static uint8_t count_bits(uint64_t n)
diff --git a/firmware/bluetooth_rxtx/bluetooth.h b/firmware/bluetooth_rxtx/bluetooth.h
index ab3a4fb..8222a1f 100644
--- a/firmware/bluetooth_rxtx/bluetooth.h
+++ b/firmware/bluetooth_rxtx/bluetooth.h
@@ -32,6 +32,7 @@ bdaddr target;
 u64 syncword;
 u8 afh_enabled;
 u8 afh_map[10];
+u8 used_channels;
 
 /* Barker distance/correct gains us very little when sniffing a known AC
 static const u8 ao_barker_distance[] = {
diff --git a/firmware/bluetooth_rxtx/bluetooth_rxtx.c b/firmware/bluetooth_rxtx/bluetooth_rxtx.c
index 703b670..f4802c9 100644
--- a/firmware/bluetooth_rxtx/bluetooth_rxtx.c
+++ b/firmware/bluetooth_rxtx/bluetooth_rxtx.c
@@ -46,7 +46,7 @@ const char compile_info[] =
  * slot.
  */
 
-volatile u32 clkn;                       // clkn 3200 Hz counter
+volatile u32 clkn, last_hop;             // clkn 3200 Hz counter
 #define CLK100NS (3125*(clkn & 0xfffff) + T0TC)
 #define LE_BASECLK (12500)               // 1.25 ms in units of 100ns
 
@@ -55,6 +55,7 @@ volatile u32 clkn;                       // clkn 3200 Hz counter
 #define HOP_BLUETOOTH 2
 #define HOP_BTLE      3
 #define HOP_DIRECT    4
+#define HOP_AFH       5
 
 #define CS_THRESHOLD_DEFAULT (uint8_t)(-120)
 #define CS_HOLD_TIME  2                     // min pkts to send on trig (>=1)
@@ -77,6 +78,8 @@ volatile u16 idle_buf_channel = 0;
 volatile u16 active_buf_channel = 0;
 u8 slave_mac_address[6] = { 0, };
 
+volatile u16 hop_timeout = 158;
+
 /* DMA buffers */
 u8 rxbuf1[DMA_SIZE];
 u8 rxbuf2[DMA_SIZE];
@@ -685,6 +688,21 @@ static int vendor_request_handler(u8 request, u16 *request_params, u8 *data, int
 		requested_mode = MODE_BT_FOLLOW;
 		break;
 
+	case UBERTOOTH_AFH:
+		hop_mode = HOP_AFH;
+		requested_mode = MODE_AFH;
+
+		for(i=0; i < 10; i++) {
+			afh_map[i] = 0;
+		}
+		used_channels = 0;
+		afh_enabled = 1;
+		break;
+
+	case UBERTOOTH_HOP:
+		do_hop = 1;
+		break;
+
 	case UBERTOOTH_SET_CLOCK:
 		clock = data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
 		clkn = clock;
@@ -902,6 +920,11 @@ void TIMER0_IRQHandler()
 				}
 			}
 		}
+		else if (hop_mode == HOP_AFH) {
+			if( (last_hop + hop_timeout) == next ) {
+				do_hop = 1;
+			}
+		}
 
 		/* Keepalive trigger fires at 3200/2^9 = 6.25 Hz */
 		if ((next & 0x1ff) == 0)
@@ -1408,6 +1431,7 @@ void le_jam(void) {
 void hop(void)
 {
 	do_hop = 0;
+	last_hop = clkn;
 
 	// No hopping, if channel is set correctly, do nothing
 	if (hop_mode == HOP_NONE) {
@@ -1415,11 +1439,27 @@ void hop(void)
 			return;
 	}
 
-	// Slow sweep (100 hops/sec)
+	/* Slow sweep (100 hops/sec)
+	 * only hop to currently used channels if AFH is enabled
+	 */
 	else if (hop_mode == HOP_SWEEP) {
-		channel += 32;
-		if (channel > 2480)
-			channel -= 79;
+		TXLED_SET;
+		do {
+			channel += 32;
+			if (channel > 2480)
+				channel -= 79;
+		} while ( used_channels != 0 && afh_enabled && !( afh_map[(channel-2402)/8] & 0x1<<((channel-2402)%8) ) );
+	}
+
+	/* AFH detection
+	 * only hop to currently unused channesl
+	 */
+	else if (hop_mode == HOP_AFH) {
+		do {
+			channel += 32;
+			if (channel > 2480)
+				channel -= 79;
+		} while( used_channels != 79 && (afh_map[(channel-2402)/8] & 0x1<<((channel-2402)%8)) );
 	}
 
 	else if (hop_mode == HOP_BLUETOOTH) {
@@ -2596,6 +2636,10 @@ int main()
 					wait(1);
 					reset();
 					break;
+				case MODE_AFH:
+					mode = MODE_AFH;
+					bt_stream_rx();
+					break;
 				case MODE_RX_SYMBOLS:
 					mode = MODE_RX_SYMBOLS;
 					bt_stream_rx();
diff --git a/firmware/common/ubertooth.h b/firmware/common/ubertooth.h
index b10c9e1..3ed10e6 100644
--- a/firmware/common/ubertooth.h
+++ b/firmware/common/ubertooth.h
@@ -46,6 +46,7 @@ enum operating_modes {
 	MODE_RESET         = 11,
 	MODE_BT_SLAVE_LE   = 12,
 	MODE_EGO           = 13,
+	MODE_AFH           = 14,
 };
 
 /* hardware identification number */
diff --git a/host/libubertooth/src/ubertooth.c b/host/libubertooth/src/ubertooth.c
index 53855ef..c4c9611 100644
--- a/host/libubertooth/src/ubertooth.c
+++ b/host/libubertooth/src/ubertooth.c
@@ -71,7 +71,7 @@ void stop_transfers(int sig __attribute__((unused))) {
 		timeout_dev->stop_ubertooth = 1;
 }
 
-void set_timeout(ubertooth_t* ut, int seconds) {
+void ubertooth_set_timeout(ubertooth_t* ut, int seconds) {
 	/* Upon SIGALRM, call stop_transfers() */
 	if (signal(SIGALRM, stop_transfers) == SIG_ERR) {
 		perror("Unable to catch SIGALRM");
@@ -255,6 +255,7 @@ int ubertooth_bulk_receive(ubertooth_t* ut, rx_callback cb, void* cb_args)
 	int i;
 	usb_pkt_rx* rx;
 
+
 	if (ut->usb_really_full) {
 		/* process each received block */
 		for (i = 0; i < PKTS_PER_XFER; i++) {
@@ -272,17 +273,17 @@ int ubertooth_bulk_receive(ubertooth_t* ut, rx_callback cb, void* cb_args)
 		ut->usb_really_full = 0;
 		fflush(stderr);
 		return 0;
-	} else {
+	}
+
+	else {
 		return -1;
 	}
 }
 
-int stream_rx_usb(ubertooth_t* ut, rx_callback cb, void* cb_args)
+static int stream_rx_usb(ubertooth_t* ut, rx_callback cb, void* cb_args)
 {
-	int r;
-
 	// init USB transfer
-	r = ubertooth_bulk_init(ut);
+	int r = ubertooth_bulk_init(ut);
 	if (r < 0)
 		return r;
 
@@ -301,7 +302,7 @@ int stream_rx_usb(ubertooth_t* ut, rx_callback cb, void* cb_args)
 }
 
 /* file should be in full USB packet format (ubertooth-dump -f) */
-int stream_rx_file(FILE* fp, rx_callback cb, void* cb_args)
+static int stream_rx_file(FILE* fp, rx_callback cb, void* cb_args)
 {
 	uint8_t buf[BUFFER_SIZE];
 	size_t nitems;
@@ -425,10 +426,8 @@ static uint64_t now_ns_from_clk100ns( ubertooth_t* ut, const usb_pkt_rx* rx )
  */
 static void cb_br_rx(ubertooth_t* ut, void* args)
 {
-	btbb_packet *pkt = NULL;
-	btbb_piconet *pn = (btbb_piconet *)args;
-	char syms[NUM_BANKS * BANK_LEN];
-	int i;
+	btbb_packet* pkt = NULL;
+	btbb_piconet* pn = (btbb_piconet *)args;
 	int8_t signal_level;
 	int8_t noise_level;
 	int8_t snr;
@@ -438,7 +437,7 @@ static void cb_br_rx(ubertooth_t* ut, void* args)
 	uint8_t uap = UAP_ANY;
 
 	/* Do analysis based on oldest packet */
-	usb_pkt_rx* rx = ringbuffer_bottom_usb(ut->packets);
+	usb_pkt_rx* rx = ringbuffer_top_usb(ut->packets);
 
 	/* Sanity check */
 	if (rx->channel > (NUM_BREDR_CHANNELS-1))
@@ -449,9 +448,6 @@ static void cb_br_rx(ubertooth_t* ut, void* args)
 	determine_signal_and_noise( rx, &signal_level, &noise_level );
 	snr = signal_level - noise_level;
 
-	/* WC4: use vm circbuf if target allows. This gets rid of this
-	 * wrapped copy step. */
-
 	/* Look for packets with specified LAP, if given. Otherwise
 	 * search for any packet.  Also determine if UAP is known. */
 	if (pn) {
@@ -461,24 +457,18 @@ static void cb_br_rx(ubertooth_t* ut, void* args)
 
 	/* Pass packet-pointer-pointer so that
 	 * packet can be created in libbtbb. */
-	offset = btbb_find_ac(ringbuffer_bottom_bt(ut->packets), BANK_LEN - 64, lap, max_ac_errors, &pkt);
+	offset = btbb_find_ac(ringbuffer_top_bt(ut->packets), BANK_LEN - 64, lap, max_ac_errors, &pkt);
 	if (offset < 0)
 		goto out;
 
 	btbb_packet_set_modulation(pkt, BTBB_MOD_GFSK);
 	btbb_packet_set_transport(pkt, BTBB_TRANSPORT_ANY);
 
-	/* Copy out remaining banks of symbols for full analysis. */
-	for (i = 0; i < NUM_BANKS; i++)
-		memcpy(syms + i * BANK_LEN,
-		       ringbuffer_get_bt(ut->packets, i),
-		       BANK_LEN);
-
 	/* Once offset is known for a valid packet, copy in symbols
 	 * and other rx data. CLKN here is the 312.5us CLK27-0. The
 	 * btbb library can shift it be CLK1 if needed. */
 	clkn = (rx->clkn_high << 20) + (le32toh(rx->clk100ns) + offset*10) / 3125;
-	btbb_packet_set_data(pkt, syms + offset, NUM_BANKS * BANK_LEN - offset,
+	btbb_packet_set_data(pkt, ringbuffer_top_bt(ut->packets) + offset, NUM_BANKS * BANK_LEN - offset,
 	                     rx->channel, clkn);
 
 	/* When reading from file, caller will read
@@ -491,11 +481,9 @@ static void cb_br_rx(ubertooth_t* ut, void* args)
 	 * file. There could be duplicate data in the dump if more
 	 * than one LAP is found within the span of NUM_BANKS. */
 	if (dumpfile) {
-		for(i = 0; i < NUM_BANKS; i++) {
-			uint32_t systime_be = htobe32(systime);
-			fwrite(&systime_be, sizeof(systime_be), 1, dumpfile);
-			fwrite(ringbuffer_get_usb(ut->packets, i), sizeof(usb_pkt_rx), 1, dumpfile);
-		}
+		uint32_t systime_be = htobe32(systime);
+		fwrite(&systime_be, sizeof(systime_be), 1, dumpfile);
+		fwrite(ringbuffer_top_usb(ut->packets), sizeof(usb_pkt_rx), 1, dumpfile);
 		fflush(dumpfile);
 	}
 
@@ -524,9 +512,8 @@ static void cb_br_rx(ubertooth_t* ut, void* args)
 		                          lap, uap, pkt);
 	}
 
-	i = btbb_process_packet(pkt, pn);
-	if(i < 0) {
-		ut->follow_pn = pn;
+	int r = btbb_process_packet(pkt, pn);
+	if(r < 0) {
 		ut->stop_ubertooth = 1;
 	}
 
@@ -535,6 +522,116 @@ out:
 		btbb_packet_unref(pkt);
 }
 
+void cb_afh_initial(ubertooth_t* ut, void* args)
+{
+	btbb_piconet* pn = (btbb_piconet*)args;
+	btbb_packet* pkt = NULL;
+	uint8_t channel;
+
+
+	if( btbb_find_ac(ringbuffer_top_bt(ut->packets), BANK_LEN - 64, btbb_piconet_get_lap(pn), max_ac_errors, &pkt) < 0 )
+		goto out;
+
+	/* detect AFH map
+	 * set current channel as used channel and send updated AFH
+	 * map to ubertooth */
+	channel = ringbuffer_top_usb(ut->packets)->channel;
+	if(btbb_piconet_set_channel_seen(pn, channel)) {
+
+		/* Don't allow single unused channels */
+		if (!btbb_piconet_get_channel_seen(pn, channel+1) &&
+		    btbb_piconet_get_channel_seen(pn, channel+2))
+		{
+			printf("activating additional channel %d\n", channel+1);
+		    btbb_piconet_set_channel_seen(pn, channel+1);
+		}
+		if (!btbb_piconet_get_channel_seen(pn, channel-1) &&
+		    btbb_piconet_get_channel_seen(pn, channel-2))
+		{
+			printf("activating additional channel %d\n", channel-1);
+			btbb_piconet_set_channel_seen(pn, channel-1);
+		}
+
+		cmd_set_afh_map(ut->devh, btbb_piconet_get_afh_map(pn));
+		btbb_print_afh_map(pn);
+	}
+	cmd_hop(ut->devh);
+
+out:
+	if (pkt)
+		btbb_packet_unref(pkt);
+}
+
+void cb_afh_monitor(ubertooth_t* ut, void* args)
+{
+	btbb_piconet* pn = (btbb_piconet*)args;
+	btbb_packet* pkt = NULL;
+	uint8_t channel;
+
+	static unsigned long last_seen[79] = {0};
+	static unsigned long counter = 0;
+
+
+	if( btbb_find_ac(ringbuffer_top_bt(ut->packets), BANK_LEN - 64, btbb_piconet_get_lap(pn), max_ac_errors, &pkt) < 0 )
+		goto out;
+
+	counter++;
+	channel = ringbuffer_top_usb(ut->packets)->channel;
+	last_seen[channel]=counter;
+
+	if(btbb_piconet_set_channel_seen(pn, channel)) {
+		printf("+ channel %2d is used now\n", channel);
+		btbb_print_afh_map(pn);
+	// } else {
+	// 	printf("channel %d is already used\n", channel);
+	}
+
+	for(int i=0; i<79; i++) {
+		if((counter - last_seen[i] >= counter_max)) {
+			if(btbb_piconet_clear_channel_seen(pn, i)) {
+				printf("- channel %2d is not used any more\n", i);
+				btbb_print_afh_map(pn);
+			}
+		}
+	}
+	cmd_hop(ut->devh);
+
+out:
+	if (pkt)
+		btbb_packet_unref(pkt);
+}
+
+void cb_afh_r(ubertooth_t* ut, void* args)
+{
+	btbb_piconet* pn = (btbb_piconet*)args;
+	btbb_packet* pkt = NULL;
+	uint8_t channel;
+
+	static unsigned long last_seen[79] = {0};
+	static unsigned long counter = 0;
+
+	if( btbb_find_ac(ringbuffer_top_bt(ut->packets), BANK_LEN - 64, btbb_piconet_get_lap(pn), max_ac_errors, &pkt) < 0 )
+		goto out;
+
+
+	counter++;
+	channel = ringbuffer_top_usb(ut->packets)->channel;
+	last_seen[channel]=counter;
+
+	btbb_piconet_set_channel_seen(pn, channel);
+
+	for(int i=0; i<79; i++) {
+		if((counter - last_seen[i] >= counter_max)) {
+			btbb_piconet_clear_channel_seen(pn, i);
+		}
+	}
+	cmd_hop(ut->devh);
+
+out:
+	if (pkt)
+		btbb_packet_unref(pkt);
+}
+
 /* Receive and process packets. For now, returning from
  * stream_rx_usb() means that UAP and clocks have been found, and that
  * hopping should be started. A more flexible framework would be
@@ -546,9 +643,9 @@ void rx_live(ubertooth_t* ut, btbb_piconet* pn, int timeout)
 		return;
 
 	if (timeout)
-		set_timeout(ut, timeout);
+		ubertooth_set_timeout(ut, timeout);
 
-	if (ut->follow_pn)
+	if (pn != NULL && btbb_piconet_get_flag(pn, BTBB_CLK27_VALID))
 		cmd_set_clock(ut->devh, 0);
 	else {
 		stream_rx_usb(ut, cb_br_rx, pn);
@@ -558,13 +655,86 @@ void rx_live(ubertooth_t* ut, btbb_piconet* pn, int timeout)
 	/* Used when follow_pn is preset OR set by stream_rx_usb above
 	 * i.e. This cannot be rolled in to the above if...else
 	 */
-	if (ut->follow_pn) {
+	if (pn != NULL && btbb_piconet_get_flag(pn, BTBB_CLK27_VALID)) {
 		ut->stop_ubertooth = 0;
 		ut->usb_really_full = 0;
 		// cmd_stop(ut->devh);
-		cmd_set_bdaddr(ut->devh, btbb_piconet_get_bdaddr(ut->follow_pn));
-		cmd_start_hopping(ut->devh, btbb_piconet_get_clk_offset(ut->follow_pn));
-		stream_rx_usb(ut, cb_br_rx, ut->follow_pn);
+		cmd_set_bdaddr(ut->devh, btbb_piconet_get_bdaddr(pn));
+		cmd_start_hopping(ut->devh, btbb_piconet_get_clk_offset(pn));
+		stream_rx_usb(ut, cb_br_rx, pn);
+	}
+}
+
+void rx_afh(ubertooth_t* ut, btbb_piconet* pn, int timeout)
+{
+	int r = btbb_init(max_ac_errors);
+	if (r < 0)
+		return;
+
+	cmd_set_channel(ut->devh, 9999);
+
+	if (timeout) {
+		ubertooth_set_timeout(ut, timeout);
+
+		cmd_afh(ut->devh);
+		stream_rx_usb(ut, cb_afh_initial, pn);
+
+		cmd_stop(ut->devh);
+
+		btbb_print_afh_map(pn);
+	}
+
+	/*
+	 * Monitor changes in AFH channel map
+	 */
+	cmd_clear_afh_map(ut->devh);
+	cmd_afh(ut->devh);
+	stream_rx_usb(ut, cb_afh_monitor, pn);
+}
+
+void rx_afh_r(ubertooth_t* ut, btbb_piconet* pn, int timeout __attribute__((unused)))
+{
+	static uint32_t lasttime;
+
+	int r = btbb_init(max_ac_errors);
+	if (r < 0)
+		return;
+
+	cmd_set_channel(ut->devh, 9999);
+
+	cmd_afh(ut->devh);
+
+	// init USB transfer
+	r = ubertooth_bulk_init(ut);
+	if (r < 0)
+		return;
+
+	// tell ubertooth to send packets
+	r = cmd_rx_syms(ut->devh);
+	if (r < 0)
+		return;
+
+	// receive and process each packet
+	while(1) {
+		// libusb_handle_events(NULL);
+		ubertooth_bulk_wait(ut);
+		r = ubertooth_bulk_receive(ut, cb_afh_r, pn);
+		if(lasttime < time(NULL)) {
+			lasttime = time(NULL);
+			printf("%u ", (uint32_t)time(NULL));
+			// btbb_print_afh_map(pn);
+
+			uint8_t* afh_map = btbb_piconet_get_afh_map(pn);
+			for (int i=0; i<10; i++)
+				for (int j=0; j<8; j++)
+					if (afh_map[i] & (1<<j))
+						printf("1");
+					else
+						printf("0");
+			printf("\n");
+		}
+		if (r == 1)
+			return;
 	}
 }
 
@@ -830,7 +1000,6 @@ ubertooth_t* ubertooth_init()
 	ut->start_clk100ns = 0;
 	ut->last_clk100ns = 0;
 	ut->clk100ns_upper = 0;
-	ut->follow_pn = NULL;
 
 #ifdef ENABLE_PCAP
 	ut->h_pcap_bredr = NULL;
@@ -843,30 +1012,38 @@ ubertooth_t* ubertooth_init()
 	return ut;
 }
 
-ubertooth_t* ubertooth_start(int ubertooth_device)
+uint8_t ubertooth_connect(ubertooth_t* ut, int ubertooth_device)
 {
-	int r;
-	ubertooth_t* ut = ubertooth_init();
-
-	r = libusb_init(NULL);
+	int r = libusb_init(NULL);
 	if (r < 0) {
 		fprintf(stderr, "libusb_init failed (got 1.0?)\n");
-		return NULL;
+		return -1;
 	}
 
 	ut->devh = find_ubertooth_device(ubertooth_device);
 	if (ut->devh == NULL) {
 		fprintf(stderr, "could not open Ubertooth device\n");
 		ubertooth_stop(ut);
-		return NULL;
+		return -1;
 	}
 
 	r = libusb_claim_interface(ut->devh, 0);
 	if (r < 0) {
 		fprintf(stderr, "usb_claim_interface error %d\n", r);
 		ubertooth_stop(ut);
-		return NULL;
+		return -1;
 	}
 
+	return 1;
+}
+
+ubertooth_t* ubertooth_start(int ubertooth_device)
+{
+	ubertooth_t* ut = ubertooth_init();
+
+	int r = ubertooth_connect(ut, ubertooth_device);
+	if (r < 0)
+		return NULL;
+
 	return ut;
 }
diff --git a/host/libubertooth/src/ubertooth.h b/host/libubertooth/src/ubertooth.h
index 3b46a2f..a3f21ff 100644
--- a/host/libubertooth/src/ubertooth.h
+++ b/host/libubertooth/src/ubertooth.h
@@ -56,7 +56,6 @@ typedef struct {
 	uint32_t start_clk100ns;
 	uint64_t last_clk100ns;
 	uint64_t clk100ns_upper;
-	btbb_piconet* follow_pn;
 
 #ifdef ENABLE_PCAP
 	btbb_pcap_handle * h_pcap_bredr;
@@ -67,6 +66,8 @@ typedef struct {
 	lell_pcapng_handle * h_pcapng_le;
 } ubertooth_t;
 
+unsigned int counter_max;
+
 typedef void (*rx_callback)(ubertooth_t* ut, void* args);
 
 typedef struct {
@@ -76,15 +77,15 @@ typedef struct {
 void print_version();
 void register_cleanup_handler(ubertooth_t* ut);
 ubertooth_t* ubertooth_init();
+uint8_t ubertooth_connect(ubertooth_t* ut, int ubertooth_device);
 ubertooth_t* ubertooth_start(int ubertooth_device);
 void ubertooth_stop(ubertooth_t* ut);
+void ubertooth_set_timeout(ubertooth_t* ut, int seconds);
 
 int ubertooth_bulk_init(ubertooth_t* ut);
 void ubertooth_bulk_wait(ubertooth_t* ut);
 int ubertooth_bulk_receive(ubertooth_t* ut, rx_callback cb, void* cb_args);
 
-int stream_rx_usb(ubertooth_t* ut, rx_callback cb, void* cb_args);
-int stream_rx_file(FILE* fp, rx_callback cb, void* cb_args);
 void rx_live(ubertooth_t* ut, btbb_piconet* pn, int timeout);
 void rx_file(FILE* fp, btbb_piconet* pn);
 void rx_dump(ubertooth_t* ut, int full);
@@ -92,5 +93,10 @@ void rx_btle(ubertooth_t* ut);
 void rx_btle_file(FILE* fp);
 void cb_btle(ubertooth_t* ut, void* args);
 void cb_ego(ubertooth_t* ut, void* args);
+void rx_afh(ubertooth_t* ut, btbb_piconet* pn, int timeout);
+void rx_afh_r(ubertooth_t* ut, btbb_piconet* pn, int timeout);
+void cb_afh_initial(ubertooth_t* ut, void* args);
+void cb_afh_monitor(ubertooth_t* ut, void* args);
+void cb_afh_r(ubertooth_t* ut, void* args);
 
 #endif /* __UBERTOOTH_H__ */
diff --git a/host/libubertooth/src/ubertooth_control.c b/host/libubertooth/src/ubertooth_control.c
index 5d99129..121268c 100644
--- a/host/libubertooth/src/ubertooth_control.c
+++ b/host/libubertooth/src/ubertooth_control.c
@@ -54,6 +54,14 @@ void show_libusb_error(int error_code)
 	fprintf(stderr,"libUSB Error: %s: %s (%d)\n", error_name, error_hint, error_code);
 }
 
+static void callback(struct libusb_transfer* transfer)
+{
+	if(transfer->status != 0) {
+		show_libusb_error(transfer->status);
+	}
+	libusb_free_transfer(transfer);
+}
+
 int cmd_ping(struct libusb_device_handle* devh)
 {
 	int r;
@@ -705,19 +713,16 @@ int cmd_btle_sniffing(struct libusb_device_handle* devh, u16 num)
 	return 0;
 }
 
-int cmd_set_afh_map(struct libusb_device_handle* devh, u8* afh_map)
+int cmd_set_afh_map(struct libusb_device_handle* devh, uint8_t* afh_map)
 {
-	int r;
-	r = libusb_control_transfer(devh, CTRL_OUT, UBERTOOTH_SET_AFHMAP, 0, 0,
-		afh_map, 10, 1000);
-	if (r < 0) {
-		if (r == LIBUSB_ERROR_PIPE) {
-			fprintf(stderr, "control message unsupported\n");
-		} else {
-			show_libusb_error(r);
-		}
-		return r;
-	}
+	uint8_t buffer[LIBUSB_CONTROL_SETUP_SIZE+10];
+	struct libusb_transfer *xfer = libusb_alloc_transfer(0);
+
+	libusb_fill_control_setup(buffer, CTRL_OUT, UBERTOOTH_SET_AFHMAP, 0, 0, 10);
+	memcpy ( &buffer[LIBUSB_CONTROL_SETUP_SIZE], afh_map, 10 );
+	libusb_fill_control_transfer(xfer, devh, buffer, callback, NULL, 1000);
+	libusb_submit_transfer(xfer);
+
 	return 0;
 }
 
@@ -948,3 +953,33 @@ int cmd_ego(struct libusb_device_handle* devh, int mode)
 	}
 	return 0;
 }
+
+int cmd_afh(struct libusb_device_handle* devh)
+{
+	int r;
+
+	r = libusb_control_transfer(devh, CTRL_OUT, UBERTOOTH_AFH, 0, 0,
+			NULL, 0, 1000);
+	if (r < 0) {
+		if (r == LIBUSB_ERROR_PIPE) {
+			fprintf(stderr, "control message unsupported\n");
+		} else {
+			show_libusb_error(r);
+		}
+		return r;
+	}
+
+	return 0;
+}
+
+int cmd_hop(struct libusb_device_handle* devh)
+{
+	uint8_t buffer[LIBUSB_CONTROL_SETUP_SIZE];
+	struct libusb_transfer *xfer = libusb_alloc_transfer(0);
+
+	libusb_fill_control_setup(buffer, CTRL_OUT, UBERTOOTH_HOP, 0, 0, 0);
+	libusb_fill_control_transfer(xfer, devh, buffer, callback, NULL, 1000);
+	libusb_submit_transfer(xfer);
+
+	return 0;
+}
diff --git a/host/libubertooth/src/ubertooth_control.h b/host/libubertooth/src/ubertooth_control.h
index f4ccb77..9712711 100644
--- a/host/libubertooth/src/ubertooth_control.h
+++ b/host/libubertooth/src/ubertooth_control.h
@@ -132,5 +132,7 @@ int cmd_btle_set_target(struct libusb_device_handle* devh, u8 *mac_address);
 int cmd_set_jam_mode(struct libusb_device_handle* devh, int mode);
 int cmd_ego(struct libusb_device_handle* devh, int mode);
 int cmd_ping(struct libusb_device_handle* devh);
+int cmd_afh(struct libusb_device_handle* devh);
+int cmd_hop(struct libusb_device_handle* devh);
 
 #endif /* __UBERTOOTH_CONTROL_H__ */
diff --git a/host/libubertooth/src/ubertooth_interface.h b/host/libubertooth/src/ubertooth_interface.h
index cb671a0..f001fe7 100644
--- a/host/libubertooth/src/ubertooth_interface.h
+++ b/host/libubertooth/src/ubertooth_interface.h
@@ -90,6 +90,8 @@ enum ubertooth_usb_commands {
     UBERTOOTH_WRITE_REGISTER  = 58,
     UBERTOOTH_JAM_MODE        = 59,
     UBERTOOTH_EGO             = 60,
+    UBERTOOTH_AFH             = 61,
+    UBERTOOTH_HOP             = 62,
 };
 
 enum jam_modes {
diff --git a/host/ubertooth-tools/src/CMakeLists.txt b/host/ubertooth-tools/src/CMakeLists.txt
index 47d9038..6e9ed41 100644
--- a/host/ubertooth-tools/src/CMakeLists.txt
+++ b/host/ubertooth-tools/src/CMakeLists.txt
@@ -59,7 +59,7 @@ if(USE_OWN_GNU_GETOPT)
 	LIST(APPEND TOOLS_LINK_LIBS libgetopt_static)
 endif(USE_OWN_GNU_GETOPT)
 
-LIST(APPEND TOOLS ubertooth-rx ubertooth-dump ubertooth-util ubertooth-btle ubertooth-dfu ubertooth-specan ubertooth-ego)
+LIST(APPEND TOOLS ubertooth-rx ubertooth-dump ubertooth-util ubertooth-btle ubertooth-dfu ubertooth-specan ubertooth-ego ubertooth-afh)
 
 if( USE_BLUEZ AND NOT ${LIBBLUETOOTH_FOUND} )
 	message( FATAL_ERROR
diff --git a/host/ubertooth-tools/src/ubertooth-afh.c b/host/ubertooth-tools/src/ubertooth-afh.c
new file mode 100644
index 0000000..359045f
--- /dev/null
+++ b/host/ubertooth-tools/src/ubertooth-afh.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright 2015 Hannes Ellinger
+ *
+ * This file is part of Project Ubertooth.
+ *
+ * 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, 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; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#include "ubertooth.h"
+#include "ubertooth_callback.h"
+#include <err.h>
+#include <getopt.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+
+extern int max_ac_errors;
+
+static void usage()
+{
+	printf("ubertooth-afh - passive detection of the AFH channel map\n");
+	printf("Usage:\n");
+	printf("\t-h this help\n");
+	printf("\t-r print AFH channel map once every second\n");
+	printf("\t-V print version information\n");
+	printf("\t-l <LAP> to decode (6 hex), otherwise sniff all LAPs\n");
+	printf("\t-u <UAP> to decode (2 hex), otherwise try to calculate (requires LAP)\n");
+	printf("\t-U <0-7> set ubertooth device to use\n");
+	printf("\t-t <seconds> timeout for initial AFH map detection\n");
+	printf("\t-m <int> threshold for channel removal\n");
+	printf("\t-e max_ac_errors (default: %d, range: 0-4)\n", max_ac_errors);
+	printf("\nIf an input file is not specified, an Ubertooth device is used for live capture.\n");
+}
+
+int main(int argc, char* argv[])
+{
+	int opt, have_lap = 0, have_uap = 0, timeout = 0;//, have_initial_afh = 0;
+	// uint8_t initial_afh[10];
+	char* end;
+	char ubertooth_device = -1;
+	btbb_piconet* pn = NULL;
+	uint32_t lap = 0;
+	uint8_t uap = 0;
+	uint8_t use_r_format = 0;
+
+	ubertooth_t* ut = NULL;
+
+	while ((opt=getopt(argc,argv,"rhVl:u:U:e:a:t:m:")) != EOF) {
+		switch(opt) {
+		case 'l':
+			lap = strtol(optarg, &end, 16);
+			have_lap = 1;
+			break;
+		case 'u':
+			uap = strtol(optarg, &end, 16);
+			have_uap = 1;
+			break;
+		case 'a':
+			// for(int i=0; i<10; i++) {
+			// 	sscanf(2+optarg+2*i, "%2hhx", &initial_afh[i]);
+			// }
+			// have_initial_afh = 1;
+			break;
+		case 't':
+			timeout = atoi(optarg);
+			break;
+		case 'U':
+			ubertooth_device = atoi(optarg);
+			break;
+		case 'e':
+			max_ac_errors = atoi(optarg);
+			break;
+		case 'm':
+			counter_max = atoi(optarg);
+			break;
+		case 'V':
+			print_version();
+			return 0;
+		case 'r':
+			use_r_format = 1;
+			break;
+		case 'h':
+		default:
+			usage();
+			return 1;
+		}
+	}
+
+	if (have_lap && have_uap) {
+		pn = btbb_piconet_new();
+		btbb_init_piconet(pn, lap);
+		btbb_piconet_set_uap(pn, uap);
+		btbb_piconet_set_flag(pn, BTBB_IS_AFH, 1);
+		btbb_piconet_set_flag(pn, BTBB_LOOKS_LIKE_AFH, 1);
+		btbb_init_survey();
+	} else {
+		printf("Error: UAP or LAP not specified\n");
+		usage();
+		return 1;
+	}
+
+	if (counter_max == 0) {
+		printf("Error: Threshold for unused channels not specified\n");
+		usage();
+		return 1;
+	}
+
+	ut = ubertooth_start(ubertooth_device);
+	if (ut->devh == NULL) {
+		usage();
+		return 1;
+	}
+
+	/* Clean up on exit. */
+	register_cleanup_handler(ut);
+
+	if (use_r_format)
+		rx_afh_r(ut, pn, timeout);
+	else
+		rx_afh(ut, pn, timeout);
+
+	ubertooth_stop(ut);
+
+	return 0;
+}
diff --git a/host/ubertooth-tools/src/ubertooth-dump.c b/host/ubertooth-tools/src/ubertooth-dump.c
index 9e0af92..d6569e5 100644
--- a/host/ubertooth-tools/src/ubertooth-dump.c
+++ b/host/ubertooth-tools/src/ubertooth-dump.c
@@ -45,7 +45,7 @@ static void usage(void)
  * representing the symbol determined by the demodulator (GnuRadio style)
  */
 
-extern FILE *dumpfile;
+extern FILE* dumpfile;
 
 int main(int argc, char *argv[])
 {
@@ -53,6 +53,7 @@ int main(int argc, char *argv[])
 	int bitstream = 0;
 	int modulation = MOD_BT_BASIC_RATE;
 	char ubertooth_device = -1;
+
 	ubertooth_t* ut = NULL;
 
 	while ((opt=getopt(argc,argv,"bhclU:d:")) != EOF) {
diff --git a/host/ubertooth-tools/src/ubertooth-follow.c b/host/ubertooth-tools/src/ubertooth-follow.c
index 4212a57..92a75aa 100644
--- a/host/ubertooth-tools/src/ubertooth-follow.c
+++ b/host/ubertooth-tools/src/ubertooth-follow.c
@@ -35,8 +35,6 @@
 extern int max_ac_errors;
 extern FILE *dumpfile;
 
-ubertooth_t* ut = NULL;
-
 static void usage()
 {
 	printf("ubertooth-follow - active(bluez) CLK discovery and follow for a particular UAP/LAP\n");
@@ -75,8 +73,8 @@ int main(int argc, char *argv[])
 	struct hci_dev_info di;
 	int cc = 0;
 
-
 	pn = btbb_piconet_new();
+	ubertooth_t* ut = ubertooth_init();
 
 	while ((opt=getopt(argc,argv,"hl:u:U:e:d:ab:w:r:q:")) != EOF) {
 		switch(opt) {
@@ -225,7 +223,7 @@ int main(int argc, char *argv[])
 	/* Clean up on exit. */
 	register_cleanup_handler(ut);
 
-	ut = ubertooth_start(ubertooth_device);
+	ubertooth_connect(ut, ubertooth_device);
 	if (ut == NULL) {
 		usage();
 		return 1;
@@ -236,7 +234,6 @@ int main(int argc, char *argv[])
 	btbb_piconet_set_clk_offset(pn, clock+delay);
 	btbb_piconet_set_flag(pn, BTBB_FOLLOWING, 1);
 	btbb_piconet_set_flag(pn, BTBB_CLK27_VALID, 1);
-	ut->follow_pn = pn;
 	rx_live(ut, pn, 0);
 	ubertooth_stop(ut);
 
diff --git a/host/ubertooth-tools/src/ubertooth-rx.c b/host/ubertooth-tools/src/ubertooth-rx.c
index a9ff601..ba00d3f 100644
--- a/host/ubertooth-tools/src/ubertooth-rx.c
+++ b/host/ubertooth-tools/src/ubertooth-rx.c
@@ -28,8 +28,6 @@ extern FILE *dumpfile;
 extern FILE *infile;
 extern int max_ac_errors;
 
-ubertooth_t* ut = NULL;
-
 static void usage()
 {
 	printf("ubertooth-rx - passive Bluetooth discovery/decode\n");
@@ -62,6 +60,8 @@ int main(int argc, char *argv[])
 	uint32_t lap = 0;
 	uint8_t uap = 0;
 
+	ubertooth_t* ut = ubertooth_init();
+
 	while ((opt=getopt(argc,argv,"hVi:l:u:U:d:e:r:sq:t:")) != EOF) {
 		switch(opt) {
 		case 'i':
@@ -148,7 +148,7 @@ int main(int argc, char *argv[])
 	}
 
 	if (infile == NULL) {
-		ut = ubertooth_start(ubertooth_device);
+		ubertooth_connect(ut, ubertooth_device);
 		if (ut == NULL) {
 			usage();
 			return 1;
diff --git a/host/ubertooth-tools/src/ubertooth-scan.c b/host/ubertooth-tools/src/ubertooth-scan.c
index 122ffa3..e872949 100644
--- a/host/ubertooth-tools/src/ubertooth-scan.c
+++ b/host/ubertooth-tools/src/ubertooth-scan.c
@@ -62,7 +62,7 @@ void extra_info(int dd, int dev_id, bdaddr_t* bdaddr)
 	struct hci_dev_info di;
 	struct hci_conn_info_req *cr;
 	int i, cc = 0;
-	
+
 	if (hci_devinfo(dev_id, &di) < 0) {
 		perror("Can't get device info");
 		exit(1);
@@ -157,7 +157,7 @@ void extra_info(int dd, int dev_id, bdaddr_t* bdaddr)
 		printf("AFH disabled.\n");
 	}
 	free(cr);
-	
+
 	if (cc) {
 		usleep(10000);
 		hci_disconnect(dd, handle, HCI_OE_USER_ENDED_CONNECTION, 10000);
@@ -189,7 +189,7 @@ int main(int argc, char *argv[])
 	char *bt_dev = "hci0";
     char addr[19] = { 0 };
 	ubertooth_t* ut = NULL;
-	btbb_piconet *pn;
+	btbb_piconet* pn;
 	bdaddr_t bdaddr;
 
 	while ((opt=getopt(argc,argv,"hU:t:e:xsb:")) != EOF) {
@@ -250,11 +250,11 @@ int main(int argc, char *argv[])
 		max_rsp = 255;
 		flags = IREQ_CACHE_FLUSH;
 		ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
-		
+
 		num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
 		if( num_rsp < 0 )
 			perror("hci_inquiry");
-	
+
 		for (i = 0; i < num_rsp; i++) {
 			ba2str(&(ii+i)->bdaddr, addr);
 			print_name_and_class(dev_handle, dev_id, &(ii+i)->bdaddr, addr,
diff --git a/host/ubertooth-tools/src/ubertooth-specan.c b/host/ubertooth-tools/src/ubertooth-specan.c
index 2df09ea..30e9543 100644
--- a/host/ubertooth-tools/src/ubertooth-specan.c
+++ b/host/ubertooth-tools/src/ubertooth-specan.c
@@ -26,8 +26,6 @@
 uint8_t debug;
 extern FILE *dumpfile;
 
-ubertooth_t* ut = NULL;
-
 void cb_specan(ubertooth_t* ut __attribute__((unused)), void* args)
 {
 	uint16_t high_freq = (((uint8_t*)args)[0]) |
@@ -94,6 +92,8 @@ int main(int argc, char *argv[])
 	int lower= 2402, upper= 2480;
 	char ubertooth_device = -1;
 
+	ubertooth_t* ut = NULL;
+
 	while ((opt=getopt(argc,argv,"vhgGd:l::u::U:")) != EOF) {
 		switch(opt) {
 		case 'v':