1 files changed, 85 insertions, 2 deletions

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index ac2c5e89617c..a40170e022e8 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -45,6 +45,8 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 
+static int enable_le;
+
 /* Handle HCI Event packets */
 
 static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb)
@@ -525,6 +527,20 @@ static void hci_setup_event_mask(struct hci_dev *hdev)
         hci_send_cmd(hdev, HCI_OP_SET_EVENT_MASK, sizeof(events), events);
 }
 
+static void hci_set_le_support(struct hci_dev *hdev)
+{
+        struct hci_cp_write_le_host_supported cp;
+
+        memset(&cp, 0, sizeof(cp));
+
+        if (enable_le) {
+                cp.le = 1;
+                cp.simul = !!(hdev->features[6] & LMP_SIMUL_LE_BR);
+        }
+
+        hci_send_cmd(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(cp), &cp);
+}
+
 static void hci_setup(struct hci_dev *hdev)
 {
         hci_setup_event_mask(hdev);
@@ -542,6 +558,17 @@ static void hci_setup(struct hci_dev *hdev)
 
         if (hdev->features[7] & LMP_INQ_TX_PWR)
                 hci_send_cmd(hdev, HCI_OP_READ_INQ_RSP_TX_POWER, 0, NULL);
+
+        if (hdev->features[7] & LMP_EXTFEATURES) {
+                struct hci_cp_read_local_ext_features cp;
+
+                cp.page = 0x01;
+                hci_send_cmd(hdev, HCI_OP_READ_LOCAL_EXT_FEATURES,
+                                                        sizeof(cp), &cp);
+        }
+
+        if (hdev->features[4] & LMP_LE)
+                hci_set_le_support(hdev);
 }
 
 static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb)
@@ -658,6 +685,21 @@ static void hci_cc_read_local_features(struct hci_dev *hdev, struct sk_buff *skb
                                         hdev->features[6], hdev->features[7]);
 }
 
+static void hci_cc_read_local_ext_features(struct hci_dev *hdev,
+                                                        struct sk_buff *skb)
+{
+        struct hci_rp_read_local_ext_features *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+        if (rp->status)
+                return;
+
+        memcpy(hdev->extfeatures, rp->features, 8);
+
+        hci_req_complete(hdev, HCI_OP_READ_LOCAL_EXT_FEATURES, rp->status);
+}
+
 static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_rp_read_buffer_size *rp = (void *) skb->data;
@@ -892,6 +934,21 @@ static void hci_cc_le_ltk_neg_reply(struct hci_dev *hdev, struct sk_buff *skb)
         hci_req_complete(hdev, HCI_OP_LE_LTK_NEG_REPLY, rp->status);
 }
 
+static inline void hci_cc_write_le_host_supported(struct hci_dev *hdev,
+                                                        struct sk_buff *skb)
+{
+        struct hci_cp_read_local_ext_features cp;
+        __u8 status = *((__u8 *) skb->data);
+
+        BT_DBG("%s status 0x%x", hdev->name, status);
+
+        if (status)
+                return;
+
+        cp.page = 0x01;
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_EXT_FEATURES, sizeof(cp), &cp);
+}
+
 static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
         BT_DBG("%s status 0x%x", hdev->name, status);
@@ -1826,6 +1883,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
                 hci_cc_read_local_features(hdev, skb);
                 break;
 
+        case HCI_OP_READ_LOCAL_EXT_FEATURES:
+                hci_cc_read_local_ext_features(hdev, skb);
+                break;
+
         case HCI_OP_READ_BUFFER_SIZE:
                 hci_cc_read_buffer_size(hdev, skb);
                 break;
@@ -1894,6 +1955,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
                 hci_cc_le_ltk_neg_reply(hdev, skb);
                 break;
 
+        case HCI_OP_WRITE_LE_HOST_SUPPORTED:
+                hci_cc_write_le_host_supported(hdev, skb);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%x", hdev->name, opcode);
                 break;
@@ -2793,21 +2858,36 @@ static inline void hci_le_ltk_request_evt(struct hci_dev *hdev,
 {
         struct hci_ev_le_ltk_req *ev = (void *) skb->data;
         struct hci_cp_le_ltk_reply cp;
+        struct hci_cp_le_ltk_neg_reply neg;
         struct hci_conn *conn;
+        struct link_key *ltk;
 
         BT_DBG("%s handle %d", hdev->name, cpu_to_le16(ev->handle));
 
         hci_dev_lock(hdev);
 
         conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
+        if (conn == NULL)
+                goto not_found;
 
-        memset(&cp, 0, sizeof(cp));
+        ltk = hci_find_ltk(hdev, ev->ediv, ev->random);
+        if (ltk == NULL)
+                goto not_found;
+
+        memcpy(cp.ltk, ltk->val, sizeof(ltk->val));
         cp.handle = cpu_to_le16(conn->handle);
-        memcpy(cp.ltk, conn->ltk, sizeof(conn->ltk));
+        conn->pin_length = ltk->pin_len;
 
         hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp);
 
         hci_dev_unlock(hdev);
+
+        return;
+
+not_found:
+        neg.handle = ev->handle;
+        hci_send_cmd(hdev, HCI_OP_LE_LTK_NEG_REPLY, sizeof(neg), &neg);
+        hci_dev_unlock(hdev);
 }
 
 static inline void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
@@ -3022,3 +3102,6 @@ void hci_si_event(struct hci_dev *hdev, int type, int dlen, void *data)
         hci_send_to_sock(hdev, skb, NULL);
         kfree_skb(skb);
 }
+
+module_param(enable_le, bool, 0444);
+MODULE_PARM_DESC(enable_le, "Enable LE support");