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