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