[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 6f0706f4af68178db7852aa15c728822aefa4a80..cd075f19798362bde43fdf7dd27efab90d12bda2 100644 (file)
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -453,6 +453,15 @@ struct inquiry_info_with_rssi {
        __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 46367bd129c34d14df31b23437b5af0d2bc3089d..632f7a9c2bcb9b81b8a43ecad82f8eedc08289c4 100644 (file)
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -484,14 +484,18 @@ static inline void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff
 /* 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 @@ static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *
                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);
 }