[Bluetooth]: Workaround for inquiry results with RSSI and page scan mode
This patch implements a workaround for buggy Bluetooth 1.2 devices from
Silicon Wave. Their inquiry results with RSSI contain the page scan mode
field. This field was removed in the final Bluetooth 1.2 specification.
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
Signed-off-by: David S. Miller <davem@davemloft.net>
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 6f0706f..cd075f1 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -453,6 +453,15 @@
__u16 clock_offset;
__s8 rssi;
} __attribute__ ((packed));
+struct inquiry_info_with_rssi_and_pscan_mode {
+ bdaddr_t bdaddr;
+ __u8 pscan_rep_mode;
+ __u8 pscan_period_mode;
+ __u8 pscan_mode;
+ __u8 dev_class[3];
+ __u16 clock_offset;
+ __s8 rssi;
+} __attribute__ ((packed));
#define HCI_EV_CONN_COMPLETE 0x03
struct hci_ev_conn_complete {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 46367bd..632f7a9 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -484,14 +484,18 @@
/* Inquiry Result */
static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
+ struct inquiry_data data;
struct inquiry_info *info = (struct inquiry_info *) (skb->data + 1);
int num_rsp = *((__u8 *) skb->data);
BT_DBG("%s num_rsp %d", hdev->name, num_rsp);
+ if (!num_rsp)
+ return;
+
hci_dev_lock(hdev);
+
for (; num_rsp; num_rsp--) {
- struct inquiry_data data;
bacpy(&data.bdaddr, &info->bdaddr);
data.pscan_rep_mode = info->pscan_rep_mode;
data.pscan_period_mode = info->pscan_period_mode;
@@ -502,30 +506,55 @@
info++;
hci_inquiry_cache_update(hdev, &data);
}
+
hci_dev_unlock(hdev);
}
/* Inquiry Result With RSSI */
static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
- struct inquiry_info_with_rssi *info = (struct inquiry_info_with_rssi *) (skb->data + 1);
+ struct inquiry_data data;
int num_rsp = *((__u8 *) skb->data);
BT_DBG("%s num_rsp %d", hdev->name, num_rsp);
+ if (!num_rsp)
+ return;
+
hci_dev_lock(hdev);
- for (; num_rsp; num_rsp--) {
- struct inquiry_data data;
- bacpy(&data.bdaddr, &info->bdaddr);
- data.pscan_rep_mode = info->pscan_rep_mode;
- data.pscan_period_mode = info->pscan_period_mode;
- data.pscan_mode = 0x00;
- memcpy(data.dev_class, info->dev_class, 3);
- data.clock_offset = info->clock_offset;
- data.rssi = info->rssi;
- info++;
- hci_inquiry_cache_update(hdev, &data);
+
+ if ((skb->len - 1) / num_rsp != sizeof(struct inquiry_info_with_rssi)) {
+ struct inquiry_info_with_rssi_and_pscan_mode *info =
+ (struct inquiry_info_with_rssi_and_pscan_mode *) (skb->data + 1);
+
+ for (; num_rsp; num_rsp--) {
+ bacpy(&data.bdaddr, &info->bdaddr);
+ data.pscan_rep_mode = info->pscan_rep_mode;
+ data.pscan_period_mode = info->pscan_period_mode;
+ data.pscan_mode = info->pscan_mode;
+ memcpy(data.dev_class, info->dev_class, 3);
+ data.clock_offset = info->clock_offset;
+ data.rssi = info->rssi;
+ info++;
+ hci_inquiry_cache_update(hdev, &data);
+ }
+ } else {
+ struct inquiry_info_with_rssi *info =
+ (struct inquiry_info_with_rssi *) (skb->data + 1);
+
+ for (; num_rsp; num_rsp--) {
+ bacpy(&data.bdaddr, &info->bdaddr);
+ data.pscan_rep_mode = info->pscan_rep_mode;
+ data.pscan_period_mode = info->pscan_period_mode;
+ data.pscan_mode = 0x00;
+ memcpy(data.dev_class, info->dev_class, 3);
+ data.clock_offset = info->clock_offset;
+ data.rssi = info->rssi;
+ info++;
+ hci_inquiry_cache_update(hdev, &data);
+ }
}
+
hci_dev_unlock(hdev);
}