diff --git a/BTD.cpp b/BTD.cpp
index 79a4b6cd..43a34d41 100755
--- a/BTD.cpp
+++ b/BTD.cpp
@@ -143,7 +143,7 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 /* We only need the Control endpoint, so we don't have to initialize the other endpoints of device */
                 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, 1);
                 if (rcode)
-                        goto FailSetConf;
+                        goto FailSetConfDescr;
 
                 if (PID == PS3_PID || PID == PS3NAVIGATION_PID) {
 #ifdef DEBUG
@@ -193,7 +193,7 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 // Set Configuration Value
                 rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum);
                 if (rcode)
-                        goto FailSetConf;
+                        goto FailSetConfDescr;
 
                 hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command
                 hci_counter = 0;
@@ -209,40 +209,30 @@ uint8_t BTD::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         /* diagnostic messages */
 FailGetDevDescr:
-#ifdef DEBUG
-        Notify(PSTR("\r\ngetDevDescr"), 0x80);
-#endif
+        NotifyFailGetDevDescr();
         goto Fail;
+
 FailSetDevTblEntry:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetDevTblEn"), 0x80);
-#endif
+        NotifyFailSetDevTblEntry();
         goto Fail;
+
 FailGetConfDescr:
-#ifdef DEBUG
-        Notify(PSTR("\r\ngetConf"), 0x80);
-#endif
+        NotifyFailGetConfDescr();
         goto Fail;
-FailSetConf:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetConf"), 0x80);
-#endif
+
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
-FailUnknownDevice:
-#ifdef DEBUG
-        Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
-        PrintHex<uint16_t > (VID, 0x80);
-        Notify(PSTR(" PID: "), 0x80);
-        PrintHex<uint16_t > (PID, 0x80);
-#endif
+
+        FailUnknownDevice:
+        NotifyFailUnknownDevice(VID,PID);
         pUsb->setAddr(bAddress, 0, 0); // Reset address
         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
-        goto Fail;
 Fail:
 #ifdef DEBUG
         Notify(PSTR("\r\nBTD Init Failed, error code: "), 0x80);
-        Serial.print(rcode);
 #endif
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/PS3USB.cpp b/PS3USB.cpp
index ae44f9c0..5a3218c4 100644
--- a/PS3USB.cpp
+++ b/PS3USB.cpp
@@ -187,7 +187,7 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         rcode = pUsb->setConf(bAddress, epInfo[ PS3_CONTROL_PIPE ].epAddr, 1);
         if (rcode)
