Added SDP support to the BTHID, as needed for the Xbox One S controller

BTD.cpp

@@ -34,6 +34,7 @@ bAddress(0), // Device address - mandatory
 bNumEP(1), // If config descriptor needs to be parsed
 qNextPollTime(0), // Reset NextPollTime
 pollInterval(0),
+simple_pairing_supported(false),
 bPollEnable(false) // Don't start polling before dongle is connected
 {
         for(uint8_t i = 0; i < BTD_NUM_SERVICES; i++)
@@ -321,6 +322,7 @@ void BTD::Initialize() {
         qNextPollTime = 0; // Reset next poll time
         pollInterval = 0;
         bPollEnable = false; // Don't start polling before dongle is connected
+        simple_pairing_supported = false;
 }
 
 /* Extracts interrupt-IN, bulk-IN, bulk-OUT endpoint information from config descriptor */
@@ -431,10 +433,13 @@ void BTD::HCI_event_task() {
 #ifdef DEBUG_USB_HOST
                                                         if(hcibuf[6] == 0) { // Page 0
                                                                 Notify(PSTR("\r\nDongle "), 0x80);
-                                                                if(hcibuf[8 + 6] & (1U << 3))
+                                                                if(hcibuf[8 + 6] & (1U << 3)) {
+                                                                        simple_pairing_supported = true;
                                                                         Notify(PSTR("supports"), 0x80);
-                                                                else
+                                                                } else {
+                                                                        simple_pairing_supported = false;
                                                                         Notify(PSTR("does NOT support"), 0x80);
+                                                                }
                                                                 Notify(PSTR(" secure simple pairing (controller support)"), 0x80);
                                                         } else if(hcibuf[6] == 1) { // Page 1
                                                                 Notify(PSTR("\r\nDongle "), 0x80);
@@ -485,7 +490,7 @@ void BTD::HCI_event_task() {
 
                         case EV_INQUIRY_RESULT:
                                 if(hcibuf[2]) { // Check that there is more than zero responses
-#if defined(EXTRADEBUG) && 0
+#ifdef EXTRADEBUG
                                         Notify(PSTR("\r\nNumber of responses: "), 0x80);
                                         Notify(hcibuf[2], 0x80);
 #endif
@@ -495,7 +500,7 @@ void BTD::HCI_event_task() {
                                                 for(uint8_t j = 0; j < 3; j++)
                                                         classOfDevice[j] = hcibuf[j + 4 + offset];
 
-#if defined(EXTRADEBUG) && 0
+#ifdef EXTRADEBUG
                                                 Notify(PSTR("\r\nClass of device: "), 0x80);
                                                 D_PrintHex<uint8_t > (classOfDevice[2], 0x80);
                                                 Notify(PSTR(" "), 0x80);
@@ -710,8 +715,8 @@ void BTD::HCI_event_task() {
 
                                 /* We will just ignore the following events */
                         case EV_MAX_SLOTS_CHANGE:
-                                break;
                         case EV_NUM_COMPLETE_PKT:
+                                break;
                         case EV_ROLE_CHANGED:
                         case EV_PAGE_SCAN_REP_MODE:
                         case EV_LOOPBACK_COMMAND:
@@ -806,29 +811,14 @@ void BTD::HCI_task() {
 
                 case HCI_LOCAL_VERSION_STATE: // The local version is used by the PS3BT class
                         if(hci_check_flag(HCI_FLAG_READ_VERSION)) {
-                                hci_read_local_extended_features(0); // "Requests the normal LMP features as returned by Read_Local_Supported_Features"
-                                //hci_read_local_extended_features(1); // Read page 1
-                                hci_state = HCI_LOCAL_EXTENDED_FEATURES_STATE;
-                        }
-                        break;
-
-                case HCI_LOCAL_EXTENDED_FEATURES_STATE:
-                        if(hci_check_flag(HCI_FLAG_LOCAL_EXTENDED_FEATURES)) {
-                                hci_write_simple_pairing_mode(true);
-                                hci_state = HCI_WRITE_SIMPLE_PAIRING_STATE;
-                        }
-                        break;
-
-                case HCI_WRITE_SIMPLE_PAIRING_STATE:
-                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
-#ifdef DEBUG_USB_HOST
-                                Notify(PSTR("\r\nSimple pairing was enabled"), 0x80);
-#endif
                                 if(btdName != NULL) {
                                         hci_write_local_name(btdName);
                                         hci_state = HCI_WRITE_NAME_STATE;
-                                } else
+                                } else {
-                                        hci_state = HCI_CHECK_DEVICE_SERVICE;
+                                        hci_read_local_extended_features(0); // "Requests the normal LMP features as returned by Read_Local_Supported_Features"
+                                        //hci_read_local_extended_features(1); // Read page 1
+                                        hci_state = HCI_LOCAL_EXTENDED_FEATURES_STATE;
+                                }
                         }
                         break;
 
@@ -837,6 +827,27 @@ void BTD::HCI_task() {
 #ifdef DEBUG_USB_HOST
                                 Notify(PSTR("\r\nThe name was set to: "), 0x80);
                                 NotifyStr(btdName, 0x80);
+#endif
+                                hci_read_local_extended_features(0); // "Requests the normal LMP features as returned by Read_Local_Supported_Features"
+                                //hci_read_local_extended_features(1); // Read page 1
+                                hci_state = HCI_LOCAL_EXTENDED_FEATURES_STATE;
+                        }
+                        break;
+
+                case HCI_LOCAL_EXTENDED_FEATURES_STATE:
+                        if(hci_check_flag(HCI_FLAG_LOCAL_EXTENDED_FEATURES)) {
+                                if(simple_pairing_supported) {
+                                        hci_write_simple_pairing_mode(true);
+                                        hci_state = HCI_WRITE_SIMPLE_PAIRING_STATE;
+                                } else
+                                        hci_state = HCI_CHECK_DEVICE_SERVICE;
+                        }
+                        break;
+
+                case HCI_WRITE_SIMPLE_PAIRING_STATE:
+                        if(hci_check_flag(HCI_FLAG_CMD_COMPLETE)) {
+#ifdef DEBUG_USB_HOST
+                                Notify(PSTR("\r\nSimple pairing was enabled"), 0x80);
 #endif
                                 hci_state = HCI_CHECK_DEVICE_SERVICE;
                         }

BTD.h

@@ -191,6 +191,17 @@
 #define HID_CTRL_PSM    0x11 // HID_Control PSM Value
 #define HID_INTR_PSM    0x13 // HID_Interrupt PSM Value
 
+/* Used for SDP */
+#define SDP_SERVICE_SEARCH_REQUEST                  0x02
+#define SDP_SERVICE_SEARCH_RESPONSE                 0x03
+#define SDP_SERVICE_ATTRIBUTE_REQUEST               0x04
+#define SDP_SERVICE_ATTRIBUTE_RESPONSE              0x05
+#define SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU    0x06 // See the RFCOMM specs
+#define SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU   0x07 // See the RFCOMM specs
+#define PNP_INFORMATION_UUID    0x1200
+#define SERIALPORT_UUID         0x1101 // See http://www.bluetooth.org/Technical/AssignedNumbers/service_discovery.htm
+#define L2CAP_UUID              0x0100
+
 // Used to determine if it is a Bluetooth dongle
 #define WI_SUBCLASS_RF      0x01 // RF Controller
 #define WI_PROTOCOL_BT      0x01 // Bluetooth Programming Interface
@@ -553,6 +564,7 @@ private:
         uint16_t PID, VID; // PID and VID of device connected
 
         uint8_t pollInterval;
+        bool simple_pairing_supported;
         bool bPollEnable;
 
         bool pairWiiUsingSync; // True if pairing was done using the Wii SYNC button.

BTHID.cpp

@@ -41,15 +41,20 @@ protocolMode(USB_HID_BOOT_PROTOCOL) {
 void BTHID::Reset() {
         connected = false;
         activeConnection = false;
+        SDPConnected = false;
         l2cap_event_flag = 0; // Reset flags
+        l2cap_sdp_state = L2CAP_SDP_WAIT;
         l2cap_state = L2CAP_WAIT;
         ResetBTHID();
 }
 
 void BTHID::disconnect() { // Use this void to disconnect the device
+        if(SDPConnected)
+                pBtd->l2cap_disconnection_request(hci_handle, ++identifier, sdp_scid, sdp_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_sdp_state = L2CAP_DISCONNECT_RESPONSE;
         l2cap_state = L2CAP_INTERRUPT_DISCONNECT;
 }
 
@@ -59,13 +64,22 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
         Notify(PSTR(" "), 0x80);
         D_PrintHex<uint8_t > (l2capinbuf[6], 0x80);
 
-        Notify(PSTR("\r\nData: "), 0x80);
+        Notify(PSTR(", data: "), 0x80);
-        Notify(PSTR("\r\n"), 0x80);
         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);
         }
 
+        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
+                        }
+                }
+        }
+
         if(!pBtd->l2capConnectionClaimed && pBtd->incomingHIDDevice && !connected && !activeConnection) {
                 if(l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) {
                         if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
@@ -124,7 +138,12 @@ 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)) == SDP_PSM) {
+                                        identifier = l2capinbuf[9];
+                                        sdp_scid[0] = l2capinbuf[14];
+                                        sdp_scid[1] = l2capinbuf[15];
+                                        l2cap_set_flag(L2CAP_FLAG_CONNECTION_SDP_REQUEST);
+                                } else if((l2capinbuf[12] | (l2capinbuf[13] << 8)) == HID_CTRL_PSM) {
                                         identifier = l2capinbuf[9];
                                         control_scid[0] = l2capinbuf[14];
                                         control_scid[1] = l2capinbuf[15];
@@ -137,7 +156,11 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                 }
                         } 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]) {
+                                        if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
+                                                //Notify(PSTR("\r\nSDP Configuration Complete"), 0x80);
+                                                identifier = l2capinbuf[9];
+                                                l2cap_set_flag(L2CAP_FLAG_CONFIG_SDP_SUCCESS);
+                                        } else if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
                                                 //Notify(PSTR("\r\nHID Control Configuration Complete"), 0x80);
                                                 identifier = l2capinbuf[9];
                                                 l2cap_set_flag(L2CAP_FLAG_CONFIG_CONTROL_SUCCESS);
