blob: eb0f4b16ff099611fb6ea0aa43add29bd1c1173f [file] [log] [blame]
Andrei Emeltchenko466f8002012-05-29 13:59:01 +03001/*
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>
Andrei Emeltchenko9740e492012-05-29 13:59:02 +030018#include <net/bluetooth/a2mp.h>
Andrei Emeltchenko903e4542012-09-27 17:26:09 +030019#include <net/bluetooth/amp.h>
Andrei Emeltchenko466f8002012-05-29 13:59:01 +030020
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +030021/* Global AMP Manager list */
22LIST_HEAD(amp_mgr_list);
23DEFINE_MUTEX(amp_mgr_list_lock);
24
Andrei Emeltchenkof6d3c6e2012-05-29 13:59:03 +030025/* 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
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +030045void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
Andrei Emeltchenkof6d3c6e2012-05-29 13:59:03 +030046{
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
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +030070u8 __next_ident(struct amp_mgr *mgr)
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +030071{
72 if (++mgr->ident == 0)
73 mgr->ident = 1;
74
75 return mgr->ident;
76}
77
Andrei Emeltchenko8598d062012-05-29 13:59:09 +030078static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
79{
80 cl->id = 0;
81 cl->type = 0;
82 cl->status = 1;
83}
84
85/* hci_dev_list shall be locked */
86static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
87{
88 int i = 0;
89 struct hci_dev *hdev;
90
91 __a2mp_cl_bredr(cl);
92
93 list_for_each_entry(hdev, &hci_dev_list, list) {
94 /* Iterate through AMP controllers */
95 if (hdev->id == HCI_BREDR_ID)
96 continue;
97
98 /* Starting from second entry */
99 if (++i >= num_ctrl)
100 return;
101
102 cl[i].id = hdev->id;
103 cl[i].type = hdev->amp_type;
104 cl[i].status = hdev->amp_status;
105 }
106}
107
Andrei Emeltchenko21dbd2c2012-05-29 13:59:08 +0300108/* Processing A2MP messages */
109static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
110 struct a2mp_cmd *hdr)
111{
112 struct a2mp_cmd_rej *rej = (void *) skb->data;
113
114 if (le16_to_cpu(hdr->len) < sizeof(*rej))
115 return -EINVAL;
116
117 BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
118
119 skb_pull(skb, sizeof(*rej));
120
121 return 0;
122}
123
Andrei Emeltchenko8598d062012-05-29 13:59:09 +0300124static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
125 struct a2mp_cmd *hdr)
126{
127 struct a2mp_discov_req *req = (void *) skb->data;
128 u16 len = le16_to_cpu(hdr->len);
129 struct a2mp_discov_rsp *rsp;
130 u16 ext_feat;
131 u8 num_ctrl;
132
133 if (len < sizeof(*req))
134 return -EINVAL;
135
136 skb_pull(skb, sizeof(*req));
137
138 ext_feat = le16_to_cpu(req->ext_feat);
139
140 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
141
142 /* check that packet is not broken for now */
143 while (ext_feat & A2MP_FEAT_EXT) {
144 if (len < sizeof(ext_feat))
145 return -EINVAL;
146
147 ext_feat = get_unaligned_le16(skb->data);
148 BT_DBG("efm 0x%4.4x", ext_feat);
149 len -= sizeof(ext_feat);
150 skb_pull(skb, sizeof(ext_feat));
151 }
152
153 read_lock(&hci_dev_list_lock);
154
155 num_ctrl = __hci_num_ctrl();
156 len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
157 rsp = kmalloc(len, GFP_ATOMIC);
158 if (!rsp) {
159 read_unlock(&hci_dev_list_lock);
160 return -ENOMEM;
161 }
162
163 rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
164 rsp->ext_feat = 0;
165
166 __a2mp_add_cl(mgr, rsp->cl, num_ctrl);
167
168 read_unlock(&hci_dev_list_lock);
169
170 a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
171
172 kfree(rsp);
173 return 0;
174}
175
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300176static 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;
Andrei Emeltchenko2766be42012-09-27 17:26:21 +0300183 bool found = false;
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300184
185 if (len < sizeof(*rsp))
186 return -EINVAL;
187
188 len -= sizeof(*rsp);
189 skb_pull(skb, sizeof(*rsp));
190
191 ext_feat = le16_to_cpu(rsp->ext_feat);
192
193 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
194
195 /* check that packet is not broken for now */
196 while (ext_feat & A2MP_FEAT_EXT) {
197 if (len < sizeof(ext_feat))
198 return -EINVAL;
199
200 ext_feat = get_unaligned_le16(skb->data);
201 BT_DBG("efm 0x%4.4x", ext_feat);
202 len -= sizeof(ext_feat);
203 skb_pull(skb, sizeof(ext_feat));
204 }
205
206 cl = (void *) skb->data;
207 while (len >= sizeof(*cl)) {
208 BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
209 cl->status);
210
211 if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
212 struct a2mp_info_req req;
213
Andrei Emeltchenko2766be42012-09-27 17:26:21 +0300214 found = true;
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300215 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
Andrei Emeltchenko2766be42012-09-27 17:26:21 +0300224 /* Fall back to L2CAP init sequence */
225 if (!found) {
226 struct l2cap_conn *conn = mgr->l2cap_conn;
227 struct l2cap_chan *chan;
228
229 mutex_lock(&conn->chan_lock);
230
231 list_for_each_entry(chan, &conn->chan_l, list) {
232
233 BT_DBG("chan %p state %s", chan,
234 state_to_string(chan->state));
235
236 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
237 continue;
238
239 l2cap_chan_lock(chan);
240
241 if (chan->state == BT_CONNECT)
242 l2cap_send_conn_req(chan);
243
244 l2cap_chan_unlock(chan);
245 }
246
247 mutex_unlock(&conn->chan_lock);
248 }
249
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300250 return 0;
251}
252
Andrei Emeltchenko329d81af2012-05-29 13:59:10 +0300253static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
254 struct a2mp_cmd *hdr)
255{
256 struct a2mp_cl *cl = (void *) skb->data;
257
258 while (skb->len >= sizeof(*cl)) {
259 BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
260 cl->status);
261 cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
262 }
263
264 /* TODO send A2MP_CHANGE_RSP */
265
266 return 0;
267}
268
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300269static 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;
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300273 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
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300280 hdev = hci_dev_get(req->id);
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300281 if (!hdev || hdev->dev_type != HCI_AMP) {
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +0300282 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);
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300289
290 goto done;
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300291 }
292
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200293 set_bit(READ_LOC_AMP_INFO, &mgr->state);
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300294 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300295
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300296done:
297 if (hdev)
298 hci_dev_put(hdev);
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300299
300 skb_pull(skb, sizeof(*req));
301 return 0;
302}
303
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300304static 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
Andrei Emeltchenkofa4ebc62012-10-05 16:56:55 +0300319 ctrl = amp_ctrl_add(mgr, rsp->id);
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300320 if (!ctrl)
321 return -ENOMEM;
322
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300323 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
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300331static 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;
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300336 struct amp_mgr *tmp;
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300337
338 if (le16_to_cpu(hdr->len) < sizeof(*req))
339 return -EINVAL;
340
341 BT_DBG("id %d", req->id);
342
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300343 /* Make sure that other request is not processed */
344 tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
345
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300346 hdev = hci_dev_get(req->id);
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300347 if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300348 struct a2mp_amp_assoc_rsp rsp;
349 rsp.id = req->id;
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300350
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 }
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300357
358 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
359 &rsp);
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300360
361 goto done;
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300362 }
363
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300364 amp_read_loc_assoc(hdev, mgr);
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300365
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300366done:
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300367 if (hdev)
368 hci_dev_put(hdev);
369
370 skb_pull(skb, sizeof(*req));
371 return 0;
372}
373
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300374static 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;
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300382 size_t assoc_len;
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300383
384 if (len < sizeof(*rsp))
385 return -EINVAL;
386
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300387 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);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300391
392 if (rsp->status)
393 return -EINVAL;
394
395 /* Save remote ASSOC data */
396 ctrl = amp_ctrl_lookup(mgr, rsp->id);
397 if (ctrl) {
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300398 u8 *assoc;
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300399
400 assoc = kzalloc(assoc_len, GFP_KERNEL);
401 if (!assoc) {
402 amp_ctrl_put(ctrl);
403 return -ENOMEM;
404 }
405
406 memcpy(assoc, rsp->amp_assoc, assoc_len);
407 ctrl->assoc = assoc;
408 ctrl->assoc_len = assoc_len;
409 ctrl->assoc_rem_len = assoc_len;
410 ctrl->assoc_len_so_far = 0;
411
412 amp_ctrl_put(ctrl);
413 }
414
415 /* Create Phys Link */
416 hdev = hci_dev_get(rsp->id);
417 if (!hdev)
418 return -EINVAL;
419
Andrei Emeltchenkoa0c234f2012-10-05 16:56:56 +0300420 hcon = phylink_add(hdev, mgr, rsp->id, true);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300421 if (!hcon)
422 goto done;
423
424 BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
425
Andrei Emeltchenkofffadc02012-11-01 15:37:03 +0200426 mgr->bredr_chan->remote_amp_id = rsp->id;
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300427
Andrei Emeltchenkoa02226d2012-09-27 17:26:19 +0300428 amp_create_phylink(hdev, mgr, hcon);
429
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300430done:
431 hci_dev_put(hdev);
432 skb_pull(skb, len);
433 return 0;
434}
435
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300436static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
437 struct a2mp_cmd *hdr)
438{
439 struct a2mp_physlink_req *req = (void *) skb->data;
440
441 struct a2mp_physlink_rsp rsp;
442 struct hci_dev *hdev;
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300443 struct hci_conn *hcon;
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300444 struct amp_ctrl *ctrl;
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300445
446 if (le16_to_cpu(hdr->len) < sizeof(*req))
447 return -EINVAL;
448
449 BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
450
451 rsp.local_id = req->remote_id;
452 rsp.remote_id = req->local_id;
453
454 hdev = hci_dev_get(req->remote_id);
455 if (!hdev || hdev->amp_type != HCI_AMP) {
456 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
457 goto send_rsp;
458 }
459
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300460 ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
461 if (!ctrl) {
Andrei Emeltchenkofa4ebc62012-10-05 16:56:55 +0300462 ctrl = amp_ctrl_add(mgr, rsp.remote_id);
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300463 if (ctrl) {
464 amp_ctrl_get(ctrl);
465 } else {
466 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
467 goto send_rsp;
468 }
469 }
470
471 if (ctrl) {
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300472 size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
473 u8 *assoc;
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300474
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300475 assoc = kzalloc(assoc_len, GFP_KERNEL);
476 if (!assoc) {
477 amp_ctrl_put(ctrl);
478 return -ENOMEM;
479 }
480
481 memcpy(assoc, req->amp_assoc, assoc_len);
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
Andrei Emeltchenkoa0c234f2012-10-05 16:56:56 +0300490 hcon = phylink_add(hdev, mgr, req->local_id, false);
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300491 if (hcon) {
Andrei Emeltchenkodffa3872012-09-27 17:26:23 +0300492 amp_accept_phylink(hdev, mgr, hcon);
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300493 rsp.status = A2MP_STATUS_SUCCESS;
494 } else {
495 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
496 }
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300497
498send_rsp:
499 if (hdev)
500 hci_dev_put(hdev);
501
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200502 /* 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 {
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200509 set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200510 mgr->ident = hdr->ident;
511 }
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300512
513 skb_pull(skb, le16_to_cpu(hdr->len));
514 return 0;
515}
516
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300517static 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;
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300523 struct hci_conn *hcon;
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300524
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
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300534 hdev = hci_dev_get(req->remote_id);
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300535 if (!hdev) {
536 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
537 goto send_rsp;
538 }
539
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300540 hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
541 if (!hcon) {
542 BT_ERR("No phys link exist");
543 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
544 goto clean;
545 }
546
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300547 /* TODO Disconnect Phys Link here */
548
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300549clean:
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300550 hci_dev_put(hdev);
551
552send_rsp:
553 a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
554
555 skb_pull(skb, sizeof(*req));
556 return 0;
557}
558
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300559static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
560 struct a2mp_cmd *hdr)
561{
Andrei Emeltchenko8e8c7e32012-07-19 17:03:41 +0300562 BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300563
564 skb_pull(skb, le16_to_cpu(hdr->len));
565 return 0;
566}
567
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300568/* Handle A2MP signalling */
569static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
570{
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300571 struct a2mp_cmd *hdr;
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300572 struct amp_mgr *mgr = chan->data;
573 int err = 0;
574
575 amp_mgr_get(mgr);
576
577 while (skb->len >= sizeof(*hdr)) {
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300578 u16 len;
579
580 hdr = (void *) skb->data;
581 len = le16_to_cpu(hdr->len);
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300582
Andrei Emeltchenko8e8c7e32012-07-19 17:03:41 +0300583 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300584
585 skb_pull(skb, sizeof(*hdr));
586
587 if (len > skb->len || !hdr->ident) {
588 err = -EINVAL;
589 break;
590 }
591
592 mgr->ident = hdr->ident;
593
594 switch (hdr->code) {
595 case A2MP_COMMAND_REJ:
Andrei Emeltchenko21dbd2c2012-05-29 13:59:08 +0300596 a2mp_command_rej(mgr, skb, hdr);
597 break;
598
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300599 case A2MP_DISCOVER_REQ:
Andrei Emeltchenko8598d062012-05-29 13:59:09 +0300600 err = a2mp_discover_req(mgr, skb, hdr);
601 break;
602
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300603 case A2MP_CHANGE_NOTIFY:
Andrei Emeltchenko329d81af2012-05-29 13:59:10 +0300604 err = a2mp_change_notify(mgr, skb, hdr);
605 break;
606
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300607 case A2MP_GETINFO_REQ:
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300608 err = a2mp_getinfo_req(mgr, skb, hdr);
609 break;
610
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300611 case A2MP_GETAMPASSOC_REQ:
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300612 err = a2mp_getampassoc_req(mgr, skb, hdr);
613 break;
614
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300615 case A2MP_CREATEPHYSLINK_REQ:
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300616 err = a2mp_createphyslink_req(mgr, skb, hdr);
617 break;
618
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300619 case A2MP_DISCONNPHYSLINK_REQ:
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300620 err = a2mp_discphyslink_req(mgr, skb, hdr);
621 break;
622
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300623 case A2MP_DISCOVER_RSP:
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300624 err = a2mp_discover_rsp(mgr, skb, hdr);
625 break;
626
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300627 case A2MP_GETINFO_RSP:
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300628 err = a2mp_getinfo_rsp(mgr, skb, hdr);
629 break;
630
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300631 case A2MP_GETAMPASSOC_RSP:
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300632 err = a2mp_getampassoc_rsp(mgr, skb, hdr);
633 break;
634
635 case A2MP_CHANGE_RSP:
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300636 case A2MP_CREATEPHYSLINK_RSP:
637 case A2MP_DISCONNPHYSLINK_RSP:
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300638 err = a2mp_cmd_rsp(mgr, skb, hdr);
639 break;
640
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300641 default:
642 BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
643 err = -EINVAL;
644 break;
645 }
646 }
647
648 if (err) {
649 struct a2mp_cmd_rej rej;
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300650
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300651 rej.reason = __constant_cpu_to_le16(0);
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300652 hdr = (void *) skb->data;
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300653
654 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
655
656 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
657 &rej);
658 }
659
660 /* Always free skb and return success error code to prevent
661 from sending L2CAP Disconnect over A2MP channel */
662 kfree_skb(skb);
663
664 amp_mgr_put(mgr);
665
666 return 0;
667}
668
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300669static void a2mp_chan_close_cb(struct l2cap_chan *chan)
670{
Jaganath Kanakkassery4af66c62012-07-13 18:17:55 +0530671 l2cap_chan_put(chan);
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300672}
673
674static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
675{
676 struct amp_mgr *mgr = chan->data;
677
678 if (!mgr)
679 return;
680
681 BT_DBG("chan %p state %s", chan, state_to_string(state));
682
683 chan->state = state;
684
685 switch (state) {
686 case BT_CLOSED:
687 if (mgr)
688 amp_mgr_put(mgr);
689 break;
690 }
691}
692
693static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
694 unsigned long len, int nb)
695{
696 return bt_skb_alloc(len, GFP_KERNEL);
697}
698
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300699static struct l2cap_ops a2mp_chan_ops = {
700 .name = "L2CAP A2MP channel",
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300701 .recv = a2mp_chan_recv_cb,
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300702 .close = a2mp_chan_close_cb,
703 .state_change = a2mp_chan_state_change_cb,
704 .alloc_skb = a2mp_chan_alloc_skb_cb,
705
706 /* Not implemented for A2MP */
Gustavo Padovan7e1af8a2012-05-29 13:19:26 -0300707 .new_connection = l2cap_chan_no_new_connection,
708 .teardown = l2cap_chan_no_teardown,
709 .ready = l2cap_chan_no_ready,
Gustavo Padovan2dc4e512012-10-12 19:35:24 +0800710 .defer = l2cap_chan_no_defer,
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300711};
712
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300713static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300714{
715 struct l2cap_chan *chan;
716 int err;
717
718 chan = l2cap_chan_create();
719 if (!chan)
720 return NULL;
721
722 BT_DBG("chan %p", chan);
723
Andrei Emeltchenko416fa752012-05-29 13:59:16 +0300724 chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300725 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
726
727 chan->ops = &a2mp_chan_ops;
728
729 l2cap_chan_set_defaults(chan);
730 chan->remote_max_tx = chan->max_tx;
731 chan->remote_tx_win = chan->tx_win;
732
733 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
734 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
735
736 skb_queue_head_init(&chan->tx_q);
737
738 chan->mode = L2CAP_MODE_ERTM;
739
740 err = l2cap_ertm_init(chan);
741 if (err < 0) {
742 l2cap_chan_del(chan, 0);
743 return NULL;
744 }
745
746 chan->conf_state = 0;
747
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300748 if (locked)
749 __l2cap_chan_add(conn, chan);
750 else
751 l2cap_chan_add(conn, chan);
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300752
753 chan->remote_mps = chan->omtu;
754 chan->mps = chan->omtu;
755
756 chan->state = BT_CONNECTED;
757
758 return chan;
759}
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300760
761/* AMP Manager functions */
Andrei Emeltchenkof706adf2012-10-18 13:16:19 +0300762struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300763{
Andrei Emeltchenkoa0dfe0a2012-07-11 14:43:35 +0300764 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300765
766 kref_get(&mgr->kref);
Andrei Emeltchenkof706adf2012-10-18 13:16:19 +0300767
768 return mgr;
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300769}
770
771static void amp_mgr_destroy(struct kref *kref)
772{
773 struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
774
775 BT_DBG("mgr %p", mgr);
776
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300777 mutex_lock(&amp_mgr_list_lock);
778 list_del(&mgr->list);
779 mutex_unlock(&amp_mgr_list_lock);
780
Andrei Emeltchenko52c0d6e2012-09-27 17:26:12 +0300781 amp_ctrl_list_flush(mgr);
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300782 kfree(mgr);
783}
784
785int amp_mgr_put(struct amp_mgr *mgr)
786{
Andrei Emeltchenkoa0dfe0a2012-07-11 14:43:35 +0300787 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300788
789 return kref_put(&mgr->kref, &amp_mgr_destroy);
790}
791
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300792static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300793{
794 struct amp_mgr *mgr;
795 struct l2cap_chan *chan;
796
797 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
798 if (!mgr)
799 return NULL;
800
801 BT_DBG("conn %p mgr %p", conn, mgr);
802
803 mgr->l2cap_conn = conn;
804
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300805 chan = a2mp_chan_open(conn, locked);
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300806 if (!chan) {
807 kfree(mgr);
808 return NULL;
809 }
810
811 mgr->a2mp_chan = chan;
812 chan->data = mgr;
813
814 conn->hcon->amp_mgr = mgr;
815
816 kref_init(&mgr->kref);
817
Andrei Emeltchenko52c0d6e2012-09-27 17:26:12 +0300818 /* Remote AMP ctrl list initialization */
819 INIT_LIST_HEAD(&mgr->amp_ctrls);
820 mutex_init(&mgr->amp_ctrls_lock);
821
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300822 mutex_lock(&amp_mgr_list_lock);
823 list_add(&mgr->list, &amp_mgr_list);
824 mutex_unlock(&amp_mgr_list_lock);
825
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300826 return mgr;
827}
Andrei Emeltchenko97e8e892012-05-29 13:59:17 +0300828
829struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
830 struct sk_buff *skb)
831{
832 struct amp_mgr *mgr;
833
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300834 mgr = amp_mgr_create(conn, false);
Andrei Emeltchenko97e8e892012-05-29 13:59:17 +0300835 if (!mgr) {
836 BT_ERR("Could not create AMP manager");
837 return NULL;
838 }
839
840 BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
841
842 return mgr->a2mp_chan;
843}
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300844
845struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
846{
847 struct amp_mgr *mgr;
848
849 mutex_lock(&amp_mgr_list_lock);
850 list_for_each_entry(mgr, &amp_mgr_list, list) {
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200851 if (test_and_clear_bit(state, &mgr->state)) {
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300852 amp_mgr_get(mgr);
853 mutex_unlock(&amp_mgr_list_lock);
854 return mgr;
855 }
856 }
857 mutex_unlock(&amp_mgr_list_lock);
858
859 return NULL;
860}
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +0300861
862void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
863{
864 struct amp_mgr *mgr;
865 struct a2mp_info_rsp rsp;
866
867 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
868 if (!mgr)
869 return;
870
871 BT_DBG("%s mgr %p", hdev->name, mgr);
872
873 rsp.id = hdev->id;
874 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
875
876 if (hdev->amp_type != HCI_BREDR) {
877 rsp.status = 0;
878 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
879 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
880 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
881 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
882 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
883 }
884
885 a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
886 amp_mgr_put(mgr);
887}
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300888
889void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
890{
891 struct amp_mgr *mgr;
892 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
893 struct a2mp_amp_assoc_rsp *rsp;
894 size_t len;
895
896 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
897 if (!mgr)
898 return;
899
900 BT_DBG("%s mgr %p", hdev->name, mgr);
901
902 len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
903 rsp = kzalloc(len, GFP_KERNEL);
904 if (!rsp) {
905 amp_mgr_put(mgr);
906 return;
907 }
908
909 rsp->id = hdev->id;
910
911 if (status) {
912 rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
913 } else {
914 rsp->status = A2MP_STATUS_SUCCESS;
915 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
916 }
917
918 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
919 amp_mgr_put(mgr);
920 kfree(rsp);
921}
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300922
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300923void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
924{
925 struct amp_mgr *mgr;
926 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
927 struct a2mp_physlink_req *req;
928 struct l2cap_chan *bredr_chan;
929 size_t len;
930
931 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
932 if (!mgr)
933 return;
934
935 len = sizeof(*req) + loc_assoc->len;
936
937 BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
938
939 req = kzalloc(len, GFP_KERNEL);
940 if (!req) {
941 amp_mgr_put(mgr);
942 return;
943 }
944
945 bredr_chan = mgr->bredr_chan;
946 if (!bredr_chan)
947 goto clean;
948
949 req->local_id = hdev->id;
Andrei Emeltchenkofffadc02012-11-01 15:37:03 +0200950 req->remote_id = bredr_chan->remote_amp_id;
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300951 memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
952
953 a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
954
955clean:
956 amp_mgr_put(mgr);
957 kfree(req);
958}
959
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200960void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
961{
962 struct amp_mgr *mgr;
963 struct a2mp_physlink_rsp rsp;
964 struct hci_conn *hs_hcon;
965
966 mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
967 if (!mgr)
968 return;
969
970 hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
971 if (!hs_hcon) {
972 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
973 } else {
974 rsp.remote_id = hs_hcon->remote_id;
975 rsp.status = A2MP_STATUS_SUCCESS;
976 }
977
978 BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
979 status);
980
981 rsp.local_id = hdev->id;
982 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
983 amp_mgr_put(mgr);
984}
985
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300986void a2mp_discover_amp(struct l2cap_chan *chan)
987{
988 struct l2cap_conn *conn = chan->conn;
989 struct amp_mgr *mgr = conn->hcon->amp_mgr;
990 struct a2mp_discov_req req;
991
992 BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
993
994 if (!mgr) {
995 mgr = amp_mgr_create(conn, true);
996 if (!mgr)
997 return;
998 }
999
1000 mgr->bredr_chan = chan;
1001
1002 req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1003 req.ext_feat = 0;
1004 a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
1005}