Merge pull request #38 from KurtE/BT-Multi-Merge

Bt multi merge

.gitignore

+*/Release/*
+*/Debug/*
+*/.vs/*
+*/*/.vs/*
+*/*/__vm/*
+*.d
+*.o
+*.eep
+*.elf
+*.hex
+*.lst
+*.vcxproj
+*.filters
+*.user
+*.a
+*.xml
+*.sln
+*.buildinfo

USBHost_t36.h

 // your best effort to read chapter 4 before asking USB questions!
 
 
-//#define USBHOST_PRINT_DEBUG
+#define USBHOST_PRINT_DEBUG
 //#define USBHDBGSerial	Serial1
 
 #ifndef USBHDBGSerial
     bool setLEDs(uint8_t lr, uint8_t lg, uint8_t lb);  // sets Leds, 
     bool inline setLEDs(uint32_t leds) {return setLEDs((leds >> 16) & 0xff, (leds >> 8) & 0xff, leds & 0xff);}  // sets Leds - passing one arg for all leds 
 	enum { STANDARD_AXIS_COUNT = 10, ADDITIONAL_AXIS_COUNT = 54, TOTAL_AXIS_COUNT = (STANDARD_AXIS_COUNT+ADDITIONAL_AXIS_COUNT) };
-	typedef enum { UNKNOWN=0, PS3, PS4, XBOXONE, XBOX360, PS3_MOTION, SpaceNav} joytype_t;
+	typedef enum { UNKNOWN=0, PS3, PS4, XBOXONE, XBOX360, PS3_MOTION, SpaceNav, SWITCH} joytype_t;
 	joytype_t joystickType() {return joystickType_;} 
 
 	// PS3 pair function. hack, requires that it be connect4ed by USB and we have the address of the Bluetooth dongle...
 
 class BluetoothController: public USBDriver {
 public:
+	static const uint8_t MAX_CONNECTIONS = 4;
+	typedef struct {
+    BTHIDInput * 	device_driver_ = nullptr;;
+    uint16_t		connection_rxid_ = 0;
+    uint16_t		control_dcid_ = 0x70;
+    uint16_t		interrupt_dcid_ = 0x71;
+    uint16_t		interrupt_scid_;
+    uint16_t		control_scid_;
+
+    uint8_t			device_bdaddr_[6];// remember devices address
+    uint8_t			device_ps_repetion_mode_ ; // mode
+    uint8_t			device_clock_offset_[2];
+    uint32_t		device_class_;	// class of device. 
+    uint16_t		device_connection_handle_;	// handle to connection 
+	uint8_t    		remote_ver_;
+	uint16_t		remote_man_;
+	uint8_t			remote_subv_;
+	uint8_t			connection_complete_ = false;	//
+	} connection_info_t;
+
 	BluetoothController(USBHost &host, bool pair = false, const char *pin = "0000") : do_pair_device_(pair), pair_pincode_(pin), delayTimer_(this) 
 			 { init(); }
 
 	BTHIDInput * find_driver(uint32_t device_type, uint8_t *remoteName=nullptr);
 
 	// Hack to allow PS3 to maybe change values
+    uint16_t		next_dcid_ = 0x70;		// Lets try not hard coding control and interrupt dcid
+#if 0    
     uint16_t		connection_rxid_ = 0;
     uint16_t		control_dcid_ = 0x70;
     uint16_t		interrupt_dcid_ = 0x71;
     uint16_t		interrupt_scid_;
     uint16_t		control_scid_;
-
+#else
+    connection_info_t connections_[MAX_CONNECTIONS];
+    uint8_t count_connections_ = 0;
+    uint8_t current_connection_ = 0;	// need to figure out when this changes and/or... 
+#endif    
 
 private:
 	friend class BTHIDInput;
 	USBDriverTimer 	delayTimer_;
     uint8_t 		my_bdaddr_[6];	// The bluetooth dongles Bluetooth address.
     uint8_t			features[8];	// remember our local features.
-    BTHIDInput * 	device_driver_ = nullptr;;
-    uint8_t			device_bdaddr_[6];// remember devices address
-    uint8_t			device_ps_repetion_mode_ ; // mode
-    uint8_t			device_clock_offset_[2];
-    uint32_t		device_class_;	// class of device. 
-    uint16_t		device_connection_handle_;	// handle to connection 
-	uint8_t    		remote_ver_;
-	uint16_t		remote_man_;
-	uint8_t			remote_subv_;
-	uint8_t			connection_complete_ = false;	//
 
 	typedef struct {
 		uint16_t 	idVendor;

bluetooth.cpp

 #define print   USBHost::print_
 #define println USBHost::println_//#define DEBUG_BT
 
-//#define DEBUG_BT
+#define DEBUG_BT
 //#define DEBUG_BT_VERBOSE
 
 #ifndef DEBUG_BT
 void BluetoothController::disconnect()
 {
 	USBHDBGSerial.printf("Bluetooth Disconnect");
-	if (device_driver_) {
-		device_driver_->release_bluetooth();
-		device_driver_->remote_name_[0] = 0;
-		device_driver_ = nullptr;
+	if (connections_[current_connection_].device_driver_) {
+		connections_[current_connection_].device_driver_->release_bluetooth();
+		connections_[current_connection_].device_driver_->remote_name_[0] = 0;
+		connections_[current_connection_].device_driver_ = nullptr;
 	}
-	connection_complete_ = false;
+	connections_[current_connection_].connection_complete_ = false;
 }
 
 void BluetoothController::timer_event(USBDriverTimer *whichTimer)
 		case HCI_WRITE_SCAN_ENABLE:				//0x0c1a
 			DBGPrintf("Write_Scan_enable Completed\n");
 			// See if we have driver and a remote
-			if (device_driver_ && connection_complete_) {	// We have a driver call their 
-				device_driver_->connectionComplete();
-				connection_complete_ = false;	// only call once
+			if (connections_[current_connection_].device_driver_ && connections_[current_connection_].connection_complete_) {	// We have a driver call their 
+				connections_[current_connection_].device_driver_->connectionComplete();
+				connections_[current_connection_].connection_complete_ = false;	// only call once
 			}
 			break;
 		case HCI_WRITE_SSP_MODE:					//0x0c56
 void BluetoothController::handle_hci_command_status() 
 {
 	// <event type><param count><status><num packets allowed to be sent><CMD><CMD>
-#ifdef DEBUG_BT
 	uint16_t hci_command = rxbuf_[4] + (rxbuf_[5] << 8);
 	if (rxbuf_[2]) {
+		#ifdef DEBUG_BT
 		DBGPrintf("    Command %x Status %x - ", hci_command, rxbuf_[2]);
 		switch (rxbuf_[2]) {
 			case 0x01: DBGPrintf("Unknown HCI Command\n"); break;
 			case 0x0F: DBGPrintf("Connection Rejected due to Unacceptable BD_ADDR\n"); break;
 			default: DBGPrintf("???\n"); break;
 		}
+		#endif
+		// lets try recovering from some errors...  
+		switch (hci_command) {
+			case HCI_OP_ACCEPT_CONN_REQ:
+				// We assume that the connection failed...
+				DBGPrintf("### Connection Failed ###");
+				if (count_connections_) count_connections_--;
+				break;
+			default:
+				break;
+		}
+
 	} else {
+		#ifdef DEBUG_BT
 		VDBGPrintf("    Command %x Status %x\n", hci_command, rxbuf_[2]);
+		#endif
 	}
-#endif
 }
 
 void BluetoothController::handle_hci_inquiry_result(bool fRSSI) 
 			}
 
 			// BUGBUG, lets hard code to go to new state...
-			for (uint8_t i = 0; i < 6; i++) device_bdaddr_[i] = rxbuf_[index_bd+i];
-			device_class_ = bluetooth_class;
-			device_driver_ = find_driver(device_class_);
+			for (uint8_t i = 0; i < 6; i++) connections_[current_connection_].device_bdaddr_[i] = rxbuf_[index_bd+i];
+			connections_[current_connection_].device_class_ = bluetooth_class;
+			connections_[current_connection_].device_driver_ = find_driver(connections_[current_connection_].device_class_);
 
-    		device_ps_repetion_mode_  = rxbuf_[index_ps]; // mode
-    		device_clock_offset_[0] = rxbuf_[index_clock_offset];
-    		device_clock_offset_[1] = rxbuf_[index_clock_offset+1];
+    		connections_[current_connection_].device_ps_repetion_mode_  = rxbuf_[index_ps]; // mode
+    		connections_[current_connection_].device_clock_offset_[0] = rxbuf_[index_clock_offset];
+    		connections_[current_connection_].device_clock_offset_[1] = rxbuf_[index_clock_offset+1];
 
 			// Now we need to bail from inquiry and setup to try to connect...
 			sendHCIInquiryCancel();
 		}
 
 		// BUGBUG, lets hard code to go to new state...
-		for (uint8_t i = 0; i < 6; i++) device_bdaddr_[i] = rxbuf_[index_bd+i];
-		device_class_ = bluetooth_class;
-		device_driver_ = find_driver(device_class_, index_local_name? &rxbuf_[index_local_name] : nullptr);
+		for (uint8_t i = 0; i < 6; i++) connections_[current_connection_].device_bdaddr_[i] = rxbuf_[index_bd+i];
+		connections_[current_connection_].device_class_ = bluetooth_class;
+		connections_[current_connection_].device_driver_ = find_driver(connections_[current_connection_].device_class_, index_local_name? &rxbuf_[index_local_name] : nullptr);
 
-		device_ps_repetion_mode_  = rxbuf_[index_ps]; // mode
-		device_clock_offset_[0] = rxbuf_[index_clock_offset];
-		device_clock_offset_[1] = rxbuf_[index_clock_offset+1];
+		connections_[current_connection_].device_ps_repetion_mode_  = rxbuf_[index_ps]; // mode
+		connections_[current_connection_].device_clock_offset_[0] = rxbuf_[index_clock_offset];
+		connections_[current_connection_].device_clock_offset_[1] = rxbuf_[index_clock_offset+1];
 
 		// and if we found a driver, save away the name 
-		if (device_driver_ && index_local_name && size_local_name) {
+		if (connections_[current_connection_].device_driver_ && index_local_name && size_local_name) {
 			uint8_t buffer_index;
 			for (buffer_index = 0; size_local_name && (buffer_index < BTHIDInput::REMOTE_NAME_SIZE-1); buffer_index++) {
-				device_driver_->remote_name_[buffer_index] = rxbuf_[index_local_name+buffer_index];
+				connections_[current_connection_].device_driver_->remote_name_[buffer_index] = rxbuf_[index_local_name+buffer_index];
 				size_local_name--;
 			}
-			device_driver_->remote_name_[buffer_index] = 0;	// make sure null terminated
+			connections_[current_connection_].device_driver_->remote_name_[buffer_index] = 0;	// make sure null terminated
 		}
 
 		// Now we need to bail from inquiry and setup to try to connect...
 	//  0  1  2  3  4  5  6  7  8 9  10 11 12
 	//       ST CH CH BD BD BD BD BD BD LT EN
 	// 03 0b 04 00 00 40 25 00 58 4b 00 01 00 
-	device_connection_handle_ = rxbuf_[3]+ (uint16_t)(rxbuf_[4]<<8);
-	DBGPrintf("    Connection Complete - ST:%x LH:%x\n", rxbuf_[2], device_connection_handle_);
-	if (do_pair_device_ && !(device_driver_ && (device_driver_->special_process_required & BTHIDInput::SP_DONT_NEED_CONNECT))) {
+	connections_[current_connection_].device_connection_handle_ = rxbuf_[3]+ (uint16_t)(rxbuf_[4]<<8);
+	DBGPrintf("    Connection Complete - ST:%x LH:%x\n", rxbuf_[2], connections_[current_connection_].device_connection_handle_);
+	if (do_pair_device_ && !(connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_DONT_NEED_CONNECT))) {
 		sendHCIAuthenticationRequested();
 		pending_control_ = PC_AUTHENTICATION_REQUESTED;
-	} else if (device_driver_ && (device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {
+	} else if (connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {
 		DBGPrintf("   Needs connect to device(PS4?)\n");
 		// The PS4 requires a connection request to it. 
 		delay(1);
-		sendl2cap_ConnectionRequest(device_connection_handle_, connection_rxid_, control_dcid_, HID_CTRL_PSM);
+		sendl2cap_ConnectionRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_dcid_, HID_CTRL_PSM);
 #if 0		
 		delay(1);
 		
 			case 8: DBGPrintf("        Gamepad\n"); break;
 			case 0xc: DBGPrintf("        Remote Control\n"); break;
 		}
-		device_driver_ = find_driver(class_of_device);
+		if (count_connections_ < MAX_CONNECTIONS) current_connection_ = count_connections_++;
+
+		connections_[current_connection_].device_driver_ = find_driver(class_of_device);
 
 		// We need to save away the BDADDR and class link type?
-		for(uint8_t i=0; i<6; i++) device_bdaddr_[i] = rxbuf_[i+2];
-		device_class_ = class_of_device;	
+		for(uint8_t i=0; i<6; i++) connections_[current_connection_].device_bdaddr_[i] = rxbuf_[i+2];
+		connections_[current_connection_].device_class_ = class_of_device;	
 		sendHCIRemoteNameRequest();
 	}
 
 	//5 4 0 48 0 13
 	DBGPrintf("    Event: HCI Disconnect complete(%d): handle: %x, reason:%x\n", rxbuf_[2], 
 		rxbuf_[3]+(rxbuf_[4]<<8), rxbuf_[5]);
-	if (device_driver_) {
-		device_driver_->release_bluetooth();
-		device_driver_->remote_name_[0] = 0;
-		device_driver_ = nullptr;
+	if (connections_[current_connection_].device_driver_) {
+		connections_[current_connection_].device_driver_->release_bluetooth();
+		connections_[current_connection_].device_driver_->remote_name_[0] = 0;
+		connections_[current_connection_].device_driver_ = nullptr;
     	
     	// Restore to normal... 
-    	control_dcid_ = 0x70;
-    	interrupt_dcid_ = 0x71;
+    	connections_[current_connection_].control_dcid_ = 0x70;
+    	connections_[current_connection_].interrupt_dcid_ = 0x71;
 	}
 	// Probably should clear out connection data. 
-	device_connection_handle_ = 0;
-	device_class_ = 0;	
-	memset(device_bdaddr_, 0, sizeof(device_bdaddr_));
+#if 0
+	connections_[current_connection_].device_connection_handle_ = 0;
+	connections_[current_connection_].device_class_ = 0;	
+	memset(connections_[current_connection_].device_bdaddr_, 0, sizeof(connections_[current_connection_].device_bdaddr_));
+#endif
+	// Now we need to remove that item from our list of connections.
+	count_connections_--;  
+	if (count_connections_ == 0) {
+		// reset the next connection counts back to initial states.
+		next_dcid_ = 0x70;		// Lets try not hard coding control and interrupt dcid
+	}
+
+	for (uint8_t i = current_connection_; i < count_connections_; i++) connections_[i] = connections_[i+1];
+	current_connection_ = 0;
+
 }
 
 void BluetoothController::handle_hci_authentication_complete()
 	DBGPrintf("    Event: HCI Authentication complete(%d): handle: %x\n", rxbuf_[2], 
 		rxbuf_[3]+(rxbuf_[4]<<8));
 	// Start up lcap connection...
-	connection_rxid_ = 0;
-	sendl2cap_ConnectionRequest(device_connection_handle_, connection_rxid_, control_dcid_, HID_CTRL_PSM);
+	connections_[current_connection_].connection_rxid_ = 0;
+	sendl2cap_ConnectionRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_dcid_, HID_CTRL_PSM);
 }
 
 
 		DBGPrintf("\n");
 	}
 
-	if (device_driver_) {
-		if (!device_driver_->remoteNameComplete(&rxbuf_[9])) {
-			device_driver_->release_bluetooth();
-			device_driver_ = nullptr;
+	if (connections_[current_connection_].device_driver_) {
+		if (!connections_[current_connection_].device_driver_->remoteNameComplete(&rxbuf_[9])) {
+			connections_[current_connection_].device_driver_->release_bluetooth();
+			connections_[current_connection_].device_driver_ = nullptr;
 		}
 	}	
-	if (!device_driver_) {
-		device_driver_ = find_driver(device_class_, &rxbuf_[9]);
+	if (!connections_[current_connection_].device_driver_) {
+		connections_[current_connection_].device_driver_ = find_driver(connections_[current_connection_].device_class_, &rxbuf_[9]);
 		// not sure I should call remote name again, but they already process...
-		if (device_driver_) {
-			device_driver_->remoteNameComplete(&rxbuf_[9]);
+		if (connections_[current_connection_].device_driver_) {
+			connections_[current_connection_].device_driver_->remoteNameComplete(&rxbuf_[9]);
 		}
 	}
-	if (device_driver_) {
+	if (connections_[current_connection_].device_driver_) {
 		// lets save away the string. 
 		uint8_t buffer_index;
 		for (buffer_index = 0; buffer_index < BTHIDInput::REMOTE_NAME_SIZE-1; buffer_index++) {
-			device_driver_->remote_name_[buffer_index] = rxbuf_[9+buffer_index];
-			if (!device_driver_->remote_name_[buffer_index]) break;
+			connections_[current_connection_].device_driver_->remote_name_[buffer_index] = rxbuf_[9+buffer_index];
+			if (!connections_[current_connection_].device_driver_->remote_name_[buffer_index]) break;
 		}
-		device_driver_->remote_name_[buffer_index] = 0;	// make sure null terminated
+		connections_[current_connection_].device_driver_->remote_name_[buffer_index] = 0;	// make sure null terminated
 
-		if (device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS) {
+		if (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS) {
 			// Real hack see if PS3... 
-	    	control_dcid_ = 0x40;
-	    	interrupt_dcid_ = 0x41;
+	    	connections_[current_connection_].control_dcid_ = 0x40;
+	    	connections_[current_connection_].interrupt_dcid_ = 0x41;
+	    } else {
+	    	connections_[current_connection_].control_dcid_ = next_dcid_++;
+	    	connections_[current_connection_].interrupt_dcid_ = next_dcid_++;
 	    }
 	}
 
 	// If we are in the connection complete mode, then this is a pairing state and needed to call
 	// get remote name later. 
-	if (connection_complete_) {	
-		if (device_driver_) {	// We have a driver call their 
-			device_driver_->connectionComplete();
-			connection_complete_ = false;	// only call once
+	if (connections_[current_connection_].connection_complete_) {	
+		if (connections_[current_connection_].device_driver_) {	// We have a driver call their 
+			connections_[current_connection_].device_driver_->connectionComplete();
+			connections_[current_connection_].connection_complete_ = false;	// only call once
 		}
 	} else {
 		sendHCIAcceptConnectionRequest();		
 void BluetoothController::handle_hci_remote_version_information_complete() {
 	//           STAT bd   bd   bd   bd    bd  bd
 	//c 8 0 48 0 5 45 0 0 0
-	remote_ver_ = rxbuf_[6];
-	remote_man_ = rxbuf_[7]+((uint16_t)rxbuf_[8]<< 8);
-	remote_subv_ = rxbuf_[9];
+	connections_[current_connection_].remote_ver_ = rxbuf_[6];
+	connections_[current_connection_].remote_man_ = rxbuf_[7]+((uint16_t)rxbuf_[8]<< 8);
+	connections_[current_connection_].remote_subv_ = rxbuf_[9];
 	
 	DBGPrintf("    Event: handle_hci_remote_version_information_complete(%d): ", rxbuf_[2]);
 	DBGPrintf(" Handle: %x, Ver:%x, Man: %x, SV: %x\n", 
-		rxbuf_[3]+((uint16_t)rxbuf_[4]<< 8), remote_ver_, remote_man_, remote_subv_);
+		rxbuf_[3]+((uint16_t)rxbuf_[4]<< 8), connections_[current_connection_].remote_ver_, connections_[current_connection_].remote_man_, connections_[current_connection_].remote_subv_);
 	// Lets now try to accept the connection. 
 	sendHCIAcceptConnectionRequest();
 }
 //	uint16_t rsp_packet_length = buffer[10] + ((uint16_t)buffer[11]<<8);
 	if ((hci_length == (l2cap_length + 4)) /*&& (hci_length == (rsp_packet_length+8))*/) {
 		// All the lengths appear to be correct...  need to do more...
+		// See if we should set the current_connection...
+		for (uint8_t i = 0; i < count_connections_; i++) {
+			//if (connections_[i].interrupt_dcid_ == buffer[6]) {
+			if (connections_[i].device_connection_handle_ == buffer[0]) {
+				current_connection_ = i;
+				break;
+			}
+		}
 		switch (buffer[8]) {
 			case L2CAP_CMD_CONNECTION_REQUEST:
 				process_l2cap_connection_request(&buffer[8]);
 	//0x05 0x04 0x0D 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x18 0xCC 0x01 0x00 0x00 0x00 0x00
 	//  05   04   0d   40   25   00   c4   01   00   18   cc   01   00   00 00     00 
 
-	for (uint8_t i=0; i<6; i++) connection_data[i] = device_bdaddr_[i];
+	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];
 	connection_data[6] = 0x18; //DM1/DH1
 	connection_data[7] = 0xcc; //
-	connection_data[8] = device_ps_repetion_mode_;  // from device
+	connection_data[8] = connections_[current_connection_].device_ps_repetion_mode_;  // from device
 	connection_data[9] = 0;	//
 	connection_data[10] = 0;  // clock offset 
 	connection_data[11] = 0;  // clock offset 
 	//  0    1    2    3    4    5    6    7    8   9    10   11   12
 	//  BD   BD   BD   BD   BD   BD  role 
 	//0x79 0x22 0x23 0x0A 0xC5 0xCC 0x00
-	for (uint8_t i=0; i<6; i++) connection_data[i] = device_bdaddr_[i];
+	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];
 	connection_data[6] = 0; // Role as master
 	sendHCICommand(HCI_OP_ACCEPT_CONN_REQ, sizeof(connection_data), connection_data);	
 }
 void BluetoothController::sendHCIAuthenticationRequested() {
 	DBGPrintf("HCI_AUTH_REQUESTED called (");
 	uint8_t connection_data[2];
-	connection_data[0] = device_connection_handle_ & 0xff;
-	connection_data[1] = (device_connection_handle_>>8) & 0xff;
+	connection_data[0] = connections_[current_connection_].device_connection_handle_ & 0xff;
+	connection_data[1] = (connections_[current_connection_].device_connection_handle_>>8) & 0xff;
 	sendHCICommand(HCI_AUTH_REQUESTED, sizeof(connection_data), connection_data);	
 }
 
 void BluetoothController::sendHCILinkKeyNegativeReply() {
 	DBGPrintf("HCI_LINK_KEY_NEG_REPLY called (");
 	uint8_t connection_data[6];
-	for (uint8_t i=0; i<6; i++) connection_data[i] = device_bdaddr_[i];
+	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];
 	sendHCICommand(HCI_LINK_KEY_NEG_REPLY, sizeof(connection_data), connection_data);	
 }
 
 	uint8_t connection_data[23];
 	uint8_t i;
 
