projects / deliverable / linux.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
Bluetooth: AMP: Process Chan Selected event
[deliverable/linux.git] / net / bluetooth / a2mp.c
CommitLineData
466f8004
AE		 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>
9740e49d 18#include <net/bluetooth/a2mp.h>
903e4541 19#include <net/bluetooth/amp.h>
466f8004 20
f97268fc
AE		 21/* Global AMP Manager list */
22LIST_HEAD(amp_mgr_list);
23DEFINE_MUTEX(amp_mgr_list_lock);
24
f6d3c6e7
AE		 25/* A2MP build & send command helper functions */
26static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
27{
28 struct a2mp_cmd *cmd;
29 int plen;
30
31 plen = sizeof(*cmd) + len;
32 cmd = kzalloc(plen, GFP_KERNEL);
33 if (!cmd)
34 return NULL;
35
36 cmd->code = code;
37 cmd->ident = ident;
38 cmd->len = cpu_to_le16(len);
39
40 memcpy(cmd->data, data, len);
41
42 return cmd;
43}
44
8e2a0d92 45void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
f6d3c6e7
AE		 46{
47 struct l2cap_chan *chan = mgr->a2mp_chan;
48 struct a2mp_cmd *cmd;
49 u16 total_len = len + sizeof(*cmd);
50 struct kvec iv;
51 struct msghdr msg;
52
53 cmd = __a2mp_build(code, ident, len, data);
54 if (!cmd)
55 return;
56
57 iv.iov_base = cmd;
58 iv.iov_len = total_len;
59
60 memset(&msg, 0, sizeof(msg));
61
62 msg.msg_iov = (struct iovec *) &iv;
63 msg.msg_iovlen = 1;
64
65 l2cap_chan_send(chan, &msg, total_len, 0);
66
67 kfree(cmd);
68}
69
9495b2ee 70u8 __next_ident(struct amp_mgr *mgr)
aa09537d
AE		 71{
72 if (++mgr->ident == 0)
73 mgr->ident = 1;
74
75 return mgr->ident;
76}
77
8598d064
AE		 78static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
79{
80 cl->id = 0;
81 cl->type = 0;
82 cl->status = 1;
83}
84
85/* hci_dev_list shall be locked */
86static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
87{
88 int i = 0;
89 struct hci_dev *hdev;
90
91 __a2mp_cl_bredr(cl);
92
93 list_for_each_entry(hdev, &hci_dev_list, list) {
94 /* Iterate through AMP controllers */
95 if (hdev->id == HCI_BREDR_ID)
96 continue;
97
98 /* Starting from second entry */
99 if (++i >= num_ctrl)
100 return;
101
102 cl[i].id = hdev->id;
103 cl[i].type = hdev->amp_type;
104 cl[i].status = hdev->amp_status;
105 }
106}
107
21dbd2ce
AE		 108/* Processing A2MP messages */
109static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
110 struct a2mp_cmd *hdr)
111{
112 struct a2mp_cmd_rej *rej = (void *) skb->data;
113
114 if (le16_to_cpu(hdr->len) < sizeof(*rej))
115 return -EINVAL;
116
117 BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
118
119 skb_pull(skb, sizeof(*rej));
120
121 return 0;
122}
123
8598d064
AE		 124static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
125 struct a2mp_cmd *hdr)
126{
127 struct a2mp_discov_req *req = (void *) skb->data;
128 u16 len = le16_to_cpu(hdr->len);
129 struct a2mp_discov_rsp *rsp;
130 u16 ext_feat;
131 u8 num_ctrl;
132
133 if (len < sizeof(*req))
134 return -EINVAL;
135
136 skb_pull(skb, sizeof(*req));
137
138 ext_feat = le16_to_cpu(req->ext_feat);
139
140 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
141
142 /* check that packet is not broken for now */
143 while (ext_feat & A2MP_FEAT_EXT) {
144 if (len < sizeof(ext_feat))
145 return -EINVAL;
146
147 ext_feat = get_unaligned_le16(skb->data);
148 BT_DBG("efm 0x%4.4x", ext_feat);
149 len -= sizeof(ext_feat);
150 skb_pull(skb, sizeof(ext_feat));
151 }
152
153 read_lock(&hci_dev_list_lock);
154
155 num_ctrl = __hci_num_ctrl();
156 len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
157 rsp = kmalloc(len, GFP_ATOMIC);
158 if (!rsp) {
159 read_unlock(&hci_dev_list_lock);
160 return -ENOMEM;
161 }
162
163 rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
164 rsp->ext_feat = 0;
165
166 __a2mp_add_cl(mgr, rsp->cl, num_ctrl);
167
168 read_unlock(&hci_dev_list_lock);
169
170 a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
171
172 kfree(rsp);
173 return 0;
174}
175
aa09537d
AE		 176static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
177 struct a2mp_cmd *hdr)
178{
179 struct a2mp_discov_rsp *rsp = (void *) skb->data;
180 u16 len = le16_to_cpu(hdr->len);
181 struct a2mp_cl *cl;
182 u16 ext_feat;
2766be48 183 bool found = false;
aa09537d
AE		 184
185 if (len < sizeof(*rsp))
186 return -EINVAL;
187
188 len -= sizeof(*rsp);
189 skb_pull(skb, sizeof(*rsp));
190
191 ext_feat = le16_to_cpu(rsp->ext_feat);
192
193 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
194
195 /* check that packet is not broken for now */
196 while (ext_feat & A2MP_FEAT_EXT) {
197 if (len < sizeof(ext_feat))
198 return -EINVAL;
199
200 ext_feat = get_unaligned_le16(skb->data);
201 BT_DBG("efm 0x%4.4x", ext_feat);
202 len -= sizeof(ext_feat);
203 skb_pull(skb, sizeof(ext_feat));
204 }
205
206 cl = (void *) skb->data;
207 while (len >= sizeof(*cl)) {
208 BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
209 cl->status);
210
211 if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
212 struct a2mp_info_req req;
213
2766be48 214 found = true;
aa09537d
AE		 215 req.id = cl->id;
216 a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
217 sizeof(req), &req);
218 }
219
220 len -= sizeof(*cl);
221 cl = (void *) skb_pull(skb, sizeof(*cl));
222 }
223
2766be48
AE		 224 /* Fall back to L2CAP init sequence */
225 if (!found) {
226 struct l2cap_conn *conn = mgr->l2cap_conn;
227 struct l2cap_chan *chan;
228
229 mutex_lock(&conn->chan_lock);
230
231 list_for_each_entry(chan, &conn->chan_l, list) {
232
233 BT_DBG("chan %p state %s", chan,
234 state_to_string(chan->state));
235
236 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
237 continue;
238
239 l2cap_chan_lock(chan);
240
241 if (chan->state == BT_CONNECT)
242 l2cap_send_conn_req(chan);
243
244 l2cap_chan_unlock(chan);
245 }
246
247 mutex_unlock(&conn->chan_lock);
248 }
249
aa09537d
AE		 250 return 0;
251}
252
329d81af
AE		 253static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
254 struct a2mp_cmd *hdr)
255{
256 struct a2mp_cl *cl = (void *) skb->data;
257
258 while (skb->len >= sizeof(*cl)) {
259 BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
260 cl->status);
261 cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
262 }
263
264 /* TODO send A2MP_CHANGE_RSP */
265
266 return 0;
267}
268
47f2d97d
AE		 269static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
270 struct a2mp_cmd *hdr)
271{
272 struct a2mp_info_req *req = (void *) skb->data;
47f2d97d
AE		 273 struct hci_dev *hdev;
274
275 if (le16_to_cpu(hdr->len) < sizeof(*req))
276 return -EINVAL;
277
278 BT_DBG("id %d", req->id);
279
47f2d97d 280 hdev = hci_dev_get(req->id);
8e2a0d92
AE		 281 if (!hdev) {
282 struct a2mp_info_rsp rsp;
283
284 rsp.id = req->id;
285 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
286
287 a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
288 &rsp);
47f2d97d
AE		 289 }
290
8e2a0d92
AE		 291 if (hdev->dev_type != HCI_BREDR) {
292 mgr->state = READ_LOC_AMP_INFO;
293 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
294 }
47f2d97d 295
8e2a0d92 296 hci_dev_put(hdev);
47f2d97d
AE		 297
298 skb_pull(skb, sizeof(*req));
299 return 0;
300}
301
0d868de9
AE		 302static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
303 struct a2mp_cmd *hdr)
304{
305 struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
306 struct a2mp_amp_assoc_req req;
307 struct amp_ctrl *ctrl;
308
309 if (le16_to_cpu(hdr->len) < sizeof(*rsp))
310 return -EINVAL;
311
312 BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
313
314 if (rsp->status)
315 return -EINVAL;
316
317 ctrl = amp_ctrl_add(mgr);
318 if (!ctrl)
319 return -ENOMEM;
320
321 ctrl->id = rsp->id;
322
323 req.id = rsp->id;
324 a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
325 &req);
326
327 skb_pull(skb, sizeof(*rsp));
328 return 0;
329}
330
a28381dc
AE		 331static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
332 struct a2mp_cmd *hdr)
333{
334 struct a2mp_amp_assoc_req *req = (void *) skb->data;
335 struct hci_dev *hdev;
903e4541 336 struct amp_mgr *tmp;
a28381dc
AE		 337
338 if (le16_to_cpu(hdr->len) < sizeof(*req))
339 return -EINVAL;
340
341 BT_DBG("id %d", req->id);
342
903e4541
AE		 343 /* Make sure that other request is not processed */
344 tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
345
a28381dc 346 hdev = hci_dev_get(req->id);
903e4541 347 if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
a28381dc
AE		 348 struct a2mp_amp_assoc_rsp rsp;
349 rsp.id = req->id;
903e4541
AE		 350
351 if (tmp) {
352 rsp.status = A2MP_STATUS_COLLISION_OCCURED;
353 amp_mgr_put(tmp);
354 } else {
355 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
356 }
a28381dc
AE		 357
358 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
359 &rsp);
903e4541
AE		 360
361 goto done;
a28381dc
AE		 362 }
363
903e4541 364 amp_read_loc_assoc(hdev, mgr);
a28381dc 365
903e4541 366done:
a28381dc
AE		 367 if (hdev)
368 hci_dev_put(hdev);
369
370 skb_pull(skb, sizeof(*req));
371 return 0;
372}
373
9a5e94db
AE		 374static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
375 struct a2mp_cmd *hdr)
376{
377 struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
378 u16 len = le16_to_cpu(hdr->len);
379 struct hci_dev *hdev;
380 struct amp_ctrl *ctrl;
381 struct hci_conn *hcon;
382
383 if (len < sizeof(*rsp))
384 return -EINVAL;
385
386 BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
387 len - sizeof(*rsp));
388
389 if (rsp->status)
390 return -EINVAL;
391
392 /* Save remote ASSOC data */
393 ctrl = amp_ctrl_lookup(mgr, rsp->id);
394 if (ctrl) {
395 u8 *assoc, assoc_len = len - sizeof(*rsp);
396
397 assoc = kzalloc(assoc_len, GFP_KERNEL);
398 if (!assoc) {
399 amp_ctrl_put(ctrl);
400 return -ENOMEM;
401 }
402
403 memcpy(assoc, rsp->amp_assoc, assoc_len);
404 ctrl->assoc = assoc;
405 ctrl->assoc_len = assoc_len;
406 ctrl->assoc_rem_len = assoc_len;
407 ctrl->assoc_len_so_far = 0;
408
409 amp_ctrl_put(ctrl);
410 }
411
412 /* Create Phys Link */
413 hdev = hci_dev_get(rsp->id);
414 if (!hdev)
415 return -EINVAL;
416
417 hcon = phylink_add(hdev, mgr, rsp->id);
418 if (!hcon)
419 goto done;
420
421 BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
422
9495b2ee
AE		 423 mgr->bredr_chan->ctrl_id = rsp->id;
424
a02226d6
AE		 425 amp_create_phylink(hdev, mgr, hcon);
426
9a5e94db
AE		 427done:
428 hci_dev_put(hdev);
429 skb_pull(skb, len);
430 return 0;
431}
432
e072f5da
AE		 433static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
434 struct a2mp_cmd *hdr)
435{
436 struct a2mp_physlink_req *req = (void *) skb->data;
437
438 struct a2mp_physlink_rsp rsp;
439 struct hci_dev *hdev;
cb8488c0 440 struct hci_conn *hcon;
e072f5da
AE		 441
442 if (le16_to_cpu(hdr->len) < sizeof(*req))
443 return -EINVAL;
444
445 BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
446
447 rsp.local_id = req->remote_id;
448 rsp.remote_id = req->local_id;
449
450 hdev = hci_dev_get(req->remote_id);
451 if (!hdev || hdev->amp_type != HCI_AMP) {
452 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
453 goto send_rsp;
454 }
455
456 /* TODO process physlink create */
457
cb8488c0
AE		 458 hcon = phylink_add(hdev, mgr, req->local_id);
459 if (hcon) {
460 BT_DBG("hcon %p", hcon);
461
462 rsp.status = A2MP_STATUS_SUCCESS;
463 } else {
464 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
465 }
e072f5da
AE		 466
467send_rsp:
468 if (hdev)
469 hci_dev_put(hdev);
470
471 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
472 &rsp);
473
474 skb_pull(skb, le16_to_cpu(hdr->len));
475 return 0;
476}
477
6113f84f
AE		 478static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
479 struct a2mp_cmd *hdr)
480{
481 struct a2mp_physlink_req *req = (void *) skb->data;
482 struct a2mp_physlink_rsp rsp;
483 struct hci_dev *hdev;
cb8488c0 484 struct hci_conn *hcon;
6113f84f
AE		 485
486 if (le16_to_cpu(hdr->len) < sizeof(*req))
487 return -EINVAL;
488
489 BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
490
491 rsp.local_id = req->remote_id;
492 rsp.remote_id = req->local_id;
493 rsp.status = A2MP_STATUS_SUCCESS;
494
cb8488c0 495 hdev = hci_dev_get(req->remote_id);
6113f84f
AE		 496 if (!hdev) {
497 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
498 goto send_rsp;
499 }
500
cb8488c0
AE		 501 hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
502 if (!hcon) {
503 BT_ERR("No phys link exist");
504 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
505 goto clean;
506 }
507
6113f84f
AE		 508 /* TODO Disconnect Phys Link here */
509
cb8488c0 510clean:
6113f84f
AE		 511 hci_dev_put(hdev);
512
513send_rsp:
514 a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
515
516 skb_pull(skb, sizeof(*req));
517 return 0;
518}
519
f6410a84
AE		 520static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
521 struct a2mp_cmd *hdr)
522{
8e8c7e36 523 BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
f6410a84
AE		 524
525 skb_pull(skb, le16_to_cpu(hdr->len));
526 return 0;
527}
528
6b44d9b8
AE		 529/* Handle A2MP signalling */
530static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
531{
d9fc1d54 532 struct a2mp_cmd *hdr;
6b44d9b8
AE		 533 struct amp_mgr *mgr = chan->data;
534 int err = 0;
535
536 amp_mgr_get(mgr);
537
538 while (skb->len >= sizeof(*hdr)) {
d9fc1d54
AE		 539 u16 len;
540
541 hdr = (void *) skb->data;
542 len = le16_to_cpu(hdr->len);
6b44d9b8 543
8e8c7e36 544 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
6b44d9b8
AE		 545
546 skb_pull(skb, sizeof(*hdr));
547
548 if (len > skb->len || !hdr->ident) {
549 err = -EINVAL;
550 break;
551 }
552
553 mgr->ident = hdr->ident;
554
555 switch (hdr->code) {
556 case A2MP_COMMAND_REJ:
21dbd2ce
AE		 557 a2mp_command_rej(mgr, skb, hdr);
558 break;
559
6b44d9b8 560 case A2MP_DISCOVER_REQ:
8598d064
AE		 561 err = a2mp_discover_req(mgr, skb, hdr);
562 break;
563
6b44d9b8 564 case A2MP_CHANGE_NOTIFY:
329d81af
AE		 565 err = a2mp_change_notify(mgr, skb, hdr);
566 break;
567
6b44d9b8 568 case A2MP_GETINFO_REQ:
47f2d97d
AE		 569 err = a2mp_getinfo_req(mgr, skb, hdr);
570 break;
571
6b44d9b8 572 case A2MP_GETAMPASSOC_REQ:
a28381dc
AE		 573 err = a2mp_getampassoc_req(mgr, skb, hdr);
574 break;
575
6b44d9b8 576 case A2MP_CREATEPHYSLINK_REQ:
e072f5da
AE		 577 err = a2mp_createphyslink_req(mgr, skb, hdr);
578 break;
579
6b44d9b8 580 case A2MP_DISCONNPHYSLINK_REQ:
6113f84f
AE		 581 err = a2mp_discphyslink_req(mgr, skb, hdr);
582 break;
583
6b44d9b8 584 case A2MP_DISCOVER_RSP:
aa09537d
AE		 585 err = a2mp_discover_rsp(mgr, skb, hdr);
586 break;
587
6b44d9b8 588 case A2MP_GETINFO_RSP:
0d868de9
AE		 589 err = a2mp_getinfo_rsp(mgr, skb, hdr);
590 break;
591
6b44d9b8 592 case A2MP_GETAMPASSOC_RSP:
9a5e94db
AE		 593 err = a2mp_getampassoc_rsp(mgr, skb, hdr);
594 break;
595
596 case A2MP_CHANGE_RSP:
6b44d9b8
AE		 597 case A2MP_CREATEPHYSLINK_RSP:
598 case A2MP_DISCONNPHYSLINK_RSP:
f6410a84
AE		 599 err = a2mp_cmd_rsp(mgr, skb, hdr);
600 break;
601
6b44d9b8
AE		 602 default:
603 BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
604 err = -EINVAL;
605 break;
606 }
607 }
608
609 if (err) {
610 struct a2mp_cmd_rej rej;
d9fc1d54 611
6b44d9b8 612 rej.reason = __constant_cpu_to_le16(0);
d9fc1d54 613 hdr = (void *) skb->data;
6b44d9b8
AE		 614
615 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
616
617 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
618 &rej);
619 }
620
621 /* Always free skb and return success error code to prevent
622 from sending L2CAP Disconnect over A2MP channel */
623 kfree_skb(skb);
624
625 amp_mgr_put(mgr);
626
627 return 0;
628}
629
46d5c908
AE		 630static void a2mp_chan_close_cb(struct l2cap_chan *chan)
631{
4af66c69 632 l2cap_chan_put(chan);
46d5c908
AE		 633}
634
635static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
636{
637 struct amp_mgr *mgr = chan->data;
638
639 if (!mgr)
640 return;
641
642 BT_DBG("chan %p state %s", chan, state_to_string(state));
643
644 chan->state = state;
645
646 switch (state) {
647 case BT_CLOSED:
648 if (mgr)
649 amp_mgr_put(mgr);
650 break;
651 }
652}
653
654static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
655 unsigned long len, int nb)
656{
657 return bt_skb_alloc(len, GFP_KERNEL);
658}
659
466f8004
AE		 660static struct l2cap_ops a2mp_chan_ops = {
661 .name = "L2CAP A2MP channel",
6b44d9b8 662 .recv = a2mp_chan_recv_cb,
46d5c908
AE		 663 .close = a2mp_chan_close_cb,
664 .state_change = a2mp_chan_state_change_cb,
665 .alloc_skb = a2mp_chan_alloc_skb_cb,
666
667 /* Not implemented for A2MP */
7e1af8a3
GP		 668 .new_connection = l2cap_chan_no_new_connection,
669 .teardown = l2cap_chan_no_teardown,
670 .ready = l2cap_chan_no_ready,
466f8004
AE		 671};
672
93c3e8f5 673static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
466f8004
AE		 674{
675 struct l2cap_chan *chan;
676 int err;
677
678 chan = l2cap_chan_create();
679 if (!chan)
680 return NULL;
681
682 BT_DBG("chan %p", chan);
683
416fa752 684 chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
466f8004
AE		 685 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
686
687 chan->ops = &a2mp_chan_ops;
688
689 l2cap_chan_set_defaults(chan);
690 chan->remote_max_tx = chan->max_tx;
691 chan->remote_tx_win = chan->tx_win;
692
693 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
694 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
695
696 skb_queue_head_init(&chan->tx_q);
697
698 chan->mode = L2CAP_MODE_ERTM;
699
700 err = l2cap_ertm_init(chan);
701 if (err < 0) {
702 l2cap_chan_del(chan, 0);
703 return NULL;
704 }
705
706 chan->conf_state = 0;
707
93c3e8f5
AE		 708 if (locked)
709 __l2cap_chan_add(conn, chan);
710 else
711 l2cap_chan_add(conn, chan);
466f8004
AE		 712
713 chan->remote_mps = chan->omtu;
714 chan->mps = chan->omtu;
715
716 chan->state = BT_CONNECTED;
717
718 return chan;
719}
9740e49d
AE		 720
721/* AMP Manager functions */
722void amp_mgr_get(struct amp_mgr *mgr)
723{
a0dfe0ab 724 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d
AE		 725
726 kref_get(&mgr->kref);
727}
728
729static void amp_mgr_destroy(struct kref *kref)
730{
731 struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
732
733 BT_DBG("mgr %p", mgr);
734
f97268fc
AE		 735 mutex_lock(&amp_mgr_list_lock);
736 list_del(&mgr->list);
737 mutex_unlock(&amp_mgr_list_lock);
738
52c0d6e5 739 amp_ctrl_list_flush(mgr);
9740e49d
AE		 740 kfree(mgr);
741}
742
743int amp_mgr_put(struct amp_mgr *mgr)
744{
a0dfe0ab 745 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d
AE		 746
747 return kref_put(&mgr->kref, &amp_mgr_destroy);
748}
749
93c3e8f5 750static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
9740e49d
AE		 751{
752 struct amp_mgr *mgr;
753 struct l2cap_chan *chan;
754
755 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
756 if (!mgr)
757 return NULL;
758
759 BT_DBG("conn %p mgr %p", conn, mgr);
760
761 mgr->l2cap_conn = conn;
762
93c3e8f5 763 chan = a2mp_chan_open(conn, locked);
9740e49d
AE		 764 if (!chan) {
765 kfree(mgr);
766 return NULL;
767 }
768
769 mgr->a2mp_chan = chan;
770 chan->data = mgr;
771
772 conn->hcon->amp_mgr = mgr;
773
774 kref_init(&mgr->kref);
775
52c0d6e5
AE		 776 /* Remote AMP ctrl list initialization */
777 INIT_LIST_HEAD(&mgr->amp_ctrls);
778 mutex_init(&mgr->amp_ctrls_lock);
779
f97268fc
AE		 780 mutex_lock(&amp_mgr_list_lock);
781 list_add(&mgr->list, &amp_mgr_list);
782 mutex_unlock(&amp_mgr_list_lock);
783
9740e49d
AE		 784 return mgr;
785}
97e8e89d
AE		 786
787struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
788 struct sk_buff *skb)
789{
790 struct amp_mgr *mgr;
791
93c3e8f5 792 mgr = amp_mgr_create(conn, false);
97e8e89d
AE		 793 if (!mgr) {
794 BT_ERR("Could not create AMP manager");
795 return NULL;
796 }
797
798 BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
799
800 return mgr->a2mp_chan;
801}
f97268fc
AE		 802
803struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
804{
805 struct amp_mgr *mgr;
806
807 mutex_lock(&amp_mgr_list_lock);
808 list_for_each_entry(mgr, &amp_mgr_list, list) {
809 if (mgr->state == state) {
810 amp_mgr_get(mgr);
811 mutex_unlock(&amp_mgr_list_lock);
812 return mgr;
813 }
814 }
815 mutex_unlock(&amp_mgr_list_lock);
816
817 return NULL;
818}
8e2a0d92
AE		 819
820void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
821{
822 struct amp_mgr *mgr;
823 struct a2mp_info_rsp rsp;
824
825 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
826 if (!mgr)
827 return;
828
829 BT_DBG("%s mgr %p", hdev->name, mgr);
830
831 rsp.id = hdev->id;
832 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
833
834 if (hdev->amp_type != HCI_BREDR) {
835 rsp.status = 0;
836 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
837 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
838 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
839 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
840 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
841 }
842
843 a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
844 amp_mgr_put(mgr);
845}
903e4541
AE		 846
847void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
848{
849 struct amp_mgr *mgr;
850 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
851 struct a2mp_amp_assoc_rsp *rsp;
852 size_t len;
853
854 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
855 if (!mgr)
856 return;
857
858 BT_DBG("%s mgr %p", hdev->name, mgr);
859
860 len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
861 rsp = kzalloc(len, GFP_KERNEL);
862 if (!rsp) {
863 amp_mgr_put(mgr);
864 return;
865 }
866
867 rsp->id = hdev->id;
868
869 if (status) {
870 rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
871 } else {
872 rsp->status = A2MP_STATUS_SUCCESS;
873 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
874 }
875
876 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
877 amp_mgr_put(mgr);
878 kfree(rsp);
879}
93c3e8f5 880
9495b2ee
AE		 881void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
882{
883 struct amp_mgr *mgr;
884 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
885 struct a2mp_physlink_req *req;
886 struct l2cap_chan *bredr_chan;
887 size_t len;
888
889 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
890 if (!mgr)
891 return;
892
893 len = sizeof(*req) + loc_assoc->len;
894
895 BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
896
897 req = kzalloc(len, GFP_KERNEL);
898 if (!req) {
899 amp_mgr_put(mgr);
900 return;
901 }
902
903 bredr_chan = mgr->bredr_chan;
904 if (!bredr_chan)
905 goto clean;
906
907 req->local_id = hdev->id;
908 req->remote_id = bredr_chan->ctrl_id;
909 memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
910
911 a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
912
913clean:
914 amp_mgr_put(mgr);
915 kfree(req);
916}
917
93c3e8f5
AE		 918void a2mp_discover_amp(struct l2cap_chan *chan)
919{
920 struct l2cap_conn *conn = chan->conn;
921 struct amp_mgr *mgr = conn->hcon->amp_mgr;
922 struct a2mp_discov_req req;
923
924 BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
925
926 if (!mgr) {
927 mgr = amp_mgr_create(conn, true);
928 if (!mgr)
929 return;
930 }
931
932 mgr->bredr_chan = chan;
933
934 req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
935 req.ext_feat = 0;
936 a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
937}
Linux kernel - Deliverables
This page took 0.076273 seconds and 5 git commands to generate.