-                goto FailSetConf;
+                goto FailSetConfDescr;
 
         if (PID == PS3_PID || PID == PS3NAVIGATION_PID) {
                 if (PID == PS3_PID) {
@@ -232,34 +232,25 @@ uint8_t PS3USB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         /* diagnostic messages */
 FailGetDevDescr:
-#ifdef DEBUG
-        Notify(PSTR("\r\ngetDevDescr:"), 0x80);
-#endif
+        NotifyFailGetDevDescr();
         goto Fail;
+
 FailSetDevTblEntry:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetDevTblEn:"), 0x80);
-#endif
+        NotifyFailSetDevTblEntry();
         goto Fail;
-FailSetConf:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetConf:"), 0x80);
-#endif
+
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
 FailUnknownDevice:
-#ifdef DEBUG
-        Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
-        PrintHex<uint16_t > (VID, 0x80);
-        Notify(PSTR(" PID: "), 0x80);
-        PrintHex<uint16_t > (PID, 0x80);
-#endif
+        NotifyFailUnknownDevice(VID,PID);
         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
-        goto Fail;
-Fail:
+        Fail:
+
 #ifdef DEBUG
         Notify(PSTR("\r\nPS3 Init Failed, error code: "), 0x80);
-        Serial.print(rcode, HEX);
 #endif
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/Usb.h b/Usb.h
index c27796d2..d9e0763a 100644
--- a/Usb.h
+++ b/Usb.h
@@ -18,10 +18,8 @@ e-mail   :  support@circuitsathome.com
 #ifndef _usb_h_
 #define _usb_h_
 
-
 //#define BOARD_BLACK_WIDDOW
 
-
 #define USB_METHODS_INLINE
 
 #include <inttypes.h>
@@ -39,12 +37,8 @@ e-mail   :  support@circuitsathome.com
 #else
 #include <WProgram.h>
 #endif
-
-#include "printhex.h"
-#include "hexdump.h"
 #include "message.h"
 
-extern int UsbDEBUGlvl;
 /* shield pins. First parameter - SS pin, second parameter - INT pin */
 
 #ifdef BOARD_BLACK_WIDDOW
diff --git a/Wii.h b/Wii.h
index 6ffe6803..e79a75b0 100755
--- a/Wii.h
+++ b/Wii.h
@@ -204,6 +204,10 @@ public:
          */
         void setLedStatus();
 
+        /**
+         * Call this to update battery level and Wiimote state
+         */
+        void statusRequest();
         /**
          * Return the battery level of the Wiimote.
          * @return The battery level in the range 0-255.
@@ -211,7 +215,6 @@ public:
         uint8_t getBatteryLevel() {
                 return batteryLevel;
         };
-
         /**
          * Return the Wiimote state.
          * @return See: http://wiibrew.org/wiki/Wiimote#0x20:_Status.
@@ -438,7 +441,6 @@ private:
         /* HID Commands */
         void HID_Command(uint8_t* data, uint8_t nbytes);
         void setReportMode(bool continuous, uint8_t mode);
-        void statusRequest();
 
         void writeData(uint32_t offset, uint8_t size, uint8_t* data);
         void initExtension1();
diff --git a/XBOXRECV.cpp b/XBOXRECV.cpp
index 72f34b50..d744743a 100644
--- a/XBOXRECV.cpp
+++ b/XBOXRECV.cpp
@@ -207,7 +207,7 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
         if (rcode)
-                goto FailSetConf;
+                goto FailSetConfDescr;
 
 #ifdef DEBUG
         Notify(PSTR("\r\nXbox Wireless Receiver Connected\r\n"), 0x80);
@@ -218,34 +218,26 @@ uint8_t XBOXRECV::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         /* diagnostic messages */
 FailGetDevDescr:
-#ifdef DEBUG
-        Notify(PSTR("\r\ngetDevDescr:"), 0x80);
-#endif
+        NotifyFailGetDevDescr();
         goto Fail;
+
 FailSetDevTblEntry:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetDevTblEn:"), 0x80);
-#endif
+        NotifyFailSetDevTblEntry();
         goto Fail;
-FailSetConf:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetConf:"), 0x80);
-#endif
+
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
-FailUnknownDevice:
-#ifdef DEBUG
-        Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
-        PrintHex<uint16_t > (VID, 0x80);
-        Notify(PSTR(" PID: "), 0x80);
-        PrintHex<uint16_t > (PID, 0x80);
-#endif
+
+        FailUnknownDevice:
+        NotifyFailUnknownDevice(VID,PID);
         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
-        goto Fail;
+
 Fail:
 #ifdef DEBUG
         Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80);
-        Serial.print(rcode, HEX);
 #endif
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
@@ -447,10 +439,7 @@ ControllerStatus Breakdown
     ControllerStatus[controller] & 0x8000   // 0
  */
 uint8_t XBOXRECV::getBatteryLevel(uint8_t controller) {
-        uint8_t batteryLevel = ((controllerStatus[controller] & 0x00C0) >> 6) * 33;
-        if (batteryLevel == 99)
-                batteryLevel = 100;
-        return batteryLevel;
+        return ((controllerStatus[controller] & 0x00C0) >> 6);
 }
 
 void XBOXRECV::XboxCommand(uint8_t controller, uint8_t* data, uint16_t nbytes) {
diff --git a/XBOXRECV.h b/XBOXRECV.h
index 1235f201..20744f01 100644
--- a/XBOXRECV.h
+++ b/XBOXRECV.h
@@ -191,7 +191,7 @@ public:
         /**
          * Used to get the battery level from the controller.
          * @param  controller The controller to read from.
-         * @return            Returns the battery level in percentage in 33% steps.
+         * @return            Returns the battery level as an integer in the range of 0-3.
          */
         uint8_t getBatteryLevel(uint8_t controller);
         /**
diff --git a/XBOXUSB.cpp b/XBOXUSB.cpp
index 89fdffd2..33f9bc5b 100644
--- a/XBOXUSB.cpp
+++ b/XBOXUSB.cpp
@@ -171,7 +171,7 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         rcode = pUsb->setConf(bAddress, epInfo[ XBOX_CONTROL_PIPE ].epAddr, 1);
         if (rcode)
-                goto FailSetConf;
+                goto FailSetConfDescr;
 
 #ifdef DEBUG
         Notify(PSTR("\r\nXbox 360 Controller Connected\r\n"), 0x80);
@@ -183,34 +183,25 @@ uint8_t XBOXUSB::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         /* diagnostic messages */
 FailGetDevDescr:
-#ifdef DEBUG
-        Notify(PSTR("\r\ngetDevDescr:"), 0x80);
-#endif
+        NotifyFailGetDevDescr();
         goto Fail;
+
 FailSetDevTblEntry:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetDevTblEn:"), 0x80);
-#endif
+        NotifyFailSetDevTblEntry();
         goto Fail;
-FailSetConf:
-#ifdef DEBUG
-        Notify(PSTR("\r\nsetConf:"), 0x80);
-#endif
+
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
 FailUnknownDevice:
-#ifdef DEBUG
-        Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
-        PrintHex<uint16_t > (VID, 0x80);
-        Notify(PSTR(" PID: "), 0x80);
-        PrintHex<uint16_t > (PID, 0x80);
-#endif
+        NotifyFailUnknownDevice(VID,PID);
         rcode = USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;
-        goto Fail;
+
 Fail:
 #ifdef DEBUG
         Notify(PSTR("\r\nXbox 360 Init Failed, error code: "), 0x80);
-        Serial.print(rcode, HEX);
 #endif
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/adk.cpp b/adk.cpp
index 0ea19de0..8e2d27ef 100644
--- a/adk.cpp
+++ b/adk.cpp
@@ -176,7 +176,7 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
                 // Set Configuration Value
                 rcode = pUsb->setConf(bAddress, 0, bConfNum);
                 if (rcode) {
-                        goto FailSetConf;
+                        goto FailSetConfDescr;
                 }
                 /* print endpoint structure */
                 //		          USBTRACE("\r\nEndpoint Structure:");
@@ -227,11 +227,19 @@ uint8_t ADK::Init(uint8_t parent, uint8_t port, bool lowspeed) {
 
         /* diagnostic messages */
 FailGetDevDescr:
-        USBTRACE("\r\ngetDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("\r\nsetDevTblEn:");
+        NotifyFailSetDevTblEntry();
+        goto Fail;
+
+FailGetConfDescr:
+        NotifyFailGetConfDescr();
+        goto Fail;
+
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
 
 FailGetProto:
@@ -244,22 +252,15 @@ FailSwAcc:
 
 SwAttempt:
         USBTRACE("\r\nAccessory mode switch attempt");
-        goto Fail;
+        // goto Fail;
 
-FailGetConfDescr:
-        //	USBTRACE("getConf:");
-        goto Fail;
-        //
-FailSetConf:
-        //	USBTRACE("setConf:");
-        goto Fail;
-        //
-        //FailOnInit:
+//FailOnInit:
         //	USBTRACE("OnInit:");
         //	goto Fail;
         //
 Fail:
         //USBTRACE2("\r\nADK Init Failed, error code: ", rcode);
+        //NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/cdcacm.cpp b/cdcacm.cpp
index 07e1e76a..84b8cb99 100644
--- a/cdcacm.cpp
+++ b/cdcacm.cpp
@@ -165,7 +165,7 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
         if (rcode)
-                goto FailSetConf;
+                goto FailSetConfDescr;
 
         rcode = pAsync->OnInit(this);
 
@@ -180,29 +180,26 @@ uint8_t ACM::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         return 0;
 
 FailGetDevDescr:
-        USBTRACE("getDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("setDevTblEn:");
+        NotifyFailSetDevTblEntry();
         goto Fail;
 
 FailGetConfDescr:
-        USBTRACE("getConf:");
+        NotifyFailGetConfDescr();
         goto Fail;
 
-FailSetConf:
-        USBTRACE("setConf:");
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
 
 FailOnInit:
         USBTRACE("OnInit:");
-        goto Fail;
 
 Fail:
-        PrintHex<uint8_t > (rcode, 0x80);
-        Notify(PSTR("\r\n"), 0x80);
-        // Serial.println(rcode, HEX);
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/cdcftdi.cpp b/cdcftdi.cpp
index 65f26cb5..b656f100 100644
--- a/cdcftdi.cpp
+++ b/cdcftdi.cpp
@@ -178,29 +178,26 @@ uint8_t FTDI::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         return 0;
 
 FailGetDevDescr:
-        USBTRACE("getDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("setDevTblEn:");
+        NotifyFailSetDevTblEntry();
         goto Fail;
 
 FailGetConfDescr:
-        USBTRACE("getConf:");
+        NotifyFailGetConfDescr();
         goto Fail;
 
 FailSetConfDescr:
-        USBTRACE("setConf:");
+        NotifyFailSetConfDescr();
         goto Fail;
 
 FailOnInit:
         USBTRACE("OnInit:");
-        goto Fail;
 
 Fail:
-        PrintHex<uint8_t > (rcode, 0x80);
-        Notify(PSTR("\r\n"), 0x80);
-        //Serial.println(rcode, HEX);
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/cdcprolific.cpp b/cdcprolific.cpp
index 139a627d..08671737 100644
--- a/cdcprolific.cpp
+++ b/cdcprolific.cpp
@@ -155,29 +155,26 @@ uint8_t PL2303::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         return 0;
 
 FailGetDevDescr:
-        USBTRACE("getDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("setDevTblEn:");
+        NotifyFailSetDevTblEntry();
         goto Fail;
 
 FailGetConfDescr:
-        USBTRACE("getConf:");
+        NotifyFailGetConfDescr();
         goto Fail;
 
 FailSetConfDescr:
-        USBTRACE("setConf:");
+        NotifyFailSetConfDescr();
         goto Fail;
 
 FailOnInit:
         USBTRACE("OnInit:");
-        goto Fail;
 
 Fail:
-        PrintHex<uint8_t > (rcode, 0x80);
-        Notify(PSTR("\r\n"), 0x80);
-        //Serial.println(rcode, HEX);
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/confdescparser.h b/confdescparser.h
index c446b05e..3ff89315 100644
--- a/confdescparser.h
+++ b/confdescparser.h
@@ -20,8 +20,6 @@ e-mail   :  support@circuitsathome.com
 #include <inttypes.h>
 
 #include <avr/pgmspace.h>
-#include "printhex.h"
-#include "hexdump.h"
 #include "message.h"
 #include "parsetools.h"
 
diff --git a/hexdump.h b/hexdump.h
index 7328d6e5..9f9dc339 100644
--- a/hexdump.h
+++ b/hexdump.h
@@ -19,7 +19,6 @@ e-mail   :  support@circuitsathome.com
 
 #include <inttypes.h>
 #include <avr/pgmspace.h>
-#include "printhex.h"
 
 template <class BASE_CLASS, class LEN_TYPE, class OFFSET_TYPE>
 class HexDumper : public BASE_CLASS {
@@ -41,19 +40,23 @@ 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) {
-        for(LEN_TYPE j = 0; j < len; j++, byteCount++, byteTotal++) {
-                if(!byteCount) {
-                        SerialPrintHex<OFFSET_TYPE > (byteTotal);
-                        Serial.print(": ");
-                }
-                SerialPrintHex<uint8_t > (pbuf[j]);
-                Serial.print(" ");
+#ifdef 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);
+                                Notify(PSTR(": "), 0x80);
+                        }
+                        PrintHex<uint8_t > (pbuf[j], 0x80);
+                        Notify(PSTR(" "), 0x80);
 
-                if(byteCount == 15) {
-                        Serial.println("");
-                        byteCount = 0xFF;
+                        if(byteCount == 15) {
+                                Notify(PSTR("\r\n"), 0x80);
+                                byteCount = 0xFF;
+                        }
                 }
         }
+#endif
 }
 
-#endif // __HEXDUMP_H__
\ No newline at end of file
+#endif // __HEXDUMP_H__
diff --git a/hidboot.h b/hidboot.h
index 581ee566..6e1fa4a6 100644
--- a/hidboot.h
+++ b/hidboot.h
@@ -390,17 +390,22 @@ uint8_t HIDBoot<BOOT_PROTOCOL>::Init(uint8_t parent, uint8_t port, bool lowspeed
         return 0;
 
 FailGetDevDescr:
-        USBTRACE("getDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("setDevTblEn:");
+        NotifyFailSetDevTblEntry();
         goto Fail;
 
 FailGetConfDescr:
-        USBTRACE("getConf:");
+        NotifyFailGetConfDescr();
         goto Fail;
 
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
+        goto Fail;
+
+
 FailSetProtocol:
         USBTRACE("SetProto:");
         goto Fail;
@@ -409,14 +414,8 @@ FailSetIdle:
         USBTRACE("SetIdle:");
         goto Fail;
 
-FailSetConfDescr:
-        USBTRACE("setConf:");
-        goto Fail;
-
 Fail:
-        PrintHex<uint8_t > (rcode, 0x80);
-        Notify(PSTR("\n"), 0x80);
-        // Serial.println(rcode, HEX);
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/hidescriptorparser.cpp b/hidescriptorparser.cpp
index 2db1e75c..a0226f2f 100644
--- a/hidescriptorparser.cpp
+++ b/hidescriptorparser.cpp
@@ -1623,4 +1623,4 @@ void UniversalReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t
 
         if (ret)
                 ErrorMessage<uint8_t > (PSTR("GetReportDescr-2"), ret);
-}
\ No newline at end of file
+}
diff --git a/hiduniversal.cpp b/hiduniversal.cpp
index 2ae0ba3c..ac595a9b 100644
--- a/hiduniversal.cpp
+++ b/hiduniversal.cpp
@@ -221,29 +221,27 @@ uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         return 0;
 
 FailGetDevDescr:
-        USBTRACE("getDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("setDevTblEn:");
+        NotifyFailSetDevTblEntry();
         goto Fail;
 
 FailGetConfDescr:
-        USBTRACE("getConf:");
+        NotifyFailGetConfDescr();
         goto Fail;
 
 FailSetConfDescr:
-        USBTRACE("setConf:");
+        NotifyFailSetConfDescr();
         goto Fail;
 
+
 FailSetIdle:
         USBTRACE("SetIdle:");
-        goto Fail;
 
 Fail:
-        PrintHex<uint8_t > (rcode, 0x80);
-        Notify(PSTR("\r\n"), 0x80);
-        //Serial.println(rcode, HEX);
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/keywords.txt b/keywords.txt
index 2f027e8d..01dcbdc1 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -238,6 +238,7 @@ getPitch	KEYWORD2
 getRoll	KEYWORD2
 getYaw	KEYWORD2
 PAIR	KEYWORD2
+statusRequest	KEYWORD2
 getBatteryLevel	KEYWORD2
 getWiiState	KEYWORD2
 
diff --git a/masstorage.cpp b/masstorage.cpp
index dde10114..3e8050aa 100644
--- a/masstorage.cpp
+++ b/masstorage.cpp
@@ -1,4 +1,5 @@
 #include "masstorage.h"
+
 const uint8_t BulkOnly::epDataInIndex = 1;
 const uint8_t BulkOnly::epDataOutIndex = 2;
 const uint8_t BulkOnly::epInterruptInIndex = 3;
@@ -160,7 +161,7 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         rcode = pUsb->setConf(bAddress, 0, bConfNum);
 
         if (rcode)
-                goto FailSetConf;
+                goto FailSetConfDescr;
 
         delay(10000);
 
@@ -280,19 +281,19 @@ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         return 0;
 
 FailGetDevDescr:
-        USBTRACE("getDevDescr:");
+        NotifyFailGetDevDescr();
         goto Fail;
 
 FailSetDevTblEntry:
-        USBTRACE("setDevTblEn:");
+        NotifyFailSetDevTblEntry();
         goto Fail;
 
 FailGetConfDescr:
-        USBTRACE("getConf:");
+        NotifyFailGetConfDescr();
         goto Fail;
 
-FailSetConf:
-        USBTRACE("setConf:");
+FailSetConfDescr:
+        NotifyFailSetConfDescr();
         goto Fail;
 
 FailOnInit:
@@ -325,12 +326,9 @@ FailModeSense0:
 
 FailModeSense1:
         USBTRACE("ModeSense1:");
-        goto Fail;
 
 Fail:
-        PrintHex<uint8_t > (rcode, 0x80);
-        Notify(PSTR("\r\n"), 0x80);
-        //Serial.println(rcode, HEX);
+        NotifyFail(rcode);
         Release();
         return rcode;
 }
diff --git a/masstorage.h b/masstorage.h
index c30f4ae6..82a8abf0 100644
--- a/masstorage.h
+++ b/masstorage.h
@@ -1,6 +1,8 @@
 #if !defined(__MASSTORAGE_H__)
 #define __MASSTORAGE_H__
 
+#define DEBUG
+
 #include <inttypes.h>
 #include "avrpins.h"
 #include <avr/pgmspace.h>
@@ -15,10 +17,6 @@
 #include <WProgram.h>
 #endif
 
-#include "printhex.h"
-#include "hexdump.h"
-#include "message.h"
-
 #include <confdescparser.h>
 
 #define SWAP(a, b) (((a) ^= (b)), ((b) ^= (a)), ((a) ^= (b)))
diff --git a/message.cpp b/message.cpp
index 47268b48..9a5395ea 100644
--- a/message.cpp
+++ b/message.cpp
@@ -14,6 +14,8 @@ Circuits At Home, LTD
 Web      :  http://www.circuitsathome.com
 e-mail   :  support@circuitsathome.com
  */
+
+#define DEBUG
 #include "message.h"
 // 0x80 is the default (i.e. trace) to turn off set this global to something lower.
 // this allows for 126 other debugging levels.
@@ -45,3 +47,30 @@ void NotifyStr(char const * msg, int lvl) {
 
         while (c = *msg++) Notifyc(c, lvl);
 }
+
+void NotifyFailGetDevDescr(void) {
+        Notify(PSTR("\r\ngetDevDescr"), 0x80);
+}
+
+void NotifyFailSetDevTblEntry(void) {
+        Notify(PSTR("\r\nsetDevTblEn"), 0x80);
+}
+void NotifyFailGetConfDescr(void) {
+        Notify(PSTR("\r\ngetConf"), 0x80);
+}
+
+void NotifyFailSetConfDescr(void) {
+        Notify(PSTR("\r\nsetConf"), 0x80);
+}
+
+void NotifyFailUnknownDevice(uint16_t VID, uint16_t PID) {
+        Notify(PSTR("\r\nUnknown Device Connected - VID: "), 0x80);
+        PrintHex<uint16_t > (VID, 0x80);
+        Notify(PSTR(" PID: "), 0x80);
+        PrintHex<uint16_t > (PID, 0x80);
+}
+
+void NotifyFail(uint8_t rcode) {
+        PrintHex<uint8_t > (rcode, 0x80);
+        Notify(PSTR("\r\n"), 0x80);
+}
diff --git a/message.h b/message.h
index fa339487..02a503ac 100644
--- a/message.h
+++ b/message.h
@@ -20,12 +20,27 @@ e-mail   :  support@circuitsathome.com
 #include <inttypes.h>
 #include <avr/pgmspace.h>
 
-void Notify(char const * msg, int lvl);
-void NotifyStr(char const * msg, int lvl);
+extern int UsbDEBUGlvl;
 
 #include "printhex.h"
 
-//void Notify(const char* msg);
+void Notify(char const * msg, int lvl);
+void NotifyStr(char const * msg, int lvl);
+#ifdef DEBUG
+void NotifyFailGetDevDescr(void);
+void NotifyFailSetDevTblEntry(void);
+void NotifyFailGetConfDescr(void);
+void NotifyFailSetConfDescr(void);
+void NotifyFailUnknownDevice(uint16_t VID, uint16_t PID);
+void NotifyFail(uint8_t rcode);
+#else
+#define NotifyFailGetDevDescr()
+#define NotifyFailSetDevTblEntry()
+#define NotifyFailGetConfDescr()
+#define NotifyFailSetConfDescr()
+#define NotifyFailUnknownDevice(VID, PID)
+#define NotifyFail(rcode)
+#endif
 
 template <class ERROR_TYPE>
 void ErrorMessage(char const * msg, ERROR_TYPE rcode = 0) {
@@ -35,5 +50,6 @@ void ErrorMessage(char const * msg, ERROR_TYPE rcode = 0) {
         Notify(PSTR("\r\n"), 0x80);
 }
 
+#include "hexdump.h"
 
 #endif // __MESSAGE_H__
diff --git a/usbhub.cpp b/usbhub.cpp
index 52e20cfb..3692a6ab 100644
--- a/usbhub.cpp
+++ b/usbhub.cpp
@@ -185,6 +185,7 @@ uint8_t USBHub::Init(uint8_t parent, uint8_t port, bool lowspeed) {
         bInitState = 0;
         return 0;
 
+// Oleg, No debugging?? -- xxxajk
 FailGetDevDescr:
         goto Fail;