-	for (i=0; i<6; i++) connection_data[i] = device_bdaddr_[i];
+	for (i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];
 
 	for (i=0; pair_pincode_[i] !=0; i++) connection_data[7+i] = pair_pincode_[i];
 	connection_data[6] = i; // remember the length	
 
 	DBGPrintf("HCI_OP_REMOTE_NAME_REQ called (");
 	uint8_t connection_data[10];
-	for (uint8_t i=0; i<6; i++) connection_data[i] = device_bdaddr_[i];
+	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];
 	connection_data[6] = 1;	// page scan repeat mode...
 	connection_data[7] = 0;	 // 0
 	connection_data[8] = 0;	// Clk offset
 
 	DBGPrintf("HCI_OP_READ_REMOTE_VERSION_INFORMATION called (");
 	uint8_t connection_data[2];
-	connection_data[0] = device_connection_handle_ & 0xff;
-	connection_data[1] = (device_connection_handle_>>8) & 0xff;
+	connection_data[0] = connections_[current_connection_].device_connection_handle_ & 0xff;
+	connection_data[1] = (connections_[current_connection_].device_connection_handle_>>8) & 0xff;
 	sendHCICommand(HCI_OP_READ_REMOTE_VERSION_INFORMATION, sizeof(connection_data), connection_data);	
 }
 
 	switch (pending_control_tx_) {
 		case STATE_TX_SEND_CONNECT_INT:
 		delay(1);
-		connection_rxid_++;
-		sendl2cap_ConnectionRequest(device_connection_handle_, connection_rxid_, interrupt_dcid_, HID_INTR_PSM);
+		connections_[current_connection_].connection_rxid_++;
+		sendl2cap_ConnectionRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].interrupt_dcid_, HID_INTR_PSM);
 		pending_control_tx_ = 0;
 		break;
 	case STATE_TX_SEND_CONECT_RSP_SUCCESS:
 		delay(1);
 		// Tell the device we are ready
