diff --git a/BTD.cpp b/BTD.cpp
index cab5f7f8..de83da95 100755
--- a/BTD.cpp
+++ b/BTD.cpp
@@ -36,18 +36,19 @@ qNextPollTime(0), // Reset NextPollTime
 pollInterval(0),
 bPollEnable(false) // Don't start polling before dongle is connected
 {
-        for (uint8_t i = 0; i < BTD_NUMSERVICES; i++)
+        for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
                 btService[i] = NULL;
 
         Initialize(); // Set all variables, endpoint structs etc. to default values
 
-        if (pUsb) // Register in USB subsystem
+        if(pUsb) // Register in USB subsystem
                 pUsb->RegisterDeviceClass(this); // Set devConfig[] entry
 }
 
 uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -59,7 +60,7 @@ uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nBTD ConfigureDevice"), 0x80);
 #endif
 
-        if (bAddress) { // Check if address has already been assigned to an instance
+        if(bAddress) { // Check if address has already been assigned to an instance
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress in use"), 0x80);
 #endif
@@ -67,14 +68,14 @@ uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         }
 
         p = addrPool.GetUsbDevicePtr(0); // Get pointer to pseudo device with address 0 assigned
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nepinfo is null"), 0x80);
 #endif
@@ -88,23 +89,23 @@ uint8_t BTD::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p->epinfo = oldep_ptr; // Restore p->epinfo
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
         bAddress = addrPool.AllocAddress(parent, false, port); // Allocate new address according to device class
 
-        if (!bAddress) {
+        if(!bAddress) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nOut of address space"), 0x80);
 #endif
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
         }
 
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Extract Max Packet Size from device descriptor
-        epInfo[1].epAddr = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Steal and abuse from epInfo structure to save memory
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0; // Extract Max Packet Size from device descriptor
+        epInfo[1].epAddr = udd->bNumConfigurations; // Steal and abuse from epInfo structure to save memory
 
-        VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
-        PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
+        VID = udd->idVendor;
+        PID = udd->idProduct;
 
         return USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET;
 
@@ -112,7 +113,7 @@ FailGetDevDescr:
 #ifdef DEBUG_USB_HOST
         NotifyFailGetDevDescr(rcode);
 #endif
-        if (rcode != hrJERR)
+        if(rcode != hrJERR)
                 rcode = USB_ERROR_FailGetDevDescr;
         Release();
         return rcode;
@@ -129,7 +130,7 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 #endif
         UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
 
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
@@ -139,7 +140,7 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         delay(300); // Assign new address to the device
 
         rcode = pUsb->setAddr(0, 0, bAddress); // Assign new address to the device
-        if (rcode) {
+        if(rcode) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nsetAddr: "), 0x80);
                 D_PrintHex<uint8_t > (rcode, 0x80);
@@ -155,7 +156,7 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         p->lowspeed = false;
 
         p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
@@ -165,18 +166,18 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         p->lowspeed = lowspeed;
 
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); // Assign epInfo to epinfo pointer - only EP0 is known
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
-        if (VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) {
+        if(VID == PS3_VID && (PID == PS3_PID || PID == PS3NAVIGATION_PID || PID == PS3MOVE_PID)) {
                 delay(100);
                 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1); // We only need the Control endpoint, so we don't have to initialize the other endpoints of device
-                if (rcode)
+                if(rcode)
                         goto FailSetConfDescr;
 
 #ifdef DEBUG_USB_HOST
-                if (PID == PS3_PID || PID == PS3NAVIGATION_PID) {
-                        if (PID == PS3_PID)
+                if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
+                        if(PID == PS3_PID)
                                 Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80);
                         else // It must be a navigation controller
                                 Notify(PSTR("\r\nNavigation Controller Connected"), 0x80);
@@ -184,18 +185,18 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                         Notify(PSTR("\r\nMotion Controller Connected"), 0x80);
 #endif
 
-                if (my_bdaddr[0] == 0x00 && my_bdaddr[1] == 0x00 && my_bdaddr[2] == 0x00 && my_bdaddr[3] == 0x00 && my_bdaddr[4] == 0x00 && my_bdaddr[5] == 0x00) {
+                if(my_bdaddr[0] == 0x00 && my_bdaddr[1] == 0x00 && my_bdaddr[2] == 0x00 && my_bdaddr[3] == 0x00 && my_bdaddr[4] == 0x00 && my_bdaddr[5] == 0x00) {
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nPlease plug in the dongle before trying to pair with the PS3 Controller\r\nor set the Bluetooth address in the constructor of the PS3BT class"), 0x80);
 #endif
                 } else {
-                        if (PID == PS3_PID || PID == PS3NAVIGATION_PID)
+                        if(PID == PS3_PID || PID == PS3NAVIGATION_PID)
                                 setBdaddr(my_bdaddr); // Set internal Bluetooth address
                         else
                                 setMoveBdaddr(my_bdaddr); // Set internal Bluetooth address
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
-                        for (int8_t i = 5; i > 0; i--) {
+                        for(int8_t i = 5; i > 0; i--) {
                                 D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
                                 Notify(PSTR(":"), 0x80);
                         }
@@ -211,31 +212,31 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 // Check if attached device is a Bluetooth dongle and fill endpoint data structure
                 // First interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol
                 // And 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT, not necessarily in this order
-                for (uint8_t i = 0; i < num_of_conf; i++) {
-                        if (VID == IOGEAR_GBU521_VID && PID == IOGEAR_GBU521_PID) {
+                for(uint8_t i = 0; i < num_of_conf; i++) {
+                        if(VID == IOGEAR_GBU521_VID && PID == IOGEAR_GBU521_PID) {
                                 ConfigDescParser<USB_CLASS_VENDOR_SPECIFIC, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this); // Needed for the IOGEAR GBU521
                                 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
                         } else {
                                 ConfigDescParser<USB_CLASS_WIRELESS_CTRL, WI_SUBCLASS_RF, WI_PROTOCOL_BT, CP_MASK_COMPARE_ALL> confDescrParser(this);
                                 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
                         }
-                        if (rcode) // Check error code
+                        if(rcode) // Check error code
                                 goto FailGetConfDescr;
-                        if (bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted
+                        if(bNumEP >= BTD_MAX_ENDPOINTS) // All endpoints extracted
                                 break;
                 }
 
-                if (bNumEP < BTD_MAX_ENDPOINTS)
+                if(bNumEP < BTD_MAX_ENDPOINTS)
                         goto FailUnknownDevice;
 
                 // Assign epInfo to epinfo pointer - this time all 3 endpoins
                 rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo);
-                if (rcode)
+                if(rcode)
                         goto FailSetDevTblEntry;
 
                 // Set Configuration Value
                 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum);
-                if (rcode)
+                if(rcode)
                         goto FailSetConfDescr;
 
                 hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command
@@ -250,13 +251,7 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         }
         return 0; // Successful configuration
 
-        /* diagnostic messages */
-FailGetDevDescr:
-#ifdef DEBUG_USB_HOST
-        NotifyFailGetDevDescr();
-        goto Fail;
-#endif
-
+        /* Diagnostic messages */
 FailSetDevTblEntry:
 #ifdef DEBUG_USB_HOST
         NotifyFailSetDevTblEntry();
@@ -292,14 +287,14 @@ Fail:
 
 void BTD::Initialize() {
         uint8_t i;
-        for (i = 0; i < BTD_MAX_ENDPOINTS; i++) {
+        for(i = 0; i < BTD_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
                 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
         }
-        for (i = 0; i < BTD_NUMSERVICES; i++) {
-                if (btService[i])
+        for(i = 0; i < BTD_NUM_SERVICES; i++) {
+                if(btService[i])
                         btService[i]->Reset(); // Reset all Bluetooth services
         }
 
@@ -307,6 +302,7 @@ void BTD::Initialize() {
         incomingWii = false;
         connectToHIDDevice = false;
         incomingHIDDevice = false;
+        incomingPS4 = false;
         bAddress = 0; // Clear device address
         bNumEP = 1; // Must have to be reset to 1
         qNextPollTime = 0; // Reset next poll time
@@ -320,17 +316,17 @@ void BTD::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto
         //ErrorMessage<uint8_t>(PSTR("Iface Num"),iface);
         //ErrorMessage<uint8_t>(PSTR("Alt.Set"),alt);
 
-        if (alt) // Wrong interface - by BT spec, no alt setting
+        if(alt) // Wrong interface - by BT spec, no alt setting
                 return;
 
         bConfNum = conf;
         uint8_t index;
 
-        if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) { // Interrupt In endpoint found
+        if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) { // Interrupt In endpoint found
                 index = BTD_EVENT_PIPE;
                 epInfo[index].bmNakPower = USB_NAK_NOWAIT;
         } else {
-                if ((pep->bmAttributes & 0x02) == 2) // Bulk endpoint found
+                if((pep->bmAttributes & 0x02) == 2) // Bulk endpoint found
                         index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE;
                 else
                         return;
@@ -342,7 +338,7 @@ void BTD::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto
 #ifdef EXTRADEBUG
         PrintEndpointDescriptor(pep);
 #endif
-        if (pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints
+        if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints
                 pollInterval = pep->bInterval;
         bNumEP++;
 }
@@ -373,40 +369,39 @@ uint8_t BTD::Release() {
 }
 
 uint8_t BTD::Poll() {
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
-        if (qNextPollTime <= millis()) { // Don't poll if shorter than polling interval
+        if((long)(millis() - qNextPollTime) >= 0L) { // Don't poll if shorter than polling interval
                 qNextPollTime = millis() + pollInterval; // Set new poll time
-                HCI_event_task(); // poll the HCI event pipe
-                ACL_event_task(); // start polling the ACL input pipe too, though discard data until connected
+                HCI_event_task(); // Poll the HCI event pipe
+                HCI_task(); // HCI state machine
+                ACL_event_task(); // Poll the ACL input pipe too
         }
         return 0;
 }
 
 void BTD::HCI_event_task() {
-        /* check the event pipe*/
-        uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this
-        uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &MAX_BUFFER_SIZE, hcibuf); // input on endpoint 1
-        if (!rcode || rcode == hrNAK) // Check for errors
-        {
-                switch (hcibuf[0]) //switch on event type
-                {
+        uint16_t length = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this
+        uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &length, hcibuf); // Input on endpoint 1
+
+        if(!rcode || rcode == hrNAK) { // Check for errors
+                switch(hcibuf[0]) { // Switch on event type
                         case EV_COMMAND_COMPLETE:
-                                if (!hcibuf[5]) { // Check if command succeeded
-                                        hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag
-                                        if ((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // parameters from read local version information
+                                if(!hcibuf[5]) { // Check if command succeeded
+                                        hci_set_flag(HCI_FLAG_CMD_COMPLETE); // Set command complete flag
+                                        if((hcibuf[3] == 0x01) && (hcibuf[4] == 0x10)) { // Parameters from read local version information
                                                 hci_version = hcibuf[6]; // Used to check if it supports 2.0+EDR - see http://www.bluetooth.org/Technical/AssignedNumbers/hci.htm
-                                                hci_event_flag |= HCI_FLAG_READ_VERSION;
-                                        } else if ((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // parameters from read local bluetooth address
-                                                for (uint8_t i = 0; i < 6; i++)
+                                                hci_set_flag(HCI_FLAG_READ_VERSION);
+                                        } else if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // Parameters from read local bluetooth address
+                                                for(uint8_t i = 0; i < 6; i++)
                                                         my_bdaddr[i] = hcibuf[6 + i];
-                                                hci_event_flag |= HCI_FLAG_READ_BDADDR;
+                                                hci_set_flag(HCI_FLAG_READ_BDADDR);
                                         }
                                 }
                                 break;
 
                         case EV_COMMAND_STATUS:
-                                if (hcibuf[2]) { // Show status on serial if not OK
+                                if(hcibuf[2]) { // Show status on serial if not OK
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nHCI Command Failed: "), 0x80);
                                         D_PrintHex<uint8_t > (hcibuf[2], 0x80);
@@ -415,10 +410,10 @@ void BTD::HCI_event_task() {
                                 break;
 
                         case EV_INQUIRY_COMPLETE:
-                                if (inquiry_counter >= 5 && (pairWithWii || pairWithHIDDevice)) {
+                                if(inquiry_counter >= 5 && (pairWithWii || pairWithHIDDevice)) {
                                         inquiry_counter = 0;
 #ifdef DEBUG_USB_HOST
-                                        if (pairWithWii)
+                                        if(pairWithWii)
                                                 Notify(PSTR("\r\nCouldn't find Wiimote"), 0x80);
                                         else
                                                 Notify(PSTR("\r\nCouldn't find HID device"), 0x80);
@@ -433,41 +428,43 @@ void BTD::HCI_event_task() {
                                 break;
 
                         case EV_INQUIRY_RESULT:
-                                if (hcibuf[2]) { // Check that there is more than zero responses
+                                if(hcibuf[2]) { // Check that there is more than zero responses
 #ifdef EXTRADEBUG
                                         Notify(PSTR("\r\nNumber of responses: "), 0x80);
                                         Notify(hcibuf[2], 0x80);
 #endif
-                                        for (uint8_t i = 0; i < hcibuf[2]; i++) {
+                                        for(uint8_t i = 0; i < hcibuf[2]; i++) {
                                                 uint8_t offset = 8 * hcibuf[2] + 3 * i;
-                                                uint8_t classOfDevice[3];
 
-                                                for (uint8_t j = 0; j < 3; j++)
+                                                for(uint8_t j = 0; j < 3; j++)
                                                         classOfDevice[j] = hcibuf[j + 4 + offset];
 
-                                                if (pairWithWii && classOfDevice[2] == 0x00 && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0x0C)) { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html and http://wiibrew.org/wiki/Wiimote#SDP_information
-                                                        if (classOfDevice[0] & 0x08) // Check if it's the new Wiimote with motion plus inside that was detected
+                                                if(pairWithWii && classOfDevice[2] == 0x00 && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0x0C)) { // See http://wiibrew.org/wiki/Wiimote#SDP_information
+                                                        if(classOfDevice[0] & 0x08) // Check if it's the new Wiimote with motion plus inside that was detected
                                                                 motionPlusInside = true;
                                                         else
                                                                 motionPlusInside = false;
 
-                                                        for (uint8_t j = 0; j < 6; j++)
+                                                        for(uint8_t j = 0; j < 6; j++)
                                                                 disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];
 
-                                                        hci_event_flag |= HCI_FLAG_DEVICE_FOUND;
+                                                        hci_set_flag(HCI_FLAG_DEVICE_FOUND);
                                                         break;
-                                                } else if (pairWithHIDDevice && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC0)) { // Check if it is a mouse or keyboard
+                                                } else if(pairWithHIDDevice && (classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad - see: http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
 #ifdef DEBUG_USB_HOST
-                                                        if (classOfDevice[0] & 0x80)
+                                                        if(classOfDevice[0] & 0x80)
                                                                 Notify(PSTR("\r\nMouse found"), 0x80);
-                                                        if (classOfDevice[0] & 0x40)
+                                                        if(classOfDevice[0] & 0x40)
                                                                 Notify(PSTR("\r\nKeyboard found"), 0x80);
+                                                        if(classOfDevice[0] & 0x08)
+                                                                Notify(PSTR("\r\nGamepad found"), 0x80);
 #endif
 
-                                                        for (uint8_t j = 0; j < 6; j++)
+                                                        for(uint8_t j = 0; j < 6; j++)
                                                                 disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];
 
-                                                        hci_event_flag |= HCI_FLAG_DEVICE_FOUND;
+                                                        hci_set_flag(HCI_FLAG_DEVICE_FOUND);
+                                                        break;
                                                 }
 #ifdef EXTRADEBUG
                                                 else {
@@ -484,13 +481,13 @@ void BTD::HCI_event_task() {
                                 break;
 
                         case EV_CONNECT_COMPLETE:
-                                hci_event_flag |= HCI_FLAG_CONNECT_EVENT;
-                                if (!hcibuf[2]) { // check if connected OK
+                                hci_set_flag(HCI_FLAG_CONNECT_EVENT);
+                                if(!hcibuf[2]) { // Check if connected OK
 #ifdef EXTRADEBUG
                                         Notify(PSTR("\r\nConnection established"), 0x80);
 #endif
-                                        hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // store the handle for the ACL connection
-                                        hci_event_flag |= HCI_FLAG_CONN_COMPLETE; // set connection complete flag
+                                        hci_handle = hcibuf[3] | ((hcibuf[4] & 0x0F) << 8); // Store the handle for the ACL connection
+                                        hci_set_flag(HCI_FLAG_CONNECT_COMPLETE); // Set connection complete flag
                                 } else {
                                         hci_state = HCI_CHECK_DEVICE_SERVICE;
 #ifdef DEBUG_USB_HOST
@@ -501,52 +498,60 @@ void BTD::HCI_event_task() {
                                 break;
 
                         case EV_DISCONNECT_COMPLETE:
-                                if (!hcibuf[2]) { // check if disconnected OK
-                                        hci_event_flag |= HCI_FLAG_DISCONN_COMPLETE; // set disconnect command complete flag
-                                        hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; // clear connection complete flag
+                                if(!hcibuf[2]) { // Check if disconnected OK
+                                        hci_set_flag(HCI_FLAG_DISCONNECT_COMPLETE); // Set disconnect command complete flag
+                                        hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE); // Clear connection complete flag
                                 }
                                 break;
 
                         case EV_REMOTE_NAME_COMPLETE:
-                                if (!hcibuf[2]) { // check if reading is OK
-                                        for (uint8_t i = 0; i < min(sizeof (remote_name), sizeof (hcibuf) - 9); i++)
+                                if(!hcibuf[2]) { // Check if reading is OK
+                                        for(uint8_t i = 0; i < min(sizeof (remote_name), sizeof (hcibuf) - 9); i++) {
                                                 remote_name[i] = hcibuf[9 + i];
-                                        hci_event_flag |= HCI_FLAG_REMOTE_NAME_COMPLETE;
+                                                if(remote_name[i] == '\0') // End of string
+                                                        break;
+                                        }
+                                        hci_set_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
                                 }
                                 break;
 
                         case EV_INCOMING_CONNECT:
-                                for (uint8_t i = 0; i < 6; i++)
+                                for(uint8_t i = 0; i < 6; i++)
                                         disc_bdaddr[i] = hcibuf[i + 2];
 
-                                if ((hcibuf[9] & 0x05) && (hcibuf[8] & 0xC0)) { // Check if it is a mouse or keyboard
+                                for(uint8_t i = 0; i < 3; i++)
+                                        classOfDevice[i] = hcibuf[i + 8];
+
+                                if((classOfDevice[1] & 0x05) && (classOfDevice[0] & 0xC8)) { // Check if it is a mouse, keyboard or a gamepad
 #ifdef DEBUG_USB_HOST
-                                        if (hcibuf[8] & 0x80)
+                                        if(classOfDevice[0] & 0x80)
                                                 Notify(PSTR("\r\nMouse is connecting"), 0x80);
-                                        if (hcibuf[8] & 0x40)
+                                        if(classOfDevice[0] & 0x40)
                                                 Notify(PSTR("\r\nKeyboard is connecting"), 0x80);
+                                        if(classOfDevice[0] & 0x08)
+                                                Notify(PSTR("\r\nGamepad is connecting"), 0x80);
 #endif
                                         incomingHIDDevice = true;
                                 }
 
 #ifdef EXTRADEBUG
                                 Notify(PSTR("\r\nClass of device: "), 0x80);
-                                D_PrintHex<uint8_t > (hcibuf[10], 0x80);
+                                D_PrintHex<uint8_t > (classOfDevice[2], 0x80);
                                 Notify(PSTR(" "), 0x80);
-                                D_PrintHex<uint8_t > (hcibuf[9], 0x80);
+                                D_PrintHex<uint8_t > (classOfDevice[1], 0x80);
                                 Notify(PSTR(" "), 0x80);
-                                D_PrintHex<uint8_t > (hcibuf[8], 0x80);
+                                D_PrintHex<uint8_t > (classOfDevice[0], 0x80);
 #endif
-                                hci_event_flag |= HCI_FLAG_INCOMING_REQUEST;
+                                hci_set_flag(HCI_FLAG_INCOMING_REQUEST);
                                 break;
 
                         case EV_PIN_CODE_REQUEST:
-                                if (pairWithWii) {
+                                if(pairWithWii) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nPairing with wiimote"), 0x80);
 #endif
                                         hci_pin_code_request_reply();
-                                } else if (btdPin != NULL) {
+                                } else if(btdPin != NULL) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nBluetooth pin is set too: "), 0x80);
                                         NotifyStr(btdPin, 0x80);
@@ -568,16 +573,16 @@ void BTD::HCI_event_task() {
                                 break;
 
                         case EV_AUTHENTICATION_COMPLETE:
-                                if (pairWithWii && !connectToWii) {
+                                if(pairWithWii && !connectToWii) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nPairing successful with Wiimote"), 0x80);
 #endif
-                                        connectToWii = true; // Only send the ACL data to the Wii service
-                                } else if (pairWithHIDDevice && !connectToHIDDevice) {
+                                        connectToWii = true; // Used to indicate to the Wii service, that it should connect to this device
+                                } else if(pairWithHIDDevice && !connectToHIDDevice) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nPairing successful with HID device"), 0x80);
 #endif
-                                        connectToHIDDevice = true; // Only send the ACL data to the Wii service
+                                        connectToHIDDevice = true; // Used to indicate to the BTHID service, that it should connect to this device
                                 }
                                 break;
                                 /* We will just ignore the following events */
@@ -595,13 +600,13 @@ void BTD::HCI_event_task() {
                                 break;
 #ifdef EXTRADEBUG
                         default:
-                                if (hcibuf[0] != 0x00) {
+                                if(hcibuf[0] != 0x00) {
                                         Notify(PSTR("\r\nUnmanaged HCI Event: "), 0x80);
                                         D_PrintHex<uint8_t > (hcibuf[0], 0x80);
                                 }
                                 break;
 #endif
-                } // switch
+                } // Switch
         }
 #ifdef EXTRADEBUG
         else {
@@ -609,15 +614,14 @@ void BTD::HCI_event_task() {
                 D_PrintHex<uint8_t > (rcode, 0x80);
         }
 #endif
-        HCI_task();
 }
 
 /* Poll Bluetooth and print result */
 void BTD::HCI_task() {
-        switch (hci_state) {
+        switch(hci_state) {
                 case HCI_INIT_STATE:
                         hci_counter++;
-                        if (hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events
+                        if(hci_counter > hci_num_reset_loops) { // wait until we have looped x times to clear any old events
                                 hci_reset();
                                 hci_state = HCI_RESET_STATE;
                                 hci_counter = 0;
@@ -626,16 +630,16 @@ void BTD::HCI_task() {
 
                 case HCI_RESET_STATE:
                         hci_counter++;
-                        if (hci_cmd_complete) {
+                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
                                 hci_counter = 0;
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHCI Reset complete"), 0x80);
 #endif
                                 hci_state = HCI_CLASS_STATE;
                                 hci_write_class_of_device();
-                        } else if (hci_counter > hci_num_reset_loops) {
+                        } else if(hci_counter > hci_num_reset_loops) {
                                 hci_num_reset_loops *= 10;
-                                if (hci_num_reset_loops > 2000)
+                                if(hci_num_reset_loops > 2000)
                                         hci_num_reset_loops = 2000;
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nNo response to HCI Reset"), 0x80);
@@ -646,7 +650,7 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_CLASS_STATE:
-                        if (hci_cmd_complete) {
+                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nWrite class of device"), 0x80);
 #endif
@@ -656,10 +660,10 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_BDADDR_STATE:
-                        if (hci_read_bdaddr_complete) {
+                        if(hci_check_flag(HCI_FLAG_READ_BDADDR)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nLocal Bluetooth Address: "), 0x80);
-                                for (int8_t i = 5; i > 0; i--) {
+                                for(int8_t i = 5; i > 0; i--) {
                                         D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
                                         Notify(PSTR(":"), 0x80);
                                 }
@@ -671,8 +675,8 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class
-                        if (hci_read_version_complete) {
-                                if (btdName != NULL) {
+                        if(hci_check_flag(HCI_FLAG_READ_VERSION)) {
+                                if(btdName != NULL) {
                                         hci_set_local_name(btdName);
                                         hci_state = HCI_SET_NAME_STATE;
                                 } else
@@ -681,7 +685,7 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_SET_NAME_STATE:
-                        if (hci_cmd_complete) {
+                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nThe name is set to: "), 0x80);
                                 NotifyStr(btdName, 0x80);
@@ -691,9 +695,9 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_CHECK_DEVICE_SERVICE:
-                        if (pairWithHIDDevice || pairWithWii) { // Check if it should try to connect to a wiimote
+                        if(pairWithHIDDevice || pairWithWii) { // Check if it should try to connect to a Wiimote
 #ifdef DEBUG_USB_HOST
-                                if (pairWithWii)
+                                if(pairWithWii)
                                         Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote\r\nOr press sync if you are using a Wii U Pro Controller"), 0x80);
                                 else
                                         Notify(PSTR("\r\nPlease enable discovery of your device"), 0x80);
@@ -705,27 +709,27 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_INQUIRY_STATE:
-                        if (hci_device_found) {
+                        if(hci_check_flag(HCI_FLAG_DEVICE_FOUND)) {
                                 hci_inquiry_cancel(); // Stop inquiry
 #ifdef DEBUG_USB_HOST
-                                if (pairWithWii)
+                                if(pairWithWii)
                                         Notify(PSTR("\r\nWiimote found"), 0x80);
                                 else
                                         Notify(PSTR("\r\nHID device found"), 0x80);
 
                                 Notify(PSTR("\r\nNow just create the instance like so:"), 0x80);
-                                if (pairWithWii)
+                                if(pairWithWii)
                                         Notify(PSTR("\r\nWII Wii(&Btd);"), 0x80);
                                 else
-                                        Notify(PSTR("\r\nBTHID hid(&Btd);"), 0x80);
+                                        Notify(PSTR("\r\nBTHID bthid(&Btd);"), 0x80);
 
                                 Notify(PSTR("\r\nAnd then press any button on the "), 0x80);
-                                if (pairWithWii)
+                                if(pairWithWii)
                                         Notify(PSTR("Wiimote"), 0x80);
                                 else
                                         Notify(PSTR("device"), 0x80);
 #endif
-                                if (motionPlusInside) {
+                                if(motionPlusInside) {
                                         hci_remote_name(); // We need to know the name to distinguish between a Wiimote and a Wii U Pro Controller
                                         hci_state = HCI_REMOTE_NAME_STATE;
                                 } else
@@ -734,9 +738,9 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_CONNECT_DEVICE_STATE:
-                        if (hci_cmd_complete) {
+                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
-                                if (pairWithWii)
+                                if(pairWithWii)
                                         Notify(PSTR("\r\nConnecting to Wiimote"), 0x80);
                                 else
                                         Notify(PSTR("\r\nConnecting to HID device"), 0x80);
@@ -747,15 +751,15 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_CONNECTED_DEVICE_STATE:
-                        if (hci_connect_event) {
-                                if (hci_connect_complete) {
+                        if(hci_check_flag(HCI_FLAG_CONNECT_EVENT)) {
+                                if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
-                                        if (pairWithWii)
+                                        if(pairWithWii)
                                                 Notify(PSTR("\r\nConnected to Wiimote"), 0x80);
                                         else
                                                 Notify(PSTR("\r\nConnected to HID device"), 0x80);
 #endif
-                                        hci_authentication_request(); // This will start the pairing with the wiimote
+                                        hci_authentication_request(); // This will start the pairing with the Wiimote
                                         hci_state = HCI_SCANNING_STATE;
                                 } else {
 #ifdef DEBUG_USB_HOST
@@ -767,7 +771,7 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_SCANNING_STATE:
-                        if (!connectToWii && !pairWithWii && !connectToHIDDevice && !pairWithHIDDevice) {
+                        if(!connectToWii && !pairWithWii && !connectToHIDDevice && !pairWithHIDDevice) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nWait For Incoming Connection Request"), 0x80);
 #endif
@@ -778,38 +782,35 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_CONNECT_IN_STATE:
-                        if (hci_incoming_connect_request) {
+                        if(hci_check_flag(HCI_FLAG_INCOMING_REQUEST)) {
                                 watingForConnection = false;
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nIncoming Connection Request"), 0x80);
 #endif
                                 hci_remote_name();
                                 hci_state = HCI_REMOTE_NAME_STATE;
-                        } else if (hci_disconnect_complete)
+                        } else if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE))
                                 hci_state = HCI_DISCONNECT_STATE;
                         break;
 
                 case HCI_REMOTE_NAME_STATE:
-                        if (hci_remote_name_complete) {
+                        if(hci_check_flag(HCI_FLAG_REMOTE_NAME_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nRemote Name: "), 0x80);
-                                for (uint8_t i = 0; i < 30; i++) {
-                                        if (remote_name[i] == NULL)
-                                                break;
+                                for(uint8_t i = 0; i < strlen(remote_name); i++)
                                         Notifyc(remote_name[i], 0x80);
-                                }
 #endif
-                                if (strncmp((const char*)remote_name, "Nintendo", 8) == 0) {
+                                if(strncmp((const char*)remote_name, "Nintendo", 8) == 0) {
                                         incomingWii = true;
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nWiimote is connecting"), 0x80);
 #endif
-                                        if (strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) {
+                                        if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-TR", 22) == 0) {
 #ifdef DEBUG_USB_HOST
                                                 Notify(PSTR(" with Motion Plus Inside"), 0x80);
 #endif
                                                 motionPlusInside = true;
-                                        } else if (strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-UC", 22) == 0) {
+                                        } else if(strncmp((const char*)remote_name, "Nintendo RVL-CNT-01-UC", 22) == 0) {
 #ifdef DEBUG_USB_HOST
                                                 Notify(PSTR(" - Wii U Pro Controller"), 0x80);
 #endif
@@ -820,7 +821,13 @@ void BTD::HCI_task() {
                                                 wiiUProController = false;
                                         }
                                 }
-                                if (pairWithWii && motionPlusInside)
+                                if(classOfDevice[2] == 0 && classOfDevice[1] == 0x25 && classOfDevice[0] == 0x08 && strncmp((const char*)remote_name, "Wireless Controller", 19) == 0) {
+#ifdef DEBUG_USB_HOST
+                                        Notify(PSTR("\r\nPS4 controller is connecting"), 0x80);
+#endif
+                                        incomingPS4 = true;
+                                }
+                                if(pairWithWii && motionPlusInside)
                                         hci_state = HCI_CONNECT_DEVICE_STATE;
                                 else {
                                         hci_accept_connection();
@@ -830,15 +837,18 @@ void BTD::HCI_task() {
                         break;
 
                 case HCI_CONNECTED_STATE:
-                        if (hci_connect_complete) {
+                        if(hci_check_flag(HCI_FLAG_CONNECT_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nConnected to Device: "), 0x80);
-                                for (int8_t i = 5; i > 0; i--) {
+                                for(int8_t i = 5; i > 0; i--) {
                                         D_PrintHex<uint8_t > (disc_bdaddr[i], 0x80);
                                         Notify(PSTR(":"), 0x80);
                                 }
                                 D_PrintHex<uint8_t > (disc_bdaddr[0], 0x80);
 #endif
+                                if(incomingPS4)
+                                        connectToHIDDevice = true; // We should always connect to the PS4 controller
+
                                 // Clear these flags for a new connection
                                 l2capConnectionClaimed = false;
                                 sdpConnectionClaimed = false;
@@ -851,32 +861,26 @@ void BTD::HCI_task() {
 
                 case HCI_DONE_STATE:
                         hci_counter++;
-                        if (hci_counter > 1000) { // Wait until we have looped 1000 times to make sure that the L2CAP connection has been started
+                        if(hci_counter > 1000) { // Wait until we have looped 1000 times to make sure that the L2CAP connection has been started
                                 hci_counter = 0;
                                 hci_state = HCI_SCANNING_STATE;
                         }
                         break;
 
                 case HCI_DISCONNECT_STATE:
-                        if (hci_disconnect_complete) {
+                        if(hci_check_flag(HCI_FLAG_DISCONNECT_COMPLETE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHCI Disconnected from Device"), 0x80);
 #endif
                                 hci_event_flag = 0; // Clear all flags
 
                                 // Reset all buffers
-                                for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++)
-                                        hcibuf[i] = 0;
-                                for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++)
-                                        l2capinbuf[i] = 0;
+                                memset(hcibuf, 0, BULK_MAXPKTSIZE);
+                                memset(l2capinbuf, 0, BULK_MAXPKTSIZE);
 
-                                connectToWii = false;
-                                incomingWii = false;
-                                pairWithWii = false;
-
-                                connectToHIDDevice = false;
-                                incomingHIDDevice = false;
-                                pairWithHIDDevice = false;
+                                connectToWii = incomingWii = pairWithWii = false;
+                                connectToHIDDevice = incomingHIDDevice = pairWithHIDDevice = false;
+                                incomingPS4 = false;
 
                                 hci_state = HCI_SCANNING_STATE;
                         }
@@ -887,21 +891,25 @@ void BTD::HCI_task() {
 }
 
 void BTD::ACL_event_task() {
-        uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE;
-        uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &MAX_BUFFER_SIZE, l2capinbuf); // input on endpoint 2
-        if (!rcode) { // Check for errors
-                for (uint8_t i = 0; i < BTD_NUMSERVICES; i++)
-                        if (btService[i])
-                                btService[i]->ACLData(l2capinbuf);
+        uint16_t length = BULK_MAXPKTSIZE;
+        uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &length, l2capinbuf); // Input on endpoint 2
+
+        if(!rcode) { // Check for errors
+                if(length > 0) { // Check if any data was read
+                        for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) {
+                                if(btService[i])
+                                        btService[i]->ACLData(l2capinbuf);
+                        }
+                }
         }
 #ifdef EXTRADEBUG
-        else if (rcode != hrNAK) {
+        else if(rcode != hrNAK) {
                 Notify(PSTR("\r\nACL data in error: "), 0x80);
                 D_PrintHex<uint8_t > (rcode, 0x80);
         }
 #endif
-        for (uint8_t i = 0; i < BTD_NUMSERVICES; i++)
-                if (btService[i])
+        for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
+                if(btService[i])
                         btService[i]->Run();
 }
 
@@ -910,7 +918,7 @@ void BTD::ACL_event_task() {
 
 /************************************************************/
 void BTD::HCI_Command(uint8_t* data, uint16_t nbytes) {
-        hci_event_flag &= ~HCI_FLAG_CMD_COMPLETE;
+        hci_clear_flag(HCI_FLAG_CMD_COMPLETE);
         pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00, 0x00, nbytes, nbytes, data, NULL);
 }
 
@@ -924,11 +932,11 @@ void BTD::hci_reset() {
 }
 
 void BTD::hci_write_scan_enable() {
-        hci_event_flag &= ~HCI_FLAG_INCOMING_REQUEST;
+        hci_clear_flag(HCI_FLAG_INCOMING_REQUEST);
         hcibuf[0] = 0x1A; // HCI OCF = 1A
         hcibuf[1] = 0x03 << 2; // HCI OGF = 3
         hcibuf[2] = 0x01; // parameter length = 1
-        if (btdName != NULL)
+        if(btdName != NULL)
                 hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled.
         else
                 hcibuf[3] = 0x02; // Inquiry Scan disabled. Page Scan enabled.
@@ -946,6 +954,7 @@ void BTD::hci_write_scan_disable() {
 }
 
 void BTD::hci_read_bdaddr() {
+        hci_clear_flag(HCI_FLAG_READ_BDADDR);
         hcibuf[0] = 0x09; // HCI OCF = 9
         hcibuf[1] = 0x04 << 2; // HCI OGF = 4
         hcibuf[2] = 0x00;
@@ -954,6 +963,7 @@ void BTD::hci_read_bdaddr() {
 }
 
 void BTD::hci_read_local_version_information() {
+        hci_clear_flag(HCI_FLAG_READ_VERSION);
         hcibuf[0] = 0x01; // HCI OCF = 1
         hcibuf[1] = 0x04 << 2; // HCI OGF = 4
         hcibuf[2] = 0x00;
@@ -962,7 +972,7 @@ void BTD::hci_read_local_version_information() {
 }
 
 void BTD::hci_accept_connection() {
-        hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE;
+        hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE);
         hcibuf[0] = 0x09; // HCI OCF = 9
         hcibuf[1] = 0x01 << 2; // HCI OGF = 1
         hcibuf[2] = 0x07; // parameter length 7
@@ -972,13 +982,13 @@ void BTD::hci_accept_connection() {
         hcibuf[6] = disc_bdaddr[3];
         hcibuf[7] = disc_bdaddr[4];
         hcibuf[8] = disc_bdaddr[5];
-        hcibuf[9] = 0x00; //switch role to master
+        hcibuf[9] = 0x00; // Switch role to master
 
         HCI_Command(hcibuf, 10);
 }
 
 void BTD::hci_remote_name() {
-        hci_event_flag &= ~HCI_FLAG_REMOTE_NAME_COMPLETE;
+        hci_clear_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
         hcibuf[0] = 0x19; // HCI OCF = 19
         hcibuf[1] = 0x01 << 2; // HCI OGF = 1
         hcibuf[2] = 0x0A; // parameter length = 10
@@ -988,10 +998,10 @@ void BTD::hci_remote_name() {
         hcibuf[6] = disc_bdaddr[3];
         hcibuf[7] = disc_bdaddr[4];
         hcibuf[8] = disc_bdaddr[5];
-        hcibuf[9] = 0x01; //Page Scan Repetition Mode
-        hcibuf[10] = 0x00; //Reserved
-        hcibuf[11] = 0x00; //Clock offset - low byte
-        hcibuf[12] = 0x00; //Clock offset - high byte
+        hcibuf[9] = 0x01; // Page Scan Repetition Mode
+        hcibuf[10] = 0x00; // Reserved
+        hcibuf[11] = 0x00; // Clock offset - low byte
+        hcibuf[12] = 0x00; // Clock offset - high byte
 
         HCI_Command(hcibuf, 13);
 }
@@ -1001,7 +1011,7 @@ void BTD::hci_set_local_name(const char* name) {
         hcibuf[1] = 0x03 << 2; // HCI OGF = 3
         hcibuf[2] = strlen(name) + 1; // parameter length = the length of the string + end byte
         uint8_t i;
-        for (i = 0; i < strlen(name); i++)
+        for(i = 0; i < strlen(name); i++)
                 hcibuf[i + 3] = name[i];
         hcibuf[i + 3] = 0x00; // End of string
 
@@ -1009,7 +1019,7 @@ void BTD::hci_set_local_name(const char* name) {
 }
 
 void BTD::hci_inquiry() {
-        hci_event_flag &= ~HCI_FLAG_DEVICE_FOUND;
+        hci_clear_flag(HCI_FLAG_DEVICE_FOUND);
         hcibuf[0] = 0x01;
         hcibuf[1] = 0x01 << 2; // HCI OGF = 1
         hcibuf[2] = 0x05; // Parameter Total Length = 5
@@ -1035,7 +1045,7 @@ void BTD::hci_connect() {
 }
 
 void BTD::hci_connect(uint8_t *bdaddr) {
-        hci_event_flag &= ~(HCI_FLAG_CONN_COMPLETE | HCI_FLAG_CONNECT_EVENT);
+        hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE | HCI_FLAG_CONNECT_EVENT);
         hcibuf[0] = 0x05;
         hcibuf[1] = 0x01 << 2; // HCI OGF = 1
         hcibuf[2] = 0x0D; // parameter Total Length = 13
@@ -1066,26 +1076,26 @@ void BTD::hci_pin_code_request_reply() {
         hcibuf[6] = disc_bdaddr[3];
         hcibuf[7] = disc_bdaddr[4];
         hcibuf[8] = disc_bdaddr[5];
-        if (pairWithWii) {
+        if(pairWithWii) {
                 hcibuf[9] = 6; // Pin length is the length of the Bluetooth address
-                if (wiiUProController) {
+                if(wiiUProController) {
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nParing with Wii U Pro Controller"), 0x80);
 #endif
-                        for (uint8_t i = 0; i < 6; i++)
+                        for(uint8_t i = 0; i < 6; i++)
                                 hcibuf[10 + i] = my_bdaddr[i]; // The pin is the Bluetooth dongles Bluetooth address backwards
                 } else {
-                        for (uint8_t i = 0; i < 6; i++)
+                        for(uint8_t i = 0; i < 6; i++)
                                 hcibuf[10 + i] = disc_bdaddr[i]; // The pin is the Wiimote's Bluetooth address backwards
                 }
-                for (uint8_t i = 16; i < 26; i++)
+                for(uint8_t i = 16; i < 26; i++)
                         hcibuf[i] = 0x00; // The rest should be 0
         } else {
                 hcibuf[9] = strlen(btdPin); // Length of pin
                 uint8_t i;
-                for (i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16
+                for(i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16
                         hcibuf[i + 10] = btdPin[i];
-                for (; i < 16; i++)
+                for(; i < 16; i++)
                         hcibuf[i + 10] = 0x00; // The rest should be 0
         }
 
@@ -1131,7 +1141,7 @@ void BTD::hci_authentication_request() {
 }
 
 void BTD::hci_disconnect(uint16_t handle) { // This is called by the different services
-        hci_event_flag &= ~HCI_FLAG_DISCONN_COMPLETE;
+        hci_clear_flag(HCI_FLAG_DISCONNECT_COMPLETE);
         hcibuf[0] = 0x06; // HCI OCF = 6
         hcibuf[1] = 0x01 << 2; // HCI OGF = 1
         hcibuf[2] = 0x03; // parameter length = 3
@@ -1189,11 +1199,11 @@ void BTD::L2CAP_Command(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t
         buf[6] = channelLow;
         buf[7] = channelHigh;
 
-        for (uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame
+        for(uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame
                 buf[8 + i] = data[i];
 
         uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf);
-        if (rcode) {
+        if(rcode) {
                 delay(100); // This small delay prevents it from overflowing if it fails
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nError sending L2CAP message: 0x"), 0x80);
@@ -1322,7 +1332,7 @@ void BTD::setBdaddr(uint8_t* bdaddr) {
         buf[0] = 0x01;
         buf[1] = 0x00;
 
-        for (uint8_t i = 0; i < 6; i++)
+        for(uint8_t i = 0; i < 6; i++)
                 buf[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
 
         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
@@ -1338,7 +1348,7 @@ void BTD::setMoveBdaddr(uint8_t* bdaddr) {
         buf[9] = 0x02;
         buf[10] = 0x12;
 
-        for (uint8_t i = 0; i < 6; i++)
+        for(uint8_t i = 0; i < 6; i++)
                 buf[i + 1] = bdaddr[i];
 
         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
diff --git a/BTD.h b/BTD.h
index a048e098..3a639abb 100755
--- a/BTD.h
+++ b/BTD.h
@@ -30,7 +30,7 @@
 #define IOGEAR_GBU521_PID       0x21E8
 
 /* Bluetooth dongle data taken from descriptors */
-#define BULK_MAXPKTSIZE         64 // max size for ACL data
+#define BULK_MAXPKTSIZE         64 // Max size for ACL data
 
 // Used in control endpoint header for HCI Commands
 #define bmREQ_HCI_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_DEVICE
@@ -47,7 +47,7 @@
 #define HCI_SET_NAME_STATE              5
 #define HCI_CHECK_DEVICE_SERVICE        6
 
-#define HCI_INQUIRY_STATE               7 // These three states are only used if it should pair and connect to a Wii controller
+#define HCI_INQUIRY_STATE               7 // These three states are only used if it should pair and connect to a device
 #define HCI_CONNECT_DEVICE_STATE        8
 #define HCI_CONNECTED_DEVICE_STATE      9
 
@@ -61,8 +61,8 @@
 
 /* HCI event flags*/
 #define HCI_FLAG_CMD_COMPLETE           0x01
-#define HCI_FLAG_CONN_COMPLETE          0x02
-#define HCI_FLAG_DISCONN_COMPLETE       0x04
+#define HCI_FLAG_CONNECT_COMPLETE       0x02
+#define HCI_FLAG_DISCONNECT_COMPLETE    0x04
 #define HCI_FLAG_REMOTE_NAME_COMPLETE   0x08
 #define HCI_FLAG_INCOMING_REQUEST       0x10
 #define HCI_FLAG_READ_BDADDR            0x20
@@ -70,16 +70,10 @@
 #define HCI_FLAG_DEVICE_FOUND           0x80
 #define HCI_FLAG_CONNECT_EVENT          0x100
 
-/*Macros for HCI event flag tests */
-#define hci_cmd_complete (hci_event_flag & HCI_FLAG_CMD_COMPLETE)
-#define hci_connect_complete (hci_event_flag & HCI_FLAG_CONN_COMPLETE)
-#define hci_disconnect_complete (hci_event_flag & HCI_FLAG_DISCONN_COMPLETE)
-#define hci_remote_name_complete (hci_event_flag & HCI_FLAG_REMOTE_NAME_COMPLETE)
-#define hci_incoming_connect_request (hci_event_flag & HCI_FLAG_INCOMING_REQUEST)
-#define hci_read_bdaddr_complete (hci_event_flag & HCI_FLAG_READ_BDADDR)
-#define hci_read_version_complete (hci_event_flag & HCI_FLAG_READ_VERSION)
-#define hci_device_found (hci_event_flag & HCI_FLAG_DEVICE_FOUND)
-#define hci_connect_event (hci_event_flag & HCI_FLAG_CONNECT_EVENT)
+/* Macros for HCI event flag tests */
+#define hci_check_flag(flag) (hci_event_flag & (flag))
+#define hci_set_flag(flag) (hci_event_flag |= (flag))
+#define hci_clear_flag(flag) (hci_event_flag &= ~(flag))
 
 /* HCI Events managed */
 #define EV_INQUIRY_COMPLETE                             0x01
@@ -105,6 +99,68 @@
 #define EV_LOOPBACK_COMMAND                             0x19
 #define EV_PAGE_SCAN_REP_MODE                           0x20
 
+/* Bluetooth states for the different Bluetooth drivers */
+#define L2CAP_WAIT                      0
+#define L2CAP_DONE                      1
+
+/* Used for HID Control channel */
+#define L2CAP_CONTROL_CONNECT_REQUEST   2
+#define L2CAP_CONTROL_CONFIG_REQUEST    3
+#define L2CAP_CONTROL_SUCCESS           4
+#define L2CAP_CONTROL_DISCONNECT        5
+
+/* Used for HID Interrupt channel */
+#define L2CAP_INTERRUPT_SETUP           6
+#define L2CAP_INTERRUPT_CONNECT_REQUEST 7
+#define L2CAP_INTERRUPT_CONFIG_REQUEST  8
+#define L2CAP_INTERRUPT_DISCONNECT      9
+
+/* Used for SDP channel */
+#define L2CAP_SDP_WAIT                  10
+#define L2CAP_SDP_SUCCESS               11
+
+/* Used for RFCOMM channel */
+#define L2CAP_RFCOMM_WAIT               12
+#define L2CAP_RFCOMM_SUCCESS            13
+
+#define L2CAP_DISCONNECT_RESPONSE       14 // Used for both SDP and RFCOMM channel
+
+/* Bluetooth states used by some drivers */
+#define TURN_ON_LED                     17
+#define PS3_ENABLE_SIXAXIS              18
+#define WII_CHECK_MOTION_PLUS_STATE     19
+#define WII_CHECK_EXTENSION_STATE       20
+#define WII_INIT_MOTION_PLUS_STATE      21
+
+/* L2CAP event flags for HID Control channel */
+#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST           0x00000001
+#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS               0x00000002
+#define L2CAP_FLAG_CONTROL_CONNECTED                    0x00000004
+#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE          0x00000008
+
+/* L2CAP event flags for HID Interrupt channel */
+#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST         0x00000010
+#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS             0x00000020
+#define L2CAP_FLAG_INTERRUPT_CONNECTED                  0x00000040
+#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE        0x00000080
+
+/* L2CAP event flags for SDP channel */
+#define L2CAP_FLAG_CONNECTION_SDP_REQUEST               0x00000100
+#define L2CAP_FLAG_CONFIG_SDP_SUCCESS                   0x00000200
+#define L2CAP_FLAG_DISCONNECT_SDP_REQUEST               0x00000400
+
+/* L2CAP event flags for RFCOMM channel */
+#define L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST            0x00000800
+#define L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS                0x00001000
+#define L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST            0x00002000
+
+#define L2CAP_FLAG_DISCONNECT_RESPONSE                  0x00004000
+
+/* Macros for L2CAP event flag tests */
+#define l2cap_check_flag(flag) (l2cap_event_flag & (flag))
+#define l2cap_set_flag(flag) (l2cap_event_flag |= (flag))
+#define l2cap_clear_flag(flag) (l2cap_event_flag &= ~(flag))
+
 /* L2CAP signaling commands */
 #define L2CAP_CMD_COMMAND_REJECT        0x01
 #define L2CAP_CMD_CONNECTION_REQUEST    0x02
@@ -131,11 +187,27 @@
 #define WI_PROTOCOL_BT      0x01 // Bluetooth Programming Interface
 
 #define BTD_MAX_ENDPOINTS   4
-#define BTD_NUMSERVICES     4 // Max number of Bluetooth services - if you need more than four simply increase this number
+#define BTD_NUM_SERVICES    4 // Max number of Bluetooth services - if you need more than 4 simply increase this number
 
 #define PAIR    1
 
-/** All Bluetooth services should include this class. */
+/* acl_handle_ok or it's a new connection */
+#if 0
+#define UHS_ACL_HANDLE_OK(x, y) ((uint16_t)(x[0]) | (uint16_t)(x[1] << 8)) == (y | 0x2000U)
+#else
+/*
+ *  Better implementation.
+ *  o One place for this code, it is reused four times in the source.
+ *    Perhaps it is better as a function.
+ *  o This should be faster since the && operation can early exit, this means
+ *    the shift would only be performed if the first byte matches.
+ *  o Casting is eliminated.
+ *  o How does this compare in code size? No difference. It is a free optimization.
+ */
+#define UHS_ACL_HANDLE_OK(x, y) ((x[0] == (y & 0xff)) && (x[1] == ((y >> 8) | 0x20)))
+#endif
+
+/** All Bluetooth services should inherit this class. */
 class BluetoothService {
 public:
         /**
@@ -165,7 +237,7 @@ public:
 
         /** @name USBDeviceConfig implementation */
         /**
-         * Address assignment and basic initilization is done here.
+         * Address assignment and basic initialization is done here.
          * @param  parent   Hub number.
          * @param  port     Port number on the hub.
          * @param  lowspeed Speed of the device.
@@ -186,7 +258,7 @@ public:
          */
         virtual uint8_t Release();
         /**
-         * Poll the USB Input endpoins and run the state machines.
+         * Poll the USB Input endpoints and run the state machines.
          * @return 0 on success.
          */
         virtual uint8_t Poll();
@@ -206,12 +278,15 @@ public:
         virtual bool isReady() {
                 return bPollEnable;
         };
+
         /**
          * Used by the USB core to check what this driver support.
          * @param  klass The device's USB class.
          * @return       Returns true if the device's USB class matches this driver.
          */
-        virtual boolean DEVCLASSOK(uint8_t klass) { return (klass == USB_CLASS_WIRELESS_CTRL); }
+        virtual boolean DEVCLASSOK(uint8_t klass) {
+                return (klass == USB_CLASS_WIRELESS_CTRL);
+        };
 
         /**
          * Used by the USB core to check what this driver support.
@@ -221,10 +296,10 @@ public:
          * @return     Returns true if the device's VID and PID matches this driver.
          */
         virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) {
-                if (vid == IOGEAR_GBU521_VID && pid == IOGEAR_GBU521_PID)
+                if(vid == IOGEAR_GBU521_VID && pid == IOGEAR_GBU521_PID)
                         return true;
-                if (my_bdaddr[0] != 0x00 || my_bdaddr[1] != 0x00 || my_bdaddr[2] != 0x00 || my_bdaddr[3] != 0x00 || my_bdaddr[4] != 0x00 || my_bdaddr[5] != 0x00) { // Check if Bluetooth address is set
-                        if (vid == PS3_VID && (pid == PS3_PID || pid == PS3NAVIGATION_PID || pid == PS3MOVE_PID))
+                if(my_bdaddr[0] != 0x00 || my_bdaddr[1] != 0x00 || my_bdaddr[2] != 0x00 || my_bdaddr[3] != 0x00 || my_bdaddr[4] != 0x00 || my_bdaddr[5] != 0x00) { // Check if Bluetooth address is set
+                        if(vid == PS3_VID && (pid == PS3_PID || pid == PS3NAVIGATION_PID || pid == PS3MOVE_PID))
                                 return true;
                 }
                 return false;
@@ -245,18 +320,18 @@ public:
 
         /** Disconnects both the L2CAP Channel and the HCI Connection for all Bluetooth services. */
         void disconnect() {
-                for(uint8_t i = 0; i < BTD_NUMSERVICES; i++)
+                for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
                         if(btService[i])
                                 btService[i]->disconnect();
         };
 
         /**
-         * Register bluetooth dongle members/services.
+         * Register Bluetooth dongle members/services.
          * @param  pService Pointer to BluetoothService class instance.
-         * @return          The serice ID on succes or -1 on fail.
+         * @return          The service ID on success or -1 on fail.
          */
         int8_t registerServiceClass(BluetoothService *pService) {
-                for(uint8_t i = 0; i < BTD_NUMSERVICES; i++) {
+                for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++) {
                         if(!btService[i]) {
                                 btService[i] = pService;
                                 return i; // Return ID
@@ -403,7 +478,7 @@ public:
 
         /** The name you wish to make the dongle show up as. It is set automatically by the SPP library. */
         const char* btdName;
-        /** The pin you wish to make the dongle use for authentication. It is set automatically by the SPP library. */
+        /** The pin you wish to make the dongle use for authentication. It is set automatically by the SPP and BTHID library. */
         const char* btdPin;
 
         /** The bluetooth dongles Bluetooth address. */
@@ -413,7 +488,7 @@ public:
         /** Last incoming devices Bluetooth address. */
         uint8_t disc_bdaddr[6];
         /** First 30 chars of last remote name. */
-        uint8_t remote_name[30];
+        char remote_name[30];
         /**
          * The supported HCI Version read from the Bluetooth dongle.
          * Used by the PS3BT library to check the HCI Version of the Bluetooth dongle,
@@ -426,7 +501,7 @@ public:
                 pairWithWii = true;
                 hci_state = HCI_CHECK_DEVICE_SERVICE;
         };
-        /** Used to only send the ACL data to the wiimote. */
+        /** Used to only send the ACL data to the Wiimote. */
         bool connectToWii;
         /** True if a Wiimote is connecting. */
         bool incomingWii;
@@ -442,7 +517,7 @@ public:
                 pairWithHIDDevice = true;
                 hci_state = HCI_CHECK_DEVICE_SERVICE;
         };
-        /** Used to only send the ACL data to the wiimote. */
+        /** Used to only send the ACL data to the Wiimote. */
         bool connectToHIDDevice;
         /** True if a Wiimote is connecting. */
         bool incomingHIDDevice;
@@ -489,23 +564,26 @@ protected:
 
 private:
         void Initialize(); // Set all variables, endpoint structs etc. to default values
-        BluetoothService* btService[BTD_NUMSERVICES];
+        BluetoothService *btService[BTD_NUM_SERVICES];
 
         uint16_t PID, VID; // PID and VID of device connected
 
-        bool bPollEnable;
         uint8_t pollInterval;
+        bool bPollEnable;
+
+        bool incomingPS4; // True if a PS4 controller is connecting
+        uint8_t classOfDevice[3]; // Class of device of last device
 
         /* Variables used by high level HCI task */
-        uint8_t hci_state; //current state of bluetooth hci connection
-        uint16_t hci_counter; // counter used for bluetooth hci reset loops
-        uint8_t hci_num_reset_loops; // this value indicate how many times it should read before trying to reset
-        uint16_t hci_event_flag; // hci flags of received bluetooth events
+        uint8_t hci_state; // Current state of Bluetooth HCI connection
+        uint16_t hci_counter; // Counter used for Bluetooth HCI reset loops
+        uint16_t hci_num_reset_loops; // This value indicate how many times it should read before trying to reset
+        uint16_t hci_event_flag; // HCI flags of received Bluetooth events
         uint8_t inquiry_counter;
 
-        uint8_t hcibuf[BULK_MAXPKTSIZE]; //General purpose buffer for hci data
-        uint8_t l2capinbuf[BULK_MAXPKTSIZE]; //General purpose buffer for l2cap in data
-        uint8_t l2capoutbuf[14]; //General purpose buffer for l2cap out data
+        uint8_t hcibuf[BULK_MAXPKTSIZE]; // General purpose buffer for HCI data
+        uint8_t l2capinbuf[BULK_MAXPKTSIZE]; // General purpose buffer for L2CAP in data
+        uint8_t l2capoutbuf[14]; // General purpose buffer for L2CAP out data
 
         /* State machines */
         void HCI_event_task(); // Poll the HCI event pipe
diff --git a/BTHID.cpp b/BTHID.cpp
index 3e626b18..01f190a7 100644
--- a/BTHID.cpp
+++ b/BTHID.cpp
@@ -22,18 +22,15 @@
 
 BTHID::BTHID(BTD *p, bool pair, const char *pin) :
 pBtd(p), // pointer to USB class instance - mandatory
-protocolMode(HID_BOOT_PROTOCOL)
-{
-        for (uint8_t i = 0; i < epMUL; i++)
+protocolMode(HID_BOOT_PROTOCOL) {
+        for(uint8_t i = 0; i < NUM_PARSERS; i++)
                 pRptParser[i] = NULL;
 
-        if (pBtd)
+        if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
 
         pBtd->pairWithHIDDevice = pair;
-
-        if (pair)
-                pBtd->btdPin= pin;
+        pBtd->btdPin = pin;
 
         /* Set device cid for the control and intterrupt channelse - LSB */
         control_dcid[0] = 0x70; // 0x0070
@@ -49,19 +46,20 @@ void BTHID::Reset() {
         activeConnection = false;
         l2cap_event_flag = 0; // Reset flags
         l2cap_state = L2CAP_WAIT;
+        ResetBTHID();
 }
 
-void BTHID::disconnect() { // Use this void to disconnect any of the controllers
+void BTHID::disconnect() { // Use this void to disconnect the device
         // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
-        pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid);
+        pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
         Reset();
         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
 }
 
 void BTHID::ACLData(uint8_t* l2capinbuf) {
-        if (!pBtd->l2capConnectionClaimed && pBtd->incomingHIDDevice && !connected && !activeConnection) {
-                if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
-                        if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+        if(!pBtd->l2capConnectionClaimed && pBtd->incomingHIDDevice && !connected && !activeConnection) {
+                if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
                                 pBtd->incomingHIDDevice = false;
                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
                                 activeConnection = true;
@@ -70,9 +68,10 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
         }
-        if ((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000)) { // acl_handle_ok or it's a new connection
-                if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { // l2cap_control - Channel ID for ACL-U
-                        if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
+        //if((l2capinbuf[0] | (uint16_t)l2capinbuf[1] << 8) == (hci_handle | 0x2000U)) { // acl_handle_ok or it's a new connection
+        if(UHS_ACL_HANDLE_OK(l2capinbuf, hci_handle)) { // acl_handle_ok or it's a new connection
+                if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { // l2cap_control - Channel ID for ACL-U
+                        if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -87,23 +86,23 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR(" "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 #endif
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
-                                if (((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
-                                        if (l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
+                                if(((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
+                                        if(l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Control Connection Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
                                                 control_scid[0] = l2capinbuf[12];
                                                 control_scid[1] = l2capinbuf[13];
-                                                l2cap_event_flag |= L2CAP_FLAG_CONTROL_CONNECTED;
-                                        } else if (l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
+                                                l2cap_set_flag(L2CAP_FLAG_CONTROL_CONNECTED);
+                                        } else if(l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Interrupt Connection Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
                                                 interrupt_scid[0] = l2capinbuf[12];
                                                 interrupt_scid[1] = l2capinbuf[13];
-                                                l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED;
+                                                l2cap_set_flag(L2CAP_FLAG_INTERRUPT_CONNECTED);
                                         }
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 #ifdef EXTRADEBUG
                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -116,46 +115,46 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR(" Identifier: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 #endif
-                                if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+                                if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
                                         identifier = l2capinbuf[9];
                                         control_scid[0] = l2capinbuf[14];
                                         control_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST;
-                                } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST);
+                                } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
                                         identifier = l2capinbuf[9];
                                         interrupt_scid[0] = l2capinbuf[14];
                                         interrupt_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST;
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
-                                if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
-                                        if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
+                                if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
+                                        if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS;
-                                        } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
+                                        } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS;
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS);
                                         }
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
-                                if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
+                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                         //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
-                                } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
                                         //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
-                                if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
+                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
 #endif
                                         identifier = l2capinbuf[9];
                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
                                         Reset();
-                                } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
 #endif
@@ -163,15 +162,15 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
                                         Reset();
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
-                                if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
+                                if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE;
-                                } else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
+                                } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE;
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
                                 }
                         }
 #ifdef EXTRADEBUG
@@ -181,37 +180,29 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
                         }
 #endif
-                } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
+                } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
 #ifdef PRINTREPORT
                         Notify(PSTR("\r\nL2CAP Interrupt: "), 0x80);
-                        for (uint16_t i = 0; i  < ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]); i++) {
+                        for(uint16_t i = 0; i < ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]); i++) {
                                 D_PrintHex<uint8_t > (l2capinbuf[i + 8], 0x80);
                                 Notify(PSTR(" "), 0x80);
                         }
 #endif
-                        if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
-                                switch (l2capinbuf[9]) {
-                                        case 0x01: // Keyboard events
-                                                if (pRptParser[KEYBOARD_PARSER_ID]) {
-                                                        uint16_t length =  ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]);
-                                                        pRptParser[KEYBOARD_PARSER_ID]->Parse(reinterpret_cast<HID *> (this), 0, (uint8_t) length, &l2capinbuf[10]); // Use reinterpret_cast again to extract the instance
-                                                }
+                        if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
+                                uint16_t length = ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]);
+                                ParseBTHIDData((uint8_t)(length - 1), &l2capinbuf[9]);
+
+                                switch(l2capinbuf[9]) {
+                                        case 0x01: // Keyboard or Joystick events
+                                                if(pRptParser[KEYBOARD_PARSER_ID])
+                                                        pRptParser[KEYBOARD_PARSER_ID]->Parse(reinterpret_cast<HID *>(this), 0, (uint8_t)(length - 2), &l2capinbuf[10]); // Use reinterpret_cast again to extract the instance
                                                 break;
 
                                         case 0x02: // Mouse events
-                                                if (pRptParser[MOUSE_PARSER_ID]) {
-                                                        uint16_t length =  ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]);
-                                                        pRptParser[MOUSE_PARSER_ID]->Parse(reinterpret_cast<HID *> (this), 0, (uint8_t) length, &l2capinbuf[10]); // Use reinterpret_cast again to extract the instance
-                                                }
+                                                if(pRptParser[MOUSE_PARSER_ID])
+                                                        pRptParser[MOUSE_PARSER_ID]->Parse(reinterpret_cast<HID *>(this), 0, (uint8_t)(length - 2), &l2capinbuf[10]); // Use reinterpret_cast again to extract the instance
                                                 break;
-
-                                        case 0x03:
-#ifdef DEBUG_USB_HOST
-                                                Notify(PSTR("\r\nChange mode event: "), 0x80);
-                                                D_PrintHex<uint8_t > (l2capinbuf[11], 0x80);
-#endif
-                                                break;
-#ifdef DEBUG_USB_HOST
+#ifdef EXTRADEBUG
                                         default:
                                                 Notify(PSTR("\r\nUnknown Report type: "), 0x80);
                                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
@@ -219,10 +210,10 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
 #endif
                                 }
                         }
-                } else if (l2capinbuf[6] == control_dcid[0] && l2capinbuf[7] == control_dcid[1]) { // l2cap_control
+                } else if(l2capinbuf[6] == control_dcid[0] && l2capinbuf[7] == control_dcid[1]) { // l2cap_control
 #ifdef PRINTREPORT
                         Notify(PSTR("\r\nL2CAP Control: "), 0x80);
-                        for (uint16_t i = 0; i  < ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]); i++) {
+                        for(uint16_t i = 0; i < ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]); i++) {
                                 D_PrintHex<uint8_t > (l2capinbuf[i + 8], 0x80);
                                 Notify(PSTR(" "), 0x80);
                         }
@@ -237,7 +228,7 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
 
                         Notify(PSTR("\r\nData: "), 0x80);
                         Notify(PSTR("\r\n"), 0x80);
-                        for (uint16_t i = 0; i  < ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]); i++) {
+                        for(uint16_t i = 0; i < ((uint16_t)l2capinbuf[5] << 8 | l2capinbuf[4]); i++) {
                                 D_PrintHex<uint8_t > (l2capinbuf[i + 8], 0x80);
                                 Notify(PSTR(" "), 0x80);
                         }
@@ -248,10 +239,10 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
 }
 
 void BTHID::L2CAP_task() {
-        switch (l2cap_state) {
+        switch(l2cap_state) {
                         /* These states are used if the HID device is the host */
                 case L2CAP_CONTROL_SUCCESS:
-                        if (l2cap_config_success_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
 #endif
@@ -261,7 +252,7 @@ void BTHID::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_SETUP:
-                        if (l2cap_connection_request_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
 #endif
@@ -278,7 +269,7 @@ void BTHID::L2CAP_task() {
 
                         /* These states are used if the Arduino is the host */
                 case L2CAP_CONTROL_CONNECT_REQUEST:
-                        if (l2cap_connected_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONTROL_CONNECTED)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSend HID Control Config Request"), 0x80);
 #endif
@@ -289,7 +280,7 @@ void BTHID::L2CAP_task() {
                         break;
 
                 case L2CAP_CONTROL_CONFIG_REQUEST:
-                        if (l2cap_config_success_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
                                 setProtocol(); // Set protocol before establishing HID interrupt channel
                                 delay(1); // Short delay between commands - just to be sure
 #ifdef DEBUG_USB_HOST
@@ -302,7 +293,7 @@ void BTHID::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_CONNECT_REQUEST:
-                        if (l2cap_connected_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_INTERRUPT_CONNECTED)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80);
 #endif
@@ -313,7 +304,7 @@ void BTHID::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_CONFIG_REQUEST:
-                        if (l2cap_config_success_interrupt_flag) { // Now the HID channels is established
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Channels Established"), 0x80);
 #endif
@@ -329,7 +320,7 @@ void BTHID::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_DISCONNECT:
-                        if (l2cap_disconnect_response_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
 #endif
@@ -340,7 +331,7 @@ void BTHID::L2CAP_task() {
                         break;
 
                 case L2CAP_CONTROL_DISCONNECT:
-                        if (l2cap_disconnect_response_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
 #endif
@@ -354,9 +345,9 @@ void BTHID::L2CAP_task() {
 }
 
 void BTHID::Run() {
-        switch (l2cap_state) {
+        switch(l2cap_state) {
                 case L2CAP_WAIT:
-                        if (pBtd->connectToHIDDevice && !pBtd->l2capConnectionClaimed && !connected && !activeConnection) {
+                        if(pBtd->connectToHIDDevice && !pBtd->l2capConnectionClaimed && !connected && !activeConnection) {
                                 pBtd->l2capConnectionClaimed = true;
                                 activeConnection = true;
 #ifdef DEBUG_USB_HOST
@@ -367,7 +358,7 @@ void BTHID::Run() {
                                 identifier = 0;
                                 pBtd->l2cap_connection_request(hci_handle, identifier, control_dcid, HID_CTRL_PSM);
                                 l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
-                        } else if (l2cap_connection_request_control_flag) {
+                        } else if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
 #endif
@@ -385,13 +376,20 @@ void BTHID::Run() {
 
 /************************************************************/
 /*                    HID Commands                          */
+
 /************************************************************/
 void BTHID::setProtocol() {
 #ifdef DEBUG_USB_HOST
         Notify(PSTR("\r\nSet protocol mode: "), 0x80);
         D_PrintHex<uint8_t > (protocolMode, 0x80);
 #endif
-        uint8_t command = 0x70 | protocolMode; // Set Protocol, see HID specs page 33
+        if (protocolMode != HID_BOOT_PROTOCOL && protocolMode != HID_RPT_PROTOCOL) {
+#ifdef DEBUG_USB_HOST
+                Notify(PSTR("\r\nNot a valid protocol mode. Using Boot protocol instead."), 0x80);
+#endif
+                protocolMode = HID_BOOT_PROTOCOL; // Use Boot Protocol by default
+        }
+        uint8_t command = 0x70 | protocolMode; // Set Protocol, see Bluetooth HID specs page 33
         pBtd->L2CAP_Command(hci_handle, &command, 1, control_scid[0], control_scid[1]);
 }
 
@@ -401,4 +399,4 @@ void BTHID::setLeds(uint8_t data) {
         buf[1] = 0x01; // Report ID
         buf[2] = data;
         pBtd->L2CAP_Command(hci_handle, buf, 3, interrupt_scid[0], interrupt_scid[1]);
-}
\ No newline at end of file
+}
diff --git a/BTHID.h b/BTHID.h
index 7eae9494..2419e58b 100644
--- a/BTHID.h
+++ b/BTHID.h
@@ -21,49 +21,11 @@
 #include "BTD.h"
 #include "hidboot.h"
 
-/* Bluetooth L2CAP states for L2CAP_task() */
-#define L2CAP_WAIT                      0
-
-// These states are used if the device is the host
-#define L2CAP_CONTROL_SUCCESS           1
-#define L2CAP_INTERRUPT_SETUP           2
-
-// These states are used if the Arduino is the host
-#define L2CAP_CONTROL_CONNECT_REQUEST   3
-#define L2CAP_CONTROL_CONFIG_REQUEST    4
-#define L2CAP_INTERRUPT_CONNECT_REQUEST 5
-
-#define L2CAP_INTERRUPT_CONFIG_REQUEST  6
-#define L2CAP_DONE                      7
-
-#define L2CAP_INTERRUPT_DISCONNECT      8
-#define L2CAP_CONTROL_DISCONNECT        9
-
-/* L2CAP event flags */
-#define L2CAP_FLAG_CONTROL_CONNECTED                0x01
-#define L2CAP_FLAG_INTERRUPT_CONNECTED              0x02
-#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS           0x04
-#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS         0x08
-#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE      0x10
-#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE    0x20
-#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST       0x40
-#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST     0x80
-
-/* Macros for L2CAP event flag tests */
-#define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED)
-#define l2cap_connected_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_INTERRUPT_CONNECTED)
-#define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)
-#define l2cap_config_success_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)
-#define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)
-#define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)
-#define l2cap_connection_request_control_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)
-#define l2cap_connection_request_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)
-
 #define KEYBOARD_PARSER_ID      0
 #define MOUSE_PARSER_ID         1
-#define epMUL                   2
+#define NUM_PARSERS             2
 
-/** This BluetoothService class implements support for the HID keyboard and mice. */
+/** This BluetoothService class implements support for Bluetooth HID devices. */
 class BTHID : public BluetoothService {
 public:
         /**
@@ -80,82 +42,130 @@ public:
          * @param ACLData Incoming acldata.
          */
         virtual void ACLData(uint8_t* ACLData);
-        /** Used to run part of the state maschine. */
+        /** Used to run part of the state machine. */
         virtual void Run();
         /** Use this to reset the service. */
         virtual void Reset();
-        /** Used this to disconnect any of the controllers. */
+        /** Used this to disconnect the devices. */
         virtual void disconnect();
         /**@}*/
 
+        /**
+         * Get HIDReportParser.
+         * @param  id ID of parser.
+         * @return    Returns the corresponding HIDReportParser. Returns NULL if id is not valid.
+         */
         HIDReportParser *GetReportParser(uint8_t id) {
+                if (id >= NUM_PARSERS)
+                        return NULL;
                 return pRptParser[id];
         };
 
+        /**
+         * Set HIDReportParser to be used.
+         * @param  id  Id of parser.
+         * @param  prs Pointer to HIDReportParser.
+         * @return     Returns true if the HIDReportParser is set. False otherwise.
+         */
         bool SetReportParser(uint8_t id, HIDReportParser *prs) {
+                if (id >= NUM_PARSERS)
+                        return false;
                 pRptParser[id] = prs;
                 return true;
         };
 
+        /**
+         * Set HID protocol mode.
+         * @param mode HID protocol to use. Either HID_BOOT_PROTOCOL or HID_RPT_PROTOCOL.
+         */
         void setProtocolMode(uint8_t mode) {
                 protocolMode = mode;
         };
 
-        /** Used to set the leds on a keyboard */
+        /**
+         * Used to set the leds on a keyboard.
+         * @param data See KBDLEDS in hidboot.h
+         */
         void setLeds(uint8_t data);
 
         /** True if a device is connected */
         bool connected;
 
-        /** Call this to start the paring sequence with a controller */
+        /** Call this to start the paring sequence with a device */
         void pair(void) {
-                if (pBtd)
+                if(pBtd)
                         pBtd->pairWithHID();
         };
 
         /**
-         * Used to call your own function when the controller is successfully initialized.
+         * Used to call your own function when the device is successfully initialized.
          * @param funcOnInit Function to call.
          */
         void attachOnInit(void (*funcOnInit)(void)) {
                 pFuncOnInit = funcOnInit;
         };
 
-private:
-        BTD *pBtd; // Pointer to BTD instance
+protected:
+        /** @name Overridable functions */
+        /**
+         * Used to parse Bluetooth HID data to any class that inherits this class.
+         * @param len The length of the incoming data.
+         * @param buf Pointer to the data buffer.
+         */
+        virtual void ParseBTHIDData(uint8_t len, uint8_t *buf) {
+                return;
+        };
+        /** Called when a device is connected */
+        virtual void OnInitBTHID() {
+                return;
+        };
+        /** Used to reset any buffers in the class that inherits this */
+        virtual void ResetBTHID() {
+                return;
+        }
+        /**@}*/
 
-        HIDReportParser *pRptParser[epMUL];
+        /** Pointer to BTD instance */
+        BTD *pBtd;
+
+        /** HCI Handle for connection */
+        uint16_t hci_handle;
+
+        /** L2CAP source CID for HID_Control */
+
+        uint8_t control_scid[2];
+
+        /** L2CAP source CID for HID_Interrupt */
+        uint8_t interrupt_scid[2];
+
+private:
+        HIDReportParser *pRptParser[NUM_PARSERS]; // Pointer to HIDReportParsers.
 
         /** Set report protocol. */
         void setProtocol();
         uint8_t protocolMode;
 
         /**
-         * Called when the controller is successfully initialized.
+         * Called when a device is successfully initialized.
          * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
          * This is useful for instance if you want to set the LEDs in a specific way.
          */
         void onInit() {
-                if (pFuncOnInit)
+                if(pFuncOnInit)
                         pFuncOnInit(); // Call the user function
-        }
+                OnInitBTHID();
+        };
         void (*pFuncOnInit)(void); // Pointer to function called in onInit()
 
         void L2CAP_task(); // L2CAP state machine
 
-        /* Variables filled from HCI event management */
-        uint16_t hci_handle;
-        bool activeConnection; // Used to indicate if it's already has established a connection
+        bool activeConnection; // Used to indicate if it already has established a connection
 
-        /* Variables used by high level L2CAP task */
-        uint8_t l2cap_state;
-        uint8_t l2cap_event_flag; // l2cap flags of received Bluetooth events
-
-        /* L2CAP Channels */
-        uint8_t control_scid[2]; // L2CAP source CID for HID_Control
-        uint8_t control_dcid[2]; // 0x0070
-        uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt
-        uint8_t interrupt_dcid[2]; // 0x0071
+        /* Variables used for L2CAP communication */
+        uint8_t control_dcid[2]; // L2CAP device CID for HID_Control - Always 0x0070
+        uint8_t interrupt_dcid[2]; // L2CAP device CID for HID_Interrupt - Always 0x0071
         uint8_t identifier; // Identifier for connection
+        uint8_t l2cap_state;
+        uint32_t l2cap_event_flag; // l2cap flags of received Bluetooth events
 };
-#endif
\ No newline at end of file
+#endif
diff --git a/PS3BT.cpp b/PS3BT.cpp
index 9ed5da75..38d6c3dc 100644
--- a/PS3BT.cpp
+++ b/PS3BT.cpp
@@ -23,7 +23,7 @@
 PS3BT::PS3BT(BTD *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0) :
 pBtd(p) // pointer to USB class instance - mandatory
 {
-        if (pBtd)
+        if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
 
         pBtd->my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
@@ -49,35 +49,35 @@ pBtd(p) // pointer to USB class instance - mandatory
         Reset();
 }
 
-bool PS3BT::getButtonPress(Button b) {
-        return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b]));
+bool PS3BT::getButtonPress(ButtonEnum b) {
+        return (ButtonState & pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]));
 }
 
-bool PS3BT::getButtonClick(Button b) {
-        uint32_t button = pgm_read_dword(&BUTTONS[(uint8_t)b]);
+bool PS3BT::getButtonClick(ButtonEnum b) {
+        uint32_t button = pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]);
         bool click = (ButtonClickState & button);
         ButtonClickState &= ~button; // Clear "click" event
         return click;
 }
 
-uint8_t PS3BT::getAnalogButton(Button a) {
-        return (uint8_t)(l2capinbuf[pgm_read_byte(&ANALOGBUTTONS[(uint8_t)a])]);
+uint8_t PS3BT::getAnalogButton(ButtonEnum a) {
+        return (uint8_t)(l2capinbuf[pgm_read_byte(&PS3_ANALOG_BUTTONS[(uint8_t)a])]);
 }
 
-uint8_t PS3BT::getAnalogHat(AnalogHat a) {
+uint8_t PS3BT::getAnalogHat(AnalogHatEnum a) {
         return (uint8_t)(l2capinbuf[(uint8_t)a + 15]);
 }
 
-int16_t PS3BT::getSensor(Sensor a) {
-        if (PS3Connected) {
-                if (a == aX || a == aY || a == aZ || a == gZ)
+int16_t PS3BT::getSensor(SensorEnum a) {
+        if(PS3Connected) {
+                if(a == aX || a == aY || a == aZ || a == gZ)
                         return ((l2capinbuf[(uint16_t)a] << 8) | l2capinbuf[(uint16_t)a + 1]);
                 else
                         return 0;
-        } else if (PS3MoveConnected) {
-                if (a == mXmove || a == mYmove) // These are all 12-bits long
+        } else if(PS3MoveConnected) {
+                if(a == mXmove || a == mYmove) // These are all 12-bits long
                         return (((l2capinbuf[(uint16_t)a] & 0x0F) << 8) | (l2capinbuf[(uint16_t)a + 1]));
-                else if (a == mZmove || a == tempMove) // The tempearature is also 12 bits long
+                else if(a == mZmove || a == tempMove) // The tempearature is also 12 bits long
                         return ((l2capinbuf[(uint16_t)a] << 4) | ((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4));
                 else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
                         return (l2capinbuf[(uint16_t)a] | (l2capinbuf[(uint16_t)a + 1] << 8));
@@ -85,16 +85,16 @@ int16_t PS3BT::getSensor(Sensor a) {
                 return 0;
 }
 
-double PS3BT::getAngle(Angle a) {
+double PS3BT::getAngle(AngleEnum a) {
         double accXval, accYval, accZval;
 
-        if (PS3Connected) {
+        if(PS3Connected) {
                 // Data for the Kionix KXPC4 used in the DualShock 3
                 const double zeroG = 511.5; // 1.65/3.3*1023 (1.65V)
                 accXval = -((double)getSensor(aX) - zeroG);
                 accYval = -((double)getSensor(aY) - zeroG);
                 accZval = -((double)getSensor(aZ) - zeroG);
-        } else if (PS3MoveConnected) {
+        } else if(PS3MoveConnected) {
                 // It's a Kionix KXSC4 inside the Motion controller
                 const uint16_t zeroG = 0x8000;
                 accXval = -(int16_t)(getSensor(aXmove) - zeroG);
@@ -106,34 +106,34 @@ double PS3BT::getAngle(Angle a) {
         // Convert to 360 degrees resolution
         // atan2 outputs the value of -π to π (radians)
         // We are then converting it to 0 to 2π and then to degrees
-        if (a == Pitch)
+        if(a == Pitch)
                 return (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
         else
                 return (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
 }
 
-double PS3BT::get9DOFValues(Sensor a) { // Thanks to Manfred Piendl
-        if (!PS3MoveConnected)
+double PS3BT::get9DOFValues(SensorEnum a) { // Thanks to Manfred Piendl
+        if(!PS3MoveConnected)
                 return 0;
         int16_t value = getSensor(a);
-        if (a == mXmove || a == mYmove || a == mZmove) {
-                if (value > 2047)
+        if(a == mXmove || a == mYmove || a == mZmove) {
+                if(value > 2047)
                         value -= 0x1000;
                 return (double)value / 3.2; // unit: muT = 10^(-6) Tesla
-        } else if (a == aXmove || a == aYmove || a == aZmove) {
-                if (value < 0)
+        } else if(a == aXmove || a == aYmove || a == aZmove) {
+                if(value < 0)
                         value += 0x8000;
                 else
                         value -= 0x8000;
                 return (double)value / 442.0; // unit: m/(s^2)
-        } else if (a == gXmove || a == gYmove || a == gZmove) {
-                if (value < 0)
+        } else if(a == gXmove || a == gYmove || a == gZmove) {
+                if(value < 0)
                         value += 0x8000;
                 else
                         value -= 0x8000;
-                if (a == gXmove)
+                if(a == gXmove)
                         return (double)value / 11.6; // unit: deg/s
-                else if (a == gYmove)
+                else if(a == gYmove)
                         return (double)value / 11.2; // unit: deg/s
                 else // gZmove
                         return (double)value / 9.6; // unit: deg/s
@@ -142,12 +142,12 @@ double PS3BT::get9DOFValues(Sensor a) { // Thanks to Manfred Piendl
 }
 
 String PS3BT::getTemperature() {
-        if (PS3MoveConnected) {
+        if(PS3MoveConnected) {
                 int16_t input = getSensor(tempMove);
 
                 String output = String(input / 100);
                 output += ".";
-                if (input % 100 < 10)
+                if(input % 100 < 10)
                         output += "0";
                 output += String(input % 100);
 
@@ -156,58 +156,52 @@ String PS3BT::getTemperature() {
                 return "Error";
 }
 
-bool PS3BT::getStatus(Status c) {
+bool PS3BT::getStatus(StatusEnum c) {
         return (l2capinbuf[(uint16_t)c >> 8] == ((uint8_t)c & 0xff));
 }
 
-String PS3BT::getStatusString() {
-        if (PS3Connected || PS3NavigationConnected) {
-                char statusOutput[100];
+void PS3BT::printStatusString() {
+        char statusOutput[100]; // Max string length plus null character
+        if(PS3Connected || PS3NavigationConnected) {
+                strcpy_P(statusOutput, PSTR("ConnectionStatus: "));
 
-                strcpy(statusOutput, "ConnectionStatus: ");
+                if(getStatus(Plugged)) strcat_P(statusOutput, PSTR("Plugged"));
+                else if(getStatus(Unplugged)) strcat_P(statusOutput, PSTR("Unplugged"));
+                else strcat_P(statusOutput, PSTR("Error"));
 
-                if (getStatus(Plugged)) strcat(statusOutput, "Plugged");
-                else if (getStatus(Unplugged)) strcat(statusOutput, "Unplugged");
-                else strcat(statusOutput, "Error");
+                strcat_P(statusOutput, PSTR(" - PowerRating: "));
 
+                if(getStatus(Charging)) strcat_P(statusOutput, PSTR("Charging"));
+                else if(getStatus(NotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
+                else if(getStatus(Shutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
+                else if(getStatus(Dying)) strcat_P(statusOutput, PSTR("Dying"));
+                else if(getStatus(Low)) strcat_P(statusOutput, PSTR("Low"));
+                else if(getStatus(High)) strcat_P(statusOutput, PSTR("High"));
+                else if(getStatus(Full)) strcat_P(statusOutput, PSTR("Full"));
+                else strcat_P(statusOutput, PSTR("Error"));
 
-                strcat(statusOutput, " - PowerRating: ");
-                if (getStatus(Charging)) strcat(statusOutput, "Charging");
-                else if (getStatus(NotCharging)) strcat(statusOutput, "Not Charging");
-                else if (getStatus(Shutdown)) strcat(statusOutput, "Shutdown");
-                else if (getStatus(Dying)) strcat(statusOutput, "Dying");
-                else if (getStatus(Low)) strcat(statusOutput, "Low");
-                else if (getStatus(High)) strcat(statusOutput, "High");
-                else if (getStatus(Full)) strcat(statusOutput, "Full");
-                else strcat(statusOutput, "Error");
+                strcat_P(statusOutput, PSTR(" - WirelessStatus: "));
 
-                strcat(statusOutput, " - WirelessStatus: ");
+                if(getStatus(CableRumble)) strcat_P(statusOutput, PSTR("Cable - Rumble is on"));
+                else if(getStatus(Cable)) strcat_P(statusOutput, PSTR("Cable - Rumble is off"));
+                else if(getStatus(BluetoothRumble)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is on"));
+                else if(getStatus(Bluetooth)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is off"));
+                else strcat_P(statusOutput, PSTR("Error"));
+        } else if(PS3MoveConnected) {
+                strcpy_P(statusOutput, PSTR("PowerRating: "));
 
-                if (getStatus(CableRumble)) strcat(statusOutput, "Cable - Rumble is on");
-                else if (getStatus(Cable)) strcat(statusOutput, "Cable - Rumble is off");
-                else if (getStatus(BluetoothRumble)) strcat(statusOutput, "Bluetooth - Rumble is on");
-                else if (getStatus(Bluetooth)) strcat(statusOutput, "Bluetooth - Rumble is off");
-                else strcat(statusOutput, "Error");
-
-                return statusOutput;
-
-        } else if (PS3MoveConnected) {
-                char statusOutput[50];
-
-                strcpy(statusOutput, "PowerRating: ");
-
-                if (getStatus(MoveCharging)) strcat(statusOutput, "Charging");
-                else if (getStatus(MoveNotCharging)) strcat(statusOutput, "Not Charging");
-                else if (getStatus(MoveShutdown)) strcat(statusOutput, "Shutdown");
-                else if (getStatus(MoveDying)) strcat(statusOutput, "Dying");
-                else if (getStatus(MoveLow)) strcat(statusOutput, "Low");
-                else if (getStatus(MoveHigh)) strcat(statusOutput, "High");
-                else if (getStatus(MoveFull)) strcat(statusOutput, "Full");
-                else strcat(statusOutput, "Error");
-
-                return statusOutput;
+                if(getStatus(MoveCharging)) strcat_P(statusOutput, PSTR("Charging"));
+                else if(getStatus(MoveNotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
+                else if(getStatus(MoveShutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
+                else if(getStatus(MoveDying)) strcat_P(statusOutput, PSTR("Dying"));
+                else if(getStatus(MoveLow)) strcat_P(statusOutput, PSTR("Low"));
+                else if(getStatus(MoveHigh)) strcat_P(statusOutput, PSTR("High"));
+                else if(getStatus(MoveFull)) strcat_P(statusOutput, PSTR("Full"));
+                else strcat_P(statusOutput, PSTR("Error"));
         } else
-                return "Error";
+                strcpy_P(statusOutput, PSTR("Error"));
+
+        USB_HOST_SERIAL.write(statusOutput);
 }
 
 void PS3BT::Reset() {
@@ -218,30 +212,30 @@ void PS3BT::Reset() {
         l2cap_event_flag = 0; // Reset flags
         l2cap_state = L2CAP_WAIT;
 
-        // Needed for PS3 Dualshock Controller commands to work via bluetooth
-        for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
+        // Needed for PS3 Dualshock Controller commands to work via Bluetooth
+        for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
                 HIDBuffer[i + 2] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // First two bytes reserved for report type and ID
 }
 
 void PS3BT::disconnect() { // Use this void to disconnect any of the controllers
-        //First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection
-        pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid);
+        // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
+        pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
         Reset();
         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
 }
 
 void PS3BT::ACLData(uint8_t* ACLData) {
-        if (!pBtd->l2capConnectionClaimed && !PS3Connected && !PS3MoveConnected && !PS3NavigationConnected && !activeConnection && !pBtd->connectToWii && !pBtd->incomingWii && !pBtd->pairWithWii) {
-                if (ACLData[8] == L2CAP_CMD_CONNECTION_REQUEST) {
-                        if ((ACLData[12] | (ACLData[13] << 8)) == HID_CTRL_PSM) {
+        if(!pBtd->l2capConnectionClaimed && !PS3Connected && !PS3MoveConnected && !PS3NavigationConnected && !activeConnection && !pBtd->connectToWii && !pBtd->incomingWii && !pBtd->pairWithWii) {
+                if(ACLData[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        if((ACLData[12] | (ACLData[13] << 8)) == HID_CTRL_PSM) {
                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
                                 activeConnection = true;
                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
                                 l2cap_state = L2CAP_WAIT;
-                                for (uint8_t i = 0; i < 30; i++)
+                                for(uint8_t i = 0; i < 30; i++)
                                         remote_name[i] = pBtd->remote_name[i]; // Store the remote name for the connection
 #ifdef DEBUG_USB_HOST
-                                if (pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle
+                                if(pBtd->hci_version < 3) { // Check the HCI Version of the Bluetooth dongle
                                         Notify(PSTR("\r\nYour dongle may not support reading the analog buttons, sensors and status\r\nYour HCI Version is: "), 0x80);
                                         Notify(pBtd->hci_version, 0x80);
                                         Notify(PSTR("\r\nBut should be at least 3\r\nThis means that it doesn't support Bluetooth Version 2.0+EDR"), 0x80);
@@ -250,10 +244,11 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                         }
                 }
         }
-        if (((ACLData[0] | (ACLData[1] << 8)) == (hci_handle | 0x2000))) { //acl_handle_ok
+        //if((ACLData[0] | (uint16_t)ACLData[1] << 8) == (hci_handle | 0x2000U)) { //acl_handle_ok
+        if(UHS_ACL_HANDLE_OK(ACLData, hci_handle)) { //acl_handle_ok
                 memcpy(l2capinbuf, ACLData, BULK_MAXPKTSIZE);
-                if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
-                        if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
+                if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { //l2cap_control - Channel ID for ACL-U
+                        if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -268,7 +263,7 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                                 Notify(PSTR(" "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 #endif
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 #ifdef EXTRADEBUG
                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -281,46 +276,44 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                                 Notify(PSTR(" Identifier: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 #endif
-                                if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+                                if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
                                         identifier = l2capinbuf[9];
                                         control_scid[0] = l2capinbuf[14];
                                         control_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST;
-                                } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST);
+                                } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
                                         identifier = l2capinbuf[9];
                                         interrupt_scid[0] = l2capinbuf[14];
                                         interrupt_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST;
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
-                                if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
-                                        if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
+                                if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
+                                        if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS;
-                                        } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
+                                        } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS;
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS);
                                         }
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
-                                if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
+                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                         //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
-                                        identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_REQUEST;
-                                } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                        pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
+                                } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
                                         //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
-                                        identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST;
+                                        pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
-                                if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
+                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
 #endif
                                         identifier = l2capinbuf[9];
                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
                                         Reset();
-                                } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
 #endif
@@ -328,15 +321,15 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
                                         Reset();
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
-                                if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
+                                if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE;
-                                } else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
+                                } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE;
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
                                 }
                         }
 #ifdef EXTRADEBUG
@@ -345,26 +338,28 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
                         }
 #endif
-                } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
+                } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
                         //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
-                        if (PS3Connected || PS3MoveConnected || PS3NavigationConnected) {
+                        if(PS3Connected || PS3MoveConnected || PS3NavigationConnected) {
                                 /* Read Report */
-                                if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
-                                        if (PS3Connected || PS3NavigationConnected)
+                                if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
+                                        lastMessageTime = millis(); // Store the last message time
+
+                                        if(PS3Connected || PS3NavigationConnected)
                                                 ButtonState = (uint32_t)(l2capinbuf[11] | ((uint16_t)l2capinbuf[12] << 8) | ((uint32_t)l2capinbuf[13] << 16));
-                                        else if (PS3MoveConnected)
+                                        else if(PS3MoveConnected)
                                                 ButtonState = (uint32_t)(l2capinbuf[10] | ((uint16_t)l2capinbuf[11] << 8) | ((uint32_t)l2capinbuf[12] << 16));
 
                                         //Notify(PSTR("\r\nButtonState", 0x80);
                                         //PrintHex<uint32_t>(ButtonState, 0x80);
 
-                                        if (ButtonState != OldButtonState) {
+                                        if(ButtonState != OldButtonState) {
                                                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
                                                 OldButtonState = ButtonState;
                                         }
 
 #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
-                                        for (uint8_t i = 10; i < 58; i++) {
+                                        for(uint8_t i = 10; i < 58; i++) {
                                                 D_PrintHex<uint8_t > (l2capinbuf[i], 0x80);
                                                 Notify(PSTR(" "), 0x80);
                                         }
@@ -378,9 +373,9 @@ void PS3BT::ACLData(uint8_t* ACLData) {
 }
 
 void PS3BT::L2CAP_task() {
-        switch (l2cap_state) {
+        switch(l2cap_state) {
                 case L2CAP_WAIT:
-                        if (l2cap_connection_request_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
 #endif
@@ -390,29 +385,21 @@ void PS3BT::L2CAP_task() {
                                 identifier++;
                                 delay(1);
                                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
-                                l2cap_state = L2CAP_CONTROL_REQUEST;
-                        }
-                        break;
-                case L2CAP_CONTROL_REQUEST:
-                        if (l2cap_config_request_control_flag) {
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
-#endif
-                                pBtd->l2cap_config_response(hci_handle, identifier, control_scid);
                                 l2cap_state = L2CAP_CONTROL_SUCCESS;
                         }
                         break;
 
                 case L2CAP_CONTROL_SUCCESS:
-                        if (l2cap_config_success_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
 #endif
                                 l2cap_state = L2CAP_INTERRUPT_SETUP;
                         }
                         break;
+
                 case L2CAP_INTERRUPT_SETUP:
-                        if (l2cap_connection_request_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
 #endif
@@ -423,28 +410,20 @@ void PS3BT::L2CAP_task() {
                                 delay(1);
                                 pBtd->l2cap_config_request(hci_handle, identifier, interrupt_scid);
 
-                                l2cap_state = L2CAP_INTERRUPT_REQUEST;
+                                l2cap_state = L2CAP_INTERRUPT_CONFIG_REQUEST;
                         }
                         break;
-                case L2CAP_INTERRUPT_REQUEST:
-                        if (l2cap_config_request_interrupt_flag) {
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
-#endif
-                                pBtd->l2cap_config_response(hci_handle, identifier, interrupt_scid);
-                                l2cap_state = L2CAP_INTERRUPT_SUCCESS;
-                        }
-                        break;
-                case L2CAP_INTERRUPT_SUCCESS:
-                        if (l2cap_config_success_interrupt_flag) {
+
+                case L2CAP_INTERRUPT_CONFIG_REQUEST:
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Interrupt Successfully Configured"), 0x80);
 #endif
-                                if (remote_name[0] == 'M') { // First letter in Motion Controller ('M')
+                                if(remote_name[0] == 'M') { // First letter in Motion Controller ('M')
                                         memset(l2capinbuf, 0, BULK_MAXPKTSIZE); // Reset l2cap in buffer as it sometimes read it as a button has been pressed
-                                        l2cap_state = L2CAP_HID_PS3_LED;
+                                        l2cap_state = TURN_ON_LED;
                                 } else
-                                        l2cap_state = L2CAP_HID_ENABLE_SIXAXIS;
+                                        l2cap_state = PS3_ENABLE_SIXAXIS;
                                 timer = millis();
                         }
                         break;
@@ -452,7 +431,7 @@ void PS3BT::L2CAP_task() {
                         /* These states are handled in Run() */
 
                 case L2CAP_INTERRUPT_DISCONNECT:
-                        if (l2cap_disconnect_response_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
 #endif
@@ -463,7 +442,7 @@ void PS3BT::L2CAP_task() {
                         break;
 
                 case L2CAP_CONTROL_DISCONNECT:
-                        if (l2cap_disconnect_response_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
 #endif
@@ -477,31 +456,31 @@ void PS3BT::L2CAP_task() {
 }
 
 void PS3BT::Run() {
-        switch (l2cap_state) {
-                case L2CAP_HID_ENABLE_SIXAXIS:
-                        if (millis() - timer > 1000) { // loop 1 second before sending the command
+        switch(l2cap_state) {
+                case PS3_ENABLE_SIXAXIS:
+                        if(millis() - timer > 1000) { // loop 1 second before sending the command
                                 memset(l2capinbuf, 0, BULK_MAXPKTSIZE); // Reset l2cap in buffer as it sometimes read it as a button has been pressed
-                                for (uint8_t i = 15; i < 19; i++)
+                                for(uint8_t i = 15; i < 19; i++)
                                         l2capinbuf[i] = 0x7F; // Set the analog joystick values to center position
                                 enable_sixaxis();
-                                l2cap_state = L2CAP_HID_PS3_LED;
+                                l2cap_state = TURN_ON_LED;
                                 timer = millis();
                         }
                         break;
 
-                case L2CAP_HID_PS3_LED:
-                        if (millis() - timer > 1000) { // loop 1 second before sending the command
-                                if (remote_name[0] == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P')
+                case TURN_ON_LED:
+                        if(millis() - timer > 1000) { // loop 1 second before sending the command
+                                if(remote_name[0] == 'P') { // First letter in PLAYSTATION(R)3 Controller ('P')
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDualshock 3 Controller Enabled\r\n"), 0x80);
 #endif
                                         PS3Connected = true;
-                                } else if (remote_name[0] == 'N') { // First letter in Navigation Controller ('N')
+                                } else if(remote_name[0] == 'N') { // First letter in Navigation Controller ('N')
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nNavigation Controller Enabled\r\n"), 0x80);
 #endif
                                         PS3NavigationConnected = true;
-                                } else if (remote_name[0] == 'M') { // First letter in Motion Controller ('M')
+                                } else if(remote_name[0] == 'M') { // First letter in Motion Controller ('M')
                                         timerBulbRumble = millis();
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nMotion Controller Enabled\r\n"), 0x80);
@@ -518,8 +497,8 @@ void PS3BT::Run() {
                         break;
 
                 case L2CAP_DONE:
-                        if (PS3MoveConnected) { // The Bulb and rumble values, has to be send at aproximatly every 5th second for it to stay on
-                                if (millis() - timerBulbRumble > 4000) { // Send at least every 4th second
+                        if(PS3MoveConnected) { // The Bulb and rumble values, has to be send at aproximatly every 5th second for it to stay on
+                                if(millis() - timerBulbRumble > 4000) { // Send at least every 4th second
                                         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on
                                         timerBulbRumble = millis();
                                 }
@@ -535,7 +514,7 @@ void PS3BT::Run() {
 // Playstation Sixaxis Dualshock and Navigation Controller commands
 
 void PS3BT::HID_Command(uint8_t* data, uint8_t nbytes) {
-        if (millis() - timerHID <= 150) // Check if is has been more than 150ms since last command
+        if(millis() - timerHID <= 150) // Check if is has been more than 150ms since last command
                 delay((uint32_t)(150 - (millis() - timerHID))); // There have to be a delay between commands
         pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]); // Both the Navigation and Dualshock controller sends data via the control channel
         timerHID = millis();
@@ -561,9 +540,9 @@ void PS3BT::setRumbleOff() {
         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 }
 
-void PS3BT::setRumbleOn(Rumble mode) {
-        uint8_t power[2] = { 0xff, 0x00 }; // Defaults to RumbleLow
-        if (mode == RumbleHigh) {
+void PS3BT::setRumbleOn(RumbleEnum mode) {
+        uint8_t power[2] = {0xff, 0x00}; // Defaults to RumbleLow
+        if(mode == RumbleHigh) {
                 power[0] = 0x00;
                 power[1] = 0xff;
         }
@@ -583,18 +562,22 @@ void PS3BT::setLedRaw(uint8_t value) {
         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 }
 
-void PS3BT::setLedOff(LED a) {
-        HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1));
+void PS3BT::setLedOff(LEDEnum a) {
+        HIDBuffer[11] &= ~((uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1));
         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 }
 
-void PS3BT::setLedOn(LED a) {
-        HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1);
-        HID_Command(HIDBuffer, HID_BUFFERSIZE);
+void PS3BT::setLedOn(LEDEnum a) {
+        if(a == OFF)
+                setLedRaw(0);
+        else {
+                HIDBuffer[11] |= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
+                HID_Command(HIDBuffer, HID_BUFFERSIZE);
+        }
 }
 
-void PS3BT::setLedToggle(LED a) {
-        HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1);
+void PS3BT::setLedToggle(LEDEnum a) {
+        HIDBuffer[11] ^= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
         HID_Command(HIDBuffer, HID_BUFFERSIZE);
 }
 
@@ -613,13 +596,13 @@ void PS3BT::enable_sixaxis() { // Command used to enable the Dualshock 3 and Nav
 // Playstation Move Controller commands
 
 void PS3BT::HIDMove_Command(uint8_t* data, uint8_t nbytes) {
-        if (millis() - timerHID <= 150)// Check if is has been less than 150ms since last command
+        if(millis() - timerHID <= 150)// Check if is has been less than 150ms since last command
                 delay((uint32_t)(150 - (millis() - timerHID))); // There have to be a delay between commands
         pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // The Move controller sends it's data via the intterrupt channel
         timerHID = millis();
 }
 
-void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the Color using RGB values
+void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { // Use this to set the Color using RGB values
         // Set the Bulb's values into the write buffer
         HIDMoveBuffer[3] = r;
         HIDMoveBuffer[4] = g;
@@ -628,13 +611,13 @@ void PS3BT::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { //Use this to set the
         HIDMove_Command(HIDMoveBuffer, HID_BUFFERSIZE);
 }
 
-void PS3BT::moveSetBulb(Colors color) { //Use this to set the Color using the predefined colors in enum
+void PS3BT::moveSetBulb(ColorsEnum color) { // Use this to set the Color using the predefined colors in enum
         moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
 }
 
 void PS3BT::moveSetRumble(uint8_t rumble) {
 #ifdef DEBUG_USB_HOST
-        if (rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
+        if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
                 Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
 #endif
         // Set the rumble value into the write buffer
@@ -644,12 +627,12 @@ void PS3BT::moveSetRumble(uint8_t rumble) {
 }
 
 void PS3BT::onInit() {
-        if (pFuncOnInit)
+        if(pFuncOnInit)
                 pFuncOnInit(); // Call the user function
         else {
-                if (PS3MoveConnected)
+                if(PS3MoveConnected)
                         moveSetBulb(Red);
                 else // Dualshock 3 or Navigation controller
                         setLedOn(LED1);
         }
-}
\ No newline at end of file
+}
diff --git a/PS3BT.h b/PS3BT.h
index 6b901402..d07a4577 100644
--- a/PS3BT.h
+++ b/PS3BT.h
@@ -21,40 +21,7 @@
 #include "BTD.h"
 #include "PS3Enums.h"
 
-#define HID_BUFFERSIZE              50 // Size of the buffer for the Playstation Motion Controller
-
-/* Bluetooth L2CAP states for L2CAP_task() */
-#define L2CAP_WAIT                   0
-#define L2CAP_CONTROL_REQUEST        1
-#define L2CAP_CONTROL_SUCCESS        2
-#define L2CAP_INTERRUPT_SETUP        3
-#define L2CAP_INTERRUPT_REQUEST      4
-#define L2CAP_INTERRUPT_SUCCESS      5
-#define L2CAP_HID_ENABLE_SIXAXIS     6
-#define L2CAP_HID_PS3_LED            7
-#define L2CAP_DONE                   8
-#define L2CAP_INTERRUPT_DISCONNECT   9
-#define L2CAP_CONTROL_DISCONNECT     10
-
-/* L2CAP event flags */
-#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST     0x01
-#define L2CAP_FLAG_CONFIG_CONTROL_REQUEST         0x02
-#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS         0x04
-#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST   0x08
-#define L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST       0x10
-#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS       0x20
-#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE    0x40
-#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE  0x80
-
-/*Macros for L2CAP event flag tests */
-#define l2cap_connection_request_control_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)
-#define l2cap_config_request_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_REQUEST)
-#define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)
-#define l2cap_connection_request_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)
-#define l2cap_config_request_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_INTERRUPT_REQUEST)
-#define l2cap_config_success_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)
-#define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)
-#define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)
+#define HID_BUFFERSIZE 50 // Size of the buffer for the Playstation Motion Controller
 
 /**
  * This BluetoothService class implements support for all the official PS3 Controllers:
@@ -79,7 +46,7 @@ public:
          * @param ACLData Incoming acldata.
          */
         virtual void ACLData(uint8_t* ACLData);
-        /** Used to run part of the state maschine. */
+        /** Used to run part of the state machine. */
         virtual void Run();
         /** Use this to reset the service. */
         virtual void Reset();
@@ -89,32 +56,34 @@ public:
 
         /** @name PS3 Controller functions */
         /**
-         * getButtonPress(Button b) will return true as long as the button is held down.
+         * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
          *
-         * While getButtonClick(Button b) will only return it once.
+         * While getButtonClick(ButtonEnum b) will only return it once.
          *
-         * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
-         * but if you need to drive a robot forward you would use getButtonPress(Button b).
+         * So you instance if you need to increase a variable once you would use getButtonClick(ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
+         * @return            getButtonPress(ButtonEnum b) will return a true as long as a button is held down, while getButtonClick(ButtonEnum b) will return true once for each button press.
          */
-        bool getButtonPress(Button b);
-        bool getButtonClick(Button b);
+        bool getButtonPress(ButtonEnum b);
+        bool getButtonClick(ButtonEnum b);
         /**@}*/
         /** @name PS3 Controller functions */
         /**
          * Used to get the analog value from button presses.
-         * @param  a The ::Button to read.
+         * @param  a The ::ButtonEnum to read.
          * The supported buttons are:
          * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2,
          * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T.
          * @return   Analog value in the range of 0-255.
          */
-        uint8_t getAnalogButton(Button a);
+        uint8_t getAnalogButton(ButtonEnum a);
         /**
          * Used to read the analog joystick.
          * @param  a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY.
          * @return   Return the analog value in the range of 0-255.
          */
-        uint8_t getAnalogHat(AnalogHat a);
+        uint8_t getAnalogHat(AnalogHatEnum a);
         /**
          * Used to read the sensors inside the Dualshock 3 and Move controller.
          * @param  a
@@ -123,47 +92,44 @@ public:
          * and a temperature sensor inside.
          * @return   Return the raw sensor value.
          */
-        int16_t getSensor(Sensor a);
+        int16_t getSensor(SensorEnum a);
         /**
          * Use this to get ::Pitch and ::Roll calculated using the accelerometer.
          * @param  a Either ::Pitch or ::Roll.
          * @return   Return the angle in the range of 0-360.
          */
-        double getAngle(Angle a);
+        double getAngle(AngleEnum a);
         /**
          * Read the sensors inside the Move controller.
          * @param  a ::aXmove, ::aYmove, ::aZmove, ::gXmove, ::gYmove, ::gZmove, ::mXmove, ::mYmove, and ::mXmove.
          * @return   The value in SI units.
          */
-        double get9DOFValues(Sensor a);
+        double get9DOFValues(SensorEnum a);
         /**
-         * Get the ::Status from the controller.
-         * @param  c The ::Status you want to read.
+         * Get the status from the controller.
+         * @param  c The ::StatusEnum you want to read.
          * @return   True if correct and false if not.
          */
-        bool getStatus(Status c);
-        /**
-         * Read all the available ::Status from the controller.
-         * @return One large string with all the information.
-         */
-        String getStatusString();
+        bool getStatus(StatusEnum c);
+        /** Read all the available statuses from the controller and prints it as a nice formated string. */
+        void printStatusString();
         /**
          * Read the temperature from the Move controller.
-         * @return The temperature in degrees celsius.
+         * @return The temperature in degrees Celsius.
          */
         String getTemperature();
 
-        /** Used to set all LEDs and ::Rumble off. */
+        /** Used to set all LEDs and rumble off. */
         void setAllOff();
-        /** Turn off ::Rumble. */
+        /** Turn off rumble. */
         void setRumbleOff();
         /**
-         * Turn on ::Rumble.
+         * Turn on rumble.
          * @param mode Either ::RumbleHigh or ::RumbleLow.
          */
-        void setRumbleOn(Rumble mode);
+        void setRumbleOn(RumbleEnum mode);
         /**
-         * Turn on ::Rumble using custom duration and power.
+         * Turn on rumble using custom duration and power.
          * @param rightDuration The duration of the right/low rumble effect.
          * @param rightPower The intensity of the right/low rumble effect.
          * @param leftDuration The duration of the left/high rumble effect.
@@ -172,29 +138,30 @@ public:
         void setRumbleOn(uint8_t rightDuration, uint8_t rightPower, uint8_t leftDuration, uint8_t leftPower);
 
         /**
-         * Set LED value without using the ::LED enum.
-         * @param value See: ::LED enum.
+         * Set LED value without using ::LEDEnum.
+         * @param value See: ::LEDEnum.
          */
         void setLedRaw(uint8_t value);
+
         /** Turn all LEDs off. */
         void setLedOff() {
                 setLedRaw(0);
-        }
+        };
         /**
-         * Turn the specific ::LED off.
-         * @param a The ::LED to turn off.
+         * Turn the specific LED off.
+         * @param a The ::LEDEnum to turn off.
          */
-        void setLedOff(LED a);
+        void setLedOff(LEDEnum a);
         /**
-         * Turn the specific ::LED on.
-         * @param a The ::LED to turn on.
+         * Turn the specific LED on.
+         * @param a The ::LEDEnum to turn on.
          */
-        void setLedOn(LED a);
+        void setLedOn(LEDEnum a);
         /**
-         * Toggle the specific ::LED.
-         * @param a The ::LED to toggle.
+         * Toggle the specific LED.
+         * @param a The ::LEDEnum to toggle.
          */
-        void setLedToggle(LED a);
+        void setLedToggle(LEDEnum a);
 
         /**
          * Use this to set the Color using RGB values.
@@ -202,16 +169,21 @@ public:
          */
         void moveSetBulb(uint8_t r, uint8_t g, uint8_t b);
         /**
-         * Use this to set the color using the predefined colors in ::Colors.
+         * Use this to set the color using the predefined colors in ::ColorsEnum.
          * @param color The desired color.
          */
-        void moveSetBulb(Colors color);
+        void moveSetBulb(ColorsEnum color);
         /**
          * Set the rumble value inside the Move controller.
          * @param rumble The desired value in the range from 64-255.
          */
         void moveSetRumble(uint8_t rumble);
 
+        /** Used to get the millis() of the last message */
+        uint32_t getLastMessageTime() {
+                return lastMessageTime;
+        };
+
         /**
          * Used to call your own function when the controller is successfully initialized.
          * @param funcOnInit Function to call.
@@ -247,9 +219,11 @@ private:
         uint8_t remote_name[30]; // First 30 chars of remote name
         bool activeConnection; // Used to indicate if it's already has established a connection
 
-        /* variables used by high level L2CAP task */
+        /* Variables used by high level L2CAP task */
         uint8_t l2cap_state;
-        uint16_t l2cap_event_flag; // L2CAP flags of received Bluetooth events
+        uint32_t l2cap_event_flag; // L2CAP flags of received Bluetooth events
+
+        uint32_t lastMessageTime; // Variable used to store the millis value of the last message.
 
         unsigned long timer;
 
diff --git a/PS3Enums.h b/PS3Enums.h
index b6a7b22d..77801945 100644
--- a/PS3Enums.h
+++ b/PS3Enums.h
@@ -20,8 +20,11 @@
 
 #include "controllerEnums.h"
 
+/** Size of the output report buffer for the Dualshock and Navigation controllers */
+#define PS3_REPORT_BUFFER_SIZE  48
+
 /** Report buffer for all PS3 commands */
-const uint8_t PS3_REPORT_BUFFER[] PROGMEM = {
+const uint8_t PS3_REPORT_BUFFER[PS3_REPORT_BUFFER_SIZE] PROGMEM = {
         0x00, 0x00, 0x00, 0x00, 0x00,
         0x00, 0x00, 0x00, 0x00, 0x00,
         0xff, 0x27, 0x10, 0x00, 0x32,
@@ -33,14 +36,12 @@ const uint8_t PS3_REPORT_BUFFER[] PROGMEM = {
         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 };
 
-/** Size of the output report buffer for the Dualshock and Navigation controllers */
-#define PS3_REPORT_BUFFER_SIZE  48
-
 /** Size of the output report buffer for the Move Controller */
 #define MOVE_REPORT_BUFFER_SIZE 7
 
 /** Used to set the LEDs on the controllers */
-const uint8_t LEDS[] PROGMEM = {
+const uint8_t PS3_LEDS[] PROGMEM = {
+        0x00, // OFF
         0x01, // LED1
         0x02, // LED2
         0x04, // LED3
@@ -51,15 +52,14 @@ const uint8_t LEDS[] PROGMEM = {
         0x0C, // LED7
         0x0D, // LED8
         0x0E, // LED9
-        0x0F // LED10
+        0x0F, // LED10
 };
 
 /**
- * Buttons on the controllers
- *
- * <B>Note:</B> that the location is shiftet 9 when it's connected via USB.
+ * Buttons on the controllers.
+ * <B>Note:</B> that the location is shifted 9 when it's connected via USB.
  */
-const uint32_t BUTTONS[] PROGMEM = {
+const uint32_t PS3_BUTTONS[] PROGMEM = {
         0x10, // UP
         0x20, // RIGHT
         0x40, // DOWN
@@ -82,15 +82,14 @@ const uint32_t BUTTONS[] PROGMEM = {
 
         0x010000, // PS
         0x080000, // MOVE - covers 12 bits - we only need to read the top 8
-        0x100000 // T - covers 12 bits - we only need to read the top 8
+        0x100000, // T - covers 12 bits - we only need to read the top 8
 };
 
 /**
- * Analog buttons on the controllers
- *
- * <B>Note:</B> that the location is shiftet 9 when it's connected via USB.
+ * Analog buttons on the controllers.
+ * <B>Note:</B> that the location is shifted 9 when it's connected via USB.
  */
-const uint8_t ANALOGBUTTONS[] PROGMEM = {
+const uint8_t PS3_ANALOG_BUTTONS[] PROGMEM = {
         23, // UP_ANALOG
         24, // RIGHT_ANALOG
         25, // DOWN_ANALOG
@@ -108,79 +107,11 @@ const uint8_t ANALOGBUTTONS[] PROGMEM = {
         0, 0, // Skip PS and MOVE
 
         // Playstation Move Controller
-        15 // T_ANALOG - Both at byte 14 (last reading) and byte 15 (current reading)
+        15, // T_ANALOG - Both at byte 14 (last reading) and byte 15 (current reading)
 };
 
-/** Used to set the colors of the move controller. */
-enum Colors {
-        /** r = 255, g = 0, b = 0 */
-        Red = 0xFF0000,
-        /** r = 0, g = 255, b = 0 */
-        Green = 0xFF00,
-        /** r = 0, g = 0, b = 255 */
-        Blue = 0xFF,
-
-        /** r = 255, g = 235, b = 4 */
-        Yellow = 0xFFEB04,
-        /** r = 0, g = 255, b = 255 */
-        Lightblue = 0xFFFF,
-        /** r = 255, g = 0, b = 255 */
-        Purble = 0xFF00FF,
-
-        /** r = 255, g = 255, b = 255 */
-        White = 0xFFFFFF,
-        /** r = 0, g = 0, b = 0 */
-        Off = 0x00,
-};
-
-/**
- * Sensors inside the Sixaxis Dualshock 3 and Move controller.
- *
- * <B>Note:</B> that the location is shiftet 9 when it's connected via USB.
- */
-enum Sensor {
-        /** Accelerometer x-axis */
-        aX = 50,
-        /** Accelerometer y-axis */
-        aY = 52,
-        /** Accelerometer z-axis */
-        aZ = 54,
-        /** Gyro z-axis */
-        gZ = 56,
-
-        /** Accelerometer x-axis */
-        aXmove = 28,
-        /** Accelerometer z-axis */
-        aZmove = 30,
-        /** Accelerometer y-axis */
-        aYmove = 32,
-
-        /** Gyro x-axis */
-        gXmove = 40,
-        /** Gyro z-axis */
-        gZmove = 42,
-        /** Gyro y-axis */
-        gYmove = 44,
-
-        /** Temperature sensor */
-        tempMove = 46,
-
-        /** Magnetometer x-axis */
-        mXmove = 47,
-        /** Magnetometer z-axis */
-        mZmove = 49,
-        /** Magnetometer y-axis */
-        mYmove = 50,
-};
-
-/** Used to get the angle calculated using the accelerometer. */
-enum Angle {
-        Pitch = 0x01,
-        Roll = 0x02,
-};
-
-enum Status {
-        // Note that the location is shiftet 9 when it's connected via USB
+enum StatusEnum {
+        // Note that the location is shifted 9 when it's connected via USB
         // Byte location | bit location
         Plugged = (38 << 8) | 0x02,
         Unplugged = (38 << 8) | 0x03,
@@ -201,15 +132,10 @@ enum Status {
         MoveHigh = (21 << 8) | 0x04,
         MoveFull = (21 << 8) | 0x05,
 
-        CableRumble = (40 << 8) | 0x10, //Opperating by USB and rumble is turned on
-        Cable = (40 << 8) | 0x12, //Opperating by USB and rumble is turned off
-        BluetoothRumble = (40 << 8) | 0x14, //Opperating by bluetooth and rumble is turned on
-        Bluetooth = (40 << 8) | 0x16, //Opperating by bluetooth and rumble is turned off
+        CableRumble = (40 << 8) | 0x10, // Operating by USB and rumble is turned on
+        Cable = (40 << 8) | 0x12, // Operating by USB and rumble is turned off
+        BluetoothRumble = (40 << 8) | 0x14, // Operating by Bluetooth and rumble is turned on
+        Bluetooth = (40 << 8) | 0x16, // Operating by Bluetooth and rumble is turned off
 };
 
-enum Rumble {
-        RumbleHigh = 0x10,
-        RumbleLow = 0x20,
-};
-
-#endif
\ No newline at end of file
+#endif
diff --git a/PS3USB.cpp b/PS3USB.cpp
index 1e0cb4fc..7e6285a2 100644
--- a/PS3USB.cpp
+++ b/PS3USB.cpp
@@ -25,14 +25,14 @@ pUsb(p), // pointer to USB class instance - mandatory
 bAddress(0), // device address - mandatory
 bPollEnable(false) // don't start polling before dongle is connected
 {
-        for (uint8_t i = 0; i < PS3_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < PS3_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
                 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
         }
 
-        if (pUsb) // register in USB subsystem
+        if(pUsb) // register in USB subsystem
                 pUsb->RegisterDeviceClass(this); //set devConfig[] entry
 
         my_bdaddr[5] = btadr5; // Change to your dongle's Bluetooth address instead
@@ -45,6 +45,7 @@ bPollEnable(false) // don't start polling before dongle is connected
 
 uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -57,7 +58,7 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nPS3USB Init"), 0x80);
 #endif
         // check if address has already been assigned to an instance
-        if (bAddress) {
+        if(bAddress) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress in use"), 0x80);
 #endif
@@ -67,14 +68,14 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nepinfo is null"), 0x80);
 #endif
@@ -94,27 +95,27 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
-        VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
-        PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
+        VID = udd->idVendor;
+        PID = udd->idProduct;
 
-        if (VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID))
+        if(VID != PS3_VID || (PID != PS3_PID && PID != PS3NAVIGATION_PID && PID != PS3MOVE_PID))
                 goto FailUnknownDevice;
 
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -128,20 +129,20 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nAddr: "), 0x80);
         D_PrintHex<uint8_t > (bAddress, 0x80);
 #endif
-        delay(300); // Spec says you should wait at least 200ms
+        //delay(300); // Spec says you should wait at least 200ms
 
         p->lowspeed = false;
 
         //get pointer to assigned address record
         p = addrPool.GetUsbDevicePtr(bAddress);
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
         // Assign epInfo to epinfo pointer - only EP0 is known
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
 
@@ -154,27 +155,27 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         epInfo[ PS3_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT;
         epInfo[ PS3_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ PS3_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ PS3_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0;
-        epInfo[ PS3_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ PS3_OUTPUT_PIPE ].bmSndToggle = 0;
+        epInfo[ PS3_OUTPUT_PIPE ].bmRcvToggle = 0;
         epInfo[ PS3_INPUT_PIPE ].epAddr = 0x01; // PS3 report endpoint
         epInfo[ PS3_INPUT_PIPE ].epAttribs = EP_INTERRUPT;
         epInfo[ PS3_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ PS3_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ PS3_INPUT_PIPE ].bmSndToggle = bmSNDTOG0;
-        epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ PS3_INPUT_PIPE ].bmSndToggle = 0;
+        epInfo[ PS3_INPUT_PIPE ].bmRcvToggle = 0;
 
         rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         delay(200); //Give time for address change
 
         rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1);
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
-        if (PID == PS3_PID || PID == PS3NAVIGATION_PID) {
-                if (PID == PS3_PID) {
+        if(PID == PS3_PID || PID == PS3NAVIGATION_PID) {
+                if(PID == PS3_PID) {
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nDualshock 3 Controller Connected"), 0x80);
 #endif
@@ -188,10 +189,10 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 enable_sixaxis(); // The PS3 controller needs a special command before it starts sending data
 
                 // Needed for PS3 Dualshock and Navigation commands to work
-                for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
+                for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
                         writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]);
 
-                for (uint8_t i = 6; i < 10; i++)
+                for(uint8_t i = 6; i < 10; i++)
                         readBuf[i] = 0x7F; // Set the analog joystick values to center position
         } else { // must be a Motion controller
 #ifdef DEBUG_USB_HOST
@@ -200,15 +201,15 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 PS3MoveConnected = true;
                 writeBuf[0] = 0x02; // Set report ID, this is needed for Move commands to work
         }
-        if (my_bdaddr[0] != 0x00 || my_bdaddr[1] != 0x00 || my_bdaddr[2] != 0x00 || my_bdaddr[3] != 0x00 || my_bdaddr[4] != 0x00 || my_bdaddr[5] != 0x00) {
-                if (PS3MoveConnected)
+        if(my_bdaddr[0] != 0x00 || my_bdaddr[1] != 0x00 || my_bdaddr[2] != 0x00 || my_bdaddr[3] != 0x00 || my_bdaddr[4] != 0x00 || my_bdaddr[5] != 0x00) {
+                if(PS3MoveConnected)
                         setMoveBdaddr(my_bdaddr); // Set internal Bluetooth address
                 else
                         setBdaddr(my_bdaddr); // Set internal Bluetooth address
 
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nBluetooth Address was set to: "), 0x80);
-                for (int8_t i = 5; i > 0; i--) {
+                for(int8_t i = 5; i > 0; i--) {
                         D_PrintHex<uint8_t > (my_bdaddr[i], 0x80);
                         Notify(PSTR(":"), 0x80);
                 }
@@ -220,14 +221,15 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         bPollEnable = true;
         Notify(PSTR("\r\n"), 0x80);
         timer = millis();
-        return 0; // successful configuration
+        return 0; // Successful configuration
 
-        /* diagnostic messages */
+        /* Diagnostic messages */
 FailGetDevDescr:
 #ifdef DEBUG_USB_HOST
         NotifyFailGetDevDescr();
         goto Fail;
 #endif
+
 FailSetDevTblEntry:
 #ifdef DEBUG_USB_HOST
         NotifyFailSetDevTblEntry();
@@ -239,13 +241,14 @@ FailSetConfDescr:
         NotifyFailSetConfDescr();
 #endif
         goto Fail;
+
 FailUnknownDevice:
 #ifdef DEBUG_USB_HOST
         NotifyFailUnknownDevice(VID, PID);
 #endif
         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
-Fail:
 
+Fail:
 #ifdef DEBUG_USB_HOST
         Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80);
         NotifyFail(rcode);
@@ -266,20 +269,20 @@ uint8_t PS3USB::Release() {
 }
 
 uint8_t PS3USB::Poll() {
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
 
-        if (PS3Connected || PS3NavigationConnected) {
+        if(PS3Connected || PS3NavigationConnected) {
                 uint16_t BUFFER_SIZE = EP_MAXPKTSIZE;
                 pUsb->inTransfer(bAddress, epInfo[ PS3_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1
-                if (millis() - timer > 100) { // Loop 100ms before processing data
+                if(millis() - timer > 100) { // Loop 100ms before processing data
                         readReport();
 #ifdef PRINTREPORT
                         printReport(); // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
 #endif
                 }
-        } else if (PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB
-                if (millis() - timer > 4000) { // Send at least every 4th second
+        } else if(PS3MoveConnected) { // One can only set the color of the bulb, set the rumble, set and get the bluetooth address and calibrate the magnetometer via USB
+                if(millis() - timer > 4000) { // Send at least every 4th second
                         Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE); // The Bulb and rumble values, has to be written again and again, for it to stay turned on
                         timer = millis();
                 }
@@ -293,7 +296,7 @@ void PS3USB::readReport() {
         //Notify(PSTR("\r\nButtonState", 0x80);
         //PrintHex<uint32_t>(ButtonState, 0x80);
 
-        if (ButtonState != OldButtonState) {
+        if(ButtonState != OldButtonState) {
                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
                 OldButtonState = ButtonState;
         }
@@ -301,7 +304,7 @@ void PS3USB::readReport() {
 
 void PS3USB::printReport() { // Uncomment "#define PRINTREPORT" to print the report send by the PS3 Controllers
 #ifdef PRINTREPORT
-        for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) {
+        for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++) {
                 D_PrintHex<uint8_t > (readBuf[i], 0x80);
                 Notify(PSTR(" "), 0x80);
         }
@@ -309,31 +312,31 @@ void PS3USB::printReport() { // Uncomment "#define PRINTREPORT" to print the rep
 #endif
 }
 
-bool PS3USB::getButtonPress(Button b) {
-        return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b]));
+bool PS3USB::getButtonPress(ButtonEnum b) {
+        return (ButtonState & pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]));
 }
 
-bool PS3USB::getButtonClick(Button b) {
-        uint32_t button = pgm_read_dword(&BUTTONS[(uint8_t)b]);
+bool PS3USB::getButtonClick(ButtonEnum b) {
+        uint32_t button = pgm_read_dword(&PS3_BUTTONS[(uint8_t)b]);
         bool click = (ButtonClickState & button);
         ButtonClickState &= ~button; // Clear "click" event
         return click;
 }
 
-uint8_t PS3USB::getAnalogButton(Button a) {
-        return (uint8_t)(readBuf[(pgm_read_byte(&ANALOGBUTTONS[(uint8_t)a])) - 9]);
+uint8_t PS3USB::getAnalogButton(ButtonEnum a) {
+        return (uint8_t)(readBuf[(pgm_read_byte(&PS3_ANALOG_BUTTONS[(uint8_t)a])) - 9]);
 }
 
-uint8_t PS3USB::getAnalogHat(AnalogHat a) {
+uint8_t PS3USB::getAnalogHat(AnalogHatEnum a) {
         return (uint8_t)(readBuf[((uint8_t)a + 6)]);
 }
 
-uint16_t PS3USB::getSensor(Sensor a) {
+uint16_t PS3USB::getSensor(SensorEnum a) {
         return ((readBuf[((uint16_t)a) - 9] << 8) | readBuf[((uint16_t)a + 1) - 9]);
 }
 
-double PS3USB::getAngle(Angle a) {
-        if (PS3Connected) {
+double PS3USB::getAngle(AngleEnum a) {
+        if(PS3Connected) {
                 double accXval;
                 double accYval;
                 double accZval;
@@ -347,7 +350,7 @@ double PS3USB::getAngle(Angle a) {
                 // Convert to 360 degrees resolution
                 // atan2 outputs the value of -π to π (radians)
                 // We are then converting it to 0 to 2π and then to degrees
-                if (a == Pitch)
+                if(a == Pitch)
                         return (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
                 else
                         return (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
@@ -355,43 +358,41 @@ double PS3USB::getAngle(Angle a) {
                 return 0;
 }
 
-bool PS3USB::getStatus(Status c) {
+bool PS3USB::getStatus(StatusEnum c) {
         return (readBuf[((uint16_t)c >> 8) - 9] == ((uint8_t)c & 0xff));
 }
 
-String PS3USB::getStatusString() {
-        if (PS3Connected || PS3NavigationConnected) {
-                char statusOutput[100];
+void PS3USB::printStatusString() {
+        char statusOutput[100]; // Max string length plus null character
+        if(PS3Connected || PS3NavigationConnected) {
+                strcpy_P(statusOutput, PSTR("ConnectionStatus: "));
 
-                strcpy(statusOutput, "ConnectionStatus: ");
+                if(getStatus(Plugged)) strcat_P(statusOutput, PSTR("Plugged"));
+                else if(getStatus(Unplugged)) strcat_P(statusOutput, PSTR("Unplugged"));
+                else strcat_P(statusOutput, PSTR("Error"));
 
-                if (getStatus(Plugged)) strcat(statusOutput, "Plugged");
-                else if (getStatus(Unplugged)) strcat(statusOutput, "Unplugged");
-                else strcat(statusOutput, "Error");
+                strcat_P(statusOutput, PSTR(" - PowerRating: "));
 
+                if(getStatus(Charging)) strcat_P(statusOutput, PSTR("Charging"));
+                else if(getStatus(NotCharging)) strcat_P(statusOutput, PSTR("Not Charging"));
+                else if(getStatus(Shutdown)) strcat_P(statusOutput, PSTR("Shutdown"));
+                else if(getStatus(Dying)) strcat_P(statusOutput, PSTR("Dying"));
+                else if(getStatus(Low)) strcat_P(statusOutput, PSTR("Low"));
+                else if(getStatus(High)) strcat_P(statusOutput, PSTR("High"));
+                else if(getStatus(Full)) strcat_P(statusOutput, PSTR("Full"));
+                else strcat_P(statusOutput, PSTR("Error"));
 
-                strcat(statusOutput, " - PowerRating: ");
+                strcat_P(statusOutput, PSTR(" - WirelessStatus: "));
 
-                if (getStatus(Charging)) strcat(statusOutput, "Charging");
-                else if (getStatus(NotCharging)) strcat(statusOutput, "Not Charging");
-                else if (getStatus(Shutdown)) strcat(statusOutput, "Shutdown");
-                else if (getStatus(Dying)) strcat(statusOutput, "Dying");
-                else if (getStatus(Low)) strcat(statusOutput, "Low");
-                else if (getStatus(High)) strcat(statusOutput, "High");
-                else if (getStatus(Full)) strcat(statusOutput, "Full");
-                else strcat(statusOutput, "Error");
-
-                strcat(statusOutput, " - WirelessStatus: ");
-
-                if (getStatus(CableRumble)) strcat(statusOutput, "Cable - Rumble is on");
-                else if (getStatus(Cable)) strcat(statusOutput, "Cable - Rumble is off");
-                else if (getStatus(BluetoothRumble)) strcat(statusOutput, "Bluetooth - Rumble is on");
-                else if (getStatus(Bluetooth)) strcat(statusOutput, "Bluetooth - Rumble is off");
-                else strcat(statusOutput, "Error");
-
-                return statusOutput;
+                if(getStatus(CableRumble)) strcat_P(statusOutput, PSTR("Cable - Rumble is on"));
+                else if(getStatus(Cable)) strcat_P(statusOutput, PSTR("Cable - Rumble is off"));
+                else if(getStatus(BluetoothRumble)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is on"));
+                else if(getStatus(Bluetooth)) strcat_P(statusOutput, PSTR("Bluetooth - Rumble is off"));
+                else strcat_P(statusOutput, PSTR("Error"));
         } else
-                return "Error";
+                strcpy_P(statusOutput, PSTR("Error"));
+
+        USB_HOST_SERIAL.write(statusOutput);
 }
 
 /* Playstation Sixaxis Dualshock and Navigation Controller commands */
@@ -401,7 +402,7 @@ void PS3USB::PS3_Command(uint8_t *data, uint16_t nbytes) {
 }
 
 void PS3USB::setAllOff() {
-        for (uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
+        for(uint8_t i = 0; i < PS3_REPORT_BUFFER_SIZE; i++)
                 writeBuf[i] = pgm_read_byte(&PS3_REPORT_BUFFER[i]); // Reset buffer
 
         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
@@ -416,10 +417,10 @@ void PS3USB::setRumbleOff() {
         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 }
 
-void PS3USB::setRumbleOn(Rumble mode) {
-        if ((mode & 0x30) > 0x00) {
+void PS3USB::setRumbleOn(RumbleEnum mode) {
+        if((mode & 0x30) > 0x00) {
                 uint8_t power[2] = {0xff, 0x00}; // Defaults to RumbleLow
-                if (mode == RumbleHigh) {
+                if(mode == RumbleHigh) {
                         power[0] = 0x00;
                         power[1] = 0xff;
                 }
@@ -440,18 +441,22 @@ void PS3USB::setLedRaw(uint8_t value) {
         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 }
 
-void PS3USB::setLedOff(LED a) {
-        writeBuf[9] &= ~((uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1));
+void PS3USB::setLedOff(LEDEnum a) {
+        writeBuf[9] &= ~((uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1));
         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 }
 
-void PS3USB::setLedOn(LED a) {
-        writeBuf[9] |= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1);
-        PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
+void PS3USB::setLedOn(LEDEnum a) {
+        if(a == OFF)
+                setLedRaw(0);
+        else {
+                writeBuf[9] |= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
+                PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
+        }
 }
 
-void PS3USB::setLedToggle(LED a) {
-        writeBuf[9] ^= (uint8_t)((pgm_read_byte(&LEDS[(uint8_t)a]) & 0x0f) << 1);
+void PS3USB::setLedToggle(LEDEnum a) {
+        writeBuf[9] ^= (uint8_t)((pgm_read_byte(&PS3_LEDS[(uint8_t)a]) & 0x0f) << 1);
         PS3_Command(writeBuf, PS3_REPORT_BUFFER_SIZE);
 }
 
@@ -461,7 +466,7 @@ void PS3USB::setBdaddr(uint8_t *bdaddr) {
         buf[0] = 0x01;
         buf[1] = 0x00;
 
-        for (uint8_t i = 0; i < 6; i++)
+        for(uint8_t i = 0; i < 6; i++)
                 buf[i + 2] = bdaddr[5 - i]; // Copy into buffer, has to be written reversed, so it is MSB first
 
         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
@@ -474,7 +479,7 @@ void PS3USB::getBdaddr(uint8_t *bdaddr) {
         // bmRequest = Device to host (0x80) | Class (0x20) | Interface (0x01) = 0xA1, bRequest = Get Report (0x01), Report ID (0xF5), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, 0xF5, 0x03, 0x00, 8, 8, buf, NULL);
 
-        for (uint8_t i = 0; i < 6; i++)
+        for(uint8_t i = 0; i < 6; i++)
                 bdaddr[5 - i] = buf[i + 2]; // Copy into buffer reversed, so it is LSB first
 }
 
@@ -503,13 +508,13 @@ void PS3USB::moveSetBulb(uint8_t r, uint8_t g, uint8_t b) { // Use this to set t
         Move_Command(writeBuf, MOVE_REPORT_BUFFER_SIZE);
 }
 
-void PS3USB::moveSetBulb(Colors color) { // Use this to set the Color using the predefined colors in "enums.h"
+void PS3USB::moveSetBulb(ColorsEnum color) { // Use this to set the Color using the predefined colors in "enums.h"
         moveSetBulb((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
 }
 
 void PS3USB::moveSetRumble(uint8_t rumble) {
 #ifdef DEBUG_USB_HOST
-        if (rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
+        if(rumble < 64 && rumble != 0) // The rumble value has to at least 64, or approximately 25% (64/255*100)
                 Notify(PSTR("\r\nThe rumble value has to at least 64, or approximately 25%"), 0x80);
 #endif
         writeBuf[6] = rumble; // Set the rumble value into the write buffer
@@ -526,7 +531,7 @@ void PS3USB::setMoveBdaddr(uint8_t *bdaddr) {
         buf[9] = 0x02;
         buf[10] = 0x12;
 
-        for (uint8_t i = 0; i < 6; i++)
+        for(uint8_t i = 0; i < 6; i++)
                 buf[i + 1] = bdaddr[i];
 
         // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x05), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
@@ -539,27 +544,27 @@ void PS3USB::getMoveBdaddr(uint8_t *bdaddr) {
         // bmRequest = Device to host (0x80) | Class (0x20) | Interface (0x01) = 0xA1, bRequest = Get Report (0x01), Report ID (0x04), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
         pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, 0x04, 0x03, 0x00, 16, 16, buf, NULL);
 
-        for (uint8_t i = 0; i < 6; i++)
+        for(uint8_t i = 0; i < 6; i++)
                 bdaddr[i] = buf[10 + i];
 }
 
 void PS3USB::getMoveCalibration(uint8_t *data) {
         uint8_t buf[49];
 
-        for (uint8_t i = 0; i < 3; i++) {
+        for(uint8_t i = 0; i < 3; i++) {
                 // bmRequest = Device to host (0x80) | Class (0x20) | Interface (0x01) = 0xA1, bRequest = Get Report (0x01), Report ID (0x10), Report Type (Feature 0x03), interface (0x00), datalength, datalength, data
                 pUsb->ctrlReq(bAddress, epInfo[PS3_CONTROL_PIPE].epAddr, bmREQ_HID_IN, HID_REQUEST_GET_REPORT, 0x10, 0x03, 0x00, 49, 49, buf, NULL);
 
-                for (byte j = 0; j < 49; j++)
+                for(byte j = 0; j < 49; j++)
                         data[49 * i + j] = buf[j];
         }
 }
 
 void PS3USB::onInit() {
-        if (pFuncOnInit)
+        if(pFuncOnInit)
                 pFuncOnInit(); // Call the user function
         else {
-                if (PS3MoveConnected)
+                if(PS3MoveConnected)
                         moveSetBulb(Red);
                 else // Dualshock 3 or Navigation controller
                         setLedOn(LED1);
diff --git a/PS3USB.h b/PS3USB.h
index 82559acd..5bbbdf2e 100644
--- a/PS3USB.h
+++ b/PS3USB.h
@@ -146,68 +146,67 @@ public:
 
         /** @name PS3 Controller functions */
         /**
-         * getButtonPress(Button b) will return true as long as the button is held down.
+         * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
          *
-         * While getButtonClick(Button b) will only return it once.
+         * While getButtonClick(ButtonEnum b) will only return it once.
          *
-         * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
-         * but if you need to drive a robot forward you would use getButtonPress(Button b).
+         * So you instance if you need to increase a variable once you would use getButtonClick(ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
+         * @return            getButtonPress(ButtonEnum b) will return a true as long as a button is held down, while getButtonClick(ButtonEnum b) will return true once for each button press.
          */
-        bool getButtonPress(Button b);
-        bool getButtonClick(Button b);
+        bool getButtonPress(ButtonEnum b);
+        bool getButtonClick(ButtonEnum b);
         /**@}*/
         /** @name PS3 Controller functions */
         /**
          * Used to get the analog value from button presses.
-         * @param  a The ::Button to read.
+         * @param  a The ::ButtonEnum to read.
          * The supported buttons are:
          * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2,
          * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T.
          * @return   Analog value in the range of 0-255.
          */
-        uint8_t getAnalogButton(Button a);
+        uint8_t getAnalogButton(ButtonEnum a);
         /**
          * Used to read the analog joystick.
          * @param  a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY.
          * @return   Return the analog value in the range of 0-255.
          */
-        uint8_t getAnalogHat(AnalogHat a);
+        uint8_t getAnalogHat(AnalogHatEnum a);
         /**
          * Used to read the sensors inside the Dualshock 3 controller.
          * @param  a
          * The Dualshock 3 has a 3-axis accelerometer and a 1-axis gyro inside.
          * @return   Return the raw sensor value.
          */
-        uint16_t getSensor(Sensor a);
+        uint16_t getSensor(SensorEnum a);
         /**
          * Use this to get ::Pitch and ::Roll calculated using the accelerometer.
          * @param  a Either ::Pitch or ::Roll.
          * @return   Return the angle in the range of 0-360.
          */
-        double getAngle(Angle a);
+        double getAngle(AngleEnum a);
         /**
-         * Get the ::Status from the controller.
-         * @param  c The ::Status you want to read.
+         * Get the ::StatusEnum from the controller.
+         * @param  c The ::StatusEnum you want to read.
          * @return   True if correct and false if not.
          */
-        bool getStatus(Status c);
-        /**
-         * Read all the available ::Status from the controller.
-         * @return One large string with all the information.
-         */
-        String getStatusString();
+        bool getStatus(StatusEnum c);
+        /** Read all the available statuses from the controller and prints it as a nice formated string. */
+        void printStatusString();
 
-        /** Used to set all LEDs and ::Rumble off. */
+        /** Used to set all LEDs and rumble off. */
         void setAllOff();
-        /** Turn off ::Rumble. */
+        /** Turn off rumble. */
         void setRumbleOff();
         /**
-         * Turn on ::Rumble.
+         * Turn on rumble.
          * @param mode Either ::RumbleHigh or ::RumbleLow.
          */
-        void setRumbleOn(Rumble mode);
+        void setRumbleOn(RumbleEnum mode);
         /**
-         * Turn on ::Rumble using custom duration and power.
+         * Turn on rumble using custom duration and power.
          * @param rightDuration The duration of the right/low rumble effect.
          * @param rightPower The intensity of the right/low rumble effect.
          * @param leftDuration The duration of the left/high rumble effect.
@@ -216,29 +215,30 @@ public:
         void setRumbleOn(uint8_t rightDuration, uint8_t rightPower, uint8_t leftDuration, uint8_t leftPower);
 
         /**
-         * Set LED value without using the ::LED enum.
-         * @param value See: ::LED enum.
+         * Set LED value without using the ::LEDEnum.
+         * @param value See: ::LEDEnum.
          */
         void setLedRaw(uint8_t value);
+
         /** Turn all LEDs off. */
         void setLedOff() {
                 setLedRaw(0);
         }
         /**
-         * Turn the specific ::LED off.
-         * @param a The ::LED to turn off.
+         * Turn the specific ::LEDEnum off.
+         * @param a The ::LEDEnum to turn off.
          */
-        void setLedOff(LED a);
+        void setLedOff(LEDEnum a);
         /**
-         * Turn the specific ::LED on.
-         * @param a The ::LED to turn on.
+         * Turn the specific ::LEDEnum on.
+         * @param a The ::LEDEnum to turn on.
          */
-        void setLedOn(LED a);
+        void setLedOn(LEDEnum a);
         /**
-         * Toggle the specific ::LED.
-         * @param a The ::LED to toggle.
+         * Toggle the specific ::LEDEnum.
+         * @param a The ::LEDEnum to toggle.
          */
-        void setLedToggle(LED a);
+        void setLedToggle(LEDEnum a);
 
         /**
          * Use this to set the Color using RGB values.
@@ -246,10 +246,10 @@ public:
          */
         void moveSetBulb(uint8_t r, uint8_t g, uint8_t b);
         /**
-         * Use this to set the color using the predefined colors in ::Colors.
+         * Use this to set the color using the predefined colors in ::ColorsEnum.
          * @param color The desired color.
          */
-        void moveSetBulb(Colors color);
+        void moveSetBulb(ColorsEnum color);
         /**
          * Set the rumble value inside the Move controller.
          * @param rumble The desired value in the range from 64-255.
diff --git a/PS4BT.h b/PS4BT.h
new file mode 100644
index 00000000..ff3168a3
--- /dev/null
+++ b/PS4BT.h
@@ -0,0 +1,131 @@
+/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+#ifndef _ps4bt_h_
+#define _ps4bt_h_
+
+#include "BTHID.h"
+#include "PS4Parser.h"
+
+/**
+ * This class implements support for the PS4 controller via Bluetooth.
+ * It uses the BTHID class for all the Bluetooth communication.
+ */
+class PS4BT : public BTHID, public PS4Parser {
+public:
+        /**
+         * Constructor for the PS4BT class.
+         * @param  p     Pointer to the BTD class instance.
+         * @param  pair  Set this to true in order to pair with the device. If the argument is omitted then it will not pair with it. One can use ::PAIR to set it to true.
+         * @param  pin   Write the pin to BTD#btdPin. If argument is omitted, then "0000" will be used.
+         */
+        PS4BT(BTD *p, bool pair = false, const char *pin = "0000") :
+        BTHID(p, pair, pin) {
+                PS4Parser::Reset();
+        };
+
+        /**
+         * Used to check if a PS4 controller is connected.
+         * @return Returns true if it is connected.
+         */
+        bool connected() {
+                return BTHID::connected;
+        };
+
+        /**
+         * Used to call your own function when the device is successfully initialized.
+         * @param funcOnInit Function to call.
+         */
+        void attachOnInit(void (*funcOnInit)(void)) {
+                pFuncOnInit = funcOnInit;
+        };
+
+protected:
+        /** @name BTHID implementation */
+        /**
+         * Used to parse Bluetooth HID data.
+         * @param len The length of the incoming data.
+         * @param buf Pointer to the data buffer.
+         */
+        virtual void ParseBTHIDData(uint8_t len, uint8_t *buf) {
+                PS4Parser::Parse(len, buf);
+        };
+
+        /**
+         * Called when a device is successfully initialized.
+         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
+         * This is useful for instance if you want to set the LEDs in a specific way.
+         */
+        virtual void OnInitBTHID() {
+                PS4Parser::Reset();
+                enable_sixaxis(); // Make the controller send out the entire output report
+                if (pFuncOnInit)
+                        pFuncOnInit(); // Call the user function
+                else
+                        setLed(Blue);
+        };
+
+        /** Used to reset the different buffers to there default values */
+        virtual void ResetBTHID() {
+                PS4Parser::Reset();
+        };
+        /**@}*/
+
+        /** @name PS4Parser implementation */
+        virtual void sendOutputReport(PS4Output *output) { // Source: https://github.com/chrippa/ds4drv
+                uint8_t buf[79];
+                memset(buf, 0, sizeof(buf));
+
+                buf[0] = 0x52; // HID BT Set_report (0x50) | Report Type (Output 0x02)
+                buf[1] = 0x11; // Report ID
+                buf[2] = 0x80;
+                buf[4]= 0xFF;
+
+                buf[7] = output->smallRumble; // Small Rumble
+                buf[8] = output->bigRumble; // Big rumble
+
+                buf[9] = output->r; // Red
+                buf[10] = output->g; // Green
+                buf[11] = output->b; // Blue
+
+                buf[12] = output->flashOn; // Time to flash bright (255 = 2.5 seconds)
+                buf[13] = output->flashOff; // Time to flash dark (255 = 2.5 seconds)
+
+                output->reportChanged = false;
+
+                // The PS4 console actually set the four last bytes to a CRC32 checksum, but it seems like it is actually not needed
+
+                HID_Command(buf, sizeof(buf));
+        };
+        /**@}*/
+
+private:
+        void enable_sixaxis() { // Command used to make the PS4 controller send out the entire output report
+                uint8_t buf[2];
+                buf[0] = 0x43; // HID BT Get_report (0x40) | Report Type (Feature 0x03)
+                buf[1] = 0x02; // Report ID
+
+                HID_Command(buf, 2);
+        };
+
+        void HID_Command(uint8_t *data, uint8_t nbytes) {
+                pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
+        };
+
+        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
+};
+#endif
\ No newline at end of file
diff --git a/PS4Parser.cpp b/PS4Parser.cpp
new file mode 100644
index 00000000..ca992866
--- /dev/null
+++ b/PS4Parser.cpp
@@ -0,0 +1,109 @@
+/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+#include "PS4Parser.h"
+
+// To enable serial debugging see "settings.h"
+//#define PRINTREPORT // Uncomment to print the report send by the PS4 Controller
+
+bool PS4Parser::checkDpad(ButtonEnum b) {
+        switch (b) {
+                case UP:
+                        return ps4Data.btn.dpad == DPAD_LEFT_UP || ps4Data.btn.dpad == DPAD_UP || ps4Data.btn.dpad == DPAD_UP_RIGHT;
+                case RIGHT:
+                        return ps4Data.btn.dpad == DPAD_UP_RIGHT || ps4Data.btn.dpad == DPAD_RIGHT || ps4Data.btn.dpad == DPAD_RIGHT_DOWN;
+                case DOWN:
+                        return ps4Data.btn.dpad == DPAD_RIGHT_DOWN || ps4Data.btn.dpad == DPAD_DOWN || ps4Data.btn.dpad == DPAD_DOWN_LEFT;
+                case LEFT:
+                        return ps4Data.btn.dpad == DPAD_DOWN_LEFT || ps4Data.btn.dpad == DPAD_LEFT || ps4Data.btn.dpad == DPAD_LEFT_UP;
+                default:
+                        return false;
+        }
+}
+
+bool PS4Parser::getButtonPress(ButtonEnum b) {
+        if (b <= LEFT) // Dpad
+                return checkDpad(b);
+        else
+                return ps4Data.btn.val & (1UL << pgm_read_byte(&PS4_BUTTONS[(uint8_t)b]));
+}
+
+bool PS4Parser::getButtonClick(ButtonEnum b) {
+        uint32_t mask = 1UL << pgm_read_byte(&PS4_BUTTONS[(uint8_t)b]);
+        bool click = buttonClickState.val & mask;
+        buttonClickState.val &= ~mask; // Clear "click" event
+        return click;
+}
+
+uint8_t PS4Parser::getAnalogButton(ButtonEnum b) {
+        if (b == L2) // These are the only analog buttons on the controller
+                return ps4Data.trigger[0];
+        else if (b == R2)
+                return ps4Data.trigger[1];
+        return 0;
+}
+
+uint8_t PS4Parser::getAnalogHat(AnalogHatEnum a) {
+        return ps4Data.hatValue[(uint8_t)a];
+}
+
+void PS4Parser::Parse(uint8_t len, uint8_t *buf) {
+        if (len > 0 && buf)  {
+#ifdef PRINTREPORT
+                Notify(PSTR("\r\n"), 0x80);
+                for (uint8_t i = 0; i < len; i++) {
+                        D_PrintHex<uint8_t > (buf[i], 0x80);
+                        Notify(PSTR(" "), 0x80);
+                }
+#endif
+
+                if (buf[0] == 0x01) // Check report ID
+                        memcpy(&ps4Data, buf + 1, min(len - 1, sizeof(ps4Data)));
+                else if (buf[0] == 0x11) // This report is send via Bluetooth, it has an offset of 2 compared to the USB data
+                        memcpy(&ps4Data, buf + 3, min(len - 3, sizeof(ps4Data)));
+                else {
+#ifdef DEBUG_USB_HOST
+                        Notify(PSTR("\r\nUnknown report id: "), 0x80);
+                        D_PrintHex<uint8_t > (buf[0], 0x80);
+#endif
+                        return;
+                }
+
+                if (ps4Data.btn.val != oldButtonState.val) { // Check if anything has changed
+                        buttonClickState.val = ps4Data.btn.val & ~oldButtonState.val; // Update click state variable
+                        oldButtonState.val = ps4Data.btn.val;
+
+                        // The DPAD buttons does not set the different bits, but set a value corresponding to the buttons pressed, we will simply set the bits ourself
+                        uint8_t newDpad = 0;
+                        if (checkDpad(UP))
+                                newDpad |= 1 << UP;
+                        if (checkDpad(RIGHT))
+                                newDpad |= 1 << RIGHT;
+                        if (checkDpad(DOWN))
+                                newDpad |= 1 << DOWN;
+                        if (checkDpad(LEFT))
+                                newDpad |= 1 << LEFT;
+                        if (newDpad != oldDpad) {
+                                buttonClickState.dpad = newDpad & ~oldDpad; // Override values
+                                oldDpad = newDpad;
+                        }
+                }
+        }
+
+        if (ps4Output.reportChanged)
+                sendOutputReport(&ps4Output); // Send output report
+}
\ No newline at end of file
diff --git a/PS4Parser.h b/PS4Parser.h
new file mode 100644
index 00000000..8b328342
--- /dev/null
+++ b/PS4Parser.h
@@ -0,0 +1,407 @@
+/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+#ifndef _ps4parser_h_
+#define _ps4parser_h_
+
+#include "Usb.h"
+#include "controllerEnums.h"
+
+/** Buttons on the controller */
+const uint8_t PS4_BUTTONS[] PROGMEM = {
+        UP, // UP
+        RIGHT, // RIGHT
+        DOWN, // DOWN
+        LEFT, // LEFT
+
+        0x0C, // SHARE
+        0x0D, // OPTIONS
+        0x0E, // L3
+        0x0F, // R3
+
+        0x0A, // L2
+        0x0B, // R2
+        0x08, // L1
+        0x09, // R1
+
+        0x07, // TRIANGLE
+        0x06, // CIRCLE
+        0x05, // CROSS
+        0x04, // SQUARE
+
+        0x10, // PS
+        0x11, // TOUCHPAD
+};
+
+union PS4Buttons {
+        struct {
+                uint8_t dpad : 4;
+                uint8_t square : 1;
+                uint8_t cross : 1;
+                uint8_t circle : 1;
+                uint8_t triangle : 1;
+
+                uint8_t l1 : 1;
+                uint8_t r1 : 1;
+                uint8_t l2 : 1;
+                uint8_t r2 : 1;
+                uint8_t share : 1;
+                uint8_t options : 1;
+                uint8_t l3 : 1;
+                uint8_t r3 : 1;
+
+                uint8_t ps : 1;
+                uint8_t touchpad : 1;
+                uint8_t reportCounter : 6;
+        } __attribute__((packed));
+        uint32_t val : 24;
+} __attribute__((packed));
+
+struct touchpadXY {
+        uint8_t dummy; // I can not figure out what this data is for, it seems to change randomly, maybe a timestamp?
+        struct {
+                uint8_t counter : 7; // Increments every time a finger is touching the touchpad
+                uint8_t touching : 1; // The top bit is cleared if the finger is touching the touchpad
+                uint16_t x : 12;
+                uint16_t y : 12;
+        } __attribute__((packed)) finger[2]; // 0 = first finger, 1 = second finger
+} __attribute__((packed));
+
+struct PS4Status {
+        uint8_t battery : 4;
+        uint8_t usb : 1;
+        uint8_t audio : 1;
+        uint8_t mic : 1;
+        uint8_t unknown : 1; // Extension port?
+} __attribute__((packed));
+
+struct PS4Data {
+        /* Button and joystick values */
+        uint8_t hatValue[4];
+        PS4Buttons btn;
+        uint8_t trigger[2];
+
+        /* Gyro and accelerometer values */
+        uint8_t dummy[3]; // First two looks random, while the third one might be some kind of status - it increments once in a while
+        int16_t gyroY, gyroZ, gyroX;
+        int16_t accX, accZ, accY;
+
+        uint8_t dummy2[5];
+        PS4Status status;
+        uint8_t dummy3[3];
+
+        /* The rest is data for the touchpad */
+        touchpadXY xy[3]; // It looks like it sends out three coordinates each time, this might be because the microcontroller inside the PS4 controller is much faster than the Bluetooth connection.
+                          // The last data is read from the last position in the array while the oldest measurement is from the first position.
+                          // The first position will also keep it's value after the finger is released, while the other two will set them to zero.
+                          // Note that if you read fast enough from the device, then only the first one will contain any data.
+
+        // The last three bytes are always: 0x00, 0x80, 0x00
+} __attribute__((packed));
+
+struct PS4Output {
+        uint8_t bigRumble, smallRumble; // Rumble
+        uint8_t r, g, b; // RGB
+        uint8_t flashOn, flashOff; // Time to flash bright/dark (255 = 2.5 seconds)
+        bool reportChanged; // The data is send when data is received from the controller
+} __attribute__((packed));
+
+enum DPADEnum {
+        DPAD_UP = 0x0,
+        DPAD_UP_RIGHT = 0x1,
+        DPAD_RIGHT = 0x2,
+        DPAD_RIGHT_DOWN = 0x3,
+        DPAD_DOWN = 0x4,
+        DPAD_DOWN_LEFT = 0x5,
+        DPAD_LEFT = 0x6,
+        DPAD_LEFT_UP = 0x7,
+        DPAD_OFF = 0x8,
+};
+
+/** This class parses all the data sent by the PS4 controller */
+class PS4Parser {
+public:
+        /** Constructor for the PS4Parser class. */
+        PS4Parser() {
+                Reset();
+        };
+
+        /** @name PS4 Controller functions */
+        /**
+         * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
+         *
+         * While getButtonClick(ButtonEnum b) will only return it once.
+         *
+         * So you instance if you need to increase a variable once you would use getButtonClick(ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
+         * @return            getButtonPress(ButtonEnum b) will return a true as long as a button is held down, while getButtonClick(ButtonEnum b) will return true once for each button press.
+         */
+        bool getButtonPress(ButtonEnum b);
+        bool getButtonClick(ButtonEnum b);
+        /**@}*/
+        /** @name PS4 Controller functions */
+        /**
+         * Used to get the analog value from button presses.
+         * @param  b The ::ButtonEnum to read.
+         * The supported buttons are:
+         * ::UP, ::RIGHT, ::DOWN, ::LEFT, ::L1, ::L2, ::R1, ::R2,
+         * ::TRIANGLE, ::CIRCLE, ::CROSS, ::SQUARE, and ::T.
+         * @return   Analog value in the range of 0-255.
+         */
+        uint8_t getAnalogButton(ButtonEnum b);
+
+        /**
+         * Used to read the analog joystick.
+         * @param  a ::LeftHatX, ::LeftHatY, ::RightHatX, and ::RightHatY.
+         * @return   Return the analog value in the range of 0-255.
+         */
+        uint8_t getAnalogHat(AnalogHatEnum a);
+
+        /**
+         * Get the x-coordinate of the touchpad. Position 0 is in the top left.
+         * @param  finger 0 = first finger, 1 = second finger. If omitted, then 0 will be used.
+         * @param  xyId   The controller sends out three packets with the same structure.
+         *                The third one will contain the last measure, but if you read from the controller then there is only be data in the first one.
+         *                For that reason it will be set to 0 if the argument is omitted.
+         * @return        Returns the x-coordinate of the finger.
+         */
+        uint16_t getX(uint8_t finger = 0, uint8_t xyId = 0) {
+                return ps4Data.xy[xyId].finger[finger].x;
+        };
+
+        /**
+         * Get the y-coordinate of the touchpad. Position 0 is in the top left.
+         * @param  finger 0 = first finger, 1 = second finger. If omitted, then 0 will be used.
+         * @param  xyId   The controller sends out three packets with the same structure.
+         *                The third one will contain the last measure, but if you read from the controller then there is only be data in the first one.
+         *                For that reason it will be set to 0 if the argument is omitted.
+         * @return        Returns the y-coordinate of the finger.
+         */
+        uint16_t getY(uint8_t finger = 0, uint8_t xyId = 0) {
+                return ps4Data.xy[xyId].finger[finger].y;
+        };
+
+        /**
+         * Returns whenever the user is toucing the touchpad.
+         * @param  finger 0 = first finger, 1 = second finger. If omitted, then 0 will be used.
+         * @param  xyId   The controller sends out three packets with the same structure.
+         *                The third one will contain the last measure, but if you read from the controller then there is only be data in the first one.
+         *                For that reason it will be set to 0 if the argument is omitted.
+         * @return        Returns true if the specific finger is touching the touchpad.
+         */
+        bool isTouching(uint8_t finger = 0, uint8_t xyId = 0) {
+                return !(ps4Data.xy[xyId].finger[finger].touching); // The bit is cleared when a finger is touching the touchpad
+        };
+
+        /**
+         * This counter increments every time a finger touches the touchpad.
+         * @param  finger 0 = first finger, 1 = second finger. If omitted, then 0 will be used.
+         * @param  xyId   The controller sends out three packets with the same structure.
+         *                The third one will contain the last measure, but if you read from the controller then there is only be data in the first one.
+         *                For that reason it will be set to 0 if the argument is omitted.
+         * @return        Return the value of the counter, note that it is only a 7-bit value.
+         */
+        uint8_t getTouchCounter(uint8_t finger = 0, uint8_t xyId = 0) {
+                return ps4Data.xy[xyId].finger[finger].counter;
+        };
+
+        /**
+         * Get the angle of the controller calculated using the accelerometer.
+         * @param  a Either ::Pitch or ::Roll.
+         * @return   Return the angle in the range of 0-360.
+         */
+        double getAngle(AngleEnum a) {
+                if (a == Pitch)
+                        return (atan2(ps4Data.accY, ps4Data.accZ) + PI) * RAD_TO_DEG;
+                else
+                        return (atan2(ps4Data.accX, ps4Data.accZ) + PI) * RAD_TO_DEG;
+        };
+
+        /**
+         * Used to get the raw values from the 3-axis gyroscope and 3-axis accelerometer inside the PS4 controller.
+         * @param  s The sensor to read.
+         * @return   Returns the raw sensor reading.
+         */
+        int16_t getSensor(SensorEnum s) {
+                switch(s) {
+                        case gX:
+                                return ps4Data.gyroX;
+                        case gY:
+                                return ps4Data.gyroY;
+                        case gZ:
+                                return ps4Data.gyroZ;
+                        case aX:
+                                return ps4Data.accX;
+                        case aY:
+                                return ps4Data.accY;
+                        case aZ:
+                                return ps4Data.accZ;
+                        default:
+                                return 0;
+                }
+        };
+
+        /**
+         * Return the battery level of the PS4 controller.
+         * @return The battery level in the range 0-15.
+         */
+        uint8_t getBatteryLevel() {
+                return ps4Data.status.battery;
+        };
+
+        /**
+         * Use this to check if an USB cable is connected to the PS4 controller.
+         * @return Returns true if an USB cable is connected.
+         */
+        bool getUsbStatus() {
+                return ps4Data.status.usb;
+        };
+
+        /**
+         * Use this to check if an audio jack cable is connected to the PS4 controller.
+         * @return Returns true if an audio jack cable is connected.
+         */
+        bool getAudioStatus() {
+                return ps4Data.status.audio;
+        };
+
+        /**
+         * Use this to check if a microphone is connected to the PS4 controller.
+         * @return Returns true if a microphone is connected.
+         */
+        bool getMicStatus() {
+                return ps4Data.status.mic;
+        };
+
+        /** Turn both rumble and the LEDs off. */
+        void setAllOff() {
+                setRumbleOff();
+                setLedOff();
+        };
+
+        /** Set rumble off. */
+        void setRumbleOff() {
+                setRumbleOn(0, 0);
+        };
+
+        /**
+         * Turn on rumble.
+         * @param mode Either ::RumbleHigh or ::RumbleLow.
+         */
+        void setRumbleOn(RumbleEnum mode) {
+                if (mode == RumbleLow)
+                        setRumbleOn(0x00, 0xFF);
+                else
+                        setRumbleOn(0xFF, 0x00);
+        };
+
+        /**
+         * Turn on rumble.
+         * @param bigRumble   Value for big motor.
+         * @param smallRumble Value for small motor.
+         */
+        void setRumbleOn(uint8_t bigRumble, uint8_t smallRumble) {
+                ps4Output.bigRumble = bigRumble;
+                ps4Output.smallRumble = smallRumble;
+                ps4Output.reportChanged = true;
+        };
+
+        /** Turn all LEDs off. */
+        void setLedOff() {
+                setLed(0, 0, 0);
+        };
+
+        /**
+         * Use this to set the color using RGB values.
+         * @param r,g,b RGB value.
+         */
+        void setLed(uint8_t r, uint8_t g, uint8_t b) {
+                ps4Output.r = r;
+                ps4Output.g = g;
+                ps4Output.b = b;
+                ps4Output.reportChanged = true;
+        };
+
+        /**
+         * Use this to set the color using the predefined colors in ::ColorsEnum.
+         * @param color The desired color.
+         */
+        void setLed(ColorsEnum color) {
+                setLed((uint8_t)(color >> 16), (uint8_t)(color >> 8), (uint8_t)(color));
+        };
+
+        /**
+         * Set the LEDs flash time.
+         * @param flashOn  Time to flash bright (255 = 2.5 seconds).
+         * @param flashOff Time to flash dark (255 = 2.5 seconds).
+         */
+        void setLedFlash(uint8_t flashOn, uint8_t flashOff) {
+                ps4Output.flashOn = flashOn;
+                ps4Output.flashOff = flashOff;
+                ps4Output.reportChanged = true;
+        };
+        /**@}*/
+
+protected:
+        /**
+         * Used to parse data sent from the PS4 controller.
+         * @param len Length of the data.
+         * @param buf Pointer to the data buffer.
+         */
+        void Parse(uint8_t len, uint8_t *buf);
+
+        /** Used to reset the different buffers to their default values */
+        void Reset() {
+                uint8_t i;
+                for (i = 0; i < sizeof(ps4Data.hatValue); i++)
+                        ps4Data.hatValue[i] = 127; // Center value
+                ps4Data.btn.val = 0;
+                oldButtonState.val = 0;
+                for (i = 0; i < sizeof(ps4Data.trigger); i++)
+                        ps4Data.trigger[i] = 0;
+                for (i = 0; i < sizeof(ps4Data.xy)/sizeof(ps4Data.xy[0]); i++) {
+                        for (uint8_t j = 0; j < sizeof(ps4Data.xy[0].finger)/sizeof(ps4Data.xy[0].finger[0]); j++)
+                                ps4Data.xy[i].finger[j].touching = 1; // The bit is cleared if the finger is touching the touchpad
+                }
+
+                ps4Data.btn.dpad = DPAD_OFF;
+                oldButtonState.dpad = DPAD_OFF;
+                buttonClickState.dpad = 0;
+                oldDpad = 0;
+
+                ps4Output.bigRumble = ps4Output.smallRumble = 0;
+                ps4Output.r = ps4Output.g = ps4Output.b = 0;
+                ps4Output.flashOn = ps4Output.flashOff = 0;
+                ps4Output.reportChanged = false;
+        };
+
+        /**
+         * Send the output to the PS4 controller. This is implemented in PS4BT.h and PS4USB.h.
+         * @param output Pointer to PS4Output buffer;
+         */
+        virtual void sendOutputReport(PS4Output *output) = 0;
+
+private:
+        bool checkDpad(ButtonEnum b); // Used to check PS4 DPAD buttons
+
+        PS4Data ps4Data;
+        PS4Buttons oldButtonState, buttonClickState;
+        PS4Output ps4Output;
+        uint8_t oldDpad;
+};
+#endif
\ No newline at end of file
diff --git a/PS4USB.h b/PS4USB.h
new file mode 100644
index 00000000..16dabad4
--- /dev/null
+++ b/PS4USB.h
@@ -0,0 +1,130 @@
+/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+#ifndef _ps4usb_h_
+#define _ps4usb_h_
+
+#include "hiduniversal.h"
+#include "PS4Parser.h"
+
+#define PS4_VID 0x054C // Sony Corporation
+#define PS4_PID 0x05C4 // PS4 Controller
+
+/**
+ * This class implements support for the PS4 controller via USB.
+ * It uses the HIDUniversal class for all the USB communication.
+ */
+class PS4USB : public HIDUniversal, public PS4Parser {
+public:
+        /**
+         * Constructor for the PS4USB class.
+         * @param  p   Pointer to the USB class instance.
+         */
+        PS4USB(USB *p) :
+        HIDUniversal(p) {
+                PS4Parser::Reset();
+        };
+
+        /**
+         * Used to check if a PS4 controller is connected.
+         * @return Returns true if it is connected.
+         */
+        bool connected() {
+                return HIDUniversal::isReady() && HIDUniversal::VID == PS4_VID && HIDUniversal::PID == PS4_PID;
+        };
+
+        /**
+         * Used to call your own function when the device is successfully initialized.
+         * @param funcOnInit Function to call.
+         */
+        void attachOnInit(void (*funcOnInit)(void)) {
+                pFuncOnInit = funcOnInit;
+        };
+
+protected:
+        /** @name HIDUniversal implementation */
+        /**
+         * Used to parse USB HID data.
+         * @param hid       Pointer to the HID class.
+         * @param is_rpt_id Only used for Hubs.
+         * @param len       The length of the incoming data.
+         * @param buf       Pointer to the data buffer.
+         */
+        virtual void ParseHIDData(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {
+                if (HIDUniversal::VID == PS4_VID && HIDUniversal::PID == PS4_PID)
+                        PS4Parser::Parse(len, buf);
+        };
+
+        /**
+         * Called when a device is successfully initialized.
+         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
+         * This is useful for instance if you want to set the LEDs in a specific way.
+         */
+        virtual uint8_t OnInitSuccessful() {
+                if (HIDUniversal::VID == PS4_VID && HIDUniversal::PID == PS4_PID) {
+                        PS4Parser::Reset();
+                        if (pFuncOnInit)
+                                pFuncOnInit(); // Call the user function
+                        else
+                                setLed(Blue);
+                };
+                return 0;
+        };
+        /**@}*/
+
+        /** @name PS4Parser implementation */
+        virtual void sendOutputReport(PS4Output *output) { // Source: https://github.com/chrippa/ds4drv
+                uint8_t buf[32];
+                memset(buf, 0, sizeof(buf));
+
+                buf[0] = 0x05; // Report ID
+                buf[1]= 0xFF;
+
+                buf[4] = output->smallRumble; // Small Rumble
+                buf[5] = output->bigRumble; // Big rumble
+
+                buf[6] = output->r; // Red
+                buf[7] = output->g; // Green
+                buf[8] = output->b; // Blue
+
+                buf[9] = output->flashOn; // Time to flash bright (255 = 2.5 seconds)
+                buf[10] = output->flashOff; // Time to flash dark (255 = 2.5 seconds)
+
+                output->reportChanged = false;
+
+                // The PS4 console actually set the four last bytes to a CRC32 checksum, but it seems like it is actually not needed
+
+                pUsb->outTransfer(bAddress, epInfo[ hidInterfaces[0].epIndex[epInterruptOutIndex] ].epAddr, sizeof(buf), buf);
+        };
+        /**@}*/
+
+        /** @name USBDeviceConfig implementation */
+        /**
+         * Used by the USB core to check what this driver support.
+         * @param  vid The device's VID.
+         * @param  pid The device's PID.
+         * @return     Returns true if the device's VID and PID matches this driver.
+         */
+        virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) {
+                return (vid == PS4_VID && pid == PS4_PID);
+        };
+        /**@}*/
+
+private:
+        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
+};
+#endif
\ No newline at end of file
diff --git a/PSBuzz.cpp b/PSBuzz.cpp
new file mode 100644
index 00000000..56ae618d
--- /dev/null
+++ b/PSBuzz.cpp
@@ -0,0 +1,82 @@
+/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+#include "PSBuzz.h"
+
+// To enable serial debugging see "settings.h"
+//#define PRINTREPORT // Uncomment to print the report send by the PS Buzz Controllers
+
+void PSBuzz::ParseHIDData(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {
+        if (HIDUniversal::VID == PSBUZZ_VID && HIDUniversal::PID == PSBUZZ_PID && len > 0 && buf) {
+#ifdef PRINTREPORT
+                Notify(PSTR("\r\n"), 0x80);
+                for (uint8_t i = 0; i < len; i++) {
+                        D_PrintHex<uint8_t > (buf[i], 0x80);
+                        Notify(PSTR(" "), 0x80);
+                }
+#endif
+                memcpy(&psbuzzButtons, buf + 2, min(len - 2, sizeof(psbuzzButtons)));
+
+                if (psbuzzButtons.val != oldButtonState.val) { // Check if anything has changed
+                        buttonClickState.val = psbuzzButtons.val & ~oldButtonState.val; // Update click state variable
+                        oldButtonState.val = psbuzzButtons.val;
+                }
+        }
+};
+
+uint8_t PSBuzz::OnInitSuccessful() {
+        if (HIDUniversal::VID == PSBUZZ_VID && HIDUniversal::PID == PSBUZZ_PID) {
+                Reset();
+                if (pFuncOnInit)
+                        pFuncOnInit(); // Call the user function
+                else
+                        setLedOnAll(); // Turn the LED on, on all four controllers
+        };
+        return 0;
+};
+
+bool PSBuzz::getButtonPress(ButtonEnum b, uint8_t controller) {
+        return psbuzzButtons.val & (1UL << (b + 5 * controller)); // Each controller uses 5 bits, so the value is shifted 5 for each controller
+};
+
+bool PSBuzz::getButtonClick(ButtonEnum b, uint8_t controller) {
+        uint32_t mask = (1UL << (b + 5 * controller)); // Each controller uses 5 bits, so the value is shifted 5 for each controller
+        bool click = buttonClickState.val & mask;
+        buttonClickState.val &= ~mask; // Clear "click" event
+        return click;
+};
+
+// Source: http://www.developerfusion.com/article/84338/making-usb-c-friendly/ and https://github.com/torvalds/linux/blob/master/drivers/hid/hid-sony.c
+void PSBuzz::setLedRaw(bool value, uint8_t controller) {
+        ledState[controller] = value; // Save value for next time it is called
+
+        uint8_t buf[7];
+        buf[0] = 0x00;
+        buf[1] = ledState[0] ? 0xFF : 0x00;
+        buf[2] = ledState[1] ? 0xFF : 0x00;
+        buf[3] = ledState[2] ? 0xFF : 0x00;
+        buf[4] = ledState[3] ? 0xFF : 0x00;
+        buf[5] = 0x00;
+        buf[6] = 0x00;
+
+        PSBuzz_Command(buf, sizeof(buf));
+};
+
+void PSBuzz::PSBuzz_Command(uint8_t *data, uint16_t nbytes) {
+        // bmRequest = Host to device (0x00) | Class (0x20) | Interface (0x01) = 0x21, bRequest = Set Report (0x09), Report ID (0x00), Report Type (Output 0x02), interface (0x00), datalength, datalength, data)
+        pUsb->ctrlReq(bAddress, epInfo[0].epAddr, bmREQ_HIDOUT, HID_REQUEST_SET_REPORT, 0x00, 0x02, 0x00, nbytes, nbytes, data, NULL);
+};
\ No newline at end of file
diff --git a/PSBuzz.h b/PSBuzz.h
new file mode 100644
index 00000000..1d7859d2
--- /dev/null
+++ b/PSBuzz.h
@@ -0,0 +1,185 @@
+/* Copyright (C) 2014 Kristian Lauszus, TKJ Electronics. All rights reserved.
+
+ This software may be distributed and modified under the terms of the GNU
+ General Public License version 2 (GPL2) as published by the Free Software
+ Foundation and appearing in the file GPL2.TXT included in the packaging of
+ this file. Please note that GPL2 Section 2[b] requires that all works based
+ on this software must also be made publicly available under the terms of
+ the GPL2 ("Copyleft").
+
+ Contact information
+ -------------------
+
+ Kristian Lauszus, TKJ Electronics
+ Web      :  http://www.tkjelectronics.com
+ e-mail   :  kristianl@tkjelectronics.com
+ */
+
+#ifndef _psbuzz_h_
+#define _psbuzz_h_
+
+#include "hiduniversal.h"
+#include "controllerEnums.h"
+
+#define PSBUZZ_VID 0x054C // Sony Corporation
+#define PSBUZZ_PID 0x1000 // PS Buzz Controller
+
+/** Struct used to easily read the different buttons on the controllers */
+union PSBUZZButtons {
+        struct {
+                uint8_t red : 1;
+                uint8_t yellow : 1;
+                uint8_t green : 1;
+                uint8_t orange : 1;
+                uint8_t blue : 1;
+        } __attribute__((packed)) btn[4];
+        uint32_t val : 20;
+} __attribute__((packed));
+
+/**
+ * This class implements support for the PS Buzz controllers via USB.
+ * It uses the HIDUniversal class for all the USB communication.
+ */
+class PSBuzz : public HIDUniversal {
+public:
+        /**
+         * Constructor for the PSBuzz class.
+         * @param  p   Pointer to the USB class instance.
+         */
+        PSBuzz(USB *p) :
+        HIDUniversal(p) {
+                Reset();
+        };
+
+        /**
+         * Used to check if a PS Buzz controller is connected.
+         * @return Returns true if it is connected.
+         */
+        bool connected() {
+                return HIDUniversal::isReady() && HIDUniversal::VID == PSBUZZ_VID && HIDUniversal::PID == PSBUZZ_PID;
+        };
+
+        /**
+         * Used to call your own function when the device is successfully initialized.
+         * @param funcOnInit Function to call.
+         */
+        void attachOnInit(void (*funcOnInit)(void)) {
+                pFuncOnInit = funcOnInit;
+        };
+
+        /** @name PS Buzzer Controller functions */
+        /**
+         * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
+         *
+         * While getButtonClick(ButtonEnum b) will only return it once.
+         *
+         * So you instance if you need to increase a variable once you would use getButtonClick(ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
+         * @param  controller The controller to read from. Default to 0.
+         * @return            getButtonPress(ButtonEnum b) will return a true as long as a button is held down, while getButtonClick(ButtonEnum b) will return true once for each button press.
+         */
+        bool getButtonPress(ButtonEnum b, uint8_t controller = 0);
+        bool getButtonClick(ButtonEnum b, uint8_t controller = 0);
+        /**@}*/
+        /** @name PS Buzzer Controller functions */
+        /**
+         * Set LED value without using ::LEDEnum.
+         * @param value See: ::LEDEnum.
+         */
+        /**
+         * Set LED values directly.
+         * @param value      Used to set whenever the LED should be on or off
+         * @param controller The controller to control. Defaults to 0.
+         */
+        void setLedRaw(bool value, uint8_t controller = 0);
+
+        /** Turn all LEDs off. */
+        void setLedOffAll() {
+                for (uint8_t i = 1; i < 4; i++) // Skip first as it will be set in setLedRaw
+                        ledState[i] = false; // Just an easy way to set all four off at the same time
+                setLedRaw(false); // Turn the LED off, on all four controllers
+        };
+
+        /**
+         * Turn the LED off on a specific controller.
+         * @param controller The controller to turn off. Defaults to 0.
+         */
+        void setLedOff(uint8_t controller = 0) {
+                setLedRaw(false, controller);
+        };
+
+
+        /** Turn all LEDs on. */
+        void setLedOnAll() {
+                for (uint8_t i = 1; i < 4; i++) // Skip first as it will be set in setLedRaw
+                        ledState[i] = true; // Just an easy way to set all four off at the same time
+                setLedRaw(true); // Turn the LED on, on all four controllers
+        };
+
+        /**
+         * Turn the LED on on a specific controller.
+         * @param controller The controller to turn off. Defaults to 0.
+         */
+        void setLedOn(uint8_t controller = 0) {
+                setLedRaw(true, controller);
+        };
+
+        /**
+         * Toggle the LED on a specific controller.
+         * @param controller The controller to turn off. Defaults to 0.
+         */
+        void setLedToggle(uint8_t controller = 0) {
+                setLedRaw(!ledState[controller], controller);
+        };
+        /**@}*/
+
+protected:
+        /** @name HIDUniversal implementation */
+        /**
+         * Used to parse USB HID data.
+         * @param hid       Pointer to the HID class.
+         * @param is_rpt_id Only used for Hubs.
+         * @param len       The length of the incoming data.
+         * @param buf       Pointer to the data buffer.
+         */
+        virtual void ParseHIDData(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf);
+
+        /**
+         * Called when a device is successfully initialized.
+         * Use attachOnInit(void (*funcOnInit)(void)) to call your own function.
+         * This is useful for instance if you want to set the LEDs in a specific way.
+         */
+        virtual uint8_t OnInitSuccessful();
+        /**@}*/
+
+        /** Used to reset the different buffers to their default values */
+        void Reset() {
+                psbuzzButtons.val = 0;
+                oldButtonState.val = 0;
+                buttonClickState.val = 0;
+                for (uint8_t i = 0; i < sizeof(ledState); i++)
+                        ledState[i] = 0;
+        };
+
+        /** @name USBDeviceConfig implementation */
+        /**
+         * Used by the USB core to check what this driver support.
+         * @param  vid The device's VID.
+         * @param  pid The device's PID.
+         * @return     Returns true if the device's VID and PID matches this driver.
+         */
+        virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) {
+                return (vid == PSBUZZ_VID && pid == PSBUZZ_PID);
+        };
+        /**@}*/
+
+private:
+        void (*pFuncOnInit)(void); // Pointer to function called in onInit()
+
+        void PSBuzz_Command(uint8_t *data, uint16_t nbytes);
+
+        PSBUZZButtons psbuzzButtons, oldButtonState, buttonClickState;
+        bool ledState[4];
+};
+#endif
\ No newline at end of file
diff --git a/README.md b/README.md
index 59c98caf..edb440a2 100644
--- a/README.md
+++ b/README.md
@@ -22,10 +22,30 @@ For more information about the hardware see the [Hardware Manual](http://www.cir
 * __Alexei Glushchenko, Circuits@Home__ - <alex-gl@mail.ru>
 	* Developers of the USB Core, HID, FTDI, ADK, ACM, and PL2303 libraries
 * __Kristian Lauszus, TKJ Electronics__ - <kristianl@tkjelectronics.com>
-	* Developer of the [BTD](#bluetooth-libraries), [BTHID](#bthid-library), [SPP](#spp-library), [PS3](#ps3-library), [Wii](#wii-library), and [Xbox](#xbox-library) libraries
+	* Developer of the [BTD](#bluetooth-libraries), [BTHID](#bthid-library), [SPP](#spp-library), [PS4](#ps4-library), [PS3](#ps3-library), [Wii](#wii-library), [Xbox](#xbox-library), and [PSBuzz](#ps-buzz-library) libraries
 * __Andrew Kroll__ - <xxxajk@gmail.com>
 	* Major contributor to mass storage code
 
+# Table of Contents
+
+* [How to include the library](#how-to-include-the-library)
+* [How to use the library](#how-to-use-the-library)
+    * [Documentation](#documentation)
+    * [Enable debugging](#enable-debugging)
+    * [Boards](#boards)
+    * [Bluetooth libraries](#bluetooth-libraries)
+    * [BTHID library](#bthid-library)
+    * [SPP library](#spp-library)
+    * [PS4 Library](#ps4-library)
+    * [PS3 Library](#ps3-library)
+    * [Xbox Libraries](#xbox-libraries)
+        * [Xbox library](#xbox-library)
+        * [Xbox 360 Library](#xbox-360-library)
+    * [Wii library](#wii-library)
+    * [PS Buzz Library](#ps-buzz-library)
+* [Interface modifications](#interface-modifications)
+* [FAQ](#faq)
+
 # How to include the library
 
 First download the library by clicking on the following link: <https://github.com/felis/USB_Host_Shield_2.0/archive/master.zip>.
@@ -57,7 +77,7 @@ Documentation for the library can be found at the following link: <http://felis.
 
 By default serial debugging is disabled. To turn it on simply change ```ENABLE_UHS_DEBUGGING``` to 1 in [settings.h](settings.h) like so:
 
-```
+```C++
 #define ENABLE_UHS_DEBUGGING 1
 ```
 
@@ -66,8 +86,10 @@ By default serial debugging is disabled. To turn it on simply change ```ENABLE_U
 Currently the following boards are supported by the library:
 
 * All official Arduino AVR boards (Uno, Duemilanove, Mega, Mega 2560, Mega ADK, Leonardo etc.)
-* Teensy (Teensy++ 1.0, Teensy 2.0, Teensy++ 2.0, and Teensy 3.0)
-	* Note if you are using the Teensy 3.0 you should download this SPI library as well: <https://github.com/xxxajk/spi4teensy3>. You should then add ```#include <spi4teensy3.h>``` to your .ino file.
+* Arduino Due
+    * If you are using the Arduino Due, then you must include the Arduino SPI library like so: ```#include <SPI.h>``` in your .ino file.
+* Teensy (Teensy++ 1.0, Teensy 2.0, Teensy++ 2.0, and Teensy 3.x)
+	* Note if you are using the Teensy 3.x you should download this SPI library as well: <https://github.com/xxxajk/spi4teensy3>. You should then add ```#include <spi4teensy3.h>``` to your .ino file.
 * Balanduino
 * Sanguino
 * Black Widdow
@@ -86,8 +108,8 @@ The [BTD library](BTD.cpp) is a general purpose library for an ordinary Bluetoot
 This library make it easy to add support for different Bluetooth services like a PS3 or a Wii controller or SPP which is a virtual serial port via Bluetooth.
 Some different examples can be found in the [example directory](examples/Bluetooth).
 
-The BTD library will also make it possible to use multiple services at once, the following example sketch is an example of this:
-<https://github.com/felis/USB_Host_Shield_2.0/blob/master/examples/Bluetooth/PS3SPP/PS3SPP.ino>.
+The BTD library also makes it possible to use multiple services at once, the following example sketch is an example of this:
+[PS3SPP.ino](examples/Bluetooth/PS3SPP/PS3SPP.ino).
 
 ### [BTHID library](BTHID.cpp)
 
@@ -95,13 +117,19 @@ The [Bluetooth HID library](BTHID.cpp) allows you to connect HID devices via Blu
 
 Currently HID mice and keyboards are supported.
 
-It uses the standard Boot protocol by default, but it is also able to use the Report protocol as well. You would simply have to call ```setProtocolMode()``` and then parse ```HID_RPT_PROTOCOL``` as an argument. You will then have to modify the parser for your device. See the example: <https://github.com/felis/USB_Host_Shield_2.0/blob/master/examples/Bluetooth/BTHID/BTHID.ino> for more information.
+It uses the standard Boot protocol by default, but it is also able to use the Report protocol as well. You would simply have to call ```setProtocolMode()``` and then parse ```HID_RPT_PROTOCOL``` as an argument. You will then have to modify the parser for your device. See the example: [BTHID.ino](examples/Bluetooth/BTHID/BTHID.ino) for more information.
+
+The [PS4 library](#ps4-library) also uses this class to handle all Bluetooth communication.
+
+For information see the following blog post: <http://blog.tkjelectronics.dk/2013/12/bluetooth-hid-devices-now-supported-by-the-usb-host-library/>.
 
 ### [SPP library](SPP.cpp)
 
 SPP stands for "Serial Port Profile" and is a Bluetooth protocol that implements a virtual comport which allows you to send data back and forth from your computer/phone to your Arduino via Bluetooth.
 It has been tested successfully on Windows, Mac OS X, Linux, and Android.
 
+Take a look at the [SPP.ino](examples/Bluetooth/SPP/SPP.ino) example for more information.
+
 More information can be found at these blog posts:
 
 * <http://www.circuitsathome.com/mcu/bluetooth-rfcommspp-service-support-for-usb-host-2-0-library-released>
@@ -110,13 +138,31 @@ More information can be found at these blog posts:
 To implement the SPP protocol I used a Bluetooth sniffing tool called [PacketLogger](http://www.tkjelectronics.com/uploads/PacketLogger.zip) developed by Apple.
 It enables me to see the Bluetooth communication between my Mac and any device.
 
+### PS4 Library
+
+The PS4BT library is split up into the [PS4BT](PS4BT.h) and the [PS4USB](PS4USB.h) library. These allow you to use the Sony PS4 controller via Bluetooth and USB.
+
+The [PS4BT.ino](examples/Bluetooth/PS4BT/PS4BT.ino) and [PS4USB.ino](examples/PS4USB/PS4USB.ino) examples shows how to easily read the buttons, joysticks, touchpad and IMU on the controller via Bluetooth and USB respectively. It is also possible to control the rumble and light on the controller and get the battery level.
+
+Before you can use the PS4 controller via Bluetooth you will need to pair with it.
+
+Simply create the PS4BT instance like so: ```PS4BT PS4(&Btd, PAIR);``` and then hold down the Share button and then hold down the PS without releasing the Share button. The PS4 controller will then start to blink rapidly indicating that it is in paring mode.
+
+It should then automatically pair the dongle with your controller. This only have to be done once.
+
+For information see the following blog post: <http://blog.tkjelectronics.dk/2014/01/ps4-controller-now-supported-by-the-usb-host-library/>.
+
+Also check out this excellent Wiki by Frank Zhao about the PS4 controller: <http://eleccelerator.com/wiki/index.php?title=DualShock_4> and this Linux driver: <https://github.com/chrippa/ds4drv>.
+
 ### PS3 Library
 
 These libraries consist of the [PS3BT](PS3BT.cpp) and [PS3USB](PS3USB.cpp). These libraries allows you to use a Dualshock 3, Navigation or a Motion controller with the USB Host Shield both via Bluetooth and USB.
 
-In order to use your Playstation controller via Bluetooth you have to set the Bluetooth address of the dongle internally to your PS3 Controller. This can be achieved by plugging the controller in via USB and letting the library set it automatically.
+In order to use your Playstation controller via Bluetooth you have to set the Bluetooth address of the dongle internally to your PS3 Controller. This can be achieved by first plugging in the Bluetooth dongle and wait a few seconds. Now plug in the controller via USB and wait until the LEDs start to flash. The library has now written the Bluetooth address of the dongle to the PS3 controller.
 
-__Note:__ To obtain the address you have to plug in the Bluetooth dongle before connecting the controller, or alternatively you could set it in code like so: <https://github.com/felis/USB_Host_Shield_2.0/blob/master/examples/Bluetooth/PS3BT/PS3BT.ino#L15>.
+Finally simply plug in the Bluetooth dongle again and press PS on the PS3 controller. After a few seconds it should be connected to the dongle and ready to use.
+
+__Note:__ You will have to plug in the Bluetooth dongle before connecting the controller, as the library needs to read the address of the dongle. Alternatively you could set it in code like so: [PS3BT.ino#L20](examples/Bluetooth/PS3BT/PS3BT.ino#L20).
 
 For more information about the PS3 protocol see the official wiki: <https://github.com/felis/USB_Host_Shield_2.0/wiki/PS3-Information>.
 
@@ -181,26 +227,31 @@ The [Wii](Wii.cpp) library support the Wiimote, but also the Nunchuch and Motion
 
 First you have to pair with the controller, this is done automatically by the library if you create the instance like so:
 
-```
-WII Wii(&Btd,PAIR);
+```C++
+WII Wii(&Btd, PAIR);
 ```
 
 And then press 1 & 2 at once on the Wiimote or press sync if you are using a Wii U Pro Controller.
 
 After that you can simply create the instance like so:
 
-```
+```C++
 WII Wii(&Btd);
 ```
 
 Then just press any button on the Wiimote and it will then connect to the dongle.
 
-Take a look at the example for more information: <https://github.com/felis/USB_Host_Shield_2.0/blob/master/examples/Bluetooth/Wii/Wii.ino>.
+Take a look at the example for more information: [Wii.ino](examples/Bluetooth/Wii/Wii.ino).
 
 Also take a look at the blog post:
 
 * <http://blog.tkjelectronics.dk/2012/08/wiimote-added-to-usb-host-library/>
 
+The Wii IR camera can also be used, but you will have to activate the code for it manually as it is quite large. Simply set ```ENABLE_WII_IR_CAMERA``` to 1 in [settings.h](settings.h).
+
+The [WiiIRCamera.ino](examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino) example shows how it can be used.
+
+
 All the information about the Wii controllers are from these sites:
 
 * <http://wiibrew.org/wiki/Wiimote>
@@ -209,8 +260,42 @@ All the information about the Wii controllers are from these sites:
 * <http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus>
 * The old library created by _Tomoyuki Tanaka_: <https://github.com/moyuchin/WiiRemote_on_Arduino> also helped a lot.
 
+### [PS Buzz Library](PSBuzz.cpp)
+
+This library implements support for the Playstation Buzz controllers via USB.
+
+It is essentially just a wrapper around the [HIDUniversal](hiduniversal.cpp) which takes care of the initializing and reading of the controllers. The [PSBuzz](PSBuzz.cpp) class simply inherits this and parses the data, so it is easy for users to read the buttons and turn the big red button on the controllers on and off.
+
+The example [PSBuzz.ino](examples/PSBuzz/PSBuzz.ino) shows how one can do this with just a few lines of code.
+
+More information about the controller can be found at the following sites:
+
+* http://www.developerfusion.com/article/84338/making-usb-c-friendly/
+* https://github.com/torvalds/linux/blob/master/drivers/hid/hid-sony.c
+
+# Interface modifications
+
+The shield is using SPI for communicating with the MAX3421E USB host controller. It uses the SCK, MISO and MOSI pins via the ICSP on your board.
+
+Furthermore it uses one pin as SS and one INT pin. These are by default located on pin 10 and 9 respectively. They can easily be reconfigured in case you need to use them for something else by cutting the jumper on the shield and then solder a wire from the pad to the new pin.
+
+After that you need modify the following entry in [UsbCore.h](UsbCore.h):
+
+```C++
+typedef MAX3421e<P10, P9> MAX3421E;
+```
+
+For instance if you have rerouted SS to pin 7 it should read:
+
+```C++
+typedef MAX3421e<P7, P9> MAX3421E;
+```
+
+See the "Interface modifications" section in the [hardware manual](https://www.circuitsathome.com/usb-host-shield-hardware-manual) for more information.
+
 # FAQ
 
 > When I plug my device into the USB connector nothing happens?
 
-Try to connect a external power supply to the Arduino - this solves the problem in most cases.
\ No newline at end of file
+* Try to connect a external power supply to the Arduino - this solves the problem in most cases.
+* You can also use a powered hub between the device and the USB Host Shield. You should then include the USB hub library: ```#include <usbhub.h>``` and create the instance like so: ```USBHub Hub1(&Usb);```.
\ No newline at end of file
diff --git a/SPP.cpp b/SPP.cpp
index 0c133830..562a68ca 100644
--- a/SPP.cpp
+++ b/SPP.cpp
@@ -45,7 +45,7 @@ const uint8_t rfcomm_crc_table[256] PROGMEM = {/* reversed, 8-bit, poly=0x07 */
 SPP::SPP(BTD *p, const char* name, const char* pin) :
 pBtd(p) // Pointer to BTD class instance - mandatory
 {
-        if (pBtd)
+        if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
 
         pBtd->btdName = name;
@@ -73,33 +73,34 @@ void SPP::Reset() {
 
 void SPP::disconnect() {
         connected = false;
-        // First the two L2CAP channels has to be disconencted and then the HCI connection
-        if (RFCOMMConnected)
-                pBtd->l2cap_disconnection_request(hci_handle, 0x0A, rfcomm_scid, rfcomm_dcid);
-        if (RFCOMMConnected && SDPConnected)
+        // First the two L2CAP channels has to be disconnected and then the HCI connection
+        if(RFCOMMConnected)
+                pBtd->l2cap_disconnection_request(hci_handle, ++identifier, rfcomm_scid, rfcomm_dcid);
+        if(RFCOMMConnected && SDPConnected)
                 delay(1); // Add delay between commands
-        if (SDPConnected)
-                pBtd->l2cap_disconnection_request(hci_handle, 0x0B, sdp_scid, sdp_dcid);
+        if(SDPConnected)
+                pBtd->l2cap_disconnection_request(hci_handle, ++identifier, sdp_scid, sdp_dcid);
         l2cap_sdp_state = L2CAP_DISCONNECT_RESPONSE;
 }
 
 void SPP::ACLData(uint8_t* l2capinbuf) {
-        if (!connected) {
-                if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
-                        if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM && !pBtd->sdpConnectionClaimed) {
+        if(!connected) {
+                if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM && !pBtd->sdpConnectionClaimed) {
                                 pBtd->sdpConnectionClaimed = true;
                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
                                 l2cap_sdp_state = L2CAP_SDP_WAIT; // Reset state
-                        } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM && !pBtd->rfcommConnectionClaimed) {
+                        } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM && !pBtd->rfcommConnectionClaimed) {
                                 pBtd->rfcommConnectionClaimed = true;
                                 hci_handle = pBtd->hci_handle; // Store the HCI Handle for the connection
                                 l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; // Reset state
                         }
                 }
         }
-        if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000))) { // acl_handle_ok
-                if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
-                        if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
+        //if((l2capinbuf[0] | (uint16_t)l2capinbuf[1] << 8) == (hci_handle | 0x2000U)) { // acl_handle_ok
+        if(UHS_ACL_HANDLE_OK(l2capinbuf, hci_handle)) { // acl_handle_ok
+                if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { //l2cap_control - Channel ID for ACL-U
+                        if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -114,7 +115,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR(" "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 #endif
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 #ifdef EXTRADEBUG
                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -127,58 +128,56 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR(" Identifier: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 #endif
-                                if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so
+                                if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { // It doesn't matter if it receives another reqeust, since it waits for the channel to disconnect in the L2CAP_SDP_DONE state, and the l2cap_event_flag will be cleared if so
                                         identifier = l2capinbuf[9];
                                         sdp_scid[0] = l2capinbuf[14];
                                         sdp_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_SDP_REQUEST;
-                                } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM) { // ----- || -----
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST);
+                                } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM) { // ----- || -----
                                         identifier = l2capinbuf[9];
                                         rfcomm_scid[0] = l2capinbuf[14];
                                         rfcomm_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST;
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
-                                if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
-                                        if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
+                                if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
+                                        if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
                                                 //Notify(PSTR("\r\nSDP Configuration Complete"), 0x80);
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_SUCCESS;
-                                        } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS);
+                                        } else if(l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
                                                 //Notify(PSTR("\r\nRFCOMM Configuration Complete"), 0x80);
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS;
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS);
                                         }
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
-                                if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
+                                if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
                                         //Notify(PSTR("\r\nSDP Configuration Request"), 0x80);
-                                        identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_REQUEST;
-                                } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
+                                        pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], sdp_scid);
+                                } else if(l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
                                         //Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80);
-                                        identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_REQUEST;
+                                        pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], rfcomm_scid);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
-                                if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
+                                if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_SDP_REQUEST;
-                                } else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST);
+                                } else if(l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST;
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
-                                if (l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
+                                if(l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: SDP Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE;
-                                } else if (l2capinbuf[12] == rfcomm_scid[0] && l2capinbuf[13] == rfcomm_scid[1]) {
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RESPONSE);
+                                } else if(l2capinbuf[12] == rfcomm_scid[0] && l2capinbuf[13] == rfcomm_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: RFCOMM Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE;
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RESPONSE);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nInformation request"), 0x80);
 #endif
@@ -191,18 +190,18 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
                         }
 #endif
-                } else if (l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP
-                        if (l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) {
-                                if (((l2capinbuf[16] << 8 | l2capinbuf[17]) == SERIALPORT_UUID) || ((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000 && (l2capinbuf[18] << 8 | l2capinbuf[19]) == SERIALPORT_UUID)) { // Check if it's sending the full UUID, see: https://www.bluetooth.org/Technical/AssignedNumbers/service_discovery.htm, we will just check the first four bytes
-                                        if (firstMessage) {
+                } else if(l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP
+                        if(l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) {
+                                if(((l2capinbuf[16] << 8 | l2capinbuf[17]) == SERIALPORT_UUID) || ((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000 && (l2capinbuf[18] << 8 | l2capinbuf[19]) == SERIALPORT_UUID)) { // Check if it's sending the full UUID, see: https://www.bluetooth.org/Technical/AssignedNumbers/service_discovery.htm, we will just check the first four bytes
+                                        if(firstMessage) {
                                                 serialPortResponse1(l2capinbuf[9], l2capinbuf[10]);
                                                 firstMessage = false;
                                         } else {
                                                 serialPortResponse2(l2capinbuf[9], l2capinbuf[10]); // Serialport continuation state
                                                 firstMessage = true;
                                         }
-                                } else if (((l2capinbuf[16] << 8 | l2capinbuf[17]) == L2CAP_UUID) || ((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000 && (l2capinbuf[18] << 8 | l2capinbuf[19]) == L2CAP_UUID)) {
-                                        if (firstMessage) {
+                                } else if(((l2capinbuf[16] << 8 | l2capinbuf[17]) == L2CAP_UUID) || ((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000 && (l2capinbuf[18] << 8 | l2capinbuf[19]) == L2CAP_UUID)) {
+                                        if(firstMessage) {
                                                 l2capResponse1(l2capinbuf[9], l2capinbuf[10]);
                                                 firstMessage = false;
                                         } else {
@@ -218,14 +217,14 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                         uuid = (l2capinbuf[18] << 8 | l2capinbuf[19]);
                                 else // Short UUID
                                         uuid = (l2capinbuf[16] << 8 | l2capinbuf[17]);
-                                D_PrintHex<uint16_t> (uuid, 0x80);
+                                D_PrintHex<uint16_t > (uuid, 0x80);
 
                                 Notify(PSTR("\r\nLength: "), 0x80);
                                 uint16_t length = l2capinbuf[11] << 8 | l2capinbuf[12];
-                                D_PrintHex<uint16_t> (length, 0x80);
+                                D_PrintHex<uint16_t > (length, 0x80);
                                 Notify(PSTR("\r\nData: "), 0x80);
-                                for (uint8_t i = 0; i < length; i++) {
-                                        D_PrintHex<uint8_t> (l2capinbuf[13+i], 0x80);
+                                for(uint8_t i = 0; i < length; i++) {
+                                        D_PrintHex<uint8_t > (l2capinbuf[13 + i], 0x80);
                                         Notify(PSTR(" "), 0x80);
                                 }
 #endif
@@ -236,14 +235,14 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
                         }
 #endif
-                } else if (l2capinbuf[6] == rfcomm_dcid[0] && l2capinbuf[7] == rfcomm_dcid[1]) { // RFCOMM
+                } else if(l2capinbuf[6] == rfcomm_dcid[0] && l2capinbuf[7] == rfcomm_dcid[1]) { // RFCOMM
                         rfcommChannel = l2capinbuf[8] & 0xF8;
                         rfcommDirection = l2capinbuf[8] & 0x04;
                         rfcommCommandResponse = l2capinbuf[8] & 0x02;
                         rfcommChannelType = l2capinbuf[9] & 0xEF;
                         rfcommPfBit = l2capinbuf[9] & 0x10;
 
-                        if (rfcommChannel >> 3 != 0x00)
+                        if(rfcommChannel >> 3 != 0x00)
                                 rfcommChannelConnection = rfcommChannel;
 
 #ifdef EXTRADEBUG
@@ -258,7 +257,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                         Notify(PSTR(" PF_BIT: "), 0x80);
                         D_PrintHex<uint8_t > (rfcommPfBit, 0x80);
 #endif
-                        if (rfcommChannelType == RFCOMM_DISC) {
+                        if(rfcommChannelType == RFCOMM_DISC) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nReceived Disconnect RFCOMM Command on channel: "), 0x80);
                                 D_PrintHex<uint8_t > (rfcommChannel >> 3, 0x80);
@@ -266,15 +265,15 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                 connected = false;
                                 sendRfcomm(rfcommChannel, rfcommDirection, rfcommCommandResponse, RFCOMM_UA, rfcommPfBit, rfcommbuf, 0x00); // UA Command
                         }
-                        if (connected) {
+                        if(connected) {
                                 /* Read the incoming message */
-                                if (rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) {
+                                if(rfcommChannelType == RFCOMM_UIH && rfcommChannel == rfcommChannelConnection) {
                                         uint8_t length = l2capinbuf[10] >> 1; // Get length
                                         uint8_t offset = l2capinbuf[4] - length - 4; // Check if there is credit
-                                        if (checkFcs(&l2capinbuf[8], l2capinbuf[11 + length + offset])) {
+                                        if(checkFcs(&l2capinbuf[8], l2capinbuf[11 + length + offset])) {
                                                 uint8_t i = 0;
-                                                for (; i < length; i++) {
-                                                        if (rfcommAvailable + i >= sizeof (rfcommDataBuffer)) {
+                                                for(; i < length; i++) {
+                                                        if(rfcommAvailable + i >= sizeof (rfcommDataBuffer)) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nWarning: Buffer is full!"), 0x80);
 #endif
@@ -286,7 +285,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
 #ifdef EXTRADEBUG
                                                 Notify(PSTR("\r\nRFCOMM Data Available: "), 0x80);
                                                 Notify(rfcommAvailable, 0x80);
-                                                if (offset) {
+                                                if(offset) {
                                                         Notify(PSTR(" - Credit: 0x"), 0x80);
                                                         D_PrintHex<uint8_t > (l2capinbuf[11], 0x80);
                                                 }
@@ -297,10 +296,10 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                                 Notify(PSTR("\r\nError in FCS checksum!"), 0x80);
 #endif
 #ifdef PRINTREPORT // Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth
-                                        for (uint8_t i = 0; i < length; i++)
+                                        for(uint8_t i = 0; i < length; i++)
                                                 Notifyc(l2capinbuf[i + 11 + offset], 0x80);
 #endif
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80);
 #endif
@@ -315,7 +314,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                         rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm.
                                         rfcommbuf[9] = l2capinbuf[20]; // Number of Frames
                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A); // UIH Remote Port Negotiation Response
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80);
 #endif
@@ -326,12 +325,12 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04);
                                 }
                         } else {
-                                if (rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish
+                                if(rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nReceived SABM Command"), 0x80);
 #endif
                                         sendRfcomm(rfcommChannel, rfcommDirection, rfcommCommandResponse, RFCOMM_UA, rfcommPfBit, rfcommbuf, 0x00); // UA Command
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nReceived UIH Parameter Negotiation Command"), 0x80);
 #endif
@@ -346,7 +345,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                         rfcommbuf[8] = 0x00; // MaxRatransm.
                                         rfcommbuf[9] = 0x00; // Number of Frames
                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x0A);
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nSend UIH Modem Status Response"), 0x80);
 #endif
@@ -366,8 +365,8 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                         rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES)
 
                                         sendRfcomm(rfcommChannel, rfcommDirection, 0, RFCOMM_UIH, rfcommPfBit, rfcommbuf, 0x04);
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response
-                                        if (!creditSent) {
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response
+                                        if(!creditSent) {
 #ifdef DEBUG_USB_HOST
                                                 Notify(PSTR("\r\nSend UIH Command with credit"), 0x80);
 #endif
@@ -376,11 +375,11 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                                 timer = millis();
                                                 waitForLastCommand = true;
                                         }
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nReceived UIH Command with credit"), 0x80);
 #endif
-                                } else if (rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
+                                } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nReceived UIH Remote Port Negotiation Command"), 0x80);
 #endif
@@ -403,8 +402,8 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                                         connected = true; // The RFCOMM channel is now established
                                         sppIndex = 0;
                                 }
-#ifdef DEBUG_USB_HOST
-                                else if (rfcommChannelType != RFCOMM_DISC) {
+#ifdef EXTRADEBUG
+                                else if(rfcommChannelType != RFCOMM_DISC) {
                                         Notify(PSTR("\r\nUnsupported RFCOMM Data - ChannelType: "), 0x80);
                                         D_PrintHex<uint8_t > (rfcommChannelType, 0x80);
                                         Notify(PSTR(" Command: "), 0x80);
@@ -427,7 +426,7 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
 }
 
 void SPP::Run() {
-        if (waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it
+        if(waitForLastCommand && (millis() - timer) > 100) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n"), 0x80);
 #endif
@@ -440,10 +439,10 @@ void SPP::Run() {
 }
 
 void SPP::SDP_task() {
-        switch (l2cap_sdp_state) {
+        switch(l2cap_sdp_state) {
                 case L2CAP_SDP_WAIT:
-                        if (l2cap_connection_request_sdp_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_SDP_REQUEST; // Clear flag
+                        if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST)) {
+                                l2cap_clear_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST); // Clear flag
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSDP Incoming Connection Request"), 0x80);
 #endif
@@ -453,64 +452,46 @@ void SPP::SDP_task() {
                                 identifier++;
                                 delay(1);
                                 pBtd->l2cap_config_request(hci_handle, identifier, sdp_scid);
-                                l2cap_sdp_state = L2CAP_SDP_REQUEST;
-                        }
-                        break;
-                case L2CAP_SDP_REQUEST:
-                        if (l2cap_config_request_sdp_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_REQUEST; // Clear flag
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nSDP Configuration Request"), 0x80);
-#endif
-                                pBtd->l2cap_config_response(hci_handle, identifier, sdp_scid);
                                 l2cap_sdp_state = L2CAP_SDP_SUCCESS;
-                        }
-                        break;
-                case L2CAP_SDP_SUCCESS:
-                        if (l2cap_config_success_sdp_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_SDP_SUCCESS; // Clear flag
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nSDP Successfully Configured"), 0x80);
-#endif
-                                firstMessage = true; // Reset bool
-                                SDPConnected = true;
-                                l2cap_sdp_state = L2CAP_SDP_DONE;
-                        }
-                        break;
-                case L2CAP_SDP_DONE:
-                        if (l2cap_disconnect_request_sdp_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_SDP_REQUEST; // Clear flag
+                        } else if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST)) {
+                                l2cap_clear_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST); // Clear flag
                                 SDPConnected = false;
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected SDP Channel"), 0x80);
 #endif
                                 pBtd->l2cap_disconnection_response(hci_handle, identifier, sdp_dcid, sdp_scid);
-                                l2cap_sdp_state = L2CAP_SDP_WAIT;
-                        } else if (l2cap_connection_request_sdp_flag)
-                                l2cap_rfcomm_state = L2CAP_SDP_WAIT;
+                        }
                         break;
+                case L2CAP_SDP_SUCCESS:
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS)) {
+                                l2cap_clear_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS); // Clear flag
+#ifdef DEBUG_USB_HOST
+                                Notify(PSTR("\r\nSDP Successfully Configured"), 0x80);
+#endif
+                                firstMessage = true; // Reset bool
+                                SDPConnected = true;
+                                l2cap_sdp_state = L2CAP_SDP_WAIT;
+                        }
+                        break;
+
                 case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected
-                        if (l2cap_disconnect_response_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_RESPONSE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80);
 #endif
-                                RFCOMMConnected = false;
-                                SDPConnected = false;
                                 pBtd->hci_disconnect(hci_handle);
                                 hci_handle = -1; // Reset handle
-                                l2cap_event_flag = 0; // Reset flags
-                                l2cap_sdp_state = L2CAP_SDP_WAIT;
-                                l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
+                                Reset();
                         }
                         break;
         }
 }
 
 void SPP::RFCOMM_task() {
-        switch (l2cap_rfcomm_state) {
+        switch(l2cap_rfcomm_state) {
                 case L2CAP_RFCOMM_WAIT:
-                        if (l2cap_connection_request_rfcomm_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; // Clear flag
+                        if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST)) {
+                                l2cap_clear_flag(L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST); // Clear flag
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nRFCOMM Incoming Connection Request"), 0x80);
 #endif
@@ -520,43 +501,28 @@ void SPP::RFCOMM_task() {
                                 identifier++;
                                 delay(1);
                                 pBtd->l2cap_config_request(hci_handle, identifier, rfcomm_scid);
-                                l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST;
-                        }
-                        break;
-                case L2CAP_RFCOMM_REQUEST:
-                        if (l2cap_config_request_rfcomm_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; // Clear flag
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nRFCOMM Configuration Request"), 0x80);
-#endif
-                                pBtd->l2cap_config_response(hci_handle, identifier, rfcomm_scid);
                                 l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS;
-                        }
-                        break;
-                case L2CAP_RFCOMM_SUCCESS:
-                        if (l2cap_config_success_rfcomm_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; // Clear flag
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80);
-#endif
-                                rfcommAvailable = 0; // Reset number of bytes available
-                                bytesRead = 0; // Reset number of bytes received
-                                RFCOMMConnected = true;
-                                l2cap_rfcomm_state = L2CAP_RFCOMM_DONE;
-                        }
-                        break;
-                case L2CAP_RFCOMM_DONE:
-                        if (l2cap_disconnect_request_rfcomm_flag) {
-                                l2cap_event_flag &= ~L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; // Clear flag
+                        } else if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST)) {
+                                l2cap_clear_flag(L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST); // Clear flag
                                 RFCOMMConnected = false;
                                 connected = false;
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected RFCOMM Channel"), 0x80);
 #endif
                                 pBtd->l2cap_disconnection_response(hci_handle, identifier, rfcomm_dcid, rfcomm_scid);
+                        }
+                        break;
+                case L2CAP_RFCOMM_SUCCESS:
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS)) {
+                                l2cap_clear_flag(L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS); // Clear flag
+#ifdef DEBUG_USB_HOST
+                                Notify(PSTR("\r\nRFCOMM Successfully Configured"), 0x80);
+#endif
+                                rfcommAvailable = 0; // Reset number of bytes available
+                                bytesRead = 0; // Reset number of bytes received
+                                RFCOMMConnected = true;
                                 l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
-                        } else if (l2cap_connection_request_rfcomm_flag)
-                                l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT;
+                        }
                         break;
         }
 }
@@ -572,15 +538,15 @@ void SPP::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLo
         l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
         l2capoutbuf[1] = transactionIDHigh;
         l2capoutbuf[2] = transactionIDLow;
-        l2capoutbuf[3] = 0x00; // Parameter Length
-        l2capoutbuf[4] = 0x05; // Parameter Length
-        l2capoutbuf[5] = 0x00; // AttributeListsByteCount
-        l2capoutbuf[6] = 0x02; // AttributeListsByteCount
+        l2capoutbuf[3] = 0x00; // MSB Parameter Length
+        l2capoutbuf[4] = 0x05; // LSB Parameter Length = 5
+        l2capoutbuf[5] = 0x00; // MSB AttributeListsByteCount
+        l2capoutbuf[6] = 0x02; // LSB AttributeListsByteCount = 2
 
         /* Attribute ID/Value Sequence: */
-        l2capoutbuf[7] = 0x35;
-        l2capoutbuf[8] = 0x00;
-        l2capoutbuf[9] = 0x00;
+        l2capoutbuf[7] = 0x35; // Data element sequence - length in next byte
+        l2capoutbuf[8] = 0x00; // Length = 0
+        l2capoutbuf[9] = 0x00; // No continuation state
 
         SDP_Command(l2capoutbuf, 10);
 }
@@ -589,56 +555,60 @@ void SPP::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLo
         l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
         l2capoutbuf[1] = transactionIDHigh;
         l2capoutbuf[2] = transactionIDLow;
-        l2capoutbuf[3] = 0x00; // Parameter Length
-        l2capoutbuf[4] = 0x2B; // Parameter Length
-        l2capoutbuf[5] = 0x00; // AttributeListsByteCount
-        l2capoutbuf[6] = 0x26; // AttributeListsByteCount
+        l2capoutbuf[3] = 0x00; // MSB Parameter Length
+        l2capoutbuf[4] = 0x2B; // LSB Parameter Length = 43
+        l2capoutbuf[5] = 0x00; // MSB AttributeListsByteCount
+        l2capoutbuf[6] = 0x26; // LSB AttributeListsByteCount = 38
 
         /* Attribute ID/Value Sequence: */
-        l2capoutbuf[7] = 0x36;
-        l2capoutbuf[8] = 0x00;
-        l2capoutbuf[9] = 0x3C;
-        l2capoutbuf[10] = 0x36;
-        l2capoutbuf[11] = 0x00;
+        l2capoutbuf[7] = 0x36; // Data element sequence - length in next two bytes
+        l2capoutbuf[8] = 0x00; // MSB Length
+        l2capoutbuf[9] = 0x3C; // LSB Length = 60
 
-        l2capoutbuf[12] = 0x39;
-        l2capoutbuf[13] = 0x09;
-        l2capoutbuf[14] = 0x00;
-        l2capoutbuf[15] = 0x00;
-        l2capoutbuf[16] = 0x0A;
-        l2capoutbuf[17] = 0x00;
+        l2capoutbuf[10] = 0x36; // Data element sequence - length in next two bytes
+        l2capoutbuf[11] = 0x00; // MSB Length
+        l2capoutbuf[12] = 0x39; // LSB Length = 57
+
+        l2capoutbuf[13] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[14] = 0x00; // MSB ServiceRecordHandle
+        l2capoutbuf[15] = 0x00; // LSB ServiceRecordHandle
+        l2capoutbuf[16] = 0x0A; // Unsigned int - length 4 bytes
+        l2capoutbuf[17] = 0x00; // ServiceRecordHandle value - TODO: Is this related to HCI_Handle?
         l2capoutbuf[18] = 0x01;
         l2capoutbuf[19] = 0x00;
         l2capoutbuf[20] = 0x06;
-        l2capoutbuf[21] = 0x09;
-        l2capoutbuf[22] = 0x00;
-        l2capoutbuf[23] = 0x01;
-        l2capoutbuf[24] = 0x35;
-        l2capoutbuf[25] = 0x03;
-        l2capoutbuf[26] = 0x19;
-        l2capoutbuf[27] = 0x11;
 
-        l2capoutbuf[28] = 0x01;
-        l2capoutbuf[29] = 0x09;
-        l2capoutbuf[30] = 0x00;
-        l2capoutbuf[31] = 0x04;
-        l2capoutbuf[32] = 0x35;
-        l2capoutbuf[33] = 0x0C;
-        l2capoutbuf[34] = 0x35;
-        l2capoutbuf[35] = 0x03;
-        l2capoutbuf[36] = 0x19;
-        l2capoutbuf[37] = 0x01;
-        l2capoutbuf[38] = 0x00;
-        l2capoutbuf[39] = 0x35;
-        l2capoutbuf[40] = 0x05;
-        l2capoutbuf[41] = 0x19;
-        l2capoutbuf[42] = 0x00;
-        l2capoutbuf[43] = 0x03;
+        l2capoutbuf[21] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[22] = 0x00; // MSB ServiceClassIDList
+        l2capoutbuf[23] = 0x01; // LSB ServiceClassIDList
+        l2capoutbuf[24] = 0x35; // Data element sequence - length in next byte
+        l2capoutbuf[25] = 0x03; // Length = 3
+        l2capoutbuf[26] = 0x19; // UUID (universally unique identifier) - length = 2 bytes
+        l2capoutbuf[27] = 0x11; // MSB SerialPort
+        l2capoutbuf[28] = 0x01; // LSB SerialPort
 
-        l2capoutbuf[44] = 0x08;
-        l2capoutbuf[45] = 0x02; // Two extra bytes
-        l2capoutbuf[46] = 0x00; // 25 (0x19) more bytes to come
-        l2capoutbuf[47] = 0x19;
+        l2capoutbuf[29] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[30] = 0x00; // MSB ProtocolDescriptorList
+        l2capoutbuf[31] = 0x04; // LSB ProtocolDescriptorList
+        l2capoutbuf[32] = 0x35; // Data element sequence - length in next byte
+        l2capoutbuf[33] = 0x0C; // Length = 12
+
+        l2capoutbuf[34] = 0x35; // Data element sequence - length in next byte
+        l2capoutbuf[35] = 0x03; // Length = 3
+        l2capoutbuf[36] = 0x19; // UUID (universally unique identifier) - length = 2 bytes
+        l2capoutbuf[37] = 0x01; // MSB L2CAP
+        l2capoutbuf[38] = 0x00; // LSB L2CAP
+
+        l2capoutbuf[39] = 0x35; // Data element sequence - length in next byte
+        l2capoutbuf[40] = 0x05; // Length = 5
+        l2capoutbuf[41] = 0x19; // UUID (universally unique identifier) - length = 2 bytes
+        l2capoutbuf[42] = 0x00; // MSB RFCOMM
+        l2capoutbuf[43] = 0x03; // LSB RFCOMM
+        l2capoutbuf[44] = 0x08; // Unsigned Integer - length 1 byte
+
+        l2capoutbuf[45] = 0x02; // ContinuationState - Two more bytes
+        l2capoutbuf[46] = 0x00; // MSB length
+        l2capoutbuf[47] = 0x19; // LSB length = 25 more bytes to come
 
         SDP_Command(l2capoutbuf, 48);
 }
@@ -647,40 +617,49 @@ void SPP::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLo
         l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
         l2capoutbuf[1] = transactionIDHigh;
         l2capoutbuf[2] = transactionIDLow;
-        l2capoutbuf[3] = 0x00; // Parameter Length
-        l2capoutbuf[4] = 0x1C; // Parameter Length
-        l2capoutbuf[5] = 0x00; // AttributeListsByteCount
-        l2capoutbuf[6] = 0x19; // AttributeListsByteCount
+        l2capoutbuf[3] = 0x00; // MSB Parameter Length
+        l2capoutbuf[4] = 0x1C; // LSB Parameter Length = 28
+        l2capoutbuf[5] = 0x00; // MSB AttributeListsByteCount
+        l2capoutbuf[6] = 0x19; // LSB AttributeListsByteCount = 25
 
         /* Attribute ID/Value Sequence: */
-        l2capoutbuf[7] = 0x01;
-        l2capoutbuf[8] = 0x09;
-        l2capoutbuf[9] = 0x00;
-        l2capoutbuf[10] = 0x06;
-        l2capoutbuf[11] = 0x35;
+        l2capoutbuf[7] = 0x01; // Channel 1 - TODO: Try different values, so multiple servers can be used at once
 
-        l2capoutbuf[12] = 0x09;
-        l2capoutbuf[13] = 0x09;
-        l2capoutbuf[14] = 0x65;
-        l2capoutbuf[15] = 0x6E;
-        l2capoutbuf[16] = 0x09;
-        l2capoutbuf[17] = 0x00;
-        l2capoutbuf[18] = 0x6A;
-        l2capoutbuf[19] = 0x09;
+        l2capoutbuf[8] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[9] = 0x00; // MSB LanguageBaseAttributeIDList
+        l2capoutbuf[10] = 0x06; // LSB LanguageBaseAttributeIDList
+        l2capoutbuf[11] = 0x35; // Data element sequence - length in next byte
+        l2capoutbuf[12] = 0x09; // Length = 9
+
+        // Identifier representing the natural language = en = English - see: "ISO 639:1988"
+        l2capoutbuf[13] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[14] = 0x65; // 'e'
+        l2capoutbuf[15] = 0x6E; // 'n'
+
+        // "The second element of each triplet contains an identifier that specifies a character encoding used for the language"
+        // Encoding is set to 106 (UTF-8) - see: http://www.iana.org/assignments/character-sets/character-sets.xhtml
+        l2capoutbuf[16] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[17] = 0x00; // MSB of character encoding
+        l2capoutbuf[18] = 0x6A; // LSB of character encoding (106)
+
+        // Attribute ID that serves as the base attribute ID for the natural language in the service record
+        // "To facilitate the retrieval of human-readable universal attributes in a principal language, the base attribute ID value for the primary language supported by a service record shall be 0x0100"
+        l2capoutbuf[19] = 0x09; // Unsigned Integer - length 2 bytes
         l2capoutbuf[20] = 0x01;
         l2capoutbuf[21] = 0x00;
-        l2capoutbuf[22] = 0x09;
-        l2capoutbuf[23] = 0x01;
-        l2capoutbuf[24] = 0x00;
-        l2capoutbuf[25] = 0x25;
 
+        l2capoutbuf[22] = 0x09; // Unsigned Integer - length 2 bytes
+        l2capoutbuf[23] = 0x01; // MSB ServiceDescription
+        l2capoutbuf[24] = 0x00; // LSB ServiceDescription
+
+        l2capoutbuf[25] = 0x25; // Text string - length in next byte
         l2capoutbuf[26] = 0x05; // Name length
         l2capoutbuf[27] = 'T';
         l2capoutbuf[28] = 'K';
         l2capoutbuf[29] = 'J';
         l2capoutbuf[30] = 'S';
         l2capoutbuf[31] = 'P';
-        l2capoutbuf[32] = 0x00; // No more data
+        l2capoutbuf[32] = 0x00; // No continuation state
 
         SDP_Command(l2capoutbuf, 33);
 }
@@ -705,12 +684,12 @@ void SPP::sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t cha
         l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control
         l2capoutbuf[2] = length << 1 | 0x01; // Length and format (always 0x01 bytes format)
         uint8_t i = 0;
-        for (; i < length; i++)
+        for(; i < length; i++)
                 l2capoutbuf[i + 3] = data[i];
         l2capoutbuf[i + 3] = calcFcs(l2capoutbuf);
 #ifdef EXTRADEBUG
         Notify(PSTR(" - RFCOMM Data: "), 0x80);
-        for (i = 0; i < length + 4; i++) {
+        for(i = 0; i < length + 4; i++) {
                 D_PrintHex<uint8_t > (l2capoutbuf[i], 0x80);
                 Notify(PSTR(" "), 0x80);
         }
@@ -726,7 +705,7 @@ void SPP::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8
         l2capoutbuf[4] = calcFcs(l2capoutbuf);
 #ifdef EXTRADEBUG
         Notify(PSTR(" - RFCOMM Credit Data: "), 0x80);
-        for (uint8_t i = 0; i < 5; i++) {
+        for(uint8_t i = 0; i < 5; i++) {
                 D_PrintHex<uint8_t > (l2capoutbuf[i], 0x80);
                 Notify(PSTR(" "), 0x80);
         }
@@ -741,36 +720,53 @@ uint8_t SPP::crc(uint8_t *data) {
 
 /* Calculate FCS */
 uint8_t SPP::calcFcs(uint8_t *data) {
-        if ((data[1] & 0xEF) == RFCOMM_UIH)
-                return (0xFF - crc(data)); // FCS on 2 bytes
+        uint8_t temp = crc(data);
+        if((data[1] & 0xEF) == RFCOMM_UIH)
+                return (0xFF - temp); // FCS on 2 bytes
         else
-                return (0xFF - pgm_read_byte(&rfcomm_crc_table[crc(data) ^ data[2]])); // FCS on 3 bytes
+                return (0xFF - pgm_read_byte(&rfcomm_crc_table[temp ^ data[2]])); // FCS on 3 bytes
 }
 
 /* Check FCS */
 bool SPP::checkFcs(uint8_t *data, uint8_t fcs) {
         uint8_t temp = crc(data);
-        if ((data[1] & 0xEF) != RFCOMM_UIH)
+        if((data[1] & 0xEF) != RFCOMM_UIH)
                 temp = pgm_read_byte(&rfcomm_crc_table[temp ^ data[2]]); // FCS on 3 bytes
         return (pgm_read_byte(&rfcomm_crc_table[temp ^ fcs]) == 0xCF);
 }
 
 /* Serial commands */
+#if defined(ARDUINO) && ARDUINO >=100
+
 size_t SPP::write(uint8_t data) {
-        return write(&data,1);
+        return write(&data, 1);
 }
+#else
+
+void SPP::write(uint8_t data) {
+        write(&data, 1);
+}
+#endif
+
+#if defined(ARDUINO) && ARDUINO >=100
 
 size_t SPP::write(const uint8_t *data, size_t size) {
+#else
+
+void SPP::write(const uint8_t *data, size_t size) {
+#endif
         for(uint8_t i = 0; i < size; i++) {
-                if(sppIndex >= sizeof(sppOutputBuffer)/sizeof(sppOutputBuffer[0]))
+                if(sppIndex >= sizeof (sppOutputBuffer) / sizeof (sppOutputBuffer[0]))
                         send(); // Send the current data in the buffer
                 sppOutputBuffer[sppIndex++] = data[i]; // All the bytes are put into a buffer and then send using the send() function
         }
+#if defined(ARDUINO) && ARDUINO >=100
         return size;
+#endif
 }
 
 void SPP::send() {
-        if (!connected || !sppIndex)
+        if(!connected || !sppIndex)
                 return;
         uint8_t length; // This is the length of the string we are sending
         uint8_t offset = 0; // This is used to keep track of where we are in the string
@@ -778,15 +774,15 @@ void SPP::send() {
         l2capoutbuf[0] = rfcommChannelConnection | 0 | 0 | extendAddress; // RFCOMM Address
         l2capoutbuf[1] = RFCOMM_UIH; // RFCOMM Control
 
-        while (sppIndex) { // We will run this while loop until this variable is 0
-                if (sppIndex > (sizeof (l2capoutbuf) - 4)) // Check if the string is larger than the outgoing buffer
+        while(sppIndex) { // We will run this while loop until this variable is 0
+                if(sppIndex > (sizeof (l2capoutbuf) - 4)) // Check if the string is larger than the outgoing buffer
                         length = sizeof (l2capoutbuf) - 4;
                 else
                         length = sppIndex;
 
                 l2capoutbuf[2] = length << 1 | 1; // Length
                 uint8_t i = 0;
-                for (; i < length; i++)
+                for(; i < length; i++)
                         l2capoutbuf[i + 3] = sppOutputBuffer[i + offset];
                 l2capoutbuf[i + 3] = calcFcs(l2capoutbuf); // Calculate checksum
 
@@ -806,20 +802,20 @@ void SPP::discard(void) {
 }
 
 int SPP::peek(void) {
-        if (rfcommAvailable == 0) // Don't read if there is nothing in the buffer
+        if(rfcommAvailable == 0) // Don't read if there is nothing in the buffer
                 return -1;
         return rfcommDataBuffer[0];
 }
 
 int SPP::read(void) {
-        if (rfcommAvailable == 0) // Don't read if there is nothing in the buffer
+        if(rfcommAvailable == 0) // Don't read if there is nothing in the buffer
                 return -1;
         uint8_t output = rfcommDataBuffer[0];
-        for (uint8_t i = 1; i < rfcommAvailable; i++)
+        for(uint8_t i = 1; i < rfcommAvailable; i++)
                 rfcommDataBuffer[i - 1] = rfcommDataBuffer[i]; // Shift the buffer one left
         rfcommAvailable--;
         bytesRead++;
-        if (bytesRead > (sizeof (rfcommDataBuffer) - 5)) { // We will send the command just before it runs out of credit
+        if(bytesRead > (sizeof (rfcommDataBuffer) - 5)) { // We will send the command just before it runs out of credit
                 bytesRead = 0;
                 sendRfcommCredit(rfcommChannelConnection, rfcommDirection, 0, RFCOMM_UIH, 0x10, sizeof (rfcommDataBuffer)); // Send more credit
 #ifdef EXTRADEBUG
diff --git a/SPP.h b/SPP.h
index 81b83829..d9f6761c 100644
--- a/SPP.h
+++ b/SPP.h
@@ -20,41 +20,6 @@
 
 #include "BTD.h"
 
-/* Bluetooth L2CAP states for SDP_task() */
-#define L2CAP_SDP_WAIT                  0
-#define L2CAP_SDP_REQUEST               1
-#define L2CAP_SDP_SUCCESS               2
-#define L2CAP_SDP_DONE                  3
-#define L2CAP_DISCONNECT_RESPONSE       4
-
-/* Bluetooth L2CAP states for RFCOMM_task() */
-#define L2CAP_RFCOMM_WAIT               0
-#define L2CAP_RFCOMM_REQUEST            1
-#define L2CAP_RFCOMM_SUCCESS            2
-#define L2CAP_RFCOMM_DONE               3
-
-/* L2CAP event flags */
-#define L2CAP_FLAG_CONNECTION_SDP_REQUEST       0x001
-#define L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST    0x002
-#define L2CAP_FLAG_CONFIG_SDP_REQUEST           0x004
-#define L2CAP_FLAG_CONFIG_RFCOMM_REQUEST        0x008
-#define L2CAP_FLAG_CONFIG_SDP_SUCCESS           0x010
-#define L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS        0x020
-#define L2CAP_FLAG_DISCONNECT_SDP_REQUEST       0x040
-#define L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST    0x080
-#define L2CAP_FLAG_DISCONNECT_RESPONSE          0x100
-
-/* Macros for L2CAP event flag tests */
-#define l2cap_connection_request_sdp_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_SDP_REQUEST)
-#define l2cap_connection_request_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST)
-#define l2cap_config_request_sdp_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_SDP_REQUEST)
-#define l2cap_config_request_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_RFCOMM_REQUEST)
-#define l2cap_config_success_sdp_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_SDP_SUCCESS)
-#define l2cap_config_success_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS)
-#define l2cap_disconnect_request_sdp_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_SDP_REQUEST)
-#define l2cap_disconnect_request_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST)
-#define l2cap_disconnect_response_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_RESPONSE)
-
 /* Used for SDP */
 #define SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU    0x06 // See the RFCOMM specs
 #define SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU   0x07 // See the RFCOMM specs
@@ -91,7 +56,7 @@
 
 /**
  * This BluetoothService class implements the Serial Port Protocol (SPP).
- * It inherits the Arduino Stream class. This allows it to use all the standard Arduino print functions.
+ * It inherits the Arduino Stream class. This allows it to use all the standard Arduino print and stream functions.
  */
 class SPP : public BluetoothService, public Stream {
 public:
@@ -133,6 +98,7 @@ public:
          * @return Return the number of bytes ready to be read.
          */
         virtual int available(void);
+
         /** Send out all bytes in the buffer. */
         virtual void flush(void) {
                 send();
@@ -147,6 +113,8 @@ public:
          * @return Return the byte. Will return -1 if no bytes are available.
          */
         virtual int read(void);
+
+#if defined(ARDUINO) && ARDUINO >=100
         /**
          * Writes the byte to send to a buffer. The message is send when either send() or after Usb.Task() is called.
          * @param  data The byte to write.
@@ -162,6 +130,20 @@ public:
         virtual size_t write(const uint8_t* data, size_t size);
         /** Pull in write(const char *str) from Print */
         using Print::write;
+#else
+        /**
+         * Writes the byte to send to a buffer. The message is send when either send() or after Usb.Task() is called.
+         * @param  data The byte to write.
+         */
+        virtual void write(uint8_t data);
+        /**
+         * Writes the bytes to send to a buffer. The message is send when either send() or after Usb.Task() is called.
+         * @param data The data array to send.
+         * @param size Size of the data.
+         */
+        virtual void write(const uint8_t* data, size_t size);
+#endif
+
         /** Discard all the bytes in the buffer. */
         void discard(void);
         /**
@@ -185,7 +167,7 @@ private:
         /* Variables used by L2CAP state machines */
         uint8_t l2cap_sdp_state;
         uint8_t l2cap_rfcomm_state;
-        uint16_t l2cap_event_flag; // l2cap flags of received Bluetooth events
+        uint32_t l2cap_event_flag; // l2cap flags of received Bluetooth events
 
         uint8_t l2capoutbuf[BULK_MAXPKTSIZE]; // General purpose buffer for l2cap out data
         uint8_t rfcommbuf[10]; // Buffer for RFCOMM Commands
@@ -237,4 +219,4 @@ private:
         bool checkFcs(uint8_t *data, uint8_t fcs);
         uint8_t crc(uint8_t *data);
 };
-#endif
\ No newline at end of file
+#endif
diff --git a/Usb.cpp b/Usb.cpp
index e47383c6..ca81cf99 100644
--- a/Usb.cpp
+++ b/Usb.cpp
@@ -44,13 +44,13 @@ void USB::setUsbTaskState(uint8_t state) {
 EpInfo* USB::getEpInfoEntry(uint8_t addr, uint8_t ep) {
         UsbDevice *p = addrPool.GetUsbDevicePtr(addr);
 
-        if (!p || !p->epinfo)
+        if(!p || !p->epinfo)
                 return NULL;
 
         EpInfo *pep = p->epinfo;
 
-        for (uint8_t i = 0; i < p->epcount; i++) {
-                if ((pep)->epAddr == ep)
+        for(uint8_t i = 0; i < p->epcount; i++) {
+                if((pep)->epAddr == ep)
                         return pep;
 
                 pep++;
@@ -62,15 +62,15 @@ EpInfo* USB::getEpInfoEntry(uint8_t addr, uint8_t ep) {
 
 /* each device is different and has different number of endpoints. This function plugs endpoint record structure, defined in application, to devtable */
 uint8_t USB::setEpInfoEntry(uint8_t addr, uint8_t epcount, EpInfo* eprecord_ptr) {
-        if (!eprecord_ptr)
+        if(!eprecord_ptr)
                 return USB_ERROR_INVALID_ARGUMENT;
 
         UsbDevice *p = addrPool.GetUsbDevicePtr(addr);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        p->address = addr;
+        p->address.devAddress = addr;
         p->epinfo = eprecord_ptr;
         p->epcount = epcount;
 
@@ -80,15 +80,15 @@ uint8_t USB::setEpInfoEntry(uint8_t addr, uint8_t epcount, EpInfo* eprecord_ptr)
 uint8_t USB::SetAddress(uint8_t addr, uint8_t ep, EpInfo **ppep, uint16_t &nak_limit) {
         UsbDevice *p = addrPool.GetUsbDevicePtr(addr);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        if (!p->epinfo)
+        if(!p->epinfo)
                 return USB_ERROR_EPINFO_IS_NULL;
 
         *ppep = getEpInfoEntry(addr, ep);
 
-        if (!*ppep)
+        if(!*ppep)
                 return USB_ERROR_EP_NOT_FOUND_IN_TBL;
 
         nak_limit = (0x0001UL << (((*ppep)->bmNakPower > USB_NAK_MAX_POWER) ? USB_NAK_MAX_POWER : (*ppep)->bmNakPower));
@@ -134,7 +134,7 @@ uint8_t USB::ctrlReq(uint8_t addr, uint8_t ep, uint8_t bmReqType, uint8_t bReque
 
         rcode = SetAddress(addr, ep, &pep, nak_limit);
 
-        if (rcode)
+        if(rcode)
                 return rcode;
 
         direction = ((bmReqType & 0x80) > 0);
@@ -151,39 +151,39 @@ uint8_t USB::ctrlReq(uint8_t addr, uint8_t ep, uint8_t bmReqType, uint8_t bReque
 
         rcode = dispatchPkt(tokSETUP, ep, nak_limit); //dispatch packet
 
-        if (rcode) //return HRSLT if not zero
+        if(rcode) //return HRSLT if not zero
                 return ( rcode);
 
-        if (dataptr != NULL) //data stage, if present
+        if(dataptr != NULL) //data stage, if present
         {
-                if (direction) //IN transfer
+                if(direction) //IN transfer
                 {
                         uint16_t left = total;
 
                         pep->bmRcvToggle = 1; //bmRCVTOG1;
 
-                        while (left) {
+                        while(left) {
                                 // Bytes read into buffer
                                 uint16_t read = nbytes;
                                 //uint16_t read = (left<nbytes) ? left : nbytes;
 
                                 rcode = InTransfer(pep, nak_limit, &read, dataptr);
-                                if (rcode == hrTOGERR) {
+                                if(rcode == hrTOGERR) {
                                         // yes, we flip it wrong here so that next time it is actually correct!
                                         pep->bmRcvToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 0 : 1;
                                         continue;
                                 }
 
-                                if (rcode)
+                                if(rcode)
                                         return rcode;
 
                                 // Invoke callback function if inTransfer completed successfully and callback function pointer is specified
-                                if (!rcode && p)
+                                if(!rcode && p)
                                         ((USBReadParser*)p)->Parse(read, dataptr, total - left);
 
                                 left -= read;
 
-                                if (read < nbytes)
+                                if(read < nbytes)
                                         break;
                         }
                 } else //OUT transfer
@@ -191,7 +191,7 @@ uint8_t USB::ctrlReq(uint8_t addr, uint8_t ep, uint8_t bmReqType, uint8_t bReque
                         pep->bmSndToggle = 1; //bmSNDTOG1;
                         rcode = OutTransfer(pep, nak_limit, nbytes, dataptr);
                 }
-                if (rcode) //return error
+                if(rcode) //return error
                         return ( rcode);
         }
         // Status stage
@@ -209,8 +209,10 @@ uint8_t USB::inTransfer(uint8_t addr, uint8_t ep, uint16_t *nbytesptr, uint8_t*
 
         uint8_t rcode = SetAddress(addr, ep, &pep, nak_limit);
 
-        if (rcode) {
-                //printf("SetAddress Failed");
+        if(rcode) {
+                USBTRACE3("(USB::InTransfer) SetAddress Failed ", rcode, 0x81);
+                USBTRACE3("(USB::InTransfer) addr requested ", addr, 0x81);
+                USBTRACE3("(USB::InTransfer) ep requested ", ep, 0x81);
                 return rcode;
         }
         return InTransfer(pep, nak_limit, nbytesptr, data);
@@ -227,22 +229,22 @@ uint8_t USB::InTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t *nbytesptr, ui
         *nbytesptr = 0;
         regWr(rHCTL, (pep->bmRcvToggle) ? bmRCVTOG1 : bmRCVTOG0); //set toggle value
 
-        while (1) // use a 'return' to exit this loop
-        {
+        // use a 'break' to exit this loop
+        while(1) {
                 rcode = dispatchPkt(tokIN, pep->epAddr, nak_limit); //IN packet to EP-'endpoint'. Function takes care of NAKS.
-                if (rcode == hrTOGERR) {
+                if(rcode == hrTOGERR) {
                         // yes, we flip it wrong here so that next time it is actually correct!
                         pep->bmRcvToggle = (regRd(rHRSL) & bmSNDTOGRD) ? 0 : 1;
                         regWr(rHCTL, (pep->bmRcvToggle) ? bmRCVTOG1 : bmRCVTOG0); //set toggle value
                         continue;
                 }
-                if (rcode) {
+                if(rcode) {
                         //printf(">>>>>>>> Problem! dispatchPkt %2.2x\r\n", rcode);
                         break; //should be 0, indicating ACK. Else return error code.
                 }
                 /* check for RCVDAVIRQ and generate error if not present */
                 /* the only case when absence of RCVDAVIRQ makes sense is when toggle error occurred. Need to add handling for that */
-                if ((regRd(rHIRQ) & bmRCVDAVIRQ) == 0) {
+                if((regRd(rHIRQ) & bmRCVDAVIRQ) == 0) {
                         //printf(">>>>>>>> Problem! NO RCVDAVIRQ!\r\n");
                         rcode = 0xf0; //receive error
                         break;
@@ -251,7 +253,7 @@ uint8_t USB::InTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t *nbytesptr, ui
                 //printf("Got %i bytes \r\n", pktsize);
                 // This would be OK, but...
                 //assert(pktsize <= nbytes);
-                if (pktsize > nbytes) {
+                if(pktsize > nbytes) {
                         // This can happen. Use of assert on Arduino locks up the Arduino.
                         // So I will trim the value, and hope for the best.
                         //printf(">>>>>>>> Problem! Wanted %i bytes but got %i.\r\n", nbytes, pktsize);
@@ -260,7 +262,7 @@ uint8_t USB::InTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t *nbytesptr, ui
 
                 int16_t mem_left = (int16_t)nbytes - *((int16_t*)nbytesptr);
 
-                if (mem_left < 0)
+                if(mem_left < 0)
                         mem_left = 0;
 
                 data = bytesRd(rRCVFIFO, ((pktsize > mem_left) ? mem_left : pktsize), data);
@@ -271,7 +273,7 @@ uint8_t USB::InTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t *nbytesptr, ui
                 /* The transfer is complete under two conditions:           */
                 /* 1. The device sent a short packet (L.T. maxPacketSize)   */
                 /* 2. 'nbytes' have been transferred.                       */
-                if ((pktsize < maxpktsize) || (*nbytesptr >= nbytes)) // have we transferred 'nbytes' bytes?
+                if((pktsize < maxpktsize) || (*nbytesptr >= nbytes)) // have we transferred 'nbytes' bytes?
                 {
                         // Save toggle value
                         pep->bmRcvToggle = ((regRd(rHRSL) & bmRCVTOGRD)) ? 1 : 0;
@@ -293,7 +295,7 @@ uint8_t USB::outTransfer(uint8_t addr, uint8_t ep, uint16_t nbytes, uint8_t* dat
 
         uint8_t rcode = SetAddress(addr, ep, &pep, nak_limit);
 
-        if (rcode)
+        if(rcode)
                 return rcode;
 
         return OutTransfer(pep, nak_limit, nbytes, data);
@@ -307,35 +309,35 @@ uint8_t USB::OutTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t nbytes, uint8
 
         uint8_t maxpktsize = pep->maxPktSize;
 
-        if (maxpktsize < 1 || maxpktsize > 64)
+        if(maxpktsize < 1 || maxpktsize > 64)
                 return USB_ERROR_INVALID_MAX_PKT_SIZE;
 
         unsigned long timeout = millis() + USB_XFER_TIMEOUT;
 
         regWr(rHCTL, (pep->bmSndToggle) ? bmSNDTOG1 : bmSNDTOG0); //set toggle value
 
-        while (bytes_left) {
+        while(bytes_left) {
                 retry_count = 0;
                 nak_count = 0;
                 bytes_tosend = (bytes_left >= maxpktsize) ? maxpktsize : bytes_left;
                 bytesWr(rSNDFIFO, bytes_tosend, data_p); //filling output FIFO
                 regWr(rSNDBC, bytes_tosend); //set number of bytes
                 regWr(rHXFR, (tokOUT | pep->epAddr)); //dispatch packet
-                while (!(regRd(rHIRQ) & bmHXFRDNIRQ)); //wait for the completion IRQ
+                while(!(regRd(rHIRQ) & bmHXFRDNIRQ)); //wait for the completion IRQ
                 regWr(rHIRQ, bmHXFRDNIRQ); //clear IRQ
                 rcode = (regRd(rHRSL) & 0x0f);
 
-                while (rcode && (timeout > millis())) {
-                        switch (rcode) {
+                while(rcode && ((long)(millis() - timeout) < 0L)) {
+                        switch(rcode) {
                                 case hrNAK:
                                         nak_count++;
-                                        if (nak_limit && (nak_count == nak_limit))
+                                        if(nak_limit && (nak_count == nak_limit))
                                                 goto breakout;
                                         //return ( rcode);
                                         break;
                                 case hrTIMEOUT:
                                         retry_count++;
-                                        if (retry_count == USB_RETRY_LIMIT)
+                                        if(retry_count == USB_RETRY_LIMIT)
                                                 goto breakout;
                                         //return ( rcode);
                                         break;
@@ -353,7 +355,7 @@ uint8_t USB::OutTransfer(EpInfo *pep, uint16_t nak_limit, uint16_t nbytes, uint8
                         regWr(rSNDFIFO, *data_p);
                         regWr(rSNDBC, bytes_tosend);
                         regWr(rHXFR, (tokOUT | pep->epAddr)); //dispatch packet
-                        while (!(regRd(rHIRQ) & bmHXFRDNIRQ)); //wait for the completion IRQ
+                        while(!(regRd(rHIRQ) & bmHXFRDNIRQ)); //wait for the completion IRQ
                         regWr(rHIRQ, bmHXFRDNIRQ); //clear IRQ
                         rcode = (regRd(rHRSL) & 0x0f);
                 }//while( rcode && ....
@@ -378,15 +380,15 @@ uint8_t USB::dispatchPkt(uint8_t token, uint8_t ep, uint16_t nak_limit) {
         uint8_t retry_count = 0;
         uint16_t nak_count = 0;
 
-        while (timeout > millis()) {
+        while((long)(millis() - timeout) < 0L) {
                 regWr(rHXFR, (token | ep)); //launch the transfer
                 rcode = USB_ERROR_TRANSFER_TIMEOUT;
 
-                while (timeout > millis()) //wait for transfer completion
+                while((long)(millis() - timeout) < 0L) //wait for transfer completion
                 {
                         tmpdata = regRd(rHIRQ);
 
-                        if (tmpdata & bmHXFRDNIRQ) {
+                        if(tmpdata & bmHXFRDNIRQ) {
                                 regWr(rHIRQ, bmHXFRDNIRQ); //clear the interrupt
                                 rcode = 0x00;
                                 break;
@@ -399,15 +401,15 @@ uint8_t USB::dispatchPkt(uint8_t token, uint8_t ep, uint16_t nak_limit) {
 
                 rcode = (regRd(rHRSL) & 0x0f); //analyze transfer result
 
-                switch (rcode) {
+                switch(rcode) {
                         case hrNAK:
                                 nak_count++;
-                                if (nak_limit && (nak_count == nak_limit))
+                                if(nak_limit && (nak_count == nak_limit))
                                         return (rcode);
                                 break;
                         case hrTIMEOUT:
                                 retry_count++;
-                                if (retry_count == USB_RETRY_LIMIT)
+                                if(retry_count == USB_RETRY_LIMIT)
                                         return (rcode);
                                 break;
                         default:
@@ -432,38 +434,38 @@ void USB::Task(void) //USB state machine
         tmpdata = getVbusState();
 
         /* modify USB task state if Vbus changed */
-        switch (tmpdata) {
+        switch(tmpdata) {
                 case SE1: //illegal state
                         usb_task_state = USB_DETACHED_SUBSTATE_ILLEGAL;
                         lowspeed = false;
                         break;
                 case SE0: //disconnected
-                        if ((usb_task_state & USB_STATE_MASK) != USB_STATE_DETACHED)
+                        if((usb_task_state & USB_STATE_MASK) != USB_STATE_DETACHED)
                                 usb_task_state = USB_DETACHED_SUBSTATE_INITIALIZE;
                         lowspeed = false;
                         break;
                 case LSHOST:
 
                         lowspeed = true;
-                				//intentional fallthrough
+                        //intentional fallthrough
                 case FSHOST: //attached
-                        if ((usb_task_state & USB_STATE_MASK) == USB_STATE_DETACHED) {
+                        if((usb_task_state & USB_STATE_MASK) == USB_STATE_DETACHED) {
                                 delay = millis() + USB_SETTLE_DELAY;
                                 usb_task_state = USB_ATTACHED_SUBSTATE_SETTLE;
                         }
                         break;
         }// switch( tmpdata
 
-        for (uint8_t i = 0; i < USB_NUMDEVICES; i++)
-                if (devConfig[i])
+        for(uint8_t i = 0; i < USB_NUMDEVICES; i++)
+                if(devConfig[i])
                         rcode = devConfig[i]->Poll();
 
-        switch (usb_task_state) {
+        switch(usb_task_state) {
                 case USB_DETACHED_SUBSTATE_INITIALIZE:
                         init();
 
-                        for (uint8_t i = 0; i < USB_NUMDEVICES; i++)
-                                if (devConfig[i])
+                        for(uint8_t i = 0; i < USB_NUMDEVICES; i++)
+                                if(devConfig[i])
                                         rcode = devConfig[i]->Release();
 
                         usb_task_state = USB_DETACHED_SUBSTATE_WAIT_FOR_DEVICE;
@@ -473,7 +475,7 @@ void USB::Task(void) //USB state machine
                 case USB_DETACHED_SUBSTATE_ILLEGAL: //just sit here
                         break;
                 case USB_ATTACHED_SUBSTATE_SETTLE: //settle time for just attached device
-                        if (delay < millis())
+                        if((long)(millis() - delay) >= 0L)
                                 usb_task_state = USB_ATTACHED_SUBSTATE_RESET_DEVICE;
                         else break; // don't fall through
                 case USB_ATTACHED_SUBSTATE_RESET_DEVICE:
@@ -481,7 +483,7 @@ void USB::Task(void) //USB state machine
                         usb_task_state = USB_ATTACHED_SUBSTATE_WAIT_RESET_COMPLETE;
                         break;
                 case USB_ATTACHED_SUBSTATE_WAIT_RESET_COMPLETE:
-                        if ((regRd(rHCTL) & bmBUSRST) == 0) {
+                        if((regRd(rHCTL) & bmBUSRST) == 0) {
                                 tmpdata = regRd(rMODE) | bmSOFKAENAB; //start SOF generation
                                 regWr(rMODE, tmpdata);
                                 usb_task_state = USB_ATTACHED_SUBSTATE_WAIT_SOF;
@@ -489,7 +491,7 @@ void USB::Task(void) //USB state machine
                         }
                         break;
                 case USB_ATTACHED_SUBSTATE_WAIT_SOF: //todo: change check order
-                        if (regRd(rHIRQ) & bmFRAMEIRQ) {
+                        if(regRd(rHIRQ) & bmFRAMEIRQ) {
                                 //when first SOF received _and_ 20ms has passed we can continue
                                 /*
                                 if (delay < millis()) //20ms passed
@@ -500,17 +502,17 @@ void USB::Task(void) //USB state machine
                         }
                         break;
                 case USB_ATTACHED_SUBSTATE_WAIT_RESET:
-                        if (delay < millis()) usb_task_state = USB_STATE_CONFIGURING;
-                        else break;  // don't fall through
+                        if((long)(millis() - delay) >= 0L) usb_task_state = USB_STATE_CONFIGURING;
+                        else break; // don't fall through
                 case USB_STATE_CONFIGURING:
 
-                				//Serial.print("\r\nConf.LS: ");
-                				//Serial.println(lowspeed, HEX);
+                        //Serial.print("\r\nConf.LS: ");
+                        //Serial.println(lowspeed, HEX);
 
                         rcode = Configuring(0, 0, lowspeed);
 
-                        if (rcode) {
-                                if (rcode != USB_DEV_CONFIG_ERROR_DEVICE_INIT_INCOMPLETE) {
+                        if(rcode) {
+                                if(rcode != USB_DEV_CONFIG_ERROR_DEVICE_INIT_INCOMPLETE) {
                                         usb_error = rcode;
                                         usb_task_state = USB_STATE_ERROR;
                                 }
@@ -533,10 +535,10 @@ uint8_t USB::DefaultAddressing(uint8_t parent, uint8_t port, bool lowspeed) {
         // Get pointer to pseudo device with address 0 assigned
         p0 = addrPool.GetUsbDevicePtr(0);
 
-        if (!p0)
+        if(!p0)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        if (!p0->epinfo)
+        if(!p0->epinfo)
                 return USB_ERROR_EPINFO_IS_NULL;
 
         p0->lowspeed = (lowspeed) ? true : false;
@@ -544,12 +546,12 @@ uint8_t USB::DefaultAddressing(uint8_t parent, uint8_t port, bool lowspeed) {
         // Allocate new address according to device class
         uint8_t bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
@@ -557,7 +559,7 @@ uint8_t USB::DefaultAddressing(uint8_t parent, uint8_t port, bool lowspeed) {
         // Assign new address to the device
         rcode = setAddr(0, 0, bAddress);
 
-        if (rcode) {
+        if(rcode) {
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
                 return rcode;
@@ -571,8 +573,8 @@ uint8_t USB::AttemptConfig(uint8_t driver, uint8_t parent, uint8_t port, bool lo
 
 again:
         uint8_t rcode = devConfig[driver]->ConfigureDevice(parent, port, lowspeed);
-        if (rcode == USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET) {
-                if (parent == 0) {
+        if(rcode == USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET) {
+                if(parent == 0) {
                         // Send a bus reset on the root interface.
                         regWr(rHCTL, bmBUSRST); //issue bus reset
                         delay(102); // delay 102ms, compensate for clock inaccuracy.
@@ -580,22 +582,22 @@ again:
                         // reset parent port
                         devConfig[parent]->ResetHubPort(port);
                 }
-        } else if (rcode == hrJERR && retries < 3) { // Some devices returns this when plugged in - trying to initialize the device again usually works
+        } else if(rcode == hrJERR && retries < 3) { // Some devices returns this when plugged in - trying to initialize the device again usually works
                 delay(100);
                 retries++;
                 goto again;
-        } else if (rcode)
+        } else if(rcode)
                 return rcode;
 
         rcode = devConfig[driver]->Init(parent, port, lowspeed);
-        if (rcode == hrJERR && retries < 3) { // Some devices returns this when plugged in - trying to initialize the device again usually works
+        if(rcode == hrJERR && retries < 3) { // Some devices returns this when plugged in - trying to initialize the device again usually works
                 delay(100);
                 retries++;
                 goto again;
         }
-        if (rcode) {
+        if(rcode) {
                 // Issue a bus reset, because the device may be in a limbo state
-                if (parent == 0) {
+                if(parent == 0) {
                         // Send a bus reset on the root interface.
                         regWr(rHCTL, bmBUSRST); //issue bus reset
                         delay(102); // delay 102ms, compensate for clock inaccuracy.
@@ -653,6 +655,7 @@ uint8_t USB::Configuring(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t devConfigIndex;
         uint8_t rcode = 0;
         uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)];
+        USB_DEVICE_DESCRIPTOR *udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR *>(buf);
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
         EpInfo epInfo;
@@ -666,7 +669,7 @@ uint8_t USB::Configuring(uint8_t parent, uint8_t port, bool lowspeed) {
         AddressPool &addrPool = GetAddressPool();
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
-        if (!p) {
+        if(!p) {
                 //printf("Configuring error: USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL\r\n");
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
@@ -686,7 +689,7 @@ uint8_t USB::Configuring(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode) {
+        if(rcode) {
                 //printf("Configuring error: Can't get USB_DEVICE_DESCRIPTOR\r\n");
                 return rcode;
         }
@@ -697,36 +700,35 @@ uint8_t USB::Configuring(uint8_t parent, uint8_t port, bool lowspeed) {
 
         //if (!bAddress)
         //        return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
-
-        uint16_t vid = (uint16_t)((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
-        uint16_t pid = (uint16_t)((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
-        uint8_t klass = ((USB_DEVICE_DESCRIPTOR*)buf)->bDeviceClass;
+        uint16_t vid = udd->idVendor;
+        uint16_t pid = udd->idProduct;
+        uint8_t klass = udd->bDeviceClass;
 
         // Attempt to configure if VID/PID or device class matches with a driver
-        for (devConfigIndex = 0; devConfigIndex < USB_NUMDEVICES; devConfigIndex++) {
-                if (!devConfig[devConfigIndex]) continue; // no driver
-                if (devConfig[devConfigIndex]->GetAddress()) continue; // consumed
-                if (devConfig[devConfigIndex]->VIDPIDOK(vid, pid) || devConfig[devConfigIndex]->DEVCLASSOK(klass)) {
+        for(devConfigIndex = 0; devConfigIndex < USB_NUMDEVICES; devConfigIndex++) {
+                if(!devConfig[devConfigIndex]) continue; // no driver
+                if(devConfig[devConfigIndex]->GetAddress()) continue; // consumed
+                if(devConfig[devConfigIndex]->VIDPIDOK(vid, pid) || devConfig[devConfigIndex]->DEVCLASSOK(klass)) {
                         rcode = AttemptConfig(devConfigIndex, parent, port, lowspeed);
-                        if (rcode != USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED)
+                        if(rcode != USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED)
                                 break;
                 }
         }
 
-        if (devConfigIndex < USB_NUMDEVICES) {
+        if(devConfigIndex < USB_NUMDEVICES) {
                 return rcode;
         }
 
 
         // blindly attempt to configure
-        for (devConfigIndex = 0; devConfigIndex < USB_NUMDEVICES; devConfigIndex++) {
-                if (!devConfig[devConfigIndex]) continue;
-                if (devConfig[devConfigIndex]->GetAddress()) continue; // consumed
-                if (devConfig[devConfigIndex]->VIDPIDOK(vid, pid) || devConfig[devConfigIndex]->DEVCLASSOK(klass)) continue; // If this is true it means it must have returned USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED above
+        for(devConfigIndex = 0; devConfigIndex < USB_NUMDEVICES; devConfigIndex++) {
+                if(!devConfig[devConfigIndex]) continue;
+                if(devConfig[devConfigIndex]->GetAddress()) continue; // consumed
+                if(devConfig[devConfigIndex]->VIDPIDOK(vid, pid) || devConfig[devConfigIndex]->DEVCLASSOK(klass)) continue; // If this is true it means it must have returned USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED above
                 rcode = AttemptConfig(devConfigIndex, parent, port, lowspeed);
 
                 //printf("ERROR ENUMERATING %2.2x\r\n", rcode);
-                if (!(rcode == USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED || rcode == USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE)) {
+                if(!(rcode == USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED || rcode == USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE)) {
                         // in case of an error dev_index should be reset to 0
                         //		in order to start from the very beginning the
                         //		next time the program gets here
@@ -742,12 +744,12 @@ uint8_t USB::Configuring(uint8_t parent, uint8_t port, bool lowspeed) {
 }
 
 uint8_t USB::ReleaseDevice(uint8_t addr) {
-        if (!addr)
+        if(!addr)
                 return 0;
 
-        for (uint8_t i = 0; i < USB_NUMDEVICES; i++) {
+        for(uint8_t i = 0; i < USB_NUMDEVICES; i++) {
                 if(!devConfig[i]) continue;
-                if (devConfig[i]->GetAddress() == addr)
+                if(devConfig[i]->GetAddress() == addr)
                         return devConfig[i]->Release();
         }
         return 0;
@@ -770,13 +772,14 @@ uint8_t USB::getConfDescr(uint8_t addr, uint8_t ep, uint16_t nbytes, uint8_t con
 uint8_t USB::getConfDescr(uint8_t addr, uint8_t ep, uint8_t conf, USBReadParser *p) {
         const uint8_t bufSize = 64;
         uint8_t buf[bufSize];
+        USB_CONFIGURATION_DESCRIPTOR *ucd = reinterpret_cast<USB_CONFIGURATION_DESCRIPTOR *>(buf);
 
         uint8_t ret = getConfDescr(addr, ep, 9, conf, buf);
 
-        if (ret)
+        if(ret)
                 return ret;
 
-        uint16_t total = ((USB_CONFIGURATION_DESCRIPTOR*)buf)->wTotalLength;
+        uint16_t total = ucd->wTotalLength;
 
         //USBTRACE2("\r\ntotal conf.size:", total);
 
@@ -791,7 +794,11 @@ uint8_t USB::getStrDescr(uint8_t addr, uint8_t ep, uint16_t ns, uint8_t index, u
 //set address
 
 uint8_t USB::setAddr(uint8_t oldaddr, uint8_t ep, uint8_t newaddr) {
-        return ( ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, 0x0000, NULL, NULL));
+        uint8_t rcode = ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, 0x0000, NULL, NULL);
+        //delay(2); //per USB 2.0 sect.9.2.6.3
+        delay(300); // Older spec says you should wait at least 200ms
+        return rcode;
+        //return ( ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, 0x0000, NULL, NULL));
 }
 //set configuration
 
@@ -800,4 +807,3 @@ uint8_t USB::setConf(uint8_t addr, uint8_t ep, uint8_t conf_value) {
 }
 
 #endif // defined(USB_METHODS_INLINE)
-
diff --git a/Usb.h b/Usb.h
index c9dccb53..d903ff51 100644
--- a/Usb.h
+++ b/Usb.h
@@ -28,6 +28,7 @@ e-mail   :  support@circuitsathome.com
 #include "printhex.h"
 #include "message.h"
 #include "hexdump.h"
+#include "sink_parser.h"
 #include "max3421e.h"
 #include "address.h"
 #include "avrpins.h"
diff --git a/UsbCore.h b/UsbCore.h
index bbcbfa0e..76d9d5ee 100644
--- a/UsbCore.h
+++ b/UsbCore.h
@@ -1,8 +1,18 @@
-/*
- * File:   UsbCore.h
- * Author: xxxajk
- *
- * Created on September 29, 2013, 9:25 PM
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
  */
 
 #if !defined(_usb_h_) || defined(USBCORE_H)
@@ -18,13 +28,17 @@
 #ifdef BOARD_BLACK_WIDDOW
 typedef MAX3421e<P6, P3> MAX3421E; // Black Widow
 #elif defined(CORE_TEENSY) && (defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__))
+#if EXT_RAM
+typedef MAX3421e<P20, P7> MAX3421E; // Teensy++ 2.0 with XMEM2
+#else
 typedef MAX3421e<P9, P8> MAX3421E; // Teensy++ 1.0 and 2.0
+#endif
 #elif defined(BOARD_MEGA_ADK)
 typedef MAX3421e<P53, P54> MAX3421E; // Arduino Mega ADK
 #elif defined(ARDUINO_AVR_BALANDUINO)
 typedef MAX3421e<P20, P19> MAX3421E; // Balanduino
 #else
-typedef MAX3421e<P10, P9> MAX3421E; // Official Arduinos (UNO, Duemilanove, Mega, 2560, Leonardo etc.) or Teensy 2.0 and 3.0
+typedef MAX3421e<P10, P9> MAX3421E; // Official Arduinos (UNO, Duemilanove, Mega, 2560, Leonardo, Due etc.) or Teensy 2.0 and 3.0
 #endif
 
 /* Common setup data constant combinations  */
@@ -104,14 +118,38 @@ typedef MAX3421e<P10, P9> MAX3421E; // Official Arduinos (UNO, Duemilanove, Mega
 
 class USBDeviceConfig {
 public:
-        virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed) { return 0; }
-        virtual uint8_t ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {return 0; }
-        virtual uint8_t Release() { return 0; }
-        virtual uint8_t Poll() { return 0; }
-        virtual uint8_t GetAddress() { return 0; }
-        virtual void ResetHubPort(uint8_t port) { return; } // Note used for hubs only!
-        virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) { return false; }
-        virtual boolean DEVCLASSOK(uint8_t klass) { return false; }
+
+        virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed) {
+                return 0;
+        }
+
+        virtual uint8_t ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
+                return 0;
+        }
+
+        virtual uint8_t Release() {
+                return 0;
+        }
+
+        virtual uint8_t Poll() {
+                return 0;
+        }
+
+        virtual uint8_t GetAddress() {
+                return 0;
+        }
+
+        virtual void ResetHubPort(uint8_t port) {
+                return;
+        } // Note used for hubs only!
+
+        virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) {
+                return false;
+        }
+
+        virtual boolean DEVCLASSOK(uint8_t klass) {
+                return false;
+        }
 };
 
 /* USB Setup Packet Structure   */
@@ -138,7 +176,7 @@ typedef struct {
         } wVal_u;
         uint16_t wIndex; //   4      Depends on bRequest
         uint16_t wLength; //   6      Depends on bRequest
-}__attribute__((packed)) SETUP_PKT, *PSETUP_PKT;
+} __attribute__((packed)) SETUP_PKT, *PSETUP_PKT;
 
 
 
@@ -166,7 +204,7 @@ public:
         };
 
         AddressPool& GetAddressPool() {
-                return(AddressPool&) addrPool;
+                return (AddressPool&)addrPool;
         };
 
         uint8_t RegisterDeviceClass(USBDeviceConfig *pdev) {
@@ -225,30 +263,29 @@ private:
 //get device descriptor
 
 inline uint8_t USB::getDevDescr(uint8_t addr, uint8_t ep, uint16_t nbytes, uint8_t* dataptr) {
-        return( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, 0x00, USB_DESCRIPTOR_DEVICE, 0x0000, nbytes, dataptr));
+        return ( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, 0x00, USB_DESCRIPTOR_DEVICE, 0x0000, nbytes, dataptr));
 }
 //get configuration descriptor
 
 inline uint8_t USB::getConfDescr(uint8_t addr, uint8_t ep, uint16_t nbytes, uint8_t conf, uint8_t* dataptr) {
-        return( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, conf, USB_DESCRIPTOR_CONFIGURATION, 0x0000, nbytes, dataptr));
+        return ( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, conf, USB_DESCRIPTOR_CONFIGURATION, 0x0000, nbytes, dataptr));
 }
 //get string descriptor
 
 inline uint8_t USB::getStrDescr(uint8_t addr, uint8_t ep, uint16_t nuint8_ts, uint8_t index, uint16_t langid, uint8_t* dataptr) {
-        return( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, index, USB_DESCRIPTOR_STRING, langid, nuint8_ts, dataptr));
+        return ( ctrlReq(addr, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, index, USB_DESCRIPTOR_STRING, langid, nuint8_ts, dataptr));
 }
 //set address
 
 inline uint8_t USB::setAddr(uint8_t oldaddr, uint8_t ep, uint8_t newaddr) {
-        return( ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, NULL));
+        return ( ctrlReq(oldaddr, ep, bmREQ_SET, USB_REQUEST_SET_ADDRESS, newaddr, 0x00, 0x0000, 0x0000, NULL));
 }
 //set configuration
 
 inline uint8_t USB::setConf(uint8_t addr, uint8_t ep, uint8_t conf_value) {
-        return( ctrlReq(addr, ep, bmREQ_SET, USB_REQUEST_SET_CONFIGURATION, conf_value, 0x00, 0x0000, 0x0000, NULL));
+        return ( ctrlReq(addr, ep, bmREQ_SET, USB_REQUEST_SET_CONFIGURATION, conf_value, 0x00, 0x0000, 0x0000, NULL));
 }
 
 #endif // defined(USB_METHODS_INLINE)
 
 #endif	/* USBCORE_H */
-
diff --git a/Wii.cpp b/Wii.cpp
index d55d4e79..8106666b 100755
--- a/Wii.cpp
+++ b/Wii.cpp
@@ -22,7 +22,8 @@
 //#define EXTRADEBUG // Uncomment to get even more debugging data
 //#define PRINTREPORT // Uncomment to print the report send by the Wii controllers
 
-const uint8_t LEDS[] PROGMEM = {
+const uint8_t WII_LEDS[] PROGMEM = {
+        0x00, // OFF
         0x10, // LED1
         0x20, // LED2
         0x40, // LED3
@@ -33,10 +34,10 @@ const uint8_t LEDS[] PROGMEM = {
         0xC0, // LED7
         0xD0, // LED8
         0xE0, // LED9
-        0xF0 // LED10
+        0xF0, // LED10
 };
 
-const uint32_t BUTTONS[] PROGMEM = {
+const uint32_t WII_BUTTONS[] PROGMEM = {
         0x00008, // UP
         0x00002, // RIGHT
         0x00004, // DOWN
@@ -53,9 +54,9 @@ const uint32_t BUTTONS[] PROGMEM = {
         0x20000, // C
 
         0x00400, // B
-        0x00800 // A
+        0x00800, // A
 };
-const uint32_t PROCONTROLLERBUTTONS[] PROGMEM = {
+const uint32_t WII_PROCONTROLLER_BUTTONS[] PROGMEM = {
         0x00100, // UP
         0x00080, // RIGHT
         0x00040, // DOWN
@@ -78,13 +79,13 @@ const uint32_t PROCONTROLLERBUTTONS[] PROGMEM = {
         0x00020, // L
         0x00002, // R
         0x08000, // ZL
-        0x00400 // ZR
+        0x00400, // ZR
 };
 
 WII::WII(BTD *p, bool pair) :
 pBtd(p) // pointer to USB class instance - mandatory
 {
-        if (pBtd)
+        if(pBtd)
                 pBtd->registerServiceClass(this); // Register it as a Bluetooth service
 
         pBtd->pairWithWii = pair;
@@ -115,8 +116,8 @@ void WII::Reset() {
 }
 
 void WII::disconnect() { // Use this void to disconnect any of the controllers
-        if (!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
-                if (motionPlusConnected) {
+        if(!motionPlusInside) { // The old Wiimote needs a delay after the first command or it will automatically reconnect
+                if(motionPlusConnected) {
 #ifdef DEBUG_USB_HOST
                         Notify(PSTR("\r\nDeactivating Motion Plus"), 0x80);
 #endif
@@ -126,15 +127,15 @@ void WII::disconnect() { // Use this void to disconnect any of the controllers
         } else
                 timer = millis(); // Don't wait
         // First the HID interrupt channel has to be disconnected, then the HID control channel and finally the HCI connection
-        pBtd->l2cap_disconnection_request(hci_handle, 0x0A, interrupt_scid, interrupt_dcid);
+        pBtd->l2cap_disconnection_request(hci_handle, ++identifier, interrupt_scid, interrupt_dcid);
         Reset();
         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
 }
 
 void WII::ACLData(uint8_t* l2capinbuf) {
-        if (!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
-                if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
-                        if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+        if(!pBtd->l2capConnectionClaimed && pBtd->incomingWii && !wiimoteConnected && !activeConnection) {
+                if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
                                 motionPlusInside = pBtd->motionPlusInside;
                                 pBtd->incomingWii = false;
                                 pBtd->l2capConnectionClaimed = true; // Claim that the incoming connection belongs to this service
@@ -144,9 +145,10 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
         }
-        if ((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000)) { // acl_handle_ok or it's a new connection
-                if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U
-                        if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
+        //if((l2capinbuf[0] | (uint16_t)l2capinbuf[1] << 8) == (hci_handle | 0x2000U)) { // acl_handle_ok or it's a new connection
+        if(UHS_ACL_HANDLE_OK(l2capinbuf, hci_handle)) { // acl_handle_ok or it's a new connection
+                if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { //l2cap_control - Channel ID for ACL-U
+                        if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nL2CAP Command Rejected - Reason: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -161,23 +163,23 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR(" "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
 #endif
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
-                                if (((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
-                                        if (l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_RESPONSE) {
+                                if(((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) && ((l2capinbuf[18] | (l2capinbuf[19] << 8)) == SUCCESSFUL)) { // Success
+                                        if(l2capinbuf[14] == control_dcid[0] && l2capinbuf[15] == control_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Control Connection Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
                                                 control_scid[0] = l2capinbuf[12];
                                                 control_scid[1] = l2capinbuf[13];
-                                                l2cap_event_flag |= L2CAP_FLAG_CONTROL_CONNECTED;
-                                        } else if (l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
+                                                l2cap_set_flag(L2CAP_FLAG_CONTROL_CONNECTED);
+                                        } else if(l2capinbuf[14] == interrupt_dcid[0] && l2capinbuf[15] == interrupt_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Interrupt Connection Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
                                                 interrupt_scid[0] = l2capinbuf[12];
                                                 interrupt_scid[1] = l2capinbuf[13];
-                                                l2cap_event_flag |= L2CAP_FLAG_INTERRUPT_CONNECTED;
+                                                l2cap_set_flag(L2CAP_FLAG_INTERRUPT_CONNECTED);
                                         }
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
 #ifdef EXTRADEBUG
                                 Notify(PSTR("\r\nL2CAP Connection Request - PSM: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
@@ -190,46 +192,46 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR(" Identifier: "), 0x80);
                                 D_PrintHex<uint8_t > (l2capinbuf[9], 0x80);
 #endif
-                                if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
+                                if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
                                         identifier = l2capinbuf[9];
                                         control_scid[0] = l2capinbuf[14];
                                         control_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_CONTROL_REQUEST;
-                                } else if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST);
+                                } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_INTR_PSM) {
                                         identifier = l2capinbuf[9];
                                         interrupt_scid[0] = l2capinbuf[14];
                                         interrupt_scid[1] = l2capinbuf[15];
-                                        l2cap_event_flag |= L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST;
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
-                                if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
-                                        if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) {
+                                if((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { // Success
+                                        if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_CONTROL_SUCCESS;
-                                        } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
+                                        } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Interrupt Configuration Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
-                                                l2cap_event_flag |= L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS;
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS);
                                         }
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
-                                if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
+                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                         //Notify(PSTR("\r\nHID Control Configuration Request"), 0x80);
                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], control_scid);
-                                } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
                                         //Notify(PSTR("\r\nHID Interrupt Configuration Request"), 0x80);
                                         pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], interrupt_scid);
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
-                                if (l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) {
+                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
 #endif
                                         identifier = l2capinbuf[9];
                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, control_dcid, control_scid);
                                         Reset();
-                                } else if (l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
+                                } else if(l2capinbuf[12] == interrupt_dcid[0] && l2capinbuf[13] == interrupt_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Interrupt Channel"), 0x80);
 #endif
@@ -237,15 +239,15 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                         pBtd->l2cap_disconnection_response(hci_handle, identifier, interrupt_dcid, interrupt_scid);
                                         Reset();
                                 }
-                        } else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
-                                if (l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
+                        } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
+                                if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE;
-                                } else if (l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
+                                } else if(l2capinbuf[12] == interrupt_scid[0] && l2capinbuf[13] == interrupt_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Interrupt Channel"), 0x80);
                                         identifier = l2capinbuf[9];
-                                        l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE;
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
                                 }
                         }
 #ifdef EXTRADEBUG
@@ -255,41 +257,41 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                 D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
                         }
 #endif
-                } else if (l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
+                } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
                         //Notify(PSTR("\r\nL2CAP Interrupt"), 0x80);
-                        if (l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
-                                if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
-                                        if ((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
+                        if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
+                                if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
+                                        if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
                                                 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
-                                        else if (wiiUProControllerConnected)
+                                        else if(wiiUProControllerConnected)
                                                 ButtonState = (uint32_t)(((~l2capinbuf[23]) & 0xFE) | ((uint16_t)(~l2capinbuf[24]) << 8) | ((uint32_t)((~l2capinbuf[25]) & 0x03) << 16));
-                                        else if (motionPlusConnected) {
-                                                if (l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus
+                                        else if(motionPlusConnected) {
+                                                if(l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus
                                                         ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000)));
-                                                else if (nunchuckConnected) // Update if it's a report from the Nunchuck
+                                                else if(nunchuckConnected) // Update if it's a report from the Nunchuck
                                                         ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x0C) << 14));
                                                 //else if(classicControllerConnected) // Update if it's a report from the Classic Controller
-                                        } else if (nunchuckConnected) // The Nunchuck is directly connected
+                                        } else if(nunchuckConnected) // The Nunchuck is directly connected
                                                 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)((~l2capinbuf[20]) & 0x03) << 16));
                                                 //else if(classicControllerConnected) // The Classic Controller is directly connected
-                                        else if (!unknownExtensionConnected)
+                                        else if(!unknownExtensionConnected)
                                                 ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
 #ifdef PRINTREPORT
                                         Notify(PSTR("ButtonState: "), 0x80);
                                         D_PrintHex<uint32_t > (ButtonState, 0x80);
                                         Notify(PSTR("\r\n"), 0x80);
 #endif
-                                        if (ButtonState != OldButtonState) {
+                                        if(ButtonState != OldButtonState) {
                                                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
                                                 OldButtonState = ButtonState;
                                         }
                                 }
-                                if (l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
+                                if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // Read the accelerometer
                                         accXwiimote = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5)) - 500;
                                         accYwiimote = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4)) - 500;
                                         accZwiimote = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5)) - 500;
                                 }
-                                switch (l2capinbuf[9]) {
+                                switch(l2capinbuf[9]) {
                                         case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV
 #ifdef EXTRADEBUG
                                                 Notify(PSTR("\r\nStatus report was received"), 0x80);
@@ -297,38 +299,38 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                 wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02:  An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
                                                 batteryLevel = l2capinbuf[15]; // Update battery level
 #ifdef DEBUG_USB_HOST
-                                                if (l2capinbuf[12] & 0x01)
+                                                if(l2capinbuf[12] & 0x01)
                                                         Notify(PSTR("\r\nWARNING: Battery is nearly empty"), 0x80);
 #endif
-                                                if (checkExtension) { // If this is false it means that the user must have called getBatteryLevel()
-                                                        if (l2capinbuf[12] & 0x02) { // Check if a extension is connected
+                                                if(checkExtension) { // If this is false it means that the user must have called getBatteryLevel()
+                                                        if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
 #ifdef DEBUG_USB_HOST
-                                                                if (!unknownExtensionConnected)
+                                                                if(!unknownExtensionConnected)
                                                                         Notify(PSTR("\r\nExtension connected"), 0x80);
 #endif
                                                                 unknownExtensionConnected = true;
 #ifdef WIICAMERA
-                                                        if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
+                                                                if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
 #endif
-                                                                setReportMode(false, 0x35); // Also read the extension
+                                                                        setReportMode(false, 0x35); // Also read the extension
                                                         } else {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nExtension disconnected"), 0x80);
 #endif
-                                                                if (motionPlusConnected) {
+                                                                if(motionPlusConnected) {
 #ifdef DEBUG_USB_HOST
                                                                         Notify(PSTR(" - from Motion Plus"), 0x80);
 #endif
-                                                                        l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
-                                                                        if (!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
+                                                                        wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED);
+                                                                        if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false
                                                                                 nunchuckConnected = false;
                                                                         //else if(classicControllerConnected)
-                                                                } else if (nunchuckConnected) {
+                                                                } else if(nunchuckConnected) {
 #ifdef DEBUG_USB_HOST
                                                                         Notify(PSTR(" - Nunchuck"), 0x80);
 #endif
                                                                         nunchuckConnected = false; // It must be the Nunchuck controller then
-                                                                        l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED;
+                                                                        wii_clear_flag(WII_FLAG_NUNCHUCK_CONNECTED);
                                                                         onInit();
                                                                         setReportMode(false, 0x31); // If there is no extension connected we will read the buttons and accelerometer
                                                                 } else
@@ -338,28 +340,28 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                         checkExtension = true; // Check for extensions by default
                                                 break;
                                         case 0x21: // Read Memory Data
-                                                if ((l2capinbuf[12] & 0x0F) == 0) { // No error
+                                                if((l2capinbuf[12] & 0x0F) == 0) { // No error
                                                         // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
-                                                        if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
+                                                        if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nNunchuck connected"), 0x80);
 #endif
-                                                                l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED;
-                                                        } else if (l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
+                                                                wii_set_flag(WII_FLAG_NUNCHUCK_CONNECTED);
+                                                        } else if(l2capinbuf[16] == 0x00 && (l2capinbuf[17] == 0xA6 || l2capinbuf[17] == 0xA4) && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nMotion Plus connected"), 0x80);
 #endif
-                                                                l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED;
-                                                        } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
+                                                                wii_set_flag(WII_FLAG_MOTION_PLUS_CONNECTED);
+                                                        } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nMotion Plus activated in normal mode"), 0x80);
 #endif
                                                                 motionPlusConnected = true;
 #ifdef WIICAMERA
-                                                                if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
+                                                                if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
 #endif
                                                                         setReportMode(false, 0x35); // Also read the extension
-                                                        } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
+                                                        } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode"), 0x80);
 #endif
@@ -367,16 +369,16 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                 motionPlusConnected = true;
                                                                 nunchuckConnected = true;
 #ifdef WIICAMERA
-                                                                if (!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
+                                                                if(!isIRCameraEnabled()) // Don't activate the Motion Plus if we are trying to initialize the IR camera
 #endif
                                                                         setReportMode(false, 0x35); // Also read the extension
-                                                        } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) {
+                                                        } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && (l2capinbuf[19] == 0x00 || l2capinbuf[19] == 0x04 || l2capinbuf[19] == 0x05 || l2capinbuf[19] == 0x07) && l2capinbuf[20] == 0x05) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nInactive Wii Motion Plus"), 0x80);
                                                                 Notify(PSTR("\r\nPlease unplug the Motion Plus, disconnect the Wiimote and then replug the Motion Plus Extension"), 0x80);
 #endif
-                                                                stateCounter = 300; // Skip the rest in "L2CAP_CHECK_MOTION_PLUS_STATE"
-                                                        } else if (l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
+                                                                stateCounter = 300; // Skip the rest in "WII_CHECK_MOTION_PLUS_STATE"
+                                                        } else if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x01 && l2capinbuf[20] == 0x20) {
 #ifdef DEBUG_USB_HOST
                                                                 Notify(PSTR("\r\nWii U Pro Controller connected"), 0x80);
 #endif
@@ -388,7 +390,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                 D_PrintHex<uint8_t > (l2capinbuf[13], 0x80);
                                                                 D_PrintHex<uint8_t > (l2capinbuf[14], 0x80);
                                                                 Notify(PSTR("\r\nData: "), 0x80);
-                                                                for (uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1
+                                                                for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1
                                                                         D_PrintHex<uint8_t > (l2capinbuf[15 + i], 0x80);
                                                                         Notify(PSTR(" "), 0x80);
                                                                 }
@@ -405,7 +407,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                 break;
                                         case 0x22: // Acknowledge output report, return function result
 #ifdef DEBUG_USB_HOST
-                                                if (l2capinbuf[13] != 0x00) { // Check if there is an error
+                                                if(l2capinbuf[13] != 0x00) { // Check if there is an error
                                                         Notify(PSTR("\r\nCommand failed: "), 0x80);
                                                         D_PrintHex<uint8_t > (l2capinbuf[12], 0x80);
                                                 }
@@ -461,9 +463,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                 break;
                                         case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes
                                                 // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE
-                                                if (motionPlusConnected) {
-                                                        if (l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
-                                                                if (motionValuesReset) { // We will only use the values when the gyro value has been set
+                                                if(motionPlusConnected) {
+                                                        if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
+                                                                if(motionValuesReset) { // We will only use the values when the gyro value has been set
                                                                         gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)) - gyroYawZero);
                                                                         gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)) - gyroRollZero);
                                                                         gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)) - gyroPitchZero);
@@ -473,11 +475,11 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                         pitchGyroSpeed = (double)gyroPitchRaw / ((double)gyroPitchZero / pitchGyroScale);
 
                                                                         /* The onboard gyro has two ranges for slow and fast mode */
-                                                                        if (!(l2capinbuf[18] & 0x02)) // Check if fast mode is used
+                                                                        if(!(l2capinbuf[18] & 0x02)) // Check if fast mode is used
                                                                                 yawGyroSpeed *= 4.545;
-                                                                        if (!(l2capinbuf[18] & 0x01)) // Check if fast mode is used
+                                                                        if(!(l2capinbuf[18] & 0x01)) // Check if fast mode is used
                                                                                 pitchGyroSpeed *= 4.545;
-                                                                        if (!(l2capinbuf[19] & 0x02)) // Check if fast mode is used
+                                                                        if(!(l2capinbuf[19] & 0x02)) // Check if fast mode is used
                                                                                 rollGyroSpeed *= 4.545;
 
                                                                         compPitch = (0.93 * (compPitch + (pitchGyroSpeed * (double)(micros() - timer) / 1000000)))+(0.07 * getWiimotePitch()); // Use a complimentary filter to calculate the angle
@@ -495,15 +497,15 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                         Notify(gyroRoll, 0x80);
                                                                         Notify(PSTR("\tgyroPitch: "), 0x80);
                                                                         Notify(gyroPitch, 0x80);
-                                                                        */
+                                                                         */
                                                                         /*
                                                                         Notify(PSTR("\twiimoteRoll: "), 0x80);
                                                                         Notify(wiimoteRoll, 0x80);
                                                                         Notify(PSTR("\twiimotePitch: "), 0x80);
                                                                         Notify(wiimotePitch, 0x80);
-                                                                        */
+                                                                         */
                                                                 } else {
-                                                                        if ((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
+                                                                        if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values
 #ifdef DEBUG_USB_HOST
                                                                                 Notify(PSTR("\r\nThe gyro values has been reset"), 0x80);
 #endif
@@ -524,7 +526,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                         }
                                                                 }
                                                         } else {
-                                                                if (nunchuckConnected) {
+                                                                if(nunchuckConnected) {
                                                                         hatValues[HatX] = l2capinbuf[15];
                                                                         hatValues[HatY] = l2capinbuf[16];
                                                                         accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3)) - 416;
@@ -533,8 +535,8 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                 }
                                                                 //else if(classicControllerConnected) { }
                                                         }
-                                                        if (l2capinbuf[19] & 0x01) {
-                                                                if (!extensionConnected) {
+                                                        if(l2capinbuf[19] & 0x01) {
+                                                                if(!extensionConnected) {
                                                                         extensionConnected = true;
                                                                         unknownExtensionConnected = true;
 #ifdef DEBUG_USB_HOST
@@ -542,7 +544,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
 #endif
                                                                 }
                                                         } else {
-                                                                if (extensionConnected && !unknownExtensionConnected) {
+                                                                if(extensionConnected && !unknownExtensionConnected) {
                                                                         extensionConnected = false;
                                                                         unknownExtensionConnected = true;
 #ifdef DEBUG_USB_HOST
@@ -552,13 +554,13 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                                                                 }
                                                         }
 
-                                                } else if (nunchuckConnected) {
+                                                } else if(nunchuckConnected) {
                                                         hatValues[HatX] = l2capinbuf[15];
                                                         hatValues[HatY] = l2capinbuf[16];
                                                         accXnunchuck = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2)) - 416;
                                                         accYnunchuck = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4)) - 416;
                                                         accZnunchuck = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6)) - 416;
-                                                } else if (wiiUProControllerConnected) {
+                                                } else if(wiiUProControllerConnected) {
                                                         hatValues[LeftHatX] = (l2capinbuf[15] | l2capinbuf[16] << 8);
                                                         hatValues[RightHatX] = (l2capinbuf[17] | l2capinbuf[18] << 8);
                                                         hatValues[LeftHatY] = (l2capinbuf[19] | l2capinbuf[20] << 8);
@@ -579,10 +581,10 @@ void WII::ACLData(uint8_t* l2capinbuf) {
 }
 
 void WII::L2CAP_task() {
-        switch (l2cap_state) {
+        switch(l2cap_state) {
                         /* These states are used if the Wiimote is the host */
                 case L2CAP_CONTROL_SUCCESS:
-                        if (l2cap_config_success_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
 #endif
@@ -591,7 +593,7 @@ void WII::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_SETUP:
-                        if (l2cap_connection_request_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Interrupt Incoming Connection Request"), 0x80);
 #endif
@@ -608,7 +610,7 @@ void WII::L2CAP_task() {
 
                         /* These states are used if the Arduino is the host */
                 case L2CAP_CONTROL_CONNECT_REQUEST:
-                        if (l2cap_connected_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONTROL_CONNECTED)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSend HID Control Config Request"), 0x80);
 #endif
@@ -619,7 +621,7 @@ void WII::L2CAP_task() {
                         break;
 
                 case L2CAP_CONTROL_CONFIG_REQUEST:
-                        if (l2cap_config_success_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
 #endif
@@ -630,7 +632,7 @@ void WII::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_CONNECT_REQUEST:
-                        if (l2cap_connected_interrupt_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_INTERRUPT_CONNECTED)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nSend HID Interrupt Config Request"), 0x80);
 #endif
@@ -641,21 +643,21 @@ void WII::L2CAP_task() {
                         break;
 
                 case L2CAP_INTERRUPT_CONFIG_REQUEST:
-                        if (l2cap_config_success_interrupt_flag) { // Now the HID channels is established
+                        if(l2cap_check_flag(L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)) { // Now the HID channels is established
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Channels Established"), 0x80);
 #endif
                                 pBtd->connectToWii = false;
                                 pBtd->pairWithWii = false;
                                 stateCounter = 0;
-                                l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
+                                l2cap_state = WII_CHECK_MOTION_PLUS_STATE;
                         }
                         break;
 
                         /* The next states are in run() */
 
                 case L2CAP_INTERRUPT_DISCONNECT:
-                        if (l2cap_disconnect_response_interrupt_flag && millis() > timer) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE) && ((long)(millis() - timer) >= 0L)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Interrupt Channel"), 0x80);
 #endif
@@ -666,7 +668,7 @@ void WII::L2CAP_task() {
                         break;
 
                 case L2CAP_CONTROL_DISCONNECT:
-                        if (l2cap_disconnect_response_control_flag) {
+                        if(l2cap_check_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nDisconnected Control Channel"), 0x80);
 #endif
@@ -680,12 +682,12 @@ void WII::L2CAP_task() {
 }
 
 void WII::Run() {
-        if (l2cap_state == L2CAP_INTERRUPT_DISCONNECT && millis() > timer)
+        if(l2cap_state == L2CAP_INTERRUPT_DISCONNECT && ((long)(millis() - timer) >= 0L))
                 L2CAP_task(); // Call the rest of the disconnection routine after we have waited long enough
 
-        switch (l2cap_state) {
+        switch(l2cap_state) {
                 case L2CAP_WAIT:
-                        if (pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
+                        if(pBtd->connectToWii && !pBtd->l2capConnectionClaimed && !wiimoteConnected && !activeConnection) {
                                 pBtd->l2capConnectionClaimed = true;
                                 activeConnection = true;
                                 motionPlusInside = pBtd->motionPlusInside;
@@ -697,7 +699,7 @@ void WII::Run() {
                                 identifier = 0;
                                 pBtd->l2cap_connection_request(hci_handle, identifier, control_dcid, HID_CTRL_PSM);
                                 l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST;
-                        } else if (l2cap_connection_request_control_flag) {
+                        } else if(l2cap_check_flag(L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)) {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Incoming Connection Request"), 0x80);
 #endif
@@ -711,76 +713,76 @@ void WII::Run() {
                         }
                         break;
 
-                case L2CAP_CHECK_MOTION_PLUS_STATE:
+                case WII_CHECK_MOTION_PLUS_STATE:
 #ifdef DEBUG_USB_HOST
-                        if (stateCounter == 0) // Only print onnce
+                        if(stateCounter == 0) // Only print onnce
                                 Notify(PSTR("\r\nChecking if a Motion Plus is connected"), 0x80);
 #endif
                         stateCounter++;
-                        if (stateCounter % 200 == 0)
+                        if(stateCounter % 200 == 0)
                                 checkMotionPresent(); // Check if there is a motion plus connected
-                        if (motion_plus_connected_flag) {
+                        if(wii_check_flag(WII_FLAG_MOTION_PLUS_CONNECTED)) {
                                 stateCounter = 0;
-                                l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE;
+                                l2cap_state = WII_INIT_MOTION_PLUS_STATE;
                                 timer = micros();
 
-                                if (unknownExtensionConnected) {
+                                if(unknownExtensionConnected) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nA extension is also connected"), 0x80);
 #endif
                                         activateNunchuck = true; // For we will just set this to true as this the only extension supported so far
                                 }
 
-                        } else if (stateCounter == 601) { // We will try three times to check for the motion plus
+                        } else if(stateCounter == 601) { // We will try three times to check for the motion plus
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nNo Motion Plus was detected"), 0x80);
 #endif
                                 stateCounter = 0;
-                                l2cap_state = L2CAP_CHECK_EXTENSION_STATE;
+                                l2cap_state = WII_CHECK_EXTENSION_STATE;
                         }
                         break;
 
-                case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
+                case WII_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port
 #ifdef DEBUG_USB_HOST
-                        if (stateCounter == 0) // Only print onnce
+                        if(stateCounter == 0) // Only print onnce
                                 Notify(PSTR("\r\nChecking if there is any extension connected"), 0x80);
 #endif
                         stateCounter++; // We use this counter as there has to be a short delay between the commands
-                        if (stateCounter == 1)
+                        if(stateCounter == 1)
                                 statusRequest(); // See if a new device has connected
-                        if (stateCounter == 100) {
-                                if (unknownExtensionConnected) // Check if there is a extension is connected to the port
+                        if(stateCounter == 100) {
+                                if(unknownExtensionConnected) // Check if there is a extension is connected to the port
                                         initExtension1();
                                 else
                                         stateCounter = 399;
-                        } else if (stateCounter == 200)
+                        } else if(stateCounter == 200)
                                 initExtension2();
-                        else if (stateCounter == 300) {
+                        else if(stateCounter == 300) {
                                 readExtensionType();
                                 unknownExtensionConnected = false;
-                        } else if (stateCounter == 400) {
+                        } else if(stateCounter == 400) {
                                 stateCounter = 0;
-                                l2cap_state = L2CAP_LED_STATE;
+                                l2cap_state = TURN_ON_LED;
                         }
                         break;
 
-                case L2CAP_INIT_MOTION_PLUS_STATE:
+                case WII_INIT_MOTION_PLUS_STATE:
                         stateCounter++;
-                        if (stateCounter == 1)
+                        if(stateCounter == 1)
                                 initMotionPlus();
-                        else if (stateCounter == 100)
+                        else if(stateCounter == 100)
                                 activateMotionPlus();
-                        else if (stateCounter == 200)
+                        else if(stateCounter == 200)
                                 readExtensionType(); // Check if it has been activated
-                        else if (stateCounter == 300) {
+                        else if(stateCounter == 300) {
                                 stateCounter = 0;
                                 unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus
-                                l2cap_state = L2CAP_LED_STATE;
+                                l2cap_state = TURN_ON_LED;
                         }
                         break;
 
-                case L2CAP_LED_STATE:
-                        if (nunchuck_connected_flag)
+                case TURN_ON_LED:
+                        if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED))
                                 nunchuckConnected = true;
                         wiimoteConnected = true;
                         onInit();
@@ -788,46 +790,46 @@ void WII::Run() {
                         break;
 
                 case L2CAP_DONE:
-                        if (unknownExtensionConnected) {
+                        if(unknownExtensionConnected) {
 #ifdef DEBUG_USB_HOST
-                                if (stateCounter == 0) // Only print once
+                                if(stateCounter == 0) // Only print once
                                         Notify(PSTR("\r\nChecking extension port"), 0x80);
 #endif
                                 stateCounter++; // We will use this counter as there has to be a short delay between the commands
-                                if (stateCounter == 50)
+                                if(stateCounter == 50)
                                         statusRequest();
-                                else if (stateCounter == 100)
+                                else if(stateCounter == 100)
                                         initExtension1();
-                                else if (stateCounter == 150)
-                                        if ((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected))
+                                else if(stateCounter == 150)
+                                        if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected))
                                                 initExtension2();
                                         else
                                                 stateCounter = 299; // There is no extension connected
-                                else if (stateCounter == 200)
+                                else if(stateCounter == 200)
                                         readExtensionType();
-                                else if (stateCounter == 250) {
-                                        if (nunchuck_connected_flag) {
+                                else if(stateCounter == 250) {
+                                        if(wii_check_flag(WII_FLAG_NUNCHUCK_CONNECTED)) {
 #ifdef DEBUG_USB_HOST
                                                 Notify(PSTR("\r\nNunchuck was reconnected"), 0x80);
 #endif
                                                 activateNunchuck = true;
                                                 nunchuckConnected = true;
                                         }
-                                        if (!motionPlusConnected)
+                                        if(!motionPlusConnected)
                                                 stateCounter = 449;
-                                } else if (stateCounter == 300) {
-                                        if (motionPlusConnected) {
+                                } else if(stateCounter == 300) {
+                                        if(motionPlusConnected) {
 #ifdef DEBUG_USB_HOST
                                                 Notify(PSTR("\r\nReactivating the Motion Plus"), 0x80);
 #endif
                                                 initMotionPlus();
                                         } else
                                                 stateCounter = 449;
-                                } else if (stateCounter == 350)
+                                } else if(stateCounter == 350)
                                         activateMotionPlus();
-                                else if (stateCounter == 400)
+                                else if(stateCounter == 400)
                                         readExtensionType(); // Check if it has been activated
-                                else if (stateCounter == 450) {
+                                else if(stateCounter == 450) {
                                         onInit();
                                         stateCounter = 0;
                                         unknownExtensionConnected = false;
@@ -843,8 +845,8 @@ void WII::Run() {
 
 /************************************************************/
 void WII::HID_Command(uint8_t* data, uint8_t nbytes) {
-        if (motionPlusInside)
-                pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new wiimote with the Motion Plus Inside
+        if(motionPlusInside)
+                pBtd->L2CAP_Command(hci_handle, data, nbytes, interrupt_scid[0], interrupt_scid[1]); // It's the new Wiimote with the Motion Plus Inside or Wii U Pro controller
         else
                 pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
 }
@@ -878,32 +880,37 @@ void WII::setLedRaw(uint8_t value) {
         HIDBuffer[2] = value | (HIDBuffer[2] & 0x01); // Keep the rumble bit
         HID_Command(HIDBuffer, 3);
 }
-void WII::setLedOff(LED a) {
+
+void WII::setLedOff(LEDEnum a) {
         HIDBuffer[1] = 0x11;
-        HIDBuffer[2] &= ~(pgm_read_byte(&LEDS[(uint8_t)a]));
+        HIDBuffer[2] &= ~(pgm_read_byte(&WII_LEDS[(uint8_t)a]));
         HID_Command(HIDBuffer, 3);
 }
 
-void WII::setLedOn(LED a) {
-        HIDBuffer[1] = 0x11;
-        HIDBuffer[2] |= pgm_read_byte(&LEDS[(uint8_t)a]);
-        HID_Command(HIDBuffer, 3);
+void WII::setLedOn(LEDEnum a) {
+        if(a == OFF)
+                setLedRaw(0);
+        else {
+                HIDBuffer[1] = 0x11;
+                HIDBuffer[2] |= pgm_read_byte(&WII_LEDS[(uint8_t)a]);
+                HID_Command(HIDBuffer, 3);
+        }
 }
 
-void WII::setLedToggle(LED a) {
+void WII::setLedToggle(LEDEnum a) {
         HIDBuffer[1] = 0x11;
-        HIDBuffer[2] ^= pgm_read_byte(&LEDS[(uint8_t)a]);
+        HIDBuffer[2] ^= pgm_read_byte(&WII_LEDS[(uint8_t)a]);
         HID_Command(HIDBuffer, 3);
 }
 
 void WII::setLedStatus() {
         HIDBuffer[1] = 0x11;
         HIDBuffer[2] = (HIDBuffer[2] & 0x01); // Keep the rumble bit
-        if (wiimoteConnected)
+        if(wiimoteConnected)
                 HIDBuffer[2] |= 0x10; // If it's connected LED1 will light up
-        if (motionPlusConnected)
+        if(motionPlusConnected)
                 HIDBuffer[2] |= 0x20; // If it's connected LED2 will light up
-        if (nunchuckConnected)
+        if(nunchuckConnected)
                 HIDBuffer[2] |= 0x40; // If it's connected LED3 will light up
 
         HID_Command(HIDBuffer, 3);
@@ -919,7 +926,7 @@ void WII::setReportMode(bool continuous, uint8_t mode) {
         uint8_t cmd_buf[4];
         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
         cmd_buf[1] = 0x12;
-        if (continuous)
+        if(continuous)
                 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
         else
                 cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Keep the rumble bit
@@ -949,9 +956,9 @@ void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) {
         cmd_buf[5] = (uint8_t)(offset & 0xFF);
         cmd_buf[6] = size;
         uint8_t i = 0;
-        for (; i < size; i++)
+        for(; i < size; i++)
                 cmd_buf[7 + i] = data[i];
-        for (; i < 16; i++) // Set the rest to zero
+        for(; i < 16; i++) // Set the rest to zero
                 cmd_buf[7 + i] = 0x00;
         HID_Command(cmd_buf, 23);
 }
@@ -976,17 +983,17 @@ void WII::initMotionPlus() {
 
 void WII::activateMotionPlus() {
         uint8_t buf[1];
-        if (pBtd->wiiUProController) {
+        if(pBtd->wiiUProController) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nActivating Wii U Pro Controller"), 0x80);
 #endif
                 buf[0] = 0x00; // It seems like you can send anything but 0x04, 0x05, and 0x07
-        } else if (activateNunchuck) {
+        } else if(activateNunchuck) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nActivating Motion Plus in pass-through mode"), 0x80);
 #endif
                 buf[0] = 0x05; // Activate nunchuck pass-through mode
-        }                //else if(classicControllerConnected && extensionConnected)
+        }//else if(classicControllerConnected && extensionConnected)
                 //buf[0] = 0x07;
         else {
 #ifdef DEBUG_USB_HOST
@@ -1001,7 +1008,7 @@ void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) {
         uint8_t cmd_buf[8];
         cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
         cmd_buf[1] = 0x17; // Read data
-        if (EEPROM)
+        if(EEPROM)
                 cmd_buf[2] = 0x00 | (HIDBuffer[2] & 0x01); // Read from EEPROM
         else
                 cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Read from memory
@@ -1031,42 +1038,42 @@ void WII::checkMotionPresent() {
 
 /************************************************************/
 
-bool WII::getButtonPress(Button b) { // Return true when a button is pressed
-        if (wiiUProControllerConnected)
-                return (ButtonState & pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]));
+bool WII::getButtonPress(ButtonEnum b) { // Return true when a button is pressed
+        if(wiiUProControllerConnected)
+                return (ButtonState & pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]));
         else
-                return (ButtonState & pgm_read_dword(&BUTTONS[(uint8_t)b]));
+                return (ButtonState & pgm_read_dword(&WII_BUTTONS[(uint8_t)b]));
 }
 
-bool WII::getButtonClick(Button b) { // Only return true when a button is clicked
+bool WII::getButtonClick(ButtonEnum b) { // Only return true when a button is clicked
         uint32_t button;
-        if (wiiUProControllerConnected)
-                button = pgm_read_dword(&PROCONTROLLERBUTTONS[(uint8_t)b]);
+        if(wiiUProControllerConnected)
+                button = pgm_read_dword(&WII_PROCONTROLLER_BUTTONS[(uint8_t)b]);
         else
-                button = pgm_read_dword(&BUTTONS[(uint8_t)b]);
+                button = pgm_read_dword(&WII_BUTTONS[(uint8_t)b]);
         bool click = (ButtonClickState & button);
         ButtonClickState &= ~button; // clear "click" event
         return click;
 }
 
-uint8_t WII::getAnalogHat(Hat a) {
-        if (!nunchuckConnected)
+uint8_t WII::getAnalogHat(HatEnum a) {
+        if(!nunchuckConnected)
                 return 127; // Return center position
         else {
                 uint8_t output = hatValues[(uint8_t)a];
-                if (output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position
+                if(output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position
                         return 127;
                 else
                         return output;
         }
 }
 
-uint16_t WII::getAnalogHat(AnalogHat a) {
-        if (!wiiUProControllerConnected)
+uint16_t WII::getAnalogHat(AnalogHatEnum a) {
+        if(!wiiUProControllerConnected)
                 return 2000;
         else {
                 uint16_t output = hatValues[(uint8_t)a];
-                if (output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position
+                if(output == 0x00) // The joystick will only read 0 when it is first initializing, so we will just return the center position
                         return 2000;
                 else
                         return output;
@@ -1074,7 +1081,7 @@ uint16_t WII::getAnalogHat(AnalogHat a) {
 }
 
 void WII::onInit() {
-        if (pFuncOnInit)
+        if(pFuncOnInit)
                 pFuncOnInit(); // Call the user function
         else
                 setLedStatus();
diff --git a/Wii.h b/Wii.h
index 397b837f..848dec93 100755
--- a/Wii.h
+++ b/Wii.h
@@ -23,61 +23,16 @@
 #include "BTD.h"
 #include "controllerEnums.h"
 
-/** You will have to uncomment this to use the IR camera */
-//#define WIICAMERA
-
-/* Bluetooth L2CAP states for L2CAP_task() */
-#define L2CAP_WAIT                      0
-
-// These states are used if the Wiimote is the host
-#define L2CAP_CONTROL_SUCCESS           1
-#define L2CAP_INTERRUPT_SETUP           2
-
-// These states are used if the Arduino is the host
-#define L2CAP_CONTROL_CONNECT_REQUEST   3
-#define L2CAP_CONTROL_CONFIG_REQUEST    4
-#define L2CAP_INTERRUPT_CONNECT_REQUEST 5
-
-#define L2CAP_INTERRUPT_CONFIG_REQUEST  6
-
-#define L2CAP_CHECK_MOTION_PLUS_STATE   7
-#define L2CAP_CHECK_EXTENSION_STATE     8
-#define L2CAP_INIT_MOTION_PLUS_STATE    9
-#define L2CAP_LED_STATE                 10
-#define L2CAP_DONE                      11
-
-#define L2CAP_INTERRUPT_DISCONNECT      12
-#define L2CAP_CONTROL_DISCONNECT        13
-
-/* L2CAP event flags */
-#define L2CAP_FLAG_CONTROL_CONNECTED                0x001
-#define L2CAP_FLAG_INTERRUPT_CONNECTED              0x002
-#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS           0x004
-#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS         0x008
-#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE      0x040
-#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE    0x080
-#define L2CAP_FLAG_CONNECTION_CONTROL_REQUEST       0x100
-#define L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST     0x200
-
-/* Macros for L2CAP event flag tests */
-#define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED)
-#define l2cap_connected_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_INTERRUPT_CONNECTED)
-#define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS)
-#define l2cap_config_success_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS)
-#define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE)
-#define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE)
-#define l2cap_connection_request_control_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_CONTROL_REQUEST)
-#define l2cap_connection_request_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_INTERRUPT_REQUEST)
-
 /* Wii event flags */
-#define WII_FLAG_MOTION_PLUS_CONNECTED              0x400
-#define WII_FLAG_NUNCHUCK_CONNECTED                 0x800
+#define WII_FLAG_MOTION_PLUS_CONNECTED  0x01
+#define WII_FLAG_NUNCHUCK_CONNECTED     0x02
 
-#define motion_plus_connected_flag (l2cap_event_flag & WII_FLAG_MOTION_PLUS_CONNECTED)
-#define nunchuck_connected_flag (l2cap_event_flag & WII_FLAG_NUNCHUCK_CONNECTED)
+#define wii_check_flag(flag)  (wii_event_flag & (flag))
+#define wii_set_flag(flag)  (wii_event_flag |= (flag))
+#define wii_clear_flag(flag)  (wii_event_flag &= ~(flag))
 
 /** Enum used to read the joystick on the Nunchuck. */
-enum Hat {
+enum HatEnum {
         /** Read the x-axis on the Nunchuck joystick. */
         HatX = 0,
         /** Read the y-axis on the Nunchuck joystick. */
@@ -105,7 +60,7 @@ public:
          * @param ACLData Incoming acldata.
          */
         virtual void ACLData(uint8_t* ACLData);
-        /** Used to run part of the state maschine. */
+        /** Used to run part of the state machine. */
         virtual void Run();
         /** Use this to reset the service. */
         virtual void Reset();
@@ -121,36 +76,39 @@ public:
          *
          * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
          * but if you need to drive a robot forward you would use getButtonPress(Button b).
+         * @param  b          ::ButtonEnum to read.
+         * @return            getButtonPress(ButtonEnum b) will return a true as long as a button is held down, while getButtonClick(ButtonEnum b) will return true once for each button press.
          */
-        bool getButtonPress(Button b);
-        bool getButtonClick(Button b);
+        bool getButtonPress(ButtonEnum b);
+        bool getButtonClick(ButtonEnum b);
         /**@}*/
 
         /** @name Wii Controller functions */
+
         /** Call this to start the paring sequence with a controller */
         void pair(void) {
                 if(pBtd)
                         pBtd->pairWithWiimote();
-        }
+        };
         /**
          * Used to read the joystick of the Nunchuck.
          * @param  a Either ::HatX or ::HatY.
          * @return   Return the analog value in the range from approximately 25-230.
          */
-        uint8_t getAnalogHat(Hat a);
+        uint8_t getAnalogHat(HatEnum a);
         /**
          * Used to read the joystick of the Wii U Pro Controller.
          * @param  a Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY.
          * @return   Return the analog value in the range from approximately 800-3200.
          */
-        uint16_t getAnalogHat(AnalogHat a);
+        uint16_t getAnalogHat(AnalogHatEnum a);
 
         /**
          * Pitch calculated from the Wiimote. A complimentary filter is used if the Motion Plus is connected.
          * @return Pitch in the range from 0-360.
          */
         double getPitch() {
-                if (motionPlusConnected)
+                if(motionPlusConnected)
                         return compPitch;
                 return getWiimotePitch();
         };
@@ -160,7 +118,7 @@ public:
          * @return Roll in the range from 0-360.
          */
         double getRoll() {
-                if (motionPlusConnected)
+                if(motionPlusConnected)
                         return compRoll;
                 return getWiimoteRoll();
         };
@@ -185,35 +143,36 @@ public:
         void setRumbleToggle();
 
         /**
-         * Set LED value without using the ::LED enum.
-         * @param value See: ::LED enum.
+         * Set LED value without using the ::LEDEnum.
+         * @param value See: ::LEDEnum.
          */
         void setLedRaw(uint8_t value);
+
         /** Turn all LEDs off. */
         void setLedOff() {
                 setLedRaw(0);
-        }
+        };
         /**
-         * Turn the specific ::LED off.
-         * @param a The ::LED to turn off.
+         * Turn the specific ::LEDEnum off.
+         * @param a The ::LEDEnum to turn off.
          */
-        void setLedOff(LED a);
+        void setLedOff(LEDEnum a);
         /**
-         * Turn the specific ::LED on.
-         * @param a The ::LED to turn on.
+         * Turn the specific ::LEDEnum on.
+         * @param a The ::LEDEnum to turn on.
          */
-        void setLedOn(LED a);
+        void setLedOn(LEDEnum a);
         /**
-         * Toggle the specific ::LED.
-         * @param a The ::LED to toggle.
+         * Toggle the specific ::LEDEnum.
+         * @param a The ::LEDEnum to toggle.
          */
-        void setLedToggle(LED a);
+        void setLedToggle(LEDEnum a);
         /**
          * This will set the LEDs, so the user can see which connections are active.
          *
-         * The first ::LED indicate that the Wiimote is connected,
-         * the second ::LED indicate indicate that a Motion Plus is also connected
-         * the third ::LED will indicate that a Nunchuck controller is also connected.
+         * The first ::LEDEnum indicate that the Wiimote is connected,
+         * the second ::LEDEnum indicate indicate that a Motion Plus is also connected
+         * the third ::LEDEnum will indicate that a Nunchuck controller is also connected.
          */
         void setLedStatus();
 
@@ -222,6 +181,7 @@ public:
          * @return The battery level in the range 0-255.
          */
         uint8_t getBatteryLevel();
+
         /**
          * Return the Wiimote state.
          * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status.
@@ -253,20 +213,24 @@ public:
         /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */
 
         /**@{*/
+
         /** Pitch and roll calculated from the accelerometer inside the Wiimote. */
         double getWiimotePitch() {
                 return (atan2(accYwiimote, accZwiimote) + PI) * RAD_TO_DEG;
         };
+
         double getWiimoteRoll() {
                 return (atan2(accXwiimote, accZwiimote) + PI) * RAD_TO_DEG;
         };
         /**@}*/
 
         /**@{*/
+
         /** Pitch and roll calculated from the accelerometer inside the Nunchuck. */
         double getNunchuckPitch() {
                 return (atan2(accYnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
         };
+
         double getNunchuckRoll() {
                 return (atan2(accXnunchuck, accZnunchuck) + PI) * RAD_TO_DEG;
         };
@@ -316,7 +280,7 @@ public:
 
 #ifdef WIICAMERA
         /** @name Wiimote IR camera functions
-         * You will have to uncomment #WIICAMERA in Wii.h to use the IR camera.
+         * You will have to set ::ENABLE_WII_IR_CAMERA in settings.h to 1 in order use the IR camera.
          */
         /** Initialises the camera as per the steps from: http://wiibrew.org/wiki/Wiimote#IR_Camera */
         void IRinitialize();
@@ -423,7 +387,7 @@ public:
          * @return     True if it's enabled, false if not.
          */
         bool isIRCameraEnabled() {
-                return(wiiState & 0x08);
+                return (wiiState & 0x08);
         };
         /**@}*/
 #endif
@@ -447,7 +411,8 @@ private:
 
         /* Variables used by high level L2CAP task */
         uint8_t l2cap_state;
-        uint16_t l2cap_event_flag; // l2cap flags of received Bluetooth events
+        uint32_t l2cap_event_flag; // L2CAP flags of received Bluetooth events
+        uint8_t wii_event_flag; // Used for Wii flags
 
         uint32_t ButtonState;
         uint32_t OldButtonState;
@@ -520,4 +485,4 @@ private:
         uint8_t IR_object_s4;
 #endif
 };
-#endif
\ No newline at end of file
+#endif
diff --git a/WiiCameraReadme.md b/WiiCameraReadme.md
index d3fc11ae..2309b4f9 100644
--- a/WiiCameraReadme.md
+++ b/WiiCameraReadme.md
@@ -2,7 +2,7 @@ Please see <http://wiibrew.org/wiki/Wiimote#IR_Camera> for the complete capabili
 
 This library is large, if you run into memory problems when uploading to the Arduino, disable serial debugging.
 
-To enable the IR camera code, uncomment \#define WIICAMERA in Wii.h.
+To enable the IR camera code, simply set ```ENABLE_WII_IR_CAMERA``` to 1 in [settings.h](settings.h).
 
 This library implements the following settings:
 
diff --git a/XBOXOLD.cpp b/XBOXOLD.cpp
index 31d2fb6e..298d5ab1 100644
--- a/XBOXOLD.cpp
+++ b/XBOXOLD.cpp
@@ -21,7 +21,7 @@
 //#define PRINTREPORT // Uncomment to print the report send by the Xbox controller
 
 /** Buttons on the controllers */
-const uint8_t XBOXOLDBUTTONS[] PROGMEM = {
+const uint8_t XBOXOLD_BUTTONS[] PROGMEM = {
         0x01, // UP
         0x08, // RIGHT
         0x02, // DOWN
@@ -48,19 +48,20 @@ XBOXOLD::XBOXOLD(USB *p) :
 pUsb(p), // pointer to USB class instance - mandatory
 bAddress(0), // device address - mandatory
 bPollEnable(false) { // don't start polling before dongle is connected
-        for (uint8_t i = 0; i < XBOX_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < XBOX_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
                 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
         }
 
-        if (pUsb) // register in USB subsystem
+        if(pUsb) // register in USB subsystem
                 pUsb->RegisterDeviceClass(this); //set devConfig[] entry
 }
 
 uint8_t XBOXOLD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -73,7 +74,7 @@ uint8_t XBOXOLD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nXBOXUSB Init"), 0x80);
 #endif
         // check if address has already been assigned to an instance
-        if (bAddress) {
+        if(bAddress) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress in use"), 0x80);
 #endif
@@ -83,14 +84,14 @@ uint8_t XBOXOLD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nepinfo is null"), 0x80);
 #endif
@@ -110,27 +111,27 @@ uint8_t XBOXOLD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
-        VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
-        PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
+        VID = udd->idVendor;
+        PID = udd->idProduct;
 
-        if ((VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) || (PID != XBOX_OLD_PID1 && PID != XBOX_OLD_PID2 && PID != XBOX_OLD_PID3 && PID != XBOX_OLD_PID4)) // Check if VID and PID match
+        if((VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) || (PID != XBOX_OLD_PID1 && PID != XBOX_OLD_PID2 && PID != XBOX_OLD_PID3 && PID != XBOX_OLD_PID4)) // Check if VID and PID match
                 goto FailUnknownDevice;
 
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -144,20 +145,20 @@ uint8_t XBOXOLD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nAddr: "), 0x80);
         D_PrintHex<uint8_t > (bAddress, 0x80);
 #endif
-        delay(300); // Spec says you should wait at least 200ms
+        //delay(300); // Spec says you should wait at least 200ms
 
         p->lowspeed = false;
 
         //get pointer to assigned address record
         p = addrPool.GetUsbDevicePtr(bAddress);
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
         // Assign epInfo to epinfo pointer - only EP0 is known
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         /* The application will work in reduced host mode, so we can save program and data
@@ -169,29 +170,29 @@ uint8_t XBOXOLD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         epInfo[ XBOX_INPUT_PIPE ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_INPUT_PIPE ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_INPUT_PIPE ].bmSndToggle = 0;
+        epInfo[ XBOX_INPUT_PIPE ].bmRcvToggle = 0;
         epInfo[ XBOX_OUTPUT_PIPE ].epAddr = 0x02; // XBOX output endpoint
         epInfo[ XBOX_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = 0;
+        epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = 0;
 
         rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         delay(200); // Give time for address change
 
         rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
 #ifdef DEBUG_USB_HOST
         Notify(PSTR("\r\nXbox Controller Connected\r\n"), 0x80);
 #endif
-        if (pFuncOnInit)
+        if(pFuncOnInit)
                 pFuncOnInit(); // Call the user function
         XboxConnected = true;
         bPollEnable = true;
@@ -213,8 +214,9 @@ FailSetDevTblEntry:
 FailSetConfDescr:
 #ifdef DEBUG_USB_HOST
         NotifyFailSetConfDescr();
-        goto Fail;
 #endif
+        goto Fail;
+
 FailUnknownDevice:
 #ifdef DEBUG_USB_HOST
         NotifyFailUnknownDevice(VID, PID);
@@ -240,7 +242,7 @@ uint8_t XBOXOLD::Release() {
 }
 
 uint8_t XBOXOLD::Poll() {
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
         uint16_t BUFFER_SIZE = EP_MAXPKTSIZE;
         pUsb->inTransfer(bAddress, epInfo[ XBOX_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1
@@ -254,8 +256,8 @@ uint8_t XBOXOLD::Poll() {
 void XBOXOLD::readReport() {
         ButtonState = readBuf[2];
 
-        for (uint8_t i = 0; i < sizeof(buttonValues); i++)
-            buttonValues[i] = readBuf[i + 4]; // A, B, X, Y, BLACK, WHITE, L1, and R1
+        for(uint8_t i = 0; i < sizeof (buttonValues); i++)
+                buttonValues[i] = readBuf[i + 4]; // A, B, X, Y, BLACK, WHITE, L1, and R1
 
         hatValue[LeftHatX] = (int16_t)(((uint16_t)readBuf[12] << 8) | readBuf[13]);
         hatValue[LeftHatY] = (int16_t)(((uint16_t)readBuf[14] << 8) | readBuf[15]);
@@ -265,23 +267,23 @@ void XBOXOLD::readReport() {
         //Notify(PSTR("\r\nButtonState"), 0x80);
         //PrintHex<uint8_t>(ButtonState, 0x80);
 
-        if (ButtonState != OldButtonState || memcmp(buttonValues, oldButtonValues, sizeof(buttonValues)) != 0) {
+        if(ButtonState != OldButtonState || memcmp(buttonValues, oldButtonValues, sizeof (buttonValues)) != 0) {
                 ButtonClickState = ButtonState & ~OldButtonState; // Update click state variable
                 OldButtonState = ButtonState;
 
-                for (uint8_t i = 0; i < sizeof(buttonValues); i++) {
-                    if (oldButtonValues[i] == 0 && buttonValues[i] != 0)
-                        buttonClicked[i] = true; // Update A, B, X, Y, BLACK, WHITE, L1, and R1 click state
-                    oldButtonValues[i] = buttonValues[i];
+                for(uint8_t i = 0; i < sizeof (buttonValues); i++) {
+                        if(oldButtonValues[i] == 0 && buttonValues[i] != 0)
+                                buttonClicked[i] = true; // Update A, B, X, Y, BLACK, WHITE, L1, and R1 click state
+                        oldButtonValues[i] = buttonValues[i];
                 }
         }
 }
 
 void XBOXOLD::printReport(uint16_t length) { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox controller
 #ifdef PRINTREPORT
-        if (readBuf == NULL)
+        if(readBuf == NULL)
                 return;
-        for (uint8_t i = 0; i < length; i++) {
+        for(uint8_t i = 0; i < length; i++) {
                 D_PrintHex<uint8_t > (readBuf[i], 0x80);
                 Notify(PSTR(" "), 0x80);
         }
@@ -289,21 +291,21 @@ void XBOXOLD::printReport(uint16_t length) { //Uncomment "#define PRINTREPORT" t
 #endif
 }
 
-uint8_t XBOXOLD::getButtonPress(Button b) {
-        uint8_t button = pgm_read_byte(&XBOXOLDBUTTONS[(uint8_t)b]);
-        if (b == A || b == B || b == X || b == Y || b == BLACK || b == WHITE || b == L1 || b == R1) // A, B, X, Y, BLACK, WHITE, L1, and R1 are analog buttons
-            return buttonValues[button]; // Analog buttons
+uint8_t XBOXOLD::getButtonPress(ButtonEnum b) {
+        uint8_t button = pgm_read_byte(&XBOXOLD_BUTTONS[(uint8_t)b]);
+        if(b == A || b == B || b == X || b == Y || b == BLACK || b == WHITE || b == L1 || b == R1) // A, B, X, Y, BLACK, WHITE, L1, and R1 are analog buttons
+                return buttonValues[button]; // Analog buttons
         return (ButtonState & button); // Digital buttons
 }
 
-bool XBOXOLD::getButtonClick(Button b) {
-        uint8_t button = pgm_read_byte(&XBOXOLDBUTTONS[(uint8_t)b]);
-        if (b == A || b == B || b == X || b == Y || b == BLACK || b == WHITE || b == L1 || b == R1) { // A, B, X, Y, BLACK, WHITE, L1, and R1 are analog buttons
-            if (buttonClicked[button]) {
-                buttonClicked[button] = false;
-                return true;
-            }
-            return false;
+bool XBOXOLD::getButtonClick(ButtonEnum b) {
+        uint8_t button = pgm_read_byte(&XBOXOLD_BUTTONS[(uint8_t)b]);
+        if(b == A || b == B || b == X || b == Y || b == BLACK || b == WHITE || b == L1 || b == R1) { // A, B, X, Y, BLACK, WHITE, L1, and R1 are analog buttons
+                if(buttonClicked[button]) {
+                        buttonClicked[button] = false;
+                        return true;
+                }
+                return false;
         }
 
         bool click = (ButtonClickState & button);
@@ -311,7 +313,7 @@ bool XBOXOLD::getButtonClick(Button b) {
         return click;
 }
 
-int16_t XBOXOLD::getAnalogHat(AnalogHat a) {
+int16_t XBOXOLD::getAnalogHat(AnalogHatEnum a) {
         return hatValue[a];
 }
 
@@ -332,4 +334,4 @@ void XBOXOLD::setRumbleOn(uint8_t lValue, uint8_t rValue) {
         writeBuf[5] = lValue; // big weight
 
         XboxCommand(writeBuf, 6);
-}
\ No newline at end of file
+}
diff --git a/XBOXOLD.h b/XBOXOLD.h
index 02eadbbc..b3c0ccb2 100644
--- a/XBOXOLD.h
+++ b/XBOXOLD.h
@@ -106,18 +106,17 @@ public:
 
         /** @name Xbox Controller functions */
         /**
-         * getButtonPress(Button b) will return true as long as the button is held down.
+         * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
          *
-         * While getButtonClick(Button b) will only return it once.
+         * While getButtonClick(ButtonEnum b) will only return it once.
          *
-         * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
-         * but if you need to drive a robot forward you would use getButtonPress(Button b).
-         * @param  b          ::Button to read.
-         * @return            getButtonClick(Button b) will return a bool, but getButtonPress(Button b)
-         * will return a byte if reading ::L2 or ::R2.
+         * So you instance if you need to increase a variable once you would use getButtonClick(ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
+         * @return            getButtonClick(ButtonEnum b) will return a bool, while getButtonPress(ButtonEnum b) will return a byte if reading ::L2 or ::R2.
          */
-        uint8_t getButtonPress(Button b);
-        bool getButtonClick(Button b);
+        uint8_t getButtonPress(ButtonEnum b);
+        bool getButtonClick(ButtonEnum b);
         /**@}*/
 
         /** @name Xbox Controller functions */
@@ -126,7 +125,7 @@ public:
          * @param  a          Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY.
          * @return            Returns a signed 16-bit integer.
          */
-        int16_t getAnalogHat(AnalogHat a);
+        int16_t getAnalogHat(AnalogHatEnum a);
 
         /** Turn rumble off the controller. */
         void setRumbleOff() {
@@ -189,4 +188,4 @@ private:
         /* Private commands */
         void XboxCommand(uint8_t* data, uint16_t nbytes);
 };
-#endif
\ No newline at end of file
+#endif
diff --git a/XBOXRECV.cpp b/XBOXRECV.cpp
index 5f54e059..144a4aa4 100644
--- a/XBOXRECV.cpp
+++ b/XBOXRECV.cpp
@@ -26,20 +26,21 @@ XBOXRECV::XBOXRECV(USB *p) :
 pUsb(p), // pointer to USB class instance - mandatory
 bAddress(0), // device address - mandatory
 bPollEnable(false) { // don't start polling before dongle is connected
-        for (uint8_t i = 0; i < XBOX_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < XBOX_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
                 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
         }
 
-        if (pUsb) // register in USB subsystem
+        if(pUsb) // register in USB subsystem
                 pUsb->RegisterDeviceClass(this); //set devConfig[] entry
 }
 
 uint8_t XBOXRECV::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -50,7 +51,7 @@ uint8_t XBOXRECV::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nXBOXRECV Init"), 0x80);
 #endif
 
-        if (bAddress) { // Check if address has already been assigned to an instance
+        if(bAddress) { // Check if address has already been assigned to an instance
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress in use"), 0x80);
 #endif
@@ -59,14 +60,14 @@ uint8_t XBOXRECV::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p = addrPool.GetUsbDevicePtr(0); // Get pointer to pseudo device with address 0 assigned
 
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nepinfo is null"), 0x80);
 #endif
@@ -81,13 +82,13 @@ uint8_t XBOXRECV::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p->epinfo = oldep_ptr; // Restore p->epinfo
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
-        VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
-        PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
+        VID = udd->idVendor;
+        PID = udd->idProduct;
 
-        if ((VID != XBOX_VID && VID != MADCATZ_VID) || (PID != XBOX_WIRELESS_RECEIVER_PID && PID != XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID)) { // Check if it's a Xbox receiver using the Vendor ID and Product ID
+        if((VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID) || (PID != XBOX_WIRELESS_RECEIVER_PID && PID != XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID)) { // Check if it's a Xbox receiver using the Vendor ID and Product ID
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nYou'll need a wireless receiver for this libary to work"), 0x80);
 #endif
@@ -96,15 +97,14 @@ uint8_t XBOXRECV::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
 
         bAddress = addrPool.AllocAddress(parent, false, port); // Allocate new address according to device class
 
-        if (!bAddress) {
+        if(!bAddress) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nOut of address space"), 0x80);
 #endif
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
         }
 
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; // Extract Max Packet Size from device descriptor
-        epInfo[1].epAddr = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; // Steal and abuse from epInfo structure to save memory
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0; // Extract Max Packet Size from device descriptor
 
         delay(20); // Wait a little before resetting device
 
@@ -115,13 +115,13 @@ FailGetDevDescr:
 #ifdef DEBUG_USB_HOST
         NotifyFailGetDevDescr(rcode);
 #endif
-        if (rcode != hrJERR)
+        if(rcode != hrJERR)
                 rcode = USB_ERROR_FailGetDevDescr;
         goto Fail;
 
 FailUnknownDevice:
 #ifdef DEBUG_USB_HOST
-        NotifyFailUnknownDevice(VID,PID);
+        NotifyFailUnknownDevice(VID, PID);
 #endif
         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
@@ -136,8 +136,6 @@ Fail:
 
 uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t rcode;
-        uint8_t num_of_conf = epInfo[1].epAddr; // Number of configurations
-        epInfo[1].epAddr = 0;
 
         AddressPool &addrPool = pUsb->GetAddressPool();
 #ifdef EXTRADEBUG
@@ -145,7 +143,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 #endif
         UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
 
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
@@ -155,7 +153,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         delay(300); // Assign new address to the device
 
         rcode = pUsb->setAddr(0, 0, bAddress); // Assign new address to the device
-        if (rcode) {
+        if(rcode) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nsetAddr: "), 0x80);
                 D_PrintHex<uint8_t > (rcode, 0x80);
@@ -171,7 +169,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         p->lowspeed = false;
 
         p = addrPool.GetUsbDevicePtr(bAddress); // Get pointer to assigned address record
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
@@ -181,7 +179,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         p->lowspeed = lowspeed;
 
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); // Assign epInfo to epinfo pointer - only EP0 is known
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         /* The application will work in reduced host mode, so we can save program and data
@@ -193,62 +191,62 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         epInfo[ XBOX_INPUT_PIPE_1 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_INPUT_PIPE_1 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_INPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_INPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_INPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_INPUT_PIPE_1 ].bmSndToggle = 0;
+        epInfo[ XBOX_INPUT_PIPE_1 ].bmRcvToggle = 0;
         epInfo[ XBOX_OUTPUT_PIPE_1 ].epAddr = 0x01; // XBOX 360 output endpoint - poll interval 8ms
         epInfo[ XBOX_OUTPUT_PIPE_1 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_OUTPUT_PIPE_1 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_OUTPUT_PIPE_1 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_OUTPUT_PIPE_1 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_OUTPUT_PIPE_1 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_OUTPUT_PIPE_1 ].bmSndToggle = 0;
+        epInfo[ XBOX_OUTPUT_PIPE_1 ].bmRcvToggle = 0;
 
         epInfo[ XBOX_INPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 report endpoint - poll interval 1ms
         epInfo[ XBOX_INPUT_PIPE_2 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_INPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_INPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_INPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_INPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_INPUT_PIPE_2 ].bmSndToggle = 0;
+        epInfo[ XBOX_INPUT_PIPE_2 ].bmRcvToggle = 0;
         epInfo[ XBOX_OUTPUT_PIPE_2 ].epAddr = 0x03; // XBOX 360 output endpoint - poll interval 8ms
         epInfo[ XBOX_OUTPUT_PIPE_2 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_OUTPUT_PIPE_2 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_OUTPUT_PIPE_2 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_OUTPUT_PIPE_2 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_OUTPUT_PIPE_2 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_OUTPUT_PIPE_2 ].bmSndToggle = 0;
+        epInfo[ XBOX_OUTPUT_PIPE_2 ].bmRcvToggle = 0;
 
         epInfo[ XBOX_INPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 report endpoint - poll interval 1ms
         epInfo[ XBOX_INPUT_PIPE_3 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_INPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_INPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_INPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_INPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_INPUT_PIPE_3 ].bmSndToggle = 0;
+        epInfo[ XBOX_INPUT_PIPE_3 ].bmRcvToggle = 0;
         epInfo[ XBOX_OUTPUT_PIPE_3 ].epAddr = 0x05; // XBOX 360 output endpoint - poll interval 8ms
         epInfo[ XBOX_OUTPUT_PIPE_3 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_OUTPUT_PIPE_3 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_OUTPUT_PIPE_3 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_OUTPUT_PIPE_3 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_OUTPUT_PIPE_3 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_OUTPUT_PIPE_3 ].bmSndToggle = 0;
+        epInfo[ XBOX_OUTPUT_PIPE_3 ].bmRcvToggle = 0;
 
         epInfo[ XBOX_INPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 report endpoint - poll interval 1ms
         epInfo[ XBOX_INPUT_PIPE_4 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_INPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_INPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_INPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_INPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_INPUT_PIPE_4 ].bmSndToggle = 0;
+        epInfo[ XBOX_INPUT_PIPE_4 ].bmRcvToggle = 0;
         epInfo[ XBOX_OUTPUT_PIPE_4 ].epAddr = 0x07; // XBOX 360 output endpoint - poll interval 8ms
         epInfo[ XBOX_OUTPUT_PIPE_4 ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_OUTPUT_PIPE_4 ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_OUTPUT_PIPE_4 ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_OUTPUT_PIPE_4 ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_OUTPUT_PIPE_4 ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_OUTPUT_PIPE_4 ].bmSndToggle = 0;
+        epInfo[ XBOX_OUTPUT_PIPE_4 ].bmRcvToggle = 0;
 
         rcode = pUsb->setEpInfoEntry(bAddress, 9, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         delay(200); //Give time for address change
 
         rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
 #ifdef DEBUG_USB_HOST
@@ -260,12 +258,6 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         return 0; // Successful configuration
 
         /* Diagnostic messages */
-FailGetDevDescr:
-#ifdef DEBUG_USB_HOST
-        NotifyFailGetDevDescr();
-        goto Fail;
-#endif
-
 FailSetDevTblEntry:
 #ifdef DEBUG_USB_HOST
         NotifyFailSetDevTblEntry();
@@ -276,7 +268,6 @@ FailSetConfDescr:
 #ifdef DEBUG_USB_HOST
         NotifyFailSetConfDescr();
 #endif
-        goto Fail;
 
 Fail:
 #ifdef DEBUG_USB_HOST
@@ -290,7 +281,7 @@ Fail:
 /* Performs a cleanup after failed Init() attempt */
 uint8_t XBOXRECV::Release() {
         XboxReceiverConnected = false;
-        for (uint8_t i = 0; i < 4; i++)
+        for(uint8_t i = 0; i < 4; i++)
                 Xbox360Connected[i] = 0x00;
         pUsb->GetAddressPool().FreeAddress(bAddress);
         bAddress = 0;
@@ -299,28 +290,28 @@ uint8_t XBOXRECV::Release() {
 }
 
 uint8_t XBOXRECV::Poll() {
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
-        if (!checkStatusTimer || ((millis() - checkStatusTimer) > 3000)) { // Run checkStatus every 3 seconds
+        if(!checkStatusTimer || ((millis() - checkStatusTimer) > 3000)) { // Run checkStatus every 3 seconds
                 checkStatusTimer = millis();
                 checkStatus();
         }
 
         uint8_t inputPipe;
         uint16_t bufferSize;
-        for (uint8_t i = 0; i < 4; i++) {
-                if (i == 0)
+        for(uint8_t i = 0; i < 4; i++) {
+                if(i == 0)
                         inputPipe = XBOX_INPUT_PIPE_1;
-                else if (i == 1)
+                else if(i == 1)
                         inputPipe = XBOX_INPUT_PIPE_2;
-                else if (i == 2)
+                else if(i == 2)
                         inputPipe = XBOX_INPUT_PIPE_3;
                 else
                         inputPipe = XBOX_INPUT_PIPE_4;
 
                 bufferSize = EP_MAXPKTSIZE; // This is the maximum number of bytes we want to receive
                 pUsb->inTransfer(bAddress, epInfo[ inputPipe ].epAddr, &bufferSize, readBuf);
-                if (bufferSize > 0) { // The number of received bytes
+                if(bufferSize > 0) { // The number of received bytes
 #ifdef EXTRADEBUG
                         Notify(PSTR("Bytes Received: "), 0x80);
                         D_PrintHex<uint16_t > (bufferSize, 0x80);
@@ -336,19 +327,19 @@ uint8_t XBOXRECV::Poll() {
 }
 
 void XBOXRECV::readReport(uint8_t controller) {
-        if (readBuf == NULL)
+        if(readBuf == NULL)
                 return;
         // This report is send when a controller is connected and disconnected
-        if (readBuf[0] == 0x08 && readBuf[1] != Xbox360Connected[controller]) {
+        if(readBuf[0] == 0x08 && readBuf[1] != Xbox360Connected[controller]) {
                 Xbox360Connected[controller] = readBuf[1];
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("Controller "), 0x80);
                 Notify(controller, 0x80);
 #endif
-                if (Xbox360Connected[controller]) {
+                if(Xbox360Connected[controller]) {
 #ifdef DEBUG_USB_HOST
                         const char* str = 0;
-                        switch (readBuf[1]) {
+                        switch(readBuf[1]) {
                                 case 0x80: str = PSTR(" as controller\r\n");
                                         break;
                                 case 0x40: str = PSTR(" as headset\r\n");
@@ -368,15 +359,15 @@ void XBOXRECV::readReport(uint8_t controller) {
                 return;
         }
         // Controller status report
-        if (readBuf[1] == 0x00 && readBuf[3] & 0x13 && readBuf[4] >= 0x22) {
+        if(readBuf[1] == 0x00 && readBuf[3] & 0x13 && readBuf[4] >= 0x22) {
                 controllerStatus[controller] = ((uint16_t)readBuf[3] << 8) | readBuf[4];
                 return;
         }
-        if (readBuf[1] != 0x01) // Check if it's the correct report - the receiver also sends different status reports
+        if(readBuf[1] != 0x01) // Check if it's the correct report - the receiver also sends different status reports
                 return;
 
         // A controller must be connected if it's sending data
-        if (!Xbox360Connected[controller])
+        if(!Xbox360Connected[controller])
                 Xbox360Connected[controller] |= 0x80;
 
         ButtonState[controller] = (uint32_t)(readBuf[9] | ((uint16_t)readBuf[8] << 8) | ((uint32_t)readBuf[7] << 16) | ((uint32_t)readBuf[6] << 24));
@@ -389,12 +380,12 @@ void XBOXRECV::readReport(uint8_t controller) {
         //Notify(PSTR("\r\nButtonState: "), 0x80);
         //PrintHex<uint32_t>(ButtonState[controller], 0x80);
 
-        if (ButtonState[controller] != OldButtonState[controller]) {
+        if(ButtonState[controller] != OldButtonState[controller]) {
                 buttonStateChanged[controller] = true;
                 ButtonClickState[controller] = (ButtonState[controller] >> 16) & ((~OldButtonState[controller]) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2
-                if (((uint8_t)OldButtonState[controller]) == 0 && ((uint8_t)ButtonState[controller]) != 0) // The L2 and R2 buttons are special as they are analog buttons
+                if(((uint8_t)OldButtonState[controller]) == 0 && ((uint8_t)ButtonState[controller]) != 0) // The L2 and R2 buttons are special as they are analog buttons
                         R2Clicked[controller] = true;
-                if ((uint8_t)(OldButtonState[controller] >> 8) == 0 && (uint8_t)(ButtonState[controller] >> 8) != 0)
+                if((uint8_t)(OldButtonState[controller] >> 8) == 0 && (uint8_t)(ButtonState[controller] >> 8) != 0)
                         L2Clicked[controller] = true;
                 OldButtonState[controller] = ButtonState[controller];
         }
@@ -402,12 +393,12 @@ void XBOXRECV::readReport(uint8_t controller) {
 
 void XBOXRECV::printReport(uint8_t controller, uint8_t nBytes) { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller
 #ifdef PRINTREPORT
-        if (readBuf == NULL)
+        if(readBuf == NULL)
                 return;
         Notify(PSTR("Controller "), 0x80);
         Notify(controller, 0x80);
         Notify(PSTR(": "), 0x80);
-        for (uint8_t i = 0; i < nBytes; i++) {
+        for(uint8_t i = 0; i < nBytes; i++) {
                 D_PrintHex<uint8_t > (readBuf[i], 0x80);
                 Notify(PSTR(" "), 0x80);
         }
@@ -415,35 +406,35 @@ void XBOXRECV::printReport(uint8_t controller, uint8_t nBytes) { //Uncomment "#d
 #endif
 }
 
-uint8_t XBOXRECV::getButtonPress(Button b, uint8_t controller) {
-        if (b == L2) // These are analog buttons
+uint8_t XBOXRECV::getButtonPress(ButtonEnum b, uint8_t controller) {
+        if(b == L2) // These are analog buttons
                 return (uint8_t)(ButtonState[controller] >> 8);
-        else if (b == R2)
+        else if(b == R2)
                 return (uint8_t)ButtonState[controller];
-        return (bool)(ButtonState[controller] & ((uint32_t)pgm_read_word(&XBOXBUTTONS[(uint8_t)b]) << 16));
+        return (bool)(ButtonState[controller] & ((uint32_t)pgm_read_word(&XBOX_BUTTONS[(uint8_t)b]) << 16));
 }
 
-bool XBOXRECV::getButtonClick(Button b, uint8_t controller) {
-        if (b == L2) {
-                if (L2Clicked[controller]) {
+bool XBOXRECV::getButtonClick(ButtonEnum b, uint8_t controller) {
+        if(b == L2) {
+                if(L2Clicked[controller]) {
                         L2Clicked[controller] = false;
                         return true;
                 }
                 return false;
-        } else if (b == R2) {
-                if (R2Clicked[controller]) {
+        } else if(b == R2) {
+                if(R2Clicked[controller]) {
                         R2Clicked[controller] = false;
                         return true;
                 }
                 return false;
         }
-        uint16_t button = pgm_read_word(&XBOXBUTTONS[(uint8_t)b]);
+        uint16_t button = pgm_read_word(&XBOX_BUTTONS[(uint8_t)b]);
         bool click = (ButtonClickState[controller] & button);
         ButtonClickState[controller] &= ~button; // clear "click" event
         return click;
 }
 
-int16_t XBOXRECV::getAnalogHat(AnalogHat a, uint8_t controller) {
+int16_t XBOXRECV::getAnalogHat(AnalogHatEnum a, uint8_t controller) {
         return hatValue[controller][a];
 }
 
@@ -477,19 +468,28 @@ uint8_t XBOXRECV::getBatteryLevel(uint8_t controller) {
 }
 
 void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) {
-        uint8_t outputPipe;
-        if (controller == 0)
-                outputPipe = XBOX_OUTPUT_PIPE_1;
-        else if (controller == 1)
-                outputPipe = XBOX_OUTPUT_PIPE_2;
-        else if (controller == 2)
-                outputPipe = XBOX_OUTPUT_PIPE_3;
-        else
-                outputPipe = XBOX_OUTPUT_PIPE_4;
-
-        uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ outputPipe ].epAddr, nbytes, data);
 #ifdef EXTRADEBUG
-        if (rcode)
+        uint8_t rcode;
+#endif
+        uint8_t outputPipe;
+        switch(controller) {
+                case 0: outputPipe = XBOX_OUTPUT_PIPE_1;
+                        break;
+                case 1: outputPipe = XBOX_OUTPUT_PIPE_2;
+                        break;
+                case 2: outputPipe = XBOX_OUTPUT_PIPE_3;
+                        break;
+                case 3: outputPipe = XBOX_OUTPUT_PIPE_4;
+                        break;
+                default:
+                        return;
+        }
+#ifdef EXTRADEBUG
+        rcode =
+#endif
+                pUsb->outTransfer(bAddress, epInfo[ outputPipe ].epAddr, nbytes, data);
+#ifdef EXTRADEBUG
+        if(rcode)
                 Notify(PSTR("Error sending Xbox message\r\n"), 0x80);
 #endif
 }
@@ -512,16 +512,18 @@ void XBOXRECV::setLedRaw(uint8_t value, uint8_t controller) {
         XboxCommand(controller, writeBuf, 4);
 }
 
-void XBOXRECV::setLedOn(LED led, uint8_t controller) {
-        if (led != ALL) // All LEDs can't be on a the same time
-                setLedRaw(pgm_read_byte(&XBOXLEDS[(uint8_t)led]) + 4, controller);
+void XBOXRECV::setLedOn(LEDEnum led, uint8_t controller) {
+        if(led == OFF)
+                setLedRaw(0, controller);
+        else if(led != ALL) // All LEDs can't be on a the same time
+                setLedRaw(pgm_read_byte(&XBOX_LEDS[(uint8_t)led]) + 4, controller);
 }
 
-void XBOXRECV::setLedBlink(LED led, uint8_t controller) {
-        setLedRaw(pgm_read_byte(&XBOXLEDS[(uint8_t)led]), controller);
+void XBOXRECV::setLedBlink(LEDEnum led, uint8_t controller) {
+        setLedRaw(pgm_read_byte(&XBOX_LEDS[(uint8_t)led]), controller);
 }
 
-void XBOXRECV::setLedMode(LEDMode ledMode, uint8_t controller) { // This function is used to do some speciel LED stuff the controller supports
+void XBOXRECV::setLedMode(LEDModeEnum ledMode, uint8_t controller) { // This function is used to do some speciel LED stuff the controller supports
         setLedRaw((uint8_t)ledMode, controller);
 }
 
@@ -530,14 +532,14 @@ Thanks to BusHound from Perisoft.net for the Windows USB Analysis output
 Found by timstamp.co.uk
  */
 void XBOXRECV::checkStatus() {
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return;
         // Get controller info
         writeBuf[0] = 0x08;
         writeBuf[1] = 0x00;
         writeBuf[2] = 0x0f;
         writeBuf[3] = 0xc0;
-        for (uint8_t i = 0; i < 4; i++) {
+        for(uint8_t i = 0; i < 4; i++) {
                 XboxCommand(i, writeBuf, 4);
         }
         // Get battery status
@@ -545,8 +547,8 @@ void XBOXRECV::checkStatus() {
         writeBuf[1] = 0x00;
         writeBuf[2] = 0x00;
         writeBuf[3] = 0x40;
-        for (uint8_t i = 0; i < 4; i++) {
-                if (Xbox360Connected[i])
+        for(uint8_t i = 0; i < 4; i++) {
+                if(Xbox360Connected[i])
                         XboxCommand(i, writeBuf, 4);
         }
 }
@@ -564,18 +566,18 @@ void XBOXRECV::setRumbleOn(uint8_t lValue, uint8_t rValue, uint8_t controller) {
 }
 
 void XBOXRECV::onInit(uint8_t controller) {
-        if (pFuncOnInit)
+        if(pFuncOnInit)
                 pFuncOnInit(); // Call the user function
         else {
-                LED led;
-                if (controller == 0)
-                    led = LED1;
-                else if (controller == 1)
-                    led = LED2;
-                else if (controller == 2)
-                    led = LED3;
+                LEDEnum led;
+                if(controller == 0)
+                        led = LED1;
+                else if(controller == 1)
+                        led = LED2;
+                else if(controller == 2)
+                        led = LED3;
                 else
-                    led = LED4;
+                        led = LED4;
                 setLedOn(led, controller);
         }
 }
diff --git a/XBOXRECV.h b/XBOXRECV.h
index eee75cda..c3964991 100644
--- a/XBOXRECV.h
+++ b/XBOXRECV.h
@@ -120,19 +120,18 @@ public:
 
         /** @name Xbox Controller functions */
         /**
-         * getButtonPress(uint8_t controller, Button b) will return true as long as the button is held down.
+         * getButtonPress(uint8_t controller, ButtonEnum b) will return true as long as the button is held down.
          *
-         * While getButtonClick(uint8_t controller, Button b) will only return it once.
+         * While getButtonClick(uint8_t controller, ButtonEnum b) will only return it once.
          *
-         * So you instance if you need to increase a variable once you would use getButtonClick(uint8_t controller, Button b),
-         * but if you need to drive a robot forward you would use getButtonPress(uint8_t controller, Button b).
-         * @param  b          ::Button to read.
+         * So you instance if you need to increase a variable once you would use getButtonClick(uint8_t controller, ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(uint8_t controller, ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
          * @param  controller The controller to read from. Default to 0.
-         * @return            getButtonClick(uint8_t controller, Button b) will return a bool, but getButtonPress(uint8_t controller, Button b)
-         * will return a byte if reading ::L2 or ::R2.
+         * @return            getButtonClick(uint8_t controller, ButtonEnum b) will return a bool, while getButtonPress(uint8_t controller, ButtonEnum b) will return a byte if reading ::L2 or ::R2.
          */
-        uint8_t getButtonPress(Button b, uint8_t controller = 0);
-        bool getButtonClick(Button b, uint8_t controller = 0);
+        uint8_t getButtonPress(ButtonEnum b, uint8_t controller = 0);
+        bool getButtonClick(ButtonEnum b, uint8_t controller = 0);
         /**@}*/
 
         /** @name Xbox Controller functions */
@@ -142,7 +141,7 @@ public:
          * @param  controller The controller to read from. Default to 0.
          * @return            Returns a signed 16-bit integer.
          */
-        int16_t getAnalogHat(AnalogHat a, uint8_t controller = 0);
+        int16_t getAnalogHat(AnalogHatEnum a, uint8_t controller = 0);
 
         /**
          * Used to disconnect any of the controllers.
@@ -174,7 +173,7 @@ public:
          */
         void setRumbleOn(uint8_t lValue, uint8_t rValue, uint8_t controller = 0);
         /**
-         * Set LED value. Without using the ::LED or ::LEDMode enum.
+         * Set LED value. Without using the ::LEDEnum or ::LEDModeEnum.
          * @param value      See:
          * setLedOff(uint8_t controller), setLedOn(uint8_t controller, LED l),
          * setLedBlink(uint8_t controller, LED l), and setLedMode(uint8_t controller, LEDMode lm).
@@ -190,23 +189,23 @@ public:
                 setLedRaw(0, controller);
         };
         /**
-         * Turn on a LED by using the ::LED enum.
-         * @param l          ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller.
+         * Turn on a LED by using ::LEDEnum.
+         * @param l          ::OFF, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller.
          * @param controller The controller to write to. Default to 0.
          */
-        void setLedOn(LED l, uint8_t controller = 0);
+        void setLedOn(LEDEnum l, uint8_t controller = 0);
         /**
-         * Turn on a LED by using the ::LED enum.
+         * Turn on a LED by using ::LEDEnum.
          * @param l          ::ALL, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller.
          * @param controller The controller to write to. Default to 0.
          */
-        void setLedBlink(LED l, uint8_t controller = 0);
+        void setLedBlink(LEDEnum l, uint8_t controller = 0);
         /**
          * Used to set special LED modes supported by the Xbox controller.
-         * @param lm         See ::LEDMode.
+         * @param lm         See ::LEDModeEnum.
          * @param controller The controller to write to. Default to 0.
          */
-        void setLedMode(LEDMode lm, uint8_t controller = 0);
+        void setLedMode(LEDModeEnum lm, uint8_t controller = 0);
         /**
          * Used to get the battery level from the controller.
          * @param  controller The controller to read from. Default to 0.
@@ -277,4 +276,4 @@ private:
         void XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes);
         void checkStatus();
 };
-#endif
\ No newline at end of file
+#endif
diff --git a/XBOXUSB.cpp b/XBOXUSB.cpp
index 1e460347..4128d231 100644
--- a/XBOXUSB.cpp
+++ b/XBOXUSB.cpp
@@ -24,19 +24,20 @@ XBOXUSB::XBOXUSB(USB *p) :
 pUsb(p), // pointer to USB class instance - mandatory
 bAddress(0), // device address - mandatory
 bPollEnable(false) { // don't start polling before dongle is connected
-        for (uint8_t i = 0; i < XBOX_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < XBOX_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
                 epInfo[i].bmNakPower = (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
         }
 
-        if (pUsb) // register in USB subsystem
+        if(pUsb) // register in USB subsystem
                 pUsb->RegisterDeviceClass(this); //set devConfig[] entry
 }
 
 uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -49,7 +50,7 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nXBOXUSB Init"), 0x80);
 #endif
         // check if address has already been assigned to an instance
-        if (bAddress) {
+        if(bAddress) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress in use"), 0x80);
 #endif
@@ -59,14 +60,14 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p) {
+        if(!p) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nAddress not found"), 0x80);
 #endif
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nepinfo is null"), 0x80);
 #endif
@@ -86,39 +87,39 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
-        VID = ((USB_DEVICE_DESCRIPTOR*)buf)->idVendor;
-        PID = ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct;
+        VID = udd->idVendor;
+        PID = udd->idProduct;
 
-        if (VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID && VID != GAMESTOP_VID) // Check VID
+        if(VID != XBOX_VID && VID != MADCATZ_VID && VID != JOYTECH_VID && VID != GAMESTOP_VID) // Check VID
                 goto FailUnknownDevice;
-        if (PID == XBOX_WIRELESS_PID) {
+        if(PID == XBOX_WIRELESS_PID) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nYou have plugged in a wireless Xbox 360 controller - it doesn't support USB communication"), 0x80);
 #endif
                 goto FailUnknownDevice;
-        } else if (PID == XBOX_WIRELESS_RECEIVER_PID || PID == XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) {
+        } else if(PID == XBOX_WIRELESS_RECEIVER_PID || PID == XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID) {
 #ifdef DEBUG_USB_HOST
                 Notify(PSTR("\r\nThis library only supports Xbox 360 controllers via USB"), 0x80);
 #endif
                 goto FailUnknownDevice;
-        } else if (PID != XBOX_WIRED_PID && PID != GAMESTOP_WIRED_PID) // Check PID
+        } else if(PID != XBOX_WIRED_PID && PID != MADCATZ_WIRED_PID && PID != GAMESTOP_WIRED_PID && PID != AFTERGLOW_WIRED_PID) // Check PID
                 goto FailUnknownDevice;
 
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -132,20 +133,20 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         Notify(PSTR("\r\nAddr: "), 0x80);
         D_PrintHex<uint8_t > (bAddress, 0x80);
 #endif
-        delay(300); // Spec says you should wait at least 200ms
+        //delay(300); // Spec says you should wait at least 200ms
 
         p->lowspeed = false;
 
         //get pointer to assigned address record
         p = addrPool.GetUsbDevicePtr(bAddress);
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
         // Assign epInfo to epinfo pointer - only EP0 is known
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         /* The application will work in reduced host mode, so we can save program and data
@@ -157,23 +158,23 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         epInfo[ XBOX_INPUT_PIPE ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_INPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_INPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_INPUT_PIPE ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_INPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_INPUT_PIPE ].bmSndToggle = 0;
+        epInfo[ XBOX_INPUT_PIPE ].bmRcvToggle = 0;
         epInfo[ XBOX_OUTPUT_PIPE ].epAddr = 0x02; // XBOX 360 output endpoint
         epInfo[ XBOX_OUTPUT_PIPE ].epAttribs = EP_INTERRUPT;
         epInfo[ XBOX_OUTPUT_PIPE ].bmNakPower = USB_NAK_NOWAIT; // Only poll once for interrupt endpoints
         epInfo[ XBOX_OUTPUT_PIPE ].maxPktSize = EP_MAXPKTSIZE;
-        epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = bmSNDTOG0;
-        epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = bmRCVTOG0;
+        epInfo[ XBOX_OUTPUT_PIPE ].bmSndToggle = 0;
+        epInfo[ XBOX_OUTPUT_PIPE ].bmRcvToggle = 0;
 
         rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         delay(200); // Give time for address change
 
         rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
 #ifdef DEBUG_USB_HOST
@@ -200,8 +201,9 @@ FailSetDevTblEntry:
 FailSetConfDescr:
 #ifdef DEBUG_USB_HOST
         NotifyFailSetConfDescr();
-        goto Fail;
 #endif
+        goto Fail;
+
 FailUnknownDevice:
 #ifdef DEBUG_USB_HOST
         NotifyFailUnknownDevice(VID, PID);
@@ -227,7 +229,7 @@ uint8_t XBOXUSB::Release() {
 }
 
 uint8_t XBOXUSB::Poll() {
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
         uint16_t BUFFER_SIZE = EP_MAXPKTSIZE;
         pUsb->inTransfer(bAddress, epInfo[ XBOX_INPUT_PIPE ].epAddr, &BUFFER_SIZE, readBuf); // input on endpoint 1
@@ -239,9 +241,9 @@ uint8_t XBOXUSB::Poll() {
 }
 
 void XBOXUSB::readReport() {
-        if (readBuf == NULL)
+        if(readBuf == NULL)
                 return;
-        if (readBuf[0] != 0x00 || readBuf[1] != 0x14) { // Check if it's the correct report - the controller also sends different status reports
+        if(readBuf[0] != 0x00 || readBuf[1] != 0x14) { // Check if it's the correct report - the controller also sends different status reports
                 return;
         }
 
@@ -255,11 +257,11 @@ void XBOXUSB::readReport() {
         //Notify(PSTR("\r\nButtonState"), 0x80);
         //PrintHex<uint32_t>(ButtonState, 0x80);
 
-        if (ButtonState != OldButtonState) {
+        if(ButtonState != OldButtonState) {
                 ButtonClickState = (ButtonState >> 16) & ((~OldButtonState) >> 16); // Update click state variable, but don't include the two trigger buttons L2 and R2
-                if (((uint8_t)OldButtonState) == 0 && ((uint8_t)ButtonState) != 0) // The L2 and R2 buttons are special as they are analog buttons
+                if(((uint8_t)OldButtonState) == 0 && ((uint8_t)ButtonState) != 0) // The L2 and R2 buttons are special as they are analog buttons
                         R2Clicked = true;
-                if ((uint8_t)(OldButtonState >> 8) == 0 && (uint8_t)(ButtonState >> 8) != 0)
+                if((uint8_t)(OldButtonState >> 8) == 0 && (uint8_t)(ButtonState >> 8) != 0)
                         L2Clicked = true;
                 OldButtonState = ButtonState;
         }
@@ -267,9 +269,9 @@ void XBOXUSB::readReport() {
 
 void XBOXUSB::printReport() { //Uncomment "#define PRINTREPORT" to print the report send by the Xbox 360 Controller
 #ifdef PRINTREPORT
-        if (readBuf == NULL)
+        if(readBuf == NULL)
                 return;
-        for (uint8_t i = 0; i < XBOX_REPORT_BUFFER_SIZE; i++) {
+        for(uint8_t i = 0; i < XBOX_REPORT_BUFFER_SIZE; i++) {
                 D_PrintHex<uint8_t > (readBuf[i], 0x80);
                 Notify(PSTR(" "), 0x80);
         }
@@ -277,35 +279,35 @@ void XBOXUSB::printReport() { //Uncomment "#define PRINTREPORT" to print the rep
 #endif
 }
 
-uint8_t XBOXUSB::getButtonPress(Button b) {
-        if (b == L2) // These are analog buttons
+uint8_t XBOXUSB::getButtonPress(ButtonEnum b) {
+        if(b == L2) // These are analog buttons
                 return (uint8_t)(ButtonState >> 8);
-        else if (b == R2)
+        else if(b == R2)
                 return (uint8_t)ButtonState;
-        return (bool)(ButtonState & ((uint32_t)pgm_read_word(&XBOXBUTTONS[(uint8_t)b]) << 16));
+        return (bool)(ButtonState & ((uint32_t)pgm_read_word(&XBOX_BUTTONS[(uint8_t)b]) << 16));
 }
 
-bool XBOXUSB::getButtonClick(Button b) {
-        if (b == L2) {
-                if (L2Clicked) {
+bool XBOXUSB::getButtonClick(ButtonEnum b) {
+        if(b == L2) {
+                if(L2Clicked) {
                         L2Clicked = false;
                         return true;
                 }
                 return false;
-        } else if (b == R2) {
-                if (R2Clicked) {
+        } else if(b == R2) {
+                if(R2Clicked) {
                         R2Clicked = false;
                         return true;
                 }
                 return false;
         }
-        uint16_t button = pgm_read_word(&XBOXBUTTONS[(uint8_t)b]);
+        uint16_t button = pgm_read_word(&XBOX_BUTTONS[(uint8_t)b]);
         bool click = (ButtonClickState & button);
         ButtonClickState &= ~button; // clear "click" event
         return click;
 }
 
-int16_t XBOXUSB::getAnalogHat(AnalogHat a) {
+int16_t XBOXUSB::getAnalogHat(AnalogHatEnum a) {
         return hatValue[a];
 }
 
@@ -323,16 +325,18 @@ void XBOXUSB::setLedRaw(uint8_t value) {
         XboxCommand(writeBuf, 3);
 }
 
-void XBOXUSB::setLedOn(LED led) {
-        if (led != ALL) // All LEDs can't be on a the same time
-                setLedRaw((pgm_read_byte(&XBOXLEDS[(uint8_t)led])) + 4);
+void XBOXUSB::setLedOn(LEDEnum led) {
+        if(led == OFF)
+                setLedRaw(0);
+        else if(led != ALL) // All LEDs can't be on a the same time
+                setLedRaw(pgm_read_byte(&XBOX_LEDS[(uint8_t)led]) + 4);
 }
 
-void XBOXUSB::setLedBlink(LED led) {
-        setLedRaw(pgm_read_byte(&XBOXLEDS[(uint8_t)led]));
+void XBOXUSB::setLedBlink(LEDEnum led) {
+        setLedRaw(pgm_read_byte(&XBOX_LEDS[(uint8_t)led]));
 }
 
-void XBOXUSB::setLedMode(LEDMode ledMode) { // This function is used to do some speciel LED stuff the controller supports
+void XBOXUSB::setLedMode(LEDModeEnum ledMode) { // This function is used to do some special LED stuff the controller supports
         setLedRaw((uint8_t)ledMode);
 }
 
@@ -350,7 +354,7 @@ void XBOXUSB::setRumbleOn(uint8_t lValue, uint8_t rValue) {
 }
 
 void XBOXUSB::onInit() {
-        if (pFuncOnInit)
+        if(pFuncOnInit)
                 pFuncOnInit(); // Call the user function
         else
                 setLedOn(LED1);
diff --git a/XBOXUSB.h b/XBOXUSB.h
index a11d55a1..aed330a2 100644
--- a/XBOXUSB.h
+++ b/XBOXUSB.h
@@ -33,16 +33,18 @@
 #define XBOX_OUTPUT_PIPE     2
 
 // PID and VID of the different devices
-#define XBOX_VID                                0x045E  // Microsoft Corporation
-#define MADCATZ_VID                             0x1BAD  // For unofficial Mad Catz controllers
-#define JOYTECH_VID                             0x162E  // For unofficial Joytech controllers
-#define GAMESTOP_VID                            0x0E6F  // Gamestop controller
+#define XBOX_VID                                0x045E // Microsoft Corporation
+#define MADCATZ_VID                             0x1BAD // For unofficial Mad Catz controllers
+#define JOYTECH_VID                             0x162E // For unofficial Joytech controllers
+#define GAMESTOP_VID                            0x0E6F // Gamestop controller
 
-#define XBOX_WIRED_PID                          0x028E  // Microsoft 360 Wired controller
-#define XBOX_WIRELESS_PID                       0x028F  // Wireless controller only support charging
-#define XBOX_WIRELESS_RECEIVER_PID              0x0719  // Microsoft Wireless Gaming Receiver
-#define XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID  0x0291  // Third party Wireless Gaming Receiver
-#define GAMESTOP_WIRED_PID                      0x0401  // Gamestop wired controller
+#define XBOX_WIRED_PID                          0x028E // Microsoft 360 Wired controller
+#define XBOX_WIRELESS_PID                       0x028F // Wireless controller only support charging
+#define XBOX_WIRELESS_RECEIVER_PID              0x0719 // Microsoft Wireless Gaming Receiver
+#define XBOX_WIRELESS_RECEIVER_THIRD_PARTY_PID  0x0291 // Third party Wireless Gaming Receiver
+#define MADCATZ_WIRED_PID                       0xF016 // Mad Catz wired controller
+#define GAMESTOP_WIRED_PID                      0x0401 // Gamestop wired controller
+#define AFTERGLOW_WIRED_PID                     0x0213 // Afterglow wired controller - it uses the same VID as a Gamestop controller
 
 #define XBOX_REPORT_BUFFER_SIZE 14 // Size of the input report buffer
 
@@ -104,24 +106,23 @@ public:
          * @return     Returns true if the device's VID and PID matches this driver.
          */
         virtual boolean VIDPIDOK(uint16_t vid, uint16_t pid) {
-                return ((vid == XBOX_VID || vid == MADCATZ_VID || vid == JOYTECH_VID || vid == GAMESTOP_VID) && (pid == XBOX_WIRED_PID || pid == GAMESTOP_WIRED_PID));
+                return ((vid == XBOX_VID || vid == MADCATZ_VID || vid == JOYTECH_VID || vid == GAMESTOP_VID) && (pid == XBOX_WIRED_PID || pid == MADCATZ_WIRED_PID || pid == GAMESTOP_WIRED_PID || pid == AFTERGLOW_WIRED_PID));
         };
         /**@}*/
 
         /** @name Xbox Controller functions */
         /**
-         * getButtonPress(Button b) will return true as long as the button is held down.
+         * getButtonPress(ButtonEnum b) will return true as long as the button is held down.
          *
-         * While getButtonClick(Button b) will only return it once.
+         * While getButtonClick(ButtonEnum b) will only return it once.
          *
-         * So you instance if you need to increase a variable once you would use getButtonClick(Button b),
-         * but if you need to drive a robot forward you would use getButtonPress(Button b).
-         * @param  b          ::Button to read.
-         * @return            getButtonClick(Button b) will return a bool, but getButtonPress(Button b)
-         * will return a byte if reading ::L2 or ::R2.
+         * So you instance if you need to increase a variable once you would use getButtonClick(ButtonEnum b),
+         * but if you need to drive a robot forward you would use getButtonPress(ButtonEnum b).
+         * @param  b          ::ButtonEnum to read.
+         * @return            getButtonClick(ButtonEnum b) will return a bool, while getButtonPress(ButtonEnum b) will return a byte if reading ::L2 or ::R2.
          */
-        uint8_t getButtonPress(Button b);
-        bool getButtonClick(Button b);
+        uint8_t getButtonPress(ButtonEnum b);
+        bool getButtonClick(ButtonEnum b);
         /**@}*/
 
         /** @name Xbox Controller functions */
@@ -130,7 +131,7 @@ public:
          * @param  a          Either ::LeftHatX, ::LeftHatY, ::RightHatX or ::RightHatY.
          * @return            Returns a signed 16-bit integer.
          */
-        int16_t getAnalogHat(AnalogHat a);
+        int16_t getAnalogHat(AnalogHatEnum a);
 
         /** Turn rumble off and all the LEDs on the controller. */
         void setAllOff() {
@@ -149,10 +150,10 @@ public:
          */
         void setRumbleOn(uint8_t lValue, uint8_t rValue);
         /**
-         * Set LED value. Without using the ::LED or ::LEDMode enum.
+         * Set LED value. Without using the ::LEDEnum or ::LEDModeEnum.
          * @param value      See:
-         * setLedOff(), setLedOn(LED l),
-         * setLedBlink(LED l), and setLedMode(LEDMode lm).
+         * setLedOff(), setLedOn(LEDEnum l),
+         * setLedBlink(LEDEnum l), and setLedMode(LEDModeEnum lm).
          */
         void setLedRaw(uint8_t value);
 
@@ -161,20 +162,20 @@ public:
                 setLedRaw(0);
         };
         /**
-         * Turn on a LED by using the ::LED enum.
-         * @param l          ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller.
+         * Turn on a LED by using ::LEDEnum.
+         * @param l          ::OFF, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller.
          */
-        void setLedOn(LED l);
+        void setLedOn(LEDEnum l);
         /**
-         * Turn on a LED by using the ::LED enum.
+         * Turn on a LED by using ::LEDEnum.
          * @param l          ::ALL, ::LED1, ::LED2, ::LED3 and ::LED4 is supported by the Xbox controller.
          */
-        void setLedBlink(LED l);
+        void setLedBlink(LEDEnum l);
         /**
          * Used to set special LED modes supported by the Xbox controller.
-         * @param lm         See ::LEDMode.
+         * @param lm         See ::LEDModeEnum.
          */
-        void setLedMode(LEDMode lm);
+        void setLedMode(LEDModeEnum lm);
 
         /**
          * Used to call your own function when the controller is successfully initialized.
diff --git a/address.h b/address.h
index d52fda73..158e287d 100644
--- a/address.h
+++ b/address.h
@@ -62,7 +62,7 @@ struct UsbDeviceAddress {
                         uint8_t bmAddress : 3; // device address/port number
                         uint8_t bmParent : 3; // parent hub address
                         uint8_t bmHub : 1; // hub flag
-                        uint8_t bmReserved : 1; // reserved, must be zerro
+                        uint8_t bmReserved : 1; // reserved, must be zero
                 } __attribute__((packed));
                 uint8_t devAddress;
         };
@@ -74,7 +74,7 @@ struct UsbDeviceAddress {
 
 struct UsbDevice {
         EpInfo *epinfo; // endpoint info pointer
-        uint8_t address; // address
+        UsbDeviceAddress address;
         uint8_t epcount; // number of endpoints
         bool lowspeed; // indicates if a device is the low speed one
         //	uint8_t			devclass;		// device class
@@ -104,47 +104,52 @@ class AddressPoolImpl : public AddressPool {
         // Initializes address pool entry
 
         void InitEntry(uint8_t index) {
-                thePool[index].address = 0;
+                thePool[index].address.devAddress = 0;
                 thePool[index].epcount = 1;
                 thePool[index].lowspeed = 0;
                 thePool[index].epinfo = &dev0ep;
         };
+
         // Returns thePool index for a given address
 
         uint8_t FindAddressIndex(uint8_t address = 0) {
                 for(uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++) {
-                        if(thePool[i].address == address)
+                        if(thePool[i].address.devAddress == address)
                                 return i;
                 }
                 return 0;
         };
+
         // Returns thePool child index for a given parent
 
         uint8_t FindChildIndex(UsbDeviceAddress addr, uint8_t start = 1) {
                 for(uint8_t i = (start < 1 || start >= MAX_DEVICES_ALLOWED) ? 1 : start; i < MAX_DEVICES_ALLOWED; i++) {
-                        if(((UsbDeviceAddress*) & thePool[i].address)->bmParent == addr.bmAddress)
+                        if(thePool[i].address.bmParent == addr.bmAddress)
                                 return i;
                 }
                 return 0;
         };
+
         // Frees address entry specified by index parameter
 
         void FreeAddressByIndex(uint8_t index) {
-                // Zerro field is reserved and should not be affected
+                // Zero field is reserved and should not be affected
                 if(index == 0)
                         return;
 
+                UsbDeviceAddress uda = thePool[index].address;
                 // If a hub was switched off all port addresses should be freed
-                if(((UsbDeviceAddress*) & thePool[index].address)->bmHub == 1) {
-                        for(uint8_t i = 1; (i = FindChildIndex(*((UsbDeviceAddress*) & thePool[index].address), i));)
+                if(uda.bmHub == 1) {
+                        for(uint8_t i = 1; (i = FindChildIndex(uda, i));)
                                 FreeAddressByIndex(i);
 
                         // If the hub had the last allocated address, hubCounter should be decremented
-                        if(hubCounter == ((UsbDeviceAddress*) & thePool[index].address)->bmAddress)
+                        if(hubCounter == uda.bmAddress)
                                 hubCounter--;
                 }
                 InitEntry(index);
         }
+
         // Initializes the whole address pool at once
 
         void InitAllAddresses() {
@@ -160,7 +165,7 @@ public:
                 // Zero address is reserved
                 InitEntry(0);
 
-                thePool[0].address = 0;
+                thePool[0].address.devAddress = 0;
                 thePool[0].epinfo = &dev0ep;
                 dev0ep.epAddr = 0;
                 dev0ep.maxPktSize = 8;
@@ -169,6 +174,7 @@ public:
 
                 InitAllAddresses();
         };
+
         // Returns a pointer to a specified address entry
 
         virtual UsbDevice* GetUsbDevicePtr(uint8_t addr) {
@@ -177,7 +183,7 @@ public:
 
                 uint8_t index = FindAddressIndex(addr);
 
-                return(!index) ? NULL : thePool + index;
+                return (!index) ? NULL : thePool + index;
         };
 
         // Performs an operation specified by pfunc for each addressed device
@@ -187,16 +193,19 @@ public:
                         return;
 
                 for(uint8_t i = 1; i < MAX_DEVICES_ALLOWED; i++)
-                        if(thePool[i].address)
+                        if(thePool[i].address.devAddress)
                                 pfunc(thePool + i);
         };
+
         // Allocates new address
 
         virtual uint8_t AllocAddress(uint8_t parent, bool is_hub = false, uint8_t port = 0) {
                 /* if (parent != 0 && port == 0)
                         USB_HOST_SERIAL.println("PRT:0"); */
-
-                if(parent > 127 || port > 7)
+                UsbDeviceAddress _parent;
+                _parent.devAddress = parent;
+                if(_parent.bmReserved || port > 7)
+                        //if(parent > 127 || port > 7)
                         return 0;
 
                 if(is_hub && hubCounter == 7)
@@ -208,21 +217,19 @@ public:
                 if(!index) // if empty entry is not found
                         return 0;
 
-                if(parent == 0) {
+                if(_parent.devAddress == 0) {
                         if(is_hub) {
-                                thePool[index].address = 0x41;
+                                thePool[index].address.devAddress = 0x41;
                                 hubCounter++;
                         } else
-                                thePool[index].address = 1;
+                                thePool[index].address.devAddress = 1;
 
-                        return thePool[index].address;
+                        return thePool[index].address.devAddress;
                 }
 
                 UsbDeviceAddress addr;
                 addr.devAddress = 0; // Ensure all bits are zero
-
-                addr.bmParent = ((UsbDeviceAddress*) & parent)->bmAddress;
-
+                addr.bmParent = _parent.bmAddress;
                 if(is_hub) {
                         addr.bmHub = 1;
                         addr.bmAddress = ++hubCounter;
@@ -230,7 +237,7 @@ public:
                         addr.bmHub = 0;
                         addr.bmAddress = port;
                 }
-                thePool[index].address = *((uint8_t*) & addr);
+                thePool[index].address = addr;
                 /*
                                 USB_HOST_SERIAL.print("Addr:");
                                 USB_HOST_SERIAL.print(addr.bmHub, HEX);
@@ -239,8 +246,9 @@ public:
                                 USB_HOST_SERIAL.print(".");
                                 USB_HOST_SERIAL.println(addr.bmAddress, HEX);
                  */
-                return thePool[index].address;
+                return thePool[index].address.devAddress;
         };
+
         // Empties pool entry
 
         virtual void FreeAddress(uint8_t addr) {
@@ -252,6 +260,7 @@ public:
                 uint8_t index = FindAddressIndex(addr);
                 FreeAddressByIndex(index);
         };
+
         // Returns number of hubs attached
         // It can be rather helpfull to find out if there are hubs attached than getting the exact number of hubs.
         //uint8_t GetNumHubs()
diff --git a/adk.cpp b/adk.cpp
index fd663101..1171da1c 100644
--- a/adk.cpp
+++ b/adk.cpp
@@ -13,7 +13,7 @@ Contact information
 Circuits At Home, LTD
 Web      :  http://www.circuitsathome.com
 e-mail   :  support@circuitsathome.com
-*/
+ */
 
 /* Google ADK interface */
 
@@ -42,7 +42,7 @@ bConfNum(0), //configuration number
 bNumEP(1), //if config descriptor needs to be parsed
 ready(false) {
         // initialize endpoint data structures
-        for (uint8_t i = 0; i < ADK_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < ADK_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
@@ -50,7 +50,7 @@ ready(false) {
         }//for(uint8_t i=0; i<ADK_MAX_ENDPOINTS; i++...
 
         // register in USB subsystem
-        if (pUsb) {
+        if(pUsb) {
                 pUsb->RegisterDeviceClass(this); //set devConfig[] entry
         }
 }
@@ -62,6 +62,7 @@ uint8_t ADK::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
 /* Connection initialization of an Android phone */
 uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t buf[sizeof (USB_DEVICE_DESCRIPTOR)];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         uint8_t num_of_conf; // number of configurations
         UsbDevice *p = NULL;
@@ -73,7 +74,7 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         USBTRACE("\r\nADK Init");
 
         // check if address has already been assigned to an instance
-        if (bAddress) {
+        if(bAddress) {
                 USBTRACE("\r\nAddress in use");
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
         }
@@ -81,12 +82,12 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p) {
+        if(!p) {
                 USBTRACE("\r\nAddress not found");
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
                 USBTRACE("epinfo is null\r\n");
                 return USB_ERROR_EPINFO_IS_NULL;
         }
@@ -105,7 +106,7 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode) {
+        if(rcode) {
                 goto FailGetDevDescr;
         }
 
@@ -113,11 +114,11 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         bAddress = addrPool.AllocAddress(parent, false, port);
 
         // Extract Max Packet Size from device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -127,13 +128,13 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         //USBTRACE2("\r\nAddr:", bAddress);
         // Spec says you should wait at least 200ms.
-        delay(300);
+        //delay(300);
 
         p->lowspeed = false;
 
         //get pointer to assigned address record
         p = addrPool.GetUsbDevicePtr(bAddress);
-        if (!p) {
+        if(!p) {
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
@@ -141,49 +142,49 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         // Assign epInfo to epinfo pointer - only EP0 is known
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
-        if (rcode) {
+        if(rcode) {
                 goto FailSetDevTblEntry;
         }
 
         //check if ADK device is already in accessory mode; if yes, configure and exit
-        if (((USB_DEVICE_DESCRIPTOR*)buf)->idVendor == ADK_VID &&
-                (((USB_DEVICE_DESCRIPTOR*)buf)->idProduct == ADK_PID || ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct == ADB_PID)) {
+        if(udd->idVendor == ADK_VID &&
+                (udd->idProduct == ADK_PID || udd->idProduct == ADB_PID)) {
                 USBTRACE("\r\nAcc.mode device detected");
                 /* go through configurations, find first bulk-IN, bulk-OUT EP, fill epInfo and quit */
-                num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
+                num_of_conf = udd->bNumConfigurations;
 
                 //USBTRACE2("\r\nNC:",num_of_conf);
-                for (uint8_t i = 0; i < num_of_conf; i++) {
+                for(uint8_t i = 0; i < num_of_conf; i++) {
                         ConfigDescParser < 0, 0, 0, 0 > confDescrParser(this);
                         delay(1);
                         rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
 #if defined(XOOM)
                         //added by Jaylen Scott Vanorden
-                        if (rcode) {
+                        if(rcode) {
                                 USBTRACE2("\r\nGot 1st bad code for config: ", rcode);
                                 // Try once more
                                 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
                         }
 #endif
-                        if (rcode) {
+                        if(rcode) {
                                 goto FailGetConfDescr;
                         }
-                        if (bNumEP > 2) {
+                        if(bNumEP > 2) {
                                 break;
                         }
                 } // for (uint8_t i=0; i<num_of_conf; i++...
 
-                if (bNumEP == 3) {
+                if(bNumEP == 3) {
                         // Assign epInfo to epinfo pointer - this time all 3 endpoins
                         rcode = pUsb->setEpInfoEntry(bAddress, 3, epInfo);
-                        if (rcode) {
+                        if(rcode) {
                                 goto FailSetDevTblEntry;
                         }
                 }
 
                 // Set Configuration Value
                 rcode = pUsb->setConf(bAddress, 0, bConfNum);
-                if (rcode) {
+                if(rcode) {
                         goto FailSetConfDescr;
                 }
                 /* print endpoint structure */
@@ -201,7 +202,7 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 USBTRACE2("\r\nAddr: ", epInfo[epDataInIndex].epAddr);
                 USBTRACE2("\r\nMax.pkt.size: ", epInfo[epDataInIndex].maxPktSize);
                 USBTRACE2("\r\nAttr: ", epInfo[epDataInIndex].epAttribs);
-                */
+                 */
 
                 USBTRACE("\r\nConfiguration successful");
                 ready = true;
@@ -215,34 +216,43 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 rcode = getProto((uint8_t*) & adkproto);
 #if defined(XOOM)
                 //added by Jaylen Scott Vanorden
-                if (rcode) {
+                if(rcode) {
                         USBTRACE2("\r\nGot 1st bad code for proto: ", rcode);
                         // Try once more
                         rcode = getProto((uint8_t*) & adkproto);
                 }
 #endif
-                if (rcode) {
+                if(rcode) {
                         goto FailGetProto; //init fails
                 }
                 USBTRACE2("\r\nADK protocol rev. ", adkproto);
         }
 
+        delay(100);
+
         //sending ID strings
         sendStr(ACCESSORY_STRING_MANUFACTURER, manufacturer);
+        delay(10);
         sendStr(ACCESSORY_STRING_MODEL, model);
+        delay(10);
         sendStr(ACCESSORY_STRING_DESCRIPTION, description);
+        delay(10);
         sendStr(ACCESSORY_STRING_VERSION, version);
+        delay(10);
         sendStr(ACCESSORY_STRING_URI, uri);
+        delay(10);
         sendStr(ACCESSORY_STRING_SERIAL, serial);
 
+        delay(100);
+
         //switch to accessory mode
         //the Android phone will reset
         rcode = switchAcc();
-        if (rcode) {
+        if(rcode) {
                 goto FailSwAcc; //init fails
         }
         rcode = USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET;
-        delay(1000); // Give Android a chance to do its reset. This is a guess, and possibly could be lower.
+        delay(100); // Give Android a chance to do its reset. This is a guess, and possibly could be lower.
         goto SwAttempt; //switch to accessory mode attempted
 
         /* diagnostic messages */
@@ -282,14 +292,15 @@ FailSwAcc:
         goto Fail;
 #endif
 
+        //FailOnInit:
+        //	USBTRACE("OnInit:");
+        //	goto Fail;
+        //
 SwAttempt:
 #ifdef DEBUG_USB_HOST
         USBTRACE("\r\nAccessory mode switch attempt");
-#endif
-//FailOnInit:
-//        USBTRACE("OnInit:");
-//        goto Fail;
 Fail:
+#endif
         //USBTRACE2("\r\nADK Init Failed, error code: ", rcode);
         //NotifyFail(rcode);
         Release();
@@ -303,13 +314,13 @@ void ADK::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto
         //ErrorMessage<uint8_t>(PSTR("Alt.Set"), alt);
 
         //added by Yuuichi Akagawa
-        if (bNumEP == 3) {
+        if(bNumEP == 3) {
                 return;
         }
 
         bConfNum = conf;
 
-        if ((pep->bmAttributes & 0x02) == 2) {
+        if((pep->bmAttributes & 0x02) == 2) {
                 uint8_t index = ((pep->bEndpointAddress & 0x80) == 0x80) ? epDataInIndex : epDataOutIndex;
                 // Fill in the endpoint info structure
                 epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F);
diff --git a/adk.h b/adk.h
index ab5ed38f..542078bb 100644
--- a/adk.h
+++ b/adk.h
@@ -13,7 +13,7 @@ Contact information
 Circuits At Home, LTD
 Web      :  http://www.circuitsathome.com
 e-mail   :  support@circuitsathome.com
-*/
+ */
 
 /* Google ADK interface support header */
 
@@ -27,8 +27,8 @@ e-mail   :  support@circuitsathome.com
 #define ADB_PID   0x2D01
 
 #define XOOM  //enables repeating getProto() and getConf() attempts
-              //necessary for slow devices such as Motorola XOOM
-              //defined by default, can be commented out to save memory
+//necessary for slow devices such as Motorola XOOM
+//defined by default, can be commented out to save memory
 
 /* requests */
 
@@ -124,17 +124,17 @@ public:
 
 /* returns 2 bytes in *adkproto */
 inline uint8_t ADK::getProto(uint8_t* adkproto) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_GET, ADK_GETPROTO, 0, 0, 0, 2, 2, adkproto, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_GET, ADK_GETPROTO, 0, 0, 0, 2, 2, adkproto, NULL));
 }
 
 /* send ADK string */
 inline uint8_t ADK::sendStr(uint8_t index, const char* str) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_SENDSTR, 0, 0, index, strlen(str) + 1, strlen(str) + 1, (uint8_t*) str, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_SENDSTR, 0, 0, index, strlen(str) + 1, strlen(str) + 1, (uint8_t*)str, NULL));
 }
 
 /* switch to accessory mode */
 inline uint8_t ADK::switchAcc(void) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_ACCSTART, 0, 0, 0, 0, 0, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_ADK_SEND, ADK_ACCSTART, 0, 0, 0, 0, 0, NULL, NULL));
 }
 
 #endif // _ADK_H_
diff --git a/avrpins.h b/avrpins.h
index 5a7e1756..da80349d 100644
--- a/avrpins.h
+++ b/avrpins.h
@@ -218,7 +218,7 @@ public:
         }
 
         static uint8_t IsSet() {
-                return PORT::PinRead() & (uint8_t) (1 << PIN);
+                return PORT::PinRead() & (uint8_t)(1 << PIN);
         }
 
         static void WaiteForSet() {
@@ -457,7 +457,7 @@ public:
 #define P2  Pe4
 #define P3  Pe5
 #define P4  Pg5
-#define P5  Pe5
+#define P5  Pe3
 #define P6  Ph3
 #define P7  Ph4
 
@@ -672,10 +672,19 @@ public:
 // http://balanduino.net/
 #define P0  Pd0 /* 0  - PD0 */
 #define P1  Pd1 /* 1  - PD1 */
-#define P2  Pb2 /* 2  - PB2 */
-#define P3  Pd6 /* 3  - PD6 */
-#define P4  Pd7 /* 4  - PD7 */
-#define P5  Pb3 /* 5  - PB3 */
+
+#if BALANDUINO_REVISION < 13
+  #define P2  Pb2 /* 2  - PB2 */
+  #define P3  Pd6 /* 3  - PD6 */
+  #define P4  Pd7 /* 4  - PD7 */
+  #define P5  Pb3 /* 5  - PB3 */
+#else
+  #define P2  Pd2 /* 2  - PD2 */
+  #define P3  Pd3 /* 3  - PD3 */
+  #define P4  Pd6 /* 4  - PD6 */
+  #define P5  Pd7 /* 5  - PD7 */
+#endif
+
 #define P6  Pb4 /* 6  - PB4 */
 #define P7  Pa0 /* 7  - PA0 */
 #define P8  Pa1 /* 8  - PA1 */
@@ -685,8 +694,15 @@ public:
 #define P12 Pa5 /* 12 - PA5 */
 #define P13 Pc1 /* 13 - PC1 */
 #define P14 Pc0 /* 14 - PC0 */
-#define P15 Pd2 /* 15 - PD2 */
-#define P16 Pd3 /* 16 - PD3 */
+
+#if BALANDUINO_REVISION < 13
+  #define P15 Pd2 /* 15 - PD2 */
+  #define P16 Pd3 /* 16 - PD3 */
+#else
+  #define P15 Pb2 /* 15 - PB2 */
+  #define P16 Pb3 /* 16 - PB2 */
+#endif
+
 #define P17 Pd4 /* 17 - PD4 */
 #define P18 Pd5 /* 18 - PD5 */
 #define P19 Pc2 /* 19 - PC2 */
@@ -749,11 +765,13 @@ public:
 
 #endif // __AVR__
 
-#if defined(__arm__) && defined(CORE_TEENSY)
+#if defined(__arm__)
 
 // pointers are 32 bits on ARM
 #define pgm_read_pointer(p) pgm_read_dword(p)
 
+#if defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__))
+
 #include "core_pins.h"
 #include "avr_emulation.h"
 
@@ -819,6 +837,135 @@ MAKE_PIN(P33, CORE_PIN33_PORTREG, CORE_PIN33_BIT, CORE_PIN33_CONFIG);
 
 #undef MAKE_PIN
 
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+
+// SetDirRead:
+//   Disable interrupts
+//   Disable the pull up resistor
+//   Set to INPUT
+//   Enable PIO
+
+// SetDirWrite:
+//   Disable interrupts
+//   Disable the pull up resistor
+//   Set to OUTPUT
+//   Enable PIO
+
+#define MAKE_PIN(className, pio, pinMask) \
+class className { \
+public: \
+  static void Set() { \
+    pio->PIO_SODR = pinMask; \
+  } \
+  static void Clear() { \
+    pio->PIO_CODR = pinMask; \
+  } \
+  static void SetDirRead() { \
+    pio->PIO_IDR = pinMask ; \
+    pio->PIO_PUDR = pinMask; \
+    pio->PIO_ODR = pinMask; \
+    pio->PIO_PER = pinMask; \
+  } \
+  static void SetDirWrite() { \
+    pio->PIO_IDR = pinMask ; \
+    pio->PIO_PUDR = pinMask; \
+    pio->PIO_OER = pinMask; \
+    pio->PIO_PER = pinMask; \
+  } \
+  static uint8_t IsSet() { \
+    return pio->PIO_PDSR & pinMask; \
+  } \
+};
+
+// See: http://arduino.cc/en/Hacking/PinMappingSAM3X and variant.cpp
+
+MAKE_PIN(P0, PIOA, PIO_PA8);
+MAKE_PIN(P1, PIOA, PIO_PA9);
+MAKE_PIN(P2, PIOB, PIO_PB25);
+MAKE_PIN(P3, PIOC, PIO_PC28);
+MAKE_PIN(P4, PIOC, PIO_PC26);
+MAKE_PIN(P5, PIOC, PIO_PC25);
+MAKE_PIN(P6, PIOC, PIO_PC24);
+MAKE_PIN(P7, PIOC, PIO_PC23);
+MAKE_PIN(P8, PIOC, PIO_PC22);
+MAKE_PIN(P9, PIOC, PIO_PC21);
+MAKE_PIN(P10, PIOC, PIO_PC29);
+MAKE_PIN(P11, PIOD, PIO_PD7);
+MAKE_PIN(P12, PIOD, PIO_PD8);
+MAKE_PIN(P13, PIOB, PIO_PB27);
+MAKE_PIN(P14, PIOD, PIO_PD4);
+MAKE_PIN(P15, PIOD, PIO_PD5);
+MAKE_PIN(P16, PIOA, PIO_PA13);
+MAKE_PIN(P17, PIOA, PIO_PA12);
+MAKE_PIN(P18, PIOA, PIO_PA11);
+MAKE_PIN(P19, PIOA, PIO_PA10);
+MAKE_PIN(P20, PIOB, PIO_PB12);
+MAKE_PIN(P21, PIOB, PIO_PB13);
+MAKE_PIN(P22, PIOB, PIO_PB26);
+MAKE_PIN(P23, PIOA, PIO_PA14);
+MAKE_PIN(P24, PIOA, PIO_PA15);
+MAKE_PIN(P25, PIOD, PIO_PD0);
+MAKE_PIN(P26, PIOD, PIO_PD1);
+MAKE_PIN(P27, PIOD, PIO_PD2);
+MAKE_PIN(P28, PIOD, PIO_PD3);
+MAKE_PIN(P29, PIOD, PIO_PD6);
+MAKE_PIN(P30, PIOD, PIO_PD9);
+MAKE_PIN(P31, PIOA, PIO_PA7);
+MAKE_PIN(P32, PIOD, PIO_PD10);
+MAKE_PIN(P33, PIOC, PIO_PC1);
+MAKE_PIN(P34, PIOC, PIO_PC2);
+MAKE_PIN(P35, PIOC, PIO_PC3);
+MAKE_PIN(P36, PIOC, PIO_PC4);
+MAKE_PIN(P37, PIOC, PIO_PC5);
+MAKE_PIN(P38, PIOC, PIO_PC6);
+MAKE_PIN(P39, PIOC, PIO_PC7);
+MAKE_PIN(P40, PIOC, PIO_PC8);
+MAKE_PIN(P41, PIOC, PIO_PC9);
+MAKE_PIN(P42, PIOA, PIO_PA19);
+MAKE_PIN(P43, PIOA, PIO_PA20);
+MAKE_PIN(P44, PIOC, PIO_PC19);
+MAKE_PIN(P45, PIOC, PIO_PC18);
+MAKE_PIN(P46, PIOC, PIO_PC17);
+MAKE_PIN(P47, PIOC, PIO_PC16);
+MAKE_PIN(P48, PIOC, PIO_PC15);
+MAKE_PIN(P49, PIOC, PIO_PC14);
+MAKE_PIN(P50, PIOC, PIO_PC13);
+MAKE_PIN(P51, PIOC, PIO_PC12);
+MAKE_PIN(P52, PIOB, PIO_PB21);
+MAKE_PIN(P53, PIOB, PIO_PB14);
+MAKE_PIN(P54, PIOA, PIO_PA16);
+MAKE_PIN(P55, PIOA, PIO_PA24);
+MAKE_PIN(P56, PIOA, PIO_PA23);
+MAKE_PIN(P57, PIOA, PIO_PA22);
+MAKE_PIN(P58, PIOA, PIO_PA6);
+MAKE_PIN(P59, PIOA, PIO_PA4);
+MAKE_PIN(P60, PIOA, PIO_PA3);
+MAKE_PIN(P61, PIOA, PIO_PA2);
+MAKE_PIN(P62, PIOB, PIO_PB17);
+MAKE_PIN(P63, PIOB, PIO_PB18);
+MAKE_PIN(P64, PIOB, PIO_PB19);
+MAKE_PIN(P65, PIOB, PIO_PB20);
+MAKE_PIN(P66, PIOB, PIO_PB15);
+MAKE_PIN(P67, PIOB, PIO_PB16);
+MAKE_PIN(P68, PIOA, PIO_PA1);
+MAKE_PIN(P69, PIOA, PIO_PA0);
+MAKE_PIN(P70, PIOA, PIO_PA17);
+MAKE_PIN(P71, PIOA, PIO_PA18);
+MAKE_PIN(P72, PIOC, PIO_PC30);
+MAKE_PIN(P73, PIOA, PIO_PA21);
+MAKE_PIN(P74, PIOA, PIO_PA25); // MISO
+MAKE_PIN(P75, PIOA, PIO_PA26); // MOSI
+MAKE_PIN(P76, PIOA, PIO_PA27); // CLK
+MAKE_PIN(P77, PIOA, PIO_PA28);
+MAKE_PIN(P78, PIOB, PIO_PB23); // Unconnected
+
+#undef MAKE_PIN
+
+#else
+#error "Please define board in avrpins.h"
+
+#endif
+
 #endif // __arm__
 
 #endif //_avrpins_h_
diff --git a/cdcacm.cpp b/cdcacm.cpp
index 0f9fffde..3d50f600 100644
--- a/cdcacm.cpp
+++ b/cdcacm.cpp
@@ -28,9 +28,9 @@ bControlIface(0),
 bDataIface(0),
 bNumEP(1),
 qNextPollTime(0),
-ready(false),
-bPollEnable(false) {
-        for (uint8_t i = 0; i < ACM_MAX_ENDPOINTS; i++) {
+bPollEnable(false),
+ready(false) {
+        for(uint8_t i = 0; i < ACM_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
@@ -40,7 +40,7 @@ bPollEnable(false) {
                 //if (!i)
                 epInfo[i].bmNakPower = USB_NAK_MAX_POWER;
         }
-        if (pUsb)
+        if(pUsb)
                 pUsb->RegisterDeviceClass(this);
 }
 
@@ -48,6 +48,8 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
 
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
+
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -57,16 +59,16 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         USBTRACE("ACM Init\r\n");
 
-        if (bAddress)
+        if(bAddress)
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
                 USBTRACE("epinfo\r\n");
                 return USB_ERROR_EPINFO_IS_NULL;
         }
@@ -85,22 +87,22 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from the device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
 
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -114,22 +116,22 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
-        num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
+        num_of_conf = udd->bNumConfigurations;
 
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         USBTRACE2("NC:", num_of_conf);
 
-        for (uint8_t i = 0; i < num_of_conf; i++) {
+        for(uint8_t i = 0; i < num_of_conf; i++) {
                 ConfigDescParser< USB_CLASS_COM_AND_CDC_CTRL,
                         CDC_SUBCLASS_ACM,
                         CDC_PROTOCOL_ITU_T_V_250,
@@ -142,19 +144,19 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &CdcControlParser);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &CdcDataParser);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
-                if (bNumEP > 1)
+                if(bNumEP > 1)
                         break;
         } // for
 
-        if (bNumEP < 4)
+        if(bNumEP < 4)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         // Assign epInfo to epinfo pointer
@@ -165,12 +167,12 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Set Configuration Value
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
         rcode = pAsync->OnInit(this);
 
-        if (rcode)
+        if(rcode)
                 goto FailOnInit;
 
         USBTRACE("ACM configured\r\n");
@@ -211,8 +213,8 @@ FailOnInit:
         USBTRACE("OnInit:");
 #endif
 
-Fail:
 #ifdef DEBUG_USB_HOST
+Fail:
         NotifyFail(rcode);
 #endif
         Release();
@@ -228,10 +230,10 @@ void ACM::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto
 
         uint8_t index;
 
-        if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
+        if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
                 index = epInterruptInIndex;
         else
-                if ((pep->bmAttributes & 0x02) == 2)
+                if((pep->bmAttributes & 0x02) == 2)
                 index = ((pep->bEndpointAddress & 0x80) == 0x80) ? epDataInIndex : epDataOutIndex;
         else
                 return;
@@ -262,7 +264,7 @@ uint8_t ACM::Release() {
 uint8_t ACM::Poll() {
         uint8_t rcode = 0;
 
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
 
         //uint32_t	time_now = millis();
diff --git a/cdcacm.h b/cdcacm.h
index e8c4e57d..4a2a1362 100644
--- a/cdcacm.h
+++ b/cdcacm.h
@@ -19,67 +19,67 @@ e-mail   :  support@circuitsathome.com
 
 #include "Usb.h"
 
-#define bmREQ_CDCOUT        USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define bmREQ_CDCIN         USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_CDCOUT                    USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_CDCIN                     USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
 
 // CDC Subclass Constants
-#define CDC_SUBCLASS_DLCM					0x01	// Direct Line Control Model
-#define CDC_SUBCLASS_ACM					0x02	// Abstract Control Model
-#define CDC_SUBCLASS_TCM					0x03	// Telephone Control Model
-#define CDC_SUBCLASS_MCCM					0x04	// Multi Channel Control Model
-#define CDC_SUBCLASS_CAPI					0x05	// CAPI Control Model
-#define CDC_SUBCLASS_ETHERNET				0x06	// Ethernet Network Control Model
-#define CDC_SUBCLASS_ATM					0x07	// ATM Network Control Model
-#define CDC_SUBCLASS_WIRELESS_HANDSET		0x08	// Wireless Handset Control Model
-#define CDC_SUBCLASS_DEVICE_MANAGEMENT		0x09	// Device Management
-#define CDC_SUBCLASS_MOBILE_DIRECT_LINE		0x0A	// Mobile Direct Line Model
-#define CDC_SUBCLASS_OBEX					0x0B	// OBEX
-#define CDC_SUBCLASS_ETHERNET_EMU			0x0C	// Ethernet Emulation Model
+#define CDC_SUBCLASS_DLCM		0x01	// Direct Line Control Model
+#define CDC_SUBCLASS_ACM		0x02	// Abstract Control Model
+#define CDC_SUBCLASS_TCM		0x03	// Telephone Control Model
+#define CDC_SUBCLASS_MCCM		0x04	// Multi Channel Control Model
+#define CDC_SUBCLASS_CAPI		0x05	// CAPI Control Model
+#define CDC_SUBCLASS_ETHERNET		0x06	// Ethernet Network Control Model
+#define CDC_SUBCLASS_ATM		0x07	// ATM Network Control Model
+#define CDC_SUBCLASS_WIRELESS_HANDSET	0x08	// Wireless Handset Control Model
+#define CDC_SUBCLASS_DEVICE_MANAGEMENT	0x09	// Device Management
+#define CDC_SUBCLASS_MOBILE_DIRECT_LINE 0x0A	// Mobile Direct Line Model
+#define CDC_SUBCLASS_OBEX		0x0B	// OBEX
+#define CDC_SUBCLASS_ETHERNET_EMU	0x0C	// Ethernet Emulation Model
 
 // Communication Interface Class Control Protocol Codes
-#define CDC_PROTOCOL_ITU_T_V_250			0x01	// AT Commands defined by ITU-T V.250
-#define CDC_PROTOCOL_PCCA_101				0x02	// AT Commands defined by PCCA-101
-#define CDC_PROTOCOL_PCCA_101_O				0x03	// AT Commands defined by PCCA-101 & Annex O
-#define CDC_PROTOCOL_GSM_7_07				0x04	// AT Commands defined by GSM 7.07
-#define CDC_PROTOCOL_3GPP_27_07				0x05	// AT Commands defined by 3GPP 27.007
-#define CDC_PROTOCOL_C_S0017_0				0x06	// AT Commands defined by TIA for CDMA
-#define CDC_PROTOCOL_USB_EEM				0x07	// Ethernet Emulation Model
+#define CDC_PROTOCOL_ITU_T_V_250	0x01	// AT Commands defined by ITU-T V.250
+#define CDC_PROTOCOL_PCCA_101		0x02	// AT Commands defined by PCCA-101
+#define CDC_PROTOCOL_PCCA_101_O		0x03	// AT Commands defined by PCCA-101 & Annex O
+#define CDC_PROTOCOL_GSM_7_07		0x04	// AT Commands defined by GSM 7.07
+#define CDC_PROTOCOL_3GPP_27_07		0x05	// AT Commands defined by 3GPP 27.007
+#define CDC_PROTOCOL_C_S0017_0		0x06	// AT Commands defined by TIA for CDMA
+#define CDC_PROTOCOL_USB_EEM		0x07	// Ethernet Emulation Model
 
 // CDC Commands defined by CDC 1.2
-#define CDC_SEND_ENCAPSULATED_COMMAND		0x00
-#define CDC_GET_ENCAPSULATED_RESPONSE		0x01
+#define CDC_SEND_ENCAPSULATED_COMMAND	0x00
+#define CDC_GET_ENCAPSULATED_RESPONSE	0x01
 
 // CDC Commands defined by PSTN 1.2
-#define CDC_SET_COMM_FEATURE				0x02
-#define CDC_GET_COMM_FEATURE				0x03
-#define CDC_CLEAR_COMM_FEATURE				0x04
-#define CDC_SET_AUX_LINE_STATE				0x10
-#define CDC_SET_HOOK_STATE					0x11
-#define CDC_PULSE_SETUP						0x12
-#define CDC_SEND_PULSE						0x13
-#define CDC_SET_PULSE_TIME					0x14
-#define CDC_RING_AUX_JACK					0x15
-#define CDC_SET_LINE_CODING					0x20
-#define CDC_GET_LINE_CODING					0x21
-#define CDC_SET_CONTROL_LINE_STATE			0x22
-#define CDC_SEND_BREAK						0x23
-#define CDC_SET_RINGER_PARMS				0x30
-#define CDC_GET_RINGER_PARMS				0x31
-#define CDC_SET_OPERATION_PARMS				0x32
-#define CDC_GET_OPERATION_PARMS				0x33
-#define CDC_SET_LINE_PARMS					0x34
-#define CDC_GET_LINE_PARMS					0x35
-#define CDC_DIAL_DIGITS						0x36
+#define CDC_SET_COMM_FEATURE		0x02
+#define CDC_GET_COMM_FEATURE		0x03
+#define CDC_CLEAR_COMM_FEATURE		0x04
+#define CDC_SET_AUX_LINE_STATE		0x10
+#define CDC_SET_HOOK_STATE		0x11
+#define CDC_PULSE_SETUP			0x12
+#define CDC_SEND_PULSE			0x13
+#define CDC_SET_PULSE_TIME		0x14
+#define CDC_RING_AUX_JACK		0x15
+#define CDC_SET_LINE_CODING		0x20
+#define CDC_GET_LINE_CODING		0x21
+#define CDC_SET_CONTROL_LINE_STATE	0x22
+#define CDC_SEND_BREAK			0x23
+#define CDC_SET_RINGER_PARMS		0x30
+#define CDC_GET_RINGER_PARMS		0x31
+#define CDC_SET_OPERATION_PARMS		0x32
+#define CDC_GET_OPERATION_PARMS		0x33
+#define CDC_SET_LINE_PARMS		0x34
+#define CDC_GET_LINE_PARMS		0x35
+#define CDC_DIAL_DIGITS			0x36
 
 //Class-Specific Notification Codes
-#define NETWORK_CONNECTION  0x00
-#define RESPONSE_AVAILABLE  0x01
-#define AUX_JACK_HOOK_STATE 0x08
-#define RING_DETECT         0x09
-#define SERIAL_STATE        0x20
-#define CALL_STATE_CHANGE   0x28
-#define LINE_STATE_CHANGE   0x29
-#define CONNECTION_SPEED_CHANGE 0x2a
+#define NETWORK_CONNECTION              0x00
+#define RESPONSE_AVAILABLE              0x01
+#define AUX_JACK_HOOK_STATE             0x08
+#define RING_DETECT                     0x09
+#define SERIAL_STATE                    0x20
+#define CALL_STATE_CHANGE               0x28
+#define LINE_STATE_CHANGE               0x29
+#define CONNECTION_SPEED_CHANGE         0x2a
 
 // CDC Functional Descriptor Structures
 
@@ -114,14 +114,13 @@ typedef struct {
         uint8_t bDataBits; // Data bits (5, 6, 7, 8 or 16)
 } LINE_CODING;
 
-typedef struct
-{
-  uint8_t   bmRequestType;  // 0xa1 for class-specific notifications
-  uint8_t   bNotification;
-  uint16_t  wValue;
-  uint16_t  wIndex;
-  uint16_t  wLength;
-  uint16_t  bmState;        //UART state bitmap for SERIAL_STATE, other notifications variable length
+typedef struct {
+        uint8_t bmRequestType; // 0xa1 for class-specific notifications
+        uint8_t bNotification;
+        uint16_t wValue;
+        uint16_t wIndex;
+        uint16_t wLength;
+        uint16_t bmState; //UART state bitmap for SERIAL_STATE, other notifications variable length
 } CLASS_NOTIFICATION;
 
 class ACM;
@@ -151,7 +150,7 @@ protected:
         uint8_t bNumEP; // total number of EP in the configuration
         uint32_t qNextPollTime; // next poll time
         bool bPollEnable; // poll enable flag
-       	bool  ready;      //device ready indicator
+        bool ready; //device ready indicator
 
         EpInfo epInfo[ACM_MAX_ENDPOINTS];
 
@@ -167,7 +166,7 @@ public:
         uint8_t GetLineCoding(LINE_CODING *dataptr);
         uint8_t SetControlLineState(uint8_t state);
         uint8_t SendBreak(uint16_t duration);
-        uint8_t GetNotif( uint16_t *bytes_rcvd, uint8_t *dataptr );
+        uint8_t GetNotif(uint16_t *bytes_rcvd, uint8_t *dataptr);
 
         // Methods for recieving and sending data
         uint8_t RcvData(uint16_t *nbytesptr, uint8_t *dataptr);
@@ -182,9 +181,9 @@ public:
                 return bAddress;
         };
 
-				virtual bool isReady() {
-								return ready;
-				};
+        virtual bool isReady() {
+                return ready;
+        };
 
         // UsbConfigXtracter implementation
         virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep);
diff --git a/cdcftdi.cpp b/cdcftdi.cpp
index 20d1dd66..13ca683f 100644
--- a/cdcftdi.cpp
+++ b/cdcftdi.cpp
@@ -26,7 +26,7 @@ pUsb(p),
 bAddress(0),
 bNumEP(1),
 wFTDIType(0) {
-        for (uint8_t i = 0; i < FTDI_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < FTDI_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
@@ -34,7 +34,7 @@ wFTDIType(0) {
                 //if (!i)
                 epInfo[i].bmNakPower = USB_NAK_MAX_POWER;
         }
-        if (pUsb)
+        if(pUsb)
                 pUsb->RegisterDeviceClass(this);
 }
 
@@ -42,6 +42,7 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
 
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -55,16 +56,16 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         USBTRACE("FTDI Init\r\n");
 
-        if (bAddress)
+        if(bAddress)
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
                 USBTRACE("epinfo\r\n");
                 return USB_ERROR_EPINFO_IS_NULL;
         }
@@ -78,33 +79,32 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         p->lowspeed = lowspeed;
 
         // Get device descriptor
-        rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);
+        rcode = pUsb->getDevDescr(0, 0, sizeof (USB_DEVICE_DESCRIPTOR), buf);
 
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
-
-        if (((USB_DEVICE_DESCRIPTOR*)buf)->idVendor != FTDI_VID || ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct != FTDI_PID)
+        if(udd->idVendor != FTDI_VID || udd->idProduct != FTDI_PID)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         // Save type of FTDI chip
-        wFTDIType = ((USB_DEVICE_DESCRIPTOR*)buf)->bcdDevice;
+        wFTDIType = udd->bcdDevice;
 
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from the device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
 
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -118,40 +118,40 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
-        num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
+        num_of_conf = udd->bNumConfigurations;
 
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         USBTRACE2("NC:", num_of_conf);
 
-        for (uint8_t i = 0; i < num_of_conf; i++) {
+        for(uint8_t i = 0; i < num_of_conf; i++) {
                 HexDumper<USBReadParser, uint16_t, uint16_t> HexDump;
                 ConfigDescParser < 0xFF, 0xFF, 0xFF, CP_MASK_COMPARE_ALL> confDescrParser(this);
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &HexDump);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
-                if (bNumEP > 1)
+                if(bNumEP > 1)
                         break;
         } // for
 
-        if (bNumEP < 2)
+        if(bNumEP < 2)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         USBTRACE2("NumEP:", bNumEP);
@@ -164,12 +164,12 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Set Configuration Value
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
         rcode = pAsync->OnInit(this);
 
-        if (rcode)
+        if(rcode)
                 goto FailOnInit;
 
         USBTRACE("FTDI configured\r\n");
@@ -204,10 +204,8 @@ FailSetConfDescr:
 FailOnInit:
 #ifdef DEBUG_USB_HOST
         USBTRACE("OnInit:");
-#endif
 
 Fail:
-#ifdef DEBUG_USB_HOST
         NotifyFail(rcode);
 #endif
         Release();
@@ -223,10 +221,10 @@ void FTDI::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t prot
 
         uint8_t index;
 
-        if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
+        if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
                 index = epInterruptInIndex;
         else
-                if ((pep->bmAttributes & 0x02) == 2)
+                if((pep->bmAttributes & 0x02) == 2)
                 index = ((pep->bEndpointAddress & 0x80) == 0x80) ? epDataInIndex : epDataOutIndex;
         else
                 return;
@@ -272,19 +270,19 @@ uint8_t FTDI::SetBaudRate(uint32_t baud) {
 
         divisor3 = 48000000 / 2 / baud; // divisor shifted 3 bits to the left
 
-        if (wFTDIType == FT232AM) {
-                if ((divisor3 & 0x7) == 7)
+        if(wFTDIType == FT232AM) {
+                if((divisor3 & 0x7) == 7)
                         divisor3++; // round x.7/8 up to x+1
 
                 baud_value = divisor3 >> 3;
                 divisor3 &= 0x7;
 
-                if (divisor3 == 1) baud_value |= 0xc000;
+                if(divisor3 == 1) baud_value |= 0xc000;
                 else // 0.125
-                        if (divisor3 >= 4) baud_value |= 0x4000;
+                        if(divisor3 >= 4) baud_value |= 0x4000;
                 else // 0.5
-                        if (divisor3 != 0) baud_value |= 0x8000; // 0.25
-                if (baud_value == 1) baud_value = 0; /* special case for maximum baud rate */
+                        if(divisor3 != 0) baud_value |= 0x8000; // 0.25
+                if(baud_value == 1) baud_value = 0; /* special case for maximum baud rate */
         } else {
                 static const unsigned char divfrac [8] = {0, 3, 2, 0, 1, 1, 2, 3};
                 static const unsigned char divindex[8] = {0, 0, 0, 1, 0, 1, 1, 1};
@@ -294,9 +292,9 @@ uint8_t FTDI::SetBaudRate(uint32_t baud) {
                 baud_index = divindex[divisor3 & 0x7];
 
                 /* Deal with special cases for highest baud rates. */
-                if (baud_value == 1) baud_value = 0;
+                if(baud_value == 1) baud_value = 0;
                 else // 1.0
-                        if (baud_value == 0x4001) baud_value = 1; // 1.5
+                        if(baud_value == 0x4001) baud_value = 1; // 1.5
         }
         USBTRACE2("baud_value:", baud_value);
         USBTRACE2("baud_index:", baud_index);
diff --git a/cdcftdi.h b/cdcftdi.h
index e576e246..866e2d7d 100644
--- a/cdcftdi.h
+++ b/cdcftdi.h
@@ -130,4 +130,4 @@ public:
         virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep);
 };
 
-#endif // __CDCFTDI_H__
\ No newline at end of file
+#endif // __CDCFTDI_H__
diff --git a/cdcprolific.cpp b/cdcprolific.cpp
index 649cab95..d629ad55 100644
--- a/cdcprolific.cpp
+++ b/cdcprolific.cpp
@@ -25,6 +25,7 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
 
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -34,16 +35,16 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         USBTRACE("PL Init\r\n");
 
-        if (bAddress)
+        if(bAddress)
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
                 USBTRACE("epinfo\r\n");
                 return USB_ERROR_EPINFO_IS_NULL;
         }
@@ -62,28 +63,28 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
-        if (((USB_DEVICE_DESCRIPTOR*)buf)->idVendor != PL_VID && ((USB_DEVICE_DESCRIPTOR*)buf)->idProduct != PL_PID)
+        if(udd->idVendor != PL_VID && udd->idProduct != PL_PID)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         // Save type of PL chip
-        wPLType = ((USB_DEVICE_DESCRIPTOR*)buf)->bcdDevice;
+        wPLType = udd->bcdDevice;
 
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from the device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
 
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -97,40 +98,40 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
-        num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
+        num_of_conf = udd->bNumConfigurations;
 
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         USBTRACE2("NC:", num_of_conf);
 
-        for (uint8_t i = 0; i < num_of_conf; i++) {
+        for(uint8_t i = 0; i < num_of_conf; i++) {
                 HexDumper<USBReadParser, uint16_t, uint16_t> HexDump;
                 ConfigDescParser < 0xFF, 0, 0, CP_MASK_COMPARE_CLASS> confDescrParser(this);
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &HexDump);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
-                if (bNumEP > 1)
+                if(bNumEP > 1)
                         break;
         } // for
 
-        if (bNumEP < 2)
+        if(bNumEP < 2)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         // Assign epInfo to epinfo pointer
@@ -141,12 +142,12 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Set Configuration Value
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
         rcode = pAsync->OnInit(this);
 
-        if (rcode)
+        if(rcode)
                 goto FailOnInit;
 
         USBTRACE("PL configured\r\n");
@@ -184,8 +185,8 @@ FailOnInit:
         USBTRACE("OnInit:");
 #endif
 
-Fail:
 #ifdef DEBUG_USB_HOST
+Fail:
         NotifyFail(rcode);
 #endif
         Release();
@@ -207,5 +208,3 @@ Fail:
 //	//}
 //	return rcode;
 //}
-
-
diff --git a/confdescparser.h b/confdescparser.h
index b0c87d31..f16d6fce 100644
--- a/confdescparser.h
+++ b/confdescparser.h
@@ -20,7 +20,6 @@ e-mail   :  support@circuitsathome.com
 
 #define __CONFDESCPARSER_H__
 
-
 class UsbConfigXtracter {
 public:
         //virtual void ConfigXtract(const USB_CONFIGURATION_DESCRIPTOR *conf) = 0;
@@ -54,11 +53,15 @@ class ConfigDescParser : public USBReadParser {
         uint8_t ifaceNumber; // Interface number
         uint8_t ifaceAltSet; // Interface alternate settings
 
+        bool UseOr;
         bool ParseDescriptor(uint8_t **pp, uint16_t *pcntdn);
-
         void PrintHidDescriptor(const USB_HID_DESCRIPTOR *pDesc);
 
 public:
+
+        void SetOR(void) {
+                UseOr = true;
+        }
         ConfigDescParser(UsbConfigXtracter *xtractor);
         virtual void Parse(const uint16_t len, const uint8_t *pbuf, const uint16_t &offset);
 };
@@ -68,7 +71,8 @@ ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::ConfigDescParser(Usb
 theXtractor(xtractor),
 stateParseDescr(0),
 dscrLen(0),
-dscrType(0) {
+dscrType(0),
+UseOr(false) {
         theBuffer.pValue = varBuffer;
         valParser.Initialize(&theBuffer);
         theSkipper.Initialize(&theBuffer);
@@ -76,8 +80,8 @@ dscrType(0) {
 
 template <const uint8_t CLASS_ID, const uint8_t SUBCLASS_ID, const uint8_t PROTOCOL_ID, const uint8_t MASK>
 void ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::Parse(const uint16_t len, const uint8_t *pbuf, const uint16_t &offset) {
-        uint16_t cntdn = (uint16_t) len;
-        uint8_t *p = (uint8_t*) pbuf;
+        uint16_t cntdn = (uint16_t)len;
+        uint8_t *p = (uint8_t*)pbuf;
 
         while(cntdn)
                 if(!ParseDescriptor(&p, &cntdn))
@@ -88,6 +92,8 @@ void ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::Parse(const uin
   compare masks for them. When the match is found, calls EndpointXtract passing buffer containing endpoint descriptor */
 template <const uint8_t CLASS_ID, const uint8_t SUBCLASS_ID, const uint8_t PROTOCOL_ID, const uint8_t MASK>
 bool ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::ParseDescriptor(uint8_t **pp, uint16_t *pcntdn) {
+        USB_CONFIGURATION_DESCRIPTOR* ucd = reinterpret_cast<USB_CONFIGURATION_DESCRIPTOR*>(varBuffer);
+        USB_INTERFACE_DESCRIPTOR* uid = reinterpret_cast<USB_INTERFACE_DESCRIPTOR*>(varBuffer);
         switch(stateParseDescr) {
                 case 0:
                         theBuffer.valueSize = 2;
@@ -96,8 +102,8 @@ bool ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::ParseDescriptor
                 case 1:
                         if(!valParser.Parse(pp, pcntdn))
                                 return false;
-                        dscrLen = *((uint8_t*) theBuffer.pValue);
-                        dscrType = *((uint8_t*) theBuffer.pValue + 1);
+                        dscrLen = *((uint8_t*)theBuffer.pValue);
+                        dscrType = *((uint8_t*)theBuffer.pValue + 1);
                         stateParseDescr = 2;
                 case 2:
                         // This is a sort of hack. Assuming that two bytes are all ready in the buffer
@@ -112,10 +118,10 @@ bool ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::ParseDescriptor
                                 case USB_DESCRIPTOR_INTERFACE:
                                         isGoodInterface = false;
                                 case USB_DESCRIPTOR_CONFIGURATION:
-                                        theBuffer.valueSize = sizeof(USB_CONFIGURATION_DESCRIPTOR) - 2;
+                                        theBuffer.valueSize = sizeof (USB_CONFIGURATION_DESCRIPTOR) - 2;
                                         break;
                                 case USB_DESCRIPTOR_ENDPOINT:
-                                        theBuffer.valueSize = sizeof(USB_ENDPOINT_DESCRIPTOR) - 2;
+                                        theBuffer.valueSize = sizeof (USB_ENDPOINT_DESCRIPTOR) - 2;
                                         break;
                                 case HID_DESCRIPTOR_HID:
                                         theBuffer.valueSize = dscrLen - 2;
@@ -128,29 +134,33 @@ bool ConfigDescParser<CLASS_ID, SUBCLASS_ID, PROTOCOL_ID, MASK>::ParseDescriptor
                                 case USB_DESCRIPTOR_CONFIGURATION:
                                         if(!valParser.Parse(pp, pcntdn))
                                                 return false;
-                                        confValue = ((USB_CONFIGURATION_DESCRIPTOR*) varBuffer)->bConfigurationValue;
+                                        confValue = ucd->bConfigurationValue;
                                         break;
                                 case USB_DESCRIPTOR_INTERFACE:
                                         if(!valParser.Parse(pp, pcntdn))
                                                 return false;
-                                        if((MASK & CP_MASK_COMPARE_CLASS) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceClass != CLASS_ID)
+                                        if((MASK & CP_MASK_COMPARE_CLASS) && uid->bInterfaceClass != CLASS_ID)
                                                 break;
-                                        if((MASK & CP_MASK_COMPARE_SUBCLASS) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceSubClass != SUBCLASS_ID)
+                                        if((MASK & CP_MASK_COMPARE_SUBCLASS) && uid->bInterfaceSubClass != SUBCLASS_ID)
                                                 break;
-                                        if((MASK & CP_MASK_COMPARE_PROTOCOL) && ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceProtocol != PROTOCOL_ID)
-                                                break;
-
+                                        if(UseOr) {
+                                                if((!((MASK & CP_MASK_COMPARE_PROTOCOL) && uid->bInterfaceProtocol)))
+                                                        break;
+                                        } else {
+                                                if((MASK & CP_MASK_COMPARE_PROTOCOL) && uid->bInterfaceProtocol != PROTOCOL_ID)
+                                                        break;
+                                        }
                                         isGoodInterface = true;
-                                        ifaceNumber = ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceNumber;
-                                        ifaceAltSet = ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bAlternateSetting;
-                                        protoValue = ((USB_INTERFACE_DESCRIPTOR*) varBuffer)->bInterfaceProtocol;
+                                        ifaceNumber = uid->bInterfaceNumber;
+                                        ifaceAltSet = uid->bAlternateSetting;
+                                        protoValue = uid->bInterfaceProtocol;
                                         break;
                                 case USB_DESCRIPTOR_ENDPOINT:
                                         if(!valParser.Parse(pp, pcntdn))
                                                 return false;
                                         if(isGoodInterface)
                                                 if(theXtractor)
-                                                        theXtractor->EndpointXtract(confValue, ifaceNumber, ifaceAltSet, protoValue, (USB_ENDPOINT_DESCRIPTOR*) varBuffer);
+                                                        theXtractor->EndpointXtract(confValue, ifaceNumber, ifaceAltSet, protoValue, (USB_ENDPOINT_DESCRIPTOR*)varBuffer);
                                         break;
                                         //case HID_DESCRIPTOR_HID:
                                         //	if (!valParser.Parse(pp, pcntdn))
diff --git a/controllerEnums.h b/controllerEnums.h
index 5e0d6dc8..0169c763 100644
--- a/controllerEnums.h
+++ b/controllerEnums.h
@@ -18,30 +18,58 @@
 #ifndef _controllerenums_h
 #define _controllerenums_h
 
-/*
- This header file is used to store different enums for the controllers,
- This is necessary so all the different libraries can be used at once
+/**
+ * This header file is used to store different enums for the controllers,
+ * This is necessary so all the different libraries can be used at once.
  */
 
 /** Enum used to turn on the LEDs on the different controllers. */
-enum LED {
-        LED1 = 0,
-        LED2 = 1,
-        LED3 = 2,
-        LED4 = 3,
+enum LEDEnum {
+        OFF = 0,
+        LED1 = 1,
+        LED2 = 2,
+        LED3 = 3,
+        LED4 = 4,
 
-        LED5 = 4,
-        LED6 = 5,
-        LED7 = 6,
-        LED8 = 7,
-        LED9 = 8,
-        LED10 = 9,
+        LED5 = 5,
+        LED6 = 6,
+        LED7 = 7,
+        LED8 = 8,
+        LED9 = 9,
+        LED10 = 10,
         /** Used to blink all LEDs on the Xbox controller */
-        ALL = 4,
+        ALL = 5,
+};
+
+/** Used to set the colors of the Move and PS4 controller. */
+enum ColorsEnum {
+        /** r = 255, g = 0, b = 0 */
+        Red = 0xFF0000,
+        /** r = 0, g = 255, b = 0 */
+        Green = 0xFF00,
+        /** r = 0, g = 0, b = 255 */
+        Blue = 0xFF,
+
+        /** r = 255, g = 235, b = 4 */
+        Yellow = 0xFFEB04,
+        /** r = 0, g = 255, b = 255 */
+        Lightblue = 0xFFFF,
+        /** r = 255, g = 0, b = 255 */
+        Purble = 0xFF00FF,
+
+        /** r = 255, g = 255, b = 255 */
+        White = 0xFFFFFF,
+        /** r = 0, g = 0, b = 0 */
+        Off = 0x00,
+};
+
+enum RumbleEnum {
+        RumbleHigh = 0x10,
+        RumbleLow = 0x20,
 };
 
 /** This enum is used to read all the different buttons on the different controllers */
-enum Button {
+enum ButtonEnum {
         /**@{*/
         /** These buttons are available on all the the controllers */
         UP = 0,
@@ -93,6 +121,12 @@ enum Button {
         T = 18, // Covers 12 bits - we only need to read the top 8
         /**@}*/
 
+        /** PS4 controllers buttons - SHARE and OPTIONS are present instead of SELECT and START */
+        SHARE = 4,
+        OPTIONS = 5,
+        TOUCHPAD = 17,
+        /**@}*/
+
         /**@{*/
         /** Xbox buttons */
         BACK = 4,
@@ -103,10 +137,18 @@ enum Button {
         BLACK = 8, // Available on the original Xbox controller
         WHITE = 9, // Available on the original Xbox controller
         /**@}*/
+
+        /** PS Buzz controllers */
+        RED = 0,
+        YELLOW = 1,
+        GREEN = 2,
+        ORANGE = 3,
+        BLUE = 4,
+        /**@}*/
 };
 
 /** Joysticks on the PS3 and Xbox controllers. */
-enum AnalogHat {
+enum AnalogHatEnum {
         /** Left joystick x-axis */
         LeftHatX = 0,
         /** Left joystick y-axis */
@@ -117,4 +159,46 @@ enum AnalogHat {
         RightHatY = 3,
 };
 
-#endif
\ No newline at end of file
+/**
+ * Sensors inside the Sixaxis Dualshock 3, Move controller and PS4 controller.
+ * <B>Note:</B> that the location is shifted 9 when it's connected via USB on the PS3 controller.
+ */
+enum SensorEnum {
+        /** Accelerometer values */
+        aX = 50, aY = 52, aZ = 54,
+        /** Gyro z-axis */
+        gZ = 56,
+        gX, gY, // These are not available on the PS3 controller
+
+        /** Accelerometer x-axis */
+        aXmove = 28,
+        /** Accelerometer z-axis */
+        aZmove = 30,
+        /** Accelerometer y-axis */
+        aYmove = 32,
+
+        /** Gyro x-axis */
+        gXmove = 40,
+        /** Gyro z-axis */
+        gZmove = 42,
+        /** Gyro y-axis */
+        gYmove = 44,
+
+        /** Temperature sensor */
+        tempMove = 46,
+
+        /** Magnetometer x-axis */
+        mXmove = 47,
+        /** Magnetometer z-axis */
+        mZmove = 49,
+        /** Magnetometer y-axis */
+        mYmove = 50,
+};
+
+/** Used to get the angle calculated using the PS3 controller and PS4 controller. */
+enum AngleEnum {
+        Pitch = 0x01,
+        Roll = 0x02,
+};
+
+#endif
diff --git a/examples/Bluetooth/BTHID/BTHID.ino b/examples/Bluetooth/BTHID/BTHID.ino
index fecc4b50..fcfd686a 100644
--- a/examples/Bluetooth/BTHID/BTHID.ino
+++ b/examples/Bluetooth/BTHID/BTHID.ino
@@ -8,6 +8,10 @@
 #include <usbhub.h>
 #include "KeyboardParser.h"
 #include "MouseParser.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
@@ -16,7 +20,7 @@ BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 /* You can create the instance of the class in two ways */
 // This will start an inquiry and then pair with your device - you only have to do this once
 // If you are using a Bluetooth keyboard, then you should type in the password on the keypad and then press enter
-BTHID hid(&Btd, PAIR, "0000");
+BTHID bthid(&Btd, PAIR, "0000");
 
 // After that you can simply create the instance like so and then press any button on the device
 //BTHID hid(&Btd);
@@ -32,13 +36,13 @@ void setup() {
     while (1); // Halt
   }
 
-  hid.SetReportParser(KEYBOARD_PARSER_ID, (HIDReportParser*)&keyboardPrs);
-  hid.SetReportParser(MOUSE_PARSER_ID, (HIDReportParser*)&mousePrs);
+  bthid.SetReportParser(KEYBOARD_PARSER_ID, (HIDReportParser*)&keyboardPrs);
+  bthid.SetReportParser(MOUSE_PARSER_ID, (HIDReportParser*)&mousePrs);
 
   // If "Boot Protocol Mode" does not work, then try "Report Protocol Mode"
   // If that does not work either, then uncomment PRINTREPORT in BTHID.cpp to see the raw report
-  hid.setProtocolMode(HID_BOOT_PROTOCOL); // Boot Protocol Mode
-  //hid.setProtocolMode(HID_RPT_PROTOCOL); // Report Protocol Mode
+  bthid.setProtocolMode(HID_BOOT_PROTOCOL); // Boot Protocol Mode
+  //bthid.setProtocolMode(HID_RPT_PROTOCOL); // Report Protocol Mode
 
   Serial.print(F("\r\nHID Bluetooth Library Started"));
 }
diff --git a/examples/Bluetooth/BTHID/KeyboardParser.h b/examples/Bluetooth/BTHID/KeyboardParser.h
index b7ab08e9..c5394331 100644
--- a/examples/Bluetooth/BTHID/KeyboardParser.h
+++ b/examples/Bluetooth/BTHID/KeyboardParser.h
@@ -17,15 +17,15 @@ uint8_t KbdRptParser::HandleLockingKeys(HID *hid, uint8_t key) {
   uint8_t old_keys = kbdLockingKeys.bLeds;
 
   switch (key) {
-    case KEY_NUM_LOCK:
+    case UHS_HID_BOOT_KEY_NUM_LOCK:
       Serial.println(F("Num lock"));
       kbdLockingKeys.kbdLeds.bmNumLock = ~kbdLockingKeys.kbdLeds.bmNumLock;
       break;
-    case KEY_CAPS_LOCK:
+    case UHS_HID_BOOT_KEY_CAPS_LOCK:
       Serial.println(F("Caps lock"));
       kbdLockingKeys.kbdLeds.bmCapsLock = ~kbdLockingKeys.kbdLeds.bmCapsLock;
       break;
-    case KEY_SCROLL_LOCK:
+    case UHS_HID_BOOT_KEY_SCROLL_LOCK:
       Serial.println(F("Scroll lock"));
       kbdLockingKeys.kbdLeds.bmScrollLock = ~kbdLockingKeys.kbdLeds.bmScrollLock;
       break;
diff --git a/examples/Bluetooth/PS3BT/PS3BT.ino b/examples/Bluetooth/PS3BT/PS3BT.ino
index f8b13ad7..982eb384 100644
--- a/examples/Bluetooth/PS3BT/PS3BT.ino
+++ b/examples/Bluetooth/PS3BT/PS3BT.ino
@@ -6,6 +6,10 @@
 
 #include <PS3BT.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
@@ -13,7 +17,7 @@ USB Usb;
 BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 /* You can create the instance of the class in two ways */
 PS3BT PS3(&Btd); // This will just create the instance
-//PS3BT PS3(&Btd,0x00,0x15,0x83,0x3D,0x0A,0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
+//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
 
 boolean printTemperature;
 boolean printAngle;
@@ -48,7 +52,7 @@ void loop() {
     if (PS3.getAnalogButton(L2) || PS3.getAnalogButton(R2)) {
       Serial.print(F("\r\nL2: "));
       Serial.print(PS3.getAnalogButton(L2));
-      if (!PS3.PS3NavigationConnected) {
+      if (PS3.PS3Connected) {
         Serial.print(F("\tR2: "));
         Serial.print(PS3.getAnalogButton(R2));
       }
@@ -107,7 +111,7 @@ void loop() {
 
       if (PS3.getButtonClick(SELECT)) {
         Serial.print(F("\r\nSelect - "));
-        Serial.print(PS3.getStatusString());
+        PS3.printStatusString();
       }
       if (PS3.getButtonClick(START)) {
         Serial.print(F("\r\nStart"));
@@ -159,7 +163,7 @@ void loop() {
         PS3.moveSetBulb(Off);
         Serial.print(F("\r\nMove"));
         Serial.print(F(" - "));
-        Serial.print(PS3.getStatusString());
+        PS3.printStatusString();
       }
     }
     if (printAngle) {
diff --git a/examples/Bluetooth/PS3Multi/PS3Multi.ino b/examples/Bluetooth/PS3Multi/PS3Multi.ino
index d7ec631c..23e8cd9a 100644
--- a/examples/Bluetooth/PS3Multi/PS3Multi.ino
+++ b/examples/Bluetooth/PS3Multi/PS3Multi.ino
@@ -7,6 +7,10 @@
 
 #include <PS3BT.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
@@ -52,7 +56,7 @@ void loop() {
       if (PS3[i]->getAnalogButton(L2) || PS3[i]->getAnalogButton(R2)) {
         Serial.print(F("\r\nL2: "));
         Serial.print(PS3[i]->getAnalogButton(L2));
-        if (!PS3[i]->PS3NavigationConnected) {
+        if (PS3[i]->PS3Connected) {
           Serial.print(F("\tR2: "));
           Serial.print(PS3[i]->getAnalogButton(R2));
         }
@@ -112,7 +116,7 @@ void loop() {
 
         if (PS3[i]->getButtonClick(SELECT)) {
           Serial.print(F("\r\nSelect - "));
-          Serial.print(PS3[i]->getStatusString());
+          PS3[i]->printStatusString();
         }
         if (PS3[i]->getButtonClick(START)) {
           Serial.print(F("\r\nStart"));
@@ -135,7 +139,7 @@ void onInit() {
   for (uint8_t i = 0; i < length; i++) {
     if ((PS3[i]->PS3Connected || PS3[i]->PS3NavigationConnected) && !oldControllerState[i]) {
       oldControllerState[i] = true; // Used to check which is the new controller
-      PS3[i]->setLedOn((LED)i); // Cast directly to LED enum - see: "controllerEnums.h"
+      PS3[i]->setLedOn((LEDEnum)(i + 1)); // Cast directly to LEDEnum - see: "controllerEnums.h"
     }
   }
 }
diff --git a/examples/Bluetooth/PS3SPP/PS3SPP.ino b/examples/Bluetooth/PS3SPP/PS3SPP.ino
index 3f9af1f3..440ea79e 100644
--- a/examples/Bluetooth/PS3SPP/PS3SPP.ino
+++ b/examples/Bluetooth/PS3SPP/PS3SPP.ino
@@ -12,6 +12,10 @@
 #include <PS3BT.h>
 #include <SPP.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
@@ -22,7 +26,7 @@ BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 SPP SerialBT(&Btd); // This will set the name to the defaults: "Arduino" and the pin to "0000"
 //SPP SerialBTBT(&Btd,"Lauszus's Arduino","0000"); // You can also set the name and pin like so
 PS3BT PS3(&Btd); // This will just create the instance
-//PS3BT PS3(&Btd,0x00,0x15,0x83,0x3D,0x0A,0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
+//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
 
 boolean firstMessage = true;
 String output = ""; // We will store the data in this string
@@ -73,7 +77,7 @@ void loop() {
         output += "\r\n";
       output += "L2: ";
       output += PS3.getAnalogButton(L2);
-      if (!PS3.PS3NavigationConnected) {
+      if (PS3.PS3Connected) {
         output += "\tR2: ";
         output += PS3.getAnalogButton(R2);
       }
@@ -137,8 +141,7 @@ void loop() {
         output += " - R3";
 
       if (PS3.getButtonClick(SELECT)) {
-        output += " - Select - ";
-        output += PS3.getStatusString();
+        output += " - Select";
       }
       if (PS3.getButtonClick(START))
         output += " - Start";
diff --git a/examples/Bluetooth/PS4BT/PS4BT.ino b/examples/Bluetooth/PS4BT/PS4BT.ino
new file mode 100644
index 00000000..e47cfae0
--- /dev/null
+++ b/examples/Bluetooth/PS4BT/PS4BT.ino
@@ -0,0 +1,143 @@
+/*
+ Example sketch for the PS4 Bluetooth library - developed by Kristian Lauszus
+ For more information visit my blog: http://blog.tkjelectronics.dk/ or
+ send me an e-mail:  kristianl@tkjelectronics.com
+ */
+
+#include <PS4BT.h>
+#include <usbhub.h>
+
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+
+USB Usb;
+//USBHub Hub1(&Usb); // Some dongles have a hub inside
+BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
+
+/* You can create the instance of the PS4BT class in two ways */
+// This will start an inquiry and then pair with the PS4 controller - you only have to do this once
+// You will need to hold down the PS and Share button at the same time, the PS4 controller will then start to blink rapidly indicating that it is in paring mode
+PS4BT PS4(&Btd, PAIR);
+
+// After that you can simply create the instance like so and then press the PS button on the device
+//PS4BT PS4(&Btd);
+
+boolean printAngle, printTouch;
+uint8_t oldL2Value, oldR2Value;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while (1); // Halt
+  }
+  Serial.print(F("\r\nPS4 Bluetooth Library Started"));
+}
+void loop() {
+  Usb.Task();
+
+  if (PS4.connected()) {
+    if (PS4.getAnalogHat(LeftHatX) > 137 || PS4.getAnalogHat(LeftHatX) < 117 || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(LeftHatY) < 117 || PS4.getAnalogHat(RightHatX) > 137 || PS4.getAnalogHat(RightHatX) < 117 || PS4.getAnalogHat(RightHatY) > 137 || PS4.getAnalogHat(RightHatY) < 117) {
+      Serial.print(F("\r\nLeftHatX: "));
+      Serial.print(PS4.getAnalogHat(LeftHatX));
+      Serial.print(F("\tLeftHatY: "));
+      Serial.print(PS4.getAnalogHat(LeftHatY));
+      Serial.print(F("\tRightHatX: "));
+      Serial.print(PS4.getAnalogHat(RightHatX));
+      Serial.print(F("\tRightHatY: "));
+      Serial.print(PS4.getAnalogHat(RightHatY));
+    }
+
+    if (PS4.getAnalogButton(L2) || PS4.getAnalogButton(R2)) { // These are the only analog buttons on the PS4 controller
+      Serial.print(F("\r\nL2: "));
+      Serial.print(PS4.getAnalogButton(L2));
+      Serial.print(F("\tR2: "));
+      Serial.print(PS4.getAnalogButton(R2));
+    }
+    if (PS4.getAnalogButton(L2) != oldL2Value || PS4.getAnalogButton(R2) != oldR2Value) // Only write value if it's different
+      PS4.setRumbleOn(PS4.getAnalogButton(L2), PS4.getAnalogButton(R2));
+    oldL2Value = PS4.getAnalogButton(L2);
+    oldR2Value = PS4.getAnalogButton(R2);
+
+    if (PS4.getButtonClick(PS)) {
+      Serial.print(F("\r\nPS"));
+      PS4.disconnect();
+    }
+    else {
+      if (PS4.getButtonClick(TRIANGLE)) {
+        Serial.print(F("\r\nTraingle"));
+        PS4.setRumbleOn(RumbleLow);
+      }
+      if (PS4.getButtonClick(CIRCLE)) {
+        Serial.print(F("\r\nCircle"));
+        PS4.setRumbleOn(RumbleHigh);
+      }
+      if (PS4.getButtonClick(CROSS)) {
+        Serial.print(F("\r\nCross"));
+        PS4.setLedFlash(10, 10); // Set it to blink rapidly
+      }
+      if (PS4.getButtonClick(SQUARE)) {
+        Serial.print(F("\r\nSquare"));
+        PS4.setLedFlash(0, 0); // Turn off blinking
+      }
+
+      if (PS4.getButtonClick(UP)) {
+        Serial.print(F("\r\nUp"));
+        PS4.setLed(Red);
+      } if (PS4.getButtonClick(RIGHT)) {
+        Serial.print(F("\r\nRight"));
+        PS4.setLed(Blue);
+      } if (PS4.getButtonClick(DOWN)) {
+        Serial.print(F("\r\nDown"));
+        PS4.setLed(Yellow);
+      } if (PS4.getButtonClick(LEFT)) {
+        Serial.print(F("\r\nLeft"));
+        PS4.setLed(Green);
+      }
+
+      if (PS4.getButtonClick(L1))
+        Serial.print(F("\r\nL1"));
+      if (PS4.getButtonClick(L3))
+        Serial.print(F("\r\nL3"));
+      if (PS4.getButtonClick(R1))
+        Serial.print(F("\r\nR1"));
+      if (PS4.getButtonClick(R3))
+        Serial.print(F("\r\nR3"));
+
+      if (PS4.getButtonClick(SHARE))
+        Serial.print(F("\r\nShare"));
+      if (PS4.getButtonClick(OPTIONS)) {
+        Serial.print(F("\r\nOptions"));
+        printAngle = !printAngle;
+      }
+      if (PS4.getButtonClick(TOUCHPAD)) {
+        Serial.print(F("\r\nTouchpad"));
+        printTouch = !printTouch;
+      }
+
+      if (printAngle) { // Print angle calculated using the accelerometer only
+        Serial.print(F("\r\nPitch: "));
+        Serial.print(PS4.getAngle(Pitch));
+        Serial.print(F("\tRoll: "));
+        Serial.print(PS4.getAngle(Roll));
+      }
+
+      if (printTouch) { // Print the x, y coordinates of the touchpad
+        if (PS4.isTouching(0) || PS4.isTouching(1)) // Print newline and carriage return if any of the fingers are touching the touchpad
+          Serial.print(F("\r\n"));
+        for (uint8_t i = 0; i < 2; i++) { // The touchpad track two fingers
+          if (PS4.isTouching(i)) { // Print the position of the finger if it is touching the touchpad
+            Serial.print(F("X")); Serial.print(i + 1); Serial.print(F(": "));
+            Serial.print(PS4.getX(i));
+            Serial.print(F("\tY")); Serial.print(i + 1); Serial.print(F(": "));
+            Serial.print(PS4.getY(i));
+            Serial.print(F("\t"));
+          }
+        }
+      }
+    }
+  }
+}
diff --git a/examples/Bluetooth/SPP/SPP.ino b/examples/Bluetooth/SPP/SPP.ino
index e5b92089..d8276b7b 100644
--- a/examples/Bluetooth/SPP/SPP.ino
+++ b/examples/Bluetooth/SPP/SPP.ino
@@ -6,14 +6,18 @@
 
 #include <SPP.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
 
 BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 /* You can create the instance of the class in two ways */
-SPP SerialBT(&Btd); // This will set the name to the defaults: "Arduino" and the pin to "1234"
-//SPP SerialBT(&Btd, "Lauszus's Arduino","0000"); // You can also set the name and pin like so
+SPP SerialBT(&Btd); // This will set the name to the defaults: "Arduino" and the pin to "0000"
+//SPP SerialBT(&Btd, "Lauszus's Arduino", "1234"); // You can also set the name and pin like so
 
 boolean firstMessage = true;
 
diff --git a/examples/Bluetooth/SPPMulti/SPPMulti.ino b/examples/Bluetooth/SPPMulti/SPPMulti.ino
index 3388bd7b..0304257e 100644
--- a/examples/Bluetooth/SPPMulti/SPPMulti.ino
+++ b/examples/Bluetooth/SPPMulti/SPPMulti.ino
@@ -6,15 +6,20 @@
 
 #include <SPP.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statement in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
 
 BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
-SPP *SerialBT[2]; // We will use this pointer to store the two instance, you can easily make it larger if you like, but it will use a lot of RAM!
-const uint8_t length = sizeof(SerialBT) / sizeof(SerialBT[0]); // Get the lenght of the array
+
+const uint8_t length = 2; // Set the number of instances here
+SPP *SerialBT[length]; // We will use this pointer to store the instances, you can easily make it larger if you like, but it will use a lot of RAM!
+
 boolean firstMessage[length] = { true }; // Set all to true
-uint8_t buffer[50];
 
 void setup() {
   for (uint8_t i = 0; i < length; i++)
@@ -24,10 +29,11 @@ void setup() {
   while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
   if (Usb.Init() == -1) {
     Serial.print(F("\r\nOSC did not start"));
-    while (1); //halt
+    while (1); // Halt
   }
   Serial.print(F("\r\nSPP Bluetooth Library Started"));
 }
+
 void loop() {
   Usb.Task(); // The SPP data is actually not send until this is called, one could call SerialBT.send() directly as well
 
@@ -43,22 +49,15 @@ void loop() {
     else
       firstMessage[i] = true;
   }
+
+  // Set the connection you want to send to using the first character
+  // For instance "0Hello World" would send "Hello World" to connection 0
   if (Serial.available()) {
     delay(10); // Wait for the rest of the data to arrive
-    uint8_t i = 0;
-    while (Serial.available() && i < sizeof(buffer)) // Read the data
-      buffer[i++] = Serial.read();
-    /*
-      Set the connection you want to send to using the first character
-      For instace "0Hello World" would send "Hello World" to connection 0
-    */
-    uint8_t id = buffer[0] - '0'; // Convert from ASCII
-    if (id < length && i > 1) { // And then compare to length and make sure there is any text
-      if (SerialBT[id]->connected) { // Check if a device is actually connected
-        for (uint8_t i2 = 0; i2 < i - 1; i2++) // Don't include the first character
-          buffer[i2] = buffer[i2 + 1];
-        SerialBT[id]->write(buffer, i - 1); // Send the data
-      }
+    uint8_t id = Serial.read() - '0'; // Convert from ASCII
+    if (id < length && SerialBT[id]->connected) { // Make sure that the id is valid and make sure that a device is actually connected
+      while (Serial.available()) // Check if data is available
+        SerialBT[id]->write(Serial.read()); // Send the data
     }
   }
 }
diff --git a/examples/Bluetooth/Wii/Wii.ino b/examples/Bluetooth/Wii/Wii.ino
index 214625c5..f4d68a62 100644
--- a/examples/Bluetooth/Wii/Wii.ino
+++ b/examples/Bluetooth/Wii/Wii.ino
@@ -6,6 +6,10 @@
 
 #include <Wii.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
diff --git a/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino b/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino
index 5d0fa91b..e7ca456b 100644
--- a/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino
+++ b/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino
@@ -13,9 +13,13 @@ Otherwise, wire up a IR LED yourself.
 
 #include <Wii.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 #ifndef WIICAMERA // Used to check if WIICAMERA is defined
-#error "Uncomment WIICAMERA in Wii.h to use this example"
+#error "Please set ENABLE_WII_IR_CAMERA to 1 in settings.h"
 #endif
 
 USB Usb;
diff --git a/examples/Bluetooth/WiiMulti/WiiMulti.ino b/examples/Bluetooth/WiiMulti/WiiMulti.ino
index 51c9160b..08444f10 100644
--- a/examples/Bluetooth/WiiMulti/WiiMulti.ino
+++ b/examples/Bluetooth/WiiMulti/WiiMulti.ino
@@ -7,6 +7,10 @@
 
 #include <Wii.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
@@ -118,7 +122,7 @@ void onInit() {
   for (uint8_t i = 0; i < length; i++) {
     if (Wii[i]->wiimoteConnected && !oldControllerState[i]) {
       oldControllerState[i] = true; // Used to check which is the new controller
-      Wii[i]->setLedOn((LED)i); // Cast directly to LED enum - see: "controllerEnums.h"
+      Wii[i]->setLedOn((LEDEnum)(i + 1)); // Cast directly to LEDEnum - see: "controllerEnums.h"
     }
   }
 }
diff --git a/examples/Bluetooth/WiiUProController/WiiUProController.ino b/examples/Bluetooth/WiiUProController/WiiUProController.ino
index eb686879..59c4f1dc 100644
--- a/examples/Bluetooth/WiiUProController/WiiUProController.ino
+++ b/examples/Bluetooth/WiiUProController/WiiUProController.ino
@@ -6,6 +6,10 @@
 
 #include <Wii.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 //USBHub Hub1(&Usb); // Some dongles have a hub inside
diff --git a/examples/HID/USBHIDBootKbd/USBHIDBootKbd.ino b/examples/HID/USBHIDBootKbd/USBHIDBootKbd.ino
index b3adb558..b5944f0a 100644
--- a/examples/HID/USBHIDBootKbd/USBHIDBootKbd.ino
+++ b/examples/HID/USBHIDBootKbd/USBHIDBootKbd.ino
@@ -1,5 +1,9 @@
 #include <hidboot.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class KbdRptParser : public KeyboardReportParser
 {
diff --git a/examples/HID/USBHIDBootKbdAndMouse/Makefile b/examples/HID/USBHIDBootKbdAndMouse/Makefile
deleted file mode 100644
index 253ecfb5..00000000
--- a/examples/HID/USBHIDBootKbdAndMouse/Makefile
+++ /dev/null
@@ -1,30 +0,0 @@
-#
-# These are set for a mega 1280 + quadram plus my serial patch.
-# If you lack quadram, or want to disable LFN, just change _FS_TINY=1 _USE_LFN=0
-#
-# If your board is a mega 2560 uncomment the following two lines
-# BOARD = mega2560
-# PROGRAMMER = wiring
-# ...and then comment out the following two lines
-BOARD = mega
-PROGRAMMER = arduino
-
-# set your Arduino tty port here
-PORT = /dev/ttyUSB0
-
-
-# uncomment the next line to enable debugging
-#EXTRA_FLAGS += -D DEBUG_USB_HOST=1
-
-#
-# Advanced debug on Serial3
-#
-
-# uncomment the next line to enable debug on Serial3
-#EXTRA_FLAGS += -D USB_HOST_SERIAL=Serial3
-
-# The following are the libraries used.
-LIB_DIRS =
-LIB_DIRS += ../libraries/USB_Host_Shield_2_0
-# And finally, the part that brings everything together for you.
-include ../Arduino_Makefile_master/_Makefile.master
diff --git a/examples/HID/USBHIDBootKbdAndMouse/nbproject/Package-Default.bash b/examples/HID/USBHIDBootKbdAndMouse/nbproject/Package-Default.bash
deleted file mode 100644
index 808a7e12..00000000
--- a/examples/HID/USBHIDBootKbdAndMouse/nbproject/Package-Default.bash
+++ /dev/null
@@ -1,75 +0,0 @@
-#!/bin/bash -x
-
-#
-# Generated - do not edit!
-#
-
-# Macros
-TOP=`pwd`
-CND_PLATFORM=AVR-Linux-x86
-CND_CONF=Default
-CND_DISTDIR=dist
-CND_BUILDDIR=build
-NBTMPDIR=${CND_BUILDDIR}/${CND_CONF}/${CND_PLATFORM}/tmp-packaging
-TMPDIRNAME=tmp-packaging
-OUTPUT_PATH=MissingOutputInProject
-OUTPUT_BASENAME=MissingOutputInProject
-PACKAGE_TOP_DIR=USBHIDBootKbdAndMouse/
-
-# Functions
-function checkReturnCode
-{
-    rc=$?
-    if [ $rc != 0 ]
-    then
-        exit $rc
-    fi
-}
-function makeDirectory
-# $1 directory path
-# $2 permission (optional)
-{
-    mkdir -p "$1"
-    checkReturnCode
-    if [ "$2" != "" ]
-    then
-      chmod $2 "$1"
-      checkReturnCode
-    fi
-}
-function copyFileToTmpDir
-# $1 from-file path
-# $2 to-file path
-# $3 permission
-{
-    cp "$1" "$2"
-    checkReturnCode
-    if [ "$3" != "" ]
-    then
-        chmod $3 "$2"
-        checkReturnCode
-    fi
-}
-
-# Setup
-cd "${TOP}"
-mkdir -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package
-rm -rf ${NBTMPDIR}
-mkdir -p ${NBTMPDIR}
-
-# Copy files and create directories and links
-cd "${TOP}"
-makeDirectory "${NBTMPDIR}/USBHIDBootKbdAndMouse"
-copyFileToTmpDir "${OUTPUT_PATH}" "${NBTMPDIR}/${PACKAGE_TOP_DIR}bin/${OUTPUT_BASENAME}" 0755
-
-
-# Generate tar file
-cd "${TOP}"
-rm -f ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package/USBHIDBootKbdAndMouse.tar
-cd ${NBTMPDIR}
-tar -vcf ../../../../${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package/USBHIDBootKbdAndMouse.tar *
-checkReturnCode
-
-# Cleanup
-cd "${TOP}"
-rm -rf ${NBTMPDIR}
diff --git a/examples/HID/USBHIDBootKbdAndMouse/nbproject/configurations.xml b/examples/HID/USBHIDBootKbdAndMouse/nbproject/configurations.xml
deleted file mode 100644
index e19c68a4..00000000
--- a/examples/HID/USBHIDBootKbdAndMouse/nbproject/configurations.xml
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<configurationDescriptor version="80">
-  <logicalFolder name="root" displayName="root" projectFiles="true" kind="ROOT">
-    <df name="USBHIDBootKbd" root=".">
-      <df name="build">
-        <in>USBHIDBootKbdAndMouse_ino.cpp</in>
-      </df>
-      <in>USBHIDBootKbdAndMouse.ino</in>
-    </df>
-    <logicalFolder name="ExternalFiles"
-                   displayName="Important Files"
-                   projectFiles="false"
-                   kind="IMPORTANT_FILES_FOLDER">
-      <itemPath>Makefile</itemPath>
-    </logicalFolder>
-  </logicalFolder>
-  <sourceFolderFilter>^(nbproject)$</sourceFolderFilter>
-  <sourceRootList>
-    <Elem>.</Elem>
-  </sourceRootList>
-  <projectmakefile>Makefile</projectmakefile>
-  <confs>
-    <conf name="Default" type="0">
-      <toolsSet>
-        <remote-sources-mode>LOCAL_SOURCES</remote-sources-mode>
-        <compilerSet>default</compilerSet>
-      </toolsSet>
-      <makefileType>
-        <makeTool>
-          <buildCommandWorkingDir>.</buildCommandWorkingDir>
-          <buildCommand>${MAKE} -f Makefile</buildCommand>
-          <cleanCommand>${MAKE} -f Makefile clean</cleanCommand>
-          <executablePath></executablePath>
-        </makeTool>
-      </makefileType>
-    </conf>
-  </confs>
-</configurationDescriptor>
diff --git a/examples/HID/USBHIDBootKbdAndMouse/nbproject/project.xml b/examples/HID/USBHIDBootKbdAndMouse/nbproject/project.xml
deleted file mode 100644
index c831722a..00000000
--- a/examples/HID/USBHIDBootKbdAndMouse/nbproject/project.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project xmlns="http://www.netbeans.org/ns/project/1">
-    <type>org.netbeans.modules.cnd.makeproject</type>
-    <configuration>
-        <data xmlns="http://www.netbeans.org/ns/make-project/1">
-            <name>USBHIDBootKbdAndMouse</name>
-            <c-extensions/>
-            <cpp-extensions>cpp,ino</cpp-extensions>
-            <header-extensions/>
-            <sourceEncoding>UTF-8</sourceEncoding>
-            <make-dep-projects/>
-            <sourceRootList>
-                <sourceRootElem>.</sourceRootElem>
-            </sourceRootList>
-            <confList>
-                <confElem>
-                    <name>Default</name>
-                    <type>0</type>
-                </confElem>
-            </confList>
-        </data>
-    </configuration>
-</project>
diff --git a/examples/HID/USBHIDBootMouse/USBHIDBootMouse.ino b/examples/HID/USBHIDBootMouse/USBHIDBootMouse.ino
index 80dbcd4d..aa2e3b34 100644
--- a/examples/HID/USBHIDBootMouse/USBHIDBootMouse.ino
+++ b/examples/HID/USBHIDBootMouse/USBHIDBootMouse.ino
@@ -1,5 +1,9 @@
 #include <hidboot.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class MouseRptParser : public MouseReportParser
 {
@@ -12,14 +16,14 @@ protected:
 	virtual void OnMiddleButtonUp	(MOUSEINFO *mi);
 	virtual void OnMiddleButtonDown	(MOUSEINFO *mi);
 };
-void MouseRptParser::OnMouseMove(MOUSEINFO *mi)	
+void MouseRptParser::OnMouseMove(MOUSEINFO *mi)
 {
     Serial.print("dx=");
     Serial.print(mi->dX, DEC);
     Serial.print(" dy=");
     Serial.println(mi->dY, DEC);
 };
-void MouseRptParser::OnLeftButtonUp	(MOUSEINFO *mi)	
+void MouseRptParser::OnLeftButtonUp	(MOUSEINFO *mi)
 {
     Serial.println("L Butt Up");
 };
@@ -60,11 +64,11 @@ void setup()
 
     if (Usb.Init() == -1)
         Serial.println("OSC did not start.");
-      
+
     delay( 200 );
-  
+
     next_time = millis() + 5000;
-  
+
     HidMouse.SetReportParser(0,(HIDReportParser*)&Prs);
 }
 
diff --git a/examples/HID/USBHIDJoystick/Makefile b/examples/HID/USBHIDJoystick/Makefile
deleted file mode 100644
index 253ecfb5..00000000
--- a/examples/HID/USBHIDJoystick/Makefile
+++ /dev/null
@@ -1,30 +0,0 @@
-#
-# These are set for a mega 1280 + quadram plus my serial patch.
-# If you lack quadram, or want to disable LFN, just change _FS_TINY=1 _USE_LFN=0
-#
-# If your board is a mega 2560 uncomment the following two lines
-# BOARD = mega2560
-# PROGRAMMER = wiring
-# ...and then comment out the following two lines
-BOARD = mega
-PROGRAMMER = arduino
-
-# set your Arduino tty port here
-PORT = /dev/ttyUSB0
-
-
-# uncomment the next line to enable debugging
-#EXTRA_FLAGS += -D DEBUG_USB_HOST=1
-
-#
-# Advanced debug on Serial3
-#
-
-# uncomment the next line to enable debug on Serial3
-#EXTRA_FLAGS += -D USB_HOST_SERIAL=Serial3
-
-# The following are the libraries used.
-LIB_DIRS =
-LIB_DIRS += ../libraries/USB_Host_Shield_2_0
-# And finally, the part that brings everything together for you.
-include ../Arduino_Makefile_master/_Makefile.master
diff --git a/examples/HID/USBHIDJoystick/nbproject/Package-Default.bash b/examples/HID/USBHIDJoystick/nbproject/Package-Default.bash
deleted file mode 100644
index 45e0f31e..00000000
--- a/examples/HID/USBHIDJoystick/nbproject/Package-Default.bash
+++ /dev/null
@@ -1,75 +0,0 @@
-#!/bin/bash -x
-
-#
-# Generated - do not edit!
-#
-
-# Macros
-TOP=`pwd`
-CND_PLATFORM=AVR-Linux-x86
-CND_CONF=Default
-CND_DISTDIR=dist
-CND_BUILDDIR=build
-NBTMPDIR=${CND_BUILDDIR}/${CND_CONF}/${CND_PLATFORM}/tmp-packaging
-TMPDIRNAME=tmp-packaging
-OUTPUT_PATH=MissingOutputInProject
-OUTPUT_BASENAME=MissingOutputInProject
-PACKAGE_TOP_DIR=USBHIDJoystick/
-
-# Functions
-function checkReturnCode
-{
-    rc=$?
-    if [ $rc != 0 ]
-    then
-        exit $rc
-    fi
-}
-function makeDirectory
-# $1 directory path
-# $2 permission (optional)
-{
-    mkdir -p "$1"
-    checkReturnCode
-    if [ "$2" != "" ]
-    then
-      chmod $2 "$1"
-      checkReturnCode
-    fi
-}
-function copyFileToTmpDir
-# $1 from-file path
-# $2 to-file path
-# $3 permission
-{
-    cp "$1" "$2"
-    checkReturnCode
-    if [ "$3" != "" ]
-    then
-        chmod $3 "$2"
-        checkReturnCode
-    fi
-}
-
-# Setup
-cd "${TOP}"
-mkdir -p ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package
-rm -rf ${NBTMPDIR}
-mkdir -p ${NBTMPDIR}
-
-# Copy files and create directories and links
-cd "${TOP}"
-makeDirectory "${NBTMPDIR}/USBHIDJoystick"
-copyFileToTmpDir "${OUTPUT_PATH}" "${NBTMPDIR}/${PACKAGE_TOP_DIR}bin/${OUTPUT_BASENAME}" 0755
-
-
-# Generate tar file
-cd "${TOP}"
-rm -f ${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package/USBHIDJoystick.tar
-cd ${NBTMPDIR}
-tar -vcf ../../../../${CND_DISTDIR}/${CND_CONF}/${CND_PLATFORM}/package/USBHIDJoystick.tar *
-checkReturnCode
-
-# Cleanup
-cd "${TOP}"
-rm -rf ${NBTMPDIR}
diff --git a/examples/HID/USBHIDJoystick/nbproject/configurations.xml b/examples/HID/USBHIDJoystick/nbproject/configurations.xml
deleted file mode 100644
index 5bb562ca..00000000
--- a/examples/HID/USBHIDJoystick/nbproject/configurations.xml
+++ /dev/null
@@ -1,55 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<configurationDescriptor version="80">
-  <logicalFolder name="root" displayName="root" projectFiles="true" kind="ROOT">
-    <df name="USBHIDJoystick" root=".">
-      <df name="build">
-        <in>CDC.lst</in>
-        <in>HID.lst</in>
-        <in>HardwareSerial.lst</in>
-        <in>IPAddress.lst</in>
-        <in>Print.lst</in>
-        <in>Stream.lst</in>
-        <in>Tone.lst</in>
-        <in>USBCore.lst</in>
-        <in>WInterrupts.lst</in>
-        <in>WMath.lst</in>
-        <in>WString.lst</in>
-        <in>wiring.lst</in>
-        <in>wiring_analog.lst</in>
-        <in>wiring_digital.lst</in>
-        <in>wiring_pulse.lst</in>
-        <in>wiring_shift.lst</in>
-      </df>
-      <in>USBHIDJoystick.ino</in>
-      <in>hidjoystickrptparser.cpp</in>
-      <in>hidjoystickrptparser.h</in>
-    </df>
-    <logicalFolder name="ExternalFiles"
-                   displayName="Important Files"
-                   projectFiles="false"
-                   kind="IMPORTANT_FILES_FOLDER">
-      <itemPath>Makefile</itemPath>
-    </logicalFolder>
-  </logicalFolder>
-  <sourceFolderFilter>^(nbproject)$</sourceFolderFilter>
-  <sourceRootList>
-    <Elem>.</Elem>
-  </sourceRootList>
-  <projectmakefile>Makefile</projectmakefile>
-  <confs>
-    <conf name="Default" type="0">
-      <toolsSet>
-        <remote-sources-mode>LOCAL_SOURCES</remote-sources-mode>
-        <compilerSet>default</compilerSet>
-      </toolsSet>
-      <makefileType>
-        <makeTool>
-          <buildCommandWorkingDir>.</buildCommandWorkingDir>
-          <buildCommand>${MAKE} -f Makefile</buildCommand>
-          <cleanCommand>${MAKE} -f Makefile clean</cleanCommand>
-          <executablePath></executablePath>
-        </makeTool>
-      </makefileType>
-    </conf>
-  </confs>
-</configurationDescriptor>
diff --git a/examples/HID/USBHIDJoystick/nbproject/project.xml b/examples/HID/USBHIDJoystick/nbproject/project.xml
deleted file mode 100644
index b912fd91..00000000
--- a/examples/HID/USBHIDJoystick/nbproject/project.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project xmlns="http://www.netbeans.org/ns/project/1">
-    <type>org.netbeans.modules.cnd.makeproject</type>
-    <configuration>
-        <data xmlns="http://www.netbeans.org/ns/make-project/1">
-            <name>USBHIDJoystick</name>
-            <c-extensions/>
-            <cpp-extensions>cpp,ino</cpp-extensions>
-            <header-extensions>h</header-extensions>
-            <sourceEncoding>UTF-8</sourceEncoding>
-            <make-dep-projects/>
-            <sourceRootList>
-                <sourceRootElem>.</sourceRootElem>
-            </sourceRootList>
-            <confList>
-                <confElem>
-                    <name>Default</name>
-                    <type>0</type>
-                </confElem>
-            </confList>
-        </data>
-    </configuration>
-</project>
diff --git a/examples/HID/USBHID_desc/USBHID_desc.ino b/examples/HID/USBHID_desc/USBHID_desc.ino
index b156cafe..806936f9 100644
--- a/examples/HID/USBHID_desc/USBHID_desc.ino
+++ b/examples/HID/USBHID_desc/USBHID_desc.ino
@@ -2,14 +2,17 @@
 #include <hiduniversal.h>
 #include <hidescriptorparser.h>
 #include <usbhub.h>
-
-#include "pgmstrings.h"  
+#include "pgmstrings.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class HIDUniversal2 : public HIDUniversal
 {
 public:
     HIDUniversal2(USB *usb) : HIDUniversal(usb) {};
-    
+
 protected:
     virtual uint8_t OnInitSuccessful();
 };
@@ -17,14 +20,14 @@ protected:
 uint8_t HIDUniversal2::OnInitSuccessful()
 {
     uint8_t    rcode;
-    
+
     HexDumper<USBReadParser, uint16_t, uint16_t>    Hex;
     ReportDescParser                                Rpt;
 
-    if (rcode = GetReportDescr(0, &Hex))
+    if ((rcode = GetReportDescr(0, &Hex)))
         goto FailGetReportDescr1;
-	        
-    if (rcode = GetReportDescr(0, &Rpt))
+
+    if ((rcode = GetReportDescr(0, &Rpt)))
 	goto FailGetReportDescr2;
 
     return 0;
@@ -43,10 +46,10 @@ Fail:
     return rcode;
 }
 
-USB                                             Usb;
-//USBHub                                          Hub(&Usb);
-HIDUniversal2                                   Hid(&Usb);
-UniversalReportParser                           Uni;
+USB Usb;
+//USBHub Hub(&Usb);
+HIDUniversal2 Hid(&Usb);
+UniversalReportParser Uni;
 
 void setup()
 {
@@ -56,11 +59,11 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSC did not start.");
-      
+
   delay( 200 );
 
   if (!Hid.SetReportParser(0, &Uni))
-      ErrorMessage<uint8_t>(PSTR("SetReportParser"), 1  ); 
+      ErrorMessage<uint8_t>(PSTR("SetReportParser"), 1  );
 }
 
 void loop()
diff --git a/examples/HID/le3dp/le3dp.ino b/examples/HID/le3dp/le3dp.ino
index 3728b307..fbf07b2f 100644
--- a/examples/HID/le3dp/le3dp.ino
+++ b/examples/HID/le3dp/le3dp.ino
@@ -5,6 +5,10 @@
 #include <usbhub.h>
 
 #include "le3dp_rptparser.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB                                             Usb;
 USBHub                                          Hub(&Usb);
@@ -20,11 +24,11 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSC did not start.");
-      
+
   delay( 200 );
 
   if (!Hid.SetReportParser(0, &Joy))
-      ErrorMessage<uint8_t>(PSTR("SetReportParser"), 1  ); 
+      ErrorMessage<uint8_t>(PSTR("SetReportParser"), 1  );
 }
 
 void loop()
diff --git a/examples/HID/scale/scale.ino b/examples/HID/scale/scale.ino
index 2f48c4c0..c790cb1c 100644
--- a/examples/HID/scale/scale.ino
+++ b/examples/HID/scale/scale.ino
@@ -6,6 +6,10 @@
 #include <usbhub.h>
 
 #include "scale_rptparser.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB                                             Usb;
 USBHub                                          Hub(&Usb);
@@ -22,18 +26,18 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSC did not start.");
-      
-    // set up the LCD's number of rows and columns: 
+
+    // set up the LCD's number of rows and columns:
     LCD.begin(16, 2);
     LCD.clear();
     LCD.home();
     LCD.setCursor(0,0);
-    LCD.write('R');  
-      
+    LCD.write('R');
+
   delay( 200 );
 
   if (!Hid.SetReportParser(0, &Scale))
-      ErrorMessage<uint8_t>(PSTR("SetReportParser"), 1  ); 
+      ErrorMessage<uint8_t>(PSTR("SetReportParser"), 1  );
 }
 
 void loop()
diff --git a/examples/PS3USB/PS3USB.ino b/examples/PS3USB/PS3USB.ino
index b3e6e2c4..2db3afd5 100644
--- a/examples/PS3USB/PS3USB.ino
+++ b/examples/PS3USB/PS3USB.ino
@@ -5,6 +5,10 @@
  */
 
 #include <PS3USB.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 /* You can create the instance of the class in two ways */
@@ -92,7 +96,7 @@ void loop() {
 
     if (PS3.getButtonClick(SELECT)) {
       Serial.print(F("\r\nSelect - "));
-      Serial.print(PS3.getStatusString());
+      PS3.printStatusString();
     }
     if (PS3.getButtonClick(START)) {
       Serial.print(F("\r\nStart"));
diff --git a/examples/PS4USB/PS4USB.ino b/examples/PS4USB/PS4USB.ino
new file mode 100644
index 00000000..b3a2436b
--- /dev/null
+++ b/examples/PS4USB/PS4USB.ino
@@ -0,0 +1,130 @@
+/*
+ Example sketch for the PS4 USB library - developed by Kristian Lauszus
+ For more information visit my blog: http://blog.tkjelectronics.dk/ or
+ send me an e-mail:  kristianl@tkjelectronics.com
+ */
+
+#include <PS4USB.h>
+
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+
+USB Usb;
+PS4USB PS4(&Usb);
+
+boolean printAngle, printTouch;
+uint8_t oldL2Value, oldR2Value;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while (1); // Halt
+  }
+  Serial.print(F("\r\nPS4 USB Library Started"));
+}
+
+void loop() {
+  Usb.Task();
+
+  if (PS4.connected()) {
+    if (PS4.getAnalogHat(LeftHatX) > 137 || PS4.getAnalogHat(LeftHatX) < 117 || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(LeftHatY) < 117 || PS4.getAnalogHat(RightHatX) > 137 || PS4.getAnalogHat(RightHatX) < 117 || PS4.getAnalogHat(RightHatY) > 137 || PS4.getAnalogHat(RightHatY) < 117) {
+      Serial.print(F("\r\nLeftHatX: "));
+      Serial.print(PS4.getAnalogHat(LeftHatX));
+      Serial.print(F("\tLeftHatY: "));
+      Serial.print(PS4.getAnalogHat(LeftHatY));
+      Serial.print(F("\tRightHatX: "));
+      Serial.print(PS4.getAnalogHat(RightHatX));
+      Serial.print(F("\tRightHatY: "));
+      Serial.print(PS4.getAnalogHat(RightHatY));
+    }
+
+    if (PS4.getAnalogButton(L2) || PS4.getAnalogButton(R2)) { // These are the only analog buttons on the PS4 controller
+      Serial.print(F("\r\nL2: "));
+      Serial.print(PS4.getAnalogButton(L2));
+      Serial.print(F("\tR2: "));
+      Serial.print(PS4.getAnalogButton(R2));
+    }
+    if (PS4.getAnalogButton(L2) != oldL2Value || PS4.getAnalogButton(R2) != oldR2Value) // Only write value if it's different
+      PS4.setRumbleOn(PS4.getAnalogButton(L2), PS4.getAnalogButton(R2));
+    oldL2Value = PS4.getAnalogButton(L2);
+    oldR2Value = PS4.getAnalogButton(R2);
+
+    if (PS4.getButtonClick(PS))
+      Serial.print(F("\r\nPS"));
+    if (PS4.getButtonClick(TRIANGLE)) {
+      Serial.print(F("\r\nTraingle"));
+      PS4.setRumbleOn(RumbleLow);
+    }
+    if (PS4.getButtonClick(CIRCLE)) {
+      Serial.print(F("\r\nCircle"));
+      PS4.setRumbleOn(RumbleHigh);
+    }
+    if (PS4.getButtonClick(CROSS)) {
+      Serial.print(F("\r\nCross"));
+      PS4.setLedFlash(10, 10); // Set it to blink rapidly
+    }
+    if (PS4.getButtonClick(SQUARE)) {
+      Serial.print(F("\r\nSquare"));
+      PS4.setLedFlash(0, 0); // Turn off blinking
+    }
+
+    if (PS4.getButtonClick(UP)) {
+      Serial.print(F("\r\nUp"));
+      PS4.setLed(Red);
+    } if (PS4.getButtonClick(RIGHT)) {
+      Serial.print(F("\r\nRight"));
+      PS4.setLed(Blue);
+    } if (PS4.getButtonClick(DOWN)) {
+      Serial.print(F("\r\nDown"));
+      PS4.setLed(Yellow);
+    } if (PS4.getButtonClick(LEFT)) {
+      Serial.print(F("\r\nLeft"));
+      PS4.setLed(Green);
+    }
+
+    if (PS4.getButtonClick(L1))
+      Serial.print(F("\r\nL1"));
+    if (PS4.getButtonClick(L3))
+      Serial.print(F("\r\nL3"));
+    if (PS4.getButtonClick(R1))
+      Serial.print(F("\r\nR1"));
+    if (PS4.getButtonClick(R3))
+      Serial.print(F("\r\nR3"));
+
+    if (PS4.getButtonClick(SHARE))
+      Serial.print(F("\r\nShare"));
+    if (PS4.getButtonClick(OPTIONS)) {
+      Serial.print(F("\r\nOptions"));
+      printAngle = !printAngle;
+    }
+    if (PS4.getButtonClick(TOUCHPAD)) {
+      Serial.print(F("\r\nTouchpad"));
+      printTouch = !printTouch;
+    }
+
+    if (printAngle) { // Print angle calculated using the accelerometer only
+      Serial.print(F("\r\nPitch: "));
+      Serial.print(PS4.getAngle(Pitch));
+      Serial.print(F("\tRoll: "));
+      Serial.print(PS4.getAngle(Roll));
+    }
+
+    if (printTouch) { // Print the x, y coordinates of the touchpad
+      if (PS4.isTouching(0) || PS4.isTouching(1)) // Print newline and carriage return if any of the fingers are touching the touchpad
+        Serial.print(F("\r\n"));
+      for (uint8_t i = 0; i < 2; i++) { // The touchpad track two fingers
+        if (PS4.isTouching(i)) { // Print the position of the finger if it is touching the touchpad
+          Serial.print(F("X")); Serial.print(i + 1); Serial.print(F(": "));
+          Serial.print(PS4.getX(i));
+          Serial.print(F("\tY")); Serial.print(i + 1); Serial.print(F(": "));
+          Serial.print(PS4.getY(i));
+          Serial.print(F("\t"));
+        }
+      }
+    }
+  }
+}
diff --git a/examples/PSBuzz/PSBuzz.ino b/examples/PSBuzz/PSBuzz.ino
new file mode 100644
index 00000000..6944cb6b
--- /dev/null
+++ b/examples/PSBuzz/PSBuzz.ino
@@ -0,0 +1,46 @@
+/*
+ Example sketch for the Playstation Buzz library - developed by Kristian Lauszus
+ For more information visit my blog: http://blog.tkjelectronics.dk/ or
+ send me an e-mail:  kristianl@tkjelectronics.com
+ */
+
+#include <PSBuzz.h>
+
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+
+USB Usb;
+PSBuzz Buzz(&Usb);
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while (1); // Halt
+  }
+  Serial.println(F("\r\nPS Buzz Library Started"));
+}
+
+void loop() {
+  Usb.Task();
+
+  if (Buzz.connected()) {
+    for (uint8_t i = 0; i < 4; i++) {
+      if (Buzz.getButtonClick(RED, i)) {
+        Buzz.setLedToggle(i); // Toggle the LED
+        Serial.println(F("RED"));
+      }
+      if (Buzz.getButtonClick(YELLOW, i))
+        Serial.println(F("YELLOW"));
+      if (Buzz.getButtonClick(GREEN, i))
+        Serial.println(F("GREEN"));
+      if (Buzz.getButtonClick(ORANGE, i))
+        Serial.println(F("ORANGE"));
+      if (Buzz.getButtonClick(BLUE, i))
+        Serial.println(F("BLUE"));
+    }
+  }
+}
diff --git a/examples/USB_desc/USB_desc.ino b/examples/USB_desc/USB_desc.ino
index 7f4a363a..b0013914 100644
--- a/examples/USB_desc/USB_desc.ino
+++ b/examples/USB_desc/USB_desc.ino
@@ -1,6 +1,10 @@
 #include <usbhub.h>
 
 #include "pgmstrings.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB     Usb;
 //USBHub  Hub1(&Usb);
@@ -16,7 +20,7 @@ uint32_t next_time;
 void PrintAllAddresses(UsbDevice *pdev)
 {
     UsbDeviceAddress adr;
-    adr.devAddress = pdev->address;
+    adr.devAddress = pdev->address.devAddress;
     Serial.print("\r\nAddr:");
     Serial.print(adr.devAddress, HEX);
     Serial.print("(");
@@ -50,9 +54,9 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSC did not start.");
-      
+
   delay( 200 );
-  
+
   next_time = millis() + 10000;
 }
 
@@ -62,19 +66,19 @@ void PrintDescriptors(uint8_t addr)
 {
     uint8_t rcode = 0;
     byte num_conf = 0;
-    
+
     rcode = getdevdescr( (byte)addr, num_conf );
-    if( rcode ) 
+    if( rcode )
     {
       printProgStr(Gen_Error_str);
       print_hex( rcode, 8 );
     }
     Serial.print("\r\n");
-    
+
     for (int i=0; i<num_conf; i++)
     {
         rcode = getconfdescr( addr, i );                 // get configuration descriptor
-        if( rcode ) 
+        if( rcode )
         {
           printProgStr(Gen_Error_str);
           print_hex(rcode, 8);
@@ -86,27 +90,27 @@ void PrintDescriptors(uint8_t addr)
 void PrintAllDescriptors(UsbDevice *pdev)
 {
     Serial.println("\r\n");
-    print_hex(pdev->address, 8);
+    print_hex(pdev->address.devAddress, 8);
     Serial.println("\r\n--");
-    PrintDescriptors( pdev->address );
+    PrintDescriptors( pdev->address.devAddress );
 }
 
 void loop()
 {
   Usb.Task();
-  
+
   if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
-  {  
+  {
     //if (millis() >= next_time)
     {
-        Usb.ForEachUsbDevice(&PrintAllDescriptors);      
+        Usb.ForEachUsbDevice(&PrintAllDescriptors);
         Usb.ForEachUsbDevice(&PrintAllAddresses);
-        
+
         while( 1 );                           //stop
     }
-  }    
+  }
 }
- 
+
 byte getdevdescr( byte addr, byte &num_conf )
 {
   USB_DEVICE_DESCRIPTOR buf;
@@ -122,7 +126,7 @@ byte getdevdescr( byte addr, byte &num_conf )
   print_hex( buf.bDescriptorType, 8 );
   printProgStr(Dev_Version_str);
   print_hex( buf.bcdUSB, 16 );
-  printProgStr(Dev_Class_str); 
+  printProgStr(Dev_Class_str);
   print_hex( buf.bDeviceClass, 8 );
   printProgStr(Dev_Subclass_str);
   print_hex( buf.bDeviceSubClass, 8 );
@@ -147,46 +151,46 @@ byte getdevdescr( byte addr, byte &num_conf )
   num_conf = buf.bNumConfigurations;
   return( 0 );
 }
- 
+
 void printhubdescr(uint8_t *descrptr, uint8_t addr)
 {
     HubDescriptor  *pHub = (HubDescriptor*) descrptr;
     uint8_t        len = *((uint8_t*)descrptr);
-    
+
     printProgStr(PSTR("\r\n\r\nHub Descriptor:\r\n"));
     printProgStr(PSTR("bDescLength:\t\t"));
     Serial.println(pHub->bDescLength, HEX);
-    
+
     printProgStr(PSTR("bDescriptorType:\t"));
     Serial.println(pHub->bDescriptorType, HEX);
-    
+
     printProgStr(PSTR("bNbrPorts:\t\t"));
     Serial.println(pHub->bNbrPorts, HEX);
-    
+
     printProgStr(PSTR("LogPwrSwitchMode:\t"));
     Serial.println(pHub->LogPwrSwitchMode, BIN);
-    
+
     printProgStr(PSTR("CompoundDevice:\t\t"));
     Serial.println(pHub->CompoundDevice, BIN);
-    
+
     printProgStr(PSTR("OverCurrentProtectMode:\t"));
     Serial.println(pHub->OverCurrentProtectMode, BIN);
-    
+
     printProgStr(PSTR("TTThinkTime:\t\t"));
     Serial.println(pHub->TTThinkTime, BIN);
-    
+
     printProgStr(PSTR("PortIndicatorsSupported:"));
     Serial.println(pHub->PortIndicatorsSupported, BIN);
-    
+
     printProgStr(PSTR("Reserved:\t\t"));
     Serial.println(pHub->Reserved, HEX);
 
     printProgStr(PSTR("bPwrOn2PwrGood:\t\t"));
     Serial.println(pHub->bPwrOn2PwrGood, HEX);
-    
+
     printProgStr(PSTR("bHubContrCurrent:\t"));
     Serial.println(pHub->bHubContrCurrent, HEX);
-    
+
     for (uint8_t i=7; i<len; i++)
         print_hex(descrptr[i], 8);
 
@@ -229,7 +233,7 @@ byte getconfdescr( byte addr, byte conf )
       default:
         printunkdescr( buf_ptr );
         break;
-        }//switch( descr_type  
+        }//switch( descr_type
     buf_ptr = ( buf_ptr + descr_length );    //advance buffer pointer
   }//while( buf_ptr <=...
   return( 0 );
@@ -240,12 +244,12 @@ byte getconfdescr( byte addr, byte conf )
 void print_hex(int v, int num_places)
 {
   int mask=0, n, num_nibbles, digit;
- 
+
   for (n=1; n<=num_places; n++) {
     mask = (mask << 1) | 0x0001;
   }
   v = v & mask; // truncate v to specified number of places
- 
+
   num_nibbles = num_places / 4;
   if ((num_places % 4) != 0) {
     ++num_nibbles;
@@ -253,7 +257,7 @@ void print_hex(int v, int num_places)
   do {
     digit = ((v >> (num_nibbles-1) * 4)) & 0x0f;
     Serial.print(digit, HEX);
-  } 
+  }
   while(--num_nibbles);
 }
 /* function to print configuration descriptor */
@@ -309,7 +313,7 @@ void printepdescr( uint8_t* descr_ptr )
   print_hex( ep_ptr->wMaxPacketSize, 16 );
   printProgStr(End_Interval_str);
   print_hex( ep_ptr->bInterval, 8 );
-  
+
   return;
 }
 /*function to print unknown descriptor */
@@ -329,8 +333,8 @@ void printunkdescr( uint8_t* descr_ptr )
     descr_ptr++;
   }
 }
- 
- 
+
+
 /* Print a string from Program Memory directly to save RAM */
 void printProgStr(const prog_char str[])
 {
diff --git a/examples/Xbox/XBOXOLD/XBOXOLD.ino b/examples/Xbox/XBOXOLD/XBOXOLD.ino
index 1bdfc9f9..968c8df7 100644
--- a/examples/Xbox/XBOXOLD/XBOXOLD.ino
+++ b/examples/Xbox/XBOXOLD/XBOXOLD.ino
@@ -6,6 +6,10 @@
 
 #include <XBOXOLD.h>
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 USBHub  Hub1(&Usb); // The controller has a built in hub, so this instance is needed
diff --git a/examples/Xbox/XBOXRECV/XBOXRECV.ino b/examples/Xbox/XBOXRECV/XBOXRECV.ino
index 5896b9dc..1f43b912 100644
--- a/examples/Xbox/XBOXRECV/XBOXRECV.ino
+++ b/examples/Xbox/XBOXRECV/XBOXRECV.ino
@@ -6,6 +6,10 @@
  */
 
 #include <XBOXRECV.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 XBOXRECV Xbox(&Usb);
diff --git a/examples/Xbox/XBOXUSB/XBOXUSB.ino b/examples/Xbox/XBOXUSB/XBOXUSB.ino
index b7d5a625..89161d24 100644
--- a/examples/Xbox/XBOXUSB/XBOXUSB.ino
+++ b/examples/Xbox/XBOXUSB/XBOXUSB.ino
@@ -5,6 +5,10 @@
  */
 
 #include <XBOXUSB.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB Usb;
 XBOXUSB Xbox(&Usb);
diff --git a/examples/acm/acm_terminal/acm_terminal.ino b/examples/acm/acm_terminal/acm_terminal.ino
index 8fc8edea..5e4d152c 100644
--- a/examples/acm/acm_terminal/acm_terminal.ino
+++ b/examples/acm/acm_terminal/acm_terminal.ino
@@ -1,7 +1,12 @@
 #include <cdcacm.h>
 #include <usbhub.h>
 
-#include "pgmstrings.h"  
+#include "pgmstrings.h"
+
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class ACMAsyncOper : public CDCAsyncOper
 {
@@ -22,16 +27,16 @@ uint8_t ACMAsyncOper::OnInit(ACM *pacm)
     }
 
     LINE_CODING	lc;
-    lc.dwDTERate	= 115200;	
+    lc.dwDTERate	= 115200;
     lc.bCharFormat	= 0;
     lc.bParityType	= 0;
-    lc.bDataBits	= 8;	
-	
+    lc.bDataBits	= 8;
+
     rcode = pacm->SetLineCoding(&lc);
 
     if (rcode)
         ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);
-            
+
     return rcode;
 }
 
@@ -48,17 +53,17 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSCOKIRQ failed to assert");
-      
-  delay( 200 ); 
+
+  delay( 200 );
 }
 
 void loop()
 {
     Usb.Task();
-  
-    if( Acm.isReady()) {  
+
+    if( Acm.isReady()) {
        uint8_t rcode;
-       
+
        /* reading the keyboard */
        if(Serial.available()) {
          uint8_t data= Serial.read();
@@ -69,24 +74,24 @@ void loop()
        }//if(Serial.available()...
 
        delay(50);
-       
+
         /* reading the phone */
         /* buffer size must be greater or equal to max.packet size */
         /* it it set to 64 (largest possible max.packet size) here, can be tuned down
         for particular endpoint */
-        uint8_t  buf[64];           
+        uint8_t  buf[64];
         uint16_t rcvd = 64;
         rcode = Acm.RcvData(&rcvd, buf);
          if (rcode && rcode != hrNAK)
             ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
-            
+
             if( rcvd ) { //more than zero bytes received
               for(uint16_t i=0; i < rcvd; i++ ) {
                 Serial.print((char)buf[i]); //printing on the screen
-              }              
+              }
             }
-        delay(10);            
-    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..    
+        delay(10);
+    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..
 }
 
 
diff --git a/examples/adk/ArduinoBlinkLED/ArduinoBlinkLED.ino b/examples/adk/ArduinoBlinkLED/ArduinoBlinkLED.ino
index bc0cb95a..dc6ab92c 100644
--- a/examples/adk/ArduinoBlinkLED/ArduinoBlinkLED.ino
+++ b/examples/adk/ArduinoBlinkLED/ArduinoBlinkLED.ino
@@ -13,6 +13,7 @@ ADK adk(&Usb, "TKJElectronics", // Manufacturer Name
 #define LED LED_BUILTIN // Use built in LED  - note that pin 13 is occupied by the SCK pin on a normal Arduino (Uno, Duemilanove etc.), so use a different pin
 
 uint32_t timer;
+boolean connected;
 
 void setup() {
   Serial.begin(115200);
@@ -27,7 +28,13 @@ void setup() {
 
 void loop() {
   Usb.Task();
+
   if (adk.isReady()) {
+    if (!connected) {
+      connected = true;
+      Serial.print(F("\r\nConnected to accessory"));
+    }
+
     uint8_t msg[1];
     uint16_t len = sizeof(msg);
     uint8_t rcode = adk.RcvData(&len, msg);
@@ -51,7 +58,11 @@ void loop() {
         Serial.print(timer);
       }
     }
+  } else {
+    if (connected) {
+      connected = false;
+      Serial.print(F("\r\nDisconnected from accessory"));
+      digitalWrite(LED, LOW);
+    }
   }
-  else
-    digitalWrite(LED, LOW);
 }
diff --git a/examples/board_qc/board_qc.ino b/examples/board_qc/board_qc.ino
index a2ca05c6..94df16cf 100644
--- a/examples/board_qc/board_qc.ino
+++ b/examples/board_qc/board_qc.ino
@@ -4,6 +4,10 @@
 /* otherwise press any key after getting GPIO error to complete the test */
 /**/
 #include <usbhub.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 /* variables */
 uint8_t rcode;
diff --git a/examples/ftdi/USBFTDILoopback/USBFTDILoopback.ino b/examples/ftdi/USBFTDILoopback/USBFTDILoopback.ino
index ad3d319e..26b5624d 100644
--- a/examples/ftdi/USBFTDILoopback/USBFTDILoopback.ino
+++ b/examples/ftdi/USBFTDILoopback/USBFTDILoopback.ino
@@ -1,7 +1,11 @@
 #include <cdcftdi.h>
 #include <usbhub.h>
 
-#include "pgmstrings.h"  
+#include "pgmstrings.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class FTDIAsync : public FTDIAsyncOper
 {
@@ -12,7 +16,7 @@ public:
 uint8_t FTDIAsync::OnInit(FTDI *pftdi)
 {
     uint8_t rcode = 0;
-    
+
     rcode = pftdi->SetBaudRate(115200);
 
     if (rcode)
@@ -21,10 +25,10 @@ uint8_t FTDIAsync::OnInit(FTDI *pftdi)
         return rcode;
     }
     rcode = pftdi->SetFlowControl(FTDI_SIO_DISABLE_FLOW_CTRL);
-    
+
     if (rcode)
         ErrorMessage<uint8_t>(PSTR("SetFlowControl"), rcode);
-            
+
     return rcode;
 }
 
@@ -43,48 +47,48 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSC did not start.");
-      
+
   delay( 200 );
-  
+
   next_time = millis() + 5000;
 }
 
 void loop()
 {
     Usb.Task();
-  
+
     if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
-    {  
+    {
         uint8_t  rcode;
         char strbuf[] = "DEADBEEF";
         //char strbuf[] = "The quick brown fox jumps over the lazy dog";
         //char strbuf[] = "This string contains 61 character to demonstrate FTDI buffers"; //add one symbol to it to see some garbage
         Serial.print(".");
-        
+
         rcode = Ftdi.SndData(strlen(strbuf), (uint8_t*)strbuf);
 
 	if (rcode)
             ErrorMessage<uint8_t>(PSTR("SndData"), rcode);
 
         delay(50);
-        
+
         uint8_t  buf[64];
-        
+
         for (uint8_t i=0; i<64; i++)
             buf[i] = 0;
-            
+
         uint16_t rcvd = 64;
         rcode = Ftdi.RcvData(&rcvd, buf);
-        
+
         if (rcode && rcode != hrNAK)
             ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
-            
+
         // The device reserves the first two bytes of data
         //   to contain the current values of the modem and line status registers.
-        if (rcvd > 2)        
-            Serial.print((char*)(buf+2)); 
-        
-        delay(10);            
-    }    
+        if (rcvd > 2)
+            Serial.print((char*)(buf+2));
+
+        delay(10);
+    }
 }
 
diff --git a/examples/hub_demo/hub_demo.ino b/examples/hub_demo/hub_demo.ino
index 77c502fe..329c6230 100644
--- a/examples/hub_demo/hub_demo.ino
+++ b/examples/hub_demo/hub_demo.ino
@@ -1,5 +1,9 @@
 #include <usbhub.h>
 #include "pgmstrings.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 USB     Usb;
 USBHub  Hub1(&Usb);
@@ -12,7 +16,7 @@ uint32_t next_time;
 void PrintAllAddresses(UsbDevice *pdev)
 {
     UsbDeviceAddress adr;
-    adr.devAddress = pdev->address;
+    adr.devAddress = pdev->address.devAddress;
     Serial.print("\r\nAddr:");
     Serial.print(adr.devAddress, HEX);
     Serial.print("(");
@@ -46,9 +50,9 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSC did not start.");
-      
+
   delay( 200 );
-  
+
   next_time = millis() + 10000;
 }
 
@@ -58,19 +62,19 @@ void PrintDescriptors(uint8_t addr)
 {
     uint8_t rcode = 0;
     byte num_conf = 0;
-    
+
     rcode = getdevdescr( (byte)addr, num_conf );
-    if( rcode ) 
+    if( rcode )
     {
       printProgStr(Gen_Error_str);
       print_hex( rcode, 8 );
     }
     Serial.print("\r\n");
-    
+
     for (int i=0; i<num_conf; i++)
     {
         rcode = getconfdescr( addr, i );                 // get configuration descriptor
-        if( rcode ) 
+        if( rcode )
         {
           printProgStr(Gen_Error_str);
           print_hex(rcode, 8);
@@ -82,27 +86,27 @@ void PrintDescriptors(uint8_t addr)
 void PrintAllDescriptors(UsbDevice *pdev)
 {
     Serial.println("\r\n");
-    print_hex(pdev->address, 8);
+    print_hex(pdev->address.devAddress, 8);
     Serial.println("\r\n--");
-    PrintDescriptors( pdev->address );
+    PrintDescriptors( pdev->address.devAddress );
 }
 
 void loop()
 {
   Usb.Task();
-  
+
   if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
-  {  
-    if (millis() >= next_time)
+  {
+    if ((millis() - next_time) >= 0L)
     {
-        Usb.ForEachUsbDevice(&PrintAllDescriptors);      
+        Usb.ForEachUsbDevice(&PrintAllDescriptors);
         Usb.ForEachUsbDevice(&PrintAllAddresses);
-        
+
         while( 1 );                           //stop
     }
-  }    
+  }
 }
- 
+
 byte getdevdescr( byte addr, byte &num_conf )
 {
   USB_DEVICE_DESCRIPTOR buf;
@@ -118,7 +122,7 @@ byte getdevdescr( byte addr, byte &num_conf )
   print_hex( buf.bDescriptorType, 8 );
   printProgStr(Dev_Version_str);
   print_hex( buf.bcdUSB, 16 );
-  printProgStr(Dev_Class_str); 
+  printProgStr(Dev_Class_str);
   print_hex( buf.bDeviceClass, 8 );
   printProgStr(Dev_Subclass_str);
   print_hex( buf.bDeviceSubClass, 8 );
@@ -143,46 +147,46 @@ byte getdevdescr( byte addr, byte &num_conf )
   num_conf = buf.bNumConfigurations;
   return( 0 );
 }
- 
+
 void printhubdescr(uint8_t *descrptr, uint8_t addr)
 {
     HubDescriptor  *pHub = (HubDescriptor*) descrptr;
     uint8_t        len = *((uint8_t*)descrptr);
-    
+
     printProgStr(PSTR("\r\n\r\nHub Descriptor:\r\n"));
     printProgStr(PSTR("bDescLength:\t\t"));
     Serial.println(pHub->bDescLength, HEX);
-    
+
     printProgStr(PSTR("bDescriptorType:\t"));
     Serial.println(pHub->bDescriptorType, HEX);
-    
+
     printProgStr(PSTR("bNbrPorts:\t\t"));
     Serial.println(pHub->bNbrPorts, HEX);
-    
+
     printProgStr(PSTR("LogPwrSwitchMode:\t"));
     Serial.println(pHub->LogPwrSwitchMode, BIN);
-    
+
     printProgStr(PSTR("CompoundDevice:\t\t"));
     Serial.println(pHub->CompoundDevice, BIN);
-    
+
     printProgStr(PSTR("OverCurrentProtectMode:\t"));
     Serial.println(pHub->OverCurrentProtectMode, BIN);
-    
+
     printProgStr(PSTR("TTThinkTime:\t\t"));
     Serial.println(pHub->TTThinkTime, BIN);
-    
+
     printProgStr(PSTR("PortIndicatorsSupported:"));
     Serial.println(pHub->PortIndicatorsSupported, BIN);
-    
+
     printProgStr(PSTR("Reserved:\t\t"));
     Serial.println(pHub->Reserved, HEX);
 
     printProgStr(PSTR("bPwrOn2PwrGood:\t\t"));
     Serial.println(pHub->bPwrOn2PwrGood, HEX);
-    
+
     printProgStr(PSTR("bHubContrCurrent:\t"));
     Serial.println(pHub->bHubContrCurrent, HEX);
-    
+
     for (uint8_t i=7; i<len; i++)
         print_hex(descrptr[i], 8);
 
@@ -225,7 +229,7 @@ byte getconfdescr( byte addr, byte conf )
       default:
         printunkdescr( buf_ptr );
         break;
-        }//switch( descr_type  
+        }//switch( descr_type
     buf_ptr = ( buf_ptr + descr_length );    //advance buffer pointer
   }//while( buf_ptr <=...
   return( 0 );
@@ -236,12 +240,12 @@ byte getconfdescr( byte addr, byte conf )
 void print_hex(int v, int num_places)
 {
   int mask=0, n, num_nibbles, digit;
- 
+
   for (n=1; n<=num_places; n++) {
     mask = (mask << 1) | 0x0001;
   }
   v = v & mask; // truncate v to specified number of places
- 
+
   num_nibbles = num_places / 4;
   if ((num_places % 4) != 0) {
     ++num_nibbles;
@@ -249,7 +253,7 @@ void print_hex(int v, int num_places)
   do {
     digit = ((v >> (num_nibbles-1) * 4)) & 0x0f;
     Serial.print(digit, HEX);
-  } 
+  }
   while(--num_nibbles);
 }
 /* function to print configuration descriptor */
@@ -305,7 +309,7 @@ void printepdescr( uint8_t* descr_ptr )
   print_hex( ep_ptr->wMaxPacketSize, 16 );
   printProgStr(End_Interval_str);
   print_hex( ep_ptr->bInterval, 8 );
-  
+
   return;
 }
 /*function to print unknown descriptor */
@@ -325,8 +329,8 @@ void printunkdescr( uint8_t* descr_ptr )
     descr_ptr++;
   }
 }
- 
- 
+
+
 /* Print a string from Program Memory directly to save RAM */
 void printProgStr(const prog_char str[])
 {
diff --git a/examples/max_LCD/max_LCD.ino b/examples/max_LCD/max_LCD.ino
new file mode 100644
index 00000000..f60ed4d5
--- /dev/null
+++ b/examples/max_LCD/max_LCD.ino
@@ -0,0 +1,23 @@
+// Just a copy of the HelloWorld example bundled with the LiquidCrystal library in the Arduino IDE
+
+// HD44780 compatible LCD display via MAX3421E GPOUT support header
+// pinout: D[4-7] -> GPOUT[4-7], RS-> GPOUT[2], E ->GPOUT[3]
+
+#include <max_LCD.h>
+
+USB Usb;
+Max_LCD lcd(&Usb);
+
+void setup() {
+  // Set up the LCD's number of columns and rows:
+  lcd.begin(16, 2);
+  // Print a message to the LCD.
+  lcd.print("Hello, World!");
+}
+
+void loop() {
+  // Set the cursor to column 0, line 1 (note: line 1 is the second row, since counting begins with 0):
+  lcd.setCursor(0, 1);
+  // Print the number of seconds since reset:
+  lcd.print(millis() / 1000);
+}
diff --git a/examples/pl2303/pl2303_gprs_terminal/pl2303_gprs_terminal.ino b/examples/pl2303/pl2303_gprs_terminal/pl2303_gprs_terminal.ino
index fea00e52..4770809d 100644
--- a/examples/pl2303/pl2303_gprs_terminal/pl2303_gprs_terminal.ino
+++ b/examples/pl2303/pl2303_gprs_terminal/pl2303_gprs_terminal.ino
@@ -4,6 +4,10 @@
 /* CDC support */
 #include <cdcacm.h>
 #include <cdcprolific.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class PLAsyncOper : public CDCAsyncOper
 {
@@ -14,7 +18,7 @@ public:
 uint8_t PLAsyncOper::OnInit(ACM *pacm)
 {
     uint8_t rcode;
-    
+
     // Set DTR = 1
     rcode = pacm->SetControlLineState(1);
 
@@ -29,13 +33,13 @@ uint8_t PLAsyncOper::OnInit(ACM *pacm)
     lc.dwDTERate = 115200;
     lc.bCharFormat	= 0;
     lc.bParityType	= 0;
-    lc.bDataBits	= 8;	
-	
+    lc.bDataBits	= 8;
+
     rcode = pacm->SetLineCoding(&lc);
 
     if (rcode)
         ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);
-            
+
     return rcode;
 }
 USB     Usb;
@@ -51,43 +55,43 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSCOKIRQ failed to assert");
-      
-  delay( 200 ); 
+
+  delay( 200 );
 }
 
 void loop()
 {
     Usb.Task();
-  
+
     if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
-    {  
+    {
        uint8_t rcode;
-       
+
        /* reading the keyboard */
        if(Serial.available()) {
          uint8_t data= Serial.read();
-       
+
          /* sending to the phone */
          rcode = Pl.SndData(1, &data);
          if (rcode)
             ErrorMessage<uint8_t>(PSTR("SndData"), rcode);
        }//if(Serial.available()...
-       
+
         /* reading the converter */
         /* buffer size must be greater or equal to max.packet size */
         /* it it set to 64 (largest possible max.packet size) here, can be tuned down
         for particular endpoint */
-        uint8_t  buf[64];           
+        uint8_t  buf[64];
         uint16_t rcvd = 64;
         rcode = Pl.RcvData(&rcvd, buf);
          if (rcode && rcode != hrNAK)
            ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
-            
+
            if( rcvd ) { //more than zero bytes received
              for(uint16_t i=0; i < rcvd; i++ ) {
-               Serial.print((char)buf[i]); //printing on the screen               
-             }              
+               Serial.print((char)buf[i]); //printing on the screen
+             }
            }//if( rcvd ...
-    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..    
+    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..
 }
 
diff --git a/examples/pl2303/pl2303_gps/pl2303_gps.ino b/examples/pl2303/pl2303_gps/pl2303_gps.ino
index f756f408..908763c4 100644
--- a/examples/pl2303/pl2303_gps/pl2303_gps.ino
+++ b/examples/pl2303/pl2303_gps/pl2303_gps.ino
@@ -5,81 +5,80 @@
 /* CDC support */
 #include <cdcacm.h>
 #include <cdcprolific.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
-class PLAsyncOper : public CDCAsyncOper
-{
+class PLAsyncOper : public CDCAsyncOper {
 public:
-    virtual uint8_t OnInit(ACM *pacm);
+        virtual uint8_t OnInit(ACM *pacm);
 };
 
-uint8_t PLAsyncOper::OnInit(ACM *pacm)
-{
-    uint8_t rcode;
-    
-    // Set DTR = 1
-    rcode = pacm->SetControlLineState(1);
+uint8_t PLAsyncOper::OnInit(ACM *pacm) {
+        uint8_t rcode;
+
+        // Set DTR = 1
+        rcode = pacm->SetControlLineState(1);
+
+        if(rcode) {
+                ErrorMessage<uint8_t>(PSTR("SetControlLineState"), rcode);
+                return rcode;
+        }
+
+        LINE_CODING lc;
+        lc.dwDTERate = 4800; //default serial speed of GPS unit
+        lc.bCharFormat = 0;
+        lc.bParityType = 0;
+        lc.bDataBits = 8;
+
+        rcode = pacm->SetLineCoding(&lc);
+
+        if(rcode)
+                ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);
 
-    if (rcode)
-    {
-        ErrorMessage<uint8_t>(PSTR("SetControlLineState"), rcode);
         return rcode;
-    }
-
-    LINE_CODING lc;
-    lc.dwDTERate  = 4800;   //default serial speed of GPS unit  
-    lc.bCharFormat  = 0;
-    lc.bParityType  = 0;
-    lc.bDataBits  = 8;  
-  
-    rcode = pacm->SetLineCoding(&lc);
-
-    if (rcode)
-        ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);
-            
-    return rcode;
 }
 
-USB     Usb;
-USBHub     Hub(&Usb);
-PLAsyncOper  AsyncOper;
-PL2303       Pl(&Usb, &AsyncOper);
+USB Usb;
+USBHub Hub(&Usb);
+PLAsyncOper AsyncOper;
+PL2303 Pl(&Usb, &AsyncOper);
 uint32_t read_delay;
 #define READ_DELAY 100
 
-void setup()
-{
-  Serial.begin( 115200 );
-  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
-  Serial.println("Start");
+void setup() {
+        Serial.begin(115200);
+        while(!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+        Serial.println("Start");
 
-  if (Usb.Init() == -1)
-      Serial.println("OSCOKIRQ failed to assert");
-      
-  delay( 200 ); 
+        if(Usb.Init() == -1)
+                Serial.println("OSCOKIRQ failed to assert");
+
+        delay(200);
 }
 
-void loop()
-{
-uint8_t rcode;
-uint8_t  buf[64];    //serial buffer equals Max.packet size of bulk-IN endpoint           
-uint16_t rcvd = 64;   
+void loop() {
+        uint8_t rcode;
+        uint8_t buf[64]; //serial buffer equals Max.packet size of bulk-IN endpoint
+        uint16_t rcvd = 64;
 
-  Usb.Task();
-    
-    if( Pl.isReady()) {  
-       /* reading the GPS */
-       if( read_delay < millis() ){
-       read_delay += READ_DELAY;  
-       rcode = Pl.RcvData(&rcvd, buf);
-        if ( rcode && rcode != hrNAK )
-           ErrorMessage<uint8_t>(PSTR("Ret"), rcode);            
-            if( rcvd ) { //more than zero bytes received
-              for( uint16_t i=0; i < rcvd; i++ ) {
-                  Serial.print((char)buf[i]); //printing on the screen
-              }//for( uint16_t i=0; i < rcvd; i++...              
-            }//if( rcvd
-       }//if( read_delay > millis()...            
-    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..    
+        Usb.Task();
+
+        if(Pl.isReady()) {
+                /* reading the GPS */
+                if((long)(millis() - read_delay) >= 0L) {
+                        read_delay += READ_DELAY;
+                        rcode = Pl.RcvData(&rcvd, buf);
+                        if(rcode && rcode != hrNAK)
+                                ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
+                        if(rcvd) { //more than zero bytes received
+                                for(uint16_t i = 0; i < rcvd; i++) {
+                                        Serial.print((char)buf[i]); //printing on the screen
+                                }//for( uint16_t i=0; i < rcvd; i++...
+                        }//if( rcvd
+                }//if( read_delay > millis()...
+        }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..
 }
 
 
diff --git a/examples/pl2303/pl2303_tinygps/pl2303_tinygps.ino b/examples/pl2303/pl2303_tinygps/pl2303_tinygps.ino
index 818df512..db25e42a 100644
--- a/examples/pl2303/pl2303_tinygps/pl2303_tinygps.ino
+++ b/examples/pl2303/pl2303_tinygps/pl2303_tinygps.ino
@@ -11,6 +11,10 @@
 #include <cdcprolific.h>
 
 #include <TinyGPS.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 /* This sample code demonstrates the normal use of a TinyGPS object.
     Modified to be used with USB Host Shield Library r2.0
@@ -26,7 +30,7 @@ public:
 uint8_t PLAsyncOper::OnInit(ACM *pacm)
 {
     uint8_t rcode;
-    
+
     // Set DTR = 1
     rcode = pacm->SetControlLineState(1);
 
@@ -36,17 +40,17 @@ uint8_t PLAsyncOper::OnInit(ACM *pacm)
     }
 
     LINE_CODING lc;
-    lc.dwDTERate  = 4800;   //default serial speed of GPS unit  
+    lc.dwDTERate  = 4800;   //default serial speed of GPS unit
     lc.bCharFormat  = 0;
     lc.bParityType  = 0;
-    lc.bDataBits  = 8;  
-  
+    lc.bDataBits  = 8;
+
     rcode = pacm->SetLineCoding(&lc);
 
     if (rcode) {
         ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);
     }
-            
+
     return rcode;
 }
 
@@ -65,7 +69,7 @@ void setup()
 
   Serial.begin(115200);
   while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
-    
+
   Serial.print("Testing TinyGPS library v. "); Serial.println(TinyGPS::library_version());
   Serial.println("by Mikal Hart");
   Serial.println();
@@ -75,16 +79,16 @@ void setup()
   if (Usb.Init() == -1) {
       Serial.println("OSCOKIRQ failed to assert");
   }
-      
-  delay( 200 ); 
+
+  delay( 200 );
 }
 
 void loop()
 {
   Usb.Task();
-  
+
   if( Pl.isReady()) {
-  
+
     bool newdata = false;
     unsigned long start = millis();
 
@@ -94,7 +98,7 @@ void loop()
         newdata = true;
       }
     }//while (millis()...
-  
+
     if (newdata) {
       Serial.println("Acquired Data");
       Serial.println("-------------");
@@ -118,7 +122,7 @@ void printFloat(double number, int digits)
   double rounding = 0.5;
   for (uint8_t i=0; i<digits; ++i)
     rounding /= 10.0;
-  
+
   number += rounding;
 
   // Extract the integer part of the number and print it
@@ -128,7 +132,7 @@ void printFloat(double number, int digits)
 
   // Print the decimal point, but only if there are digits beyond
   if (digits > 0)
-    Serial.print("."); 
+    Serial.print(".");
 
   // Extract digits from the remainder one at a time
   while (digits-- > 0)
@@ -136,8 +140,8 @@ void printFloat(double number, int digits)
     remainder *= 10.0;
     int toPrint = int(remainder);
     Serial.print(toPrint);
-    remainder -= toPrint; 
-  } 
+    remainder -= toPrint;
+  }
 }
 
 void gpsdump(TinyGPS &gps)
@@ -150,9 +154,9 @@ void gpsdump(TinyGPS &gps)
   unsigned short sentences, failed;
 
   gps.get_position(&lat, &lon, &age);
-  Serial.print("Lat/Long(10^-5 deg): "); Serial.print(lat); Serial.print(", "); Serial.print(lon); 
+  Serial.print("Lat/Long(10^-5 deg): "); Serial.print(lat); Serial.print(", "); Serial.print(lon);
   Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");
-  
+
   feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
 
   gps.f_get_position(&flat, &flon, &age);
@@ -171,7 +175,7 @@ void gpsdump(TinyGPS &gps)
   Serial.print("Date: "); Serial.print(static_cast<int>(month)); Serial.print("/"); Serial.print(static_cast<int>(day)); Serial.print("/"); Serial.print(year);
   Serial.print("  Time: "); Serial.print(static_cast<int>(hour)); Serial.print(":"); Serial.print(static_cast<int>(minute)); Serial.print(":"); Serial.print(static_cast<int>(second)); Serial.print("."); Serial.print(static_cast<int>(hundredths));
   Serial.print("  Fix age: ");  Serial.print(age); Serial.println("ms.");
-  
+
   feedgps();
 
   Serial.print("Alt(cm): "); Serial.print(gps.altitude()); Serial.print(" Course(10^-2 deg): "); Serial.print(gps.course()); Serial.print(" Speed(10^-2 knots): "); Serial.println(gps.speed());
@@ -184,13 +188,13 @@ void gpsdump(TinyGPS &gps)
   gps.stats(&chars, &sentences, &failed);
   Serial.print("Stats: characters: "); Serial.print(chars); Serial.print(" sentences: "); Serial.print(sentences); Serial.print(" failed checksum: "); Serial.println(failed);
 }
-  
+
 bool feedgps()
 {
   uint8_t rcode;
-  uint8_t  buf[64];    //serial buffer equals Max.packet size of bulk-IN endpoint           
-  uint16_t rcvd = 64;       
-    {  
+  uint8_t  buf[64];    //serial buffer equals Max.packet size of bulk-IN endpoint
+  uint16_t rcvd = 64;
+    {
         /* reading the GPS */
         rcode = Pl.RcvData(&rcvd, buf);
          if (rcode && rcode != hrNAK)
@@ -201,7 +205,7 @@ bool feedgps()
                 if( gps.encode((char)buf[i])) { //feed a character to gps object
                   rcode = true;
                 }//if( gps.encode(buf[i]...
-              }//for( uint16_t i=0; i < rcvd; i++...              
+              }//for( uint16_t i=0; i < rcvd; i++...
             }//if( rcvd...
     }
   return( rcode );
diff --git a/examples/pl2303/pl2303_xbee_terminal/pl2303_xbee_terminal.ino b/examples/pl2303/pl2303_xbee_terminal/pl2303_xbee_terminal.ino
index 60a3e15d..9e17d3cf 100644
--- a/examples/pl2303/pl2303_xbee_terminal/pl2303_xbee_terminal.ino
+++ b/examples/pl2303/pl2303_xbee_terminal/pl2303_xbee_terminal.ino
@@ -5,6 +5,10 @@
 /* CDC support */
 #include <cdcacm.h>
 #include <cdcprolific.h>
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
 
 class PLAsyncOper : public CDCAsyncOper
 {
@@ -15,7 +19,7 @@ public:
 uint8_t PLAsyncOper::OnInit(ACM *pacm)
 {
     uint8_t rcode;
-    
+
     // Set DTR = 1
     rcode = pacm->SetControlLineState(1);
 
@@ -26,16 +30,16 @@ uint8_t PLAsyncOper::OnInit(ACM *pacm)
     }
 
     LINE_CODING	lc;
-    lc.dwDTERate	= 115200;	
+    lc.dwDTERate	= 115200;
     lc.bCharFormat	= 0;
     lc.bParityType	= 0;
-    lc.bDataBits	= 8;	
-	
+    lc.bDataBits	= 8;
+
     rcode = pacm->SetLineCoding(&lc);
 
     if (rcode)
         ErrorMessage<uint8_t>(PSTR("SetLineCoding"), rcode);
-            
+
     return rcode;
 }
 USB     Usb;
@@ -51,29 +55,29 @@ void setup()
 
   if (Usb.Init() == -1)
       Serial.println("OSCOKIRQ failed to assert");
-      
-  delay( 200 ); 
+
+  delay( 200 );
 }
 
 void loop()
 {
     Usb.Task();
-  
+
     if( Usb.getUsbTaskState() == USB_STATE_RUNNING )
-    {  
+    {
        uint8_t rcode;
-       
+
        /* reading the keyboard */
        if(Serial.available()) {
          uint8_t data= Serial.read();
-         
+
          if ( data == '\r' ) {
            Serial.print("\r\n");  //insert linefeed
          }
          else {
            Serial.print( data );  //echo back to the screen
          }
-         
+
          /* sending to the phone */
          rcode = Pl.SndData(1, &data);
          if (rcode)
@@ -81,17 +85,17 @@ void loop()
        }//if(Serial.available()...
 
        delay(50);
-       
+
         /* reading the converter */
         /* buffer size must be greater or equal to max.packet size */
         /* it it set to 64 (largest possible max.packet size) here, can be tuned down
         for particular endpoint */
-        uint8_t  buf[64];           
+        uint8_t  buf[64];
         uint16_t rcvd = 64;
         rcode = Pl.RcvData(&rcvd, buf);
          if (rcode && rcode != hrNAK)
             ErrorMessage<uint8_t>(PSTR("Ret"), rcode);
-            
+
             if( rcvd ) { //more than zero bytes received
               for(uint16_t i=0; i < rcvd; i++ ) {
                 if( buf[i] =='\r' ) {
@@ -100,10 +104,10 @@ void loop()
                 else {
                   Serial.print((char)buf[i]); //printing on the screen
                 }
-              }              
+              }
             }
-        delay(10);            
-    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..    
+        delay(10);
+    }//if( Usb.getUsbTaskState() == USB_STATE_RUNNING..
 }
 
 
diff --git a/examples/testusbhostFAT/Makefile b/examples/testusbhostFAT/Makefile
index 7e5ebaec..232d338e 100644
--- a/examples/testusbhostFAT/Makefile
+++ b/examples/testusbhostFAT/Makefile
@@ -9,6 +9,10 @@
 BOARD = mega
 PROGRAMMER = arduino
 
+#BOARD = teensypp2
+#BOARD = teensy3
+#BOARD = teensy31
+
 # set your Arduino tty port here
 PORT = /dev/ttyUSB0
 
diff --git a/examples/testusbhostFAT/RTClib b/examples/testusbhostFAT/RTClib
index a4bd6f50..b119b97e 160000
--- a/examples/testusbhostFAT/RTClib
+++ b/examples/testusbhostFAT/RTClib
@@ -1 +1 @@
-Subproject commit a4bd6f500f70599847de60973371ee973d094a34
+Subproject commit b119b97e1484a08aebcf24e070113d78c82fb023
diff --git a/examples/testusbhostFAT/avr-gdb.conf b/examples/testusbhostFAT/avr-gdb.conf
deleted file mode 100644
index 7ab686f2..00000000
--- a/examples/testusbhostFAT/avr-gdb.conf
+++ /dev/null
@@ -1,6 +0,0 @@
-file build/testusbhostFAT.elf
-target remote localhost:4242
-set {int}0x802200 = 0xffff
-set {int}0x802220 = 0x0000
-#graph display `x /3xh 0x802200`
-
diff --git a/examples/testusbhostFAT/testusbhostFAT.ino b/examples/testusbhostFAT/testusbhostFAT.ino
old mode 100644
new mode 100755
index 49713d36..70107963
--- a/examples/testusbhostFAT/testusbhostFAT.ino
+++ b/examples/testusbhostFAT/testusbhostFAT.ino
@@ -19,35 +19,39 @@
  *
  */
 
+/////////////////////////////////////////////////////////////
+// Please Note:                                            //
+// This section is for info with the Arduino IDE ONLY.     //
+// Unfortunately due to short sightedness of the Arduino   //
+// code team, that you must set the following in the       //
+// respective libraries.                                   //
+// Changing them here will have _NO_ effect!               //
+/////////////////////////////////////////////////////////////
+
+// Uncomment to enable debugging
+//#define DEBUG_USB_HOST
+// This is where stderr/USB debugging goes to
+//#define USB_HOST_SERIAL Serial3
+
+// If you have external memory, setting this to 0 enables FAT table caches.
+// The 0 setting is recommended only if you have external memory.
+//#define _FS_TINY 1
+
+//#define _USE_LFN 3
+//#define EXT_RAM_STACK 1
+//#define EXT_RAM_HEAP 1
+//#define _MAX_SS 512
+/////////////////////////////////////////////////////////////
+// End of Arduino IDE specific information                 //
+/////////////////////////////////////////////////////////////
+
 // You can set this to 0 if you are not using a USB hub.
 // It will save a little bit of flash and RAM.
 // Set to 1 if you want to use a hub.
 #define WANT_HUB_TEST 0
 
 
-/////////////////////////////////////////////////////////////
-// Please Note: This section is for Arduino IDE ONLY.      //
-// Use of Make creates a flash image that is 3.3KB smaller //
-/////////////////////////////////////////////////////////////
-#ifndef USING_MAKEFILE
-// Uncomment to enable debugging
-//#define DEBUG_USB_HOST
-// This is where stderr/USB debugging goes to
-#define USB_HOST_SERIAL Serial3
-// If you have external memory, setting this to 0 enables FAT table caches.
-// The 0 setting is recommended only if you have external memory.
-#define _FS_TINY 1
-
-// These you can safely leave alone.
-#define _USE_LFN 3
-#define EXT_RAM_STACK 1
-#define EXT_RAM_HEAP 1
-#define _MAX_SS 512
-#endif
-/////////////////////////////////////////////////////////////
-// End of Arduino IDE specific hacks                       //
-/////////////////////////////////////////////////////////////
-#if defined(AVR)
+#if defined(__AVR__)
 #include <xmem.h>
 #else
 #include <spi4teensy3.h>
@@ -63,7 +67,7 @@
 #include <Wire.h>
 #include <RTClib.h>
 #include <stdio.h>
-#if defined(AVR)
+#if defined(__AVR__)
 static FILE tty_stdio;
 static FILE tty_stderr;
 volatile uint32_t LEDnext_time; // fade timeout
@@ -100,7 +104,7 @@ static storage_t sto[_VOLUMES];
 #define mbxs 128
 static uint8_t My_Buff_x[mbxs]; /* File read buffer */
 
-#if defined(AVR)
+#if defined(__AVR__)
 
 #define prescale1       ((1 << WGM12) | (1 << CS10))
 #define prescale8       ((1 << WGM12) | (1 << CS11))
@@ -126,7 +130,7 @@ static int tty_std_putc(char c, FILE *t) {
 }
 
 static int tty_std_getc(FILE *t) {
-        while (!Serial.available());
+        while(!Serial.available());
         return Serial.read();
 }
 
@@ -140,18 +144,18 @@ extern "C" {
 
         int _write(int fd, const char *ptr, int len) {
                 int j;
-                for (j = 0; j < len; j++) {
-                        if (fd == 1)
+                for(j = 0; j < len; j++) {
+                        if(fd == 1)
                                 Serial.write(*ptr++);
-                        else if (fd == 2)
+                        else if(fd == 2)
                                 USB_HOST_SERIAL.write(*ptr++);
                 }
                 return len;
         }
 
         int _read(int fd, char *ptr, int len) {
-                if (len > 0 && fd == 0) {
-                        while (!Serial.available());
+                if(len > 0 && fd == 0) {
+                        while(!Serial.available());
                         *ptr = Serial.read();
                         return 1;
                 }
@@ -175,7 +179,7 @@ extern "C" {
 
 void setup() {
         boolean serr = false;
-        for (int i = 0; i < _VOLUMES; i++) {
+        for(int i = 0; i < _VOLUMES; i++) {
                 Fats[i] = NULL;
                 sto[i].private_data = new pvt_t;
                 ((pvt_t *)sto[i].private_data)->B = 255; // impossible
@@ -184,7 +188,7 @@ void setup() {
         // minimum 0x00, maximum 0xff
         UsbDEBUGlvl = 0x51;
 
-#if defined(AVR)
+#if !defined(CORE_TEENSY) && defined(__AVR__)
         // make LED pin as an output:
         pinMode(LED_BUILTIN, OUTPUT);
         pinMode(2, OUTPUT);
@@ -193,11 +197,23 @@ void setup() {
         // Initialize 'debug' serial port
         USB_HOST_SERIAL.begin(115200);
         // Do not start primary Serial port if already started.
-        if (bit_is_clear(UCSR0B, TXEN0)) {
+        if(bit_is_clear(UCSR0B, TXEN0)) {
                 Serial.begin(115200);
                 serr = true;
         }
 
+
+        // Blink LED
+        delay(500);
+        analogWrite(LED_BUILTIN, 255);
+        delay(500);
+        analogWrite(LED_BUILTIN, 0);
+        delay(500);
+#else
+        while(!Serial);
+        Serial.begin(115200); // On the Teensy 3.x we get a delay at least!
+#endif
+#if defined(__AVR__)
         // Set up stdio/stderr
         tty_stdio.put = tty_std_putc;
         tty_stdio.get = tty_std_getc;
@@ -212,17 +228,7 @@ void setup() {
         stdout = &tty_stdio;
         stdin = &tty_stdio;
         stderr = &tty_stderr;
-
-        // Blink LED
-        delay(500);
-        analogWrite(LED_BUILTIN, 255);
-        delay(500);
-        analogWrite(LED_BUILTIN, 0);
-        delay(500);
-#else
-        while (!Serial);
 #endif
-
         printf_P(PSTR("\r\n\n\n\n\n\n\n\n\n\n\n\n\n\n\nStart\r\n"));
         printf_P(PSTR("Current UsbDEBUGlvl %02x\r\n"), UsbDEBUGlvl);
         printf_P(PSTR("'+' and '-' increase/decrease by 0x01\r\n"));
@@ -236,7 +242,7 @@ void setup() {
                 "Disabled"
 #endif
                 "\r\n"));
-        if (serr) {
+        if(serr) {
                 fprintf_P(stderr, PSTR("\r\n\n\n\n\n\n\n\n\n\n\n\n\n\n\nStart\r\n"));
                 fprintf_P(stderr, PSTR("Current UsbDEBUGlvl %02x\r\n"), UsbDEBUGlvl);
                 fprintf_P(stderr, PSTR("Long filename support: "
@@ -247,8 +253,8 @@ void setup() {
 #endif
                         "\r\n"));
         }
-#if defined(AVR)
 
+#if !defined(CORE_TEENSY) && defined(__AVR__)
         analogWrite(LED_BUILTIN, 255);
         delay(500);
         analogWrite(LED_BUILTIN, 0);
@@ -263,7 +269,7 @@ void setup() {
         delay(500);
 
         LEDnext_time = millis() + 1;
-#ifdef EXT_RAM
+#if EXT_RAM
         printf_P(PSTR("Total EXT RAM banks %i\r\n"), xmem::getTotalBanks());
 #endif
         printf_P(PSTR("Available heap: %u Bytes\r\n"), freeHeap());
@@ -274,22 +280,22 @@ void setup() {
         // I want to be able to have slightly more control.
         // Besides, it is easier to initialize stuff...
 #if WANT_HUB_TEST
-        for (int i = 0; i < MAX_HUBS; i++) {
+        for(int i = 0; i < MAX_HUBS; i++) {
                 Hubs[i] = new USBHub(&Usb);
-#if defined(AVR)
+#if defined(__AVR__)
                 printf_P(PSTR("Available heap: %u Bytes\r\n"), freeHeap());
 #endif
         }
 #endif
         // Initialize generic storage. This must be done before USB starts.
-        InitStorage();
+        Init_Generic_Storage();
 
-        while (Usb.Init(1000) == -1) {
+        while(Usb.Init(1000) == -1) {
                 printf_P(PSTR("No USB HOST Shield?\r\n"));
                 Notify(PSTR("OSC did not start."), 0x40);
         }
 
-#if defined(AVR)
+#if !defined(CORE_TEENSY) && defined(__AVR__)
         cli();
         TCCR3A = 0;
         TCCR3B = 0;
@@ -300,32 +306,10 @@ void setup() {
         sei();
 
         HEAPnext_time = millis() + 10000;
-#else
-#if 0
-        //
-        // On the teensy 3 we can raise the speed of SPI here.
-        //
-        // Default seen is 0xB8011001.
-        //
-
-        uint32_t ctar = SPI0_CTAR0;
-        //printf("SPI_CTAR0 = %8.8X\r\n", ctar);
-        ctar &= 0x7FFCFFF0; // 1/4 fSYS, 12.5Mhz
-        //printf("SPI_CTAR0 = %8.8X\r\n", ctar);
-        ctar |= 0x80000000; // 1/2 fSYS 25Mhz
-        //printf("SPI_CTAR0 = %8.8X\r\n", ctar);
-
-        uint32_t mcr = SPI0_MCR;
-        if (mcr & SPI_MCR_MDIS) {
-                SPI0_CTAR0 = ctar;
-        } else {
-                SPI0_MCR = mcr | SPI_MCR_MDIS | SPI_MCR_HALT;
-                SPI0_CTAR0 = ctar;
-                SPI0_MCR = mcr;
-        }
 #endif
+#if defined(__AVR__)
+        HEAPnext_time = millis() + 10000;
 #endif
-
 }
 
 void serialEvent() {
@@ -334,23 +318,23 @@ void serialEvent() {
         // . to increase by 16, , to decrease by 16
         // e to flick VBUS
         // * to report debug level
-        if (Serial.available()) {
+        if(Serial.available()) {
                 int inByte = Serial.read();
-                switch (inByte) {
+                switch(inByte) {
                         case '+':
-                                if (UsbDEBUGlvl < 0xff) UsbDEBUGlvl++;
+                                if(UsbDEBUGlvl < 0xff) UsbDEBUGlvl++;
                                 reportlvl = true;
                                 break;
                         case '-':
-                                if (UsbDEBUGlvl > 0x00) UsbDEBUGlvl--;
+                                if(UsbDEBUGlvl > 0x00) UsbDEBUGlvl--;
                                 reportlvl = true;
                                 break;
                         case '.':
-                                if (UsbDEBUGlvl < 0xf0) UsbDEBUGlvl += 16;
+                                if(UsbDEBUGlvl < 0xf0) UsbDEBUGlvl += 16;
                                 reportlvl = true;
                                 break;
                         case ',':
-                                if (UsbDEBUGlvl > 0x0f) UsbDEBUGlvl -= 16;
+                                if(UsbDEBUGlvl > 0x0f) UsbDEBUGlvl -= 16;
                                 reportlvl = true;
                                 break;
                         case '*':
@@ -367,10 +351,11 @@ void serialEvent() {
         }
 }
 
-#if defined(AVR)
+#if !defined(CORE_TEENSY) && defined(__AVR__)
+// ALL teensy versions LACK PWM ON LED
 
 ISR(TIMER3_COMPA_vect) {
-        if (millis() >= LEDnext_time) {
+        if((long)(millis() - LEDnext_time) >= 0L) {
                 LEDnext_time = millis() + 30;
 
                 // set the brightness of LED
@@ -380,11 +365,11 @@ ISR(TIMER3_COMPA_vect) {
                 brightness = brightness + fadeAmount;
 
                 // reverse the direction of the fading at the ends of the fade:
-                if (brightness <= 0) {
+                if(brightness <= 0) {
                         brightness = 0;
                         fadeAmount = -fadeAmount;
                 }
-                if (brightness >= 255) {
+                if(brightness >= 255) {
                         brightness = 255;
                         fadeAmount = -fadeAmount;
                 }
@@ -404,29 +389,30 @@ void die(FRESULT rc) {
 void loop() {
         FIL My_File_Object_x; /* File object */
 
-#if defined(AVR)
+#if defined(__AVR__)
         // Print a heap status report about every 10 seconds.
-        if (millis() >= HEAPnext_time) {
-                if (UsbDEBUGlvl > 0x50) {
+        if((long)(millis() - HEAPnext_time) >= 0L) {
+                if(UsbDEBUGlvl > 0x50) {
                         printf_P(PSTR("Available heap: %u Bytes\r\n"), freeHeap());
                 }
                 HEAPnext_time = millis() + 10000;
         }
         TCCR3B = 0;
-#else
-        // Arm suffers here, oh well...
+#endif
+#if defined(CORE_TEENSY)
+        // Teensy suffers here, oh well...
         serialEvent();
 #endif
         // Horrid! This sort of thing really belongs in an ISR, not here!
         // We also will be needing to test each hub port, we don't do this yet!
-        if (!change && !usbon && millis() >= usbon_time) {
+        if(!change && !usbon && (long)(millis() - usbon_time) >= 0L) {
                 change = true;
                 usbon = true;
         }
 
-        if (change) {
+        if(change) {
                 change = false;
-                if (usbon) {
+                if(usbon) {
                         Usb.vbusPower(vbus_on);
                         printf_P(PSTR("VBUS on\r\n"));
                 } else {
@@ -436,21 +422,21 @@ void loop() {
         }
         Usb.Task();
         current_state = Usb.getUsbTaskState();
-        if (current_state != last_state) {
-                if (UsbDEBUGlvl > 0x50)
+        if(current_state != last_state) {
+                if(UsbDEBUGlvl > 0x50)
                         printf_P(PSTR("USB state = %x\r\n"), current_state);
-#if defined(AVR)
-                if (current_state == USB_STATE_RUNNING) {
+#if !defined(CORE_TEENSY) && defined(__AVR__)
+                if(current_state == USB_STATE_RUNNING) {
                         fadeAmount = 30;
                 }
 #endif
-                if (current_state == USB_DETACHED_SUBSTATE_WAIT_FOR_DEVICE) {
-#if defined(AVR)
+                if(current_state == USB_DETACHED_SUBSTATE_WAIT_FOR_DEVICE) {
+#if !defined(CORE_TEENSY) && defined(__AVR__)
                         fadeAmount = 80;
 #endif
                         partsready = false;
-                        for (int i = 0; i < cpart; i++) {
-                                if (Fats[i] != NULL)
+                        for(int i = 0; i < cpart; i++) {
+                                if(Fats[i] != NULL)
                                         delete Fats[i];
                                 Fats[i] = NULL;
                         }
@@ -462,48 +448,48 @@ void loop() {
         }
 
         // only do any of this if usb is on
-        if (usbon) {
-                if (partsready && !fatready) {
-                        if (cpart > 0) fatready = true;
+        if(usbon) {
+                if(partsready && !fatready) {
+                        if(cpart > 0) fatready = true;
                 }
                 // This is horrible, and needs to be moved elsewhere!
-                for (int B = 0; B < MAX_USB_MS_DRIVERS; B++) {
-                        if (!partsready && (Bulk[B]->GetAddress() != NULL)) {
+                for(int B = 0; B < MAX_USB_MS_DRIVERS; B++) {
+                        if(!partsready && (UHS_USB_BulkOnly[B]->GetAddress() != NULL)) {
 
                                 // Build a list.
-                                int ML = Bulk[B]->GetbMaxLUN();
+                                int ML = UHS_USB_BulkOnly[B]->GetbMaxLUN();
                                 //printf("MAXLUN = %i\r\n", ML);
                                 ML++;
-                                for (int i = 0; i < ML; i++) {
-                                        if (Bulk[B]->LUNIsGood(i)) {
+                                for(int i = 0; i < ML; i++) {
+                                        if(UHS_USB_BulkOnly[B]->LUNIsGood(i)) {
                                                 partsready = true;
                                                 ((pvt_t *)(sto[i].private_data))->lun = i;
                                                 ((pvt_t *)(sto[i].private_data))->B = B;
-                                                sto[i].Read = *PRead;
-                                                sto[i].Write = *PWrite;
-                                                sto[i].Reads = *PReads;
-                                                sto[i].Writes = *PWrites;
-                                                sto[i].Status = *PStatus;
-                                                sto[i].TotalSectors = Bulk[B]->GetCapacity(i);
-                                                sto[i].SectorSize = Bulk[B]->GetSectorSize(i);
+                                                sto[i].Reads = *UHS_USB_BulkOnly_Read;
+                                                sto[i].Writes = *UHS_USB_BulkOnly_Write;
+                                                sto[i].Status = *UHS_USB_BulkOnly_Status;
+                                                sto[i].Initialize = *UHS_USB_BulkOnly_Initialize;
+                                                sto[i].Commit = *UHS_USB_BulkOnly_Commit;
+                                                sto[i].TotalSectors = UHS_USB_BulkOnly[B]->GetCapacity(i);
+                                                sto[i].SectorSize = UHS_USB_BulkOnly[B]->GetSectorSize(i);
                                                 printf_P(PSTR("LUN:\t\t%u\r\n"), i);
                                                 printf_P(PSTR("Total Sectors:\t%08lx\t%lu\r\n"), sto[i].TotalSectors, sto[i].TotalSectors);
                                                 printf_P(PSTR("Sector Size:\t%04x\t\t%u\r\n"), sto[i].SectorSize, sto[i].SectorSize);
                                                 // get the partition data...
                                                 PT = new PCPartition;
 
-                                                if (!PT->Init(&sto[i])) {
+                                                if(!PT->Init(&sto[i])) {
                                                         part_t *apart;
-                                                        for (int j = 0; j < 4; j++) {
+                                                        for(int j = 0; j < 4; j++) {
                                                                 apart = PT->GetPart(j);
-                                                                if (apart != NULL && apart->type != 0x00) {
+                                                                if(apart != NULL && apart->type != 0x00) {
                                                                         memcpy(&(parts[cpart]), apart, sizeof (part_t));
                                                                         printf_P(PSTR("Partition %u type %#02x\r\n"), j, parts[cpart].type);
                                                                         // for now
-                                                                        if (isfat(parts[cpart].type)) {
+                                                                        if(isfat(parts[cpart].type)) {
                                                                                 Fats[cpart] = new PFAT(&sto[i], cpart, parts[cpart].firstSector);
                                                                                 //int r = Fats[cpart]->Good();
-                                                                                if (Fats[cpart]->Good()) {
+                                                                                if(Fats[cpart]->MountStatus()) {
                                                                                         delete Fats[cpart];
                                                                                         Fats[cpart] = NULL;
                                                                                 } else cpart++;
@@ -514,7 +500,7 @@ void loop() {
                                                         // try superblock
                                                         Fats[cpart] = new PFAT(&sto[i], cpart, 0);
                                                         //int r = Fats[cpart]->Good();
-                                                        if (Fats[cpart]->Good()) {
+                                                        if(Fats[cpart]->MountStatus()) {
                                                                 //printf_P(PSTR("Superblock error %x\r\n"), r);
                                                                 delete Fats[cpart];
                                                                 Fats[cpart] = NULL;
@@ -523,10 +509,9 @@ void loop() {
                                                 }
                                                 delete PT;
                                         } else {
-                                                sto[i].Read = NULL;
-                                                sto[i].Write = NULL;
                                                 sto[i].Writes = NULL;
                                                 sto[i].Reads = NULL;
+                                                sto[i].Initialize = NULL;
                                                 sto[i].TotalSectors = 0UL;
                                                 sto[i].SectorSize = 0;
                                         }
@@ -535,18 +520,18 @@ void loop() {
                         }
                 }
 
-                if (fatready) {
-                        if (Fats[0] != NULL) {
+                if(fatready) {
+                        if(Fats[0] != NULL) {
                                 struct Pvt * p;
                                 p = ((struct Pvt *)(Fats[0]->storage->private_data));
-                                if (!Bulk[p->B]->LUNIsGood(p->lun)) {
+                                if(!UHS_USB_BulkOnly[p->B]->LUNIsGood(p->lun)) {
                                         // media change
-#if defined(AVR)
+#if !defined(CORE_TEENSY) && defined(__AVR__)
                                         fadeAmount = 80;
 #endif
                                         partsready = false;
-                                        for (int i = 0; i < cpart; i++) {
-                                                if (Fats[i] != NULL)
+                                        for(int i = 0; i < cpart; i++) {
+                                                if(Fats[i] != NULL)
                                                         delete Fats[i];
                                                 Fats[cpart] = NULL;
                                         }
@@ -557,62 +542,64 @@ void loop() {
 
                         }
                 }
-                if (fatready) {
+                if(fatready) {
                         FRESULT rc; /* Result code */
                         UINT bw, br, i;
-
-                        if (!notified) {
-#if defined(AVR)
+                        if(!notified) {
+#if !defined(CORE_TEENSY) && defined(__AVR__)
                                 fadeAmount = 5;
 #endif
                                 notified = true;
+                                FATFS *fs = NULL;
+                                for(int zz = 0; zz < _VOLUMES; zz++) {
+                                        if(Fats[zz]->volmap == 0) fs = Fats[zz]->ffs;
+                                }
                                 printf_P(PSTR("\r\nOpen an existing file (message.txt).\r\n"));
                                 rc = f_open(&My_File_Object_x, "0:/MESSAGE.TXT", FA_READ);
-                                if (rc) printf_P(PSTR("Error %i, message.txt not found.\r\n"), rc);
+                                if(rc) printf_P(PSTR("Error %i, message.txt not found.\r\n"), rc);
                                 else {
                                         printf_P(PSTR("\r\nType the file content.\r\n"));
-                                        for (;;) {
+                                        for(;;) {
                                                 rc = f_read(&My_File_Object_x, My_Buff_x, mbxs, &br); /* Read a chunk of file */
-                                                if (rc || !br) break; /* Error or end of file */
-                                                for (i = 0; i < br; i++) {
+                                                if(rc || !br) break; /* Error or end of file */
+                                                for(i = 0; i < br; i++) {
                                                         /* Type the data */
-                                                        if (My_Buff_x[i] == '\n')
+                                                        if(My_Buff_x[i] == '\n')
                                                                 Serial.write('\r');
-                                                        if (My_Buff_x[i] != '\r')
+                                                        if(My_Buff_x[i] != '\r')
                                                                 Serial.write(My_Buff_x[i]);
                                                         Serial.flush();
                                                 }
                                         }
-                                        if (rc) {
+                                        if(rc) {
                                                 f_close(&My_File_Object_x);
                                                 goto out;
                                         }
 
                                         printf_P(PSTR("\r\nClose the file.\r\n"));
                                         rc = f_close(&My_File_Object_x);
-                                        if (rc) goto out;
+                                        if(rc) goto out;
                                 }
                                 printf_P(PSTR("\r\nCreate a new file (hello.txt).\r\n"));
                                 rc = f_open(&My_File_Object_x, "0:/Hello.TxT", FA_WRITE | FA_CREATE_ALWAYS);
-                                if (rc) {
+                                if(rc) {
                                         die(rc);
                                         goto outdir;
                                 }
                                 printf_P(PSTR("\r\nWrite a text data. (Hello world!)\r\n"));
                                 rc = f_write(&My_File_Object_x, "Hello world!\r\n", 14, &bw);
-                                if (rc) {
+                                if(rc) {
                                         goto out;
                                 }
                                 printf_P(PSTR("%u bytes written.\r\n"), bw);
 
                                 printf_P(PSTR("\r\nClose the file.\r\n"));
                                 rc = f_close(&My_File_Object_x);
-                                if (rc) {
+                                if(rc) {
                                         die(rc);
                                         goto out;
                                 }
-outdir:
-                                {
+outdir:{
 #if _USE_LFN
                                         char lfn[_MAX_LFN + 1];
                                         FILINFO My_File_Info_Object_x; /* File information object */
@@ -621,55 +608,55 @@ outdir:
                                         DIR My_Dir_Object_x; /* Directory object */
                                         printf_P(PSTR("\r\nOpen root directory.\r\n"));
                                         rc = f_opendir(&My_Dir_Object_x, "0:/");
-                                        if (rc) {
+                                        if(rc) {
                                                 die(rc);
                                                 goto out;
                                         }
 
                                         printf_P(PSTR("\r\nDirectory listing...\r\n"));
-#if defined(AVR)
+#if defined(__AVR__)
                                         printf_P(PSTR("Available heap: %u Bytes\r\n"), freeHeap());
 #endif
-                                        for (;;) {
+                                        for(;;) {
 #if _USE_LFN
                                                 My_File_Info_Object_x.lfsize = _MAX_LFN;
 #endif
 
                                                 rc = f_readdir(&My_Dir_Object_x, &My_File_Info_Object_x); /* Read a directory item */
-                                                if (rc || !My_File_Info_Object_x.fname[0]) break; /* Error or end of dir */
+                                                if(rc || !My_File_Info_Object_x.fname[0]) break; /* Error or end of dir */
 
-                                                if (My_File_Info_Object_x.fattrib & AM_DIR) {
+                                                if(My_File_Info_Object_x.fattrib & AM_DIR) {
                                                         Serial.write('d');
                                                 } else {
                                                         Serial.write('-');
                                                 }
                                                 Serial.write('r');
 
-                                                if (My_File_Info_Object_x.fattrib & AM_RDO) {
+                                                if(My_File_Info_Object_x.fattrib & AM_RDO) {
                                                         Serial.write('-');
                                                 } else {
                                                         Serial.write('w');
                                                 }
-                                                if (My_File_Info_Object_x.fattrib & AM_HID) {
+                                                if(My_File_Info_Object_x.fattrib & AM_HID) {
                                                         Serial.write('h');
                                                 } else {
                                                         Serial.write('-');
                                                 }
 
-                                                if (My_File_Info_Object_x.fattrib & AM_SYS) {
+                                                if(My_File_Info_Object_x.fattrib & AM_SYS) {
                                                         Serial.write('s');
                                                 } else {
                                                         Serial.write('-');
                                                 }
 
-                                                if (My_File_Info_Object_x.fattrib & AM_ARC) {
+                                                if(My_File_Info_Object_x.fattrib & AM_ARC) {
                                                         Serial.write('a');
                                                 } else {
                                                         Serial.write('-');
                                                 }
 
 #if _USE_LFN
-                                                if (*My_File_Info_Object_x.lfname)
+                                                if(*My_File_Info_Object_x.lfname)
                                                         printf_P(PSTR(" %8lu  %s (%s)\r\n"), My_File_Info_Object_x.fsize, My_File_Info_Object_x.fname, My_File_Info_Object_x.lfname);
                                                 else
 #endif
@@ -677,48 +664,55 @@ outdir:
                                         }
                                 }
 out:
-                                if (rc) die(rc);
+                                if(rc) die(rc);
+
+                                DISK_IOCTL(fs->drv, CTRL_COMMIT, 0);
                                 printf_P(PSTR("\r\nTest completed.\r\n"));
 
                         }
 
-                        if (runtest) {
+                        if(runtest) {
                                 ULONG ii, wt, rt, start, end;
+                                FATFS *fs = NULL;
+                                for(int zz = 0; zz < _VOLUMES; zz++) {
+                                        if(Fats[zz]->volmap == 0) fs = Fats[zz]->ffs;
+                                }
                                 runtest = false;
                                 f_unlink("0:/10MB.bin");
                                 printf_P(PSTR("\r\nCreate a new 10MB test file (10MB.bin).\r\n"));
                                 rc = f_open(&My_File_Object_x, "0:/10MB.bin", FA_WRITE | FA_CREATE_ALWAYS);
-                                if (rc) goto failed;
-                                for (bw = 0; bw < mbxs; bw++) My_Buff_x[bw] = bw & 0xff;
+                                if(rc) goto failed;
+                                for(bw = 0; bw < mbxs; bw++) My_Buff_x[bw] = bw & 0xff;
                                 fflush(stdout);
                                 start = millis();
-                                while (start == millis());
-                                for (ii = 10485760LU / mbxs; ii > 0LU; ii--) {
+                                while(start == millis());
+                                for(ii = 10485760LU / mbxs; ii > 0LU; ii--) {
                                         rc = f_write(&My_File_Object_x, My_Buff_x, mbxs, &bw);
-                                        if (rc || !bw) goto failed;
+                                        if(rc || !bw) goto failed;
                                 }
                                 rc = f_close(&My_File_Object_x);
-                                if (rc) goto failed;
+                                if(rc) goto failed;
                                 end = millis();
                                 wt = (end - start) - 1;
                                 printf_P(PSTR("Time to write 10485760 bytes: %lu ms (%lu sec) \r\n"), wt, (500 + wt) / 1000UL);
                                 rc = f_open(&My_File_Object_x, "0:/10MB.bin", FA_READ);
                                 fflush(stdout);
                                 start = millis();
-                                while (start == millis());
-                                if (rc) goto failed;
-                                for (;;) {
+                                while(start == millis());
+                                if(rc) goto failed;
+                                for(;;) {
                                         rc = f_read(&My_File_Object_x, My_Buff_x, mbxs, &bw); /* Read a chunk of file */
-                                        if (rc || !bw) break; /* Error or end of file */
+                                        if(rc || !bw) break; /* Error or end of file */
                                 }
                                 end = millis();
-                                if (rc) goto failed;
+                                if(rc) goto failed;
                                 rc = f_close(&My_File_Object_x);
-                                if (rc) goto failed;
+                                if(rc) goto failed;
                                 rt = (end - start) - 1;
                                 printf_P(PSTR("Time to read 10485760 bytes: %lu ms (%lu sec)\r\nDelete test file\r\n"), rt, (500 + rt) / 1000UL);
 failed:
-                                if (rc) die(rc);
+                                if(rc) die(rc);
+                                DISK_IOCTL(fs->drv, CTRL_COMMIT, 0);
                                 printf_P(PSTR("10MB timing test finished.\r\n"));
                         }
                 }
diff --git a/hexdump.h b/hexdump.h
index cf491d94..3818d850 100644
--- a/hexdump.h
+++ b/hexdump.h
@@ -41,7 +41,7 @@ public:
 
 template <class BASE_CLASS, class LEN_TYPE, class OFFSET_TYPE>
 void HexDumper<BASE_CLASS, LEN_TYPE, OFFSET_TYPE>::Parse(const LEN_TYPE len, const uint8_t *pbuf, const OFFSET_TYPE &offset) {
-        if(UsbDEBUGlvl >= 0x80) {  // Fully bypass this block of code if we do not debug.
+        if(UsbDEBUGlvl >= 0x80) { // Fully bypass this block of code if we do not debug.
                 for(LEN_TYPE j = 0; j < len; j++, byteCount++, byteTotal++) {
                         if(!byteCount) {
                                 PrintHex<OFFSET_TYPE > (byteTotal, 0x80);
@@ -58,4 +58,4 @@ void HexDumper<BASE_CLASS, LEN_TYPE, OFFSET_TYPE>::Parse(const LEN_TYPE len, con
         }
 }
 
-#endif // __HEXDUMP_H__
\ No newline at end of file
+#endif // __HEXDUMP_H__
diff --git a/hid.cpp b/hid.cpp
index 0405b04b..c3c425b8 100644
--- a/hid.cpp
+++ b/hid.cpp
@@ -1,7 +1,25 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+
 #include "hid.h"
 
 //get HID report descriptor
 
+/* WRONG! Endpoint is _ALWAYS_ ZERO for HID! We want the _INTERFACE_ value here!
 uint8_t HID::GetReportDescr(uint8_t ep, USBReadParser *parser) {
         const uint8_t constBufLen = 64;
         uint8_t buf[constBufLen];
@@ -12,6 +30,18 @@ uint8_t HID::GetReportDescr(uint8_t ep, USBReadParser *parser) {
         //return ((rcode != hrSTALL) ? rcode : 0);
         return rcode;
 }
+ */
+uint8_t HID::GetReportDescr(uint16_t wIndex, USBReadParser *parser) {
+        const uint8_t constBufLen = 64;
+        uint8_t buf[constBufLen];
+
+        uint8_t rcode = pUsb->ctrlReq(bAddress, 0x00, bmREQ_HIDREPORT, USB_REQUEST_GET_DESCRIPTOR, 0x00,
+                HID_DESCRIPTOR_REPORT, wIndex, 128, constBufLen, buf, (USBReadParser*)parser);
+
+        //return ((rcode != hrSTALL) ? rcode : 0);
+        return rcode;
+}
+
 //uint8_t HID::getHidDescr( uint8_t ep, uint16_t nbytes, uint8_t* dataptr )
 //{
 //    return( pUsb->ctrlReq( bAddress, ep, bmREQ_GET_DESCR, USB_REQUEST_GET_DESCRIPTOR, 0x00, HID_DESCRIPTOR_HID, 0x0000, nbytes, dataptr ));
diff --git a/hid.h b/hid.h
index 8afd9936..a61cfb90 100644
--- a/hid.h
+++ b/hid.h
@@ -20,75 +20,101 @@ e-mail   :  support@circuitsathome.com
 #include "Usb.h"
 #include "hidusagestr.h"
 
-#define DATA_SIZE_MASK           0x03
-#define TYPE_MASK                0x0C
-#define TAG_MASK                 0xF0
+#define MAX_REPORT_PARSERS			2
+#define HID_MAX_HID_CLASS_DESCRIPTORS		5
 
-#define DATA_SIZE_0              0x00
-#define DATA_SIZE_1              0x01
-#define DATA_SIZE_2              0x02
-#define DATA_SIZE_4              0x03
+#define DATA_SIZE_MASK                          0x03
+#define TYPE_MASK                               0x0C
+#define TAG_MASK                                0xF0
 
-#define TYPE_MAIN                0x00
-#define TYPE_GLOBAL              0x04
-#define TYPE_LOCAL               0x08
+#define DATA_SIZE_0                             0x00
+#define DATA_SIZE_1                             0x01
+#define DATA_SIZE_2                             0x02
+#define DATA_SIZE_4                             0x03
 
-#define TAG_MAIN_INPUT           0x80
-#define TAG_MAIN_OUTPUT          0x90
-#define TAG_MAIN_COLLECTION      0xA0
-#define TAG_MAIN_FEATURE         0xB0
-#define TAG_MAIN_ENDCOLLECTION   0xC0
+#define TYPE_MAIN                               0x00
+#define TYPE_GLOBAL                             0x04
+#define TYPE_LOCAL                              0x08
 
-#define TAG_GLOBAL_USAGEPAGE     0x00
-#define TAG_GLOBAL_LOGICALMIN    0x10
-#define TAG_GLOBAL_LOGICALMAX    0x20
-#define TAG_GLOBAL_PHYSMIN       0x30
-#define TAG_GLOBAL_PHYSMAX       0x40
-#define TAG_GLOBAL_UNITEXP       0x50
-#define TAG_GLOBAL_UNIT          0x60
-#define TAG_GLOBAL_REPORTSIZE    0x70
-#define TAG_GLOBAL_REPORTID      0x80
-#define TAG_GLOBAL_REPORTCOUNT   0x90
-#define TAG_GLOBAL_PUSH          0xA0
-#define TAG_GLOBAL_POP           0xB0
+#define TAG_MAIN_INPUT                          0x80
+#define TAG_MAIN_OUTPUT                         0x90
+#define TAG_MAIN_COLLECTION                     0xA0
+#define TAG_MAIN_FEATURE                        0xB0
+#define TAG_MAIN_ENDCOLLECTION                  0xC0
 
-#define TAG_LOCAL_USAGE          0x00
-#define TAG_LOCAL_USAGEMIN       0x10
-#define TAG_LOCAL_USAGEMAX       0x20
+#define TAG_GLOBAL_USAGEPAGE                    0x00
+#define TAG_GLOBAL_LOGICALMIN                   0x10
+#define TAG_GLOBAL_LOGICALMAX                   0x20
+#define TAG_GLOBAL_PHYSMIN                      0x30
+#define TAG_GLOBAL_PHYSMAX                      0x40
+#define TAG_GLOBAL_UNITEXP                      0x50
+#define TAG_GLOBAL_UNIT                         0x60
+#define TAG_GLOBAL_REPORTSIZE                   0x70
+#define TAG_GLOBAL_REPORTID                     0x80
+#define TAG_GLOBAL_REPORTCOUNT                  0x90
+#define TAG_GLOBAL_PUSH                         0xA0
+#define TAG_GLOBAL_POP                          0xB0
+
+#define TAG_LOCAL_USAGE                         0x00
+#define TAG_LOCAL_USAGEMIN                      0x10
+#define TAG_LOCAL_USAGEMAX                      0x20
 
 /* HID requests */
-#define bmREQ_HIDOUT        USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define bmREQ_HIDIN         USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
-#define bmREQ_HIDREPORT     USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_STANDARD|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_HIDOUT                            USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_HIDIN                             USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE
+#define bmREQ_HIDREPORT                         USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_STANDARD|USB_SETUP_RECIPIENT_INTERFACE
 
 /* HID constants. Not part of chapter 9 */
 /* Class-Specific Requests */
-#define HID_REQUEST_GET_REPORT      0x01
-#define HID_REQUEST_GET_IDLE        0x02
-#define HID_REQUEST_GET_PROTOCOL    0x03
-#define HID_REQUEST_SET_REPORT      0x09
-#define HID_REQUEST_SET_IDLE        0x0A
-#define HID_REQUEST_SET_PROTOCOL    0x0B
+#define HID_REQUEST_GET_REPORT                  0x01
+#define HID_REQUEST_GET_IDLE                    0x02
+#define HID_REQUEST_GET_PROTOCOL                0x03
+#define HID_REQUEST_SET_REPORT                  0x09
+#define HID_REQUEST_SET_IDLE                    0x0A
+#define HID_REQUEST_SET_PROTOCOL                0x0B
 
 /* Class Descriptor Types */
-#define HID_DESCRIPTOR_HID			0x21
-#define HID_DESCRIPTOR_REPORT		0x22
-#define HID_DESRIPTOR_PHY			0x23
+#define HID_DESCRIPTOR_HID                      0x21
+#define HID_DESCRIPTOR_REPORT                   0x22
+#define HID_DESRIPTOR_PHY                       0x23
 
 /* Protocol Selection */
-#define HID_BOOT_PROTOCOL			0x00
-#define HID_RPT_PROTOCOL			0x01
+#define HID_BOOT_PROTOCOL                       0x00
+#define HID_RPT_PROTOCOL                        0x01
 
 /* HID Interface Class Code */
-#define HID_INTF                    0x03
+#define HID_INTF                                0x03
 
 /* HID Interface Class SubClass Codes */
-#define HID_BOOT_INTF_SUBCLASS      0x01
+#define HID_BOOT_INTF_SUBCLASS                  0x01
 
 /* HID Interface Class Protocol Codes */
-#define HID_PROTOCOL_NONE           0x00
-#define HID_PROTOCOL_KEYBOARD       0x01
-#define HID_PROTOCOL_MOUSE          0x02
+#define HID_PROTOCOL_NONE                       0x00
+#define HID_PROTOCOL_KEYBOARD                   0x01
+#define HID_PROTOCOL_MOUSE                      0x02
+
+#define HID_ITEM_TYPE_MAIN                      0
+#define HID_ITEM_TYPE_GLOBAL                    1
+#define HID_ITEM_TYPE_LOCAL                     2
+#define HID_ITEM_TYPE_RESERVED                  3
+
+#define HID_LONG_ITEM_PREFIX                    0xfe	// Long item prefix value
+
+#define bmHID_MAIN_ITEM_TAG                     0xfc	// Main item tag mask
+
+#define bmHID_MAIN_ITEM_INPUT                   0x80	// Main item Input tag value
+#define bmHID_MAIN_ITEM_OUTPUT                  0x90	// Main item Output tag value
+#define bmHID_MAIN_ITEM_FEATURE                 0xb0	// Main item Feature tag value
+#define bmHID_MAIN_ITEM_COLLECTION              0xa0	// Main item Collection tag value
+#define bmHID_MAIN_ITEM_END_COLLECTION          0xce	// Main item End Collection tag value
+
+#define HID_MAIN_ITEM_COLLECTION_PHYSICAL	0
+#define HID_MAIN_ITEM_COLLECTION_APPLICATION	1
+#define HID_MAIN_ITEM_COLLECTION_LOGICAL	2
+#define HID_MAIN_ITEM_COLLECTION_REPORT		3
+#define HID_MAIN_ITEM_COLLECTION_NAMED_ARRAY	4
+#define HID_MAIN_ITEM_COLLECTION_USAGE_SWITCH	5
+#define HID_MAIN_ITEM_COLLECTION_USAGE_MODIFIER	6
 
 struct HidItemPrefix {
         uint8_t bSize : 2;
@@ -96,29 +122,6 @@ struct HidItemPrefix {
         uint8_t bTag : 4;
 };
 
-#define HID_ITEM_TYPE_MAIN							0
-#define HID_ITEM_TYPE_GLOBAL						1
-#define HID_ITEM_TYPE_LOCAL							2
-#define HID_ITEM_TYPE_RESERVED						3
-
-#define HID_LONG_ITEM_PREFIX						0xfe		// Long item prefix value
-
-#define bmHID_MAIN_ITEM_TAG							0xfc		// Main item tag mask
-
-#define bmHID_MAIN_ITEM_INPUT						0x80		// Main item Input tag value
-#define bmHID_MAIN_ITEM_OUTPUT						0x90		// Main item Output tag value
-#define bmHID_MAIN_ITEM_FEATURE						0xb0		// Main item Feature tag value
-#define bmHID_MAIN_ITEM_COLLECTION					0xa0		// Main item Collection tag value
-#define bmHID_MAIN_ITEM_END_COLLECTION				0xce		// Main item End Collection tag value
-
-#define HID_MAIN_ITEM_COLLECTION_PHYSICAL			0
-#define HID_MAIN_ITEM_COLLECTION_APPLICATION		1
-#define HID_MAIN_ITEM_COLLECTION_LOGICAL			2
-#define HID_MAIN_ITEM_COLLECTION_REPORT				3
-#define HID_MAIN_ITEM_COLLECTION_NAMED_ARRAY		4
-#define HID_MAIN_ITEM_COLLECTION_USAGE_SWITCH		5
-#define HID_MAIN_ITEM_COLLECTION_USAGE_MODIFIER		6
-
 struct MainItemIOFeature {
         uint8_t bmIsConstantOrData : 1;
         uint8_t bmIsArrayOrVariable : 1;
@@ -137,9 +140,6 @@ public:
         virtual void Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) = 0;
 };
 
-#define MAX_REPORT_PARSERS					2
-#define HID_MAX_HID_CLASS_DESCRIPTORS		5
-
 class HID : public USBDeviceConfig, public UsbConfigXtracter {
 protected:
         USB *pUsb; // USB class instance pointer
@@ -173,7 +173,7 @@ public:
         uint8_t GetIdle(uint8_t iface, uint8_t reportID, uint8_t* dataptr);
         uint8_t SetIdle(uint8_t iface, uint8_t reportID, uint8_t duration);
 
-        uint8_t GetReportDescr(uint8_t ep, USBReadParser *parser = NULL);
+        uint8_t GetReportDescr(uint16_t wIndex, USBReadParser *parser = NULL);
 
         uint8_t GetHidDescr(uint8_t ep, uint16_t nbytes, uint8_t* dataptr);
         uint8_t GetReport(uint8_t ep, uint8_t iface, uint8_t report_type, uint8_t report_id, uint16_t nbytes, uint8_t* dataptr);
diff --git a/hidboot.cpp b/hidboot.cpp
index c7f33789..c32443bb 100644
--- a/hidboot.cpp
+++ b/hidboot.cpp
@@ -18,7 +18,10 @@ e-mail   :  support@circuitsathome.com
 
 void MouseReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {
         MOUSEINFO *pmi = (MOUSEINFO*)buf;
+        // Future:
+        // bool event;
 
+#if 0
         if (prevState.mouseInfo.bmLeftButton == 0 && pmi->bmLeftButton == 1)
                 OnLeftButtonDown(pmi);
 
@@ -43,6 +46,82 @@ void MouseReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *bu
         if (len > sizeof (MOUSEINFO))
                 for (uint8_t i = 0; i<sizeof (MOUSEINFO); i++)
                         prevState.bInfo[i] = buf[i];
+#else
+        //
+        // Optimization idea:
+        //
+        // 1: Don't pass the structure on every event. Buttons would not need it.
+        // 2: Only pass x/y values in the movement routine.
+        //
+        // These two changes (with the ones I have made) will save extra flash.
+        // The only "bad" thing is that it could break old code.
+        //
+        // Future thoughts:
+        //
+        // The extra space gained can be used for a generic mouse event that can be called
+        // when there are _ANY_ changes. This one you _MAY_ want to pass everything, however the
+        // sketch could already have noted these facts to support drag/drop scroll wheel stuff, etc.
+        //
+
+        // Why do we need to pass the structure for buttons?
+        // The function call not enough of a hint for what is happening?
+        if(prevState.mouseInfo.bmLeftButton != pmi->bmLeftButton ) {
+                if(pmi->bmLeftButton) {
+                        OnLeftButtonDown(pmi);
+                } else {
+                        OnLeftButtonUp(pmi);
+                }
+                // Future:
+                // event = true;
+        }
+
+        if(prevState.mouseInfo.bmRightButton != pmi->bmRightButton) {
+                if(pmi->bmRightButton) {
+                        OnRightButtonDown(pmi);
+                } else {
+                        OnRightButtonUp(pmi);
+                }
+                // Future:
+                // event = true;
+        }
+
+        if(prevState.mouseInfo.bmMiddleButton != pmi->bmMiddleButton) {
+                if(pmi->bmMiddleButton) {
+                        OnMiddleButtonDown(pmi);
+                } else {
+                        OnMiddleButtonUp(pmi);
+                }
+                // Future:
+                // event = true;
+        }
+
+        //
+        // Scroll wheel(s), are not part of the spec, but we could support it.
+        // Logitech wireless keyboard and mouse combo reports scroll wheel in byte 4
+        // We wouldn't even need to save this information.
+        //if(len > 3) {
+        //}
+        //
+
+        // Mice only report motion when they actually move!
+        // Why not just pass the x/y values to simplify things??
+        if(pmi->dX || pmi->dY) {
+                OnMouseMove(pmi);
+                // Future:
+                // event = true;
+        }
+
+        //
+        // Future:
+        // Provide a callback that operates on the gathered events from above.
+        //
+        // if(event) OnMouse();
+        //
+
+        // Only the first byte matters (buttons). We do NOT need to save position info.
+        prevState.bInfo[0] = buf[0];
+#endif
+
 };
 
 void KeyboardReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {
@@ -52,11 +131,11 @@ void KeyboardReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t
 
         //KBDINFO	*pki = (KBDINFO*)buf;
 
-	    // provide event for changed control key state
-		if (prevState.bInfo[0x00] != buf[0x00]) {
-			OnControlKeysChanged(prevState.bInfo[0x00], buf[0x00]);
-		}
-	
+	// provide event for changed control key state
+	if (prevState.bInfo[0x00] != buf[0x00]) {
+		OnControlKeysChanged(prevState.bInfo[0x00], buf[0x00]);
+	}
+
         for (uint8_t i = 2; i < 8; i++) {
                 bool down = false;
                 bool up = false;
@@ -82,13 +161,13 @@ uint8_t KeyboardReportParser::HandleLockingKeys(HID *hid, uint8_t key) {
         uint8_t old_keys = kbdLockingKeys.bLeds;
 
         switch (key) {
-                case KEY_NUM_LOCK:
+                case UHS_HID_BOOT_KEY_NUM_LOCK:
                         kbdLockingKeys.kbdLeds.bmNumLock = ~kbdLockingKeys.kbdLeds.bmNumLock;
                         break;
-                case KEY_CAPS_LOCK:
+                case UHS_HID_BOOT_KEY_CAPS_LOCK:
                         kbdLockingKeys.kbdLeds.bmCapsLock = ~kbdLockingKeys.kbdLeds.bmCapsLock;
                         break;
-                case KEY_SCROLL_LOCK:
+                case UHS_HID_BOOT_KEY_SCROLL_LOCK:
                         kbdLockingKeys.kbdLeds.bmScrollLock = ~kbdLockingKeys.kbdLeds.bmScrollLock;
                         break;
         }
@@ -99,16 +178,16 @@ uint8_t KeyboardReportParser::HandleLockingKeys(HID *hid, uint8_t key) {
         return 0;
 }
 
-const uint8_t KeyboardReportParser::numKeys[] PROGMEM = {'!', '@', '#', '$', '%', '^', '&', '*', '(', ')'};
-const uint8_t KeyboardReportParser::symKeysUp[] PROGMEM = {'_', '+', '{', '}', '|', '~', ':', '"', '~', '<', '>', '?'};
-const uint8_t KeyboardReportParser::symKeysLo[] PROGMEM = {'-', '=', '[', ']', '\\', ' ', ';', '\'', '`', ',', '.', '/'};
-const uint8_t KeyboardReportParser::padKeys[] PROGMEM = {'/', '*', '-', '+', 0x13};
+const uint8_t KeyboardReportParser::numKeys[10] PROGMEM = {'!', '@', '#', '$', '%', '^', '&', '*', '(', ')'};
+const uint8_t KeyboardReportParser::symKeysUp[12] PROGMEM = {'_', '+', '{', '}', '|', '~', ':', '"', '~', '<', '>', '?'};
+const uint8_t KeyboardReportParser::symKeysLo[12] PROGMEM = {'-', '=', '[', ']', '\\', ' ', ';', '\'', '`', ',', '.', '/'};
+const uint8_t KeyboardReportParser::padKeys[5] PROGMEM = {'/', '*', '-', '+', 0x13};
 
 uint8_t KeyboardReportParser::OemToAscii(uint8_t mod, uint8_t key) {
         uint8_t shift = (mod & 0x22);
 
         // [a-z]
-        if (key > 0x03 && key < 0x1e) {
+        if (VALUE_WITHIN(key, 0x04, 0x1d)) {
                 // Upper case letters
                 if ((kbdLockingKeys.kbdLeds.bmCapsLock == 0 && (mod & 2)) ||
                         (kbdLockingKeys.kbdLeds.bmCapsLock == 1 && (mod & 2) == 0))
@@ -118,26 +197,25 @@ uint8_t KeyboardReportParser::OemToAscii(uint8_t mod, uint8_t key) {
                 else
                         return (key - 4 + 'a');
         }// Numbers
-        else if (key > 0x1d && key < 0x27) {
+        else if (VALUE_WITHIN(key, 0x1e, 0x27)) {
                 if (shift)
-                        return ((uint8_t)pgm_read_byte(&numKeys[key - 0x1e]));
+                        return ((uint8_t)pgm_read_byte(&getNumKeys()[key - 0x1e]));
                 else
-                        return (key - 0x1e + '1');
+                        return ((key == UHS_HID_BOOT_KEY_ZERO) ? '0' : key - 0x1e + '1');
         }// Keypad Numbers
-        else if (key > 0x58 && key < 0x62) {
-                if (kbdLockingKeys.kbdLeds.bmNumLock == 1)
+        else if(VALUE_WITHIN(key, 0x59, 0x61)) {
+                if(kbdLockingKeys.kbdLeds.bmNumLock == 1)
                         return (key - 0x59 + '1');
-        } else if (key > 0x2c && key < 0x39)
-                return ((shift) ? (uint8_t)pgm_read_byte(&symKeysUp[key - 0x2d]) : (uint8_t)pgm_read_byte(&symKeysLo[key - 0x2d]));
-        else if (key > 0x53 && key < 0x59)
-                return (uint8_t)pgm_read_byte(&padKeys[key - 0x54]);
+        } else if(VALUE_WITHIN(key, 0x2d, 0x38))
+                return ((shift) ? (uint8_t)pgm_read_byte(&getSymKeysUp()[key - 0x2d]) : (uint8_t)pgm_read_byte(&getSymKeysLo()[key - 0x2d]));
+        else if(VALUE_WITHIN(key, 0x54, 0x58))
+                return (uint8_t)pgm_read_byte(&getPadKeys()[key - 0x54]);
         else {
-                switch (key) {
-                        case KEY_SPACE: return (0x20);
-                        case KEY_ENTER: return (0x13);
-                        case KEY_ZERO: return ((shift) ? ')': '0');
-                        case KEY_ZERO2: return ((kbdLockingKeys.kbdLeds.bmNumLock == 1) ? '0': 0);
-                        case KEY_PERIOD: return ((kbdLockingKeys.kbdLeds.bmNumLock == 1) ? '.': 0);
+                switch(key) {
+                        case UHS_HID_BOOT_KEY_SPACE: return (0x20);
+                        case UHS_HID_BOOT_KEY_ENTER: return (0x13);
+                        case UHS_HID_BOOT_KEY_ZERO2: return ((kbdLockingKeys.kbdLeds.bmNumLock == 1) ? '0': 0);
+                        case UHS_HID_BOOT_KEY_PERIOD: return ((kbdLockingKeys.kbdLeds.bmNumLock == 1) ? '.': 0);
                 }
         }
         return ( 0);
diff --git a/hidboot.h b/hidboot.h
index 42224c68..0326bf25 100644
--- a/hidboot.h
+++ b/hidboot.h
@@ -19,11 +19,22 @@ e-mail   :  support@circuitsathome.com
 
 #include "hid.h"
 
-#define KEY_SPACE							0x2c
-#define KEY_ZERO							0x27
-#define KEY_ZERO2							0x62
-#define KEY_ENTER							0x28
-#define KEY_PERIOD							0x63
+#define UHS_HID_BOOT_KEY_ZERO           0x27
+#define UHS_HID_BOOT_KEY_ENTER          0x28
+#define UHS_HID_BOOT_KEY_SPACE          0x2c
+#define UHS_HID_BOOT_KEY_CAPS_LOCK      0x39
+#define UHS_HID_BOOT_KEY_SCROLL_LOCK    0x47
+#define UHS_HID_BOOT_KEY_NUM_LOCK       0x53
+#define UHS_HID_BOOT_KEY_ZERO2          0x62
+#define UHS_HID_BOOT_KEY_PERIOD         0x63
+
+// Don't worry, GCC will optimize the result to a final value.
+#define bitsEndpoints(p) ((((p) & HID_PROTOCOL_KEYBOARD)? 2 : 0) | (((p) & HID_PROTOCOL_MOUSE)? 1 : 0))
+#define totalEndpoints(p) ((bitsEndpoints(p) == 3) ? 3 : 2)
+#define epMUL(p) ((((p) & HID_PROTOCOL_KEYBOARD)? 1 : 0) + (((p) & HID_PROTOCOL_MOUSE)? 1 : 0))
+
+// Already defined in hid.h
+// #define HID_MAX_HID_CLASS_DESCRIPTORS 5
 
 struct MOUSEINFO {
 
@@ -31,7 +42,7 @@ struct MOUSEINFO {
                 uint8_t bmLeftButton : 1;
                 uint8_t bmRightButton : 1;
                 uint8_t bmMiddleButton : 1;
-                uint8_t bmDummy : 1;
+                uint8_t bmDummy : 5;
         };
         int8_t dX;
         int8_t dY;
@@ -41,7 +52,7 @@ class MouseReportParser : public HIDReportParser {
 
         union {
                 MOUSEINFO mouseInfo;
-                uint8_t bInfo[sizeof(MOUSEINFO)];
+                uint8_t bInfo[sizeof (MOUSEINFO)];
         } prevState;
 
 public:
@@ -107,21 +118,17 @@ struct KBDLEDS {
         uint8_t bmReserved : 3;
 };
 
-#define KEY_NUM_LOCK    0x53
-#define KEY_CAPS_LOCK	0x39
-#define KEY_SCROLL_LOCK	0x47
-
 class KeyboardReportParser : public HIDReportParser {
-        static const uint8_t numKeys[];
-        static const uint8_t symKeysUp[];
-        static const uint8_t symKeysLo[];
-        static const uint8_t padKeys[];
+        static const uint8_t numKeys[10];
+        static const uint8_t symKeysUp[12];
+        static const uint8_t symKeysLo[12];
+        static const uint8_t padKeys[5];
 
 protected:
 
         union {
                 KBDINFO kbdInfo;
-                uint8_t bInfo[sizeof(KBDINFO)];
+                uint8_t bInfo[sizeof (KBDINFO)];
         } prevState;
 
         union {
@@ -150,17 +157,29 @@ protected:
 
         virtual void OnKeyUp(uint8_t mod, uint8_t key) {
         };
-};
 
-#define totalEndpoints (((BOOT_PROTOCOL & HID_PROTOCOL_KEYBOARD)? 2 : 0)+((BOOT_PROTOCOL & HID_PROTOCOL_MOUSE)? 1 : 0))
-#define epMUL (((BOOT_PROTOCOL & HID_PROTOCOL_KEYBOARD)? 1 : 0)+((BOOT_PROTOCOL & HID_PROTOCOL_MOUSE)? 1 : 0))
-#define HID_MAX_HID_CLASS_DESCRIPTORS 5
+        virtual const uint8_t *getNumKeys() {
+                return numKeys;
+        };
+
+        virtual const uint8_t *getSymKeysUp() {
+                return symKeysUp;
+        };
+
+        virtual const uint8_t *getSymKeysLo() {
+                return symKeysLo;
+        };
+
+        virtual const uint8_t *getPadKeys() {
+                return padKeys;
+        };
+};
 
 template <const uint8_t BOOT_PROTOCOL>
 class HIDBoot : public HID //public USBDeviceConfig, public UsbConfigXtracter
 {
-        EpInfo epInfo[totalEndpoints];
-        HIDReportParser *pRptParser[epMUL];
+        EpInfo epInfo[totalEndpoints(BOOT_PROTOCOL)];
+        HIDReportParser *pRptParser[epMUL(BOOT_PROTOCOL)];
 
         uint8_t bConfNum; // configuration number
         uint8_t bIfaceNum; // Interface Number
@@ -168,6 +187,7 @@ class HIDBoot : public HID //public USBDeviceConfig, public UsbConfigXtracter
         uint8_t bNumEP; // total number of EP in the configuration
         uint32_t qNextPollTime; // next poll time
         bool bPollEnable; // poll enable flag
+        uint8_t bInterval; // largest interval
 
         void Initialize();
 
@@ -203,7 +223,7 @@ qNextPollTime(0),
 bPollEnable(false) {
         Initialize();
 
-        for(uint8_t i = 0; i < epMUL; i++) {
+        for(int i = 0; i < epMUL(BOOT_PROTOCOL); i++) {
                 pRptParser[i] = NULL;
         }
         if(pUsb)
@@ -212,7 +232,7 @@ bPollEnable(false) {
 
 template <const uint8_t BOOT_PROTOCOL>
 void HIDBoot<BOOT_PROTOCOL>::Initialize() {
-        for(uint8_t i = 0; i < totalEndpoints; i++) {
+        for(int i = 0; i < totalEndpoints(BOOT_PROTOCOL); i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
@@ -225,7 +245,7 @@ void HIDBoot<BOOT_PROTOCOL>::Initialize() {
 
 template <const uint8_t BOOT_PROTOCOL>
 uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed) {
-        const uint8_t constBufSize = sizeof(USB_DEVICE_DESCRIPTOR);
+        const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
 
         uint8_t buf[constBufSize];
         uint8_t rcode;
@@ -240,10 +260,13 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
         AddressPool &addrPool = pUsb->GetAddressPool();
 
         USBTRACE("BM Init\r\n");
+        //USBTRACE2("totalEndpoints:", (uint8_t) (totalEndpoints(BOOT_PROTOCOL)));
+        //USBTRACE2("epMUL:", epMUL(BOOT_PROTOCOL));
 
         if(bAddress)
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
+        bInterval = 0;
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
@@ -264,7 +287,7 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
         p->lowspeed = lowspeed;
 
         // Get device descriptor
-        rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*) buf);
+        rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*)buf);
 
         if(!rcode)
                 len = (buf[0] > constBufSize) ? constBufSize : buf[0];
@@ -286,7 +309,7 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from the device descriptor
-        epInfo[0].maxPktSize = (uint8_t) ((USB_DEVICE_DESCRIPTOR*) buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
@@ -298,6 +321,7 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
                 USBTRACE2("setAddr:", rcode);
                 return rcode;
         }
+        //delay(2); //per USB 2.0 sect.9.2.6.3
 
         USBTRACE2("Addr:", bAddress);
 
@@ -311,63 +335,76 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
         p->lowspeed = lowspeed;
 
         if(len)
-                rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*) buf);
+                rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*)buf);
 
         if(rcode)
                 goto FailGetDevDescr;
 
-        num_of_conf = ((USB_DEVICE_DESCRIPTOR*) buf)->bNumConfigurations;
+        num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
 
-        // Assign epInfo to epinfo pointer
-        rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
-
-        if(rcode)
-                goto FailSetDevTblEntry;
-
-        //USBTRACE2("NC:", num_of_conf);
+        USBTRACE2("NC:", num_of_conf);
 
         // GCC will optimize unused stuff away.
-        if(BOOT_PROTOCOL & HID_PROTOCOL_KEYBOARD) {
+        if((BOOT_PROTOCOL & (HID_PROTOCOL_KEYBOARD | HID_PROTOCOL_MOUSE)) == (HID_PROTOCOL_KEYBOARD | HID_PROTOCOL_MOUSE)) {
+                USBTRACE("HID_PROTOCOL_KEYBOARD AND MOUSE\r\n");
+                ConfigDescParser<
+                        USB_CLASS_HID,
+                        HID_BOOT_INTF_SUBCLASS,
+                        HID_PROTOCOL_KEYBOARD | HID_PROTOCOL_MOUSE,
+                        CP_MASK_COMPARE_ALL > confDescrParser(this);
+                confDescrParser.SetOR(); // Use the OR variant.
                 for(uint8_t i = 0; i < num_of_conf; i++) {
-                        ConfigDescParser<
-                                USB_CLASS_HID,
-                                HID_BOOT_INTF_SUBCLASS,
-                                HID_PROTOCOL_KEYBOARD,
-                                CP_MASK_COMPARE_ALL> confDescrParserA(this);
-
-                        if(bNumEP == totalEndpoints)
+                        pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
+                        if(bNumEP == (uint8_t)(totalEndpoints(BOOT_PROTOCOL)))
                                 break;
-                        pUsb->getConfDescr(bAddress, 0, i, &confDescrParserA);
+                }
+        } else {
+                // GCC will optimize unused stuff away.
+                if(BOOT_PROTOCOL & HID_PROTOCOL_KEYBOARD) {
+                        USBTRACE("HID_PROTOCOL_KEYBOARD\r\n");
+                        for(uint8_t i = 0; i < num_of_conf; i++) {
+                                ConfigDescParser<
+                                        USB_CLASS_HID,
+                                        HID_BOOT_INTF_SUBCLASS,
+                                        HID_PROTOCOL_KEYBOARD,
+                                        CP_MASK_COMPARE_ALL> confDescrParserA(this);
+
+                                pUsb->getConfDescr(bAddress, 0, i, &confDescrParserA);
+                                if(bNumEP == (uint8_t)(totalEndpoints(BOOT_PROTOCOL)))
+                                        break;
+                        }
+                }
+
+                // GCC will optimize unused stuff away.
+                if(BOOT_PROTOCOL & HID_PROTOCOL_MOUSE) {
+                        USBTRACE("HID_PROTOCOL_MOUSE\r\n");
+                        for(uint8_t i = 0; i < num_of_conf; i++) {
+                                ConfigDescParser<
+                                        USB_CLASS_HID,
+                                        HID_BOOT_INTF_SUBCLASS,
+                                        HID_PROTOCOL_MOUSE,
+                                        CP_MASK_COMPARE_ALL> confDescrParserB(this);
+
+                                pUsb->getConfDescr(bAddress, 0, i, &confDescrParserB);
+                                if(bNumEP == ((uint8_t)(totalEndpoints(BOOT_PROTOCOL))))
+                                        break;
+
+                        }
                 }
         }
+        USBTRACE2("bNumEP:", bNumEP);
 
-        // GCC will optimize unused stuff away.
-        if(BOOT_PROTOCOL & HID_PROTOCOL_MOUSE) {
-                for(uint8_t i = 0; i < num_of_conf; i++) {
-                        ConfigDescParser<
-                                USB_CLASS_HID,
-                                HID_BOOT_INTF_SUBCLASS,
-                                HID_PROTOCOL_MOUSE,
-                                CP_MASK_COMPARE_ALL> confDescrParserB(this);
-                        if(bNumEP == totalEndpoints)
-                                break;
-
-                        pUsb->getConfDescr(bAddress, 0, i, &confDescrParserB);
-                }
-        }
-
-        USBTRACE2("\r\nbAddr:", bAddress);
-        USBTRACE2("\r\nbNumEP:", bNumEP);
-        USBTRACE2("\r\ntotalEndpoints:", totalEndpoints);
-        if(bNumEP != totalEndpoints) {
+        if(bNumEP != (uint8_t)(totalEndpoints(BOOT_PROTOCOL))) {
                 rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
                 goto Fail;
         }
 
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo);
+        //USBTRACE2("setEpInfoEntry returned ", rcode);
+        USBTRACE2("Cnf:", bConfNum);
 
-        USBTRACE2("\r\nCnf:", bConfNum);
+        delay(1000);
 
         // Set Configuration Value
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
@@ -375,19 +412,38 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
         if(rcode)
                 goto FailSetConfDescr;
 
-        USBTRACE2("\r\nIf:", bIfaceNum);
+        delay(1000);
 
-        rcode = SetProtocol(bIfaceNum, HID_BOOT_PROTOCOL);
+        USBTRACE2("bIfaceNum:", bIfaceNum);
+        USBTRACE2("bNumIface:", bNumIface);
 
-        if(rcode)
-                goto FailSetProtocol;
+        // Yes, mouse wants SetProtocol and SetIdle too!
+        for(uint8_t i = 0; i < epMUL(BOOT_PROTOCOL); i++) {
+                USBTRACE2("\r\nInterface:", i);
+                rcode = SetProtocol(i, HID_BOOT_PROTOCOL);
+                if(rcode) goto FailSetProtocol;
+                USBTRACE2("PROTOCOL SET HID_BOOT rcode:", rcode);
+                rcode = SetIdle(i, 0, 0);
+                USBTRACE2("SET_IDLE rcode:", rcode);
+                // if(rcode) goto FailSetIdle; This can fail.
+                // Get the RPIPE and just throw it away.
+                SinkParser<USBReadParser, uint16_t, uint16_t> sink;
+                rcode = GetReportDescr(i, &sink);
+                USBTRACE2("RPIPE rcode:", rcode);
+        }
+
+        // Get RPIPE and throw it away.
 
-        // GCC will optimize unused stuff away.
         if(BOOT_PROTOCOL & HID_PROTOCOL_KEYBOARD) {
-                rcode = SetIdle(0/* bIfaceNum*/, 0, 0);
-
-                if(rcode)
-                        goto FailSetIdle;
+                // Wake keyboard interface by twinkling up to 5 LEDs that are in the spec.
+                // kana, compose, scroll, caps, num
+                rcode = 0x20; // Reuse rcode.
+                while(rcode) {
+                        rcode >>= 1;
+                        // Ignore any error returned, we don't care if LED is not supported
+                        SetReport(0, 0, 2, 0, 1, &rcode); // Eventually becomes zero (All off)
+                        delay(25);
+                }
         }
         USBTRACE("BM configured\r\n");
 
@@ -400,17 +456,17 @@ FailGetDevDescr:
         goto Fail;
 #endif
 
-FailSetDevTblEntry:
-#ifdef DEBUG_USB_HOST
-        NotifyFailSetDevTblEntry();
-        goto Fail;
-#endif
+        //FailSetDevTblEntry:
+        //#ifdef DEBUG_USB_HOST
+        //        NotifyFailSetDevTblEntry();
+        //        goto Fail;
+        //#endif
 
-FailGetConfDescr:
-#ifdef DEBUG_USB_HOST
-        NotifyFailGetConfDescr();
-        goto Fail;
-#endif
+        //FailGetConfDescr:
+        //#ifdef DEBUG_USB_HOST
+        //        NotifyFailGetConfDescr();
+        //        goto Fail;
+        //#endif
 
 FailSetConfDescr:
 #ifdef DEBUG_USB_HOST
@@ -424,36 +480,39 @@ FailSetProtocol:
         goto Fail;
 #endif
 
-FailSetIdle:
-#ifdef DEBUG_USB_HOST
-        USBTRACE("SetIdle:");
-#endif
+        //FailSetIdle:
+        //#ifdef DEBUG_USB_HOST
+        //        USBTRACE("SetIdle:");
+        //#endif
 
 Fail:
 #ifdef DEBUG_USB_HOST
         NotifyFail(rcode);
 #endif
         Release();
+
         return rcode;
 }
 
 template <const uint8_t BOOT_PROTOCOL>
 void HIDBoot<BOOT_PROTOCOL>::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) {
+
         // If the first configuration satisfies, the others are not considered.
-        if(bNumEP > 1 && conf != bConfNum)
+        //if(bNumEP > 1 && conf != bConfNum)
+        if(bNumEP == totalEndpoints(BOOT_PROTOCOL))
                 return;
 
         bConfNum = conf;
         bIfaceNum = iface;
 
         if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) {
-                uint8_t index = bNumEP;//epInterruptInIndex; //+ bNumEP;
+                if(pep->bInterval > bInterval) bInterval = pep->bInterval;
 
                 // Fill in the endpoint info structure
-                epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F);
-                epInfo[index].maxPktSize = (uint8_t) pep->wMaxPacketSize;
-                epInfo[index].epAttribs = 0;
-                epInfo[index].bmNakPower = USB_NAK_NOWAIT;
+                epInfo[bNumEP].epAddr = (pep->bEndpointAddress & 0x0F);
+                epInfo[bNumEP].maxPktSize = (uint8_t)pep->wMaxPacketSize;
+                epInfo[bNumEP].epAttribs = 0;
+                epInfo[bNumEP].bmNakPower = USB_NAK_NOWAIT;
                 bNumEP++;
 
         }
@@ -469,6 +528,7 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Release() {
         bAddress = 0;
         qNextPollTime = 0;
         bPollEnable = false;
+
         return 0;
 }
 
@@ -476,40 +536,48 @@ template <const uint8_t BOOT_PROTOCOL>
 uint8_t HIDBoot<BOOT_PROTOCOL>::Poll() {
         uint8_t rcode = 0;
 
-        if(!bPollEnable)
-                return 0;
+        if(bPollEnable && ((long)(millis() - qNextPollTime) >= 0L)) {
 
-        if(qNextPollTime <= millis()) {
-                qNextPollTime = millis() + 10;
-
-                // To-do: optimize manually, getting rid of the loop
-                for(uint8_t i = 0; i < epMUL; i++) {
-                        const uint8_t const_buff_len = 16;
+                // To-do: optimize manually, using the for loop only if needed.
+                for(int i = 0; i < epMUL(BOOT_PROTOCOL); i++) {
+                        const uint16_t const_buff_len = 16;
                         uint8_t buf[const_buff_len];
 
+                        USBTRACE3("(hidboot.h) i=", i, 0x81);
+                        USBTRACE3("(hidboot.h) epInfo[epInterruptInIndex + i].epAddr=", epInfo[epInterruptInIndex + i].epAddr, 0x81);
+                        USBTRACE3("(hidboot.h) epInfo[epInterruptInIndex + i].maxPktSize=", epInfo[epInterruptInIndex + i].maxPktSize, 0x81);
+                        uint16_t read = (uint16_t)epInfo[epInterruptInIndex + i].maxPktSize;
 
-                        uint16_t read = (uint16_t) epInfo[epInterruptInIndex + i].maxPktSize;
-
-                        uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[epInterruptInIndex + i].epAddr, &read, buf);
-                        if(!rcode) {
+                        rcode = pUsb->inTransfer(bAddress, epInfo[epInterruptInIndex + i].epAddr, &read, buf);
+                        // SOME buggy dongles report extra keys (like sleep) using a 2 byte packet on the wrong endpoint.
+                        // Since keyboard and mice must report at least 3 bytes, we ignore the extra data.
+                        if(!rcode && read > 2) {
                                 if(pRptParser[i])
-                                        pRptParser[i]->Parse((HID*)this, 0, (uint8_t) read, buf);
-
-#if 0 // Set this to 1 to print the incoming data
-                                for (uint8_t i=0; i < read; i++) {
-                                        PrintHex<uint8_t > (buf[i], 0x80);
-                                        USB_HOST_SERIAL.write(' ');
-                                }
-                                if (read)
-                                        USB_HOST_SERIAL.println();
-#endif
+                                        pRptParser[i]->Parse((HID*)this, 0, (uint8_t)read, buf);
+#ifdef DEBUG_USB_HOST
+                                // We really don't care about errors and anomalies unless we are debugging.
                         } else {
                                 if(rcode != hrNAK) {
-                                        USBTRACE2("Poll:", rcode);
-                                        break;
+                                        USBTRACE3("(hidboot.h) Poll:", rcode, 0x81);
+                                }
+                                if(!rcode && read) {
+                                        USBTRACE3("(hidboot.h) Strange read count: ", read, 0x80);
+                                        USBTRACE3("(hidboot.h) Interface:", i, 0x80);
                                 }
                         }
+
+                        if(!rcode && read && (UsbDEBUGlvl > 0x7f)) {
+                                for(uint8_t i = 0; i < read; i++) {
+                                        PrintHex<uint8_t > (buf[i], 0x80);
+                                        USBTRACE1(" ", 0x80);
+                                }
+                                if(read)
+                                        USBTRACE1("\r\n", 0x80);
+#endif
+                        }
+
                 }
+                qNextPollTime = millis() + bInterval;
         }
         return rcode;
 }
diff --git a/hidescriptorparser.cpp b/hidescriptorparser.cpp
index d4c96f20..f3040ce8 100644
--- a/hidescriptorparser.cpp
+++ b/hidescriptorparser.cpp
@@ -1,3 +1,20 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+
 #include "hidescriptorparser.h"
 
 const char * const ReportDescParserBase::usagePageTitles0[] PROGMEM = {
@@ -980,7 +997,7 @@ void ReportDescParserBase::Parse(const uint16_t len, const uint8_t *pbuf, const
 
         totalSize = 0;
 
-        while (cntdn) {
+        while(cntdn) {
                 //USB_HOST_SERIAL.println("");
                 //PrintHex<uint16_t>(offset + len - cntdn);
                 //USB_HOST_SERIAL.print(":");
@@ -995,7 +1012,7 @@ void ReportDescParserBase::Parse(const uint16_t len, const uint8_t *pbuf, const
 
 void ReportDescParserBase::PrintValue(uint8_t *p, uint8_t len) {
         E_Notify(PSTR("("), 0x80);
-        for (; len; p++, len--)
+        for(; len; p++, len--)
                 PrintHex<uint8_t > (*p, 0x80);
         E_Notify(PSTR(")"), 0x80);
 }
@@ -1007,7 +1024,7 @@ void ReportDescParserBase::PrintByteValue(uint8_t data) {
 }
 
 void ReportDescParserBase::PrintItemTitle(uint8_t prefix) {
-        switch (prefix & (TYPE_MASK | TAG_MASK)) {
+        switch(prefix & (TYPE_MASK | TAG_MASK)) {
                 case (TYPE_GLOBAL | TAG_GLOBAL_PUSH):
                         E_Notify(PSTR("\r\nPush"), 0x80);
                         break;
@@ -1073,10 +1090,10 @@ void ReportDescParserBase::PrintItemTitle(uint8_t prefix) {
 
 uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) {
         //uint8_t	ret = enErrorSuccess;
-
-        switch (itemParseState) {
+        //reinterpret_cast<>(varBuffer);
+        switch(itemParseState) {
                 case 0:
-                        if (**pp == HID_LONG_ITEM_PREFIX)
+                        if(**pp == HID_LONG_ITEM_PREFIX)
                                 USBTRACE("\r\nLONG\r\n");
                         else {
                                 uint8_t size = ((**pp) & DATA_SIZE_MASK);
@@ -1091,10 +1108,10 @@ uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) {
                         itemSize--;
                         itemParseState = 1;
 
-                        if (!itemSize)
+                        if(!itemSize)
                                 break;
 
-                        if (!pcntdn)
+                        if(!pcntdn)
                                 return enErrorIncomplete;
                 case 1:
                         //USBTRACE2("\r\niSz:",itemSize);
@@ -1103,19 +1120,20 @@ uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) {
                         valParser.Initialize(&theBuffer);
                         itemParseState = 2;
                 case 2:
-                        if (!valParser.Parse(pp, pcntdn))
+                        if(!valParser.Parse(pp, pcntdn))
                                 return enErrorIncomplete;
                         itemParseState = 3;
                 case 3:
                 {
                         uint8_t data = *((uint8_t*)varBuffer);
 
-                        switch (itemPrefix & (TYPE_MASK | TAG_MASK)) {
+                        switch(itemPrefix & (TYPE_MASK | TAG_MASK)) {
                                 case (TYPE_LOCAL | TAG_LOCAL_USAGE):
-                                        if (pfUsage) {
-                                                if (theBuffer.valueSize > 1)
-                                                        pfUsage(*((uint16_t*)varBuffer));
-                                                else
+                                        if(pfUsage) {
+                                                if(theBuffer.valueSize > 1) {
+                                                        uint16_t* ui16 = reinterpret_cast<uint16_t *>(varBuffer);
+                                                        pfUsage(*ui16);
+                                                } else
                                                         pfUsage(data);
                                         }
                                         break;
@@ -1148,7 +1166,7 @@ uint8_t ReportDescParserBase::ParseItem(uint8_t **pp, uint16_t *pcntdn) {
                                         break;
                                 case (TYPE_MAIN | TAG_MAIN_COLLECTION):
                                 case (TYPE_MAIN | TAG_MAIN_ENDCOLLECTION):
-                                        switch (data) {
+                                        switch(data) {
                                                 case 0x00:
                                                         E_Notify(PSTR(" Physical"), 0x80);
                                                         break;
@@ -1215,18 +1233,23 @@ ReportDescParserBase::UsagePageFunc ReportDescParserBase::usagePageFunctions[] /
 void ReportDescParserBase::SetUsagePage(uint16_t page) {
         pfUsage = NULL;
 
-        if (page > 0x00 && page < 0x11)
-                pfUsage = /*(UsagePageFunc)pgm_read_pointer*/(usagePageFunctions[page - 1]);
+        if(VALUE_BETWEEN(page, 0x00, 0x11))
+                pfUsage = (usagePageFunctions[page - 1]);
+
+                // Dead code...
+                //
+                //      pfUsage = (UsagePageFunc)pgm_read_pointer(usagePageFunctions[page - 1]);
                 //else if (page > 0x7f && page < 0x84)
-                //	E_Notify(pstrUsagePageMonitor);
+                //      E_Notify(pstrUsagePageMonitor);
                 //else if (page > 0x83 && page < 0x8c)
                 //	E_Notify(pstrUsagePagePower);
                 //else if (page > 0x8b && page < 0x92)
                 //	E_Notify((char*)pgm_read_pointer(&usagePageTitles1[page - 0x8c]));
                 //else if (page > 0xfeff && page <= 0xffff)
                 //	E_Notify(pstrUsagePageVendorDefined);
+                //
         else
-                switch (page) {
+                switch(page) {
                         case 0x14:
                                 pfUsage = &ReportDescParserBase::PrintAlphanumDisplayPageUsage;
                                 break;
@@ -1237,20 +1260,19 @@ void ReportDescParserBase::SetUsagePage(uint16_t page) {
 }
 
 void ReportDescParserBase::PrintUsagePage(uint16_t page) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (page > 0x00 && page < 0x11)
-                E_Notify((char*)pgm_read_pointer(&usagePageTitles0[page - 1]), 0x80);
-        else if (page > 0x7f && page < 0x84)
+        output_if_between(page, 0x00, 0x11, w, E_Notify, usagePageTitles0, 0x80)
+        else output_if_between(page, 0x8b, 0x92, w, E_Notify, usagePageTitles1, 0x80)
+        else if(VALUE_BETWEEN(page, 0x7f, 0x84))
                 E_Notify(pstrUsagePageMonitor, 0x80);
-        else if (page > 0x83 && page < 0x8c)
+        else if(VALUE_BETWEEN(page, 0x83, 0x8c))
                 E_Notify(pstrUsagePagePower, 0x80);
-        else if (page > 0x8b && page < 0x92)
-                E_Notify((char*)pgm_read_pointer(&usagePageTitles1[page - 0x8c]), 0x80);
-        else if (page > 0xfeff && page <= 0xffff)
+        else if(page > 0xfeff /* && page <= 0xffff */)
                 E_Notify(pstrUsagePageVendorDefined, 0x80);
         else
-                switch (page) {
+                switch(page) {
                         case 0x14:
                                 E_Notify(pstrUsagePageAlphaNumericDisplay, 0x80);
                                 break;
@@ -1280,197 +1302,147 @@ void ReportDescParserBase::PrintOrdinalPageUsage(uint16_t usage) {
 }
 
 void ReportDescParserBase::PrintGenericDesktopPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x0a)
-                E_Notify((char*)pgm_read_pointer(&genDesktopTitles0[usage - 1]), 0x80);
-        else if (usage > 0x2f && usage < 0x49)
-                E_Notify((char*)pgm_read_pointer(&genDesktopTitles1[usage - 0x30]), 0x80);
-        else if (usage > 0x7f && usage < 0x94)
-                E_Notify((char*)pgm_read_pointer(&genDesktopTitles2[usage - 0x80]), 0x80);
-        else if (usage > 0x9f && usage < 0xa9)
-                E_Notify((char*)pgm_read_pointer(&genDesktopTitles3[usage - 0xa0]), 0x80);
-        else if (usage > 0xaf && usage < 0xb8)
-                E_Notify((char*)pgm_read_pointer(&genDesktopTitles4[usage - 0xb0]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x0a, w, E_Notify, genDesktopTitles0, 0x80)
+        else output_if_between(usage, 0x2f, 0x49, w, E_Notify, genDesktopTitles1, 0x80)
+        else output_if_between(usage, 0x7f, 0x94, w, E_Notify, genDesktopTitles2, 0x80)
+        else output_if_between(usage, 0x9f, 0xa9, w, E_Notify, genDesktopTitles3, 0x80)
+        else output_if_between(usage, 0xaf, 0xb8, w, E_Notify, genDesktopTitles4, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintSimulationControlsPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x0d)
-                E_Notify((char*)pgm_read_pointer(&simuTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x26)
-                E_Notify((char*)pgm_read_pointer(&simuTitles1[usage - 0x20]), 0x80);
-        else if (usage > 0xaf && usage < 0xd1)
-                E_Notify((char*)pgm_read_pointer(&simuTitles2[usage - 0xb0]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x0d, w, E_Notify, simuTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x26, w, E_Notify, simuTitles1, 0x80)
+        else output_if_between(usage, 0xaf, 0xd1, w, E_Notify, simuTitles2, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintVRControlsPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x0b)
-                E_Notify((char*)pgm_read_pointer(&vrTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x22)
-                E_Notify((char*)pgm_read_pointer(&vrTitles1[usage - 0x20]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x0b, w, E_Notify, vrTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x22, w, E_Notify, vrTitles1, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintSportsControlsPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x05)
-                E_Notify((char*)pgm_read_pointer(&sportsCtrlTitles0[usage - 1]), 0x80);
-        else if (usage > 0x2f && usage < 0x3a)
-                E_Notify((char*)pgm_read_pointer(&sportsCtrlTitles1[usage - 0x30]), 0x80);
-        else if (usage > 0x4f && usage < 0x64)
-                E_Notify((char*)pgm_read_pointer(&sportsCtrlTitles2[usage - 0x50]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x05, w, E_Notify, sportsCtrlTitles0, 0x80)
+        else output_if_between(usage, 0x2f, 0x3a, w, E_Notify, sportsCtrlTitles1, 0x80)
+        else output_if_between(usage, 0x4f, 0x64, w, E_Notify, sportsCtrlTitles2, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintGameControlsPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x04)
-                E_Notify((char*)pgm_read_pointer(&gameTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x3a)
-                E_Notify((char*)pgm_read_pointer(&gameTitles1[usage - 0x20]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x04, w, E_Notify, gameTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x3a, w, E_Notify, gameTitles1, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintGenericDeviceControlsPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x1f && usage < 0x27)
-                E_Notify((char*)pgm_read_pointer(&genDevCtrlTitles[usage - 0x20]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x1f, 0x27, w, E_Notify, genDevCtrlTitles, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintLEDPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x4e)
-                E_Notify((char*)pgm_read_pointer(&ledTitles[usage - 1]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x4e, w, E_Notify, ledTitles, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintTelephonyPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x08)
-                E_Notify((char*)pgm_read_pointer(&telTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x32)
-                E_Notify((char*)pgm_read_pointer(&telTitles1[usage - 0x1f]), 0x80);
-        else if (usage > 0x4f && usage < 0x54)
-                E_Notify((char*)pgm_read_pointer(&telTitles2[usage - 0x4f]), 0x80);
-        else if (usage > 0x6f && usage < 0x75)
-                E_Notify((char*)pgm_read_pointer(&telTitles3[usage - 0x6f]), 0x80);
-        else if (usage > 0x8f && usage < 0x9f)
-                E_Notify((char*)pgm_read_pointer(&telTitles4[usage - 0x8f]), 0x80);
-        else if (usage > 0xaf && usage < 0xc0)
-                E_Notify((char*)pgm_read_pointer(&telTitles5[usage - 0xaf]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x08, w, E_Notify, telTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x32, w, E_Notify, telTitles1, 0x80)
+        else output_if_between(usage, 0x4f, 0x54, w, E_Notify, telTitles2, 0x80)
+        else output_if_between(usage, 0x6f, 0x75, w, E_Notify, telTitles3, 0x80)
+        else output_if_between(usage, 0x8f, 0x9f, w, E_Notify, telTitles4, 0x80)
+        else output_if_between(usage, 0xaf, 0xc0, w, E_Notify, telTitles5, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintConsumerPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x07)
-                E_Notify((char*)pgm_read_pointer(&consTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x23)
-                E_Notify((char*)pgm_read_pointer(&consTitles1[usage - 0x1f]), 0x80);
-        else if (usage > 0x2f && usage < 0x37)
-                E_Notify((char*)pgm_read_pointer(&consTitles2[usage - 0x2f]), 0x80);
-        else if (usage > 0x3f && usage < 0x49)
-                E_Notify((char*)pgm_read_pointer(&consTitles3[usage - 0x3f]), 0x80);
-        else if (usage > 0x5f && usage < 0x67)
-                E_Notify((char*)pgm_read_pointer(&consTitles4[usage - 0x5f]), 0x80);
-        else if (usage > 0x7f && usage < 0xa5)
-                E_Notify((char*)pgm_read_pointer(&consTitles5[usage - 0x7f]), 0x80);
-        else if (usage > 0xaf && usage < 0xcf)
-                E_Notify((char*)pgm_read_pointer(&consTitles6[usage - 0xaf]), 0x80);
-        else if (usage > 0xdf && usage < 0xeb)
-                E_Notify((char*)pgm_read_pointer(&consTitles7[usage - 0xdf]), 0x80);
-        else if (usage > 0xef && usage < 0xf6)
-                E_Notify((char*)pgm_read_pointer(&consTitles8[usage - 0xef]), 0x80);
-        else if (usage > 0xff && usage < 0x10e)
-                E_Notify((char*)pgm_read_pointer(&consTitles9[usage - 0xff]), 0x80);
-        else if (usage > 0x14f && usage < 0x156)
-                E_Notify((char*)pgm_read_pointer(&consTitlesA[usage - 0x14f]), 0x80);
-        else if (usage > 0x15f && usage < 0x16b)
-                E_Notify((char*)pgm_read_pointer(&consTitlesB[usage - 0x15f]), 0x80);
-        else if (usage > 0x16f && usage < 0x175)
-                E_Notify((char*)pgm_read_pointer(&consTitlesC[usage - 0x16f]), 0x80);
-        else if (usage > 0x17f && usage < 0x1c8)
-                E_Notify((char*)pgm_read_pointer(&consTitlesD[usage - 0x17f]), 0x80);
-        else if (usage > 0x1ff && usage < 0x29d)
-                E_Notify((char*)pgm_read_pointer(&consTitlesE[usage - 0x1ff]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x07, w, E_Notify, consTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x23, w, E_Notify, consTitles1, 0x80)
+        else output_if_between(usage, 0x2f, 0x37, w, E_Notify, consTitles2, 0x80)
+        else output_if_between(usage, 0x3f, 0x49, w, E_Notify, consTitles3, 0x80)
+        else output_if_between(usage, 0x5f, 0x67, w, E_Notify, consTitles4, 0x80)
+        else output_if_between(usage, 0x7f, 0xa5, w, E_Notify, consTitles5, 0x80)
+        else output_if_between(usage, 0xaf, 0xcf, w, E_Notify, consTitles6, 0x80)
+        else output_if_between(usage, 0xdf, 0xeb, w, E_Notify, consTitles7, 0x80)
+        else output_if_between(usage, 0xef, 0xf6, w, E_Notify, consTitles8, 0x80)
+        else output_if_between(usage, 0xff, 0x10e, w, E_Notify, consTitles9, 0x80)
+        else output_if_between(usage, 0x14f, 0x156, w, E_Notify, consTitlesA, 0x80)
+        else output_if_between(usage, 0x15f, 0x16b, w, E_Notify, consTitlesB, 0x80)
+        else output_if_between(usage, 0x16f, 0x175, w, E_Notify, consTitlesC, 0x80)
+        else output_if_between(usage, 0x17f, 0x1c8, w, E_Notify, consTitlesD, 0x80)
+        else output_if_between(usage, 0x1ff, 0x29d, w, E_Notify, consTitlesE, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintDigitizerPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x0e)
-                E_Notify((char*)pgm_read_pointer(&digitTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x23)
-                E_Notify((char*)pgm_read_pointer(&digitTitles1[usage - 0x1f]), 0x80);
-        else if (usage > 0x2f && usage < 0x47)
-                E_Notify((char*)pgm_read_pointer(&digitTitles2[usage - 0x2f]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x0e, w, E_Notify, digitTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x23, w, E_Notify, digitTitles1, 0x80)
+        else output_if_between(usage, 0x2f, 0x47, w, E_Notify, digitTitles2, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintAlphanumDisplayPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage > 0x00 && usage < 0x03)
-                E_Notify((char*)pgm_read_pointer(&aplphanumTitles0[usage - 1]), 0x80);
-        else if (usage > 0x1f && usage < 0x4e)
-                E_Notify((char*)pgm_read_pointer(&aplphanumTitles1[usage - 0x1f]), 0x80);
-        else if (usage > 0x7f && usage < 0x96)
-                E_Notify((char*)pgm_read_pointer(&digitTitles2[usage - 0x80]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        output_if_between(usage, 0x00, 0x03, w, E_Notify, aplphanumTitles0, 0x80)
+        else output_if_between(usage, 0x1f, 0x4e, w, E_Notify, aplphanumTitles1, 0x80)
+        else output_if_between(usage, 0x7f, 0x96, w, E_Notify, digitTitles2, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 void ReportDescParserBase::PrintMedicalInstrumentPageUsage(uint16_t usage) {
+        const char * const * w;
         E_Notify(pstrSpace, 0x80);
 
-        if (usage == 1)
-                E_Notify(pstrUsageMedicalUltrasound, 0x80);
-        else if (usage > 0x1f && usage < 0x28)
-                E_Notify((char*)pgm_read_pointer(&medInstrTitles0[usage - 0x1f]), 0x80);
-        else if (usage > 0x3f && usage < 0x45)
-                E_Notify((char*)pgm_read_pointer(&medInstrTitles1[usage - 0x40]), 0x80);
-        else if (usage > 0x5f && usage < 0x62)
-                E_Notify((char*)pgm_read_pointer(&medInstrTitles2[usage - 0x60]), 0x80);
-        else if (usage == 0x70)
+        if(usage == 1) E_Notify(pstrUsageMedicalUltrasound, 0x80);
+        else if(usage == 0x70)
                 E_Notify(pstrUsageDepthGainCompensation, 0x80);
-        else if (usage > 0x7f && usage < 0x8a)
-                E_Notify((char*)pgm_read_pointer(&medInstrTitles3[usage - 0x80]), 0x80);
-        else if (usage > 0x9f && usage < 0xa2)
-                E_Notify((char*)pgm_read_pointer(&medInstrTitles4[usage - 0xa0]), 0x80);
-        else
-                E_Notify(pstrUsagePageUndefined, 0x80);
+        else output_if_between(usage, 0x1f, 0x28, w, E_Notify, medInstrTitles0, 0x80)
+        else output_if_between(usage, 0x3f, 0x45, w, E_Notify, medInstrTitles1, 0x80)
+        else output_if_between(usage, 0x5f, 0x62, w, E_Notify, medInstrTitles2, 0x80)
+        else output_if_between(usage, 0x7f, 0x8a, w, E_Notify, medInstrTitles3, 0x80)
+        else output_if_between(usage, 0x9f, 0xa2, w, E_Notify, medInstrTitles4, 0x80)
+        else E_Notify(pstrUsagePageUndefined, 0x80);
 }
 
 uint8_t ReportDescParser2::ParseItem(uint8_t **pp, uint16_t *pcntdn) {
         //uint8_t	ret = enErrorSuccess;
 
-        switch (itemParseState) {
+        switch(itemParseState) {
                 case 0:
-                        if (**pp == HID_LONG_ITEM_PREFIX)
+                        if(**pp == HID_LONG_ITEM_PREFIX)
                                 USBTRACE("\r\nLONG\r\n");
                         else {
                                 uint8_t size = ((**pp) & DATA_SIZE_MASK);
@@ -1482,29 +1454,30 @@ uint8_t ReportDescParser2::ParseItem(uint8_t **pp, uint16_t *pcntdn) {
                         itemSize--;
                         itemParseState = 1;
 
-                        if (!itemSize)
+                        if(!itemSize)
                                 break;
 
-                        if (!pcntdn)
+                        if(!pcntdn)
                                 return enErrorIncomplete;
                 case 1:
                         theBuffer.valueSize = itemSize;
                         valParser.Initialize(&theBuffer);
                         itemParseState = 2;
                 case 2:
-                        if (!valParser.Parse(pp, pcntdn))
+                        if(!valParser.Parse(pp, pcntdn))
                                 return enErrorIncomplete;
                         itemParseState = 3;
                 case 3:
                 {
                         uint8_t data = *((uint8_t*)varBuffer);
 
-                        switch (itemPrefix & (TYPE_MASK | TAG_MASK)) {
+                        switch(itemPrefix & (TYPE_MASK | TAG_MASK)) {
                                 case (TYPE_LOCAL | TAG_LOCAL_USAGE):
-                                        if (pfUsage) {
-                                                if (theBuffer.valueSize > 1)
-                                                        pfUsage(*((uint16_t*)varBuffer));
-                                                else
+                                        if(pfUsage) {
+                                                if(theBuffer.valueSize > 1) {
+                                                        uint16_t* ui16 = reinterpret_cast<uint16_t *>(varBuffer);
+                                                        pfUsage(*ui16);
+                                                } else
                                                         pfUsage(data);
                                         }
                                         break;
@@ -1556,7 +1529,7 @@ void ReportDescParser2::OnInputItem(uint8_t itm) {
         uint8_t bit_offset = totalSize - tmp; // number of bits in the current byte already handled
         uint8_t *p = pBuf + byte_offset; // current byte pointer
 
-        if (bit_offset)
+        if(bit_offset)
                 *p >>= bit_offset;
 
         uint8_t usage = useMin;
@@ -1566,7 +1539,7 @@ void ReportDescParser2::OnInputItem(uint8_t itm) {
         uint8_t bits_of_byte = 8;
 
         // for each field in field array defined by rptCount
-        for (uint8_t field = 0; field < rptCount; field++, usage++) {
+        for(uint8_t field = 0; field < rptCount; field++, usage++) {
 
                 union {
                         uint8_t bResult[4];
@@ -1577,7 +1550,7 @@ void ReportDescParser2::OnInputItem(uint8_t itm) {
                 result.dwResult = 0;
                 uint8_t mask = 0;
 
-                if (print_usemin_usemax)
+                if(print_usemin_usemax)
                         pfUsage(usage);
 
                 // bits_left		- number of bits in the field(array of fields, depending on Report Count) left to process
@@ -1585,7 +1558,7 @@ void ReportDescParser2::OnInputItem(uint8_t itm) {
                 // bits_to_copy		- number of bits to copy to result buffer
 
                 // for each bit in a field
-                for (uint8_t bits_left = rptSize, bits_to_copy = 0; bits_left;
+                for(uint8_t bits_left = rptSize, bits_to_copy = 0; bits_left;
                         bits_left -= bits_to_copy) {
                         bits_to_copy = (bits_left > bits_of_byte) ? bits_of_byte : bits_left;
 
@@ -1597,7 +1570,7 @@ void ReportDescParser2::OnInputItem(uint8_t itm) {
 
                         mask = 0;
 
-                        for (uint8_t j = bits_to_copy; j; j--) {
+                        for(uint8_t j = bits_to_copy; j; j--) {
                                 mask <<= 1;
                                 mask |= 1;
                         }
@@ -1606,7 +1579,7 @@ void ReportDescParser2::OnInputItem(uint8_t itm) {
 
                         bits_of_byte -= bits_to_copy;
 
-                        if (bits_of_byte < 1) {
+                        if(bits_of_byte < 1) {
                                 bits_of_byte = 8;
                                 p++;
                         }
@@ -1621,6 +1594,6 @@ void UniversalReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t
 
         uint8_t ret = hid->GetReportDescr(0, &prs);
 
-        if (ret)
+        if(ret)
                 ErrorMessage<uint8_t > (PSTR("GetReportDescr-2"), ret);
 }
diff --git a/hidescriptorparser.h b/hidescriptorparser.h
index a28ff212..c43aaaea 100644
--- a/hidescriptorparser.h
+++ b/hidescriptorparser.h
@@ -101,7 +101,7 @@ protected:
         MultiValueBuffer theBuffer;
         MultiByteValueParser valParser;
         ByteSkipper theSkipper;
-        uint8_t varBuffer[sizeof(USB_CONFIGURATION_DESCRIPTOR)];
+        uint8_t varBuffer[sizeof (USB_CONFIGURATION_DESCRIPTOR)];
 
         uint8_t itemParseState; // Item parser state variable
         uint8_t itemSize; // Item size
diff --git a/hiduniversal.cpp b/hiduniversal.cpp
index 536b0193..e7a65e39 100644
--- a/hiduniversal.cpp
+++ b/hiduniversal.cpp
@@ -1,20 +1,38 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+
 #include "hiduniversal.h"
 
 HIDUniversal::HIDUniversal(USB *p) :
 HID(p),
 qNextPollTime(0),
+pollInterval(0),
 bPollEnable(false),
 bHasReportId(false) {
         Initialize();
 
-        if (pUsb)
+        if(pUsb)
                 pUsb->RegisterDeviceClass(this);
 }
 
 uint16_t HIDUniversal::GetHidClassDescrLen(uint8_t type, uint8_t num) {
-        for (uint8_t i = 0, n = 0; i < HID_MAX_HID_CLASS_DESCRIPTORS; i++) {
-                if (descrInfo[i].bDescrType == type) {
-                        if (n == num)
+        for(uint8_t i = 0, n = 0; i < HID_MAX_HID_CLASS_DESCRIPTORS; i++) {
+                if(descrInfo[i].bDescrType == type) {
+                        if(n == num)
                                 return descrInfo[i].wDescriptorLength;
                         n++;
                 }
@@ -23,22 +41,22 @@ uint16_t HIDUniversal::GetHidClassDescrLen(uint8_t type, uint8_t num) {
 }
 
 void HIDUniversal::Initialize() {
-        for (uint8_t i = 0; i < MAX_REPORT_PARSERS; i++) {
+        for(uint8_t i = 0; i < MAX_REPORT_PARSERS; i++) {
                 rptParsers[i].rptId = 0;
                 rptParsers[i].rptParser = NULL;
         }
-        for (uint8_t i = 0; i < HID_MAX_HID_CLASS_DESCRIPTORS; i++) {
+        for(uint8_t i = 0; i < HID_MAX_HID_CLASS_DESCRIPTORS; i++) {
                 descrInfo[i].bDescrType = 0;
                 descrInfo[i].wDescriptorLength = 0;
         }
-        for (uint8_t i = 0; i < maxHidInterfaces; i++) {
+        for(uint8_t i = 0; i < maxHidInterfaces; i++) {
                 hidInterfaces[i].bmInterface = 0;
                 hidInterfaces[i].bmProtocol = 0;
 
-                for (uint8_t j = 0; j < maxEpPerInterface; j++)
+                for(uint8_t j = 0; j < maxEpPerInterface; j++)
                         hidInterfaces[i].epIndex[j] = 0;
         }
-        for (uint8_t i = 0; i < totalEndpoints; i++) {
+        for(uint8_t i = 0; i < totalEndpoints; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
@@ -47,13 +65,14 @@ void HIDUniversal::Initialize() {
         bNumEP = 1;
         bNumIface = 0;
         bConfNum = 0;
+        pollInterval = 0;
 
         ZeroMemory(constBuffLen, prevBuf);
 }
 
 bool HIDUniversal::SetReportParser(uint8_t id, HIDReportParser *prs) {
-        for (uint8_t i = 0; i < MAX_REPORT_PARSERS; i++) {
-                if (rptParsers[i].rptId == 0 && rptParsers[i].rptParser == NULL) {
+        for(uint8_t i = 0; i < MAX_REPORT_PARSERS; i++) {
+                if(rptParsers[i].rptId == 0 && rptParsers[i].rptParser == NULL) {
                         rptParsers[i].rptId = id;
                         rptParsers[i].rptParser = prs;
                         return true;
@@ -63,11 +82,11 @@ bool HIDUniversal::SetReportParser(uint8_t id, HIDReportParser *prs) {
 }
 
 HIDReportParser* HIDUniversal::GetReportParser(uint8_t id) {
-        if (!bHasReportId)
+        if(!bHasReportId)
                 return ((rptParsers[0].rptParser) ? rptParsers[0].rptParser : NULL);
 
-        for (uint8_t i = 0; i < MAX_REPORT_PARSERS; i++) {
-                if (rptParsers[i].rptId == id)
+        for(uint8_t i = 0; i < MAX_REPORT_PARSERS; i++) {
+                if(rptParsers[i].rptId == id)
                         return rptParsers[i].rptParser;
         }
         return NULL;
@@ -77,6 +96,7 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
 
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -89,16 +109,16 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         USBTRACE("HU Init\r\n");
 
-        if (bAddress)
+        if(bAddress)
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
                 USBTRACE("epinfo\r\n");
                 return USB_ERROR_EPINFO_IS_NULL;
         }
@@ -114,10 +134,10 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Get device descriptor
         rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*)buf);
 
-        if (!rcode)
+        if(!rcode)
                 len = (buf[0] > constBufSize) ? constBufSize : buf[0];
 
-        if (rcode) {
+        if(rcode) {
                 // Restore p->epinfo
                 p->epinfo = oldep_ptr;
 
@@ -130,16 +150,16 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from the device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
         // Assign new address to the device
         rcode = pUsb->setAddr(0, 0, bAddress);
 
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -147,7 +167,7 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 return rcode;
         }
 
-				delay( 2 ); //per USB 2.0 sect.9.2.6.3 
+        //delay(2); //per USB 2.0 sect.9.2.6.3
 
         USBTRACE2("Addr:", bAddress);
 
@@ -155,28 +175,31 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
 
-        if (len)
+        if(len)
                 rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*)buf);
 
-        if (rcode)
+        if(rcode)
                 goto FailGetDevDescr;
 
-        num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
+        VID = udd->idVendor; // Can be used by classes that inherits this class to check the VID and PID of the connected device
+        PID = udd->idProduct;
+
+        num_of_conf = udd->bNumConfigurations;
 
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         USBTRACE2("NC:", num_of_conf);
 
-        for (uint8_t i = 0; i < num_of_conf; i++) {
+        for(uint8_t i = 0; i < num_of_conf; i++) {
                 //HexDumper<USBReadParser, uint16_t, uint16_t>		HexDump;
                 ConfigDescParser<USB_CLASS_HID, 0, 0,
                         CP_MASK_COMPARE_CLASS> confDescrParser(this);
@@ -184,34 +207,34 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 //rcode = pUsb->getConfDescr(bAddress, 0, i, &HexDump);
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
-                if (bNumEP > 1)
+                if(bNumEP > 1)
                         break;
         } // for
 
-        if (bNumEP < 2)
+        if(bNumEP < 2)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo);
 
-        USBTRACE2("\r\nCnf:", bConfNum);
+        USBTRACE2("Cnf:", bConfNum);
 
         // Set Configuration Value
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
-        for (uint8_t i = 0; i < bNumIface; i++) {
-                if (hidInterfaces[i].epIndex[epInterruptInIndex] == 0)
+        for(uint8_t i = 0; i < bNumIface; i++) {
+                if(hidInterfaces[i].epIndex[epInterruptInIndex] == 0)
                         continue;
 
                 rcode = SetIdle(hidInterfaces[i].bmInterface, 0, 0);
 
-                if (rcode && rcode != hrSTALL)
+                if(rcode && rcode != hrSTALL)
                         goto FailSetIdle;
         }
 
@@ -252,8 +275,8 @@ FailSetIdle:
         USBTRACE("SetIdle:");
 #endif
 
-Fail:
 #ifdef DEBUG_USB_HOST
+Fail:
         NotifyFail(rcode);
 #endif
         Release();
@@ -261,8 +284,8 @@ Fail:
 }
 
 HIDUniversal::HIDInterface* HIDUniversal::FindInterface(uint8_t iface, uint8_t alt, uint8_t proto) {
-        for (uint8_t i = 0; i < bNumIface && i < maxHidInterfaces; i++)
-                if (hidInterfaces[i].bmInterface == iface && hidInterfaces[i].bmAltSet == alt
+        for(uint8_t i = 0; i < bNumIface && i < maxHidInterfaces; i++)
+                if(hidInterfaces[i].bmInterface == iface && hidInterfaces[i].bmAltSet == alt
                         && hidInterfaces[i].bmProtocol == proto)
                         return hidInterfaces + i;
         return NULL;
@@ -270,7 +293,7 @@ HIDUniversal::HIDInterface* HIDUniversal::FindInterface(uint8_t iface, uint8_t a
 
 void HIDUniversal::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) {
         // If the first configuration satisfies, the others are not concidered.
-        if (bNumEP > 1 && conf != bConfNum)
+        if(bNumEP > 1 && conf != bConfNum)
                 return;
 
         //ErrorMessage<uint8_t>(PSTR("\r\nConf.Val"), conf);
@@ -283,7 +306,7 @@ void HIDUniversal::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint
         HIDInterface *piface = FindInterface(iface, alt, proto);
 
         // Fill in interface structure in case of new interface
-        if (!piface) {
+        if(!piface) {
                 piface = hidInterfaces + bNumIface;
                 piface->bmInterface = iface;
                 piface->bmAltSet = alt;
@@ -291,12 +314,12 @@ void HIDUniversal::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint
                 bNumIface++;
         }
 
-        if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
+        if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
                 index = epInterruptInIndex;
         else
                 index = epInterruptOutIndex;
 
-        if (index) {
+        if(index) {
                 // Fill in the endpoint info structure
                 epInfo[bNumEP].epAddr = (pep->bEndpointAddress & 0x0F);
                 epInfo[bNumEP].maxPktSize = (uint8_t)pep->wMaxPacketSize;
@@ -306,6 +329,9 @@ void HIDUniversal::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint
                 // Fill in the endpoint index list
                 piface->epIndex[index] = bNumEP; //(pep->bEndpointAddress & 0x0F);
 
+                if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints
+                        pollInterval = pep->bInterval;
+
                 bNumEP++;
         }
         //PrintEndpointDescriptor(pep);
@@ -322,34 +348,34 @@ uint8_t HIDUniversal::Release() {
 }
 
 bool HIDUniversal::BuffersIdentical(uint8_t len, uint8_t *buf1, uint8_t *buf2) {
-        for (uint8_t i = 0; i < len; i++)
-                if (buf1[i] != buf2[i])
+        for(uint8_t i = 0; i < len; i++)
+                if(buf1[i] != buf2[i])
                         return false;
         return true;
 }
 
 void HIDUniversal::ZeroMemory(uint8_t len, uint8_t *buf) {
-        for (uint8_t i = 0; i < len; i++)
+        for(uint8_t i = 0; i < len; i++)
                 buf[i] = 0;
 }
 
 void HIDUniversal::SaveBuffer(uint8_t len, uint8_t *src, uint8_t *dest) {
-        for (uint8_t i = 0; i < len; i++)
+        for(uint8_t i = 0; i < len; i++)
                 dest[i] = src[i];
 }
 
 uint8_t HIDUniversal::Poll() {
         uint8_t rcode = 0;
 
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
 
-        if (qNextPollTime <= millis()) {
-                qNextPollTime = millis() + 50;
+        if((long)(millis() - qNextPollTime) >= 0L) {
+                qNextPollTime = millis() + pollInterval;
 
                 uint8_t buf[constBuffLen];
 
-                for (uint8_t i = 0; i < bNumIface; i++) {
+                for(uint8_t i = 0; i < bNumIface; i++) {
                         uint8_t index = hidInterfaces[i].epIndex[epInterruptInIndex];
                         uint16_t read = (uint16_t)epInfo[index].maxPktSize;
 
@@ -357,32 +383,36 @@ uint8_t HIDUniversal::Poll() {
 
                         uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[index].epAddr, &read, buf);
 
-                        if (rcode) {
-                                if (rcode != hrNAK)
-                                        USBTRACE2("Poll:", rcode);
+                        if(rcode) {
+                                if(rcode != hrNAK)
+                                        USBTRACE3("(hiduniversal.h) Poll:", rcode, 0x81);
                                 return rcode;
                         }
 
-                        if (read > constBuffLen)
+                        if(read > constBuffLen)
                                 read = constBuffLen;
 
                         bool identical = BuffersIdentical(read, buf, prevBuf);
 
                         SaveBuffer(read, buf, prevBuf);
 
-                        if (identical)
+                        if(identical)
                                 return 0;
-
+#if 0
                         Notify(PSTR("\r\nBuf: "), 0x80);
 
-                        for (uint8_t i = 0; i < read; i++)
+                        for(uint8_t i = 0; i < read; i++) {
                                 D_PrintHex<uint8_t > (buf[i], 0x80);
+                                Notify(PSTR(" "), 0x80);
+                        }
 
                         Notify(PSTR("\r\n"), 0x80);
+#endif
+                        ParseHIDData(this, bHasReportId, (uint8_t)read, buf);
 
                         HIDReportParser *prs = GetReportParser(((bHasReportId) ? *buf : 0));
 
-                        if (prs)
+                        if(prs)
                                 prs->Parse(this, bHasReportId, (uint8_t)read, buf);
                 }
         }
diff --git a/hiduniversal.h b/hiduniversal.h
index e1ea26c4..3e090aa7 100644
--- a/hiduniversal.h
+++ b/hiduniversal.h
@@ -1,3 +1,20 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+
 #if !defined(__HIDUNIVERSAL_H__)
 #define __HIDUNIVERSAL_H__
 
@@ -17,10 +34,7 @@ class HIDUniversal : public HID {
         // Returns HID class specific descriptor length by its type and order number
         uint16_t GetHidClassDescrLen(uint8_t type, uint8_t num);
 
-        EpInfo epInfo[totalEndpoints];
-
         struct HIDInterface {
-
                 struct {
                         uint8_t bmInterface : 3;
                         uint8_t bmAltSet : 3;
@@ -29,12 +43,11 @@ class HIDUniversal : public HID {
                 uint8_t epIndex[maxEpPerInterface];
         };
 
-        HIDInterface hidInterfaces[maxHidInterfaces];
-
         uint8_t bConfNum; // configuration number
         uint8_t bNumIface; // number of interfaces in the configuration
         uint8_t bNumEP; // total number of EP in the configuration
         uint32_t qNextPollTime; // next poll time
+        uint8_t pollInterval;
         bool bPollEnable; // poll enable flag
 
         static const uint16_t constBuffLen = 64; // event buffer length
@@ -48,8 +61,13 @@ class HIDUniversal : public HID {
         void SaveBuffer(uint8_t len, uint8_t *src, uint8_t *dest);
 
 protected:
+        EpInfo epInfo[totalEndpoints];
+        HIDInterface hidInterfaces[maxHidInterfaces];
+
         bool bHasReportId;
 
+        uint16_t PID, VID; // PID and VID of connected device
+
         // HID implementation
         virtual HIDReportParser* GetReportParser(uint8_t id);
 
@@ -57,6 +75,10 @@ protected:
                 return 0;
         };
 
+        virtual void ParseHIDData(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf) {
+                return;
+        };
+
 public:
         HIDUniversal(USB *p);
 
@@ -72,8 +94,12 @@ public:
                 return bAddress;
         };
 
+        virtual bool isReady() {
+                return bPollEnable;
+        };
+
         // UsbConfigXtracter implementation
         virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep);
 };
 
-#endif // __HIDUNIVERSAL_H__
\ No newline at end of file
+#endif // __HIDUNIVERSAL_H__
diff --git a/hidusagestr.h b/hidusagestr.h
index 6c137e02..f99f0e39 100644
--- a/hidusagestr.h
+++ b/hidusagestr.h
@@ -17,7 +17,7 @@ e-mail   :  support@circuitsathome.com
 #if !defined( __HIDUSAGESTR_H__)
 #define  __HIDUSAGESTR_H__
 
-#include <avr/pgmspace.h>
+#include "Usb.h"
 
 const char pstrSpace [] PROGMEM = " ";
 const char pstrCRLF [] PROGMEM = "\r\n";
@@ -974,4 +974,4 @@ const char pstrUsageSoftControlAdjust [] PROGMEM = "Soft Ctrl Adj";
 //const char *medInstrTitles3[];
 //const char *medInstrTitles4[];
 
-#endif //__HIDUSAGESTR_H__
\ No newline at end of file
+#endif //__HIDUSAGESTR_H__
diff --git a/hidusagetitlearrays.cpp b/hidusagetitlearrays.cpp
index 09d59302..8bdd41b5 100644
--- a/hidusagetitlearrays.cpp
+++ b/hidusagetitlearrays.cpp
@@ -17,7 +17,6 @@ e-mail   :  support@circuitsathome.com
 #if !defined(__HIDUSAGETITLEARRAYS_H__)
 #define __HIDUSAGETITLEARRAYS_H__
 
-#include <avr/pgmspace.h>
 #include "hidusagestr.h"
 
 //const char *usagePageTitles0[]	PROGMEM =
@@ -1044,4 +1043,4 @@ e-mail   :  support@circuitsathome.com
 //	pstrUsageSoftControlAdjust
 //};
 
-#endif // __HIDUSAGETITLEARRAYS_H__
\ No newline at end of file
+#endif // __HIDUSAGETITLEARRAYS_H__
diff --git a/keywords.txt b/keywords.txt
index c4f5bd87..c7bd007c 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -25,7 +25,7 @@ BTD	KEYWORD1
 Task	KEYWORD2
 
 ####################################################
-# Syntax Coloring Map For PS3 Bluetooth/USB Library
+# Syntax Coloring Map For PS3/PS4 Bluetooth/USB Library
 ####################################################
 
 ####################################################
@@ -34,6 +34,8 @@ Task	KEYWORD2
 
 PS3BT	KEYWORD1
 PS3USB	KEYWORD1
+PS4BT	KEYWORD1
+PS4USB	KEYWORD1
 
 ####################################################
 # Methods and Functions (KEYWORD2)
@@ -52,7 +54,7 @@ getSensor	KEYWORD2
 getAngle	KEYWORD2
 get9DOFValues	KEYWORD2
 getStatus	KEYWORD2
-getStatusString	KEYWORD2
+printStatusString	KEYWORD2
 getTemperature	KEYWORD2
 disconnect	KEYWORD2
 
@@ -62,6 +64,7 @@ setRumbleOn	KEYWORD2
 setLedOff	KEYWORD2
 setLedOn	KEYWORD2
 setLedToggle	KEYWORD2
+setLedFlash	KEYWORD2
 moveSetBulb	KEYWORD2
 moveSetRumble	KEYWORD2
 
@@ -74,9 +77,19 @@ PS3NavigationConnected	KEYWORD2
 isReady	KEYWORD2
 watingForConnection	KEYWORD2
 
+isTouching	KEYWORD2
+getX	KEYWORD2
+getY	KEYWORD2
+getTouchCounter	KEYWORD2
+
+getUsbStatus    KEYWORD2
+getAudioStatus  KEYWORD2
+getMicStatus    KEYWORD2
+
 ####################################################
 # Constants and enums (LITERAL1)
 ####################################################
+OFF	LITERAL1
 LED1	LITERAL1
 LED2	LITERAL1
 LED3	LITERAL1
@@ -117,6 +130,10 @@ PS	LITERAL1
 MOVE	LITERAL1
 T	LITERAL1
 
+SHARE	LITERAL1
+OPTIONS	LITERAL1
+TOUCHPAD	LITERAL1
+
 LeftHatX	LITERAL1
 LeftHatY	LITERAL1
 RightHatX	LITERAL1
@@ -125,6 +142,8 @@ RightHatY	LITERAL1
 aX	LITERAL1
 aY	LITERAL1
 aZ	LITERAL1
+gX	LITERAL1
+gY	LITERAL1
 gZ	LITERAL1
 aXmove	LITERAL1
 aYmove	LITERAL1
@@ -299,7 +318,7 @@ getIRy4	KEYWORD2
 getIRs4	KEYWORD2
 
 ####################################################
-# Syntax Coloring Map For RFCOMM/SPP Library
+# Syntax Coloring Map For BTHID Library
 ####################################################
 
 ####################################################
@@ -312,4 +331,31 @@ BTHID	KEYWORD1
 # Methods and Functions (KEYWORD2)
 ####################################################
 SetReportParser	KEYWORD2
-setProtocolMode	KEYWORD2
\ No newline at end of file
+setProtocolMode	KEYWORD2
+
+####################################################
+# Syntax Coloring Map For PS Buzz Library
+####################################################
+
+####################################################
+# Datatypes (KEYWORD1)
+####################################################
+
+PSBuzz	KEYWORD1
+
+####################################################
+# Methods and Functions (KEYWORD2)
+####################################################
+
+setLedOnAll	KEYWORD2
+setLedOffAll	KEYWORD2
+
+####################################################
+# Constants and enums (LITERAL1)
+####################################################
+
+RED	LITERAL1
+YELLOW	LITERAL1
+GREEN	LITERAL1
+ORANGE	LITERAL1
+BLUE	LITERAL1
\ No newline at end of file
diff --git a/macros.h b/macros.h
index 8877f48e..51476830 100644
--- a/macros.h
+++ b/macros.h
@@ -1,8 +1,18 @@
-/*
- * File:   macros.h
- * Author: AJK
- *
- * Created on September 23, 2013, 12:31 AM
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
  */
 
 #if !defined(_usb_h_) || defined(MACROS_H)
@@ -14,6 +24,11 @@
 // HANDY MACROS
 ////////////////////////////////////////////////////////////////////////////////
 
+#define VALUE_BETWEEN(v,l,h) (((v)>(l)) && ((v)<(h)))
+#define VALUE_WITHIN(v,l,h) (((v)>=(l)) && ((v)<=(h)))
+#define output_pgm_message(wa,fp,mp,el) wa = &mp, fp((char *)pgm_read_pointer(wa), el)
+#define output_if_between(v,l,h,wa,fp,mp,el) if(VALUE_BETWEEN(v,l,h)) output_pgm_message(wa,fp,mp[v-(l+1)],el);
+
 #define SWAP(a, b) (((a) ^= (b)), ((b) ^= (a)), ((a) ^= (b)))
 #ifndef __BYTE_GRABBING_DEFINED__
 #define __BYTE_GRABBING_DEFINED__ 1
@@ -58,7 +73,9 @@
  * Debug macros: Strings are stored in progmem (flash) instead of RAM.
  */
 #define USBTRACE(s) (Notify(PSTR(s), 0x80))
+#define USBTRACE1(s,l) (Notify(PSTR(s), l))
 #define USBTRACE2(s,r) (Notify(PSTR(s), 0x80), D_PrintHex((r), 0x80), Notify(PSTR("\r\n"), 0x80))
+#define USBTRACE3(s,r,l) (Notify(PSTR(s), l), D_PrintHex((r), l), Notify(PSTR("\r\n"), l))
 
 
 #endif	/* MACROS_H */
diff --git a/masstorage.cpp b/masstorage.cpp
index c593ae22..ab9ad4ff 100644
--- a/masstorage.cpp
+++ b/masstorage.cpp
@@ -1,3 +1,20 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+
 #include "masstorage.h"
 
 const uint8_t BulkOnly::epDataInIndex = 1;
@@ -17,7 +34,7 @@ const uint8_t BulkOnly::epInterruptInIndex = 3;
  * @return media capacity
  */
 uint32_t BulkOnly::GetCapacity(uint8_t lun) {
-        if (LUNOk[lun])
+        if(LUNOk[lun])
                 return CurrentCapacity[lun];
         return 0LU;
 }
@@ -29,7 +46,7 @@ uint32_t BulkOnly::GetCapacity(uint8_t lun) {
  * @return media sector size
  */
 uint16_t BulkOnly::GetSectorSize(uint8_t lun) {
-        if (LUNOk[lun])
+        if(LUNOk[lun])
                 return CurrentSectorSize[lun];
         return 0U;
 }
@@ -115,7 +132,7 @@ uint8_t BulkOnly::MediaCTL(uint8_t lun, uint8_t ctl) {
         Notify(PSTR("-----------------\r\n"), 0x80);
 
         uint8_t rcode = MASS_ERR_UNIT_NOT_READY;
-        if (bAddress) {
+        if(bAddress) {
                 CDB6_t cdb = CDB6_t(SCSI_CMD_START_STOP_UNIT, lun, ctl & 0x03, 0);
                 rcode = SCSITransaction6(&cdb, (uint16_t)0, NULL, (uint8_t)MASS_CMD_DIR_OUT);
         } else {
@@ -135,7 +152,7 @@ uint8_t BulkOnly::MediaCTL(uint8_t lun, uint8_t ctl) {
  * @return 0 on success
  */
 uint8_t BulkOnly::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, uint8_t *buf) {
-        if (!LUNOk[lun]) return MASS_ERR_NO_MEDIA;
+        if(!LUNOk[lun]) return MASS_ERR_NO_MEDIA;
         Notify(PSTR("\r\nRead LUN:\t"), 0x80);
         D_PrintHex<uint8_t > (lun, 0x90);
         Notify(PSTR("\r\nLBA:\t\t"), 0x90);
@@ -150,10 +167,10 @@ uint8_t BulkOnly::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t block
 again:
         uint8_t er = SCSITransaction10(&cdb, ((uint16_t)bsize * blocks), buf, (uint8_t)MASS_CMD_DIR_IN);
 
-        if (er == MASS_ERR_STALL) {
+        if(er == MASS_ERR_STALL) {
                 MediaCTL(lun, 1);
                 delay(150);
-                if (!TestUnitReady(lun)) goto again;
+                if(!TestUnitReady(lun)) goto again;
         }
         return er;
 }
@@ -169,8 +186,8 @@ again:
  * @return 0 on success
  */
 uint8_t BulkOnly::Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, const uint8_t * buf) {
-        if (!LUNOk[lun]) return MASS_ERR_NO_MEDIA;
-        if (!WriteOk[lun]) return MASS_ERR_WRITE_PROTECTED;
+        if(!LUNOk[lun]) return MASS_ERR_NO_MEDIA;
+        if(!WriteOk[lun]) return MASS_ERR_WRITE_PROTECTED;
         Notify(PSTR("\r\nWrite LUN:\t"), 0x80);
         D_PrintHex<uint8_t > (lun, 0x90);
         Notify(PSTR("\r\nLBA:\t\t"), 0x90);
@@ -185,10 +202,10 @@ uint8_t BulkOnly::Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t bloc
 again:
         uint8_t er = SCSITransaction10(&cdb, ((uint16_t)bsize * blocks), (void*)buf, (uint8_t)MASS_CMD_DIR_OUT);
 
-        if (er == MASS_ERR_WRITE_STALL) {
+        if(er == MASS_ERR_WRITE_STALL) {
                 MediaCTL(lun, 1);
                 delay(150);
-                if (!TestUnitReady(lun)) goto again;
+                if(!TestUnitReady(lun)) goto again;
         }
         return er;
 }
@@ -213,7 +230,7 @@ bPollEnable(false),
 bLastUsbError(0) {
         ClearAllEP();
         dCBWTag = 0;
-        if (pUsb)
+        if(pUsb)
                 pUsb->RegisterDeviceClass(this);
 }
 
@@ -236,6 +253,7 @@ uint8_t BulkOnly::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         const uint8_t constBufSize = sizeof (USB_DEVICE_DESCRIPTOR);
 
         uint8_t buf[constBufSize];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -244,17 +262,17 @@ uint8_t BulkOnly::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         AddressPool &addrPool = pUsb->GetAddressPool();
 
 
-        if (bAddress)
+        if(bAddress)
                 return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
         // <TECHNICAL>
         // Get pointer to pseudo device with address 0 assigned
         p = addrPool.GetUsbDevicePtr(0);
-        if (!p) {
+        if(!p) {
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
         }
 
-        if (!p->epinfo) {
+        if(!p->epinfo) {
                 USBTRACE("epinfo\r\n");
                 return USB_ERROR_EPINFO_IS_NULL;
         }
@@ -272,19 +290,19 @@ uint8_t BulkOnly::ConfigureDevice(uint8_t parent, uint8_t port, bool lowspeed) {
         // Restore p->epinfo
         p->epinfo = oldep_ptr;
 
-        if (rcode) {
+        if(rcode) {
                 goto FailGetDevDescr;
         }
         // Allocate new address according to device class
         bAddress = addrPool.AllocAddress(parent, false, port);
 
-        if (!bAddress)
+        if(!bAddress)
                 return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
         // Extract Max Packet Size from the device descriptor
-        epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
         // Steal and abuse from epInfo structure to save on memory.
-        epInfo[1].epAddr = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;
+        epInfo[1].epAddr = udd->bNumConfigurations;
         // </TECHNICAL>
         return USB_ERROR_CONFIG_REQUIRES_ADDITIONAL_RESET;
 
@@ -294,7 +312,6 @@ FailGetDevDescr:
 #endif
         rcode = USB_ERROR_FailGetDevDescr;
 
-Fail:
         Release();
         return rcode;
 };
@@ -315,14 +332,14 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         AddressPool &addrPool = pUsb->GetAddressPool();
         UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         // Assign new address to the device
         delay(2000);
         rcode = pUsb->setAddr(0, 0, bAddress);
 
-        if (rcode) {
+        if(rcode) {
                 p->lowspeed = false;
                 addrPool.FreeAddress(bAddress);
                 bAddress = 0;
@@ -336,7 +353,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         p = addrPool.GetUsbDevicePtr(bAddress);
 
-        if (!p)
+        if(!p)
                 return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
         p->lowspeed = lowspeed;
@@ -344,12 +361,12 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Assign epInfo to epinfo pointer
         rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetDevTblEntry;
 
         USBTRACE2("NC:", num_of_conf);
 
-        for (uint8_t i = 0; i < num_of_conf; i++) {
+        for(uint8_t i = 0; i < num_of_conf; i++) {
                 ConfigDescParser< USB_CLASS_MASS_STORAGE,
                         MASS_SUBCLASS_SCSI,
                         MASS_PROTO_BBB,
@@ -359,14 +376,14 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
                 rcode = pUsb->getConfDescr(bAddress, 0, i, &BulkOnlyParser);
 
-                if (rcode)
+                if(rcode)
                         goto FailGetConfDescr;
 
-                if (bNumEP > 1)
+                if(bNumEP > 1)
                         break;
         }
 
-        if (bNumEP < 3)
+        if(bNumEP < 3)
                 return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
         // Assign epInfo to epinfo pointer
@@ -377,38 +394,38 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         // Set Configuration Value
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
-        if (rcode)
+        if(rcode)
                 goto FailSetConfDescr;
 
         //Linux does a 1sec delay after this.
         delay(1000);
 
         rcode = GetMaxLUN(&bMaxLUN);
-        if (rcode)
+        if(rcode)
                 goto FailGetMaxLUN;
 
-        if (bMaxLUN >= MASS_MAX_SUPPORTED_LUN) bMaxLUN = MASS_MAX_SUPPORTED_LUN - 1;
+        if(bMaxLUN >= MASS_MAX_SUPPORTED_LUN) bMaxLUN = MASS_MAX_SUPPORTED_LUN - 1;
         ErrorMessage<uint8_t > (PSTR("MaxLUN"), bMaxLUN);
 
         delay(1000); // Delay a bit for slow firmware.
 
-        for (uint8_t lun = 0; lun <= bMaxLUN; lun++) {
+        for(uint8_t lun = 0; lun <= bMaxLUN; lun++) {
                 InquiryResponse response;
                 rcode = Inquiry(lun, sizeof (InquiryResponse), (uint8_t*) & response);
-                if (rcode) {
+                if(rcode) {
                         ErrorMessage<uint8_t > (PSTR("Inquiry"), rcode);
                 } else {
 #if 0
                         printf("LUN %i `", lun);
                         uint8_t *buf = response.VendorID;
-                        for (int i = 0; i < 28; i++) printf("%c", buf[i]);
+                        for(int i = 0; i < 28; i++) printf("%c", buf[i]);
                         printf("'\r\nQualifier %1.1X ", response.PeripheralQualifier);
                         printf("Device type %2.2X ", response.DeviceType);
                         printf("RMB %1.1X ", response.Removable);
                         printf("SSCS %1.1X ", response.SCCS);
                         uint8_t sv = response.Version;
                         printf("SCSI version %2.2X\r\nDevice conforms to ", sv);
-                        switch (sv) {
+                        switch(sv) {
                                 case 0:
                                         printf("No specific");
                                         break;
@@ -436,20 +453,20 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                         printf(" standards.\r\n");
 #endif
                         uint8_t tries = 0xf0;
-                        while (rcode = TestUnitReady(lun)) {
-                                if (rcode == 0x08) break; // break on no media, this is OK to do.
+                        while((rcode = TestUnitReady(lun))) {
+                                if(rcode == 0x08) break; // break on no media, this is OK to do.
                                 // try to lock media and spin up
-                                if (tries < 14) {
+                                if(tries < 14) {
                                         LockMedia(lun, 1);
                                         MediaCTL(lun, 1); // I actually have a USB stick that needs this!
                                 } else delay(2 * (tries + 1));
                                 tries++;
-                                if (!tries) break;
+                                if(!tries) break;
                         }
-                        if (!rcode) {
+                        if(!rcode) {
                                 delay(1000);
                                 LUNOk[lun] = CheckLUN(lun);
-                                if (!LUNOk[lun]) LUNOk[lun] = CheckLUN(lun);
+                                if(!LUNOk[lun]) LUNOk[lun] = CheckLUN(lun);
                         }
                 }
         }
@@ -459,7 +476,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         rcode = OnInit();
 
-        if (rcode)
+        if(rcode)
                 goto FailOnInit;
 
 #ifdef DEBUG_USB_HOST
@@ -489,11 +506,11 @@ FailGetMaxLUN:
         goto Fail;
 #endif
 
-FailInvalidSectorSize:
-#ifdef DEBUG_USB_HOST
-        USBTRACE("Sector Size is NOT VALID: ");
-        goto Fail;
-#endif
+        //#ifdef DEBUG_USB_HOST
+        //FailInvalidSectorSize:
+        //        USBTRACE("Sector Size is NOT VALID: ");
+        //        goto Fail;
+        //#endif
 
 FailSetDevTblEntry:
 #ifdef DEBUG_USB_HOST
@@ -506,8 +523,8 @@ FailGetConfDescr:
         NotifyFailGetConfDescr();
 #endif
 
-Fail:
 #ifdef DEBUG_USB_HOST
+Fail:
         NotifyFail(rcode);
 #endif
         Release();
@@ -532,10 +549,24 @@ void BulkOnly::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t
 
         uint8_t index;
 
-        if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
+#if 1
+        if((pep->bmAttributes & 0x02) == 2) {
+                index = ((pep->bEndpointAddress & 0x80) == 0x80) ? epDataInIndex : epDataOutIndex;
+                // Fill in the endpoint info structure
+                epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F);
+                epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize;
+                epInfo[index].epAttribs = 0;
+
+                bNumEP++;
+
+                PrintEndpointDescriptor(pep);
+
+        }
+#else
+        if((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
                 index = epInterruptInIndex;
         else
-                if ((pep->bmAttributes & 0x02) == 2)
+                if((pep->bmAttributes & 0x02) == 2)
                 index = ((pep->bEndpointAddress & 0x80) == 0x80) ? epDataInIndex : epDataOutIndex;
         else
                 return;
@@ -548,6 +579,7 @@ void BulkOnly::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t
         bNumEP++;
 
         PrintEndpointDescriptor(pep);
+#endif
 }
 
 /**
@@ -570,35 +602,35 @@ uint8_t BulkOnly::Release() {
 boolean BulkOnly::CheckLUN(uint8_t lun) {
         uint8_t rcode;
         Capacity capacity;
-        for (uint8_t i = 0; i < 8; i++) capacity.data[i] = 0;
+        for(uint8_t i = 0; i < 8; i++) capacity.data[i] = 0;
 
         rcode = ReadCapacity10(lun, (uint8_t*)capacity.data);
-        if (rcode) {
+        if(rcode) {
                 //printf(">>>>>>>>>>>>>>>>ReadCapacity returned %i\r\n", rcode);
                 return false;
         }
         ErrorMessage<uint8_t > (PSTR(">>>>>>>>>>>>>>>>CAPACITY OK ON LUN"), lun);
-        for (uint8_t i = 0; i < 8 /*sizeof (Capacity)*/; i++)
+        for(uint8_t i = 0; i < 8 /*sizeof (Capacity)*/; i++)
                 D_PrintHex<uint8_t > (capacity.data[i], 0x80);
         Notify(PSTR("\r\n\r\n"), 0x80);
         // Only 512/1024/2048/4096 are valid values!
         uint32_t c = BMAKE32(capacity.data[4], capacity.data[5], capacity.data[6], capacity.data[7]);
-        if (c != 0x0200LU && c != 0x0400LU && c != 0x0800LU && c != 0x1000LU) {
+        if(c != 0x0200LU && c != 0x0400LU && c != 0x0800LU && c != 0x1000LU) {
                 return false;
         }
         // Store capacity information.
         CurrentSectorSize[lun] = (uint16_t)(c); // & 0xFFFF);
 
         CurrentCapacity[lun] = BMAKE32(capacity.data[0], capacity.data[1], capacity.data[2], capacity.data[3]) + 1;
-        if (CurrentCapacity[lun] == /*0xffffffffLU */ 0x01LU || CurrentCapacity[lun] == 0x00LU) {
+        if(CurrentCapacity[lun] == /*0xffffffffLU */ 0x01LU || CurrentCapacity[lun] == 0x00LU) {
                 // Buggy firmware will report 0xffffffff or 0 for no media
-                if (CurrentCapacity[lun])
+                if(CurrentCapacity[lun])
                         ErrorMessage<uint8_t > (PSTR(">>>>>>>>>>>>>>>>BUGGY FIRMWARE. CAPACITY FAIL ON LUN"), lun);
                 return false;
         }
         delay(20);
         Page3F(lun);
-        if (!TestUnitReady(lun)) return true;
+        if(!TestUnitReady(lun)) return true;
         return false;
 }
 
@@ -608,18 +640,18 @@ boolean BulkOnly::CheckLUN(uint8_t lun) {
  * Scan for media change on all LUNs
  */
 void BulkOnly::CheckMedia() {
-        for (uint8_t lun = 0; lun <= bMaxLUN; lun++) {
-                if (TestUnitReady(lun)) {
+        for(uint8_t lun = 0; lun <= bMaxLUN; lun++) {
+                if(TestUnitReady(lun)) {
                         LUNOk[lun] = false;
                         continue;
                 }
-                if (!LUNOk[lun])
+                if(!LUNOk[lun])
                         LUNOk[lun] = CheckLUN(lun);
         }
 #if 0
         printf("}}}}}}}}}}}}}}}}STATUS ");
-        for (uint8_t lun = 0; lun <= bMaxLUN; lun++) {
-                if (LUNOk[lun])
+        for(uint8_t lun = 0; lun <= bMaxLUN; lun++) {
+                if(LUNOk[lun])
                         printf("#");
                 else printf(".");
         }
@@ -636,10 +668,10 @@ void BulkOnly::CheckMedia() {
 uint8_t BulkOnly::Poll() {
         //uint8_t rcode = 0;
 
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
 
-        if (qNextPollTime <= millis()) {
+        if((long)(millis() - qNextPollTime) >= 0L) {
                 CheckMedia();
         }
         //rcode = 0;
@@ -664,7 +696,7 @@ uint8_t BulkOnly::Poll() {
 uint8_t BulkOnly::GetMaxLUN(uint8_t *plun) {
         uint8_t ret = pUsb->ctrlReq(bAddress, 0, bmREQ_MASSIN, MASS_REQ_GET_MAX_LUN, 0, 0, bIface, 1, 1, plun, NULL);
 
-        if (ret == hrSTALL)
+        if(ret == hrSTALL)
                 *plun = 0;
 
         return 0;
@@ -696,7 +728,7 @@ uint8_t BulkOnly::Inquiry(uint8_t lun, uint16_t bsize, uint8_t *buf) {
  */
 uint8_t BulkOnly::TestUnitReady(uint8_t lun) {
         //SetCurLUN(lun);
-        if (!bAddress)
+        if(!bAddress)
                 return MASS_ERR_UNIT_NOT_READY;
 
         Notify(PSTR("\r\nTestUnitReady\r\n"), 0x80);
@@ -752,15 +784,15 @@ uint8_t BulkOnly::ReadCapacity10(uint8_t lun, uint8_t *buf) {
  */
 uint8_t BulkOnly::Page3F(uint8_t lun) {
         uint8_t buf[192];
-        for (int i = 0; i < 192; i++) {
+        for(int i = 0; i < 192; i++) {
                 buf[i] = 0x00;
         }
         WriteOk[lun] = true;
         uint8_t rc = ModeSense6(lun, 0, 0x3f, 0, 192, buf);
-        if (!rc) {
+        if(!rc) {
                 WriteOk[lun] = ((buf[2] & 0x80) == 0);
                 Notify(PSTR("Mode Sense: "), 0x80);
-                for (int i = 0; i < 4; i++) {
+                for(int i = 0; i < 4; i++) {
                         D_PrintHex<uint8_t > (buf[i], 0x80);
                         Notify(PSTR(" "), 0x80);
                 }
@@ -803,16 +835,15 @@ uint8_t BulkOnly::RequestSense(uint8_t lun, uint16_t size, uint8_t *buf) {
  * @return
  */
 uint8_t BulkOnly::ClearEpHalt(uint8_t index) {
-        if (index == 0)
+        if(index == 0)
                 return 0;
 
         uint8_t ret = 0;
 
-        while (ret = (pUsb->ctrlReq(bAddress, 0, USB_SETUP_HOST_TO_DEVICE | USB_SETUP_TYPE_STANDARD | USB_SETUP_RECIPIENT_ENDPOINT,
-                USB_REQUEST_CLEAR_FEATURE, USB_FEATURE_ENDPOINT_HALT, 0, ((index == epDataInIndex) ? (0x80 | epInfo[index].epAddr) : epInfo[index].epAddr), 0, 0, NULL, NULL))
-                == 0x01) delay(6);
+        while((ret = (pUsb->ctrlReq(bAddress, 0, USB_SETUP_HOST_TO_DEVICE | USB_SETUP_TYPE_STANDARD | USB_SETUP_RECIPIENT_ENDPOINT, USB_REQUEST_CLEAR_FEATURE, USB_FEATURE_ENDPOINT_HALT, 0, ((index == epDataInIndex) ? (0x80 | epInfo[index].epAddr) : epInfo[index].epAddr), 0, 0, NULL, NULL)) == 0x01))
+                delay(6);
 
-        if (ret) {
+        if(ret) {
                 ErrorMessage<uint8_t > (PSTR("ClearEpHalt"), ret);
                 ErrorMessage<uint8_t > (PSTR("EP"), ((index == epDataInIndex) ? (0x80 | epInfo[index].epAddr) : epInfo[index].epAddr));
                 return ret;
@@ -828,7 +859,7 @@ uint8_t BulkOnly::ClearEpHalt(uint8_t index) {
  *
  */
 void BulkOnly::Reset() {
-        while (pUsb->ctrlReq(bAddress, 0, bmREQ_MASSOUT, MASS_REQ_BOMSR, 0, 0, bIface, 0, 0, NULL, NULL) == 0x01) delay(6);
+        while(pUsb->ctrlReq(bAddress, 0, bmREQ_MASSOUT, MASS_REQ_BOMSR, 0, 0, bIface, 0, 0, NULL, NULL) == 0x01) delay(6);
 }
 
 /**
@@ -856,7 +887,7 @@ uint8_t BulkOnly::ResetRecovery() {
  * Clear all EP data and clear all LUN status
  */
 void BulkOnly::ClearAllEP() {
-        for (uint8_t i = 0; i < MASS_MAX_ENDPOINTS; i++) {
+        for(uint8_t i = 0; i < MASS_MAX_ENDPOINTS; i++) {
                 epInfo[i].epAddr = 0;
                 epInfo[i].maxPktSize = (i) ? 0 : 8;
                 epInfo[i].epAttribs = 0;
@@ -864,7 +895,7 @@ void BulkOnly::ClearAllEP() {
                 epInfo[i].bmNakPower = USB_NAK_DEFAULT;
         }
 
-        for (uint8_t i = 0; i < MASS_MAX_SUPPORTED_LUN; i++) {
+        for(uint8_t i = 0; i < MASS_MAX_SUPPORTED_LUN; i++) {
                 LUNOk[i] = false;
                 WriteOk[i] = false;
                 CurrentCapacity[i] = 0lu;
@@ -889,11 +920,11 @@ void BulkOnly::ClearAllEP() {
  * @return
  */
 bool BulkOnly::IsValidCSW(CommandStatusWrapper *pcsw, CommandBlockWrapperBase *pcbw) {
-        if (pcsw->dCSWSignature != MASS_CSW_SIGNATURE) {
+        if(pcsw->dCSWSignature != MASS_CSW_SIGNATURE) {
                 Notify(PSTR("CSW:Sig error\r\n"), 0x80);
                 return false;
         }
-        if (pcsw->dCSWTag != pcbw->dCBWTag) {
+        if(pcsw->dCSWTag != pcbw->dCBWTag) {
                 Notify(PSTR("CSW:Wrong tag\r\n"), 0x80);
                 return false;
         }
@@ -913,12 +944,12 @@ uint8_t BulkOnly::HandleUsbError(uint8_t error, uint8_t index) {
         bLastUsbError = error;
         //if (error)
         //ClearEpHalt(index);
-        while (error && count) {
-                if (error != hrSUCCESS) {
+        while(error && count) {
+                if(error != hrSUCCESS) {
                         ErrorMessage<uint8_t > (PSTR("USB Error"), error);
                         ErrorMessage<uint8_t > (PSTR("Index"), index);
                 }
-                switch (error) {
+                switch(error) {
                                 // case hrWRONGPID:
                         case hrSUCCESS:
                                 return MASS_ERR_SUCCESS;
@@ -928,15 +959,15 @@ uint8_t BulkOnly::HandleUsbError(uint8_t error, uint8_t index) {
                         case hrTIMEOUT:
                         case hrJERR: return MASS_ERR_DEVICE_DISCONNECTED;
                         case hrSTALL:
-                                if (index == 0)
+                                if(index == 0)
                                         return MASS_ERR_STALL;
                                 ClearEpHalt(index);
-                                if (index != epDataInIndex)
+                                if(index != epDataInIndex)
                                         return MASS_ERR_WRITE_STALL;
                                 return MASS_ERR_STALL;
 
                         case hrNAK:
-                                if (index == 0)
+                                if(index == 0)
                                         return MASS_ERR_UNIT_BUSY;
                                 return MASS_ERR_UNIT_BUSY;
 
@@ -945,10 +976,10 @@ uint8_t BulkOnly::HandleUsbError(uint8_t error, uint8_t index) {
                                 // I have only ran into one device that has this firmware bug, and this is
                                 // the only clean way to get back into sync with the buggy device firmware.
                                 //   --AJK
-                                if (bAddress && bConfNum) {
+                                if(bAddress && bConfNum) {
                                         error = pUsb->setConf(bAddress, 0, bConfNum);
 
-                                        if (error)
+                                        if(error)
                                                 break;
                                 }
                                 return MASS_ERR_SUCCESS;
@@ -1000,33 +1031,33 @@ uint8_t BulkOnly::Transaction(CommandBlockWrapper *pcbw, uint16_t buf_size, void
         SetCurLUN(pcbw->bmCBWLUN);
         ErrorMessage<uint32_t > (PSTR("CBW.dCBWTag"), pcbw->dCBWTag);
 
-        while ((usberr = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, sizeof (CommandBlockWrapper), (uint8_t*)pcbw)) == hrBUSY) delay(1);
+        while((usberr = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, sizeof (CommandBlockWrapper), (uint8_t*)pcbw)) == hrBUSY) delay(1);
 
         ret = HandleUsbError(usberr, epDataOutIndex);
         //ret = HandleUsbError(pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, sizeof (CommandBlockWrapper), (uint8_t*)pcbw), epDataOutIndex);
-        if (ret) {
+        if(ret) {
                 ErrorMessage<uint8_t > (PSTR("============================ CBW"), ret);
         } else {
-                if (bytes) {
-                        if (!write) {
+                if(bytes) {
+                        if(!write) {
 #if MS_WANT_PARSER
-                                if (callback) {
+                                if(callback) {
                                         uint8_t rbuf[bytes];
-                                        while ((usberr = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &bytes, rbuf)) == hrBUSY) delay(1);
-                                        if (usberr == hrSUCCESS) ((USBReadParser*)buf)->Parse(bytes, rbuf, 0);
+                                        while((usberr = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &bytes, rbuf)) == hrBUSY) delay(1);
+                                        if(usberr == hrSUCCESS) ((USBReadParser*)buf)->Parse(bytes, rbuf, 0);
                                 } else {
 #endif
-                                        while ((usberr = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &bytes, (uint8_t*)buf)) == hrBUSY) delay(1);
+                                        while((usberr = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &bytes, (uint8_t*)buf)) == hrBUSY) delay(1);
 #if MS_WANT_PARSER
 
                                 }
 #endif
                                 ret = HandleUsbError(usberr, epDataInIndex);
                         } else {
-                                while ((usberr = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, bytes, (uint8_t*)buf)) == hrBUSY) delay(1);
+                                while((usberr = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, bytes, (uint8_t*)buf)) == hrBUSY) delay(1);
                                 ret = HandleUsbError(usberr, epDataOutIndex);
                         }
-                        if (ret) {
+                        if(ret) {
                                 ErrorMessage<uint8_t > (PSTR("============================ DAT"), ret);
                         }
                 }
@@ -1035,13 +1066,13 @@ uint8_t BulkOnly::Transaction(CommandBlockWrapper *pcbw, uint16_t buf_size, void
         {
                 bytes = sizeof (CommandStatusWrapper);
                 int tries = 2;
-                while (tries--) {
-                        while ((usberr = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &bytes, (uint8_t*) & csw)) == hrBUSY) delay(1);
-                        if (!usberr) break;
+                while(tries--) {
+                        while((usberr = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &bytes, (uint8_t*) & csw)) == hrBUSY) delay(1);
+                        if(!usberr) break;
                         ClearEpHalt(epDataInIndex);
-                        if (tries) ResetRecovery();
+                        if(tries) ResetRecovery();
                 }
-                if (!ret) {
+                if(!ret) {
                         Notify(PSTR("CBW:\t\tOK\r\n"), 0x80);
                         Notify(PSTR("Data Stage:\tOK\r\n"), 0x80);
                 } else {
@@ -1050,11 +1081,11 @@ uint8_t BulkOnly::Transaction(CommandBlockWrapper *pcbw, uint16_t buf_size, void
                         return ret;
                 }
                 ret = HandleUsbError(usberr, epDataInIndex);
-                if (ret) {
+                if(ret) {
                         ErrorMessage<uint8_t > (PSTR("============================ CSW"), ret);
                 }
-                if (usberr == hrSUCCESS) {
-                        if (IsValidCSW(&csw, pcbw)) {
+                if(usberr == hrSUCCESS) {
+                        if(IsValidCSW(&csw, pcbw)) {
                                 //ErrorMessage<uint32_t > (PSTR("CSW.dCBWTag"), csw.dCSWTag);
                                 //ErrorMessage<uint8_t > (PSTR("bCSWStatus"), csw.bCSWStatus);
                                 //ErrorMessage<uint32_t > (PSTR("dCSWDataResidue"), csw.dCSWDataResidue);
@@ -1083,7 +1114,7 @@ uint8_t BulkOnly::Transaction(CommandBlockWrapper *pcbw, uint16_t buf_size, void
  * @return
  */
 uint8_t BulkOnly::SetCurLUN(uint8_t lun) {
-        if (lun > bMaxLUN)
+        if(lun > bMaxLUN)
                 return MASS_ERR_INVALID_LUN;
         bTheLUN = lun;
         return MASS_ERR_SUCCESS;
@@ -1098,7 +1129,7 @@ uint8_t BulkOnly::SetCurLUN(uint8_t lun) {
 uint8_t BulkOnly::HandleSCSIError(uint8_t status) {
         uint8_t ret = 0;
 
-        switch (status) {
+        switch(status) {
                 case 0: return MASS_ERR_SUCCESS;
 
                 case 2:
@@ -1114,13 +1145,13 @@ uint8_t BulkOnly::HandleSCSIError(uint8_t status) {
 
                         ret = RequestSense(bTheLUN, sizeof (RequestSenseResponce), (uint8_t*) & rsp);
 
-                        if (ret) {
+                        if(ret) {
                                 return MASS_ERR_GENERAL_SCSI_ERROR;
                         }
                         ErrorMessage<uint8_t > (PSTR("Response Code"), rsp.bResponseCode);
-                        if (rsp.bResponseCode & 0x80) {
+                        if(rsp.bResponseCode & 0x80) {
                                 Notify(PSTR("Information field: "), 0x80);
-                                for (int i = 0; i < 4; i++) {
+                                for(int i = 0; i < 4; i++) {
                                         D_PrintHex<uint8_t > (rsp.CmdSpecificInformation[i], 0x80);
                                         Notify(PSTR(" "), 0x80);
                                 }
@@ -1130,23 +1161,23 @@ uint8_t BulkOnly::HandleSCSIError(uint8_t status) {
                         ErrorMessage<uint8_t > (PSTR("Add Sense Code"), rsp.bAdditionalSenseCode);
                         ErrorMessage<uint8_t > (PSTR("Add Sense Qual"), rsp.bAdditionalSenseQualifier);
                         // warning, this is not testing ASQ, only SK and ASC.
-                        switch (rsp.bmSenseKey) {
+                        switch(rsp.bmSenseKey) {
                                 case SCSI_S_UNIT_ATTENTION:
-                                        switch (rsp.bAdditionalSenseCode) {
+                                        switch(rsp.bAdditionalSenseCode) {
                                                 case SCSI_ASC_MEDIA_CHANGED:
                                                         return MASS_ERR_MEDIA_CHANGED;
                                                 default:
                                                         return MASS_ERR_UNIT_NOT_READY;
                                         }
                                 case SCSI_S_NOT_READY:
-                                        switch (rsp.bAdditionalSenseCode) {
+                                        switch(rsp.bAdditionalSenseCode) {
                                                 case SCSI_ASC_MEDIUM_NOT_PRESENT:
                                                         return MASS_ERR_NO_MEDIA;
                                                 default:
                                                         return MASS_ERR_UNIT_NOT_READY;
                                         }
                                 case SCSI_S_ILLEGAL_REQUEST:
-                                        switch (rsp.bAdditionalSenseCode) {
+                                        switch(rsp.bAdditionalSenseCode) {
                                                 case SCSI_ASC_LBA_OUT_OF_RANGE:
                                                         return MASS_ERR_BAD_LBA;
                                                 default:
@@ -1208,7 +1239,7 @@ void BulkOnly::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR * ep_ptr) {
 /* We won't be needing this... */
 uint8_t BulkOnly::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, USBReadParser * prs) {
 #if MS_WANT_PARSER
-        if (!LUNOk[lun]) return MASS_ERR_NO_MEDIA;
+        if(!LUNOk[lun]) return MASS_ERR_NO_MEDIA;
         Notify(PSTR("\r\nRead (With parser)\r\n"), 0x80);
         Notify(PSTR("---------\r\n"), 0x80);
 
diff --git a/masstorage.h b/masstorage.h
index 12a61ec9..176752fc 100644
--- a/masstorage.h
+++ b/masstorage.h
@@ -1,3 +1,20 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+
 #if !defined(__MASSTORAGE_H__)
 #define __MASSTORAGE_H__
 
@@ -394,9 +411,9 @@ public:
         bmCBWLUN(lu), bmReserved1(0), bmCBWCBLength(cmdlen), bmReserved2(0) {
                 for(int i = 0; i < 16; i++) CBWCB[i] = 0;
                 // Type punning can cause optimization problems and bugs.
-                // Using reinterpret_cast to a different object is the proper way to do this.
+                // Using reinterpret_cast to a dreinterpretifferent object is the proper way to do this.
                 //(((BASICCDB_t *) CBWCB)->LUN) = cmd;
-                BASICCDB_t *x = reinterpret_cast<BASICCDB_t *> (CBWCB);
+                BASICCDB_t *x = reinterpret_cast<BASICCDB_t *>(CBWCB);
                 x->LUN = cmd;
         }
 
@@ -516,7 +533,7 @@ public:
         virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep);
 
         virtual boolean DEVCLASSOK(uint8_t klass) {
-                return(klass == USB_CLASS_MASS_STORAGE);
+                return (klass == USB_CLASS_MASS_STORAGE);
         }
 
         uint8_t SCSITransaction6(CDB6_t *cdb, uint16_t buf_size, void *buf, uint8_t dir);
diff --git a/max_LCD.cpp b/max_LCD.cpp
index 71c2247a..db61ea3b 100644
--- a/max_LCD.cpp
+++ b/max_LCD.cpp
@@ -52,14 +52,14 @@ void Max_LCD::init() {
 }
 
 void Max_LCD::begin(uint8_t cols, uint8_t lines, uint8_t dotsize) {
-        if (lines > 1) {
+        if(lines > 1) {
                 _displayfunction |= LCD_2LINE;
         }
         _numlines = lines;
         _currline = 0;
 
         // for some 1 line displays you can select a 10 pixel high font
-        if ((dotsize != 0) && (lines == 1)) {
+        if((dotsize != 0) && (lines == 1)) {
                 _displayfunction |= LCD_5x10DOTS;
         }
 
@@ -123,7 +123,7 @@ void Max_LCD::home() {
 
 void Max_LCD::setCursor(uint8_t col, uint8_t row) {
         int row_offsets[] = {0x00, 0x40, 0x14, 0x54};
-        if (row > _numlines) {
+        if(row > _numlines) {
                 row = _numlines - 1; // we count rows starting w/0
         }
 
@@ -211,7 +211,7 @@ void Max_LCD::noAutoscroll(void) {
 void Max_LCD::createChar(uint8_t location, uint8_t charmap[]) {
         location &= 0x7; // we only have 8 locations 0-7
         command(LCD_SETCGRAMADDR | (location << 3));
-        for (int i = 0; i < 8; i++) {
+        for(int i = 0; i < 8; i++) {
                 write(charmap[i]);
         }
 }
@@ -223,9 +223,18 @@ inline void Max_LCD::command(uint8_t value) {
         delayMicroseconds(100);
 }
 
+#if defined(ARDUINO) && ARDUINO >=100
+
+inline size_t Max_LCD::write(uint8_t value) {
+        LCD_sendchar(value);
+        return 1; // Assume success
+}
+#else
+
 inline void Max_LCD::write(uint8_t value) {
         LCD_sendchar(value);
 }
+#endif
 
 void Max_LCD::sendbyte(uint8_t val) {
         lcdPins &= 0x0f; //prepare place for the upper nibble
diff --git a/max_LCD.h b/max_LCD.h
index be9f2615..48946a1f 100644
--- a/max_LCD.h
+++ b/max_LCD.h
@@ -62,8 +62,7 @@ e-mail   :  support@circuitsathome.com
 #define LCD_5x10DOTS			0x04
 #define LCD_5x8DOTS				0x00
 
-class Max_LCD //: public Print
-{
+class Max_LCD : public Print {
         USB *pUsb;
 
 public:
@@ -86,9 +85,15 @@ public:
         void noAutoscroll();
         void createChar(uint8_t, uint8_t[]);
         void setCursor(uint8_t, uint8_t);
-        virtual void write(uint8_t);
         void command(uint8_t);
 
+#if defined(ARDUINO) && ARDUINO >=100
+        virtual size_t write(uint8_t);
+        using Print::write;
+#else
+        virtual void write(uint8_t);
+#endif
+
 private:
         void sendbyte(uint8_t val);
         uint8_t _displayfunction; //tokill
@@ -98,4 +103,4 @@ private:
         uint8_t _numlines, _currline;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/message.cpp b/message.cpp
index 1ac73f48..960a9fb4 100644
--- a/message.cpp
+++ b/message.cpp
@@ -18,11 +18,11 @@ e-mail   :  support@circuitsathome.com
 #include "Usb.h"
 // 0x80 is the default (i.e. trace) to turn off set this global to something lower.
 // this allows for 126 other debugging levels.
-// TO-DO: Allow assignment to a different serial port
+// TO-DO: Allow assignment to a different serial port by software
 int UsbDEBUGlvl = 0x80;
 
 void E_Notifyc(char c, int lvl) {
-        if (UsbDEBUGlvl < lvl) return;
+        if(UsbDEBUGlvl < lvl) return;
 #if defined(ARDUINO) && ARDUINO >=100
         USB_HOST_SERIAL.print(c);
 #else
@@ -32,23 +32,23 @@ void E_Notifyc(char c, int lvl) {
 }
 
 void E_Notify(char const * msg, int lvl) {
-        if (UsbDEBUGlvl < lvl) return;
-        if (!msg) return;
+        if(UsbDEBUGlvl < lvl) return;
+        if(!msg) return;
         char c;
 
-        while ((c = pgm_read_byte(msg++))) E_Notifyc(c, lvl);
+        while((c = pgm_read_byte(msg++))) E_Notifyc(c, lvl);
 }
 
 void E_NotifyStr(char const * msg, int lvl) {
-        if (UsbDEBUGlvl < lvl) return;
-        if (!msg) return;
+        if(UsbDEBUGlvl < lvl) return;
+        if(!msg) return;
         char c;
 
-        while (c = *msg++) E_Notifyc(c, lvl);
+        while((c = *msg++)) E_Notifyc(c, lvl);
 }
 
 void E_Notify(uint8_t b, int lvl) {
-        if (UsbDEBUGlvl < lvl) return;
+        if(UsbDEBUGlvl < lvl) return;
 #if defined(ARDUINO) && ARDUINO >=100
         USB_HOST_SERIAL.print(b);
 #else
@@ -58,12 +58,13 @@ void E_Notify(uint8_t b, int lvl) {
 }
 
 void E_Notify(double d, int lvl) {
-        if (UsbDEBUGlvl < lvl) return;
+        if(UsbDEBUGlvl < lvl) return;
         USB_HOST_SERIAL.print(d);
         //USB_HOST_SERIAL.flush();
 }
 
 #ifdef DEBUG_USB_HOST
+
 void NotifyFailGetDevDescr(void) {
         Notify(PSTR("\r\ngetDevDescr "), 0x80);
 }
@@ -71,6 +72,7 @@ void NotifyFailGetDevDescr(void) {
 void NotifyFailSetDevTblEntry(void) {
         Notify(PSTR("\r\nsetDevTblEn "), 0x80);
 }
+
 void NotifyFailGetConfDescr(void) {
         Notify(PSTR("\r\ngetConf "), 0x80);
 }
diff --git a/parsetools.cpp b/parsetools.cpp
index 0918abef..02a7ee27 100644
--- a/parsetools.cpp
+++ b/parsetools.cpp
@@ -17,14 +17,14 @@ e-mail   :  support@circuitsathome.com
 #include "Usb.h"
 
 bool MultiByteValueParser::Parse(uint8_t **pp, uint16_t *pcntdn) {
-        if (!pBuf) {
+        if(!pBuf) {
                 Notify(PSTR("Buffer pointer is NULL!\r\n"), 0x80);
                 return false;
         }
-        for (; countDown && (*pcntdn); countDown--, (*pcntdn)--, (*pp)++)
+        for(; countDown && (*pcntdn); countDown--, (*pcntdn)--, (*pp)++)
                 pBuf[valueSize - countDown] = (**pp);
 
-        if (countDown)
+        if(countDown)
                 return false;
 
         countDown = valueSize;
@@ -32,14 +32,14 @@ bool MultiByteValueParser::Parse(uint8_t **pp, uint16_t *pcntdn) {
 }
 
 bool PTPListParser::Parse(uint8_t **pp, uint16_t *pcntdn, PTP_ARRAY_EL_FUNC pf, const void *me) {
-        switch (nStage) {
+        switch(nStage) {
                 case 0:
                         pBuf->valueSize = lenSize;
                         theParser.Initialize(pBuf);
                         nStage = 1;
 
                 case 1:
-                        if (!theParser.Parse(pp, pcntdn))
+                        if(!theParser.Parse(pp, pcntdn))
                                 return false;
 
                         arLen = 0;
@@ -53,11 +53,11 @@ bool PTPListParser::Parse(uint8_t **pp, uint16_t *pcntdn, PTP_ARRAY_EL_FUNC pf,
                         nStage = 3;
 
                 case 3:
-                        for (; arLenCntdn; arLenCntdn--) {
-                                if (!theParser.Parse(pp, pcntdn))
+                        for(; arLenCntdn; arLenCntdn--) {
+                                if(!theParser.Parse(pp, pcntdn))
                                         return false;
 
-                                if (pf)
+                                if(pf)
                                         pf(pBuf, (arLen - arLenCntdn), me);
                         }
 
diff --git a/parsetools.h b/parsetools.h
index ab83c724..0e77de31 100644
--- a/parsetools.h
+++ b/parsetools.h
@@ -40,7 +40,7 @@ public:
         };
 
         void Initialize(MultiValueBuffer * const pbuf) {
-                pBuf = (uint8_t*) pbuf->pValue;
+                pBuf = (uint8_t*)pbuf->pValue;
                 countDown = valueSize = pbuf->valueSize;
         };
 
@@ -58,7 +58,7 @@ public:
         };
 
         void Initialize(MultiValueBuffer *pbuf) {
-                pBuf = (uint8_t*) pbuf->pValue;
+                pBuf = (uint8_t*)pbuf->pValue;
                 countDown = 0;
         };
 
@@ -73,7 +73,7 @@ public:
                                 if(!countDown)
                                         nStage = 0;
                 };
-                return(!countDown);
+                return (!countDown);
         };
 };
 
diff --git a/printhex.h b/printhex.h
index ed495ce9..369d7e1f 100644
--- a/printhex.h
+++ b/printhex.h
@@ -24,7 +24,7 @@ void E_Notifyc(char c, int lvl);
 
 template <class T>
 void PrintHex(T val, int lvl) {
-        int num_nibbles = sizeof(T) * 2;
+        int num_nibbles = sizeof (T) * 2;
 
         do {
                 char v = 48 + (((val >> (num_nibbles - 1) * 4)) & 0x0f);
@@ -35,7 +35,7 @@ void PrintHex(T val, int lvl) {
 
 template <class T>
 void PrintBin(T val, int lvl) {
-        for(T mask = (((T) 1) << ((sizeof(T) << 3) - 1)); mask; mask >>= 1)
+        for(T mask = (((T)1) << ((sizeof (T) << 3) - 1)); mask; mask >>= 1)
                 if(val & mask)
                         E_Notifyc('1', lvl);
                 else
@@ -44,7 +44,7 @@ void PrintBin(T val, int lvl) {
 
 template <class T>
 void SerialPrintHex(T val) {
-        int num_nibbles = sizeof(T) * 2;
+        int num_nibbles = sizeof (T) * 2;
 
         do {
                 char v = 48 + (((val >> (num_nibbles - 1) * 4)) & 0x0f);
@@ -55,7 +55,7 @@ void SerialPrintHex(T val) {
 
 template <class T>
 void PrintHex2(Print *prn, T val) {
-        T mask = (((T) 1) << (((sizeof(T) << 1) - 1) << 2));
+        T mask = (((T)1) << (((sizeof (T) << 1) - 1) << 2));
 
         while(mask > 1) {
                 if(val < mask)
@@ -63,19 +63,19 @@ void PrintHex2(Print *prn, T val) {
 
                 mask >>= 4;
         }
-        prn->print((T) val, HEX);
+        prn->print((T)val, HEX);
 }
 
 template <class T> void D_PrintHex(T val, int lvl) {
 #ifdef DEBUG_USB_HOST
-        PrintHex<T>(val, lvl);
+        PrintHex<T > (val, lvl);
 #endif
 }
 
 template <class T>
 void D_PrintBin(T val, int lvl) {
 #ifdef DEBUG_USB_HOST
-        PrintBin<T>(val, lvl);
+        PrintBin<T > (val, lvl);
 #endif
 }
 
diff --git a/settings.h b/settings.h
index 83d818b1..3ab12541 100644
--- a/settings.h
+++ b/settings.h
@@ -1,8 +1,18 @@
-/*
- * File:   settings.h
- * Author: xxxajk
- *
- * Created on September 23, 2013, 12:00 AM
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
  */
 
 #ifndef USB_HOST_SHIELD_SETTINGS_H
@@ -37,6 +47,13 @@
 /* Set this to a one to use the xmem2 lock. This is needed for multitasking and threading */
 #define USE_XMEM_SPI_LOCK 0
 
+////////////////////////////////////////////////////////////////////////////////
+// Wii IR camera
+////////////////////////////////////////////////////////////////////////////////
+
+/* Set this to 1 to activate code for the Wii IR camera */
+#define ENABLE_WII_IR_CAMERA 0
+
 ////////////////////////////////////////////////////////////////////////////////
 // MASS STORAGE
 ////////////////////////////////////////////////////////////////////////////////
@@ -47,7 +64,6 @@
 #define MASS_MAX_SUPPORTED_LUN 8
 #endif
 
-
 ////////////////////////////////////////////////////////////////////////////////
 // Set to 1 to use the faster spi4teensy3 driver.
 ////////////////////////////////////////////////////////////////////////////////
@@ -62,32 +78,67 @@
 // No user serviceable parts below this line.
 // DO NOT change anything below here unless you are a developer!
 
-#if !defined(DEBUG_USB_HOST) && ENABLE_UHS_DEBUGGING
-#define DEBUG_USB_HOST
-#endif
-
-// When will we drop support for the older bug-ridden stuff?
 #if defined(ARDUINO) && ARDUINO >=100
 #include <Arduino.h>
 #else
 #include <WProgram.h>
-// I am not sure what WProgram.h does not include, so these are here. --xxxajk
 #include <pins_arduino.h>
 #include <avr/pgmspace.h>
 #include <avr/io.h>
+#define F(str) (str)
 #endif
 
-#if USE_XMEM_SPI_LOCK | defined(USE_MULTIPLE_APP_API)
+#if defined(__GNUC__) && defined(__AVR__)
+#ifndef GCC_VERSION
+#define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__)
+#endif
+#if GCC_VERSION < 40602 // Test for GCC < 4.6.2
+#ifdef PROGMEM
+#undef PROGMEM
+#define PROGMEM __attribute__((section(".progmem.data"))) // Workaround for http://gcc.gnu.org/bugzilla/show_bug.cgi?id=34734#c4
+#ifdef PSTR
+#undef PSTR
+#define PSTR(s) (__extension__({static const char __c[] PROGMEM = (s); &__c[0];})) // Copied from pgmspace.h in avr-libc source
+#endif
+#endif
+#endif
+#endif
+
+#if !defined(DEBUG_USB_HOST) && ENABLE_UHS_DEBUGGING
+#define DEBUG_USB_HOST
+#endif
+
+#if !defined(WIICAMERA) && ENABLE_WII_IR_CAMERA
+#define WIICAMERA
+#endif
+
+// To use some other locking (e.g. freertos),
+// define XMEM_ACQUIRE_SPI and XMEM_RELEASE_SPI to point to your lock and unlock.
+// NOTE: NO argument is passed. You have to do this within your routine for
+// whatever you are using to lock and unlock.
+#if !defined(XMEM_ACQUIRE_SPI)
+#if USE_XMEM_SPI_LOCK || defined(USE_MULTIPLE_APP_API)
 #include <xmem.h>
 #else
 #define XMEM_ACQUIRE_SPI() (void(0))
 #define XMEM_RELEASE_SPI() (void(0))
 #endif
+#endif
 
-#ifdef __MK20DX128__
+#if !defined(EXT_RAM) && defined(EXT_RAM_STACK) || defined(EXT_RAM_HEAP)
+#include <xmem.h>
+#else
+#define EXT_RAM 0
+#endif
+
+#if defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__))
 #define USING_SPI4TEENSY3 USE_SPI4TEENSY3
 #else
 #define USING_SPI4TEENSY3 0
 #endif
 
+#if defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+#include <SPI.h> // Use the Arduino SPI library for the Arduino Due
+#endif
+
 #endif	/* SETTINGS_H */
diff --git a/sink_parser.h b/sink_parser.h
new file mode 100644
index 00000000..a23637d2
--- /dev/null
+++ b/sink_parser.h
@@ -0,0 +1,41 @@
+/* Copyright (C) 2011 Circuits At Home, LTD. All rights reserved.
+
+This software may be distributed and modified under the terms of the GNU
+General Public License version 2 (GPL2) as published by the Free Software
+Foundation and appearing in the file GPL2.TXT included in the packaging of
+this file. Please note that GPL2 Section 2[b] requires that all works based
+on this software must also be made publicly available under the terms of
+the GPL2 ("Copyleft").
+
+Contact information
+-------------------
+
+Circuits At Home, LTD
+Web      :  http://www.circuitsathome.com
+e-mail   :  support@circuitsathome.com
+ */
+#if !defined(_usb_h_) || defined(__SINK_PARSER_H__)
+#error "Never include hexdump.h directly; include Usb.h instead"
+#else
+#define __SINK_PARSER_H__
+
+extern int UsbDEBUGlvl;
+
+// This parser does absolutely nothing with the data, just swallows it.
+
+template <class BASE_CLASS, class LEN_TYPE, class OFFSET_TYPE>
+class SinkParser : public BASE_CLASS {
+public:
+
+        SinkParser() {
+        };
+
+        void Initialize() {
+        };
+
+        void Parse(const LEN_TYPE len, const uint8_t *pbuf, const OFFSET_TYPE &offset) {
+        };
+};
+
+
+#endif // __HEXDUMP_H__
diff --git a/usbhost.h b/usbhost.h
index bc65e106..cf26c1f0 100644
--- a/usbhost.h
+++ b/usbhost.h
@@ -28,10 +28,9 @@ e-mail   :  support@circuitsathome.com
 #endif
 
 /* SPI initialization */
-template< typename CLK, typename MOSI, typename MISO, typename SPI_SS > class SPi {
-#if USING_SPI4TEENSY3
+template< typename SPI_CLK, typename SPI_MOSI, typename SPI_MISO, typename SPI_SS > class SPi {
 public:
-
+#if USING_SPI4TEENSY3
         static void init() {
                 // spi4teensy3 inits everything for us, except /SS
                 // CLK, MOSI and MISO are hard coded for now.
@@ -40,15 +39,19 @@ public:
                 SPI_SS::SetDirWrite();
                 SPI_SS::Set();
         }
-
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+        static void init() {
+                SPI_SS::SetDirWrite();
+                SPI_SS::Set();
+                SPI.begin();
+                SPI.setClockDivider(4); // Set speed to 84MHz/4=21MHz - the MAX3421E can handle up to 26MHz
+        }
 #else
-public:
-
         static void init() {
                 //uint8_t tmp;
-                CLK::SetDirWrite();
-                MOSI::SetDirWrite();
-                MISO::SetDirRead();
+                SPI_CLK::SetDirWrite();
+                SPI_MOSI::SetDirWrite();
+                SPI_MISO::SetDirRead();
                 SPI_SS::SetDirWrite();
                 /* mode 00 (CPOL=0, CPHA=0) master, fclk/2. Mode 11 (CPOL=11, CPHA=11) is also supported by MAX3421E */
                 SPCR = 0x50;
@@ -67,8 +70,10 @@ typedef SPi< Pb1, Pb2, Pb3, Pb0 > spi;
 typedef SPi< Pb5, Pb3, Pb4, Pb2 > spi;
 #elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)
 typedef SPi< Pb7, Pb5, Pb6, Pb4 > spi;
-#elif defined(__MK20DX128__)
+#elif defined(CORE_TEENSY) && (defined(__MK20DX128__) || defined(__MK20DX256__))
 typedef SPi< P13, P11, P12, P10 > spi;
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+typedef SPi< P76, P75, P74, P10 > spi;
 #else
 #error "No SPI entry in usbhost.h"
 #endif
@@ -78,7 +83,7 @@ typedef enum {
         vbus_off = GPX_VBDET
 } VBUS_t;
 
-template< typename SS, typename INTR > class MAX3421e /* : public spi */ {
+template< typename SPI_SS, typename INTR > class MAX3421e /* : public spi */ {
         static uint8_t vbusState;
 
 public:
@@ -106,12 +111,12 @@ public:
         uint8_t Task();
 };
 
-template< typename SS, typename INTR >
-        uint8_t MAX3421e< SS, INTR >::vbusState = 0;
+template< typename SPI_SS, typename INTR >
+        uint8_t MAX3421e< SPI_SS, INTR >::vbusState = 0;
 
 /* constructor */
-template< typename SS, typename INTR >
-MAX3421e< SS, INTR >::MAX3421e() {
+template< typename SPI_SS, typename INTR >
+MAX3421e< SPI_SS, INTR >::MAX3421e() {
         // Leaving ADK hardware setup in here, for now. This really belongs with the other parts.
 #ifdef BOARD_MEGA_ADK
         // For Mega ADK, which has a Max3421e on-board, set MAX_RESET to output mode, and then set it to HIGH
@@ -121,55 +126,66 @@ MAX3421e< SS, INTR >::MAX3421e() {
 };
 
 /* write single byte into MAX3421 register */
-template< typename SS, typename INTR >
-void MAX3421e< SS, INTR >::regWr(uint8_t reg, uint8_t data) {
+template< typename SPI_SS, typename INTR >
+void MAX3421e< SPI_SS, INTR >::regWr(uint8_t reg, uint8_t data) {
         XMEM_ACQUIRE_SPI();
-        SS::Clear();
+        SPI_SS::Clear();
 #if USING_SPI4TEENSY3
         uint8_t c[2];
         c[0] = reg | 0x02;
         c[1] = data;
         spi4teensy3::send(c, 2);
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+        SPI.transfer(reg | 0x02);
+        SPI.transfer(data);
 #else
         SPDR = (reg | 0x02);
         while(!(SPSR & (1 << SPIF)));
         SPDR = data;
         while(!(SPSR & (1 << SPIF)));
 #endif
-        SS::Set();
+        SPI_SS::Set();
         XMEM_RELEASE_SPI();
         return;
 };
 /* multiple-byte write                            */
 
 /* returns a pointer to memory position after last written */
-template< typename SS, typename INTR >
-uint8_t* MAX3421e< SS, INTR >::bytesWr(uint8_t reg, uint8_t nbytes, uint8_t* data_p) {
+template< typename SPI_SS, typename INTR >
+uint8_t* MAX3421e< SPI_SS, INTR >::bytesWr(uint8_t reg, uint8_t nbytes, uint8_t* data_p) {
         XMEM_ACQUIRE_SPI();
-        SS::Clear();
+        SPI_SS::Clear();
 #if USING_SPI4TEENSY3
         spi4teensy3::send(reg | 0x02);
         spi4teensy3::send(data_p, nbytes);
         data_p += nbytes;
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+        SPI.transfer(reg | 0x02);
+        while(nbytes) {
+                SPI.transfer(*data_p);
+                nbytes--;
+                data_p++; // advance data pointer
+        }
 #else
         SPDR = (reg | 0x02); //set WR bit and send register number
-        while(nbytes--) {
+        while(nbytes) {
                 while(!(SPSR & (1 << SPIF))); //check if previous byte was sent
                 SPDR = (*data_p); // send next data byte
+                nbytes--;
                 data_p++; // advance data pointer
         }
         while(!(SPSR & (1 << SPIF)));
 #endif
-        SS::Set();
+        SPI_SS::Set();
         XMEM_RELEASE_SPI();
-        return( data_p);
+        return ( data_p);
 }
 /* GPIO write                                           */
 /*GPIO byte is split between 2 registers, so two writes are needed to write one byte */
 
 /* GPOUT bits are in the low nibble. 0-3 in IOPINS1, 4-7 in IOPINS2 */
-template< typename SS, typename INTR >
-void MAX3421e< SS, INTR >::gpioWr(uint8_t data) {
+template< typename SPI_SS, typename INTR >
+void MAX3421e< SPI_SS, INTR >::gpioWr(uint8_t data) {
         regWr(rIOPINS1, data);
         data >>= 4;
         regWr(rIOPINS2, data);
@@ -177,36 +193,46 @@ void MAX3421e< SS, INTR >::gpioWr(uint8_t data) {
 }
 
 /* single host register read    */
-template< typename SS, typename INTR >
-uint8_t MAX3421e< SS, INTR >::regRd(uint8_t reg) {
+template< typename SPI_SS, typename INTR >
+uint8_t MAX3421e< SPI_SS, INTR >::regRd(uint8_t reg) {
         XMEM_ACQUIRE_SPI();
-        SS::Clear();
+        SPI_SS::Clear();
 #if USING_SPI4TEENSY3
         spi4teensy3::send(reg);
         uint8_t rv = spi4teensy3::receive();
-        SS::Set();
+        SPI_SS::Set();
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+        SPI.transfer(reg);
+        uint8_t rv = SPI.transfer(0);
+        SPI_SS::Set();
 #else
         SPDR = reg;
         while(!(SPSR & (1 << SPIF)));
         SPDR = 0; //send empty byte
         while(!(SPSR & (1 << SPIF)));
-        SS::Set();
+        SPI_SS::Set();
         uint8_t rv = SPDR;
 #endif
         XMEM_RELEASE_SPI();
-        return(rv);
+        return (rv);
 }
 /* multiple-byte register read  */
 
 /* returns a pointer to a memory position after last read   */
-template< typename SS, typename INTR >
-uint8_t* MAX3421e< SS, INTR >::bytesRd(uint8_t reg, uint8_t nbytes, uint8_t* data_p) {
+template< typename SPI_SS, typename INTR >
+uint8_t* MAX3421e< SPI_SS, INTR >::bytesRd(uint8_t reg, uint8_t nbytes, uint8_t* data_p) {
         XMEM_ACQUIRE_SPI();
-        SS::Clear();
+        SPI_SS::Clear();
 #if USING_SPI4TEENSY3
         spi4teensy3::send(reg);
         spi4teensy3::receive(data_p, nbytes);
         data_p += nbytes;
+#elif defined(ARDUINO_SAM_DUE) && defined(__SAM3X8E__)
+        SPI.transfer(reg);
+        while(nbytes) {
+            *data_p++ = SPI.transfer(0);
+            nbytes--;
+        }
 #else
         SPDR = reg;
         while(!(SPSR & (1 << SPIF))); //wait
@@ -227,26 +253,26 @@ uint8_t* MAX3421e< SS, INTR >::bytesRd(uint8_t reg, uint8_t nbytes, uint8_t* dat
         }
 #endif
 #endif
-        SS::Set();
+        SPI_SS::Set();
         XMEM_RELEASE_SPI();
-        return( data_p);
+        return ( data_p);
 }
 /* GPIO read. See gpioWr for explanation */
 
 /* GPIN pins are in high nibbles of IOPINS1, IOPINS2    */
-template< typename SS, typename INTR >
-uint8_t MAX3421e< SS, INTR >::gpioRd() {
+template< typename SPI_SS, typename INTR >
+uint8_t MAX3421e< SPI_SS, INTR >::gpioRd() {
         uint8_t gpin = 0;
         gpin = regRd(rIOPINS2); //pins 4-7
         gpin &= 0xf0; //clean lower nibble
         gpin |= (regRd(rIOPINS1) >> 4); //shift low bits and OR with upper from previous operation.
-        return( gpin);
+        return ( gpin);
 }
 
 /* reset MAX3421E. Returns number of cycles it took for PLL to stabilize after reset
   or zero if PLL haven't stabilized in 65535 cycles */
-template< typename SS, typename INTR >
-uint16_t MAX3421e< SS, INTR >::reset() {
+template< typename SPI_SS, typename INTR >
+uint16_t MAX3421e< SPI_SS, INTR >::reset() {
         uint16_t i = 0;
         regWr(rUSBCTL, bmCHIPRES);
         regWr(rUSBCTL, 0x00);
@@ -255,19 +281,19 @@ uint16_t MAX3421e< SS, INTR >::reset() {
                         break;
                 }
         }
-        return( i);
+        return ( i);
 }
 
 /* initialize MAX3421E. Set Host mode, pullups, and stuff. Returns 0 if success, -1 if not */
-template< typename SS, typename INTR >
-int8_t MAX3421e< SS, INTR >::Init() {
+template< typename SPI_SS, typename INTR >
+int8_t MAX3421e< SPI_SS, INTR >::Init() {
         XMEM_ACQUIRE_SPI();
         // Moved here.
         // you really should not init hardware in the constructor when it involves locks.
         // Also avoids the vbus flicker issue confusing some devices.
         /* pin and peripheral setup */
-        SS::SetDirWrite();
-        SS::Set();
+        SPI_SS::SetDirWrite();
+        SPI_SS::Set();
         spi::init();
         INTR::SetDirRead();
         XMEM_RELEASE_SPI();
@@ -276,7 +302,7 @@ int8_t MAX3421e< SS, INTR >::Init() {
         regWr(rPINCTL, (bmFDUPSPI | bmINTLEVEL));
 
         if(reset() == 0) { //OSCOKIRQ hasn't asserted in time
-                return( -1);
+                return ( -1);
         }
 
         regWr(rMODE, bmDPPULLDN | bmDMPULLDN | bmHOST); // set pull-downs, Host
@@ -292,19 +318,19 @@ int8_t MAX3421e< SS, INTR >::Init() {
         regWr(rHIRQ, bmCONDETIRQ); //clear connection detect interrupt
         regWr(rCPUCTL, 0x01); //enable interrupt pin
 
-        return( 0);
+        return ( 0);
 }
 
 /* initialize MAX3421E. Set Host mode, pullups, and stuff. Returns 0 if success, -1 if not */
-template< typename SS, typename INTR >
-int8_t MAX3421e< SS, INTR >::Init(int mseconds) {
+template< typename SPI_SS, typename INTR >
+int8_t MAX3421e< SPI_SS, INTR >::Init(int mseconds) {
         XMEM_ACQUIRE_SPI();
         // Moved here.
         // you really should not init hardware in the constructor when it involves locks.
         // Also avoids the vbus flicker issue confusing some devices.
         /* pin and peripheral setup */
-        SS::SetDirWrite();
-        SS::Set();
+        SPI_SS::SetDirWrite();
+        SPI_SS::Set();
         spi::init();
         INTR::SetDirRead();
         XMEM_RELEASE_SPI();
@@ -312,7 +338,7 @@ int8_t MAX3421e< SS, INTR >::Init(int mseconds) {
         regWr(rPINCTL, (bmFDUPSPI | bmINTLEVEL | GPX_VBDET));
 
         if(reset() == 0) { //OSCOKIRQ hasn't asserted in time
-                return( -1);
+                return ( -1);
         }
 
         // Delay a minimum of 1 second to ensure any capacitors are drained.
@@ -336,12 +362,12 @@ int8_t MAX3421e< SS, INTR >::Init(int mseconds) {
         // GPX pin on. This is done here so that busprobe will fail if we have a switch connected.
         regWr(rPINCTL, (bmFDUPSPI | bmINTLEVEL));
 
-        return( 0);
+        return ( 0);
 }
 
 /* probe bus to determine device presence and speed and switch host to this speed */
-template< typename SS, typename INTR >
-void MAX3421e< SS, INTR >::busprobe() {
+template< typename SPI_SS, typename INTR >
+void MAX3421e< SPI_SS, INTR >::busprobe() {
         uint8_t bus_sample;
         bus_sample = regRd(rHRSL); //Get J,K status
         bus_sample &= (bmJSTATUS | bmKSTATUS); //zero the rest of the byte
@@ -375,8 +401,8 @@ void MAX3421e< SS, INTR >::busprobe() {
 }
 
 /* MAX3421 state change task and interrupt handler */
-template< typename SS, typename INTR >
-uint8_t MAX3421e< SS, INTR >::Task(void) {
+template< typename SPI_SS, typename INTR >
+uint8_t MAX3421e< SPI_SS, INTR >::Task(void) {
         uint8_t rcode = 0;
         uint8_t pinvalue;
         //USB_HOST_SERIAL.print("Vbus state: ");
@@ -391,11 +417,11 @@ uint8_t MAX3421e< SS, INTR >::Task(void) {
         //        GpxHandler();
         //    }
         //    usbSM();                                //USB state machine
-        return( rcode);
+        return ( rcode);
 }
 
-template< typename SS, typename INTR >
-uint8_t MAX3421e< SS, INTR >::IntHandler() {
+template< typename SPI_SS, typename INTR >
+uint8_t MAX3421e< SPI_SS, INTR >::IntHandler() {
         uint8_t HIRQ;
         uint8_t HIRQ_sendback = 0x00;
         HIRQ = regRd(rHIRQ); //determine interrupt source
@@ -408,10 +434,10 @@ uint8_t MAX3421e< SS, INTR >::IntHandler() {
         }
         /* End HIRQ interrupts handling, clear serviced IRQs    */
         regWr(rHIRQ, HIRQ_sendback);
-        return( HIRQ_sendback);
+        return ( HIRQ_sendback);
 }
-//template< typename SS, typename INTR >
-//uint8_t MAX3421e< SS, INTR >::GpxHandler()
+//template< typename SPI_SS, typename INTR >
+//uint8_t MAX3421e< SPI_SS, INTR >::GpxHandler()
 //{
 //	uint8_t GPINIRQ = regRd( rGPINIRQ );          //read GPIN IRQ register
 ////    if( GPINIRQ & bmGPINIRQ7 ) {            //vbus overload
diff --git a/usbhub.cpp b/usbhub.cpp
index e117a069..a3ab21eb 100644
--- a/usbhub.cpp
+++ b/usbhub.cpp
@@ -35,12 +35,15 @@ bPollEnable(false) {
         epInfo[1].epAttribs = 0;
         epInfo[1].bmNakPower = USB_NAK_NOWAIT;
 
-        if (pUsb)
+        if(pUsb)
                 pUsb->RegisterDeviceClass(this);
 }
 
 uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         uint8_t buf[32];
+        USB_DEVICE_DESCRIPTOR * udd = reinterpret_cast<USB_DEVICE_DESCRIPTOR*>(buf);
+        HubDescriptor* hd = reinterpret_cast<HubDescriptor*>(buf);
+        USB_CONFIGURATION_DESCRIPTOR * ucd = reinterpret_cast<USB_CONFIGURATION_DESCRIPTOR*>(buf);
         uint8_t rcode;
         UsbDevice *p = NULL;
         EpInfo *oldep_ptr = NULL;
@@ -54,133 +57,133 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         //switch (bInitState) {
         //        case 0:
-                        if (bAddress)
-                                return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
+        if(bAddress)
+                return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;
 
-                        // Get pointer to pseudo device with address 0 assigned
-                        p = addrPool.GetUsbDevicePtr(0);
+        // Get pointer to pseudo device with address 0 assigned
+        p = addrPool.GetUsbDevicePtr(0);
 
-                        if (!p)
-                                return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
+        if(!p)
+                return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;
 
-                        if (!p->epinfo)
-                                return USB_ERROR_EPINFO_IS_NULL;
+        if(!p->epinfo)
+                return USB_ERROR_EPINFO_IS_NULL;
 
-                        // Save old pointer to EP_RECORD of address 0
-                        oldep_ptr = p->epinfo;
+        // Save old pointer to EP_RECORD of address 0
+        oldep_ptr = p->epinfo;
 
-                        // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
-                        p->epinfo = epInfo;
+        // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
+        p->epinfo = epInfo;
 
-                        p->lowspeed = lowspeed;
+        p->lowspeed = lowspeed;
 
-                        // Get device descriptor
-                        rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*)buf);
+        // Get device descriptor
+        rcode = pUsb->getDevDescr(0, 0, 8, (uint8_t*)buf);
 
-                        p->lowspeed = false;
+        p->lowspeed = false;
 
-                        if (!rcode)
-                                len = (buf[0] > 32) ? 32 : buf[0];
+        if(!rcode)
+                len = (buf[0] > 32) ? 32 : buf[0];
 
-                        if (rcode) {
-                                // Restore p->epinfo
-                                p->epinfo = oldep_ptr;
-                                return rcode;
-                        }
+        if(rcode) {
+                // Restore p->epinfo
+                p->epinfo = oldep_ptr;
+                return rcode;
+        }
 
-                        // Extract device class from device descriptor
-                        // If device class is not a hub return
-                        if (((USB_DEVICE_DESCRIPTOR*)buf)->bDeviceClass != 0x09)
-                                return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
+        // Extract device class from device descriptor
+        // If device class is not a hub return
+        if(udd->bDeviceClass != 0x09)
+                return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
 
-                        // Allocate new address according to device class
-                        bAddress = addrPool.AllocAddress(parent, (((USB_DEVICE_DESCRIPTOR*)buf)->bDeviceClass == 0x09) ? true : false, port);
+        // Allocate new address according to device class
+        bAddress = addrPool.AllocAddress(parent, (udd->bDeviceClass == 0x09) ? true : false, port);
 
-                        if (!bAddress)
-                                return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
+        if(!bAddress)
+                return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;
 
-                        // Extract Max Packet Size from the device descriptor
-                        epInfo[0].maxPktSize = ((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0;
+        // Extract Max Packet Size from the device descriptor
+        epInfo[0].maxPktSize = udd->bMaxPacketSize0;
 
-                        // Assign new address to the device
-                        rcode = pUsb->setAddr(0, 0, bAddress);
+        // Assign new address to the device
+        rcode = pUsb->setAddr(0, 0, bAddress);
 
-                        if (rcode) {
-                                // Restore p->epinfo
-                                p->epinfo = oldep_ptr;
-                                addrPool.FreeAddress(bAddress);
-                                bAddress = 0;
-                                return rcode;
-                        }
+        if(rcode) {
+                // Restore p->epinfo
+                p->epinfo = oldep_ptr;
+                addrPool.FreeAddress(bAddress);
+                bAddress = 0;
+                return rcode;
+        }
 
-                        //USBTRACE2("\r\nHub address: ", bAddress );
+        //USBTRACE2("\r\nHub address: ", bAddress );
 
-                        // Restore p->epinfo
-                        p->epinfo = oldep_ptr;
+        // Restore p->epinfo
+        p->epinfo = oldep_ptr;
 
-                        if (len)
-                                rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*)buf);
+        if(len)
+                rcode = pUsb->getDevDescr(bAddress, 0, len, (uint8_t*)buf);
 
-                        if (rcode)
-                                goto FailGetDevDescr;
+        if(rcode)
+                goto FailGetDevDescr;
 
-                        // Assign epInfo to epinfo pointer
-                        rcode = pUsb->setEpInfoEntry(bAddress, 2, epInfo);
+        // Assign epInfo to epinfo pointer
+        rcode = pUsb->setEpInfoEntry(bAddress, 2, epInfo);
 
-                        if (rcode)
-                                goto FailSetDevTblEntry;
+        if(rcode)
+                goto FailSetDevTblEntry;
 
         //                bInitState = 1;
 
         //        case 1:
-                        // Get hub descriptor
-                        rcode = GetHubDescriptor(0, 8, buf);
+        // Get hub descriptor
+        rcode = GetHubDescriptor(0, 8, buf);
 
-                        if (rcode)
-                                goto FailGetHubDescr;
+        if(rcode)
+                goto FailGetHubDescr;
 
-                        // Save number of ports for future use
-                        bNbrPorts = ((HubDescriptor*)buf)->bNbrPorts;
+        // Save number of ports for future use
+        bNbrPorts = hd->bNbrPorts;
 
         //                bInitState = 2;
 
         //        case 2:
-                        // Read configuration Descriptor in Order To Obtain Proper Configuration Value
-                        rcode = pUsb->getConfDescr(bAddress, 0, 8, 0, buf);
+        // Read configuration Descriptor in Order To Obtain Proper Configuration Value
+        rcode = pUsb->getConfDescr(bAddress, 0, 8, 0, buf);
 
-                        if (!rcode) {
-                                cd_len = ((USB_CONFIGURATION_DESCRIPTOR*)buf)->wTotalLength;
-                                rcode = pUsb->getConfDescr(bAddress, 0, cd_len, 0, buf);
-                        }
-                        if (rcode)
-                                goto FailGetConfDescr;
+        if(!rcode) {
+                cd_len = ucd->wTotalLength;
+                rcode = pUsb->getConfDescr(bAddress, 0, cd_len, 0, buf);
+        }
+        if(rcode)
+                goto FailGetConfDescr;
 
-                        // The following code is of no practical use in real life applications.
-                        // It only intended for the usb protocol sniffer to properly parse hub-class requests.
-                {
-                        uint8_t buf2[24];
+        // The following code is of no practical use in real life applications.
+        // It only intended for the usb protocol sniffer to properly parse hub-class requests.
+        {
+                uint8_t buf2[24];
 
-                        rcode = pUsb->getConfDescr(bAddress, 0, buf[0], 0, buf2);
+                rcode = pUsb->getConfDescr(bAddress, 0, buf[0], 0, buf2);
 
-                        if (rcode)
-                                goto FailGetConfDescr;
-                }
+                if(rcode)
+                        goto FailGetConfDescr;
+        }
 
-                        // Set Configuration Value
-                        rcode = pUsb->setConf(bAddress, 0, buf[5]);
+        // Set Configuration Value
+        rcode = pUsb->setConf(bAddress, 0, buf[5]);
 
-                        if (rcode)
-                                goto FailSetConfDescr;
+        if(rcode)
+                goto FailSetConfDescr;
 
         //                bInitState = 3;
 
         //        case 3:
-                        // Power on all ports
-                        for (uint8_t j = 1; j <= bNbrPorts; j++)
-                                SetPortFeature(HUB_FEATURE_PORT_POWER, j, 0); //HubPortPowerOn(j);
+        // Power on all ports
+        for(uint8_t j = 1; j <= bNbrPorts; j++)
+                SetPortFeature(HUB_FEATURE_PORT_POWER, j, 0); //HubPortPowerOn(j);
 
-                        pUsb->SetHubPreMask();
-                        bPollEnable = true;
+        pUsb->SetHubPreMask();
+        bPollEnable = true;
         //                bInitState = 0;
         //}
         //bInitState = 0;
@@ -211,7 +214,7 @@ Fail:
 uint8_t USBHub::Release() {
         pUsb->GetAddressPool().FreeAddress(bAddress);
 
-        if (bAddress == 0x41)
+        if(bAddress == 0x41)
                 pUsb->SetHubPreMask();
 
         bAddress = 0;
@@ -224,10 +227,10 @@ uint8_t USBHub::Release() {
 uint8_t USBHub::Poll() {
         uint8_t rcode = 0;
 
-        if (!bPollEnable)
+        if(!bPollEnable)
                 return 0;
 
-        if (qNextPollTime <= millis()) {
+        if(((long)(millis() - qNextPollTime) >= 0L)) {
                 rcode = CheckHubStatus();
                 qNextPollTime = millis() + 100;
         }
@@ -241,7 +244,7 @@ uint8_t USBHub::CheckHubStatus() {
 
         rcode = pUsb->inTransfer(bAddress, 1, &read, buf);
 
-        if (rcode)
+        if(rcode)
                 return rcode;
 
         //if (buf[0] & 0x01) // Hub Status Change
@@ -255,36 +258,36 @@ uint8_t USBHub::CheckHubStatus() {
         //        	return rcode;
         //        }
         //}
-        for (uint8_t port = 1, mask = 0x02; port < 8; mask <<= 1, port++) {
-                if (buf[0] & mask) {
+        for(uint8_t port = 1, mask = 0x02; port < 8; mask <<= 1, port++) {
+                if(buf[0] & mask) {
                         HubEvent evt;
                         evt.bmEvent = 0;
 
                         rcode = GetPortStatus(port, 4, evt.evtBuff);
 
-                        if (rcode)
+                        if(rcode)
                                 continue;
 
                         rcode = PortStatusChange(port, evt);
 
-                        if (rcode == HUB_ERROR_PORT_HAS_BEEN_RESET)
+                        if(rcode == HUB_ERROR_PORT_HAS_BEEN_RESET)
                                 return 0;
 
-                        if (rcode)
+                        if(rcode)
                                 return rcode;
                 }
         } // for
 
-        for (uint8_t port = 1; port <= bNbrPorts; port++) {
+        for(uint8_t port = 1; port <= bNbrPorts; port++) {
                 HubEvent evt;
                 evt.bmEvent = 0;
 
                 rcode = GetPortStatus(port, 4, evt.evtBuff);
 
-                if (rcode)
+                if(rcode)
                         continue;
 
-                if ((evt.bmStatus & bmHUB_PORT_STATE_CHECK_DISABLED) != bmHUB_PORT_STATE_DISABLED)
+                if((evt.bmStatus & bmHUB_PORT_STATE_CHECK_DISABLED) != bmHUB_PORT_STATE_DISABLED)
                         continue;
 
                 // Emulate connection event for the port
@@ -292,10 +295,10 @@ uint8_t USBHub::CheckHubStatus() {
 
                 rcode = PortStatusChange(port, evt);
 
-                if (rcode == HUB_ERROR_PORT_HAS_BEEN_RESET)
+                if(rcode == HUB_ERROR_PORT_HAS_BEEN_RESET)
                         return 0;
 
-                if (rcode)
+                if(rcode)
                         return rcode;
         } // for
         return 0;
@@ -311,10 +314,10 @@ void USBHub::ResetHubPort(uint8_t port) {
         SetPortFeature(HUB_FEATURE_PORT_RESET, port, 0);
 
 
-        for (int i = 0; i < 3; i++) {
+        for(int i = 0; i < 3; i++) {
                 rcode = GetPortStatus(port, 4, evt.evtBuff);
-                if (rcode) break; // Some kind of error, bail.
-                if (evt.bmEvent == bmHUB_PORT_EVENT_RESET_COMPLETE || evt.bmEvent == bmHUB_PORT_EVENT_LS_RESET_COMPLETE) {
+                if(rcode) break; // Some kind of error, bail.
+                if(evt.bmEvent == bmHUB_PORT_EVENT_RESET_COMPLETE || evt.bmEvent == bmHUB_PORT_EVENT_LS_RESET_COMPLETE) {
                         break;
                 }
                 delay(100); // simulate polling.
@@ -325,11 +328,11 @@ void USBHub::ResetHubPort(uint8_t port) {
 }
 
 uint8_t USBHub::PortStatusChange(uint8_t port, HubEvent &evt) {
-        switch (evt.bmEvent) {
+        switch(evt.bmEvent) {
                         // Device connected event
                 case bmHUB_PORT_EVENT_CONNECT:
                 case bmHUB_PORT_EVENT_LS_CONNECT:
-                        if (bResetInitiated)
+                        if(bResetInitiated)
                                 return 0;
 
                         ClearPortFeature(HUB_FEATURE_C_PORT_ENABLE, port, 0);
@@ -376,7 +379,7 @@ void PrintHubPortStatus(USBHub *hubptr, uint8_t addr, uint8_t port, bool print_c
 
         rcode = hubptr->GetPortStatus(port, 4, evt.evtBuff);
 
-        if (rcode) {
+        if(rcode) {
                 USB_HOST_SERIAL.println("ERROR!");
                 return;
         }
@@ -405,7 +408,7 @@ void PrintHubPortStatus(USBHub *hubptr, uint8_t addr, uint8_t port, bool print_c
         USB_HOST_SERIAL.print("INDICATOR:\t");
         USB_HOST_SERIAL.println((evt.bmStatus & bmHUB_PORT_STATUS_PORT_INDICATOR) > 0, DEC);
 
-        if (!print_changes)
+        if(!print_changes)
                 return;
 
         USB_HOST_SERIAL.println("\r\nChange");
diff --git a/usbhub.h b/usbhub.h
index 2fc5ff64..aa1281b2 100644
--- a/usbhub.h
+++ b/usbhub.h
@@ -170,7 +170,7 @@ class USBHub : USBDeviceConfig {
 
         uint8_t bAddress; // address
         uint8_t bNbrPorts; // number of ports
-//        uint8_t bInitState; // initialization state variable
+        //        uint8_t bInitState; // initialization state variable
         uint32_t qNextPollTime; // next poll time
         bool bPollEnable; // poll enable flag
 
@@ -195,52 +195,56 @@ public:
         virtual uint8_t Release();
         virtual uint8_t Poll();
         virtual void ResetHubPort(uint8_t port);
+
         virtual uint8_t GetAddress() {
                 return bAddress;
         };
-        virtual boolean DEVCLASSOK(uint8_t klass) { return (klass == 0x09); }
+
+        virtual boolean DEVCLASSOK(uint8_t klass) {
+                return (klass == 0x09);
+        }
 
 };
 
 // Clear Hub Feature
 
 inline uint8_t USBHub::ClearHubFeature(uint8_t fid) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_HUB_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, 0, 0, 0, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_HUB_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, 0, 0, 0, NULL, NULL));
 }
 // Clear Port Feature
 
 inline uint8_t USBHub::ClearPortFeature(uint8_t fid, uint8_t port, uint8_t sel) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_PORT_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, ((0x0000 | port) | (sel << 8)), 0, 0, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_CLEAR_PORT_FEATURE, USB_REQUEST_CLEAR_FEATURE, fid, 0, ((0x0000 | port) | (sel << 8)), 0, 0, NULL, NULL));
 }
 // Get Hub Descriptor
 
 inline uint8_t USBHub::GetHubDescriptor(uint8_t index, uint16_t nbytes, uint8_t *dataptr) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_DESCRIPTOR, USB_REQUEST_GET_DESCRIPTOR, index, 0x29, 0, nbytes, nbytes, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_DESCRIPTOR, USB_REQUEST_GET_DESCRIPTOR, index, 0x29, 0, nbytes, nbytes, dataptr, NULL));
 }
 // Get Hub Status
 
 inline uint8_t USBHub::GetHubStatus(uint16_t nbytes, uint8_t* dataptr) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_STATUS, USB_REQUEST_GET_STATUS, 0, 0, 0x0000, nbytes, nbytes, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_HUB_STATUS, USB_REQUEST_GET_STATUS, 0, 0, 0x0000, nbytes, nbytes, dataptr, NULL));
 }
 // Get Port Status
 
 inline uint8_t USBHub::GetPortStatus(uint8_t port, uint16_t nbytes, uint8_t* dataptr) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_PORT_STATUS, USB_REQUEST_GET_STATUS, 0, 0, port, nbytes, nbytes, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_GET_PORT_STATUS, USB_REQUEST_GET_STATUS, 0, 0, port, nbytes, nbytes, dataptr, NULL));
 }
 // Set Hub Descriptor
 
 inline uint8_t USBHub::SetHubDescriptor(uint8_t port, uint16_t nbytes, uint8_t* dataptr) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_DESCRIPTOR, USB_REQUEST_SET_DESCRIPTOR, 0, 0, port, nbytes, nbytes, dataptr, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_DESCRIPTOR, USB_REQUEST_SET_DESCRIPTOR, 0, 0, port, nbytes, nbytes, dataptr, NULL));
 }
 // Set Hub Feature
 
 inline uint8_t USBHub::SetHubFeature(uint8_t fid) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, 0, 0, 0, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_HUB_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, 0, 0, 0, NULL, NULL));
 }
 // Set Port Feature
 
 inline uint8_t USBHub::SetPortFeature(uint8_t fid, uint8_t port, uint8_t sel) {
-        return( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_PORT_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, (((0x0000 | sel) << 8) | port), 0, 0, NULL, NULL));
+        return ( pUsb->ctrlReq(bAddress, 0, bmREQ_SET_PORT_FEATURE, USB_REQUEST_SET_FEATURE, fid, 0, (((0x0000 | sel) << 8) | port), 0, 0, NULL, NULL));
 }
 
 void PrintHubPortStatus(USB *usbptr, uint8_t addr, uint8_t port, bool print_changes = false);
diff --git a/xboxEnums.h b/xboxEnums.h
index adf60f9b..84b137bb 100644
--- a/xboxEnums.h
+++ b/xboxEnums.h
@@ -21,7 +21,7 @@
 #include "controllerEnums.h"
 
 /** Enum used to set special LED modes supported by the Xbox controller. */
-enum LEDMode {
+enum LEDModeEnum {
         ROTATING = 0x0A,
         FASTBLINK = 0x0B,
         SLOWBLINK = 0x0C,
@@ -29,15 +29,16 @@ enum LEDMode {
 };
 
 /** Used to set the LEDs on the controllers */
-const uint8_t XBOXLEDS[] PROGMEM = {
+const uint8_t XBOX_LEDS[] PROGMEM = {
+        0x00, // OFF
         0x02, // LED1
         0x03, // LED2
         0x04, // LED3
         0x05, // LED4
-        0x01 // ALL - Used to blink all LEDs
+        0x01, // ALL - Used to blink all LEDs
 };
 /** Buttons on the controllers */
-const uint16_t XBOXBUTTONS[] PROGMEM = {
+const uint16_t XBOX_BUTTONS[] PROGMEM = {
         0x0100, // UP
         0x0800, // RIGHT
         0x0200, // DOWN
@@ -58,7 +59,7 @@ const uint16_t XBOXBUTTONS[] PROGMEM = {
         0x0080, // Y
 
         0x0004, // XBOX
-        0x0008 // SYNC
+        0x0008, // SYNC
 };
 
-#endif
\ No newline at end of file
+#endif