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