@@ -148,7 +171,10 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                         }
                                 }
                         } else if(l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) {
-                                if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
+                                if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
+                                        //Notify(PSTR("\r\nSDP Configuration Request"), 0x80);
+                                        pBtd->l2cap_config_response(hci_handle, l2capinbuf[9], sdp_scid);
+                                } else 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]) {
@@ -156,7 +182,13 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                         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]) {
+                                if(l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) {
+#ifdef DEBUG_USB_HOST
+                                        Notify(PSTR("\r\nDisconnect Request: SDP Channel"), 0x80);
+#endif
+                                        identifier = l2capinbuf[9];
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_SDP_REQUEST);
+                                } else if(l2capinbuf[12] == control_dcid[0] && l2capinbuf[13] == control_dcid[1]) {
 #ifdef DEBUG_USB_HOST
                                         Notify(PSTR("\r\nDisconnect Request: Control Channel"), 0x80);
 #endif
@@ -172,7 +204,11 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                         Reset();
                                 }
                         } else if(l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) {
-                                if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
+                                if(l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) {
+                                        //Notify(PSTR("\r\nDisconnect Response: SDP Channel"), 0x80);
+                                        identifier = l2capinbuf[9];
+                                        l2cap_set_flag(L2CAP_FLAG_DISCONNECT_RESPONSE);
+                                } else if(l2capinbuf[12] == control_scid[0] && l2capinbuf[13] == control_scid[1]) {
                                         //Notify(PSTR("\r\nDisconnect Response: Control Channel"), 0x80);
                                         identifier = l2capinbuf[9];
                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE);
@@ -181,6 +217,12 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                         identifier = l2capinbuf[9];
                                         l2cap_set_flag(L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE);
                                 }
