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 0x%2.2x", 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;
 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                 u16 len;
 336
 337                 hdr = (void *) skb->data;
 338                 len = le16_to_cpu(hdr->len);
 339
 340                 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
 341
 342                 skb_pull(skb, sizeof(*hdr));
 343
 344                 if (len > skb->len || !hdr->ident) {
 345                         err = -EINVAL;
 346                         break;
 347                 }
 348
 349                 mgr->ident = hdr->ident;
 350
 351                 switch (hdr->code) {
 352                 case A2MP_COMMAND_REJ:
 353                         a2mp_command_rej(mgr, skb, hdr);
 354                         break;
 355
 356                 case A2MP_DISCOVER_REQ:
 357                         err = a2mp_discover_req(mgr, skb, hdr);
 358                         break;
 359
 360                 case A2MP_CHANGE_NOTIFY:
 361                         err = a2mp_change_notify(mgr, skb, hdr);
 362                         break;
 363
 364                 case A2MP_GETINFO_REQ:
 365                         err = a2mp_getinfo_req(mgr, skb, hdr);
 366                         break;
 367
 368                 case A2MP_GETAMPASSOC_REQ:
 369                         err = a2mp_getampassoc_req(mgr, skb, hdr);
 370                         break;
 371
 372                 case A2MP_CREATEPHYSLINK_REQ:
 373                         err = a2mp_createphyslink_req(mgr, skb, hdr);
 374                         break;
 375
 376                 case A2MP_DISCONNPHYSLINK_REQ:
 377                         err = a2mp_discphyslink_req(mgr, skb, hdr);
 378                         break;
 379
 380                 case A2MP_CHANGE_RSP:
 381                 case A2MP_DISCOVER_RSP:
 382                 case A2MP_GETINFO_RSP:
 383                 case A2MP_GETAMPASSOC_RSP:
 384                 case A2MP_CREATEPHYSLINK_RSP:
 385                 case A2MP_DISCONNPHYSLINK_RSP:
 386                         err = a2mp_cmd_rsp(mgr, skb, hdr);
 387                         break;
 388
 389                 default:
 390                         BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
 391                         err = -EINVAL;
 392                         break;
 393                 }
 394         }
 395
 396         if (err) {
 397                 struct a2mp_cmd_rej rej;
 398
 399                 rej.reason = __constant_cpu_to_le16(0);
 400                 hdr = (void *) skb->data;
 401
 402                 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
 403
 404                 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
 405                           &rej);
 406         }
 407
 408         /* Always free skb and return success error code to prevent
 409            from sending L2CAP Disconnect over A2MP channel */
 410         kfree_skb(skb);
 411
 412         amp_mgr_put(mgr);
 413
 414         return 0;
 415 }
 416
 417 static void a2mp_chan_close_cb(struct l2cap_chan *chan)
 418 {
 419         l2cap_chan_put(chan);
 420 }
 421
 422 static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
 423 {
 424         struct amp_mgr *mgr = chan->data;
 425
 426         if (!mgr)
 427                 return;
 428
 429         BT_DBG("chan %p state %s", chan, state_to_string(state));
 430
 431         chan->state = state;
 432
 433         switch (state) {
 434         case BT_CLOSED:
 435                 if (mgr)
 436                         amp_mgr_put(mgr);
 437                 break;
 438         }
 439 }
 440
 441 static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 442                                               unsigned long len, int nb)
 443 {
 444         return bt_skb_alloc(len, GFP_KERNEL);
 445 }
 446
 447 static struct l2cap_ops a2mp_chan_ops = {
 448         .name = "L2CAP A2MP channel",
 449         .recv = a2mp_chan_recv_cb,
 450         .close = a2mp_chan_close_cb,
 451         .state_change = a2mp_chan_state_change_cb,
 452         .alloc_skb = a2mp_chan_alloc_skb_cb,
 453
 454         /* Not implemented for A2MP */
 455         .new_connection = l2cap_chan_no_new_connection,
 456         .teardown = l2cap_chan_no_teardown,
 457         .ready = l2cap_chan_no_ready,
 458 };
 459
 460 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 461 {
 462         struct l2cap_chan *chan;
 463         int err;
 464
 465         chan = l2cap_chan_create();
 466         if (!chan)
 467                 return NULL;
 468
 469         BT_DBG("chan %p", chan);
 470
 471         chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
 472         chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 473
 474         chan->ops = &a2mp_chan_ops;
 475
 476         l2cap_chan_set_defaults(chan);
 477         chan->remote_max_tx = chan->max_tx;
 478         chan->remote_tx_win = chan->tx_win;
 479
 480         chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
 481         chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
 482
 483         skb_queue_head_init(&chan->tx_q);
 484
 485         chan->mode = L2CAP_MODE_ERTM;
 486
 487         err = l2cap_ertm_init(chan);
 488         if (err < 0) {
 489                 l2cap_chan_del(chan, 0);
 490                 return NULL;
 491         }
 492
 493         chan->conf_state = 0;
 494
 495         l2cap_chan_add(conn, chan);
 496
 497         chan->remote_mps = chan->omtu;
 498         chan->mps = chan->omtu;
 499
 500         chan->state = BT_CONNECTED;
 501
 502         return chan;
 503 }
 504
 505 /* AMP Manager functions */
 506 void amp_mgr_get(struct amp_mgr *mgr)
 507 {
 508         BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 509
 510         kref_get(&mgr->kref);
 511 }
 512
 513 static void amp_mgr_destroy(struct kref *kref)
 514 {
 515         struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
 516
 517         BT_DBG("mgr %p", mgr);
 518
 519         kfree(mgr);
 520 }
 521
 522 int amp_mgr_put(struct amp_mgr *mgr)
 523 {
 524         BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 525
 526         return kref_put(&mgr->kref, &amp_mgr_destroy);
 527 }
 528
 529 static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 530 {
 531         struct amp_mgr *mgr;
 532         struct l2cap_chan *chan;
 533
 534         mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
 535         if (!mgr)
 536                 return NULL;
 537
 538         BT_DBG("conn %p mgr %p", conn, mgr);
 539
 540         mgr->l2cap_conn = conn;
 541
 542         chan = a2mp_chan_open(conn);
 543         if (!chan) {
 544                 kfree(mgr);
 545                 return NULL;
 546         }
 547
 548         mgr->a2mp_chan = chan;
 549         chan->data = mgr;
 550
 551         conn->hcon->amp_mgr = mgr;
 552
 553         kref_init(&mgr->kref);
 554
 555         return mgr;
 556 }
 557
 558 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 559                                        struct sk_buff *skb)
 560 {
 561         struct amp_mgr *mgr;
 562
 563         mgr = amp_mgr_create(conn);
 564         if (!mgr) {
 565                 BT_ERR("Could not create AMP manager");
 566                 return NULL;
 567         }
 568
 569         BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
 570
 571         return mgr->a2mp_chan;
 572 }