Root/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 */
23LIST_HEAD(amp_mgr_list);
24DEFINE_MUTEX(amp_mgr_list_lock);
25
26/* A2MP build & send command helper functions */
27static 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
46void 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
71u8 __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 */
80static 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 */
103static 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
118static 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
178static 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
255static 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
271static 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
298done:
299    if (hdev)
300        hci_dev_put(hdev);
301
302    skb_pull(skb, sizeof(*req));
303    return 0;
304}
305
306static 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
333static 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
368done:
369    if (hdev)
370        hci_dev_put(hdev);
371
372    skb_pull(skb, sizeof(*req));
373    return 0;
374}
375
376static 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
431done:
432    hci_dev_put(hdev);
433    skb_pull(skb, len);
434    return 0;
435}
436
437static 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
498send_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
517static 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
550clean:
551    hci_dev_put(hdev);
552
553send_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
560static 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 */
570static 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
670static void a2mp_chan_close_cb(struct l2cap_chan *chan)
671{
672    l2cap_chan_put(chan);
673}
674
675static 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
695static 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
701static 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
718static 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 */
767struct 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
776static 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
790int 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
797static 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
834struct 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
853struct 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
870void 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
897void 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
931void 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
963clean:
964    amp_mgr_put(mgr);
965    kfree(req);
966}
967
968void 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
994void 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}
1014

Archive Download this file

Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master

Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9



interactive