-		sendl2cap_ConnectionResponse(device_connection_handle_, connection_rxid_++, control_dcid_, control_scid_, SUCCESSFUL);
+		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_++, connections_[current_connection_].control_dcid_, connections_[current_connection_].control_scid_, SUCCESSFUL);
 		pending_control_tx_ = STATE_TX_SEND_CONFIG_REQ;
 		break;
 	case STATE_TX_SEND_CONFIG_REQ:
 		delay(1);
-		sendl2cap_ConfigRequest(device_connection_handle_, connection_rxid_, control_scid_);
+		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_scid_);
 		pending_control_tx_ = 0;
 		break;
 	case STATE_TX_SEND_CONECT_ISR_RSP_SUCCESS:
 		delay(1);
 		// Tell the device we are ready
-		sendl2cap_ConnectionResponse(device_connection_handle_, connection_rxid_++, interrupt_dcid_, interrupt_scid_, SUCCESSFUL);
+		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_++, connections_[current_connection_].interrupt_dcid_, connections_[current_connection_].interrupt_scid_, SUCCESSFUL);
 		pending_control_tx_ = STATE_TX_SEND_CONFIG_ISR_REQ;
 		break;
 	case STATE_TX_SEND_CONFIG_ISR_REQ:
 		delay(1);
-		sendl2cap_ConfigRequest(device_connection_handle_, connection_rxid_, interrupt_scid_);
+		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].interrupt_scid_);
 		pending_control_tx_ = 0;
 		break;
 	}
 	uint16_t channel_out;
 	switch (channel) {
 		case CONTROL_SCID:
-			channel_out = control_scid_;
+			channel_out = connections_[current_connection_].control_scid_;
 			break;
 		case INTERRUPT_SCID:
-			channel_out = interrupt_scid_;
+			channel_out = connections_[current_connection_].interrupt_scid_;
 			break;
 		default:
 			channel_out = (uint16_t)channel;
 	}
 	DBGPrintf("sendL2CapCommand: %x %d %x\n", (uint32_t)data, nbytes, channel, channel_out);
-	sendL2CapCommand (device_connection_handle_, data, nbytes, channel_out & 0xff, (channel_out >> 8) & 0xff);
+	sendL2CapCommand (connections_[current_connection_].device_connection_handle_, data, nbytes, channel_out & 0xff, (channel_out >> 8) & 0xff);
 }
 
 
 
 	uint16_t psm = data[4]+((uint16_t)data[5] << 8); 
 	uint16_t scid = data[6]+((uint16_t)data[7] << 8);
-	connection_rxid_ = data[1];
-	DBGPrintf("    L2CAP Connection Request: ID: %d, PSM: %x, SCID: %x\n",connection_rxid_, psm, scid);
+	connections_[current_connection_].connection_rxid_ = data[1];
+	DBGPrintf("    L2CAP Connection Request: ID: %d, PSM: %x, SCID: %x\n",connections_[current_connection_].connection_rxid_, psm, scid);
 
 	// Assuming not pair mode Send response like:
 	//      RXID Len  LEN  DCID DCID  SCID SCID RES 0     0    0
 	// 0x03 0x02 0x08 0x00 0x70 0x00 0x43 0x00 0x01 0x00 0x00 0x00
 	if (psm == HID_CTRL_PSM) {
-		control_scid_ = scid;
-		sendl2cap_ConnectionResponse(device_connection_handle_, connection_rxid_, control_dcid_, control_scid_, PENDING);
+		connections_[current_connection_].control_scid_ = scid;
+		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_dcid_, connections_[current_connection_].control_scid_, PENDING);
 		pending_control_tx_ = STATE_TX_SEND_CONECT_RSP_SUCCESS;		
 	} else if (psm == HID_INTR_PSM) {
-		interrupt_scid_ = scid;
-		sendl2cap_ConnectionResponse(device_connection_handle_, connection_rxid_, interrupt_dcid_, interrupt_scid_, PENDING);
+		connections_[current_connection_].interrupt_scid_ = scid;
+		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].interrupt_dcid_, connections_[current_connection_].interrupt_scid_, PENDING);
 		pending_control_tx_ = STATE_TX_SEND_CONECT_ISR_RSP_SUCCESS;		
 
 	}
 		data[8]+((uint16_t)data[9] << 8), data[10]+((uint16_t)data[11] << 8));
 
 	//48 20 10 0 | c 0 1 0 | 3 0 8 0 44 0 70 0 0 0 0 0
-	if (dcid == interrupt_dcid_) {
-		interrupt_scid_ = scid;
+	if (dcid == connections_[current_connection_].interrupt_dcid_) {
+		connections_[current_connection_].interrupt_scid_ = scid;
 		DBGPrintf("      Interrupt Response\n");
-		connection_rxid_++;
-		sendl2cap_ConfigRequest(device_connection_handle_, connection_rxid_, scid);
-	} else if (dcid == control_dcid_) {
-		control_scid_ = scid;
+		connections_[current_connection_].connection_rxid_++;
+		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, scid);
+	} else if (dcid == connections_[current_connection_].control_dcid_) {
+		connections_[current_connection_].control_scid_ = scid;
 		DBGPrintf("      Control Response\n");
-		sendl2cap_ConfigRequest(device_connection_handle_, connection_rxid_, scid);
+		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, scid);
 	}
 }
 
 		data[1], dcid, data[6]+((uint16_t)data[7] << 8),
 		data[8], data[9], data[10], data[11]);
 	// Now see which dest was specified
-	if (dcid == control_dcid_) {
+	if (dcid == connections_[current_connection_].control_dcid_) {
 		DBGPrintf("      Control Configuration request\n");
-		sendl2cap_ConfigResponse(device_connection_handle_, data[1], control_scid_);
-	} else if (dcid == interrupt_dcid_) {
+		sendl2cap_ConfigResponse(connections_[current_connection_].device_connection_handle_, data[1], connections_[current_connection_].control_scid_);
+	} else if (dcid == connections_[current_connection_].interrupt_dcid_) {
 		DBGPrintf("      Interrupt Configuration request\n");
-		sendl2cap_ConfigResponse(device_connection_handle_, data[1], interrupt_scid_);
+		sendl2cap_ConfigResponse(connections_[current_connection_].device_connection_handle_, data[1], connections_[current_connection_].interrupt_scid_);
 	}
 }
 
 	DBGPrintf("    L2CAP config Response: ID: %d, Source:%x, Flags:%x, Result:%x, Config: %x\n",
 		data[1], scid, data[6]+((uint16_t)data[7] << 8),
 		data[8]+((uint16_t)data[9] << 8), data[10]+((uint16_t)data[11] << 8));
