From 253b43c06fdd87b03165c86828ef0d1b4493fb20 Mon Sep 17 00:00:00 2001
From: Kristian Lauszus <lauszus@gmail.com>
Date: Mon, 25 Nov 2013 02:15:33 +0100
Subject: [PATCH] Set protocol before establishing interrupt channel

---
 BTHID.cpp | 23 ++++++++++++++++-------
 BTHID.h   | 13 +++++++------
 2 files changed, 23 insertions(+), 13 deletions(-)

diff --git a/BTHID.cpp b/BTHID.cpp
index 2fe77163..7820d2d5 100644
--- a/BTHID.cpp
+++ b/BTHID.cpp
@@ -248,6 +248,7 @@ void BTHID::L2CAP_task() {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nHID Control Successfully Configured"), 0x80);
 #endif
+                                setProtocol();
                                 l2cap_state = L2CAP_INTERRUPT_SETUP;
                         }
                         break;
@@ -276,19 +277,24 @@ void BTHID::L2CAP_task() {
 #endif
                                 identifier++;
                                 pBtd->l2cap_config_request(hci_handle, identifier, control_scid);
+                                l2cap_state = L2CAP_SET_PROTOCOL;
+                        }
+                        break;
+
+                case L2CAP_SET_PROTOCOL:
+                        if (l2cap_config_success_control_flag) {
+                                setProtocol();
                                 l2cap_state = L2CAP_CONTROL_CONFIG_REQUEST;
                         }
                         break;
 
                 case L2CAP_CONTROL_CONFIG_REQUEST:
-                        if (l2cap_config_success_control_flag) {
 #ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
+                        Notify(PSTR("\r\nSend HID Interrupt Connection Request"), 0x80);
 #endif
-                                identifier++;
-                                pBtd->l2cap_connection_request(hci_handle, identifier, interrupt_dcid, HID_INTR_PSM);
-                                l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
-                        }
+                        identifier++;
+                        pBtd->l2cap_connection_request(hci_handle, identifier, interrupt_dcid, HID_INTR_PSM);
+                        l2cap_state = L2CAP_INTERRUPT_CONNECT_REQUEST;
                         break;
 
                 case L2CAP_INTERRUPT_CONNECT_REQUEST:
@@ -310,7 +316,6 @@ void BTHID::L2CAP_task() {
                                 pBtd->connectToHIDDevice = false;
                                 pBtd->pairWithHIDDevice = false;
                                 connected = true;
-                                setProtocol();
                                 onInit();
                                 l2cap_state = L2CAP_DONE;
                         }
@@ -378,6 +383,10 @@ 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
         pBtd->L2CAP_Command(hci_handle, &command, 1, control_scid[0], control_scid[1]);
 }
diff --git a/BTHID.h b/BTHID.h
index c26de15b..e6e168b6 100644
--- a/BTHID.h
+++ b/BTHID.h
@@ -31,14 +31,15 @@
 
 // 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_SET_PROTOCOL              4
+#define L2CAP_CONTROL_CONFIG_REQUEST    5
+#define L2CAP_INTERRUPT_CONNECT_REQUEST 6
 
-#define L2CAP_INTERRUPT_CONFIG_REQUEST  6
-#define L2CAP_DONE                      7
+#define L2CAP_INTERRUPT_CONFIG_REQUEST  7
+#define L2CAP_DONE                      8
 
-#define L2CAP_INTERRUPT_DISCONNECT      8
-#define L2CAP_CONTROL_DISCONNECT        9
+#define L2CAP_INTERRUPT_DISCONNECT      9
+#define L2CAP_CONTROL_DISCONNECT        10
 
 /* L2CAP event flags */
 #define L2CAP_FLAG_CONTROL_CONNECTED                0x01