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