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 {
 677         struct amp_mgr *mgr = chan->data;
 678
 679         if (!mgr)
 680                 return;
 681
 682         BT_DBG("chan %p state %s", chan, state_to_string(state));
 683
 684         chan->state = state;
 685
 686         switch (state) {
 687         case BT_CLOSED:
 688                 if (mgr)
 689                         amp_mgr_put(mgr);
 690                 break;
 691         }
 692 }
 693
 694 static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 695                                               unsigned long len, int nb)
 696 {
 697         return bt_skb_alloc(len, GFP_KERNEL);
 698 }
 699
 700 static struct l2cap_ops a2mp_chan_ops = {
 701         .name = "L2CAP A2MP channel",
 702         .recv = a2mp_chan_recv_cb,
 703         .close = a2mp_chan_close_cb,
 704         .state_change = a2mp_chan_state_change_cb,
 705         .alloc_skb = a2mp_chan_alloc_skb_cb,
 706
 707         /* Not implemented for A2MP */
 708         .new_connection = l2cap_chan_no_new_connection,
 709         .teardown = l2cap_chan_no_teardown,
 710         .ready = l2cap_chan_no_ready,
 711         .defer = l2cap_chan_no_defer,
 712 };
 713
 714 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 715 {
 716         struct l2cap_chan *chan;
 717         int err;
 718
 719         chan = l2cap_chan_create();
 720         if (!chan)
 721                 return NULL;
 722
 723         BT_DBG("chan %p", chan);
 724
 725         chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
 726         chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 727
 728         chan->ops = &a2mp_chan_ops;
 729
 730         l2cap_chan_set_defaults(chan);
 731         chan->remote_max_tx = chan->max_tx;
 732         chan->remote_tx_win = chan->tx_win;
 733
 734         chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
 735         chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
 736
 737         skb_queue_head_init(&chan->tx_q);
 738
 739         chan->mode = L2CAP_MODE_ERTM;
 740
 741         err = l2cap_ertm_init(chan);
 742         if (err < 0) {
 743                 l2cap_chan_del(chan, 0);
 744                 return NULL;
 745         }
 746
 747         chan->conf_state = 0;
 748
 749         if (locked)
 750                 __l2cap_chan_add(conn, chan);
 751         else
 752                 l2cap_chan_add(conn, chan);
 753
 754         chan->remote_mps = chan->omtu;
 755         chan->mps = chan->omtu;
 756
 757         chan->state = BT_CONNECTED;
 758
 759         return chan;
 760 }
 761
 762 /* AMP Manager functions */
 763 struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
 764 {
 765         BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 766
 767         kref_get(&mgr->kref);
 768
 769         return mgr;
 770 }
 771
 772 static void amp_mgr_destroy(struct kref *kref)
 773 {
 774         struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
 775
 776         BT_DBG("mgr %p", mgr);
 777
 778         mutex_lock(&amp_mgr_list_lock);
 779         list_del(&mgr->list);
 780         mutex_unlock(&amp_mgr_list_lock);
 781
 782         amp_ctrl_list_flush(mgr);
 783         kfree(mgr);
 784 }
 785
 786 int amp_mgr_put(struct amp_mgr *mgr)
 787 {
 788         BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 789
 790         return kref_put(&mgr->kref, &amp_mgr_destroy);
 791 }
 792
 793 static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 794 {
 795         struct amp_mgr *mgr;
 796         struct l2cap_chan *chan;
 797
 798         mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
 799         if (!mgr)
 800                 return NULL;
 801
 802         BT_DBG("conn %p mgr %p", conn, mgr);
 803
 804         mgr->l2cap_conn = conn;
 805
 806         chan = a2mp_chan_open(conn, locked);
 807         if (!chan) {
 808                 kfree(mgr);
 809                 return NULL;
 810         }
 811
 812         mgr->a2mp_chan = chan;
 813         chan->data = mgr;
 814
 815         conn->hcon->amp_mgr = mgr;
 816
 817         kref_init(&mgr->kref);
 818
 819         /* Remote AMP ctrl list initialization */
 820         INIT_LIST_HEAD(&mgr->amp_ctrls);
 821         mutex_init(&mgr->amp_ctrls_lock);
 822
 823         mutex_lock(&amp_mgr_list_lock);
 824         list_add(&mgr->list, &amp_mgr_list);
 825         mutex_unlock(&amp_mgr_list_lock);
 826
 827         return mgr;
 828 }
 829
 830 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 831                                        struct sk_buff *skb)
 832 {
 833         struct amp_mgr *mgr;
 834
 835         mgr = amp_mgr_create(conn, false);
 836         if (!mgr) {
 837                 BT_ERR("Could not create AMP manager");
 838                 return NULL;
 839         }
 840
 841         BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
 842
 843         return mgr->a2mp_chan;
 844 }
 845
 846 struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 847 {
 848         struct amp_mgr *mgr;
 849
 850         mutex_lock(&amp_mgr_list_lock);
 851         list_for_each_entry(mgr, &amp_mgr_list, list) {
 852                 if (test_and_clear_bit(state, &mgr->state)) {
 853                         amp_mgr_get(mgr);
 854                         mutex_unlock(&amp_mgr_list_lock);
 855                         return mgr;
 856                 }
 857         }
 858         mutex_unlock(&amp_mgr_list_lock);
 859
 860         return NULL;
 861 }
 862
 863 void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 864 {
 865         struct amp_mgr *mgr;
 866         struct a2mp_info_rsp rsp;
 867
 868         mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
 869         if (!mgr)
 870                 return;
 871
 872         BT_DBG("%s mgr %p", hdev->name, mgr);
 873
 874         rsp.id = hdev->id;
 875         rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 876
 877         if (hdev->amp_type != AMP_TYPE_BREDR) {
 878                 rsp.status = 0;
 879                 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
 880                 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
 881                 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
 882                 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
 883                 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
 884         }
 885
 886         a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 887         amp_mgr_put(mgr);
 888 }
 889
 890 void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 891 {
 892         struct amp_mgr *mgr;
 893         struct amp_assoc *loc_assoc = &hdev->loc_assoc;
 894         struct a2mp_amp_assoc_rsp *rsp;
 895         size_t len;
 896
 897         mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
 898         if (!mgr)
 899                 return;
 900
 901         BT_DBG("%s mgr %p", hdev->name, mgr);
 902
 903         len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
 904         rsp = kzalloc(len, GFP_KERNEL);
 905         if (!rsp) {
 906                 amp_mgr_put(mgr);
 907                 return;
 908         }
 909
 910         rsp->id = hdev->id;
 911
 912         if (status) {
 913                 rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
 914         } else {
 915                 rsp->status = A2MP_STATUS_SUCCESS;
 916                 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
 917         }
 918
 919         a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
 920         amp_mgr_put(mgr);
 921         kfree(rsp);
 922 }
 923
 924 void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
 925 {
 926         struct amp_mgr *mgr;
 927         struct amp_assoc *loc_assoc = &hdev->loc_assoc;
 928         struct a2mp_physlink_req *req;
 929         struct l2cap_chan *bredr_chan;
 930         size_t len;
 931
 932         mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
 933         if (!mgr)
 934                 return;
 935
 936         len = sizeof(*req) + loc_assoc->len;
 937
 938         BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
 939
 940         req = kzalloc(len, GFP_KERNEL);
 941         if (!req) {
 942                 amp_mgr_put(mgr);
 943                 return;
 944         }
 945
 946         bredr_chan = mgr->bredr_chan;
 947         if (!bredr_chan)
 948                 goto clean;
 949
 950         req->local_id = hdev->id;
 951         req->remote_id = bredr_chan->remote_amp_id;
 952         memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
 953
 954         a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
 955
 956 clean:
 957         amp_mgr_put(mgr);
 958         kfree(req);
 959 }
 960
 961 void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
 962 {
 963         struct amp_mgr *mgr;
 964         struct a2mp_physlink_rsp rsp;
 965         struct hci_conn *hs_hcon;
 966
 967         mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
 968         if (!mgr)
 969                 return;
 970
 971         hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
 972         if (!hs_hcon) {
 973                 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
 974         } else {
 975                 rsp.remote_id = hs_hcon->remote_id;
 976                 rsp.status = A2MP_STATUS_SUCCESS;
 977         }
 978
 979         BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
 980                status);
 981
 982         rsp.local_id = hdev->id;
 983         a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
 984         amp_mgr_put(mgr);
 985 }
 986
 987 void a2mp_discover_amp(struct l2cap_chan *chan)
 988 {
 989         struct l2cap_conn *conn = chan->conn;
 990         struct amp_mgr *mgr = conn->hcon->amp_mgr;
 991         struct a2mp_discov_req req;
 992
 993         BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
 994
 995         if (!mgr) {
 996                 mgr = amp_mgr_create(conn, true);
 997                 if (!mgr)
 998                         return;
 999         }
1000
1001         mgr->bredr_chan = chan;
1002
1003         req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1004         req.ext_feat = 0;
1005         a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
1006 }