Bluetooth: Add support for LE Start Encryption

This adds support for starting SMP Phase 2 Encryption, when the initial
SMP negotiation is successful. This adds the LE Start Encryption and LE
Long Term Key Request commands and related events.

Signed-off-by: Vinicius Costa Gomes <vinicius.gomes@openbossa.org>
Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>

3 files changed, 128 insertions, 4 deletions

diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 37f5a174f072..18193831bbf1 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -204,6 +204,55 @@ void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max,
 }
 EXPORT_SYMBOL(hci_le_conn_update);
 
+void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8],
+                                                        __u8 ltk[16])
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct hci_cp_le_start_enc cp;
+
+        BT_DBG("%p", conn);
+
+        memset(&cp, 0, sizeof(cp));
+
+        cp.handle = cpu_to_le16(conn->handle);
+        memcpy(cp.ltk, ltk, sizeof(cp.ltk));
+        cp.ediv = ediv;
+        memcpy(cp.rand, rand, sizeof(rand));
+
+        hci_send_cmd(hdev, HCI_OP_LE_START_ENC, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_le_start_enc);
+
+void hci_le_ltk_reply(struct hci_conn *conn, u8 ltk[16])
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct hci_cp_le_ltk_reply cp;
+
+        BT_DBG("%p", conn);
+
+        memset(&cp, 0, sizeof(cp));
+
+        cp.handle = cpu_to_le16(conn->handle);
+        memcpy(cp.ltk, ltk, sizeof(ltk));
+
+        hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_le_ltk_reply);
+
+void hci_le_ltk_neg_reply(struct hci_conn *conn)
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct hci_cp_le_ltk_neg_reply cp;
+
+        BT_DBG("%p", conn);
+
+        memset(&cp, 0, sizeof(cp));
+
+        cp.handle = cpu_to_le16(conn->handle);
+
+        hci_send_cmd(hdev, HCI_OP_LE_LTK_NEG_REPLY, sizeof(cp), &cp);
+}
+
 /* Device _must_ be locked */
 void hci_sco_setup(struct hci_conn *conn, __u8 status)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0f643f84131f..166fa113721c 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -868,6 +868,30 @@ static void hci_cc_le_set_scan_enable(struct hci_dev *hdev,
         hci_dev_unlock(hdev);
 }
 
+static void hci_cc_le_ltk_reply(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_rp_le_ltk_reply *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+        if (rp->status)
+                return;
+
+        hci_req_complete(hdev, HCI_OP_LE_LTK_REPLY, rp->status);
+}
+
+static void hci_cc_le_ltk_neg_reply(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_rp_le_ltk_neg_reply *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+        if (rp->status)
+                return;
+
+        hci_req_complete(hdev, HCI_OP_LE_LTK_NEG_REPLY, rp->status);
+}
+
 static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
         BT_DBG("%s status 0x%x", hdev->name, status);
@@ -1248,6 +1272,11 @@ static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status)
         hci_dev_unlock(hdev);
 }
 
+static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
+{
+        BT_DBG("%s status 0x%x", hdev->name, status);
+}
+
 static inline void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
@@ -1856,6 +1885,14 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
                 hci_cc_le_set_scan_enable(hdev, skb);
                 break;
 
+        case HCI_OP_LE_LTK_REPLY:
+                hci_cc_le_ltk_reply(hdev, skb);
+                break;
+
+        case HCI_OP_LE_LTK_NEG_REPLY:
+                hci_cc_le_ltk_neg_reply(hdev, skb);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%x", hdev->name, opcode);
                 break;
@@ -1934,6 +1971,10 @@ static inline void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cs_le_create_conn(hdev, ev->status);
                 break;
 
+        case HCI_OP_LE_START_ENC:
+                hci_cs_le_start_enc(hdev, ev->status);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%x", hdev->name, opcode);
                 break;
@@ -2745,6 +2786,28 @@ static inline void hci_le_adv_report_evt(struct hci_dev *hdev,
         hci_dev_unlock(hdev);
 }
 
+static inline void hci_le_ltk_request_evt(struct hci_dev *hdev,
+                                                struct sk_buff *skb)
+{
+        struct hci_ev_le_ltk_req *ev = (void *) skb->data;
+        struct hci_cp_le_ltk_reply cp;
+        struct hci_conn *conn;
+
+        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));
+
+        memset(&cp, 0, sizeof(cp));
+        cp.handle = cpu_to_le16(conn->handle);
+        memcpy(cp.ltk, conn->ltk, sizeof(conn->ltk));
+
+        hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp);
+
+        hci_dev_unlock(hdev);
+}
+
 static inline void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_le_meta *le_ev = (void *) skb->data;
@@ -2760,6 +2823,10 @@ static inline void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_le_adv_report_evt(hdev, skb);
                 break;
 
+        case HCI_EV_LE_LTK_REQ:
+                hci_le_ltk_request_evt(hdev, skb);
+                break;
+
         default:
                 break;
         }
diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index 7a9a195c27d3..56828db68109 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -277,13 +277,16 @@ static void smp_cmd_pairing_confirm(struct l2cap_conn *conn,
 
 static void smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
 {
-        struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
+        struct hci_conn *hcon = conn->hcon;
+        struct crypto_blkcipher *tfm = hcon->hdev->tfm;
         int ret;
         u8 key[16], res[16], random[16], confirm[16], buf[128];
 
         swap128(skb->data, random);
         skb_pull(skb, sizeof(random));
 
+        memset(hcon->ltk, 0, sizeof(hcon->ltk));
+
         if (conn->hcon->out)
                 ret = smp_c1(tfm, conn->tk, random, conn->preq, conn->prsp, 0,
                                 conn->src, conn->hcon->dst_type, conn->dst,
@@ -309,11 +312,15 @@ static void smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
         }
 
         if (conn->hcon->out) {
+                __le16 ediv;
+                u8 rand[8];
+
                 smp_s1(tfm, conn->tk, random, conn->prnd, key);
+                swap128(key, hcon->ltk);
 
-                hex_dump_to_buffer(key, sizeof(key), 16, 1, buf,
-                                                        sizeof(buf), 0);
-                BT_DBG("key %s", buf);
+                memset(rand, 0, sizeof(rand));
+                ediv = 0;
+                hci_le_start_enc(hcon, ediv, rand, hcon->ltk);
         } else {
                 u8 r[16];
 
@@ -321,6 +328,7 @@ static void smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
 
                 smp_s1(tfm, conn->tk, conn->prnd, random, key);
+                swap128(key, hcon->ltk);
 
                 hex_dump_to_buffer(key, sizeof(key), 16, 1, buf,
                                                         sizeof(buf), 0);