-	if (scid == control_dcid_) {
+	if (scid == connections_[current_connection_].control_dcid_) {
 		// Set HID Boot mode
 		// Don't do if PS3...
-		if (!(device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS)) {
+		if (!(connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS)) {
 			setHIDProtocol(HID_BOOT_PROTOCOL);  //
 		}
 		//setHIDProtocol(HID_RPT_PROTOCOL);  //HID_RPT_PROTOCOL
-		if (do_pair_device_ && !(device_driver_ && (device_driver_->special_process_required & BTHIDInput::SP_DONT_NEED_CONNECT))) {
+		if (do_pair_device_ && !(connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_DONT_NEED_CONNECT))) {
 			pending_control_tx_ = STATE_TX_SEND_CONNECT_INT;
-		} else if (device_driver_ && (device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {
+		} else if (connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {
 			DBGPrintf("   Needs connect to device INT(PS4?)\n");
 			// The PS4 requires a connection request to it. 
 			pending_control_tx_ = STATE_TX_SEND_CONNECT_INT;
 		} else {
 			pending_control_ = 0;
 		}
-	} else if (scid == interrupt_dcid_) {
+	} else if (scid == connections_[current_connection_].interrupt_dcid_) {
 		// Enable SCan to page mode
-		connection_complete_ = true;
+		connections_[current_connection_].connection_complete_ = true;
 		sendHCIWriteScanEnable(2);
 	}
 }
 	uint8_t l2capbuf[1];
     l2capbuf[0] = 0x70 | protocol; // Set Protocol, see Bluetooth HID specs page 33
     DBGPrintf("Set HID Protocol %d (", protocol);
-    sendL2CapCommand(device_connection_handle_, l2capbuf, sizeof(l2capbuf), control_scid_ & 0xff, control_scid_ >> 8);
+    sendL2CapCommand(connections_[current_connection_].device_connection_handle_, l2capbuf, sizeof(l2capbuf), connections_[current_connection_].control_scid_ & 0xff, connections_[current_connection_].control_scid_ >> 8);
 }
 
 void BluetoothController::handleHIDTHDRData(uint8_t *data) {
 	//                      T HID data
 	//48 20 d 0 9 0 71 0 a1 3 8a cc c5 a 23 22 79
 	uint16_t len = data[4] + ((uint16_t)data[5] << 8);
-	DBGPrintf("HID HDR Data: len: %d, Type: %d\n", len, data[9]);
+	DBGPrintf("HID HDR Data: len: %d, Type: %d Con:%d\n", len, data[9], current_connection_);
 
 	// ??? How to parse??? Use HID object??? 
-	if (device_driver_) {
-		device_driver_->process_bluetooth_HID_data(&data[9], len-1); // We skip the first byte...
+	if (connections_[current_connection_].device_driver_) {
+		connections_[current_connection_].device_driver_->process_bluetooth_HID_data(&data[9], len-1); // We skip the first byte...
 	} else {
 		switch (data[9]) {
 			case 1:

examples/Bluetooth/Mouse_KeyboardBT/Mouse_KeyboardBT.ino

+// Simple test of USB Host Mouse/Keyboard
+//
+// This example is in the public domain
+
+#include "USBHost_t36.h"
+
+USBHost myusb;
+USBHub hub1(myusb);
+USBHub hub2(myusb);
+KeyboardController keyboard1(myusb);
+//KeyboardController keyboard2(myusb);
+USBHIDParser hid1(myusb);
+USBHIDParser hid2(myusb);
+USBHIDParser hid3(myusb);
+USBHIDParser hid4(myusb);
+USBHIDParser hid5(myusb);
+MouseController mouse1(myusb);
+//JoystickController joystick1(myusb);
+//BluetoothController bluet(myusb, true, "0000");   // Version does pairing to device
+
+BluetoothController bluet(myusb);   // version assumes it already was paired
+
+USBDriver *drivers[] = {&hub1, &hub2, &keyboard1, &bluet, &hid1, &hid2, &hid3, &hid4, &hid5};
+
+#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
+const char * driver_names[CNT_DEVICES] = {"Hub1", "Hub2", "KB1", "Bluet", "HID1" , "HID2", "HID3", "HID4", "HID5"};
+
+bool driver_active[CNT_DEVICES] = {false, false, false, false};
+
+// Lets also look at HID Input devices
+USBHIDInput *hiddrivers[] = {&mouse1};
+
+#define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))
+const char * hid_driver_names[CNT_HIDDEVICES] = {"Mouse1"};
+
+bool hid_driver_active[CNT_HIDDEVICES] = {false};
+
+BTHIDInput *bthiddrivers[] = {&keyboard1, &mouse1};
+#define CNT_BTHIDDEVICES (sizeof(bthiddrivers)/sizeof(bthiddrivers[0]))
+const char * bthid_driver_names[CNT_BTHIDDEVICES] = {"Keyboard(BT)", "Mouse(BT)"};
+bool bthid_driver_active[CNT_BTHIDDEVICES] = {false};
+
+
+
+bool show_changed_only = false;
+
+uint8_t joystick_left_trigger_value = 0;
+uint8_t joystick_right_trigger_value = 0;
+uint64_t joystick_full_notify_mask = (uint64_t) - 1;
+
+void setup()
+{
+  while (!Serial) ; // wait for Arduino Serial Monitor
+  Serial.begin(115200);
+  Serial1.begin(115200);
+  Serial.println("\n\nUSB Host Testing");
+  Serial.println(sizeof(USBHub), DEC);
+  myusb.begin();
+  keyboard1.attachPress(OnPress);
+  //keyboard2.attachPress(OnPress);
+  keyboard1.attachExtrasPress(OnHIDExtrasPress);
+  keyboard1.attachExtrasRelease(OnHIDExtrasRelease);
+  //keyboard2.attachExtrasPress(OnHIDExtrasPress);
+  //keyboard2.attachExtrasRelease(OnHIDExtrasRelease);
+}
+
+
+void loop()
+{
+  // check to see if the device list has changed:
+  UpdateActiveDeviceInfo();
+
+  myusb.Task();
+
+  if (mouse1.available()) {
+    Serial.print("Mouse: buttons = ");
+    Serial.print(mouse1.getButtons());
+    Serial.print(",  mouseX = ");
+    Serial.print(mouse1.getMouseX());
+    Serial.print(",  mouseY = ");
+    Serial.print(mouse1.getMouseY());
+    Serial.print(",  wheel = ");
+    Serial.print(mouse1.getWheel());
+    Serial.print(",  wheelH = ");
+    Serial.print(mouse1.getWheelH());
+    Serial.println();
+    mouse1.mouseDataClear();
+  }
+}
+
+void OnPress(int key)
+{
+  Serial.print("key '");
+  switch (key) {
+    case KEYD_UP       : Serial.print("UP"); break;
+    case KEYD_DOWN    : Serial.print("DN"); break;
+    case KEYD_LEFT     : Serial.print("LEFT"); break;
+    case KEYD_RIGHT   : Serial.print("RIGHT"); break;
+    case KEYD_INSERT   : Serial.print("Ins"); break;
+    case KEYD_DELETE   : Serial.print("Del"); break;
+    case KEYD_PAGE_UP  : Serial.print("PUP"); break;
+    case KEYD_PAGE_DOWN: Serial.print("PDN"); break;
+    case KEYD_HOME     : Serial.print("HOME"); break;
+    case KEYD_END      : Serial.print("END"); break;
+    case KEYD_F1       : Serial.print("F1"); break;
+    case KEYD_F2       : Serial.print("F2"); break;
+    case KEYD_F3       : Serial.print("F3"); break;
+    case KEYD_F4       : Serial.print("F4"); break;
+    case KEYD_F5       : Serial.print("F5"); break;
+    case KEYD_F6       : Serial.print("F6"); break;
+    case KEYD_F7       : Serial.print("F7"); break;
+    case KEYD_F8       : Serial.print("F8"); break;
+    case KEYD_F9       : Serial.print("F9"); break;
+    case KEYD_F10      : Serial.print("F10"); break;
+    case KEYD_F11      : Serial.print("F11"); break;
+    case KEYD_F12      : Serial.print("F12"); break;
+    default: Serial.print((char)key); break;
+  }
+  Serial.print("'  ");
+  Serial.print(key);
+  Serial.print(" MOD: ");
+  if (keyboard1) {
+    Serial.print(keyboard1.getModifiers(), HEX);
+    Serial.print(" OEM: ");
+    Serial.print(keyboard1.getOemKey(), HEX);
+    Serial.print(" LEDS: ");
+    Serial.println(keyboard1.LEDS(), HEX);
+    //} else {
+    //Serial.print(keyboard2.getModifiers(), HEX);
+    //Serial.print(" OEM: ");
+    //Serial.print(keyboard2.getOemKey(), HEX);
+    //Serial.print(" LEDS: ");
+    //Serial.println(keyboard2.LEDS(), HEX);
+  }
+
+  //Serial.print("key ");
+  //Serial.print((char)keyboard1.getKey());
+  //Serial.print("  ");
+  //Serial.print((char)keyboard2.getKey());
+  //Serial.println();
+}
+void OnHIDExtrasPress(uint32_t top, uint16_t key)
+{
+  Serial.print("HID (");
+  Serial.print(top, HEX);
+  Serial.print(") key press:");
+  Serial.print(key, HEX);
+  if (top == 0xc0000) {
+    switch (key) {
+      case  0x20 : Serial.print(" - +10"); break;
+      case  0x21 : Serial.print(" - +100"); break;
+      case  0x22 : Serial.print(" - AM/PM"); break;
+      case  0x30 : Serial.print(" - Power"); break;
+      case  0x31 : Serial.print(" - Reset"); break;
+      case  0x32 : Serial.print(" - Sleep"); break;
+      case  0x33 : Serial.print(" - Sleep After"); break;
+      case  0x34 : Serial.print(" - Sleep Mode"); break;
+      case  0x35 : Serial.print(" - Illumination"); break;
+      case  0x36 : Serial.print(" - Function Buttons"); break;
+      case  0x40 : Serial.print(" - Menu"); break;
+      case  0x41 : Serial.print(" - Menu  Pick"); break;
+      case  0x42 : Serial.print(" - Menu Up"); break;
+      case  0x43 : Serial.print(" - Menu Down"); break;
+      case  0x44 : Serial.print(" - Menu Left"); break;
+      case  0x45 : Serial.print(" - Menu Right"); break;
+      case  0x46 : Serial.print(" - Menu Escape"); break;
+      case  0x47 : Serial.print(" - Menu Value Increase"); break;
+      case  0x48 : Serial.print(" - Menu Value Decrease"); break;
+      case  0x60 : Serial.print(" - Data On Screen"); break;
+      case  0x61 : Serial.print(" - Closed Caption"); break;
+      case  0x62 : Serial.print(" - Closed Caption Select"); break;
+      case  0x63 : Serial.print(" - VCR/TV"); break;
+      case  0x64 : Serial.print(" - Broadcast Mode"); break;
+      case  0x65 : Serial.print(" - Snapshot"); break;
+      case  0x66 : Serial.print(" - Still"); break;
+      case  0x80 : Serial.print(" - Selection"); break;
+      case  0x81 : Serial.print(" - Assign Selection"); break;
+      case  0x82 : Serial.print(" - Mode Step"); break;
+      case  0x83 : Serial.print(" - Recall Last"); break;
+      case  0x84 : Serial.print(" - Enter Channel"); break;
+      case  0x85 : Serial.print(" - Order Movie"); break;
+      case  0x86 : Serial.print(" - Channel"); break;
+      case  0x87 : Serial.print(" - Media Selection"); break;
+      case  0x88 : Serial.print(" - Media Select Computer"); break;
+      case  0x89 : Serial.print(" - Media Select TV"); break;
+      case  0x8A : Serial.print(" - Media Select WWW"); break;
+      case  0x8B : Serial.print(" - Media Select DVD"); break;
+      case  0x8C : Serial.print(" - Media Select Telephone"); break;
+      case  0x8D : Serial.print(" - Media Select Program Guide"); break;
+      case  0x8E : Serial.print(" - Media Select Video Phone"); break;
+      case  0x8F : Serial.print(" - Media Select Games"); break;
+      case  0x90 : Serial.print(" - Media Select Messages"); break;
+      case  0x91 : Serial.print(" - Media Select CD"); break;
+      case  0x92 : Serial.print(" - Media Select VCR"); break;
+      case  0x93 : Serial.print(" - Media Select Tuner"); break;
+      case  0x94 : Serial.print(" - Quit"); break;
+      case  0x95 : Serial.print(" - Help"); break;
+      case  0x96 : Serial.print(" - Media Select Tape"); break;
+      case  0x97 : Serial.print(" - Media Select Cable"); break;
+      case  0x98 : Serial.print(" - Media Select Satellite"); break;
+      case  0x99 : Serial.print(" - Media Select Security"); break;
+      case  0x9A : Serial.print(" - Media Select Home"); break;
+      case  0x9B : Serial.print(" - Media Select Call"); break;
+      case  0x9C : Serial.print(" - Channel Increment"); break;
+      case  0x9D : Serial.print(" - Channel Decrement"); break;
+      case  0x9E : Serial.print(" - Media Select SAP"); break;
+      case  0xA0 : Serial.print(" - VCR Plus"); break;
+      case  0xA1 : Serial.print(" - Once"); break;
+      case  0xA2 : Serial.print(" - Daily"); break;
+      case  0xA3 : Serial.print(" - Weekly"); break;
+      case  0xA4 : Serial.print(" - Monthly"); break;
+      case  0xB0 : Serial.print(" - Play"); break;
+      case  0xB1 : Serial.print(" - Pause"); break;
+      case  0xB2 : Serial.print(" - Record"); break;
+      case  0xB3 : Serial.print(" - Fast Forward"); break;
+      case  0xB4 : Serial.print(" - Rewind"); break;
+      case  0xB5 : Serial.print(" - Scan Next Track"); break;
+      case  0xB6 : Serial.print(" - Scan Previous Track"); break;
+      case  0xB7 : Serial.print(" - Stop"); break;
+      case  0xB8 : Serial.print(" - Eject"); break;
+      case  0xB9 : Serial.print(" - Random Play"); break;
+      case  0xBA : Serial.print(" - Select DisC"); break;
+      case  0xBB : Serial.print(" - Enter Disc"); break;
+      case  0xBC : Serial.print(" - Repeat"); break;
+      case  0xBD : Serial.print(" - Tracking"); break;
+      case  0xBE : Serial.print(" - Track Normal"); break;
+      case  0xBF : Serial.print(" - Slow Tracking"); break;
+      case  0xC0 : Serial.print(" - Frame Forward"); break;
+      case  0xC1 : Serial.print(" - Frame Back"); break;
+      case  0xC2 : Serial.print(" - Mark"); break;
+      case  0xC3 : Serial.print(" - Clear Mark"); break;
+      case  0xC4 : Serial.print(" - Repeat From Mark"); break;
+      case  0xC5 : Serial.print(" - Return To Mark"); break;
+      case  0xC6 : Serial.print(" - Search Mark Forward"); break;
+      case  0xC7 : Serial.print(" - Search Mark Backwards"); break;
+      case  0xC8 : Serial.print(" - Counter Reset"); break;
+      case  0xC9 : Serial.print(" - Show Counter"); break;
+      case  0xCA : Serial.print(" - Tracking Increment"); break;
+      case  0xCB : Serial.print(" - Tracking Decrement"); break;
+      case  0xCD : Serial.print(" - Pause/Continue"); break;
+      case  0xE0 : Serial.print(" - Volume"); break;
+      case  0xE1 : Serial.print(" - Balance"); break;
+      case  0xE2 : Serial.print(" - Mute"); break;
+      case  0xE3 : Serial.print(" - Bass"); break;
+      case  0xE4 : Serial.print(" - Treble"); break;
+      case  0xE5 : Serial.print(" - Bass Boost"); break;
+      case  0xE6 : Serial.print(" - Surround Mode"); break;
+      case  0xE7 : Serial.print(" - Loudness"); break;
+      case  0xE8 : Serial.print(" - MPX"); break;
+      case  0xE9 : Serial.print(" - Volume Up"); break;
+      case  0xEA : Serial.print(" - Volume Down"); break;
+      case  0xF0 : Serial.print(" - Speed Select"); break;
+      case  0xF1 : Serial.print(" - Playback Speed"); break;
+      case  0xF2 : Serial.print(" - Standard Play"); break;
+      case  0xF3 : Serial.print(" - Long Play"); break;
+      case  0xF4 : Serial.print(" - Extended Play"); break;
+      case  0xF5 : Serial.print(" - Slow"); break;
+      case  0x100: Serial.print(" - Fan Enable"); break;
+      case  0x101: Serial.print(" - Fan Speed"); break;
+      case  0x102: Serial.print(" - Light"); break;
+      case  0x103: Serial.print(" - Light Illumination Level"); break;
+      case  0x104: Serial.print(" - Climate Control Enable"); break;
+      case  0x105: Serial.print(" - Room Temperature"); break;
+      case  0x106: Serial.print(" - Security Enable"); break;
+      case  0x107: Serial.print(" - Fire Alarm"); break;
+      case  0x108: Serial.print(" - Police Alarm"); break;
+      case  0x150: Serial.print(" - Balance Right"); break;
+      case  0x151: Serial.print(" - Balance Left"); break;
+      case  0x152: Serial.print(" - Bass Increment"); break;
+      case  0x153: Serial.print(" - Bass Decrement"); break;
+      case  0x154: Serial.print(" - Treble Increment"); break;
+      case  0x155: Serial.print(" - Treble Decrement"); break;
+      case  0x160: Serial.print(" - Speaker System"); break;
+      case  0x161: Serial.print(" - Channel Left"); break;
+      case  0x162: Serial.print(" - Channel Right"); break;
+      case  0x163: Serial.print(" - Channel Center"); break;
+      case  0x164: Serial.print(" - Channel Front"); break;
+      case  0x165: Serial.print(" - Channel Center Front"); break;
+      case  0x166: Serial.print(" - Channel Side"); break;
+      case  0x167: Serial.print(" - Channel Surround"); break;
+      case  0x168: Serial.print(" - Channel Low Frequency Enhancement"); break;
+      case  0x169: Serial.print(" - Channel Top"); break;
+      case  0x16A: Serial.print(" - Channel Unknown"); break;
+      case  0x170: Serial.print(" - Sub-channel"); break;
+      case  0x171: Serial.print(" - Sub-channel Increment"); break;
+      case  0x172: Serial.print(" - Sub-channel Decrement"); break;
+      case  0x173: Serial.print(" - Alternate Audio Increment"); break;
+      case  0x174: Serial.print(" - Alternate Audio Decrement"); break;
+      case  0x180: Serial.print(" - Application Launch Buttons"); break;
+      case  0x181: Serial.print(" - AL Launch Button Configuration Tool"); break;
+      case  0x182: Serial.print(" - AL Programmable Button Configuration"); break;
+      case  0x183: Serial.print(" - AL Consumer Control Configuration"); break;
+      case  0x184: Serial.print(" - AL Word Processor"); break;
+      case  0x185: Serial.print(" - AL Text Editor"); break;
+      case  0x186: Serial.print(" - AL Spreadsheet"); break;
+      case  0x187: Serial.print(" - AL Graphics Editor"); break;
+      case  0x188: Serial.print(" - AL Presentation App"); break;
+      case  0x189: Serial.print(" - AL Database App"); break;
+      case  0x18A: Serial.print(" - AL Email Reader"); break;
+      case  0x18B: Serial.print(" - AL Newsreader"); break;
+      case  0x18C: Serial.print(" - AL Voicemail"); break;
+      case  0x18D: Serial.print(" - AL Contacts/Address Book"); break;
+      case  0x18E: Serial.print(" - AL Calendar/Schedule"); break;
+      case  0x18F: Serial.print(" - AL Task/Project Manager"); break;
+      case  0x190: Serial.print(" - AL Log/Journal/Timecard"); break;
+      case  0x191: Serial.print(" - AL Checkbook/Finance"); break;
+      case  0x192: Serial.print(" - AL Calculator"); break;
+      case  0x193: Serial.print(" - AL A/V Capture/Playback"); break;
+      case  0x194: Serial.print(" - AL Local Machine Browser"); break;
+      case  0x195: Serial.print(" - AL LAN/WAN Browser"); break;
+      case  0x196: Serial.print(" - AL Internet Browser"); break;
+      case  0x197: Serial.print(" - AL Remote Networking/ISP Connect"); break;
+      case  0x198: Serial.print(" - AL Network Conference"); break;
+      case  0x199: Serial.print(" - AL Network Chat"); break;
+      case  0x19A: Serial.print(" - AL Telephony/Dialer"); break;
+      case  0x19B: Serial.print(" - AL Logon"); break;
+      case  0x19C: Serial.print(" - AL Logoff"); break;
+      case  0x19D: Serial.print(" - AL Logon/Logoff"); break;
+      case  0x19E: Serial.print(" - AL Terminal Lock/Screensaver"); break;
+      case  0x19F: Serial.print(" - AL Control Panel"); break;
+      case  0x1A0: Serial.print(" - AL Command Line Processor/Run"); break;
+      case  0x1A1: Serial.print(" - AL Process/Task Manager"); break;
+      case  0x1A2: Serial.print(" - AL Select Tast/Application"); break;
+      case  0x1A3: Serial.print(" - AL Next Task/Application"); break;
+      case  0x1A4: Serial.print(" - AL Previous Task/Application"); break;
+      case  0x1A5: Serial.print(" - AL Preemptive Halt Task/Application"); break;
+      case  0x200: Serial.print(" - Generic GUI Application Controls"); break;
+      case  0x201: Serial.print(" - AC New"); break;
+      case  0x202: Serial.print(" - AC Open"); break;
+      case  0x203: Serial.print(" - AC Close"); break;
+      case  0x204: Serial.print(" - AC Exit"); break;
+      case  0x205: Serial.print(" - AC Maximize"); break;
+      case  0x206: Serial.print(" - AC Minimize"); break;
+      case  0x207: Serial.print(" - AC Save"); break;
+      case  0x208: Serial.print(" - AC Print"); break;
+      case  0x209: Serial.print(" - AC Properties"); break;
+      case  0x21A: Serial.print(" - AC Undo"); break;
+      case  0x21B: Serial.print(" - AC Copy"); break;
+      case  0x21C: Serial.print(" - AC Cut"); break;
+      case  0x21D: Serial.print(" - AC Paste"); break;
+      case  0x21E: Serial.print(" - AC Select All"); break;
+      case  0x21F: Serial.print(" - AC Find"); break;
+      case  0x220: Serial.print(" - AC Find and Replace"); break;
+      case  0x221: Serial.print(" - AC Search"); break;
+      case  0x222: Serial.print(" - AC Go To"); break;
+      case  0x223: Serial.print(" - AC Home"); break;
+      case  0x224: Serial.print(" - AC Back"); break;
+      case  0x225: Serial.print(" - AC Forward"); break;
+      case  0x226: Serial.print(" - AC Stop"); break;
+      case  0x227: Serial.print(" - AC Refresh"); break;
+      case  0x228: Serial.print(" - AC Previous Link"); break;
+      case  0x229: Serial.print(" - AC Next Link"); break;
+      case  0x22A: Serial.print(" - AC Bookmarks"); break;
+      case  0x22B: Serial.print(" - AC History"); break;
+      case  0x22C: Serial.print(" - AC Subscriptions"); break;
+      case  0x22D: Serial.print(" - AC Zoom In"); break;
+      case  0x22E: Serial.print(" - AC Zoom Out"); break;
+      case  0x22F: Serial.print(" - AC Zoom"); break;
+      case  0x230: Serial.print(" - AC Full Screen View"); break;
+      case  0x231: Serial.print(" - AC Normal View"); break;
+      case  0x232: Serial.print(" - AC View Toggle"); break;
+      case  0x233: Serial.print(" - AC Scroll Up"); break;
+      case  0x234: Serial.print(" - AC Scroll Down"); break;
+      case  0x235: Serial.print(" - AC Scroll"); break;
+      case  0x236: Serial.print(" - AC Pan Left"); break;
+      case  0x237: Serial.print(" - AC Pan Right"); break;
+      case  0x238: Serial.print(" - AC Pan"); break;
+      case  0x239: Serial.print(" - AC New Window"); break;
+      case  0x23A: Serial.print(" - AC Tile Horizontally"); break;
+      case  0x23B: Serial.print(" - AC Tile Vertically"); break;
+      case  0x23C: Serial.print(" - AC Format"); break;
+
+    }
+  }
+  Serial.println();
+}
+
+void OnHIDExtrasRelease(uint32_t top, uint16_t key)
+{
+  Serial.print("HID (");
+  Serial.print(top, HEX);
+  Serial.print(") key release:");
+  Serial.println(key, HEX);
+}
+
+//=============================================================================
+// UpdateActiveDeviceInfo
+//=============================================================================
+void UpdateActiveDeviceInfo() {
+  for (uint8_t i = 0; i < CNT_DEVICES; i++) {
+    if (*drivers[i] != driver_active[i]) {
+      if (driver_active[i]) {
+        Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
+        driver_active[i] = false;
+      } else {
+        Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
+        driver_active[i] = true;
+
+        const uint8_t *psz = drivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = drivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = drivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+
+        if (drivers[i] == &bluet) {
+          const uint8_t *bdaddr = bluet.myBDAddr();
+          // remember it...
+          Serial.printf("  BDADDR: %x:%x:%x:%x:%x:%x\n", bdaddr[0], bdaddr[1], bdaddr[2], bdaddr[3], bdaddr[4], bdaddr[5]);
+        }
+      }
+    }
+  }
+  for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {
+    if (*hiddrivers[i] != hid_driver_active[i]) {
+      if (hid_driver_active[i]) {
+        Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
+        hid_driver_active[i] = false;
+      } else {
+        Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
+        hid_driver_active[i] = true;
+
+        const uint8_t *psz = hiddrivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = hiddrivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = hiddrivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+      }
+    }
+  }
+  // Then Bluetooth devices
+  for (uint8_t i = 0; i < CNT_BTHIDDEVICES; i++) {
+    if (*bthiddrivers[i] != bthid_driver_active[i]) {
+      if (bthid_driver_active[i]) {
+        Serial.printf("*** BTHID Device %s - disconnected ***\n", bthid_driver_names[i]);
+        bthid_driver_active[i] = false;
+      } else {
+        Serial.printf("*** BTHID Device %s - connected ***\n", bthid_driver_names[i]); Serial.flush();
+        bthid_driver_active[i] = true;
+        #if 0
+
+        const uint8_t *psz = bthiddrivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = bthiddrivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = bthiddrivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+    #endif  
+      }
+    }
+  }
+}

examples/USBHost_viewer/USBHost_viewer.ino

 //=============================================================================
-// Simple test viewer app for several of the USB devices on ili9341 display
+// Simple test viewer app for several of the USB devices on a display
+// This sketch as well as all others in this library will only work with the
+// Teensy 3.6 and the T4.x boards.
 //
-// Currently requires the libraries
+// The default display is the ILI9341, which uses the ILI9341_t3n library,
+// which is located at:
 //    ili9341_t3n that can be located: https://github.com/KurtE/ILI9341_t3n
-//    spin: https://github.com/KurtE/SPIN
 //
-// Teensy 3.6 Pins
+// Alternate display ST7735 or ST7789 using the ST7735_t3 library which comes
+// with Teensyduino.
+//
+// Default pins
 //   8 = RST
 //   9 = D/C
 //  10 = CS
 //
-// Teensy 4.0 Beta Pins
-//  23 = RST (Marked MCLK on T4 beta breakout)
-//  10 = CS (Marked CS)
-//  9 = DC  (Marked MEMCS)
-//
 // This example is in the public domain
 //=============================================================================
+//#define USE_ST77XX // define this if you wish to use one of these displays.
 
 #include "USBHost_t36.h"
+
+#ifdef USE_ST77XX
+#include <ST7735_t3.h>
+#include <st7735_t3_font_Arial.h>
+#include <ST7789_t3.h>
+#define BLACK ST77XX_BLACK
+#define WHITE ST77XX_WHITE
+#define YELLOW ST77XX_YELLOW
+#define GREEN ST77XX_GREEN
+#define RED ST77XX_RED
+#else
+#include <ILI9341_t3n.h>
 #include <ili9341_t3n_font_Arial.h>
-#define TEENSY64
+#define BLACK ILI9341_BLACK
+#define WHITE ILI9341_WHITE
+#define YELLOW ILI9341_YELLOW
+#define GREEN ILI9341_GREEN
+#define RED ILI9341_RED
+#endif
 
 //=============================================================================
 // Connection configuration of ILI9341 LCD TFT
 //=============================================================================
-DMAMEM uint16_t frame_buffer[ILI9341_TFTWIDTH * ILI9341_TFTHEIGHT];
-#if defined(__MK66FX1M0__) && !defined(TEENSY64)
-#define TFT_RST 255
-#define TFT_DC 20
-#define TFT_CS 21
-ILI9341_t3n tft = ILI9341_t3n(TFT_CS, TFT_DC, TFT_RST);
-#elif defined(__IMXRT1052__) || defined(__IMXRT1062__)
-// On Teensy 4 beta with Paul's breakout out:
-// Using pins (MOSI, MISO, SCK which are labeled on Audio board breakout location
-// which are not in the Normal processor positions
-// Also DC=10(CS), CS=9(BCLK) and RST 23(MCLK)
-#define TFT_RST 23
+#define TFT_RST 8
 #define TFT_DC 9
 #define TFT_CS 10
-ILI9341_t3n tft = ILI9341_t3n(TFT_CS, TFT_DC, TFT_RST);
-#elif defined(TEENSY64)
-#define TFT_RST 255
-#define TFT_DC 20
-#define TFT_CS 21
-#define TFT_SCK 14
-#define TFT_MISO 39
-#define TFT_MOSI 28
-ILI9341_t3n tft = ILI9341_t3n(TFT_CS, TFT_DC, TFT_RST, TFT_MOSI, TFT_SCK, TFT_MISO);
+
+#ifdef USE_ST77XX
+// define which one you are using
+//DMAMEM uint16_t frame_buffer[ILI9341_TFTWIDTH * ILI9341_TFTHEIGHT];
+// Note there are other options, like defining MOSI, SCK...
+//ST7735_t3 tft = ST7735_t3(cs, dc, rst);
+// For 1.54" TFT with ST7789
+ST7789_t3 tft = ST7789_t3(TFT_CS, TFT_DC, TFT_RST);
 #else
-#error "This example App will only work with Teensy 3.6 or Teensy 4."
+ILI9341_t3n tft = ILI9341_t3n(TFT_CS, TFT_DC, TFT_RST);
 #endif
-
 //=============================================================================
 // USB Host Ojbects
 //=============================================================================
 MouseController mouse(myusb);
 DigitizerController tablet(myusb);
 JoystickController joystick(myusb);
-BluetoothController bluet(myusb, true, "0000");   // Version does pairing to device
-//BluetoothController bluet(myusb);   // version assumes it already was paired
+//BluetoothController bluet(myusb, true, "0000");   // Version does pairing to device
+BluetoothController bluet(myusb);   // version assumes it already was paired
 RawHIDController rawhid2(myusb);
 
 // Lets only include in the lists The most top level type devices we wish to show information for.
 //=============================================================================
 
 // Save away values for buttons, x, y, wheel, wheelh
-int buttons_cur = 0;
+uint32_t buttons_cur = 0;
 int x_cur = 0,
     y_cur = 0,
     z_cur = 0;
   // device like wireless mouse and keyboard this can cause mouse problems.
   //keyboard1.forceBootProtocol();
 
+#ifdef USE_ST77XX
+  // Only uncomment one of these init options.
+  // ST7735 - More options mentioned in examples for st7735_t3 library
+  //tft.initR(INITR_BLACKTAB); // if you're using a 1.8" TFT 128x160 displays
+  //tft.initR(INITR_144GREENTAB); // if you're using a 1.44" TFT (128x128)
+  //tft.initR(INITR_MINI160x80);  //if you're using a .96" TFT(160x80)
+
+  // ST7789
+  tft.init(240, 240);           // initialize a ST7789 chip, 240x240 pixels
+  //tft.init(240, 320);           // Init ST7789 2.0" 320x240
+  //tft.init(135, 240);             // Init ST7789 1.4" 135x240
+  //tft.init(240, 240, SPI_MODE2);    // clones Init ST7789 240x240 no CS
+#else
   tft.begin();
+#endif
   // explicitly set the frame buffer
-  tft.setFrameBuffer(frame_buffer);
+  //  tft.setFrameBuffer(frame_buffer);
   delay(100);
   tft.setRotation(3); // 180
   delay(100);
 
-  tft.fillScreen(ILI9341_BLACK);
-  tft.setTextColor(ILI9341_YELLOW);
+  tft.fillScreen(BLACK);
+  tft.setTextColor(YELLOW);
   tft.setTextSize(2);
   tft.println("Waiting for Device...");
   tft.useFrameBuffer(true);
         new_device_detected = true;
         Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
         driver_active[i] = true;
-        tft.fillScreen(ILI9341_BLACK);  // clear the screen.
+        tft.fillScreen(BLACK);  // clear the screen.
         tft.setCursor(0, 0);
-        tft.setTextColor(ILI9341_YELLOW);
+        tft.setTextColor(YELLOW);
         tft.setFont(Arial_12);
         tft.printf("Device %s %x:%x\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
 
         new_device_detected = true;
         Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
         hid_driver_active[i] = true;
-        tft.fillScreen(ILI9341_BLACK);  // clear the screen.
+        tft.fillScreen(BLACK);  // clear the screen.
         tft.setCursor(0, 0);
-        tft.setTextColor(ILI9341_YELLOW);
+        tft.setTextColor(YELLOW);
         tft.setFont(Arial_12);
         tft.printf("HID Device %s %x:%x\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
 
         new_device_detected = true;
         Serial.printf("*** BTHID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
         bthid_driver_active[i] = true;
-        tft.fillScreen(ILI9341_BLACK);  // clear the screen.
+        tft.fillScreen(BLACK);  // clear the screen.
         tft.setCursor(0, 0);
-        tft.setTextColor(ILI9341_YELLOW);
+        tft.setTextColor(YELLOW);
         tft.setFont(Arial_12);
         tft.printf("Bluetooth Device %s %x:%x\n", bthid_driver_names[i], bthiddrivers[i]->idVendor(), bthiddrivers[i]->idProduct());
 
       // Lets display the titles.
       int16_t x;
       tft.getCursor(&x, &y_position_after_device_info);
-      tft.setTextColor(ILI9341_YELLOW);
+      tft.setTextColor(YELLOW);
       tft.printf("Buttons:\nX:\nY:\nWheel:\nWheel H:\nAxis:");
       new_device_detected = false;
     }
 #define TABLET_DATA_X 100
       int16_t x, y2;
       unsigned char line_space = Arial_12.line_space;
-      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
+      tft.setTextColor(WHITE, BLACK);
       //tft.setTextDatum(BR_DATUM);
       int16_t y = y_position_after_device_info;
       tft.setCursor(TABLET_DATA_X, y);
       tft.printf("%d(%x)", buttons_cur, buttons_cur);
       tft.getCursor(&x, &y2);
-      tft.fillRect(x, y, 320, line_space, ILI9341_BLACK);
+      tft.fillRect(x, y, 320, line_space, BLACK);
 
       y += line_space; OutputNumberField(TABLET_DATA_X, y, x_cur, 320);
       y += line_space; OutputNumberField(TABLET_DATA_X, y, y_cur, 320);
   int16_t x2, y2;
   tft.setCursor(x, y);
   tft.print(val, DEC); tft.getCursor(&x2, &y2);
-  tft.fillRect(x2, y, field_width - (x2 - x), Arial_12.line_space, ILI9341_BLACK);
+  tft.fillRect(x2, y, field_width - (x2 - x), Arial_12.line_space, BLACK);
 }
 
 //=============================================================================
       // Lets display the titles.
       int16_t x;
       tft.getCursor(&x, &y_position_after_device_info);
-      tft.setTextColor(ILI9341_YELLOW);
+      tft.setTextColor(YELLOW);
       tft.printf("Buttons:\nX:\nY:\nWheel:\nWheel H:");
       new_device_detected = false;
     }
 #define MOUSE_DATA_X 100
       int16_t x, y2;
       unsigned char line_space = Arial_12.line_space;
-      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
+      tft.setTextColor(WHITE, BLACK);
       //tft.setTextDatum(BR_DATUM);
       int16_t y = y_position_after_device_info;
       tft.setCursor(TABLET_DATA_X, y);
       tft.printf("%d(%x)", buttons_cur, buttons_cur);
       tft.getCursor(&x, &y2);
-      tft.fillRect(x, y, 320, line_space, ILI9341_BLACK);
+      tft.fillRect(x, y, 320, line_space, BLACK);
 
       y += line_space; OutputNumberField(MOUSE_DATA_X, y, x_cur, 320);
       y += line_space; OutputNumberField(MOUSE_DATA_X, y, y_cur, 320);
 
       case JoystickController::XBOXONE:
       case JoystickController::XBOX360:
+      case JoystickController::SWITCH:
         ltv = joystick.getAxis(4);
         rtv = joystick.getAxis(5);
         if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
 // TFT_joystick
 //=============================================================================
 void tft_JoystickData() {
-    if (new_device_detected) {
-      // Lets display the titles.
-      int16_t x;
-      tft.getCursor(&x, &y_position_after_device_info);
-      tft.setTextColor(ILI9341_YELLOW);
-      tft.printf("Buttons:\nX:\nY:\nX2\nY2(Z):\nL1:\nR1:\nHAT:");
-      new_device_detected = false;
-    }
+  if (new_device_detected) {
+    // Lets display the titles.
+    int16_t x;
+    tft.getCursor(&x, &y_position_after_device_info);
+    tft.setTextColor(YELLOW);
+    tft.printf("Buttons:\nX:\nY:\nX2\nY2(Z):\nL1:\nR1:\nHAT:");
+    new_device_detected = false;
+  }
 
-    bool something_changed = false;
-    if (buttons != buttons_prev) {  //buttons
-      something_changed = true;
-    }
-    if (user_axis[0] != x_cur) {  //xL
-      x_cur = user_axis[0];
-      something_changed = true;
-    }
-    if (user_axis[1] != y_cur) {  //yL
-      y_cur = user_axis[1];
-      something_changed = true;
-    }
-    if (user_axis[9] != wheel_cur) {  //Hat
-      wheel_cur = user_axis[9];
-      something_changed = true;
-    }
+  bool something_changed = false;
+  if (buttons != buttons_prev) {  //buttons
+    something_changed = true;
+  }
+  if (user_axis[0] != x_cur) {  //xL
+    x_cur = user_axis[0];
+    something_changed = true;
+  }
+  if (user_axis[1] != y_cur) {  //yL
+    y_cur = user_axis[1];
+    something_changed = true;
+  }
+  if (user_axis[9] != wheel_cur) {  //Hat
+    wheel_cur = user_axis[9];
+    something_changed = true;
+  }
   //Second Axis
-    if (user_axis[2] != x2_cur) {  //xR
-      x2_cur = user_axis[2];
-      something_changed = true;
-    }
-    if (user_axis[5] != y2_cur) {  //yR or z-axis
-      y2_cur = user_axis[5];
-      something_changed = true;
-    } 
-    //Rumble Axis
-     switch (joystick.joystickType()) {
-        case JoystickController::XBOXONE:
-        case JoystickController::XBOX360:
-        case JoystickController::PS4:
-          if (user_axis[3] != L1_cur) {  //xR
-            L1_cur = user_axis[3];
-            something_changed = true;
-          }
-          if (user_axis[4] != R1_cur) {  //yR or z-axis
-            R1_cur = user_axis[4];
-            something_changed = true;
-          }
-          break;
-      case JoystickController::PS3:
-          if (user_axis[18] != L1_cur) {  //xR
-            L1_cur = user_axis[18];
-            something_changed = true;
-          }
-          if (user_axis[19] != R1_cur) {  //yR or z-axis
-            R1_cur = user_axis[19];
-            something_changed = true;
-          }
-          break;
-     }
-    if (something_changed) {
-#define MOUSE_DATA_X 100
-      int16_t x, y2;
-      unsigned char line_space = Arial_12.line_space;
-      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
-      //tft.setTextDatum(BR_DATUM);
-      int16_t y = y_position_after_device_info;
-      tft.setCursor(TABLET_DATA_X, y);
-      tft.printf("%d(%x)", buttons, buttons);
-      tft.getCursor(&x, &y2);
-      tft.fillRect(x, y, 320, line_space, ILI9341_BLACK);
-
-      y += line_space; OutputNumberField(MOUSE_DATA_X, y, x_cur, 320);  //x
-      y += line_space; OutputNumberField(MOUSE_DATA_X, y, y_cur, 320);  //y
-      y += line_space; OutputNumberField(MOUSE_DATA_X, y, x2_cur, 320); //x2(z)
-      y += line_space; OutputNumberField(MOUSE_DATA_X, y, y2_cur, 320); //y2
-      switch (joystick.joystickType()) {
-        case JoystickController::PS4:
-        case JoystickController::PS3:
-        case JoystickController::XBOXONE:
-        case JoystickController::XBOX360:
-          y += line_space; OutputNumberField(MOUSE_DATA_X, y, L1_cur, 320); 
-          y += line_space; OutputNumberField(MOUSE_DATA_X, y, R1_cur, 320);
-          break;
-        default:
-          y += line_space; OutputNumberField(MOUSE_DATA_X, y, 0, 320); 
-          y += line_space; OutputNumberField(MOUSE_DATA_X, y, 0, 320);
-          y += line_space; OutputNumberField(MOUSE_DATA_X, y, wheel_cur, 320); //hat
-          break;
+  if (user_axis[2] != x2_cur) {  //xR
+    x2_cur = user_axis[2];
+    something_changed = true;
+  }
+  if (user_axis[5] != y2_cur) {  //yR or z-axis
+    y2_cur = user_axis[5];
+    something_changed = true;
+  }
+  //Rumble Axis
+  switch (joystick.joystickType()) {
+    case JoystickController::XBOXONE:
+    case JoystickController::XBOX360:
+    case JoystickController::PS4:
+      if (user_axis[3] != L1_cur) {  //xR
+        L1_cur = user_axis[3];
+        something_changed = true;
       }
-      tft.updateScreen(); // update the screen now
+      if (user_axis[4] != R1_cur) {  //yR or z-axis
+        R1_cur = user_axis[4];
+        something_changed = true;
+      }
+      break;
+    case JoystickController::PS3:
+      if (user_axis[18] != L1_cur) {  //xR
+        L1_cur = user_axis[18];
+        something_changed = true;
+      }
+      if (user_axis[19] != R1_cur) {  //yR or z-axis
+        R1_cur = user_axis[19];
+        something_changed = true;
+      }
+      break;
+    default: // All others
+      break;
+  }
+  if (something_changed) {
+#define MOUSE_DATA_X 100
+    int16_t x, y2;
+    unsigned char line_space = Arial_12.line_space;
+    tft.setTextColor(WHITE, BLACK);
+    //tft.setTextDatum(BR_DATUM);
+    int16_t y = y_position_after_device_info;
+    tft.setCursor(TABLET_DATA_X, y);
+    tft.printf("%d(%x)", buttons, buttons);
+    tft.getCursor(&x, &y2);
+    tft.fillRect(x, y, 320, line_space, BLACK);
+
+    y += line_space; OutputNumberField(MOUSE_DATA_X, y, x_cur, 320);  //x
+    y += line_space; OutputNumberField(MOUSE_DATA_X, y, y_cur, 320);  //y
+    y += line_space; OutputNumberField(MOUSE_DATA_X, y, x2_cur, 320); //x2(z)
+    y += line_space; OutputNumberField(MOUSE_DATA_X, y, y2_cur, 320); //y2
+    switch (joystick.joystickType()) {
+      case JoystickController::PS4:
+      case JoystickController::PS3:
+      case JoystickController::XBOXONE:
+      case JoystickController::XBOX360:
+        y += line_space; OutputNumberField(MOUSE_DATA_X, y, L1_cur, 320);
+        y += line_space; OutputNumberField(MOUSE_DATA_X, y, R1_cur, 320);
+        break;
+      default:
+        y += line_space; OutputNumberField(MOUSE_DATA_X, y, 0, 320);
+        y += line_space; OutputNumberField(MOUSE_DATA_X, y, 0, 320);
+        y += line_space; OutputNumberField(MOUSE_DATA_X, y, wheel_cur, 320); //hat
+        break;
     }
+    tft.updateScreen(); // update the screen now
+  }
 }
 
 //=============================================================================
     }
 
     bool something_changed = false;
-    if (user_axis[3] != buttons_cur) {
-      buttons_cur = user_axis[3];
+    if ((uint32_t)user_axis[3] != buttons_cur) {
+      buttons_cur = (uint32_t)user_axis[3];
       something_changed = true;
     }
     if (user_axis[4] != x_cur) {
       something_changed = true;
     }
     if (something_changed) {
-      tft.fillRect(45, 197, 240, 20, ILI9341_RED);
+      tft.fillRect(45, 197, 240, 20, RED);
       tft.drawNumber(buttons_cur, 50, 200);
       tft.drawNumber(x_cur, 100, 200);
       tft.drawNumber(y_cur, 150, 200);
   if (BT == 0) {
     tft.enableScroll();
     tft.setScrollTextArea(20, 70, 280, 140);
-    tft.setScrollBackgroundColor(ILI9341_GREEN);
+    tft.setScrollBackgroundColor(GREEN);
     tft.setFont(Arial_11);
-    tft.setTextColor(ILI9341_BLACK);
+    tft.setTextColor(BLACK);
     tft.setCursor(20, 70);
     BT = 1;
   }

joystick.cpp

 JoystickController::product_vendor_mapping_t JoystickController::pid_vid_mapping[] = {
 	{ 0x045e, 0x02ea, XBOXONE, false },{ 0x045e, 0x02dd, XBOXONE, false },
 	{ 0x045e, 0x0719, XBOX360, false},
+	{ 0x045e, 0x028E, SWITCH, false},  // Switch? 
 	{ 0x054C, 0x0268, PS3, true}, 
 	{ 0x054C, 0x042F, PS3, true},	// PS3 Navigation controller
 	{ 0x054C, 0x03D5, PS3_MOTION, true},	// PS3 Motion controller
 }
 
 
+static uint8_t rumble_counter = 0; 
+
 bool JoystickController::setRumble(uint8_t lValue, uint8_t rValue, uint8_t timeout)
 {
 	// Need to know which joystick we are on.  Start off with XBox support - maybe need to add some enum value for the known
 				println("XBox360 rumble transfer fail");
 			}
 			return true;
+		case SWITCH:
+			memset(txbuf_, 0, 10);	// make sure it is cleared out
+			txbuf_[0] = 0x80;
+			txbuf_[1] = 0x92;
+			txbuf_[3] = 0x31;
+			txbuf_[8] = 0x10;	// Command
+
+			// Now add in subcommand data:
+			// Probably do this better soon
+			txbuf_[9+0] = rumble_counter++;	//
+			txbuf_[9+1] = 0x80;
+    		txbuf_[9+2] = 0x00;
+    		txbuf_[9+3] = 0x40;
+    		txbuf_[9+4] = 0x40;
+    		txbuf_[9+5] = 0x80;
+    		txbuf_[9+6] = 0x00;
+    		txbuf_[9+7] = 0x40;
+    		txbuf_[9+8] = 0x40;
+
+    		if (lValue != 0) {
+	    		txbuf_[9+5] = 0x08;
+	    		txbuf_[9+6] = lValue;
+    		} else if (rValue != 0) {
+	    		txbuf_[9+5] = 0x10;
+	    		txbuf_[9+6] = rValue;
+    		}
+
+			if (!queue_Data_Transfer(txpipe_, txbuf_, 18, this)) {
+				println("switch rumble transfer fail");
+				Serial.printf("Switch Rumble transfer fail\n");
+			}
+			return true;
 	} 
 	return false;
 }
 {
 	// Need to know which joystick we are on.  Start off with XBox support - maybe need to add some enum value for the known
 	// joystick types. 
+	Serial.printf("::setLEDS(%x %x %x)\n", lr, lg, lb);
 	if ((leds_[0] != lr) || (leds_[1] != lg) || (leds_[2] != lb)) {
 		leds_[0] = lr;
 		leds_[1] = lg;
 					println("XBox360 set leds fail");
 				}
 				return true;
+			case SWITCH:
+				memset(txbuf_, 0, 10);	// make sure it is cleared out
+				txbuf_[0] = 0x80;
+				txbuf_[1] = 0x92;
+				txbuf_[3] = 0x31;
+				txbuf_[8] = 0x01;	// Command
+
+				// Now add in subcommand data:
+				// Probably do this better soon
+				txbuf_[9+0] = rumble_counter++;	//
+				txbuf_[9+1] = 0x00;
+        		txbuf_[9+2] = 0x01;
+        		txbuf_[9+3] = 0x40;
+        		txbuf_[9+4] = 0x40;
+        		txbuf_[9+5] = 0x00;
+        		txbuf_[9+6] = 0x01;
+        		txbuf_[9+7] = 0x40;
+        		txbuf_[9+8] = 0x40;
+
+        		txbuf_[9+9] = 0x30;	// LED Command
+        		txbuf_[9+10] = lr;
+       			println("Switch set leds: driver? ", (uint32_t)driver_, HEX);
+				print_hexbytes((uint8_t*)txbuf_, 20);
+				if (!queue_Data_Transfer(txpipe_, txbuf_, 20, this)) {
+					println("switch set leds fail");
+				}
+
 			case XBOXONE:
 			default:
 				return false;
 	return false;
 }
 
+
 bool JoystickController::transmitPS4UserFeedbackMsg() {
 	if (driver_)  {
 		uint8_t packet[32];
 
 static  uint8_t xboxone_start_input[] = {0x05, 0x20, 0x00, 0x01, 0x00};
 static  uint8_t xbox360w_inquire_present[] = {0x08, 0x00, 0x0F, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-
+//static  uint8_t switch_start_input[] = {0x19, 0x01, 0x03, 0x07, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
+static  uint8_t switch_start_input[] = {0x80, 0x02};
 bool JoystickController::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)
 {
 	println("JoystickController claim this=", (uint32_t)this, HEX);
   	// 29 30  1  2  3  4  5  6  7  8  9 40 41 42
 	// 07 05 81 03 20 00 01 07 05 01 03 20 00 08 
 
+	// Switch
+	// 09 04 00 00 02 FF 5D 01 00 
+	// 10 21 10 01 01 24 81 14 03 00 03 13 02 00 03 00 
+	// 07 05 81 03 20 00 08 
+	// 07 05 02 03 20 00 08 
+
+
+
 	if (len < 9+7+7) return false;
 
 	// Some common stuff for both XBoxs
 		if (descriptors[descriptor_index] != 0x14) return false; // only support specific versions...
 		descriptor_index += descriptors[descriptor_index]; // XBox360w ignore this unknown setup...
 	}	
-	while (count_end_points-- && ((rx_ep_ == 0) || txep == 0)) {
-		if (descriptors[descriptor_index] != 7) return false; // length 7
-		if (descriptors[descriptor_index+1] != 5) return false; // ep desc
-		if ((descriptors[descriptor_index+3] == 3) 				// Type 3...
-			&& (descriptors[descriptor_index+4] <= 64)
-			&& (descriptors[descriptor_index+5] == 0)) {
-			// have a bulk EP size 
-			if (descriptors[descriptor_index+2] & 0x80 ) {
-				rx_ep_ = descriptors[descriptor_index+2];
-				rx_size_ = descriptors[descriptor_index+4];
-				rx_interval = descriptors[descriptor_index+6];
-			} else {
-				txep = descriptors[descriptor_index+2]; 
-				tx_size_ = descriptors[descriptor_index+4];
-				tx_interval = descriptors[descriptor_index+6];
+	while ((rx_ep_ == 0) || txep == 0) {
+		print("  Index:", descriptor_index, DEC);
+
+		if (descriptor_index >= len) return false;  		// we ran off the end and did not get end points
+		// see if the next data is an end point descript
+		if ((descriptors[descriptor_index] == 7) && (descriptors[descriptor_index+1] == 5)) {
+			if ((descriptors[descriptor_index+3] == 3) 				// Type 3...
+				&& (descriptors[descriptor_index+4] <= 64)
+				&& (descriptors[descriptor_index+5] == 0)) {
+				// have a bulk EP size 
+				if (descriptors[descriptor_index+2] & 0x80 ) {
+					rx_ep_ = descriptors[descriptor_index+2];
+					rx_size_ = descriptors[descriptor_index+4];
+					rx_interval = descriptors[descriptor_index+6];
+				} else {
+					txep = descriptors[descriptor_index+2]; 
+					tx_size_ = descriptors[descriptor_index+4];
+					tx_interval = descriptors[descriptor_index+6];
+				}
 			}
 		}
-		descriptor_index += 7;  // setup to look at next one...
+		descriptor_index += descriptors[descriptor_index];  // setup to look at next one...
 	}
 	if ((rx_ep_ == 0) || (txep == 0)) return false; // did not find two end points.
 	print("JoystickController, rx_ep_=", rx_ep_ & 15);
 	} else if (jtype == XBOX360) {
 		queue_Data_Transfer(txpipe_, xbox360w_inquire_present, sizeof(xbox360w_inquire_present), this);
 		connected_ = 0;		// remember that hardware is actually connected...
+	} else if (jtype == SWITCH) {
+		queue_Data_Transfer(txpipe_, switch_start_input, sizeof(switch_start_input), this);
+		connected_ = true;		// remember that hardware is actually connected...
 	}
 	memset(axis, 0, sizeof(axis));	// clear out any data. 
 	joystickType_ = jtype;		// remember we are an XBox One. 
 	int16_t	axis[4];
 } xbox360data_t;
 
+typedef struct {
+	uint8_t state;
+	uint8_t id_or_type;
+	// From online references button order: 
+	//     sync, dummy, start, back, a, b, x, y
+	//     dpad up, down left, right
+	//	   lb, rb, left stick, right stick
+	// Axis: 
+	//     lt, rt, lx, ly, rx, ry
+	//
+	uint8_t buttons_h; 
+	uint8_t buttons_l; 
+	uint8_t lt;
+	uint8_t rt;
+	int16_t	axis[4];
+} switchdataUSB_t;
+
 static const uint8_t xbox_axis_order_mapping[] = {3, 4, 0, 1, 2, 5};
 
 void JoystickController::rx_data(const Transfer_t *transfer)
 {
+	#ifdef  DEBUG_JOYSTICK
 	print("JoystickController::rx_data (", joystickType_, DEC);
 	print("): ");
 	print_hexbytes((uint8_t*)transfer->buffer, transfer->length);
+	#endif
 
 	if (joystickType_ == XBOXONE) {
 		// Process XBOX One data
 
 			if (anychange) joystickEvent = true;
 		}
+	} else if (joystickType_ == SWITCH) {
+		switchdataUSB_t  *switchd = (switchdataUSB_t *)transfer->buffer;
+		uint16_t cur_buttons = (switchd->buttons_h << 8) | switchd->buttons_l;
+        if (buttons != cur_buttons) {
+        	buttons = cur_buttons;
+        	anychange = true;
+        }
+		axis_mask_ = 0x3f;	
+		axis_changed_mask_ = 0;	// assume none for now
+
+		for (uint8_t i = 0; i < 4; i++) {
+			if (axis[i] != switchd->axis[i]) {
+				axis[i] = switchd->axis[i];
+				axis_changed_mask_ |= (1 << i);
+				anychange = true;
+			}
+		}
+		// the two triggers show up as 4 and 5
+		if (axis[4] != switchd->lt) {
+			axis[4] = switchd->lt;
+			axis_changed_mask_ |= (1 << 4);
+			anychange = true;
+		}
+
+		if (axis[5] != switchd->rt) {
+			axis[5] = switchd->rt;
+			axis_changed_mask_ |= (1 << 5);
+			anychange = true;
+		}
+
+		if (anychange) joystickEvent = true;
 	}
 
 	queue_Data_Transfer(rxpipe_, rxbuf_, rx_size_, this);
 
 bool JoystickController::claim_bluetooth(BluetoothController *driver, uint32_t bluetooth_class, uint8_t *remoteName) 
 {
+	// If we are already in use than don't grab another one.  Likewise don't grab if it is used as USB or HID object
+	if (btdevice && (btdevice != (Device_t*)driver)) return false;
+	if (mydevice != NULL) return false;
+	if (device != nullptr) return false;
+
 	if ((((bluetooth_class & 0xff00) == 0x2500) || (((bluetooth_class & 0xff00) == 0x500))) && ((bluetooth_class & 0x3C) == 0x08)) {
 		DBGPrintf("JoystickController::claim_bluetooth TRUE\n");
 		btdriver_ = driver;
 		//set buttons for last 4bits in the axis[5]
 		tmp_data[5] = tmp_data[5] >> 4;
 		
-	
+		// Lets try mapping the DPAD buttons to high bits 
+		//								              up    up/right  right    R DN      DOWN    L DN      Left    LUP
+		static const uint32_t dpad_to_buttons[] = {0x10000, 0x30000, 0x20000, 0x60000, 0x40000, 0xC0000, 0x80000, 0x90000};
+		
 		// Quick and dirty hack to match PS4 HID data
-		uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ; 
+		uint32_t cur_buttons = ((uint32_t)tmp_data[7] << 12) | (((uint32_t)tmp_data[6]*0x10)) | ((uint16_t)tmp_data[5] ) ;
+
+		if (tmp_data[10] < 8) cur_buttons |= dpad_to_buttons[tmp_data[10]];
+
 		if (cur_buttons != buttons) {
 			buttons = cur_buttons;
 			joystickEvent = true;	// something changed.

keyboard.cpp

 bool KeyboardController::claim_bluetooth(BluetoothController *driver, uint32_t bluetooth_class, uint8_t *remoteName) 
 {
 	USBHDBGSerial.printf("Keyboard Controller::claim_bluetooth - Class %x\n", bluetooth_class);
+	// If we are already in use than don't grab another one.  Likewise don't grab if it is used as USB or HID object
+	if (btdevice && (btdevice != (Device_t*)driver)) return false;
+	if (mydevice != NULL) return false;
+	if (device != nullptr) return false;
+
 	if ((((bluetooth_class & 0xff00) == 0x2500) || (((bluetooth_class & 0xff00) == 0x500))) && (bluetooth_class & 0x40)) {
 		if (remoteName && (strncmp((const char *)remoteName, "PLAYSTATION(R)3", 15) == 0)) {
 			USBHDBGSerial.printf("KeyboardController::claim_bluetooth Reject PS3 hack\n");
+			btdevice = nullptr;	// remember this way 
+
 			return false;
 		}
 		USBHDBGSerial.printf("KeyboardController::claim_bluetooth TRUE\n");
-		//btdevice = driver;
+		btdevice = (Device_t*)driver;	// remember this way 
 		return true;
 	}
 	return false;
 
 void KeyboardController::release_bluetooth() 
 {
-	//btdevice = nullptr;
+	btdevice = nullptr;
 }
 
 //*****************************************************************************

keywords.txt

 PS4	LITERAL1
 XBOXONE	LITERAL1
 XBOX360	LITERAL1
-
+SWITCH	LITERAL1
 # USBSerial
 USBHOST_SERIAL_7E1	LITERAL1
 USBHOST_SERIAL_7O1	LITERAL1

mouse.cpp

 {
 	// How to handle combo devices? 
 	USBHDBGSerial.printf("MouseController Controller::claim_bluetooth - Class %x\n", bluetooth_class);
+	// If we are already in use than don't grab another one.  Likewise don't grab if it is used as USB or HID object
+	if (btdevice && (btdevice != (Device_t*)driver)) return false;
+	if (mydevice != NULL) return false;
 	if ((((bluetooth_class & 0xff00) == 0x2500) || (((bluetooth_class & 0xff00) == 0x500))) && (bluetooth_class & 0x80)) {
 		USBHDBGSerial.printf("MouseController::claim_bluetooth TRUE\n");
 		btdevice = (Device_t*)driver;	// remember this way 
 
 void MouseController::release_bluetooth() 
 {
-	//btdevice = nullptr;
+	btdevice = nullptr;
 }