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 "a2mp.h" 20#include "amp.h" 21 22/* Remote AMP Controllers interface */ 23void amp_ctrl_get(struct amp_ctrl *ctrl) 24{ 25 BT_DBG("ctrl %p orig refcnt %d", ctrl, 26 atomic_read(&ctrl->kref.refcount)); 27 28 kref_get(&ctrl->kref); 29} 30 31static void amp_ctrl_destroy(struct kref *kref) 32{ 33 struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref); 34 35 BT_DBG("ctrl %p", ctrl); 36 37 kfree(ctrl->assoc); 38 kfree(ctrl); 39} 40 41int amp_ctrl_put(struct amp_ctrl *ctrl) 42{ 43 BT_DBG("ctrl %p orig refcnt %d", ctrl, 44 atomic_read(&ctrl->kref.refcount)); 45 46 return kref_put(&ctrl->kref, &_ctrl_destroy); 47} 48 49struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id) 50{ 51 struct amp_ctrl *ctrl; 52 53 ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL); 54 if (!ctrl) 55 return NULL; 56 57 kref_init(&ctrl->kref); 58 ctrl->id = id; 59 60 mutex_lock(&mgr->amp_ctrls_lock); 61 list_add(&ctrl->list, &mgr->amp_ctrls); 62 mutex_unlock(&mgr->amp_ctrls_lock); 63 64 BT_DBG("mgr %p ctrl %p", mgr, ctrl); 65 66 return ctrl; 67} 68 69void amp_ctrl_list_flush(struct amp_mgr *mgr) 70{ 71 struct amp_ctrl *ctrl, *n; 72 73 BT_DBG("mgr %p", mgr); 74 75 mutex_lock(&mgr->amp_ctrls_lock); 76 list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) { 77 list_del(&ctrl->list); 78 amp_ctrl_put(ctrl); 79 } 80 mutex_unlock(&mgr->amp_ctrls_lock); 81} 82 83struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id) 84{ 85 struct amp_ctrl *ctrl; 86 87 BT_DBG("mgr %p id %d", mgr, id); 88 89 mutex_lock(&mgr->amp_ctrls_lock); 90 list_for_each_entry(ctrl, &mgr->amp_ctrls, list) { 91 if (ctrl->id == id) { 92 amp_ctrl_get(ctrl); 93 mutex_unlock(&mgr->amp_ctrls_lock); 94 return ctrl; 95 } 96 } 97 mutex_unlock(&mgr->amp_ctrls_lock); 98 99 return NULL; 100} 101 102/* Physical Link interface */ 103static u8 __next_handle(struct amp_mgr *mgr) 104{ 105 if (++mgr->handle == 0) 106 mgr->handle = 1; 107 108 return mgr->handle; 109} 110 111struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr, 112 u8 remote_id, bool out) 113{ 114 bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst; 115 struct hci_conn *hcon; 116 u8 role = out ? HCI_ROLE_MASTER : HCI_ROLE_SLAVE; 117 118 hcon = hci_conn_add(hdev, AMP_LINK, dst, role); 119 if (!hcon) 120 return NULL; 121 122 BT_DBG("hcon %p dst %pMR", hcon, dst); 123 124 hcon->state = BT_CONNECT; 125 hcon->attempt++; 126 hcon->handle = __next_handle(mgr); 127 hcon->remote_id = remote_id; 128 hcon->amp_mgr = amp_mgr_get(mgr); 129 130 return hcon; 131} 132 133/* AMP crypto key generation interface */ 134static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output) 135{ 136 struct crypto_shash *tfm; 137 struct shash_desc *shash; 138 int ret; 139 140 if (!ksize) 141 return -EINVAL; 142 143 tfm = crypto_alloc_shash("hmac(sha256)", 0, 0); 144 if (IS_ERR(tfm)) { 145 BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm)); 146 return PTR_ERR(tfm); 147 } 148 149 ret = crypto_shash_setkey(tfm, key, ksize); 150 if (ret) { 151 BT_DBG("crypto_ahash_setkey failed: err %d", ret); 152 goto failed; 153 } 154 155 shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm), 156 GFP_KERNEL); 157 if (!shash) { 158 ret = -ENOMEM; 159 goto failed; 160 } 161 162 shash->tfm = tfm; 163 shash->flags = CRYPTO_TFM_REQ_MAY_SLEEP; 164 165 ret = crypto_shash_digest(shash, plaintext, psize, output); 166 167 kfree(shash); 168 169failed: 170 crypto_free_shash(tfm); 171 return ret; 172} 173 174int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type) 175{ 176 struct hci_dev *hdev = conn->hdev; 177 struct link_key *key; 178 u8 keybuf[HCI_AMP_LINK_KEY_SIZE]; 179 u8 gamp_key[HCI_AMP_LINK_KEY_SIZE]; 180 int err; 181 182 if (!hci_conn_check_link_mode(conn)) 183 return -EACCES; 184 185 BT_DBG("conn %p key_type %d", conn, conn->key_type); 186 187 /* Legacy key */ 188 if (conn->key_type < 3) { 189 BT_ERR("Legacy key type %d", conn->key_type); 190 return -EACCES; 191 } 192 193 *type = conn->key_type; 194 *len = HCI_AMP_LINK_KEY_SIZE; 195 196 key = hci_find_link_key(hdev, &conn->dst); 197 if (!key) { 198 BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst); 199 return -EACCES; 200 } 201 202 /* BR/EDR Link Key concatenated together with itself */ 203 memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE); 204 memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE); 205 206 /* Derive Generic AMP Link Key (gamp) */ 207 err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key); 208 if (err) { 209 BT_ERR("Could not derive Generic AMP Key: err %d", err); 210 return err; 211 } 212 213 if (conn->key_type == HCI_LK_DEBUG_COMBINATION) { 214 BT_DBG("Use Generic AMP Key (gamp)"); 215 memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE); 216 return err; 217 } 218 219 /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */ 220 return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data); 221} 222 223void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle) 224{ 225 struct hci_cp_read_local_amp_assoc cp; 226 struct amp_assoc *loc_assoc = &hdev->loc_assoc; 227 228 BT_DBG("%s handle %d", hdev->name, phy_handle); 229 230 cp.phy_handle = phy_handle; 231 cp.max_len = cpu_to_le16(hdev->amp_assoc_size); 232 cp.len_so_far = cpu_to_le16(loc_assoc->offset); 233 234 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); 235} 236 237void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr) 238{ 239 struct hci_cp_read_local_amp_assoc cp; 240 241 memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc)); 242 memset(&cp, 0, sizeof(cp)); 243 244 cp.max_len = cpu_to_le16(hdev->amp_assoc_size); 245 246 set_bit(READ_LOC_AMP_ASSOC, &mgr->state); 247 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); 248} 249 250void amp_read_loc_assoc_final_data(struct hci_dev *hdev, 251 struct hci_conn *hcon) 252{ 253 struct hci_cp_read_local_amp_assoc cp; 254 struct amp_mgr *mgr = hcon->amp_mgr; 255 256 cp.phy_handle = hcon->handle; 257 cp.len_so_far = cpu_to_le16(0); 258 cp.max_len = cpu_to_le16(hdev->amp_assoc_size); 259 260 set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state); 261 262 /* Read Local AMP Assoc final link information data */ 263 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); 264} 265 266/* Write AMP Assoc data fragments, returns true with last fragment written*/ 267static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, 268 struct hci_conn *hcon) 269{ 270 struct hci_cp_write_remote_amp_assoc *cp; 271 struct amp_mgr *mgr = hcon->amp_mgr; 272 struct amp_ctrl *ctrl; 273 u16 frag_len, len; 274 275 ctrl = amp_ctrl_lookup(mgr, hcon->remote_id); 276 if (!ctrl) 277 return false; 278 279 if (!ctrl->assoc_rem_len) { 280 BT_DBG("all fragments are written"); 281 ctrl->assoc_rem_len = ctrl->assoc_len; 282 ctrl->assoc_len_so_far = 0; 283 284 amp_ctrl_put(ctrl); 285 return true; 286 } 287 288 frag_len = min_t(u16, 248, ctrl->assoc_rem_len); 289 len = frag_len + sizeof(*cp); 290 291 cp = kzalloc(len, GFP_KERNEL); 292 if (!cp) { 293 amp_ctrl_put(ctrl); 294 return false; 295 } 296 297 BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u", 298 hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len); 299 300 cp->phy_handle = hcon->handle; 301 cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far); 302 cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len); 303 memcpy(cp->frag, ctrl->assoc, frag_len); 304 305 ctrl->assoc_len_so_far += frag_len; 306 ctrl->assoc_rem_len -= frag_len; 307 308 amp_ctrl_put(ctrl); 309 310 hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp); 311 312 kfree(cp); 313 314 return false; 315} 316 317void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle) 318{ 319 struct hci_conn *hcon; 320 321 BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle); 322 323 hcon = hci_conn_hash_lookup_handle(hdev, handle); 324 if (!hcon) 325 return; 326 327 /* Send A2MP create phylink rsp when all fragments are written */ 328 if (amp_write_rem_assoc_frag(hdev, hcon)) 329 a2mp_send_create_phy_link_rsp(hdev, 0); 330} 331 332void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle) 333{ 334 struct hci_conn *hcon; 335 336 BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle); 337 338 hcon = hci_conn_hash_lookup_handle(hdev, handle); 339 if (!hcon) 340 return; 341 342 BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon); 343 344 amp_write_rem_assoc_frag(hdev, hcon); 345} 346 347void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, 348 struct hci_conn *hcon) 349{ 350 struct hci_cp_create_phy_link cp; 351 352 cp.phy_handle = hcon->handle; 353 354 BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon, 355 hcon->handle); 356 357 if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len, 358 &cp.key_type)) { 359 BT_DBG("Cannot create link key"); 360 return; 361 } 362 363 hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp); 364} 365 366void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, 367 struct hci_conn *hcon) 368{ 369 struct hci_cp_accept_phy_link cp; 370 371 cp.phy_handle = hcon->handle; 372 373 BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon, 374 hcon->handle); 375 376 if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len, 377 &cp.key_type)) { 378 BT_DBG("Cannot create link key"); 379 return; 380 } 381 382 hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp); 383} 384 385void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon) 386{ 387 struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev); 388 struct amp_mgr *mgr = hs_hcon->amp_mgr; 389 struct l2cap_chan *bredr_chan; 390 391 BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr); 392 393 if (!bredr_hdev || !mgr || !mgr->bredr_chan) 394 return; 395 396 bredr_chan = mgr->bredr_chan; 397 398 l2cap_chan_lock(bredr_chan); 399 400 set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags); 401 bredr_chan->remote_amp_id = hs_hcon->remote_id; 402 bredr_chan->local_amp_id = hs_hcon->hdev->id; 403 bredr_chan->hs_hcon = hs_hcon; 404 bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu; 405 406 __l2cap_physical_cfm(bredr_chan, 0); 407 408 l2cap_chan_unlock(bredr_chan); 409 410 hci_dev_put(bredr_hdev); 411} 412 413void amp_create_logical_link(struct l2cap_chan *chan) 414{ 415 struct hci_conn *hs_hcon = chan->hs_hcon; 416 struct hci_cp_create_accept_logical_link cp; 417 struct hci_dev *hdev; 418 419 BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon, 420 &chan->conn->hcon->dst); 421 422 if (!hs_hcon) 423 return; 424 425 hdev = hci_dev_hold(chan->hs_hcon->hdev); 426 if (!hdev) 427 return; 428 429 cp.phy_handle = hs_hcon->handle; 430 431 cp.tx_flow_spec.id = chan->local_id; 432 cp.tx_flow_spec.stype = chan->local_stype; 433 cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu); 434 cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime); 435 cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat); 436 cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to); 437 438 cp.rx_flow_spec.id = chan->remote_id; 439 cp.rx_flow_spec.stype = chan->remote_stype; 440 cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu); 441 cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime); 442 cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat); 443 cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to); 444 445 if (hs_hcon->out) 446 hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp), 447 &cp); 448 else 449 hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp), 450 &cp); 451 452 hci_dev_put(hdev); 453} 454 455void amp_disconnect_logical_link(struct hci_chan *hchan) 456{ 457 struct hci_conn *hcon = hchan->conn; 458 struct hci_cp_disconn_logical_link cp; 459 460 if (hcon->state != BT_CONNECTED) { 461 BT_DBG("hchan %p not connected", hchan); 462 return; 463 } 464 465 cp.log_handle = cpu_to_le16(hchan->handle); 466 hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp); 467} 468 469void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason) 470{ 471 BT_DBG("hchan %p", hchan); 472 473 hci_chan_del(hchan); 474} 475