Linux Kernel  3.7.1
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
a2mp.c
Go to the documentation of this file.
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 
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 
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 
369  err = a2mp_getampassoc_req(mgr, skb, hdr);
370  break;
371 
373  err = a2mp_createphyslink_req(mgr, skb, hdr);
374  break;
375 
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:
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 
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 
473 
474  chan->ops = &a2mp_chan_ops;
475 
477  chan->remote_max_tx = chan->max_tx;
478  chan->remote_tx_win = chan->tx_win;
479 
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 
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 }