+                        } else if(l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) {
+#ifdef DEBUG_USB_HOST
+                                Notify(PSTR("\r\nInformation request"), 0x80);
+#endif
+                                identifier = l2capinbuf[9];
+                                pBtd->l2cap_information_response(hci_handle, identifier, l2capinbuf[12], l2capinbuf[13]);
                         }
 #ifdef EXTRADEBUG
                         else {
@@ -188,6 +230,97 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                                 Notify(PSTR("\r\nL2CAP Unknown Signaling Command: "), 0x80);
                                 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_REQUEST) {
+                                Notify(PSTR("\r\nSDP_SERVICE_SEARCH_REQUEST"), 0x80);
+
+                                // Send response
+                                l2capoutbuf[0] = SDP_SERVICE_SEARCH_RESPONSE;
+                                l2capoutbuf[1] = l2capinbuf[9];//transactionIDHigh;
+                                l2capoutbuf[2] = l2capinbuf[10];//transactionIDLow;
+#if 1
+                                l2capoutbuf[3] = 0x00; // MSB Parameter Length
+                                l2capoutbuf[4] = 0x05; // LSB Parameter Length = 5
+
+                                l2capoutbuf[5] = 0x00; // MSB TotalServiceRecordCount
+                                l2capoutbuf[6] = 0x00; // LSB TotalServiceRecordCount = 0
+
+                                l2capoutbuf[7] = 0x00; // MSB CurrentServiceRecordCount
+                                l2capoutbuf[8] = 0x00; // LSB CurrentServiceRecordCount = 0
+
+                                l2capoutbuf[9] = 0x00; // No continuation state
+
+                                SDP_Command(l2capoutbuf, 10);
+#else
+                                l2capoutbuf[3] = 0x00; // MSB Parameter Length
+                                l2capoutbuf[4] = 0x09; // LSB Parameter Length = 9
+
+                                l2capoutbuf[5] = 0x00; // MSB TotalServiceRecordCount
+                                l2capoutbuf[6] = 0x01; // LSB TotalServiceRecordCount = 1
+
+                                l2capoutbuf[7] = 0x00; // MSB CurrentServiceRecordCount
+                                l2capoutbuf[8] = 0x01; // LSB CurrentServiceRecordCount = 1
+
+                                l2capoutbuf[9] = 0x00; // ServiceRecordHandleList
+                                l2capoutbuf[10] = 0x01; // ServiceRecordHandleList
+                                l2capoutbuf[11] = 0x00; // ServiceRecordHandleList
+                                l2capoutbuf[12] = 0x01; // ServiceRecordHandleList: 0x10001.
+
+                                l2capoutbuf[13] = 0x00; // No continuation state
+
+                                SDP_Command(l2capoutbuf, 14);
+#endif
+                        } else if(l2capinbuf[8] == SDP_SERVICE_ATTRIBUTE_REQUEST) {
+                                Notify(PSTR("\r\nSDP_SERVICE_ATTRIBUTE_REQUEST"), 0x80);
+
+                                // Send response
+                                l2capoutbuf[0] = SDP_SERVICE_ATTRIBUTE_RESPONSE;
+                                l2capoutbuf[1] = l2capinbuf[9];//transactionIDHigh;
+                                l2capoutbuf[2] = l2capinbuf[10];//transactionIDLow;
+
+                                l2capoutbuf[3] = 0x00; // MSB Parameter Length
+                                l2capoutbuf[4] = 0x05; // LSB Parameter Length = 5
+
+                                l2capoutbuf[5] = 0x00; // MSB AttributeListByteCount
+                                l2capoutbuf[6] = 0x02; // LSB AttributeListByteCount = 2
+
+                                // TODO: What to send?
+                                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);
+                        } else if(l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) {
+                                Notify(PSTR("\r\nSDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU"), 0x80);
+
+#ifdef EXTRADEBUG
+                                Notify(PSTR("\r\nUUID: "), 0x80);
+                                uint16_t uuid;
+                                if((l2capinbuf[16] << 8 | l2capinbuf[17]) == 0x0000) // Check if it's sending the UUID as a 128-bit UUID
+                                        uuid = (l2capinbuf[18] << 8 | l2capinbuf[19]);
+                                else // Short UUID
+                                        uuid = (l2capinbuf[16] << 8 | l2capinbuf[17]);
+                                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);
+                                Notify(PSTR("\r\nData: "), 0x80);
+                                for(uint8_t i = 0; i < length; i++) {
+                                        D_PrintHex<uint8_t > (l2capinbuf[13 + i], 0x80);
+                                        Notify(PSTR(" "), 0x80);
+                                }
+#endif
+
+                                serviceNotSupported(l2capinbuf[9], l2capinbuf[10]); // The service is not supported
+                        }
+#ifdef EXTRADEBUG
+                        else {
+                                Notify(PSTR("\r\nUnknown PDU: "), 0x80);
+                                D_PrintHex<uint8_t > (l2capinbuf[8], 0x80);
+                        }
 #endif
                 } else if(l2capinbuf[6] == interrupt_dcid[0] && l2capinbuf[7] == interrupt_dcid[1]) { // l2cap_interrupt
 #ifdef PRINTREPORT
@@ -243,10 +376,59 @@ void BTHID::ACLData(uint8_t* l2capinbuf) {
                         }
                 }
 #endif
