bluetooth: Add support for HCI_EVENT_INQUIRY_RESULT_WITH_RSSI

Follow up of a4e1f4d8ce, details given
in that commit message on parsing of this event message

Change-Id: Icc0b54bdafe3cb167553d2fda3f62762d2d2595e
Reviewed-on: https://review.haiku-os.org/c/haiku/+/10485
Reviewed-by: waddlesplash <[email protected]>

headers/os/bluetooth/HCI/btHCI_event.h

@@ -21,14 +21,6 @@ struct hci_event_header {
 #define HCI_EVENT_INQUIRY_COMPLETE					0x01
 
 #define HCI_EVENT_INQUIRY_RESULT					0x02
-struct inquiry_info {
-	bdaddr_t	bdaddr;
-	uint8		pscan_rep_mode;
-	uint8		pscan_period_mode;
-	uint8		pscan_mode;
-	uint8		dev_class[3];
-	uint16		clock_offset;
-} __attribute__ ((packed));
 
 #define HCI_EVENT_CONN_COMPLETE						0x03
 struct hci_ev_conn_complete {
@@ -248,14 +240,7 @@ struct hci_ev_flow_specification {
 } __attribute__ ((packed));
 
 #define HCI_EVENT_INQUIRY_RESULT_WITH_RSSI			0x22
-struct hci_ev_inquiry_info_with_rssi {
+#define HCI_RSSI_INVALID 127
-	bdaddr_t bdaddr;
-	uint8		pscan_rep_mode;
-	uint8		pscan_period_mode;
-	uint8		dev_class[3];
-	uint16		clock_offset;
-	int8		rssi;
-} __attribute__ ((packed));
 
 #define HCI_EVENT_REMOTE_EXTENDED_FEATURES			0x23
 struct hci_ev_remote_extended_features {

headers/os/bluetooth/RemoteDevice.h

@@ -68,7 +68,7 @@ private:
 	uint8		fScanPeriodMode;
 	uint8		fScanMode;
 	uint16		fClockOffset;
-
+	int8        fRSSI;
 };
 
 }

src/kits/bluetooth/DiscoveryAgent.cpp

@@ -82,6 +82,7 @@ DiscoveryAgent::StartInquiry(uint32 accessCode, DiscoveryListener* listener,
 
 	// For getting each discovered message
     request.AddInt16("eventExpected",  HCI_EVENT_INQUIRY_RESULT);
+	request.AddInt16("eventExpected", HCI_EVENT_INQUIRY_RESULT_WITH_RSSI);
 
 	// For finishing each discovered message
     request.AddInt16("eventExpected",  HCI_EVENT_INQUIRY_COMPLETE);

src/kits/bluetooth/DiscoveryListener.cpp

@@ -83,9 +83,11 @@ DiscoveryListener::MessageReceived(BMessage* message)
 				const uint8* devClass;
 				uint8 pageRepetitionMode = 0;
 				uint8 scanPeriodMode = 0;
+				// default value is 0 only, in newer specs this has been removed in such case it
+				// should be set to zero
 				uint8 scanMode = 0;
 				uint16 clockOffset = 0;
-				bool duplicatedFound = false;
+				int8 rssi = HCI_RSSI_INVALID;
 
 				if (message->FindData("bdaddr", B_ANY_TYPE, i, (const void**)&bdaddr, &size) != B_OK
 					|| message->FindData("dev_class", B_ANY_TYPE, i, (const void**)&devClass, &size)
@@ -95,10 +97,14 @@ DiscoveryListener::MessageReceived(BMessage* message)
 
 				message->FindUInt8("page_repetition_mode", i, &pageRepetitionMode);
 				message->FindUInt8("scan_period_mode", i, &scanPeriodMode);
+
+				// if not present, the default value of these fields will be used
 				message->FindUInt8("scan_mode", i, &scanMode);
 				message->FindUInt16("clock_offset", i, &clockOffset);
+				message->FindInt8("rssi", i, &rssi);
 
 				// Skip duplicated replies
+				bool duplicatedFound = false;
 				for (int32 index = 0; index < fRemoteDevicesList.CountItems(); index++) {
 					RemoteDevice* existingDevice = fRemoteDevicesList.ItemAt(index);
 					bdaddr_t b1 = existingDevice->GetBluetoothAddress();
@@ -108,6 +114,7 @@ DiscoveryListener::MessageReceived(BMessage* message)
 						existingDevice->fScanPeriodMode = scanPeriodMode;
 						existingDevice->fScanMode = scanMode;
 						existingDevice->fClockOffset = clockOffset;
+						existingDevice->fRSSI = rssi;
 						duplicatedFound = true;
 						break;
 					}
@@ -122,6 +129,7 @@ DiscoveryListener::MessageReceived(BMessage* message)
 					rd->fScanPeriodMode = scanPeriodMode;
 					rd->fScanMode = scanMode;
 					rd->fClockOffset = clockOffset;
+					rd->fRSSI = rssi;
 					DeviceDiscovered(rd, rd->GetDeviceClass());
 				}
 			}

src/servers/bluetooth/LocalDeviceImpl.cpp

@@ -253,6 +253,7 @@ LocalDeviceImpl::HandleExpectedRequest(struct hci_event_header* event,
 			break;
 
 		case HCI_EVENT_INQUIRY_RESULT_WITH_RSSI:
+			InquiryResultWithRSSI(JumpEventHeader<uint8>(event), request);
 			break;
 
 		case HCI_EVENT_REMOTE_EXTENDED_FEATURES:
@@ -820,6 +821,47 @@ LocalDeviceImpl::InquiryResult(uint8* numberOfResponses, BMessage* request)
 }
 
 
+void
+LocalDeviceImpl::InquiryResultWithRSSI(uint8* numberOfResponses, BMessage* request)
+{
+	uint8 count = *numberOfResponses;
+	TRACE_BT("LocalDeviceImpl: %s #responses=%d\n", __FUNCTION__, count);
+	if (count == 0 || request == NULL)
+		return;
+
+	uint8* base_ptr = numberOfResponses + 1;
+
+	// get pointers ready for the parallel arrays
+	bdaddr_t* bdaddr_array = (bdaddr_t*)base_ptr;
+	uint8* page_repetition_mode_array = (uint8*)(bdaddr_array + count);
+	uint8* scan_period_mode_array = page_repetition_mode_array + count;
+	uint8* dev_class_array = scan_period_mode_array + count;
+	uint16* clock_offset_array = (uint16*)(dev_class_array + (count * 3));
+	int8* rssi_array = (int8*)(clock_offset_array + count);
+
+	BMessage reply(BT_MSG_INQUIRY_DEVICE);
+	reply.AddUInt8("count", count);
+
+	for (uint8 i = 0; i < count; i++) {
+		TRACE_BT("LocalDeviceImpl: page_rep=%d scan_period=%d clock=%d rssi=%d\n",
+			page_repetition_mode_array[i], scan_period_mode_array[i], clock_offset_array[i],
+			rssi_array[i]);
+
+		reply.AddData("bdaddr", B_ANY_TYPE, &bdaddr_array[i], sizeof(bdaddr_t));
+		reply.AddData("dev_class", B_ANY_TYPE, &dev_class_array[i * 3], 3);
+		reply.AddUInt8("page_repetition_mode", page_repetition_mode_array[i]);
+		reply.AddUInt8("scan_period_mode", scan_period_mode_array[i]);
+		reply.AddUInt16("clock_offset", clock_offset_array[i]);
+		reply.AddInt8("rssi", rssi_array[i]);
+	}
+
+	printf("%s: Sending reply...\n", __func__);
+	status_t status = request->SendReply(&reply);
+	if (status < B_OK)
+		printf("%s: Error sending reply!\n", __func__);
+}
+
+
 void
 LocalDeviceImpl::InquiryComplete(uint8* status, BMessage* request)
 {

src/servers/bluetooth/LocalDeviceImpl.h

@@ -48,6 +48,7 @@ private:
 
 	// Inquiry
 	void InquiryResult(uint8* numberOfResponses, BMessage* request);
+	void InquiryResultWithRSSI(uint8* numberOfResponses, BMessage* request);
 	void InquiryComplete(uint8* status, BMessage* request);
 	void RemoteNameRequestComplete(struct hci_ev_remote_name_request_complete_reply*
 		remotename, BMessage* request);