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         if (!mgr)
 309                 return;
 310
 311         cp.phy_handle = hcon->handle;
 312         cp.len_so_far = cpu_to_le16(0);
 313         cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
 314
 315         set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state);
 316
 317         /* Read Local AMP Assoc final link information data */
 318         hci_req_init(&req, hdev);
 319         hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 320         hci_req_run_skb(&req, read_local_amp_assoc_complete);
 321         if (err < 0)
 322                 a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
 323 }
 324
 325 static void write_remote_amp_assoc_complete(struct hci_dev *hdev, u8 status,
 326                                             u16 opcode, struct sk_buff *skb)
 327 {
 328         struct hci_rp_write_remote_amp_assoc *rp = (void *)skb->data;
 329
 330         BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
 331                hdev->name, rp->status, rp->phy_handle);
 332
 333         if (rp->status)
 334                 return;
 335
 336         amp_write_rem_assoc_continue(hdev, rp->phy_handle);
 337 }
 338
 339 /* Write AMP Assoc data fragments, returns true with last fragment written*/
 340 static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
 341                                      struct hci_conn *hcon)
 342 {
 343         struct hci_cp_write_remote_amp_assoc *cp;
 344         struct amp_mgr *mgr = hcon->amp_mgr;
 345         struct amp_ctrl *ctrl;
 346         struct hci_request req;
 347         u16 frag_len, len;
 348
 349         ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
 350         if (!ctrl)
 351                 return false;
 352
 353         if (!ctrl->assoc_rem_len) {
 354                 BT_DBG("all fragments are written");
 355                 ctrl->assoc_rem_len = ctrl->assoc_len;
 356                 ctrl->assoc_len_so_far = 0;
 357
 358                 amp_ctrl_put(ctrl);
 359                 return true;
 360         }
 361
 362         frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
 363         len = frag_len + sizeof(*cp);
 364
 365         cp = kzalloc(len, GFP_KERNEL);
 366         if (!cp) {
 367                 amp_ctrl_put(ctrl);
 368                 return false;
 369         }
 370
 371         BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
 372                hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
 373
 374         cp->phy_handle = hcon->handle;
 375         cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
 376         cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
 377         memcpy(cp->frag, ctrl->assoc, frag_len);
 378
 379         ctrl->assoc_len_so_far += frag_len;
 380         ctrl->assoc_rem_len -= frag_len;
 381
 382         amp_ctrl_put(ctrl);
 383
 384         hci_req_init(&req, hdev);
 385         hci_req_add(&req, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
 386         hci_req_run_skb(&req, write_remote_amp_assoc_complete);
 387
 388         kfree(cp);
 389
 390         return false;
 391 }
 392
 393 void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
 394 {
 395         struct hci_conn *hcon;
 396
 397         BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
 398
 399         hcon = hci_conn_hash_lookup_handle(hdev, handle);
 400         if (!hcon)
 401                 return;
 402
 403         /* Send A2MP create phylink rsp when all fragments are written */
 404         if (amp_write_rem_assoc_frag(hdev, hcon))
 405                 a2mp_send_create_phy_link_rsp(hdev, 0);
 406 }
 407
 408 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
 409 {
 410         struct hci_conn *hcon;
 411
 412         BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
 413
 414         hcon = hci_conn_hash_lookup_handle(hdev, handle);
 415         if (!hcon)
 416                 return;
 417
 418         BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
 419
 420         amp_write_rem_assoc_frag(hdev, hcon);
 421 }
 422
 423 static void create_phylink_complete(struct hci_dev *hdev, u8 status,
 424                                     u16 opcode)
 425 {
 426         struct hci_cp_create_phy_link *cp;
 427
 428         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 429
 430         cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
 431         if (!cp)
 432                 return;
 433
 434         hci_dev_lock(hdev);
 435
 436         if (status) {
 437                 struct hci_conn *hcon;
 438
 439                 hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle);
 440                 if (hcon)
 441                         hci_conn_del(hcon);
 442         } else {
 443                 amp_write_remote_assoc(hdev, cp->phy_handle);
 444         }
 445
 446         hci_dev_unlock(hdev);
 447 }
 448
 449 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 450                         struct hci_conn *hcon)
 451 {
 452         struct hci_cp_create_phy_link cp;
 453         struct hci_request req;
 454
 455         cp.phy_handle = hcon->handle;
 456
 457         BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
 458                hcon->handle);
 459
 460         if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
 461                             &cp.key_type)) {
 462                 BT_DBG("Cannot create link key");
 463                 return;
 464         }
 465
 466         hci_req_init(&req, hdev);
 467         hci_req_add(&req, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
 468         hci_req_run(&req, create_phylink_complete);
 469 }
 470
 471 static void accept_phylink_complete(struct hci_dev *hdev, u8 status,
 472                                     u16 opcode)
 473 {
 474         struct hci_cp_accept_phy_link *cp;
 475
 476         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 477
 478         if (status)
 479                 return;
 480
 481         cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
 482         if (!cp)
 483                 return;
 484
 485         amp_write_remote_assoc(hdev, cp->phy_handle);
 486 }
 487
 488 void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 489                         struct hci_conn *hcon)
 490 {
 491         struct hci_cp_accept_phy_link cp;
 492         struct hci_request req;
 493
 494         cp.phy_handle = hcon->handle;
 495
 496         BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
 497                hcon->handle);
 498
 499         if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
 500                             &cp.key_type)) {
 501                 BT_DBG("Cannot create link key");
 502                 return;
 503         }
 504
 505         hci_req_init(&req, hdev);
 506         hci_req_add(&req, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
 507         hci_req_run(&req, accept_phylink_complete);
 508 }
 509
 510 void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon)
 511 {
 512         struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev);
 513         struct amp_mgr *mgr = hs_hcon->amp_mgr;
 514         struct l2cap_chan *bredr_chan;
 515
 516         BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr);
 517
 518         if (!bredr_hdev || !mgr || !mgr->bredr_chan)
 519                 return;
 520
 521         bredr_chan = mgr->bredr_chan;
 522
 523         l2cap_chan_lock(bredr_chan);
 524
 525         set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags);
 526         bredr_chan->remote_amp_id = hs_hcon->remote_id;
 527         bredr_chan->local_amp_id = hs_hcon->hdev->id;
 528         bredr_chan->hs_hcon = hs_hcon;
 529         bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu;
 530
 531         __l2cap_physical_cfm(bredr_chan, 0);
 532
 533         l2cap_chan_unlock(bredr_chan);
 534
 535         hci_dev_put(bredr_hdev);
 536 }
 537
 538 void amp_create_logical_link(struct l2cap_chan *chan)
 539 {
 540         struct hci_conn *hs_hcon = chan->hs_hcon;
 541         struct hci_cp_create_accept_logical_link cp;
 542         struct hci_dev *hdev;
 543
 544         BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon,
 545                &chan->conn->hcon->dst);
 546
 547         if (!hs_hcon)
 548                 return;
 549
 550         hdev = hci_dev_hold(chan->hs_hcon->hdev);
 551         if (!hdev)
 552                 return;
 553
 554         cp.phy_handle = hs_hcon->handle;
 555
 556         cp.tx_flow_spec.id = chan->local_id;
 557         cp.tx_flow_spec.stype = chan->local_stype;
 558         cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu);
 559         cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
 560         cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat);
 561         cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to);
 562
 563         cp.rx_flow_spec.id = chan->remote_id;
 564         cp.rx_flow_spec.stype = chan->remote_stype;
 565         cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu);
 566         cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime);
 567         cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat);
 568         cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to);
 569
 570         if (hs_hcon->out)
 571                 hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp),
 572                              &cp);
 573         else
 574                 hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp),
 575                              &cp);
 576
 577         hci_dev_put(hdev);
 578 }
 579
 580 void amp_disconnect_logical_link(struct hci_chan *hchan)
 581 {
 582         struct hci_conn *hcon = hchan->conn;
 583         struct hci_cp_disconn_logical_link cp;
 584
 585         if (hcon->state != BT_CONNECTED) {
 586                 BT_DBG("hchan %p not connected", hchan);
 587                 return;
 588         }
 589
 590         cp.log_handle = cpu_to_le16(hchan->handle);
 591         hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp);
 592 }
 593
 594 void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason)
 595 {
 596         BT_DBG("hchan %p", hchan);
 597
 598         hci_chan_del(hchan);
 599 }