4 files changed, 60 insertions, 5 deletions

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 6d0c04a81fc7..5ac0a18db63c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -816,6 +816,12 @@ struct hci_ev_simple_pair_complete {
         bdaddr_t bdaddr;
 } __attribute__ ((packed));
 
+#define HCI_EV_REMOTE_HOST_FEATURES     0x3d
+struct hci_ev_remote_host_features {
+        bdaddr_t bdaddr;
+        __u8     features[8];
+} __attribute__ ((packed));
+
 /* Internal events generated by Bluetooth stack */
 #define HCI_EV_STACK_INTERNAL   0xfd
 struct hci_ev_stack_internal {
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b85754e29a78..f73cc2945700 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -40,6 +40,7 @@ struct inquiry_data {
         __u8            dev_class[3];
         __le16          clock_offset;
         __s8            rssi;
+        __u8            ssp_mode;
 };
 
 struct inquiry_entry {
@@ -162,6 +163,7 @@ struct hci_conn {
         __u8             attempt;
         __u8             dev_class[3];
         __u8             features[8];
+        __u8             ssp_mode;
         __u16            interval;
         __u16            pkt_type;
         __u16            link_policy;
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 6175ce841e9e..41351ba692e9 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -71,12 +71,16 @@ void hci_acl_connect(struct hci_conn *conn)
         bacpy(&cp.bdaddr, &conn->dst);
         cp.pscan_rep_mode = 0x02;
 
-        if ((ie = hci_inquiry_cache_lookup(hdev, &conn->dst)) &&
-                        inquiry_entry_age(ie) <= INQUIRY_ENTRY_AGE_MAX) {
-                cp.pscan_rep_mode = ie->data.pscan_rep_mode;
-                cp.pscan_mode     = ie->data.pscan_mode;
-                cp.clock_offset   = ie->data.clock_offset | cpu_to_le16(0x8000);
+        if ((ie = hci_inquiry_cache_lookup(hdev, &conn->dst))) {
+                if (inquiry_entry_age(ie) <= INQUIRY_ENTRY_AGE_MAX) {
+                        cp.pscan_rep_mode = ie->data.pscan_rep_mode;
+                        cp.pscan_mode     = ie->data.pscan_mode;
+                        cp.clock_offset   = ie->data.clock_offset |
+                                                        cpu_to_le16(0x8000);
+                }
+
                 memcpy(conn->dev_class, ie->data.dev_class, 3);
+                conn->ssp_mode = ie->data.ssp_mode;
         }
 
         cp.pkt_type = cpu_to_le16(conn->pkt_type);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 6077a651aac6..c8fda7dc2986 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -736,6 +736,7 @@ static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *
                 memcpy(data.dev_class, info->dev_class, 3);
                 data.clock_offset       = info->clock_offset;
                 data.rssi               = 0x00;
+                data.ssp_mode           = 0x00;
                 info++;
                 hci_inquiry_cache_update(hdev, &data);
         }
@@ -1390,6 +1391,7 @@ static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct
                         memcpy(data.dev_class, info->dev_class, 3);
                         data.clock_offset       = info->clock_offset;
                         data.rssi               = info->rssi;
+                        data.ssp_mode           = 0x00;
                         info++;
                         hci_inquiry_cache_update(hdev, &data);
                 }
@@ -1404,6 +1406,7 @@ static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct
                         memcpy(data.dev_class, info->dev_class, 3);
                         data.clock_offset       = info->clock_offset;
                         data.rssi               = info->rssi;
+                        data.ssp_mode           = 0x00;
                         info++;
                         hci_inquiry_cache_update(hdev, &data);
                 }
@@ -1414,7 +1417,27 @@ static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct
 
 static inline void hci_remote_ext_features_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
+        struct hci_ev_remote_ext_features *ev = (void *) skb->data;
+        struct hci_conn *conn;
+
         BT_DBG("%s", hdev->name);
+
+        if (ev->status || ev->page != 0x01)
+                return;
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
+        if (conn) {
+                struct inquiry_entry *ie;
+
+                if ((ie = hci_inquiry_cache_lookup(hdev, &conn->dst)))
+                        ie->data.ssp_mode = (ev->features[0] & 0x01);
+
+                conn->ssp_mode = (ev->features[0] & 0x01);
+        }
+
+        hci_dev_unlock(hdev);
 }
 
 static inline void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
@@ -1494,6 +1517,7 @@ static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct
                 memcpy(data.dev_class, info->dev_class, 3);
                 data.clock_offset       = info->clock_offset;
                 data.rssi               = info->rssi;
+                data.ssp_mode           = 0x01;
                 info++;
                 hci_inquiry_cache_update(hdev, &data);
         }
@@ -1533,6 +1557,21 @@ static inline void hci_simple_pair_complete_evt(struct hci_dev *hdev, struct sk_
         hci_dev_unlock(hdev);
 }
 
+static inline void hci_remote_host_features_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_remote_host_features *ev = (void *) skb->data;
+        struct inquiry_entry *ie;
+
+        BT_DBG("%s", hdev->name);
+
+        hci_dev_lock(hdev);
+
+        if ((ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr)))
+                ie->data.ssp_mode = (ev->features[0] & 0x01);
+
+        hci_dev_unlock(hdev);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_event_hdr *hdr = (void *) skb->data;
@@ -1665,6 +1704,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_simple_pair_complete_evt(hdev, skb);
                 break;
 
+        case HCI_EV_REMOTE_HOST_FEATURES:
+                hci_remote_host_features_evt(hdev, skb);
+                break;
+
         default:
                 BT_DBG("%s event 0x%x", hdev->name, event);
                 break;