diff --git a/BTD.h b/BTD.h
index e870427d..e5b06499 100755
--- a/BTD.h
+++ b/BTD.h
@@ -191,6 +191,22 @@
 
 #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
+
 /** All Bluetooth services should inherit this class. */
 class BluetoothService {
 public:
diff --git a/BTHID.cpp b/BTHID.cpp
index 39575aea..b240ec68 100644
--- a/BTHID.cpp
+++ b/BTHID.cpp
@@ -67,7 +67,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((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
diff --git a/PS3BT.cpp b/PS3BT.cpp
index fbcb5dfc..103d71ae 100644
--- a/PS3BT.cpp
+++ b/PS3BT.cpp
@@ -250,7 +250,8 @@ void PS3BT::ACLData(uint8_t* ACLData) {
                         }
                 }
         }
-        if((ACLData[0] | (uint16_t)ACLData[1] << 8) == (hci_handle | 0x2000U)) { //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)) == 0x0001U) { //l2cap_control - Channel ID for ACL-U
                         if(l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) {
diff --git a/SPP.cpp b/SPP.cpp
index ff75c9cb..ef0cf216 100644
--- a/SPP.cpp
+++ b/SPP.cpp
@@ -97,7 +97,8 @@ void SPP::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
         }
-        if((l2capinbuf[0] | (uint16_t)l2capinbuf[1] << 8) == (hci_handle | 0x2000U)) { // acl_handle_ok
+        //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
diff --git a/Wii.cpp b/Wii.cpp
index 1fbd95f4..6a6f9f21 100755
--- a/Wii.cpp
+++ b/Wii.cpp
@@ -144,7 +144,8 @@ 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((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