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