From 6c1832bd3a702e1dca593493f5e71313ee052970 Mon Sep 17 00:00:00 2001 From: Kristian Sloth Lauszus Date: Sun, 19 Dec 2021 18:41:04 +0100 Subject: [PATCH] Parse Extended Inquiry Result events and fix parsing class of device when more than one response was returned in a Inquiry Result event --- BTD.cpp | 15 ++++++++++----- BTD.h | 1 + 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/BTD.cpp b/BTD.cpp index 436a0cb3..d9817cd3 100644 --- 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 index a070cb58..3cd54a6c 100644 --- 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