diff --git a/BTD.h b/BTD.h
index 806bb63e..37afb2ec 100755
--- a/BTD.h
+++ b/BTD.h
@@ -189,21 +189,6 @@
 
 #define PAIR    1
 
-/* 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
 class BluetoothService;
 
 /**
@@ -609,6 +594,11 @@ protected:
          */
         virtual void onInit() = 0;
 
+        /** Used to check if the incoming L2CAP data matches the HCI Handle */
+        bool checkHciHandle(uint8_t *buf, uint16_t handle) {
+                return (buf[0] == (handle & 0xFF)) && (buf[1] == ((handle >> 8) | 0x20));
+        }
+
         /** Pointer to function called in onInit(). */
         void (*pFuncOnInit)(void);
 
diff --git a/BTHID.cpp b/BTHID.cpp
index e5a86b47..bfa9202c 100644
--- a/BTHID.cpp
+++ b/BTHID.cpp
@@ -65,8 +65,8 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
         }
-        //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(checkHciHandle(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
diff --git a/PS3BT.cpp b/PS3BT.cpp
index 73e2c1e6..fbd4e5f0 100644
--- a/PS3BT.cpp
+++ b/PS3BT.cpp
@@ -241,10 +241,10 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                         }
                 }
         }
-        //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
+
+        if(checkHciHandle(ACLData, hci_handle)) { // acl_handle_ok
                 memcpy(l2capinbuf, ACLData, BULK_MAXPKTSIZE);
-                if((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001U) { //l2cap_control - Channel ID for ACL-U
+                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);
diff --git a/SPP.cpp b/SPP.cpp
index cd5072da..0f4ee5e9 100644
--- a/SPP.cpp
+++ b/SPP.cpp
@@ -95,9 +95,9 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
         }
-        //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(checkHciHandle(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);
diff --git a/Wii.cpp b/Wii.cpp
index 742a67b5..008e5329 100755
--- a/Wii.cpp
+++ b/Wii.cpp
@@ -142,9 +142,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
         }
-        //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(checkHciHandle(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);