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 /* Handle A2MP signalling */
 215 static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 216 {
 217         struct a2mp_cmd *hdr = (void *) skb->data;
 218         struct amp_mgr *mgr = chan->data;
 219         int err = 0;
 220
 221         amp_mgr_get(mgr);
 222
 223         while (skb->len >= sizeof(*hdr)) {
 224                 struct a2mp_cmd *hdr = (void *) skb->data;
 225                 u16 len = le16_to_cpu(hdr->len);
 226
 227                 BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);
 228
 229                 skb_pull(skb, sizeof(*hdr));
 230
 231                 if (len > skb->len || !hdr->ident) {
 232                         err = -EINVAL;
 233                         break;
 234                 }
 235
 236                 mgr->ident = hdr->ident;
 237
 238                 switch (hdr->code) {
 239                 case A2MP_COMMAND_REJ:
 240                         a2mp_command_rej(mgr, skb, hdr);
 241                         break;
 242
 243                 case A2MP_DISCOVER_REQ:
 244                         err = a2mp_discover_req(mgr, skb, hdr);
 245                         break;
 246
 247                 case A2MP_CHANGE_NOTIFY:
 248                         err = a2mp_change_notify(mgr, skb, hdr);
 249                         break;
 250
 251                 case A2MP_GETINFO_REQ:
 252                         err = a2mp_getinfo_req(mgr, skb, hdr);
 253                         break;
 254
 255                 case A2MP_GETAMPASSOC_REQ:
 256                 case A2MP_CREATEPHYSLINK_REQ:
 257                 case A2MP_DISCONNPHYSLINK_REQ:
 258                 case A2MP_CHANGE_RSP:
 259                 case A2MP_DISCOVER_RSP:
 260                 case A2MP_GETINFO_RSP:
 261                 case A2MP_GETAMPASSOC_RSP:
 262                 case A2MP_CREATEPHYSLINK_RSP:
 263                 case A2MP_DISCONNPHYSLINK_RSP:
 264                 default:
 265                         BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
 266                         err = -EINVAL;
 267                         break;
 268                 }
 269         }
 270
 271         if (err) {
 272                 struct a2mp_cmd_rej rej;
 273                 rej.reason = __constant_cpu_to_le16(0);
 274
 275                 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
 276
 277                 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
 278                           &rej);
 279         }
 280
 281         /* Always free skb and return success error code to prevent
 282            from sending L2CAP Disconnect over A2MP channel */
 283         kfree_skb(skb);
 284
 285         amp_mgr_put(mgr);
 286
 287         return 0;
 288 }
 289
 290 static void a2mp_chan_close_cb(struct l2cap_chan *chan)
 291 {
 292         l2cap_chan_destroy(chan);
 293 }
 294
 295 static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
 296 {
 297         struct amp_mgr *mgr = chan->data;
 298
 299         if (!mgr)
 300                 return;
 301
 302         BT_DBG("chan %p state %s", chan, state_to_string(state));
 303
 304         chan->state = state;
 305
 306         switch (state) {
 307         case BT_CLOSED:
 308                 if (mgr)
 309                         amp_mgr_put(mgr);
 310                 break;
 311         }
 312 }
 313
 314 static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 315                                               unsigned long len, int nb)
 316 {
 317         return bt_skb_alloc(len, GFP_KERNEL);
 318 }
 319
 320 static struct l2cap_chan *a2mp_chan_no_new_conn_cb(struct l2cap_chan *chan)
 321 {
 322         BT_ERR("new_connection for chan %p not implemented", chan);
 323
 324         return NULL;
 325 }
 326
 327 static void a2mp_chan_no_teardown_cb(struct l2cap_chan *chan, int err)
 328 {
 329         BT_ERR("teardown for chan %p not implemented", chan);
 330 }
 331
 332 static void a2mp_chan_no_ready(struct l2cap_chan *chan)
 333 {
 334         BT_ERR("ready for chan %p not implemented", chan);
 335 }
 336
 337 static struct l2cap_ops a2mp_chan_ops = {
 338         .name = "L2CAP A2MP channel",
 339         .recv = a2mp_chan_recv_cb,
 340         .close = a2mp_chan_close_cb,
 341         .state_change = a2mp_chan_state_change_cb,
 342         .alloc_skb = a2mp_chan_alloc_skb_cb,
 343
 344         /* Not implemented for A2MP */
 345         .new_connection = a2mp_chan_no_new_conn_cb,
 346         .teardown = a2mp_chan_no_teardown_cb,
 347         .ready = a2mp_chan_no_ready,
 348 };
 349
 350 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 351 {
 352         struct l2cap_chan *chan;
 353         int err;
 354
 355         chan = l2cap_chan_create();
 356         if (!chan)
 357                 return NULL;
 358
 359         BT_DBG("chan %p", chan);
 360
 361         hci_conn_hold(conn->hcon);
 362
 363         chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
 364         chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
 365         chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 366
 367         chan->ops = &a2mp_chan_ops;
 368
 369         l2cap_chan_set_defaults(chan);
 370         chan->remote_max_tx = chan->max_tx;
 371         chan->remote_tx_win = chan->tx_win;
 372
 373         chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
 374         chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
 375
 376         skb_queue_head_init(&chan->tx_q);
 377
 378         chan->mode = L2CAP_MODE_ERTM;
 379
 380         err = l2cap_ertm_init(chan);
 381         if (err < 0) {
 382                 l2cap_chan_del(chan, 0);
 383                 return NULL;
 384         }
 385
 386         chan->conf_state = 0;
 387
 388         l2cap_chan_add(conn, chan);
 389
 390         chan->remote_mps = chan->omtu;
 391         chan->mps = chan->omtu;
 392
 393         chan->state = BT_CONNECTED;
 394
 395         return chan;
 396 }
 397
 398 /* AMP Manager functions */
 399 void amp_mgr_get(struct amp_mgr *mgr)
 400 {
 401         BT_DBG("mgr %p", mgr);
 402
 403         kref_get(&mgr->kref);
 404 }
 405
 406 static void amp_mgr_destroy(struct kref *kref)
 407 {
 408         struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
 409
 410         BT_DBG("mgr %p", mgr);
 411
 412         kfree(mgr);
 413 }
 414
 415 int amp_mgr_put(struct amp_mgr *mgr)
 416 {
 417         BT_DBG("mgr %p", mgr);
 418
 419         return kref_put(&mgr->kref, &amp_mgr_destroy);
 420 }
 421
 422 static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 423 {
 424         struct amp_mgr *mgr;
 425         struct l2cap_chan *chan;
 426
 427         mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
 428         if (!mgr)
 429                 return NULL;
 430
 431         BT_DBG("conn %p mgr %p", conn, mgr);
 432
 433         mgr->l2cap_conn = conn;
 434
 435         chan = a2mp_chan_open(conn);
 436         if (!chan) {
 437                 kfree(mgr);
 438                 return NULL;
 439         }
 440
 441         mgr->a2mp_chan = chan;
 442         chan->data = mgr;
 443
 444         conn->hcon->amp_mgr = mgr;
 445
 446         kref_init(&mgr->kref);
 447
 448         return mgr;
 449 }