net/bluetooth/amp.c

   1 /*
   2    Copyright (c) 2011,2012 Intel Corp.
   3
   4    This program is free software; you can redistribute it and/or modify
   5    it under the terms of the GNU General Public License version 2 and
   6    only version 2 as published by the Free Software Foundation.
   7
   8    This program is distributed in the hope that it will be useful,
   9    but WITHOUT ANY WARRANTY; without even the implied warranty of
  10    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11    GNU General Public License for more details.
  12 */
  13
  14 #include <net/bluetooth/bluetooth.h>
  15 #include <net/bluetooth/hci.h>
  16 #include <net/bluetooth/hci_core.h>
  17 #include <crypto/hash.h>
  18
  19 #include "hci_request.h"
  20 #include "a2mp.h"
  21 #include "amp.h"
  22
  23 /* Remote AMP Controllers interface */
  24 void amp_ctrl_get(struct amp_ctrl *ctrl)
  25 {
  26         BT_DBG("ctrl %p orig refcnt %d", ctrl,
  27                atomic_read(&ctrl->kref.refcount));
  28
  29         kref_get(&ctrl->kref);
  30 }
  31
  32 static void amp_ctrl_destroy(struct kref *kref)
  33 {
  34         struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
  35
  36         BT_DBG("ctrl %p", ctrl);
  37
  38         kfree(ctrl->assoc);
  39         kfree(ctrl);
  40 }
  41
  42 int amp_ctrl_put(struct amp_ctrl *ctrl)
  43 {
  44         BT_DBG("ctrl %p orig refcnt %d", ctrl,
  45                atomic_read(&ctrl->kref.refcount));
  46
  47         return kref_put(&ctrl->kref, &amp_ctrl_destroy);
  48 }
  49
  50 struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id)
  51 {
  52         struct amp_ctrl *ctrl;
  53
  54         ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
  55         if (!ctrl)
  56                 return NULL;
  57
  58         kref_init(&ctrl->kref);
  59         ctrl->id = id;
  60
  61         mutex_lock(&mgr->amp_ctrls_lock);
  62         list_add(&ctrl->list, &mgr->amp_ctrls);
  63         mutex_unlock(&mgr->amp_ctrls_lock);
  64
  65         BT_DBG("mgr %p ctrl %p", mgr, ctrl);
  66
  67         return ctrl;
  68 }
  69
  70 void amp_ctrl_list_flush(struct amp_mgr *mgr)
  71 {
  72         struct amp_ctrl *ctrl, *n;
  73
  74         BT_DBG("mgr %p", mgr);
  75
  76         mutex_lock(&mgr->amp_ctrls_lock);
  77         list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
  78                 list_del(&ctrl->list);
  79                 amp_ctrl_put(ctrl);
  80         }
  81         mutex_unlock(&mgr->amp_ctrls_lock);
  82 }
  83
  84 struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
  85 {
  86         struct amp_ctrl *ctrl;
  87
  88         BT_DBG("mgr %p id %d", mgr, id);
  89
  90         mutex_lock(&mgr->amp_ctrls_lock);
  91         list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
  92                 if (ctrl->id == id) {
  93                         amp_ctrl_get(ctrl);
  94                         mutex_unlock(&mgr->amp_ctrls_lock);
  95                         return ctrl;
  96                 }
  97         }
  98         mutex_unlock(&mgr->amp_ctrls_lock);
  99
 100         return NULL;
 101 }
 102
 103 /* Physical Link interface */
 104 static u8 __next_handle(struct amp_mgr *mgr)
 105 {
 106         if (++mgr->handle == 0)
 107                 mgr->handle = 1;
 108
 109         return mgr->handle;
 110 }
 111
 112 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 113                              u8 remote_id, bool out)
 114 {
 115         bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst;
 116         struct hci_conn *hcon;
 117         u8 role = out ? HCI_ROLE_MASTER : HCI_ROLE_SLAVE;
 118
 119         hcon = hci_conn_add(hdev, AMP_LINK, dst, role);
 120         if (!hcon)
 121                 return NULL;
 122
 123         BT_DBG("hcon %p dst %pMR", hcon, dst);
 124
 125         hcon->state = BT_CONNECT;
 126         hcon->attempt++;
 127         hcon->handle = __next_handle(mgr);
 128         hcon->remote_id = remote_id;
 129         hcon->amp_mgr = amp_mgr_get(mgr);
 130
 131         return hcon;
 132 }
 133
 134 /* AMP crypto key generation interface */
 135 static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 136 {
 137         struct crypto_shash *tfm;
 138         struct shash_desc *shash;
 139         int ret;
 140
 141         if (!ksize)
 142                 return -EINVAL;
 143
 144         tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
 145         if (IS_ERR(tfm)) {
 146                 BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
 147                 return PTR_ERR(tfm);
 148         }
 149
 150         ret = crypto_shash_setkey(tfm, key, ksize);
 151         if (ret) {
 152                 BT_DBG("crypto_ahash_setkey failed: err %d", ret);
 153                 goto failed;
 154         }
 155
 156         shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm),
 157                         GFP_KERNEL);
 158         if (!shash) {
 159                 ret = -ENOMEM;
 160                 goto failed;
 161         }
 162
 163         shash->tfm = tfm;
 164         shash->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
 165
 166         ret = crypto_shash_digest(shash, plaintext, psize, output);
 167
 168         kfree(shash);
 169
 170 failed:
 171         crypto_free_shash(tfm);
 172         return ret;
 173 }
 174
 175 int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
 176 {
 177         struct hci_dev *hdev = conn->hdev;
 178         struct link_key *key;
 179         u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
 180         u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
 181         int err;
 182
 183         if (!hci_conn_check_link_mode(conn))
 184                 return -EACCES;
 185
 186         BT_DBG("conn %p key_type %d", conn, conn->key_type);
 187
 188         /* Legacy key */
 189         if (conn->key_type < 3) {
 190                 BT_ERR("Legacy key type %d", conn->key_type);
 191                 return -EACCES;
 192         }
 193
 194         *type = conn->key_type;
 195         *len = HCI_AMP_LINK_KEY_SIZE;
 196
 197         key = hci_find_link_key(hdev, &conn->dst);
 198         if (!key) {
 199                 BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst);
 200                 return -EACCES;
 201         }
 202
 203         /* BR/EDR Link Key concatenated together with itself */
 204         memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
 205         memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
 206
 207         /* Derive Generic AMP Link Key (gamp) */
 208         err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
 209         if (err) {
 210                 BT_ERR("Could not derive Generic AMP Key: err %d", err);
 211                 return err;
 212         }
 213
 214         if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
 215                 BT_DBG("Use Generic AMP Key (gamp)");
 216                 memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
 217                 return err;
 218         }
 219
 220         /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
 221         return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
 222 }
 223
 224 static void read_local_amp_assoc_complete(struct hci_dev *hdev, u8 status,
 225                                           u16 opcode, struct sk_buff *skb)
 226 {
 227         struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
 228         struct amp_assoc *assoc = &hdev->loc_assoc;
 229         size_t rem_len, frag_len;
 230
 231         BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 232
 233         if (rp->status)
 234                 goto send_rsp;
 235
 236         frag_len = skb->len - sizeof(*rp);
 237         rem_len = __le16_to_cpu(rp->rem_len);
 238
 239         if (rem_len > frag_len) {
 240                 BT_DBG("frag_len %zu rem_len %zu", frag_len, rem_len);
 241
 242                 memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
 243                 assoc->offset += frag_len;
 244
 245                 /* Read other fragments */
 246                 amp_read_loc_assoc_frag(hdev, rp->phy_handle);
 247
 248                 return;
 249         }
 250
 251         memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
 252         assoc->len = assoc->offset + rem_len;
 253         assoc->offset = 0;
 254
 255 send_rsp:
 256         /* Send A2MP Rsp when all fragments are received */
 257         a2mp_send_getampassoc_rsp(hdev, rp->status);
 258         a2mp_send_create_phy_link_req(hdev, rp->status);
 259 }
 260
 261 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 262 {
 263         struct hci_cp_read_local_amp_assoc cp;
 264         struct amp_assoc *loc_assoc = &hdev->loc_assoc;
 265         struct hci_request req;
 266         int err = 0;
 267
 268         BT_DBG("%s handle %d", hdev->name, phy_handle);
 269
 270         cp.phy_handle = phy_handle;
 271         cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
 272         cp.len_so_far = cpu_to_le16(loc_assoc->offset);
 273
 274         hci_req_init(&req, hdev);
 275         hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 276         err = hci_req_run_skb(&req, read_local_amp_assoc_complete);
 277         if (err < 0)
 278                 a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
 279 }
 280
 281 void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 282 {
 283         struct hci_cp_read_local_amp_assoc cp;
 284         struct hci_request req;
 285         int err = 0;
 286
 287         memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
 288         memset(&cp, 0, sizeof(cp));
 289
 290         cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
 291
 292         set_bit(READ_LOC_AMP_ASSOC, &mgr->state);
 293         hci_req_init(&req, hdev);
 294         hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 295         hci_req_run_skb(&req, read_local_amp_assoc_complete);
 296         if (err < 0)
 297                 a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
 298 }
 299
 300 void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
 301                                    struct hci_conn *hcon)
 302 {
 303         struct hci_cp_read_local_amp_assoc cp;
 304         struct amp_mgr *mgr = hcon->amp_mgr;
 305         struct hci_request req;
 306         int err = 0;
 307
 308         cp.phy_handle = hcon->handle;
 309         cp.len_so_far = cpu_to_le16(0);
 310         cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
 311
 312         set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state);
 313
 314         /* Read Local AMP Assoc final link information data */
 315         hci_req_init(&req, hdev);
 316         hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 317         hci_req_run_skb(&req, read_local_amp_assoc_complete);
 318         if (err < 0)
 319                 a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
 320 }
 321
 322 static void write_remote_amp_assoc_complete(struct hci_dev *hdev, u8 status,
 323                                             u16 opcode, struct sk_buff *skb)
 324 {
 325         struct hci_rp_write_remote_amp_assoc *rp = (void *)skb->data;
 326
 327         BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
 328                hdev->name, rp->status, rp->phy_handle);
 329
 330         if (rp->status)
 331                 return;
 332
 333         amp_write_rem_assoc_continue(hdev, rp->phy_handle);
 334 }
 335
 336 /* Write AMP Assoc data fragments, returns true with last fragment written*/
 337 static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
 338                                      struct hci_conn *hcon)
 339 {
 340         struct hci_cp_write_remote_amp_assoc *cp;
 341         struct amp_mgr *mgr = hcon->amp_mgr;
 342         struct amp_ctrl *ctrl;
 343         struct hci_request req;
 344         u16 frag_len, len;
 345
 346         ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
 347         if (!ctrl)
 348                 return false;
 349
 350         if (!ctrl->assoc_rem_len) {
 351                 BT_DBG("all fragments are written");
 352                 ctrl->assoc_rem_len = ctrl->assoc_len;
 353                 ctrl->assoc_len_so_far = 0;
 354
 355                 amp_ctrl_put(ctrl);
 356                 return true;
 357         }
 358
 359         frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
 360         len = frag_len + sizeof(*cp);
 361
 362         cp = kzalloc(len, GFP_KERNEL);
 363         if (!cp) {
 364                 amp_ctrl_put(ctrl);
 365                 return false;
 366         }
 367
 368         BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
 369                hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
 370
 371         cp->phy_handle = hcon->handle;
 372         cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
 373         cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
 374         memcpy(cp->frag, ctrl->assoc, frag_len);
 375
 376         ctrl->assoc_len_so_far += frag_len;
 377         ctrl->assoc_rem_len -= frag_len;
 378
 379         amp_ctrl_put(ctrl);
 380
 381         hci_req_init(&req, hdev);
 382         hci_req_add(&req, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
 383         hci_req_run_skb(&req, write_remote_amp_assoc_complete);
 384
 385         kfree(cp);
 386
 387         return false;
 388 }
 389
 390 void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
 391 {
 392         struct hci_conn *hcon;
 393
 394         BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
 395
 396         hcon = hci_conn_hash_lookup_handle(hdev, handle);
 397         if (!hcon)
 398                 return;
 399
 400         /* Send A2MP create phylink rsp when all fragments are written */
 401         if (amp_write_rem_assoc_frag(hdev, hcon))
 402                 a2mp_send_create_phy_link_rsp(hdev, 0);
 403 }
 404
 405 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
 406 {
 407         struct hci_conn *hcon;
 408
 409         BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
 410
 411         hcon = hci_conn_hash_lookup_handle(hdev, handle);
 412         if (!hcon)
 413                 return;
 414
 415         BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
 416
 417         amp_write_rem_assoc_frag(hdev, hcon);
 418 }
 419
 420 static void create_phylink_complete(struct hci_dev *hdev, u8 status,
 421                                     u16 opcode)
 422 {
 423         struct hci_cp_create_phy_link *cp;
 424
 425         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 426
 427         cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
 428         if (!cp)
 429                 return;
 430
 431         hci_dev_lock(hdev);
 432
 433         if (status) {
 434                 struct hci_conn *hcon;
 435
 436                 hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle);
 437                 if (hcon)
 438                         hci_conn_del(hcon);
 439         } else {
 440                 amp_write_remote_assoc(hdev, cp->phy_handle);
 441         }
 442
 443         hci_dev_unlock(hdev);
 444 }
 445
 446 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 447                         struct hci_conn *hcon)
 448 {
 449         struct hci_cp_create_phy_link cp;
 450         struct hci_request req;
 451
 452         cp.phy_handle = hcon->handle;
 453
 454         BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
 455                hcon->handle);
 456
 457         if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
 458                             &cp.key_type)) {
 459                 BT_DBG("Cannot create link key");
 460                 return;
 461         }
 462
 463         hci_req_init(&req, hdev);
 464         hci_req_add(&req, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
 465         hci_req_run(&req, create_phylink_complete);
 466 }
 467
 468 static void accept_phylink_complete(struct hci_dev *hdev, u8 status,
 469                                     u16 opcode)
 470 {
 471         struct hci_cp_accept_phy_link *cp;
 472
 473         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 474
 475         if (status)
 476                 return;
 477
 478         cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
 479         if (!cp)
 480                 return;
 481
 482         amp_write_remote_assoc(hdev, cp->phy_handle);
 483 }
 484
 485 void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 486                         struct hci_conn *hcon)
 487 {
 488         struct hci_cp_accept_phy_link cp;
 489         struct hci_request req;
 490
 491         cp.phy_handle = hcon->handle;
 492
 493         BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
 494                hcon->handle);
 495
 496         if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
 497                             &cp.key_type)) {
 498                 BT_DBG("Cannot create link key");
 499                 return;
 500         }
 501
 502         hci_req_init(&req, hdev);
 503         hci_req_add(&req, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
 504         hci_req_run(&req, accept_phylink_complete);
 505 }
 506
 507 void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon)
 508 {
 509         struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev);
 510         struct amp_mgr *mgr = hs_hcon->amp_mgr;
 511         struct l2cap_chan *bredr_chan;
 512
 513         BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr);
 514
 515         if (!bredr_hdev || !mgr || !mgr->bredr_chan)
 516                 return;
 517
 518         bredr_chan = mgr->bredr_chan;
 519
 520         l2cap_chan_lock(bredr_chan);
 521
 522         set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags);
 523         bredr_chan->remote_amp_id = hs_hcon->remote_id;
 524         bredr_chan->local_amp_id = hs_hcon->hdev->id;
 525         bredr_chan->hs_hcon = hs_hcon;
 526         bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu;
 527
 528         __l2cap_physical_cfm(bredr_chan, 0);
 529
 530         l2cap_chan_unlock(bredr_chan);
 531
 532         hci_dev_put(bredr_hdev);
 533 }
 534
 535 void amp_create_logical_link(struct l2cap_chan *chan)
 536 {
 537         struct hci_conn *hs_hcon = chan->hs_hcon;
 538         struct hci_cp_create_accept_logical_link cp;
 539         struct hci_dev *hdev;
 540
 541         BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon,
 542                &chan->conn->hcon->dst);
 543
 544         if (!hs_hcon)
 545                 return;
 546
 547         hdev = hci_dev_hold(chan->hs_hcon->hdev);
 548         if (!hdev)
 549                 return;
 550
 551         cp.phy_handle = hs_hcon->handle;
 552
 553         cp.tx_flow_spec.id = chan->local_id;
 554         cp.tx_flow_spec.stype = chan->local_stype;
 555         cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu);
 556         cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
 557         cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat);
 558         cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to);
 559
 560         cp.rx_flow_spec.id = chan->remote_id;
 561         cp.rx_flow_spec.stype = chan->remote_stype;
 562         cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu);
 563         cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime);
 564         cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat);
 565         cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to);
 566
 567         if (hs_hcon->out)
 568                 hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp),
 569                              &cp);
 570         else
 571                 hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp),
 572                              &cp);
 573
 574         hci_dev_put(hdev);
 575 }
 576
 577 void amp_disconnect_logical_link(struct hci_chan *hchan)
 578 {
 579         struct hci_conn *hcon = hchan->conn;
 580         struct hci_cp_disconn_logical_link cp;
 581
 582         if (hcon->state != BT_CONNECTED) {
 583                 BT_DBG("hchan %p not connected", hchan);
 584                 return;
 585         }
 586
 587         cp.log_handle = cpu_to_le16(hchan->handle);
 588         hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp);
 589 }
 590
 591 void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason)
 592 {
 593         BT_DBG("hchan %p", hchan);
 594
 595         hci_chan_del(hchan);
 596 }