+                SDP_task();
                 L2CAP_task();
         }
 }
 
+void BTHID::SDP_task() {
+        switch(l2cap_sdp_state) {
+                case L2CAP_SDP_WAIT:
+                        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
+                                pBtd->l2cap_connection_response(hci_handle, identifier, sdp_dcid, sdp_scid, PENDING);
+                                delay(1);
+                                pBtd->l2cap_connection_response(hci_handle, identifier, sdp_dcid, sdp_scid, SUCCESSFUL);
+                                identifier++;
+                                delay(1);
+                                pBtd->l2cap_config_request(hci_handle, identifier, sdp_scid);
+                                l2cap_sdp_state = L2CAP_SDP_SUCCESS;
+                        } 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);
+                        }
+                        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
+                                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_check_flag(L2CAP_FLAG_DISCONNECT_RESPONSE)) {
+#ifdef DEBUG_USB_HOST
+                                Notify(PSTR("\r\nDisconnected L2CAP Connection"), 0x80);
+#endif
+                                pBtd->hci_disconnect(hci_handle);
+                                hci_handle = -1; // Reset handle
+                                Reset();
+                        }
+                        break;
+        }
+}
+
 void BTHID::L2CAP_task() {
         switch(l2cap_state) {
                         /* These states are used if the HID device is the host */
@@ -383,6 +565,27 @@ void BTHID::Run() {
         }
 }
 
+void BTHID::SDP_Command(uint8_t* data, uint8_t nbytes) { // See page 223 in the Bluetooth specs
+        pBtd->L2CAP_Command(hci_handle, data, nbytes, sdp_scid[0], sdp_scid[1]);
+}
+
+void BTHID::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow) { // See page 235 in the Bluetooth specs
+        l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU;
+        l2capoutbuf[1] = transactionIDHigh;
+        l2capoutbuf[2] = transactionIDLow;
+        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; // Data element sequence - length in next byte
+        l2capoutbuf[8] = 0x00; // Length = 0
+        l2capoutbuf[9] = 0x00; // No continuation state
+
+        SDP_Command(l2capoutbuf, 10);
+}
+
 /************************************************************/
 /*                    HID Commands                          */
 

