1/* 2 Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved. 3 Copyright (c) 2011,2012 Intel Corp. 4 5 This program is free software; you can redistribute it and/or modify 6 it under the terms of the GNU General Public License version 2 and 7 only version 2 as published by the Free Software Foundation. 8 9 This program is distributed in the hope that it will be useful, 10 but WITHOUT ANY WARRANTY; without even the implied warranty of 11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 GNU General Public License for more details. 13*/ 14 15#include <net/bluetooth/bluetooth.h> 16#include <net/bluetooth/hci_core.h> 17#include <net/bluetooth/l2cap.h> 18 19#include "a2mp.h" 20#include "amp.h" 21 22#define A2MP_FEAT_EXT 0x8000 23 24/* Global AMP Manager list */ 25static LIST_HEAD(amp_mgr_list); 26static DEFINE_MUTEX(amp_mgr_list_lock); 27 28/* A2MP build & send command helper functions */ 29static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data) 30{ 31 struct a2mp_cmd *cmd; 32 int plen; 33 34 plen = sizeof(*cmd) + len; 35 cmd = kzalloc(plen, GFP_KERNEL); 36 if (!cmd) 37 return NULL; 38 39 cmd->code = code; 40 cmd->ident = ident; 41 cmd->len = cpu_to_le16(len); 42 43 memcpy(cmd->data, data, len); 44 45 return cmd; 46} 47 48static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data) 49{ 50 struct l2cap_chan *chan = mgr->a2mp_chan; 51 struct a2mp_cmd *cmd; 52 u16 total_len = len + sizeof(*cmd); 53 struct kvec iv; 54 struct msghdr msg; 55 56 cmd = __a2mp_build(code, ident, len, data); 57 if (!cmd) 58 return; 59 60 iv.iov_base = cmd; 61 iv.iov_len = total_len; 62 63 memset(&msg, 0, sizeof(msg)); 64 65 iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, &iv, 1, total_len); 66 67 l2cap_chan_send(chan, &msg, total_len); 68 69 kfree(cmd); 70} 71 72static u8 __next_ident(struct amp_mgr *mgr) 73{ 74 if (++mgr->ident == 0) 75 mgr->ident = 1; 76 77 return mgr->ident; 78} 79 80static struct amp_mgr *amp_mgr_lookup_by_state(u8 state) 81{ 82 struct amp_mgr *mgr; 83 84 mutex_lock(&_mgr_list_lock); 85 list_for_each_entry(mgr, &_mgr_list, list) { 86 if (test_and_clear_bit(state, &mgr->state)) { 87 amp_mgr_get(mgr); 88 mutex_unlock(&_mgr_list_lock); 89 return mgr; 90 } 91 } 92 mutex_unlock(&_mgr_list_lock); 93 94 return NULL; 95} 96 97/* hci_dev_list shall be locked */ 98static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl) 99{ 100 struct hci_dev *hdev; 101 int i = 1; 102 103 cl[0].id = AMP_ID_BREDR; 104 cl[0].type = AMP_TYPE_BREDR; 105 cl[0].status = AMP_STATUS_BLUETOOTH_ONLY; 106 107 list_for_each_entry(hdev, &hci_dev_list, list) { 108 if (hdev->dev_type == HCI_AMP) { 109 cl[i].id = hdev->id; 110 cl[i].type = hdev->amp_type; 111 if (test_bit(HCI_UP, &hdev->flags)) 112 cl[i].status = hdev->amp_status; 113 else 114 cl[i].status = AMP_STATUS_POWERED_DOWN; 115 i++; 116 } 117 } 118} 119 120/* Processing A2MP messages */ 121static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, 122 struct a2mp_cmd *hdr) 123{ 124 struct a2mp_cmd_rej *rej = (void *) skb->data; 125 126 if (le16_to_cpu(hdr->len) < sizeof(*rej)) 127 return -EINVAL; 128 129 BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason)); 130 131 skb_pull(skb, sizeof(*rej)); 132 133 return 0; 134} 135 136static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, 137 struct a2mp_cmd *hdr) 138{ 139 struct a2mp_discov_req *req = (void *) skb->data; 140 u16 len = le16_to_cpu(hdr->len); 141 struct a2mp_discov_rsp *rsp; 142 u16 ext_feat; 143 u8 num_ctrl; 144 struct hci_dev *hdev; 145 146 if (len < sizeof(*req)) 147 return -EINVAL; 148 149 skb_pull(skb, sizeof(*req)); 150 151 ext_feat = le16_to_cpu(req->ext_feat); 152 153 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat); 154 155 /* check that packet is not broken for now */ 156 while (ext_feat & A2MP_FEAT_EXT) { 157 if (len < sizeof(ext_feat)) 158 return -EINVAL; 159 160 ext_feat = get_unaligned_le16(skb->data); 161 BT_DBG("efm 0x%4.4x", ext_feat); 162 len -= sizeof(ext_feat); 163 skb_pull(skb, sizeof(ext_feat)); 164 } 165 166 read_lock(&hci_dev_list_lock); 167 168 /* at minimum the BR/EDR needs to be listed */ 169 num_ctrl = 1; 170 171 list_for_each_entry(hdev, &hci_dev_list, list) { 172 if (hdev->dev_type == HCI_AMP) 173 num_ctrl++; 174 } 175 176 len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp); 177 rsp = kmalloc(len, GFP_ATOMIC); 178 if (!rsp) { 179 read_unlock(&hci_dev_list_lock); 180 return -ENOMEM; 181 } 182 183 rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); 184 rsp->ext_feat = 0; 185 186 __a2mp_add_cl(mgr, rsp->cl); 187 188 read_unlock(&hci_dev_list_lock); 189 190 a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp); 191 192 kfree(rsp); 193 return 0; 194} 195 196static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 197 struct a2mp_cmd *hdr) 198{ 199 struct a2mp_discov_rsp *rsp = (void *) skb->data; 200 u16 len = le16_to_cpu(hdr->len); 201 struct a2mp_cl *cl; 202 u16 ext_feat; 203 bool found = false; 204 205 if (len < sizeof(*rsp)) 206 return -EINVAL; 207 208 len -= sizeof(*rsp); 209 skb_pull(skb, sizeof(*rsp)); 210 211 ext_feat = le16_to_cpu(rsp->ext_feat); 212 213 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat); 214 215 /* check that packet is not broken for now */ 216 while (ext_feat & A2MP_FEAT_EXT) { 217 if (len < sizeof(ext_feat)) 218 return -EINVAL; 219 220 ext_feat = get_unaligned_le16(skb->data); 221 BT_DBG("efm 0x%4.4x", ext_feat); 222 len -= sizeof(ext_feat); 223 skb_pull(skb, sizeof(ext_feat)); 224 } 225 226 cl = (void *) skb->data; 227 while (len >= sizeof(*cl)) { 228 BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type, 229 cl->status); 230 231 if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) { 232 struct a2mp_info_req req; 233 234 found = true; 235 req.id = cl->id; 236 a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr), 237 sizeof(req), &req); 238 } 239 240 len -= sizeof(*cl); 241 cl = (void *) skb_pull(skb, sizeof(*cl)); 242 } 243 244 /* Fall back to L2CAP init sequence */ 245 if (!found) { 246 struct l2cap_conn *conn = mgr->l2cap_conn; 247 struct l2cap_chan *chan; 248 249 mutex_lock(&conn->chan_lock); 250 251 list_for_each_entry(chan, &conn->chan_l, list) { 252 253 BT_DBG("chan %p state %s", chan, 254 state_to_string(chan->state)); 255 256 if (chan->scid == L2CAP_CID_A2MP) 257 continue; 258 259 l2cap_chan_lock(chan); 260 261 if (chan->state == BT_CONNECT) 262 l2cap_send_conn_req(chan); 263 264 l2cap_chan_unlock(chan); 265 } 266 267 mutex_unlock(&conn->chan_lock); 268 } 269 270 return 0; 271} 272 273static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, 274 struct a2mp_cmd *hdr) 275{ 276 struct a2mp_cl *cl = (void *) skb->data; 277 278 while (skb->len >= sizeof(*cl)) { 279 BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, 280 cl->status); 281 cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); 282 } 283 284 /* TODO send A2MP_CHANGE_RSP */ 285 286 return 0; 287} 288 289static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, 290 struct a2mp_cmd *hdr) 291{ 292 struct a2mp_info_req *req = (void *) skb->data; 293 struct hci_dev *hdev; 294 295 if (le16_to_cpu(hdr->len) < sizeof(*req)) 296 return -EINVAL; 297 298 BT_DBG("id %d", req->id); 299 300 hdev = hci_dev_get(req->id); 301 if (!hdev || hdev->dev_type != HCI_AMP) { 302 struct a2mp_info_rsp rsp; 303 304 rsp.id = req->id; 305 rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 306 307 a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), 308 &rsp); 309 310 goto done; 311 } 312 313 set_bit(READ_LOC_AMP_INFO, &mgr->state); 314 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL); 315 316done: 317 if (hdev) 318 hci_dev_put(hdev); 319 320 skb_pull(skb, sizeof(*req)); 321 return 0; 322} 323 324static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 325 struct a2mp_cmd *hdr) 326{ 327 struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data; 328 struct a2mp_amp_assoc_req req; 329 struct amp_ctrl *ctrl; 330 331 if (le16_to_cpu(hdr->len) < sizeof(*rsp)) 332 return -EINVAL; 333 334 BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status); 335 336 if (rsp->status) 337 return -EINVAL; 338 339 ctrl = amp_ctrl_add(mgr, rsp->id); 340 if (!ctrl) 341 return -ENOMEM; 342 343 req.id = rsp->id; 344 a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req), 345 &req); 346 347 skb_pull(skb, sizeof(*rsp)); 348 return 0; 349} 350 351static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, 352 struct a2mp_cmd *hdr) 353{ 354 struct a2mp_amp_assoc_req *req = (void *) skb->data; 355 struct hci_dev *hdev; 356 struct amp_mgr *tmp; 357 358 if (le16_to_cpu(hdr->len) < sizeof(*req)) 359 return -EINVAL; 360 361 BT_DBG("id %d", req->id); 362 363 /* Make sure that other request is not processed */ 364 tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); 365 366 hdev = hci_dev_get(req->id); 367 if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) { 368 struct a2mp_amp_assoc_rsp rsp; 369 rsp.id = req->id; 370 371 if (tmp) { 372 rsp.status = A2MP_STATUS_COLLISION_OCCURED; 373 amp_mgr_put(tmp); 374 } else { 375 rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 376 } 377 378 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), 379 &rsp); 380 381 goto done; 382 } 383 384 amp_read_loc_assoc(hdev, mgr); 385 386done: 387 if (hdev) 388 hci_dev_put(hdev); 389 390 skb_pull(skb, sizeof(*req)); 391 return 0; 392} 393 394static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 395 struct a2mp_cmd *hdr) 396{ 397 struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data; 398 u16 len = le16_to_cpu(hdr->len); 399 struct hci_dev *hdev; 400 struct amp_ctrl *ctrl; 401 struct hci_conn *hcon; 402 size_t assoc_len; 403 404 if (len < sizeof(*rsp)) 405 return -EINVAL; 406 407 assoc_len = len - sizeof(*rsp); 408 409 BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status, 410 assoc_len); 411 412 if (rsp->status) 413 return -EINVAL; 414 415 /* Save remote ASSOC data */ 416 ctrl = amp_ctrl_lookup(mgr, rsp->id); 417 if (ctrl) { 418 u8 *assoc; 419 420 assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL); 421 if (!assoc) { 422 amp_ctrl_put(ctrl); 423 return -ENOMEM; 424 } 425 426 ctrl->assoc = assoc; 427 ctrl->assoc_len = assoc_len; 428 ctrl->assoc_rem_len = assoc_len; 429 ctrl->assoc_len_so_far = 0; 430 431 amp_ctrl_put(ctrl); 432 } 433 434 /* Create Phys Link */ 435 hdev = hci_dev_get(rsp->id); 436 if (!hdev) 437 return -EINVAL; 438 439 hcon = phylink_add(hdev, mgr, rsp->id, true); 440 if (!hcon) 441 goto done; 442 443 BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id); 444 445 mgr->bredr_chan->remote_amp_id = rsp->id; 446 447 amp_create_phylink(hdev, mgr, hcon); 448 449done: 450 hci_dev_put(hdev); 451 skb_pull(skb, len); 452 return 0; 453} 454 455static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, 456 struct a2mp_cmd *hdr) 457{ 458 struct a2mp_physlink_req *req = (void *) skb->data; 459 460 struct a2mp_physlink_rsp rsp; 461 struct hci_dev *hdev; 462 struct hci_conn *hcon; 463 struct amp_ctrl *ctrl; 464 465 if (le16_to_cpu(hdr->len) < sizeof(*req)) 466 return -EINVAL; 467 468 BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); 469 470 rsp.local_id = req->remote_id; 471 rsp.remote_id = req->local_id; 472 473 hdev = hci_dev_get(req->remote_id); 474 if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) { 475 rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 476 goto send_rsp; 477 } 478 479 ctrl = amp_ctrl_lookup(mgr, rsp.remote_id); 480 if (!ctrl) { 481 ctrl = amp_ctrl_add(mgr, rsp.remote_id); 482 if (ctrl) { 483 amp_ctrl_get(ctrl); 484 } else { 485 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; 486 goto send_rsp; 487 } 488 } 489 490 if (ctrl) { 491 size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req); 492 u8 *assoc; 493 494 assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL); 495 if (!assoc) { 496 amp_ctrl_put(ctrl); 497 return -ENOMEM; 498 } 499 500 ctrl->assoc = assoc; 501 ctrl->assoc_len = assoc_len; 502 ctrl->assoc_rem_len = assoc_len; 503 ctrl->assoc_len_so_far = 0; 504 505 amp_ctrl_put(ctrl); 506 } 507 508 hcon = phylink_add(hdev, mgr, req->local_id, false); 509 if (hcon) { 510 amp_accept_phylink(hdev, mgr, hcon); 511 rsp.status = A2MP_STATUS_SUCCESS; 512 } else { 513 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; 514 } 515 516send_rsp: 517 if (hdev) 518 hci_dev_put(hdev); 519 520 /* Reply error now and success after HCI Write Remote AMP Assoc 521 command complete with success status 522 */ 523 if (rsp.status != A2MP_STATUS_SUCCESS) { 524 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, 525 sizeof(rsp), &rsp); 526 } else { 527 set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state); 528 mgr->ident = hdr->ident; 529 } 530 531 skb_pull(skb, le16_to_cpu(hdr->len)); 532 return 0; 533} 534 535static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, 536 struct a2mp_cmd *hdr) 537{ 538 struct a2mp_physlink_req *req = (void *) skb->data; 539 struct a2mp_physlink_rsp rsp; 540 struct hci_dev *hdev; 541 struct hci_conn *hcon; 542 543 if (le16_to_cpu(hdr->len) < sizeof(*req)) 544 return -EINVAL; 545 546 BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); 547 548 rsp.local_id = req->remote_id; 549 rsp.remote_id = req->local_id; 550 rsp.status = A2MP_STATUS_SUCCESS; 551 552 hdev = hci_dev_get(req->remote_id); 553 if (!hdev) { 554 rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 555 goto send_rsp; 556 } 557 558 hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, 559 &mgr->l2cap_conn->hcon->dst); 560 if (!hcon) { 561 BT_ERR("No phys link exist"); 562 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS; 563 goto clean; 564 } 565 566 /* TODO Disconnect Phys Link here */ 567 568clean: 569 hci_dev_put(hdev); 570 571send_rsp: 572 a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); 573 574 skb_pull(skb, sizeof(*req)); 575 return 0; 576} 577 578static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 579 struct a2mp_cmd *hdr) 580{ 581 BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code); 582 583 skb_pull(skb, le16_to_cpu(hdr->len)); 584 return 0; 585} 586 587/* Handle A2MP signalling */ 588static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) 589{ 590 struct a2mp_cmd *hdr; 591 struct amp_mgr *mgr = chan->data; 592 int err = 0; 593 594 amp_mgr_get(mgr); 595 596 while (skb->len >= sizeof(*hdr)) { 597 u16 len; 598 599 hdr = (void *) skb->data; 600 len = le16_to_cpu(hdr->len); 601 602 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len); 603 604 skb_pull(skb, sizeof(*hdr)); 605 606 if (len > skb->len || !hdr->ident) { 607 err = -EINVAL; 608 break; 609 } 610 611 mgr->ident = hdr->ident; 612 613 switch (hdr->code) { 614 case A2MP_COMMAND_REJ: 615 a2mp_command_rej(mgr, skb, hdr); 616 break; 617 618 case A2MP_DISCOVER_REQ: 619 err = a2mp_discover_req(mgr, skb, hdr); 620 break; 621 622 case A2MP_CHANGE_NOTIFY: 623 err = a2mp_change_notify(mgr, skb, hdr); 624 break; 625 626 case A2MP_GETINFO_REQ: 627 err = a2mp_getinfo_req(mgr, skb, hdr); 628 break; 629 630 case A2MP_GETAMPASSOC_REQ: 631 err = a2mp_getampassoc_req(mgr, skb, hdr); 632 break; 633 634 case A2MP_CREATEPHYSLINK_REQ: 635 err = a2mp_createphyslink_req(mgr, skb, hdr); 636 break; 637 638 case A2MP_DISCONNPHYSLINK_REQ: 639 err = a2mp_discphyslink_req(mgr, skb, hdr); 640 break; 641 642 case A2MP_DISCOVER_RSP: 643 err = a2mp_discover_rsp(mgr, skb, hdr); 644 break; 645 646 case A2MP_GETINFO_RSP: 647 err = a2mp_getinfo_rsp(mgr, skb, hdr); 648 break; 649 650 case A2MP_GETAMPASSOC_RSP: 651 err = a2mp_getampassoc_rsp(mgr, skb, hdr); 652 break; 653 654 case A2MP_CHANGE_RSP: 655 case A2MP_CREATEPHYSLINK_RSP: 656 case A2MP_DISCONNPHYSLINK_RSP: 657 err = a2mp_cmd_rsp(mgr, skb, hdr); 658 break; 659 660 default: 661 BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); 662 err = -EINVAL; 663 break; 664 } 665 } 666 667 if (err) { 668 struct a2mp_cmd_rej rej; 669 670 rej.reason = cpu_to_le16(0); 671 hdr = (void *) skb->data; 672 673 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); 674 675 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), 676 &rej); 677 } 678 679 /* Always free skb and return success error code to prevent 680 from sending L2CAP Disconnect over A2MP channel */ 681 kfree_skb(skb); 682 683 amp_mgr_put(mgr); 684 685 return 0; 686} 687 688static void a2mp_chan_close_cb(struct l2cap_chan *chan) 689{ 690 l2cap_chan_put(chan); 691} 692 693static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state, 694 int err) 695{ 696 struct amp_mgr *mgr = chan->data; 697 698 if (!mgr) 699 return; 700 701 BT_DBG("chan %p state %s", chan, state_to_string(state)); 702 703 chan->state = state; 704 705 switch (state) { 706 case BT_CLOSED: 707 if (mgr) 708 amp_mgr_put(mgr); 709 break; 710 } 711} 712 713static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, 714 unsigned long hdr_len, 715 unsigned long len, int nb) 716{ 717 struct sk_buff *skb; 718 719 skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL); 720 if (!skb) 721 return ERR_PTR(-ENOMEM); 722 723 return skb; 724} 725 726static const struct l2cap_ops a2mp_chan_ops = { 727 .name = "L2CAP A2MP channel", 728 .recv = a2mp_chan_recv_cb, 729 .close = a2mp_chan_close_cb, 730 .state_change = a2mp_chan_state_change_cb, 731 .alloc_skb = a2mp_chan_alloc_skb_cb, 732 733 /* Not implemented for A2MP */ 734 .new_connection = l2cap_chan_no_new_connection, 735 .teardown = l2cap_chan_no_teardown, 736 .ready = l2cap_chan_no_ready, 737 .defer = l2cap_chan_no_defer, 738 .resume = l2cap_chan_no_resume, 739 .set_shutdown = l2cap_chan_no_set_shutdown, 740 .get_sndtimeo = l2cap_chan_no_get_sndtimeo, 741}; 742 743static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked) 744{ 745 struct l2cap_chan *chan; 746 int err; 747 748 chan = l2cap_chan_create(); 749 if (!chan) 750 return NULL; 751 752 BT_DBG("chan %p", chan); 753 754 chan->chan_type = L2CAP_CHAN_FIXED; 755 chan->scid = L2CAP_CID_A2MP; 756 chan->dcid = L2CAP_CID_A2MP; 757 chan->omtu = L2CAP_A2MP_DEFAULT_MTU; 758 chan->imtu = L2CAP_A2MP_DEFAULT_MTU; 759 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; 760 761 chan->ops = &a2mp_chan_ops; 762 763 l2cap_chan_set_defaults(chan); 764 chan->remote_max_tx = chan->max_tx; 765 chan->remote_tx_win = chan->tx_win; 766 767 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; 768 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; 769 770 skb_queue_head_init(&chan->tx_q); 771 772 chan->mode = L2CAP_MODE_ERTM; 773 774 err = l2cap_ertm_init(chan); 775 if (err < 0) { 776 l2cap_chan_del(chan, 0); 777 return NULL; 778 } 779 780 chan->conf_state = 0; 781 782 if (locked) 783 __l2cap_chan_add(conn, chan); 784 else 785 l2cap_chan_add(conn, chan); 786 787 chan->remote_mps = chan->omtu; 788 chan->mps = chan->omtu; 789 790 chan->state = BT_CONNECTED; 791 792 return chan; 793} 794 795/* AMP Manager functions */ 796struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr) 797{ 798 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); 799 800 kref_get(&mgr->kref); 801 802 return mgr; 803} 804 805static void amp_mgr_destroy(struct kref *kref) 806{ 807 struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); 808 809 BT_DBG("mgr %p", mgr); 810 811 mutex_lock(&_mgr_list_lock); 812 list_del(&mgr->list); 813 mutex_unlock(&_mgr_list_lock); 814 815 amp_ctrl_list_flush(mgr); 816 kfree(mgr); 817} 818 819int amp_mgr_put(struct amp_mgr *mgr) 820{ 821 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); 822 823 return kref_put(&mgr->kref, &_mgr_destroy); 824} 825 826static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked) 827{ 828 struct amp_mgr *mgr; 829 struct l2cap_chan *chan; 830 831 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); 832 if (!mgr) 833 return NULL; 834 835 BT_DBG("conn %p mgr %p", conn, mgr); 836 837 mgr->l2cap_conn = conn; 838 839 chan = a2mp_chan_open(conn, locked); 840 if (!chan) { 841 kfree(mgr); 842 return NULL; 843 } 844 845 mgr->a2mp_chan = chan; 846 chan->data = mgr; 847 848 conn->hcon->amp_mgr = mgr; 849 850 kref_init(&mgr->kref); 851 852 /* Remote AMP ctrl list initialization */ 853 INIT_LIST_HEAD(&mgr->amp_ctrls); 854 mutex_init(&mgr->amp_ctrls_lock); 855 856 mutex_lock(&_mgr_list_lock); 857 list_add(&mgr->list, &_mgr_list); 858 mutex_unlock(&_mgr_list_lock); 859 860 return mgr; 861} 862 863struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, 864 struct sk_buff *skb) 865{ 866 struct amp_mgr *mgr; 867 868 if (conn->hcon->type != ACL_LINK) 869 return NULL; 870 871 mgr = amp_mgr_create(conn, false); 872 if (!mgr) { 873 BT_ERR("Could not create AMP manager"); 874 return NULL; 875 } 876 877 BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan); 878 879 return mgr->a2mp_chan; 880} 881 882void a2mp_send_getinfo_rsp(struct hci_dev *hdev) 883{ 884 struct amp_mgr *mgr; 885 struct a2mp_info_rsp rsp; 886 887 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO); 888 if (!mgr) 889 return; 890 891 BT_DBG("%s mgr %p", hdev->name, mgr); 892 893 rsp.id = hdev->id; 894 rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 895 896 if (hdev->amp_type != AMP_TYPE_BREDR) { 897 rsp.status = 0; 898 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw); 899 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw); 900 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency); 901 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap); 902 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size); 903 } 904 905 a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp); 906 amp_mgr_put(mgr); 907} 908 909void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status) 910{ 911 struct amp_mgr *mgr; 912 struct amp_assoc *loc_assoc = &hdev->loc_assoc; 913 struct a2mp_amp_assoc_rsp *rsp; 914 size_t len; 915 916 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); 917 if (!mgr) 918 return; 919 920 BT_DBG("%s mgr %p", hdev->name, mgr); 921 922 len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len; 923 rsp = kzalloc(len, GFP_KERNEL); 924 if (!rsp) { 925 amp_mgr_put(mgr); 926 return; 927 } 928 929 rsp->id = hdev->id; 930 931 if (status) { 932 rsp->status = A2MP_STATUS_INVALID_CTRL_ID; 933 } else { 934 rsp->status = A2MP_STATUS_SUCCESS; 935 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len); 936 } 937 938 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp); 939 amp_mgr_put(mgr); 940 kfree(rsp); 941} 942 943void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status) 944{ 945 struct amp_mgr *mgr; 946 struct amp_assoc *loc_assoc = &hdev->loc_assoc; 947 struct a2mp_physlink_req *req; 948 struct l2cap_chan *bredr_chan; 949 size_t len; 950 951 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL); 952 if (!mgr) 953 return; 954 955 len = sizeof(*req) + loc_assoc->len; 956 957 BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len); 958 959 req = kzalloc(len, GFP_KERNEL); 960 if (!req) { 961 amp_mgr_put(mgr); 962 return; 963 } 964 965 bredr_chan = mgr->bredr_chan; 966 if (!bredr_chan) 967 goto clean; 968 969 req->local_id = hdev->id; 970 req->remote_id = bredr_chan->remote_amp_id; 971 memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len); 972 973 a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req); 974 975clean: 976 amp_mgr_put(mgr); 977 kfree(req); 978} 979 980void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status) 981{ 982 struct amp_mgr *mgr; 983 struct a2mp_physlink_rsp rsp; 984 struct hci_conn *hs_hcon; 985 986 mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC); 987 if (!mgr) 988 return; 989 990 hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT); 991 if (!hs_hcon) { 992 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; 993 } else { 994 rsp.remote_id = hs_hcon->remote_id; 995 rsp.status = A2MP_STATUS_SUCCESS; 996 } 997 998 BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon, 999 status); 1000 1001 rsp.local_id = hdev->id; 1002 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp); 1003 amp_mgr_put(mgr); 1004} 1005 1006void a2mp_discover_amp(struct l2cap_chan *chan) 1007{ 1008 struct l2cap_conn *conn = chan->conn; 1009 struct amp_mgr *mgr = conn->hcon->amp_mgr; 1010 struct a2mp_discov_req req; 1011 1012 BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr); 1013 1014 if (!mgr) { 1015 mgr = amp_mgr_create(conn, true); 1016 if (!mgr) 1017 return; 1018 } 1019 1020 mgr->bredr_chan = chan; 1021 1022 req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); 1023 req.ext_feat = 0; 1024 a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req); 1025} 1026