1
2
3
4
5
6
7
8
9
10
11
12
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#include <net/bluetooth/amp.h>
20
21
22LIST_HEAD(amp_mgr_list);
23DEFINE_MUTEX(amp_mgr_list_lock);
24
25
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
45void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
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
70u8 __next_ident(struct amp_mgr *mgr)
71{
72 if (++mgr->ident == 0)
73 mgr->ident = 1;
74
75 return mgr->ident;
76}
77
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
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
95 if (hdev->id == HCI_BREDR_ID)
96 continue;
97
98
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
108
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
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
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
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;
183 bool found = false;
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
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
214 found = true;
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
224
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
250 return 0;
251}
252
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
265
266 return 0;
267}
268
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;
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
280 hdev = hci_dev_get(req->id);
281 if (!hdev || hdev->dev_type != HCI_AMP) {
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);
289
290 goto done;
291 }
292
293 set_bit(READ_LOC_AMP_INFO, &mgr->state);
294 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
295
296done:
297 if (hdev)
298 hci_dev_put(hdev);
299
300 skb_pull(skb, sizeof(*req));
301 return 0;
302}
303
304static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
305 struct a2mp_cmd *hdr)
306{
307 struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
308 struct a2mp_amp_assoc_req req;
309 struct amp_ctrl *ctrl;
310
311 if (le16_to_cpu(hdr->len) < sizeof(*rsp))
312 return -EINVAL;
313
314 BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
315
316 if (rsp->status)
317 return -EINVAL;
318
319 ctrl = amp_ctrl_add(mgr, rsp->id);
320 if (!ctrl)
321 return -ENOMEM;
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
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;
336 struct amp_mgr *tmp;
337
338 if (le16_to_cpu(hdr->len) < sizeof(*req))
339 return -EINVAL;
340
341 BT_DBG("id %d", req->id);
342
343
344 tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
345
346 hdev = hci_dev_get(req->id);
347 if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
348 struct a2mp_amp_assoc_rsp rsp;
349 rsp.id = req->id;
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 }
357
358 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
359 &rsp);
360
361 goto done;
362 }
363
364 amp_read_loc_assoc(hdev, mgr);
365
366done:
367 if (hdev)
368 hci_dev_put(hdev);
369
370 skb_pull(skb, sizeof(*req));
371 return 0;
372}
373
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 size_t assoc_len;
383
384 if (len < sizeof(*rsp))
385 return -EINVAL;
386
387 assoc_len = len - sizeof(*rsp);
388
389 BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
390 assoc_len);
391
392 if (rsp->status)
393 return -EINVAL;
394
395
396 ctrl = amp_ctrl_lookup(mgr, rsp->id);
397 if (ctrl) {
398 u8 *assoc;
399
400 assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
401 if (!assoc) {
402 amp_ctrl_put(ctrl);
403 return -ENOMEM;
404 }
405
406 ctrl->assoc = assoc;
407 ctrl->assoc_len = assoc_len;
408 ctrl->assoc_rem_len = assoc_len;
409 ctrl->assoc_len_so_far = 0;
410
411 amp_ctrl_put(ctrl);
412 }
413
414
415 hdev = hci_dev_get(rsp->id);
416 if (!hdev)
417 return -EINVAL;
418
419 hcon = phylink_add(hdev, mgr, rsp->id, true);
420 if (!hcon)
421 goto done;
422
423 BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
424
425 mgr->bredr_chan->remote_amp_id = rsp->id;
426
427 amp_create_phylink(hdev, mgr, hcon);
428
429done:
430 hci_dev_put(hdev);
431 skb_pull(skb, len);
432 return 0;
433}
434
435static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
436 struct a2mp_cmd *hdr)
437{
438 struct a2mp_physlink_req *req = (void *) skb->data;
439
440 struct a2mp_physlink_rsp rsp;
441 struct hci_dev *hdev;
442 struct hci_conn *hcon;
443 struct amp_ctrl *ctrl;
444
445 if (le16_to_cpu(hdr->len) < sizeof(*req))
446 return -EINVAL;
447
448 BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
449
450 rsp.local_id = req->remote_id;
451 rsp.remote_id = req->local_id;
452
453 hdev = hci_dev_get(req->remote_id);
454 if (!hdev || hdev->amp_type != HCI_AMP) {
455 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
456 goto send_rsp;
457 }
458
459 ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
460 if (!ctrl) {
461 ctrl = amp_ctrl_add(mgr, rsp.remote_id);
462 if (ctrl) {
463 amp_ctrl_get(ctrl);
464 } else {
465 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
466 goto send_rsp;
467 }
468 }
469
470 if (ctrl) {
471 size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
472 u8 *assoc;
473
474 assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
475 if (!assoc) {
476 amp_ctrl_put(ctrl);
477 return -ENOMEM;
478 }
479
480 ctrl->assoc = assoc;
481 ctrl->assoc_len = assoc_len;
482 ctrl->assoc_rem_len = assoc_len;
483 ctrl->assoc_len_so_far = 0;
484
485 amp_ctrl_put(ctrl);
486 }
487
488 hcon = phylink_add(hdev, mgr, req->local_id, false);
489 if (hcon) {
490 amp_accept_phylink(hdev, mgr, hcon);
491 rsp.status = A2MP_STATUS_SUCCESS;
492 } else {
493 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
494 }
495
496send_rsp:
497 if (hdev)
498 hci_dev_put(hdev);
499
500
501
502
503 if (rsp.status != A2MP_STATUS_SUCCESS) {
504 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
505 sizeof(rsp), &rsp);
506 } else {
507 set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
508 mgr->ident = hdr->ident;
509 }
510
511 skb_pull(skb, le16_to_cpu(hdr->len));
512 return 0;
513}
514
515static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
516 struct a2mp_cmd *hdr)
517{
518 struct a2mp_physlink_req *req = (void *) skb->data;
519 struct a2mp_physlink_rsp rsp;
520 struct hci_dev *hdev;
521 struct hci_conn *hcon;
522
523 if (le16_to_cpu(hdr->len) < sizeof(*req))
524 return -EINVAL;
525
526 BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
527
528 rsp.local_id = req->remote_id;
529 rsp.remote_id = req->local_id;
530 rsp.status = A2MP_STATUS_SUCCESS;
531
532 hdev = hci_dev_get(req->remote_id);
533 if (!hdev) {
534 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
535 goto send_rsp;
536 }
537
538 hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
539 if (!hcon) {
540 BT_ERR("No phys link exist");
541 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
542 goto clean;
543 }
544
545
546
547clean:
548 hci_dev_put(hdev);
549
550send_rsp:
551 a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
552
553 skb_pull(skb, sizeof(*req));
554 return 0;
555}
556
557static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
558 struct a2mp_cmd *hdr)
559{
560 BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
561
562 skb_pull(skb, le16_to_cpu(hdr->len));
563 return 0;
564}
565
566
567static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
568{
569 struct a2mp_cmd *hdr;
570 struct amp_mgr *mgr = chan->data;
571 int err = 0;
572
573 amp_mgr_get(mgr);
574
575 while (skb->len >= sizeof(*hdr)) {
576 u16 len;
577
578 hdr = (void *) skb->data;
579 len = le16_to_cpu(hdr->len);
580
581 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
582
583 skb_pull(skb, sizeof(*hdr));
584
585 if (len > skb->len || !hdr->ident) {
586 err = -EINVAL;
587 break;
588 }
589
590 mgr->ident = hdr->ident;
591
592 switch (hdr->code) {
593 case A2MP_COMMAND_REJ:
594 a2mp_command_rej(mgr, skb, hdr);
595 break;
596
597 case A2MP_DISCOVER_REQ:
598 err = a2mp_discover_req(mgr, skb, hdr);
599 break;
600
601 case A2MP_CHANGE_NOTIFY:
602 err = a2mp_change_notify(mgr, skb, hdr);
603 break;
604
605 case A2MP_GETINFO_REQ:
606 err = a2mp_getinfo_req(mgr, skb, hdr);
607 break;
608
609 case A2MP_GETAMPASSOC_REQ:
610 err = a2mp_getampassoc_req(mgr, skb, hdr);
611 break;
612
613 case A2MP_CREATEPHYSLINK_REQ:
614 err = a2mp_createphyslink_req(mgr, skb, hdr);
615 break;
616
617 case A2MP_DISCONNPHYSLINK_REQ:
618 err = a2mp_discphyslink_req(mgr, skb, hdr);
619 break;
620
621 case A2MP_DISCOVER_RSP:
622 err = a2mp_discover_rsp(mgr, skb, hdr);
623 break;
624
625 case A2MP_GETINFO_RSP:
626 err = a2mp_getinfo_rsp(mgr, skb, hdr);
627 break;
628
629 case A2MP_GETAMPASSOC_RSP:
630 err = a2mp_getampassoc_rsp(mgr, skb, hdr);
631 break;
632
633 case A2MP_CHANGE_RSP:
634 case A2MP_CREATEPHYSLINK_RSP:
635 case A2MP_DISCONNPHYSLINK_RSP:
636 err = a2mp_cmd_rsp(mgr, skb, hdr);
637 break;
638
639 default:
640 BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
641 err = -EINVAL;
642 break;
643 }
644 }
645
646 if (err) {
647 struct a2mp_cmd_rej rej;
648
649 rej.reason = __constant_cpu_to_le16(0);
650 hdr = (void *) skb->data;
651
652 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
653
654 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
655 &rej);
656 }
657
658
659
660 kfree_skb(skb);
661
662 amp_mgr_put(mgr);
663
664 return 0;
665}
666
667static void a2mp_chan_close_cb(struct l2cap_chan *chan)
668{
669 l2cap_chan_put(chan);
670}
671
672static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
673{
674 struct amp_mgr *mgr = chan->data;
675
676 if (!mgr)
677 return;
678
679 BT_DBG("chan %p state %s", chan, state_to_string(state));
680
681 chan->state = state;
682
683 switch (state) {
684 case BT_CLOSED:
685 if (mgr)
686 amp_mgr_put(mgr);
687 break;
688 }
689}
690
691static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
692 unsigned long len, int nb)
693{
694 return bt_skb_alloc(len, GFP_KERNEL);
695}
696
697static struct l2cap_ops a2mp_chan_ops = {
698 .name = "L2CAP A2MP channel",
699 .recv = a2mp_chan_recv_cb,
700 .close = a2mp_chan_close_cb,
701 .state_change = a2mp_chan_state_change_cb,
702 .alloc_skb = a2mp_chan_alloc_skb_cb,
703
704
705 .new_connection = l2cap_chan_no_new_connection,
706 .teardown = l2cap_chan_no_teardown,
707 .ready = l2cap_chan_no_ready,
708 .defer = l2cap_chan_no_defer,
709};
710
711static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
712{
713 struct l2cap_chan *chan;
714 int err;
715
716 chan = l2cap_chan_create();
717 if (!chan)
718 return NULL;
719
720 BT_DBG("chan %p", chan);
721
722 chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
723 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
724
725 chan->ops = &a2mp_chan_ops;
726
727 l2cap_chan_set_defaults(chan);
728 chan->remote_max_tx = chan->max_tx;
729 chan->remote_tx_win = chan->tx_win;
730
731 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
732 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
733
734 skb_queue_head_init(&chan->tx_q);
735
736 chan->mode = L2CAP_MODE_ERTM;
737
738 err = l2cap_ertm_init(chan);
739 if (err < 0) {
740 l2cap_chan_del(chan, 0);
741 return NULL;
742 }
743
744 chan->conf_state = 0;
745
746 if (locked)
747 __l2cap_chan_add(conn, chan);
748 else
749 l2cap_chan_add(conn, chan);
750
751 chan->remote_mps = chan->omtu;
752 chan->mps = chan->omtu;
753
754 chan->state = BT_CONNECTED;
755
756 return chan;
757}
758
759
760struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
761{
762 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
763
764 kref_get(&mgr->kref);
765
766 return mgr;
767}
768
769static void amp_mgr_destroy(struct kref *kref)
770{
771 struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
772
773 BT_DBG("mgr %p", mgr);
774
775 mutex_lock(&_mgr_list_lock);
776 list_del(&mgr->list);
777 mutex_unlock(&_mgr_list_lock);
778
779 amp_ctrl_list_flush(mgr);
780 kfree(mgr);
781}
782
783int amp_mgr_put(struct amp_mgr *mgr)
784{
785 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
786
787 return kref_put(&mgr->kref, &_mgr_destroy);
788}
789
790static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
791{
792 struct amp_mgr *mgr;
793 struct l2cap_chan *chan;
794
795 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
796 if (!mgr)
797 return NULL;
798
799 BT_DBG("conn %p mgr %p", conn, mgr);
800
801 mgr->l2cap_conn = conn;
802
803 chan = a2mp_chan_open(conn, locked);
804 if (!chan) {
805 kfree(mgr);
806 return NULL;
807 }
808
809 mgr->a2mp_chan = chan;
810 chan->data = mgr;
811
812 conn->hcon->amp_mgr = mgr;
813
814 kref_init(&mgr->kref);
815
816
817 INIT_LIST_HEAD(&mgr->amp_ctrls);
818 mutex_init(&mgr->amp_ctrls_lock);
819
820 mutex_lock(&_mgr_list_lock);
821 list_add(&mgr->list, &_mgr_list);
822 mutex_unlock(&_mgr_list_lock);
823
824 return mgr;
825}
826
827struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
828 struct sk_buff *skb)
829{
830 struct amp_mgr *mgr;
831
832 mgr = amp_mgr_create(conn, false);
833 if (!mgr) {
834 BT_ERR("Could not create AMP manager");
835 return NULL;
836 }
837
838 BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
839
840 return mgr->a2mp_chan;
841}
842
843struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
844{
845 struct amp_mgr *mgr;
846
847 mutex_lock(&_mgr_list_lock);
848 list_for_each_entry(mgr, &_mgr_list, list) {
849 if (test_and_clear_bit(state, &mgr->state)) {
850 amp_mgr_get(mgr);
851 mutex_unlock(&_mgr_list_lock);
852 return mgr;
853 }
854 }
855 mutex_unlock(&_mgr_list_lock);
856
857 return NULL;
858}
859
860void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
861{
862 struct amp_mgr *mgr;
863 struct a2mp_info_rsp rsp;
864
865 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
866 if (!mgr)
867 return;
868
869 BT_DBG("%s mgr %p", hdev->name, mgr);
870
871 rsp.id = hdev->id;
872 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
873
874 if (hdev->amp_type != HCI_BREDR) {
875 rsp.status = 0;
876 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
877 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
878 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
879 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
880 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
881 }
882
883 a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
884 amp_mgr_put(mgr);
885}
886
887void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
888{
889 struct amp_mgr *mgr;
890 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
891 struct a2mp_amp_assoc_rsp *rsp;
892 size_t len;
893
894 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
895 if (!mgr)
896 return;
897
898 BT_DBG("%s mgr %p", hdev->name, mgr);
899
900 len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
901 rsp = kzalloc(len, GFP_KERNEL);
902 if (!rsp) {
903 amp_mgr_put(mgr);
904 return;
905 }
906
907 rsp->id = hdev->id;
908
909 if (status) {
910 rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
911 } else {
912 rsp->status = A2MP_STATUS_SUCCESS;
913 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
914 }
915
916 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
917 amp_mgr_put(mgr);
918 kfree(rsp);
919}
920
921void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
922{
923 struct amp_mgr *mgr;
924 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
925 struct a2mp_physlink_req *req;
926 struct l2cap_chan *bredr_chan;
927 size_t len;
928
929 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
930 if (!mgr)
931 return;
932
933 len = sizeof(*req) + loc_assoc->len;
934
935 BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
936
937 req = kzalloc(len, GFP_KERNEL);
938 if (!req) {
939 amp_mgr_put(mgr);
940 return;
941 }
942
943 bredr_chan = mgr->bredr_chan;
944 if (!bredr_chan)
945 goto clean;
946
947 req->local_id = hdev->id;
948 req->remote_id = bredr_chan->remote_amp_id;
949 memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
950
951 a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
952
953clean:
954 amp_mgr_put(mgr);
955 kfree(req);
956}
957
958void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
959{
960 struct amp_mgr *mgr;
961 struct a2mp_physlink_rsp rsp;
962 struct hci_conn *hs_hcon;
963
964 mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
965 if (!mgr)
966 return;
967
968 hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
969 if (!hs_hcon) {
970 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
971 } else {
972 rsp.remote_id = hs_hcon->remote_id;
973 rsp.status = A2MP_STATUS_SUCCESS;
974 }
975
976 BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
977 status);
978
979 rsp.local_id = hdev->id;
980 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
981 amp_mgr_put(mgr);
982}
983
984void a2mp_discover_amp(struct l2cap_chan *chan)
985{
986 struct l2cap_conn *conn = chan->conn;
987 struct amp_mgr *mgr = conn->hcon->amp_mgr;
988 struct a2mp_discov_req req;
989
990 BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
991
992 if (!mgr) {
993 mgr = amp_mgr_create(conn, true);
994 if (!mgr)
995 return;
996 }
997
998 mgr->bredr_chan = chan;
999
1000 req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1001 req.ext_feat = 0;
1002 a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
1003}
1004
