Pileus Git - ~andy/linux/blob - 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 #include <net/bluetooth/a2mp.h>
  19
  20 /* A2MP build & send command helper functions */
  21 static 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
  40 static 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
  66 static 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 */
  74 static 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 */
  97 static 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
 112 static 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
 164 static 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
 180 static 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
 214 static 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
 238 clean:
 239         if (hdev)
 240                 hci_dev_put(hdev);
 241
 242         skb_pull(skb, sizeof(*req));
 243         return 0;
 244 }
 245
 246 static 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
 272 send_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
 283 static 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
 309 send_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
 316 static 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 */
 326 static 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
 413 static void a2mp_chan_close_cb(struct l2cap_chan *chan)
 414 {
 415         l2cap_chan_destroy(chan);
 416 }
 417
 418 static 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
 437 static 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
 443 static 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
 456 static 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 */
 502 void 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
 509 static 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
 518 int 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
 525 static 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
 554 struct 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 }