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