Bluetooth: Add support for LE SC DHKey check PDU

Once we receive the DHKey check PDU it's time to first verify that the
value is correct and then proceed with encrypting the link.

Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index b12f076d78b12bc832e1084f96a484b9eb42429f..f59f0510e0b0d57e6db8be64d7238bf96884cce4 100644 (file)
--- 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;