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