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