Merge pull request #681 from felis/parse_extended_result

Parse Extended Inquiry Result events and fix parsing class of device …
felis · Dec 19, 2021 · 651d7b0 · 651d7b0
2 parents e33c3fa + 6c1832b
commit 651d7b0
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 5 deletions.
diff --git a/BTD.cpp b/BTD.cpp
@@ -504,16 +504,21 @@ void BTD::HCI_event_task() {
                                 break;
 
                         case EV_INQUIRY_RESULT:
+                        case EV_EXTENDED_INQUIRY_RESULT:
                                 if(hcibuf[2]) { // Check that there is more than zero responses
 #ifdef EXTRADEBUG
                                         Notify(PSTR("\r\nNumber of responses: "), 0x80);
-                                        Notify(hcibuf[2], 0x80);
+                                        Notify(hcibuf[2], 0x80); // This will always be 1 for an extended inquiry result
 #endif
                                         for(uint8_t i = 0; i < hcibuf[2]; i++) {
-                                                uint8_t offset = 8 * hcibuf[2] + 3 * i;
+                                                uint8_t classOfDevice_offset;
+                                                if(hcibuf[0] == EV_INQUIRY_RESULT)
+                                                        classOfDevice_offset = 9 * hcibuf[2]; // 6-byte bd_addr, 1 byte page_scan_repetition_mode, 2 byte reserved
+                                                else
+                                                        classOfDevice_offset = 8 * hcibuf[2]; // 6-byte bd_addr, 1 byte page_scan_repetition_mode, 1 byte reserved
 
                                                 for(uint8_t j = 0; j < 3; j++)
-                                                        classOfDevice[j] = hcibuf[j + 4 + offset];
+                                                        classOfDevice[j] = hcibuf[3 + classOfDevice_offset + 3 * i + j];
 
 #ifdef EXTRADEBUG
                                                 Notify(PSTR("\r\nClass of device: "), 0x80);
@@ -528,7 +533,7 @@ void BTD::HCI_event_task() {
                                                         checkRemoteName = true; // Check remote name to distinguish between the different controllers
 
                                                         for(uint8_t j = 0; j < 6; j++)
-                                                                disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];
+                                                                disc_bdaddr[j] = hcibuf[3 + 6 * i + j];
 
                                                         hci_set_flag(HCI_FLAG_DEVICE_FOUND);
                                                         break;
@@ -544,7 +549,7 @@ void BTD::HCI_event_task() {
                                                                 Notify(PSTR("\r\nGamepad found"), 0x80);
 #endif
                                                         for(uint8_t j = 0; j < 6; j++)
-                                                                disc_bdaddr[j] = hcibuf[j + 3 + 6 * i];
+                                                                disc_bdaddr[j] = hcibuf[3 + 6 * i + j];
 
                                                         hci_set_flag(HCI_FLAG_DEVICE_FOUND);
                                                         break;

diff --git a/BTD.h b/BTD.h
@@ -104,6 +104,7 @@
 #define EV_LOOPBACK_COMMAND                             0x19
 #define EV_PAGE_SCAN_REP_MODE                           0x20
 #define EV_READ_REMOTE_EXTENDED_FEATURES_COMPLETE       0x23
+#define EV_EXTENDED_INQUIRY_RESULT                      0x2F
 #define EV_IO_CAPABILITY_REQUEST                        0x31
 #define EV_IO_CAPABILITY_RESPONSE                       0x32
 #define EV_USER_CONFIRMATION_REQUEST                    0x33