1 files changed, 56 insertions, 0 deletions

diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index b12f076d78b1..f59f0510e0b0 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -1894,6 +1894,58 @@ static int smp_cmd_public_key(struct l2cap_conn *conn, struct sk_buff *skb)
         return 0;
 }
 
+static int smp_cmd_dhkey_check(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+        struct smp_cmd_dhkey_check *check = (void *) skb->data;
+        struct l2cap_chan *chan = conn->smp;
+        struct hci_conn *hcon = conn->hcon;
+        struct smp_chan *smp = chan->data;
+        u8 a[7], b[7], *local_addr, *remote_addr;
+        u8 io_cap[3], r[16], e[16];
+        int err;
+
+        BT_DBG("conn %p", conn);
+
+        if (skb->len < sizeof(*check))
+                return SMP_INVALID_PARAMS;
+
+        memcpy(a, &hcon->init_addr, 6);
+        memcpy(b, &hcon->resp_addr, 6);
+        a[6] = hcon->init_addr_type;
+        b[6] = hcon->resp_addr_type;
+
+        if (hcon->out) {
+                local_addr = a;
+                remote_addr = b;
+                memcpy(io_cap, &smp->prsp[1], 3);
+        } else {
+                local_addr = b;
+                remote_addr = a;
+                memcpy(io_cap, &smp->preq[1], 3);
+        }
+
+        memset(r, 0, sizeof(r));
+
+        err = smp_f6(smp->tfm_cmac, smp->mackey, smp->rrnd, smp->prnd, r,
+                     io_cap, remote_addr, local_addr, e);
+        if (err)
+                return SMP_UNSPECIFIED;
+
+        if (memcmp(check->e, e, 16))
+                return SMP_DHKEY_CHECK_FAILED;
+
+        smp->ltk = hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
+                               SMP_LTK_P256, 0, smp->tk, smp->enc_key_size,
+                               0, 0);
+
+        if (hcon->out) {
+                hci_le_start_enc(hcon, 0, 0, smp->tk);
+                hcon->enc_key_size = smp->enc_key_size;
+        }
+
+        return 0;
+}
+
 static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb)
 {
         struct l2cap_conn *conn = chan->conn;
@@ -1982,6 +2034,10 @@ static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb)
                 reason = smp_cmd_public_key(conn, skb);
                 break;
 
+        case SMP_CMD_DHKEY_CHECK:
+                reason = smp_cmd_dhkey_check(conn, skb);
+                break;
+
         default:
                 BT_DBG("Unknown command code 0x%2.2x", code);
                 reason = SMP_CMD_NOTSUPP;