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