net/bluetooth/a2mp.c

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