BTHID.h

@@ -141,20 +141,30 @@ protected:
         /** L2CAP source CID for HID_Interrupt */
         uint8_t interrupt_scid[2];
 
+        uint8_t l2cap_sdp_state;
+        uint8_t sdp_scid[2]; // L2CAP source CID for SDP
+
 private:
         HIDReportParser *pRptParser[NUM_PARSERS]; // Pointer to HIDReportParsers.
 
+        uint8_t l2capoutbuf[BULK_MAXPKTSIZE]; // General purpose buffer for l2cap out data
+        void SDP_Command(uint8_t* data, uint8_t nbytes);
+        void serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow);
+
         /** Set report protocol. */
         void setProtocol();
         uint8_t protocolMode;
 
+        void SDP_task();
         void L2CAP_task(); // L2CAP state machine
 
         bool activeConnection; // Used to indicate if it already has established a connection
+        bool SDPConnected;
 
         /* 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 sdp_dcid[2];
         uint8_t l2cap_state;
 };
 #endif

SPP.h

@@ -20,12 +20,6 @@
 
 #include "BTD.h"
 
-/* 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
-#define SERIALPORT_UUID     0x1101 // See http://www.bluetooth.org/Technical/AssignedNumbers/service_discovery.htm
-#define L2CAP_UUID          0x0100
-
 /* Used for RFCOMM */
 #define RFCOMM_SABM     0x2F
 #define RFCOMM_UA       0x63