linux/net/bluetooth/a2mp.c
<<
>>
Prefs
   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#include <net/bluetooth/a2mp.h>
  19
  20/* A2MP build & send command helper functions */
  21static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
  22{
  23        struct a2mp_cmd *cmd;
  24        int plen;
  25
  26        plen = sizeof(*cmd) + len;
  27        cmd = kzalloc(plen, GFP_KERNEL);
  28        if (!cmd)
  29                return NULL;
  30
  31        cmd->code = code;
  32        cmd->ident = ident;
  33        cmd->len = cpu_to_le16(len);
  34
  35        memcpy(cmd->data, data, len);
  36
  37        return cmd;
  38}
  39
  40static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
  41                      void *data)
  42{
  43        struct l2cap_chan *chan = mgr->a2mp_chan;
  44        struct a2mp_cmd *cmd;
  45        u16 total_len = len + sizeof(*cmd);
  46        struct kvec iv;
  47        struct msghdr msg;
  48
  49        cmd = __a2mp_build(code, ident, len, data);
  50        if (!cmd)
  51                return;
  52
  53        iv.iov_base = cmd;
  54        iv.iov_len = total_len;
  55
  56        memset(&msg, 0, sizeof(msg));
  57
  58        msg.msg_iov = (struct iovec *) &iv;
  59        msg.msg_iovlen = 1;
  60
  61        l2cap_chan_send(chan, &msg, total_len, 0);
  62
  63        kfree(cmd);
  64}
  65
  66static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
  67{
  68        cl->id = 0;
  69        cl->type = 0;
  70        cl->status = 1;
  71}
  72
  73/* hci_dev_list shall be locked */
  74static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
  75{
  76        int i = 0;
  77        struct hci_dev *hdev;
  78
  79        __a2mp_cl_bredr(cl);
  80
  81        list_for_each_entry(hdev, &hci_dev_list, list) {
  82                /* Iterate through AMP controllers */
  83                if (hdev->id == HCI_BREDR_ID)
  84                        continue;
  85
  86                /* Starting from second entry */
  87                if (++i >= num_ctrl)
  88                        return;
  89
  90                cl[i].id = hdev->id;
  91                cl[i].type = hdev->amp_type;
  92                cl[i].status = hdev->amp_status;
  93        }
  94}
  95
  96/* Processing A2MP messages */
  97static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
  98                            struct a2mp_cmd *hdr)
  99{
 100        struct a2mp_cmd_rej *rej = (void *) skb->data;
 101
 102        if (le16_to_cpu(hdr->len) < sizeof(*rej))
 103                return -EINVAL;
 104
 105        BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
 106
 107        skb_pull(skb, sizeof(*rej));
 108
 109        return 0;
 110}
 111
 112static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 113                             struct a2mp_cmd *hdr)
 114{
 115        struct a2mp_discov_req *req = (void *) skb->data;
 116        u16 len = le16_to_cpu(hdr->len);
 117        struct a2mp_discov_rsp *rsp;
 118        u16 ext_feat;
 119        u8 num_ctrl;
 120
 121        if (len < sizeof(*req))
 122                return -EINVAL;
 123
 124        skb_pull(skb, sizeof(*req));
 125
 126        ext_feat = le16_to_cpu(req->ext_feat);
 127
 128        BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
 129
 130        /* check that packet is not broken for now */
 131        while (ext_feat & A2MP_FEAT_EXT) {
 132                if (len < sizeof(ext_feat))
 133                        return -EINVAL;
 134
 135                ext_feat = get_unaligned_le16(skb->data);
 136                BT_DBG("efm 0x%4.4x", ext_feat);
 137                len -= sizeof(ext_feat);
 138                skb_pull(skb, sizeof(ext_feat));
 139        }
 140
 141        read_lock(&hci_dev_list_lock);
 142
 143        num_ctrl = __hci_num_ctrl();
 144        len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
 145        rsp = kmalloc(len, GFP_ATOMIC);
 146        if (!rsp) {
 147                read_unlock(&hci_dev_list_lock);
 148                return -ENOMEM;
 149        }
 150
 151        rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
 152        rsp->ext_feat = 0;
 153
 154        __a2mp_add_cl(mgr, rsp->cl, num_ctrl);
 155
 156        read_unlock(&hci_dev_list_lock);
 157
 158        a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
 159
 160        kfree(rsp);
 161        return 0;
 162}
 163
 164static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 165                              struct a2mp_cmd *hdr)
 166{
 167        struct a2mp_cl *cl = (void *) skb->data;
 168
 169        while (skb->len >= sizeof(*cl)) {
 170                BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
 171                       cl->status);
 172                cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
 173        }
 174
 175        /* TODO send A2MP_CHANGE_RSP */
 176
 177        return 0;
 178}
 179
 180static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 181                            struct a2mp_cmd *hdr)
 182{
 183        struct a2mp_info_req *req  = (void *) skb->data;
 184        struct a2mp_info_rsp rsp;
 185        struct hci_dev *hdev;
 186
 187        if (le16_to_cpu(hdr->len) < sizeof(*req))
 188                return -EINVAL;
 189
 190        BT_DBG("id %d", req->id);
 191
 192        rsp.id = req->id;
 193        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 194
 195        hdev = hci_dev_get(req->id);
 196        if (hdev && hdev->amp_type != HCI_BREDR) {
 197                rsp.status = 0;
 198                rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
 199                rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
 200                rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
 201                rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
 202                rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
 203        }
 204
 205        if (hdev)
 206                hci_dev_put(hdev);
 207
 208        a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
 209
 210        skb_pull(skb, sizeof(*req));
 211        return 0;
 212}
 213
 214static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 215                                struct a2mp_cmd *hdr)
 216{
 217        struct a2mp_amp_assoc_req *req = (void *) skb->data;
 218        struct hci_dev *hdev;
 219
 220        if (le16_to_cpu(hdr->len) < sizeof(*req))
 221                return -EINVAL;
 222
 223        BT_DBG("id %d", req->id);
 224
 225        hdev = hci_dev_get(req->id);
 226        if (!hdev || hdev->amp_type == HCI_BREDR) {
 227                struct a2mp_amp_assoc_rsp rsp;
 228                rsp.id = req->id;
 229                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 230
 231                a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 232                          &rsp);
 233                goto clean;
 234        }
 235
 236        /* Placeholder for HCI Read AMP Assoc */
 237
 238clean:
 239        if (hdev)
 240                hci_dev_put(hdev);
 241
 242        skb_pull(skb, sizeof(*req));
 243        return 0;
 244}
 245
 246static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 247                                   struct a2mp_cmd *hdr)
 248{
 249        struct a2mp_physlink_req *req = (void *) skb->data;
 250
 251        struct a2mp_physlink_rsp rsp;
 252        struct hci_dev *hdev;
 253
 254        if (le16_to_cpu(hdr->len) < sizeof(*req))
 255                return -EINVAL;
 256
 257        BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
 258
 259        rsp.local_id = req->remote_id;
 260        rsp.remote_id = req->local_id;
 261
 262        hdev = hci_dev_get(req->remote_id);
 263        if (!hdev || hdev->amp_type != HCI_AMP) {
 264                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 265                goto send_rsp;
 266        }
 267
 268        /* TODO process physlink create */
 269
 270        rsp.status = A2MP_STATUS_SUCCESS;
 271
 272send_rsp:
 273        if (hdev)
 274                hci_dev_put(hdev);
 275
 276        a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
 277                  &rsp);
 278
 279        skb_pull(skb, le16_to_cpu(hdr->len));
 280        return 0;
 281}
 282
 283static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 284                                 struct a2mp_cmd *hdr)
 285{
 286        struct a2mp_physlink_req *req = (void *) skb->data;
 287        struct a2mp_physlink_rsp rsp;
 288        struct hci_dev *hdev;
 289
 290        if (le16_to_cpu(hdr->len) < sizeof(*req))
 291                return -EINVAL;
 292
 293        BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
 294
 295        rsp.local_id = req->remote_id;
 296        rsp.remote_id = req->local_id;
 297        rsp.status = A2MP_STATUS_SUCCESS;
 298
 299        hdev = hci_dev_get(req->local_id);
 300        if (!hdev) {
 301                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 302                goto send_rsp;
 303        }
 304
 305        /* TODO Disconnect Phys Link here */
 306
 307        hci_dev_put(hdev);
 308
 309send_rsp:
 310        a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
 311
 312        skb_pull(skb, sizeof(*req));
 313        return 0;
 314}
 315
 316static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 317                               struct a2mp_cmd *hdr)
 318{
 319        BT_DBG("ident %d code %d", hdr->ident, hdr->code);
 320
 321        skb_pull(skb, le16_to_cpu(hdr->len));
 322        return 0;
 323}
 324
 325/* Handle A2MP signalling */
 326static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 327{
 328        struct a2mp_cmd *hdr = (void *) skb->data;
 329        struct amp_mgr *mgr = chan->data;
 330        int err = 0;
 331
 332        amp_mgr_get(mgr);
 333
 334        while (skb->len >= sizeof(*hdr)) {
 335                struct a2mp_cmd *hdr = (void *) skb->data;
 336                u16 len = le16_to_cpu(hdr->len);
 337
 338                BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);
 339
 340                skb_pull(skb, sizeof(*hdr));
 341
 342                if (len > skb->len || !hdr->ident) {
 343                        err = -EINVAL;
 344                        break;
 345                }
 346
 347                mgr->ident = hdr->ident;
 348
 349                switch (hdr->code) {
 350                case A2MP_COMMAND_REJ:
 351                        a2mp_command_rej(mgr, skb, hdr);
 352                        break;
 353
 354                case A2MP_DISCOVER_REQ:
 355                        err = a2mp_discover_req(mgr, skb, hdr);
 356                        break;
 357
 358                case A2MP_CHANGE_NOTIFY:
 359                        err = a2mp_change_notify(mgr, skb, hdr);
 360                        break;
 361
 362                case A2MP_GETINFO_REQ:
 363                        err = a2mp_getinfo_req(mgr, skb, hdr);
 364                        break;
 365
 366                case A2MP_GETAMPASSOC_REQ:
 367                        err = a2mp_getampassoc_req(mgr, skb, hdr);
 368                        break;
 369
 370                case A2MP_CREATEPHYSLINK_REQ:
 371                        err = a2mp_createphyslink_req(mgr, skb, hdr);
 372                        break;
 373
 374                case A2MP_DISCONNPHYSLINK_REQ:
 375                        err = a2mp_discphyslink_req(mgr, skb, hdr);
 376                        break;
 377
 378                case A2MP_CHANGE_RSP:
 379                case A2MP_DISCOVER_RSP:
 380                case A2MP_GETINFO_RSP:
 381                case A2MP_GETAMPASSOC_RSP:
 382                case A2MP_CREATEPHYSLINK_RSP:
 383                case A2MP_DISCONNPHYSLINK_RSP:
 384                        err = a2mp_cmd_rsp(mgr, skb, hdr);
 385                        break;
 386
 387                default:
 388                        BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
 389                        err = -EINVAL;
 390                        break;
 391                }
 392        }
 393
 394        if (err) {
 395                struct a2mp_cmd_rej rej;
 396                rej.reason = __constant_cpu_to_le16(0);
 397
 398                BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
 399
 400                a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
 401                          &rej);
 402        }
 403
 404        /* Always free skb and return success error code to prevent
 405           from sending L2CAP Disconnect over A2MP channel */
 406        kfree_skb(skb);
 407
 408        amp_mgr_put(mgr);
 409
 410        return 0;
 411}
 412
 413static void a2mp_chan_close_cb(struct l2cap_chan *chan)
 414{
 415        l2cap_chan_destroy(chan);
 416}
 417
 418static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
 419{
 420        struct amp_mgr *mgr = chan->data;
 421
 422        if (!mgr)
 423                return;
 424
 425        BT_DBG("chan %p state %s", chan, state_to_string(state));
 426
 427        chan->state = state;
 428
 429        switch (state) {
 430        case BT_CLOSED:
 431                if (mgr)
 432                        amp_mgr_put(mgr);
 433                break;
 434        }
 435}
 436
 437static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 438                                              unsigned long len, int nb)
 439{
 440        return bt_skb_alloc(len, GFP_KERNEL);
 441}
 442
 443static struct l2cap_ops a2mp_chan_ops = {
 444        .name = "L2CAP A2MP channel",
 445        .recv = a2mp_chan_recv_cb,
 446        .close = a2mp_chan_close_cb,
 447        .state_change = a2mp_chan_state_change_cb,
 448        .alloc_skb = a2mp_chan_alloc_skb_cb,
 449
 450        /* Not implemented for A2MP */
 451        .new_connection = l2cap_chan_no_new_connection,
 452        .teardown = l2cap_chan_no_teardown,
 453        .ready = l2cap_chan_no_ready,
 454};
 455
 456static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 457{
 458        struct l2cap_chan *chan;
 459        int err;
 460
 461        chan = l2cap_chan_create();
 462        if (!chan)
 463                return NULL;
 464
 465        BT_DBG("chan %p", chan);
 466
 467        chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
 468        chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 469
 470        chan->ops = &a2mp_chan_ops;
 471
 472        l2cap_chan_set_defaults(chan);
 473        chan->remote_max_tx = chan->max_tx;
 474        chan->remote_tx_win = chan->tx_win;
 475
 476        chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
 477        chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
 478
 479        skb_queue_head_init(&chan->tx_q);
 480
 481        chan->mode = L2CAP_MODE_ERTM;
 482
 483        err = l2cap_ertm_init(chan);
 484        if (err < 0) {
 485                l2cap_chan_del(chan, 0);
 486                return NULL;
 487        }
 488
 489        chan->conf_state = 0;
 490
 491        l2cap_chan_add(conn, chan);
 492
 493        chan->remote_mps = chan->omtu;
 494        chan->mps = chan->omtu;
 495
 496        chan->state = BT_CONNECTED;
 497
 498        return chan;
 499}
 500
 501/* AMP Manager functions */
 502void amp_mgr_get(struct amp_mgr *mgr)
 503{
 504        BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 505
 506        kref_get(&mgr->kref);
 507}
 508
 509static void amp_mgr_destroy(struct kref *kref)
 510{
 511        struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
 512
 513        BT_DBG("mgr %p", mgr);
 514
 515        kfree(mgr);
 516}
 517
 518int amp_mgr_put(struct amp_mgr *mgr)
 519{
 520        BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 521
 522        return kref_put(&mgr->kref, &amp_mgr_destroy);
 523}
 524
 525static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 526{
 527        struct amp_mgr *mgr;
 528        struct l2cap_chan *chan;
 529
 530        mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
 531        if (!mgr)
 532                return NULL;
 533
 534        BT_DBG("conn %p mgr %p", conn, mgr);
 535
 536        mgr->l2cap_conn = conn;
 537
 538        chan = a2mp_chan_open(conn);
 539        if (!chan) {
 540                kfree(mgr);
 541                return NULL;
 542        }
 543
 544        mgr->a2mp_chan = chan;
 545        chan->data = mgr;
 546
 547        conn->hcon->amp_mgr = mgr;
 548
 549        kref_init(&mgr->kref);
 550
 551        return mgr;
 552}
 553
 554struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 555                                       struct sk_buff *skb)
 556{
 557        struct amp_mgr *mgr;
 558
 559        mgr = amp_mgr_create(conn);
 560        if (!mgr) {
 561                BT_ERR("Could not create AMP manager");
 562                return NULL;
 563        }
 564
 565        BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
 566
 567        return mgr->a2mp_chan;
 568}
 569
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.