blob: b2ef3d392a0e8c03c9652fa8444c552676334f75 [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;
Marcel Holtmannece69122013-10-05 11:47:43 -070081 cl->type = AMP_TYPE_BREDR;
Andrei Emeltchenko8598d062012-05-29 13:59:09 +030082 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 */
Marcel Holtmann23b90032013-10-05 11:47:40 -070095 if (hdev->dev_type != HCI_AMP)
Andrei Emeltchenko8598d062012-05-29 13:59:09 +030096 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;
Marcel Holtmannf822c412013-10-05 11:47:41 -0700132 struct hci_dev *hdev;
Andrei Emeltchenko8598d062012-05-29 13:59:09 +0300133
134 if (len < sizeof(*req))
135 return -EINVAL;
136
137 skb_pull(skb, sizeof(*req));
138
139 ext_feat = le16_to_cpu(req->ext_feat);
140
141 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
142
143 /* check that packet is not broken for now */
144 while (ext_feat & A2MP_FEAT_EXT) {
145 if (len < sizeof(ext_feat))
146 return -EINVAL;
147
148 ext_feat = get_unaligned_le16(skb->data);
149 BT_DBG("efm 0x%4.4x", ext_feat);
150 len -= sizeof(ext_feat);
151 skb_pull(skb, sizeof(ext_feat));
152 }
153
154 read_lock(&hci_dev_list_lock);
155
Marcel Holtmannf822c412013-10-05 11:47:41 -0700156 /* at minimum the BR/EDR needs to be listed */
157 num_ctrl = 1;
158
159 list_for_each_entry(hdev, &hci_dev_list, list) {
160 if (hdev->dev_type == HCI_AMP)
161 num_ctrl++;
162 }
163
Andrei Emeltchenko8598d062012-05-29 13:59:09 +0300164 len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
165 rsp = kmalloc(len, GFP_ATOMIC);
166 if (!rsp) {
167 read_unlock(&hci_dev_list_lock);
168 return -ENOMEM;
169 }
170
171 rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
172 rsp->ext_feat = 0;
173
174 __a2mp_add_cl(mgr, rsp->cl, num_ctrl);
175
176 read_unlock(&hci_dev_list_lock);
177
178 a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
179
180 kfree(rsp);
181 return 0;
182}
183
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300184static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
185 struct a2mp_cmd *hdr)
186{
187 struct a2mp_discov_rsp *rsp = (void *) skb->data;
188 u16 len = le16_to_cpu(hdr->len);
189 struct a2mp_cl *cl;
190 u16 ext_feat;
Andrei Emeltchenko2766be42012-09-27 17:26:21 +0300191 bool found = false;
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300192
193 if (len < sizeof(*rsp))
194 return -EINVAL;
195
196 len -= sizeof(*rsp);
197 skb_pull(skb, sizeof(*rsp));
198
199 ext_feat = le16_to_cpu(rsp->ext_feat);
200
201 BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
202
203 /* check that packet is not broken for now */
204 while (ext_feat & A2MP_FEAT_EXT) {
205 if (len < sizeof(ext_feat))
206 return -EINVAL;
207
208 ext_feat = get_unaligned_le16(skb->data);
209 BT_DBG("efm 0x%4.4x", ext_feat);
210 len -= sizeof(ext_feat);
211 skb_pull(skb, sizeof(ext_feat));
212 }
213
214 cl = (void *) skb->data;
215 while (len >= sizeof(*cl)) {
216 BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
217 cl->status);
218
219 if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
220 struct a2mp_info_req req;
221
Andrei Emeltchenko2766be42012-09-27 17:26:21 +0300222 found = true;
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300223 req.id = cl->id;
224 a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
225 sizeof(req), &req);
226 }
227
228 len -= sizeof(*cl);
229 cl = (void *) skb_pull(skb, sizeof(*cl));
230 }
231
Andrei Emeltchenko2766be42012-09-27 17:26:21 +0300232 /* Fall back to L2CAP init sequence */
233 if (!found) {
234 struct l2cap_conn *conn = mgr->l2cap_conn;
235 struct l2cap_chan *chan;
236
237 mutex_lock(&conn->chan_lock);
238
239 list_for_each_entry(chan, &conn->chan_l, list) {
240
241 BT_DBG("chan %p state %s", chan,
242 state_to_string(chan->state));
243
244 if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
245 continue;
246
247 l2cap_chan_lock(chan);
248
249 if (chan->state == BT_CONNECT)
250 l2cap_send_conn_req(chan);
251
252 l2cap_chan_unlock(chan);
253 }
254
255 mutex_unlock(&conn->chan_lock);
256 }
257
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300258 return 0;
259}
260
Andrei Emeltchenko329d81af2012-05-29 13:59:10 +0300261static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
262 struct a2mp_cmd *hdr)
263{
264 struct a2mp_cl *cl = (void *) skb->data;
265
266 while (skb->len >= sizeof(*cl)) {
267 BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
268 cl->status);
269 cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
270 }
271
272 /* TODO send A2MP_CHANGE_RSP */
273
274 return 0;
275}
276
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300277static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
278 struct a2mp_cmd *hdr)
279{
280 struct a2mp_info_req *req = (void *) skb->data;
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300281 struct hci_dev *hdev;
282
283 if (le16_to_cpu(hdr->len) < sizeof(*req))
284 return -EINVAL;
285
286 BT_DBG("id %d", req->id);
287
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300288 hdev = hci_dev_get(req->id);
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300289 if (!hdev || hdev->dev_type != HCI_AMP) {
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +0300290 struct a2mp_info_rsp rsp;
291
292 rsp.id = req->id;
293 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
294
295 a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
296 &rsp);
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300297
298 goto done;
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300299 }
300
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200301 set_bit(READ_LOC_AMP_INFO, &mgr->state);
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300302 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300303
Andrei Emeltchenkobc8dce42012-09-28 14:28:50 +0300304done:
305 if (hdev)
306 hci_dev_put(hdev);
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300307
308 skb_pull(skb, sizeof(*req));
309 return 0;
310}
311
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300312static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
313 struct a2mp_cmd *hdr)
314{
315 struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
316 struct a2mp_amp_assoc_req req;
317 struct amp_ctrl *ctrl;
318
319 if (le16_to_cpu(hdr->len) < sizeof(*rsp))
320 return -EINVAL;
321
322 BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
323
324 if (rsp->status)
325 return -EINVAL;
326
Andrei Emeltchenkofa4ebc62012-10-05 16:56:55 +0300327 ctrl = amp_ctrl_add(mgr, rsp->id);
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300328 if (!ctrl)
329 return -ENOMEM;
330
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300331 req.id = rsp->id;
332 a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
333 &req);
334
335 skb_pull(skb, sizeof(*rsp));
336 return 0;
337}
338
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300339static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
340 struct a2mp_cmd *hdr)
341{
342 struct a2mp_amp_assoc_req *req = (void *) skb->data;
343 struct hci_dev *hdev;
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300344 struct amp_mgr *tmp;
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300345
346 if (le16_to_cpu(hdr->len) < sizeof(*req))
347 return -EINVAL;
348
349 BT_DBG("id %d", req->id);
350
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300351 /* Make sure that other request is not processed */
352 tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
353
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300354 hdev = hci_dev_get(req->id);
Marcel Holtmannece69122013-10-05 11:47:43 -0700355 if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) {
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300356 struct a2mp_amp_assoc_rsp rsp;
357 rsp.id = req->id;
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300358
359 if (tmp) {
360 rsp.status = A2MP_STATUS_COLLISION_OCCURED;
361 amp_mgr_put(tmp);
362 } else {
363 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
364 }
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300365
366 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
367 &rsp);
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300368
369 goto done;
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300370 }
371
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300372 amp_read_loc_assoc(hdev, mgr);
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300373
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300374done:
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300375 if (hdev)
376 hci_dev_put(hdev);
377
378 skb_pull(skb, sizeof(*req));
379 return 0;
380}
381
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300382static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
383 struct a2mp_cmd *hdr)
384{
385 struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
386 u16 len = le16_to_cpu(hdr->len);
387 struct hci_dev *hdev;
388 struct amp_ctrl *ctrl;
389 struct hci_conn *hcon;
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300390 size_t assoc_len;
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300391
392 if (len < sizeof(*rsp))
393 return -EINVAL;
394
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300395 assoc_len = len - sizeof(*rsp);
396
397 BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
398 assoc_len);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300399
400 if (rsp->status)
401 return -EINVAL;
402
403 /* Save remote ASSOC data */
404 ctrl = amp_ctrl_lookup(mgr, rsp->id);
405 if (ctrl) {
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300406 u8 *assoc;
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300407
Alexandru Gheorghiu5ae327f2013-03-17 07:16:50 +0200408 assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300409 if (!assoc) {
410 amp_ctrl_put(ctrl);
411 return -ENOMEM;
412 }
413
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300414 ctrl->assoc = assoc;
415 ctrl->assoc_len = assoc_len;
416 ctrl->assoc_rem_len = assoc_len;
417 ctrl->assoc_len_so_far = 0;
418
419 amp_ctrl_put(ctrl);
420 }
421
422 /* Create Phys Link */
423 hdev = hci_dev_get(rsp->id);
424 if (!hdev)
425 return -EINVAL;
426
Andrei Emeltchenkoa0c234f2012-10-05 16:56:56 +0300427 hcon = phylink_add(hdev, mgr, rsp->id, true);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300428 if (!hcon)
429 goto done;
430
431 BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
432
Andrei Emeltchenkofffadc02012-11-01 15:37:03 +0200433 mgr->bredr_chan->remote_amp_id = rsp->id;
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300434
Andrei Emeltchenkoa02226d2012-09-27 17:26:19 +0300435 amp_create_phylink(hdev, mgr, hcon);
436
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300437done:
438 hci_dev_put(hdev);
439 skb_pull(skb, len);
440 return 0;
441}
442
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300443static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
444 struct a2mp_cmd *hdr)
445{
446 struct a2mp_physlink_req *req = (void *) skb->data;
447
448 struct a2mp_physlink_rsp rsp;
449 struct hci_dev *hdev;
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300450 struct hci_conn *hcon;
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300451 struct amp_ctrl *ctrl;
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300452
453 if (le16_to_cpu(hdr->len) < sizeof(*req))
454 return -EINVAL;
455
456 BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
457
458 rsp.local_id = req->remote_id;
459 rsp.remote_id = req->local_id;
460
461 hdev = hci_dev_get(req->remote_id);
Marcel Holtmannece69122013-10-05 11:47:43 -0700462 if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) {
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300463 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
464 goto send_rsp;
465 }
466
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300467 ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
468 if (!ctrl) {
Andrei Emeltchenkofa4ebc62012-10-05 16:56:55 +0300469 ctrl = amp_ctrl_add(mgr, rsp.remote_id);
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300470 if (ctrl) {
471 amp_ctrl_get(ctrl);
472 } else {
473 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
474 goto send_rsp;
475 }
476 }
477
478 if (ctrl) {
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300479 size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
480 u8 *assoc;
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300481
Alexandru Gheorghiu5ae327f2013-03-17 07:16:50 +0200482 assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300483 if (!assoc) {
484 amp_ctrl_put(ctrl);
485 return -ENOMEM;
486 }
487
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300488 ctrl->assoc = assoc;
489 ctrl->assoc_len = assoc_len;
490 ctrl->assoc_rem_len = assoc_len;
491 ctrl->assoc_len_so_far = 0;
492
493 amp_ctrl_put(ctrl);
494 }
495
Andrei Emeltchenkoa0c234f2012-10-05 16:56:56 +0300496 hcon = phylink_add(hdev, mgr, req->local_id, false);
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300497 if (hcon) {
Andrei Emeltchenkodffa3872012-09-27 17:26:23 +0300498 amp_accept_phylink(hdev, mgr, hcon);
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300499 rsp.status = A2MP_STATUS_SUCCESS;
500 } else {
501 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
502 }
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300503
504send_rsp:
505 if (hdev)
506 hci_dev_put(hdev);
507
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200508 /* Reply error now and success after HCI Write Remote AMP Assoc
509 command complete with success status
510 */
511 if (rsp.status != A2MP_STATUS_SUCCESS) {
512 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
513 sizeof(rsp), &rsp);
514 } else {
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200515 set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200516 mgr->ident = hdr->ident;
517 }
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300518
519 skb_pull(skb, le16_to_cpu(hdr->len));
520 return 0;
521}
522
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300523static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
524 struct a2mp_cmd *hdr)
525{
526 struct a2mp_physlink_req *req = (void *) skb->data;
527 struct a2mp_physlink_rsp rsp;
528 struct hci_dev *hdev;
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300529 struct hci_conn *hcon;
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300530
531 if (le16_to_cpu(hdr->len) < sizeof(*req))
532 return -EINVAL;
533
534 BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
535
536 rsp.local_id = req->remote_id;
537 rsp.remote_id = req->local_id;
538 rsp.status = A2MP_STATUS_SUCCESS;
539
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300540 hdev = hci_dev_get(req->remote_id);
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300541 if (!hdev) {
542 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
543 goto send_rsp;
544 }
545
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300546 hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
547 if (!hcon) {
548 BT_ERR("No phys link exist");
549 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
550 goto clean;
551 }
552
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300553 /* TODO Disconnect Phys Link here */
554
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300555clean:
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300556 hci_dev_put(hdev);
557
558send_rsp:
559 a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
560
561 skb_pull(skb, sizeof(*req));
562 return 0;
563}
564
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300565static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
566 struct a2mp_cmd *hdr)
567{
Andrei Emeltchenko8e8c7e32012-07-19 17:03:41 +0300568 BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300569
570 skb_pull(skb, le16_to_cpu(hdr->len));
571 return 0;
572}
573
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300574/* Handle A2MP signalling */
575static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
576{
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300577 struct a2mp_cmd *hdr;
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300578 struct amp_mgr *mgr = chan->data;
579 int err = 0;
580
581 amp_mgr_get(mgr);
582
583 while (skb->len >= sizeof(*hdr)) {
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300584 u16 len;
585
586 hdr = (void *) skb->data;
587 len = le16_to_cpu(hdr->len);
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300588
Andrei Emeltchenko8e8c7e32012-07-19 17:03:41 +0300589 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300590
591 skb_pull(skb, sizeof(*hdr));
592
593 if (len > skb->len || !hdr->ident) {
594 err = -EINVAL;
595 break;
596 }
597
598 mgr->ident = hdr->ident;
599
600 switch (hdr->code) {
601 case A2MP_COMMAND_REJ:
Andrei Emeltchenko21dbd2c2012-05-29 13:59:08 +0300602 a2mp_command_rej(mgr, skb, hdr);
603 break;
604
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300605 case A2MP_DISCOVER_REQ:
Andrei Emeltchenko8598d062012-05-29 13:59:09 +0300606 err = a2mp_discover_req(mgr, skb, hdr);
607 break;
608
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300609 case A2MP_CHANGE_NOTIFY:
Andrei Emeltchenko329d81af2012-05-29 13:59:10 +0300610 err = a2mp_change_notify(mgr, skb, hdr);
611 break;
612
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300613 case A2MP_GETINFO_REQ:
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300614 err = a2mp_getinfo_req(mgr, skb, hdr);
615 break;
616
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300617 case A2MP_GETAMPASSOC_REQ:
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300618 err = a2mp_getampassoc_req(mgr, skb, hdr);
619 break;
620
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300621 case A2MP_CREATEPHYSLINK_REQ:
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300622 err = a2mp_createphyslink_req(mgr, skb, hdr);
623 break;
624
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300625 case A2MP_DISCONNPHYSLINK_REQ:
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300626 err = a2mp_discphyslink_req(mgr, skb, hdr);
627 break;
628
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300629 case A2MP_DISCOVER_RSP:
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300630 err = a2mp_discover_rsp(mgr, skb, hdr);
631 break;
632
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300633 case A2MP_GETINFO_RSP:
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300634 err = a2mp_getinfo_rsp(mgr, skb, hdr);
635 break;
636
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300637 case A2MP_GETAMPASSOC_RSP:
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300638 err = a2mp_getampassoc_rsp(mgr, skb, hdr);
639 break;
640
641 case A2MP_CHANGE_RSP:
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300642 case A2MP_CREATEPHYSLINK_RSP:
643 case A2MP_DISCONNPHYSLINK_RSP:
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300644 err = a2mp_cmd_rsp(mgr, skb, hdr);
645 break;
646
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300647 default:
648 BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
649 err = -EINVAL;
650 break;
651 }
652 }
653
654 if (err) {
655 struct a2mp_cmd_rej rej;
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300656
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300657 rej.reason = __constant_cpu_to_le16(0);
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300658 hdr = (void *) skb->data;
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300659
660 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
661
662 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
663 &rej);
664 }
665
666 /* Always free skb and return success error code to prevent
667 from sending L2CAP Disconnect over A2MP channel */
668 kfree_skb(skb);
669
670 amp_mgr_put(mgr);
671
672 return 0;
673}
674
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300675static void a2mp_chan_close_cb(struct l2cap_chan *chan)
676{
Jaganath Kanakkassery4af66c62012-07-13 18:17:55 +0530677 l2cap_chan_put(chan);
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300678}
679
680static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
681{
682 struct amp_mgr *mgr = chan->data;
683
684 if (!mgr)
685 return;
686
687 BT_DBG("chan %p state %s", chan, state_to_string(state));
688
689 chan->state = state;
690
691 switch (state) {
692 case BT_CLOSED:
693 if (mgr)
694 amp_mgr_put(mgr);
695 break;
696 }
697}
698
699static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
700 unsigned long len, int nb)
701{
702 return bt_skb_alloc(len, GFP_KERNEL);
703}
704
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300705static struct l2cap_ops a2mp_chan_ops = {
706 .name = "L2CAP A2MP channel",
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300707 .recv = a2mp_chan_recv_cb,
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300708 .close = a2mp_chan_close_cb,
709 .state_change = a2mp_chan_state_change_cb,
710 .alloc_skb = a2mp_chan_alloc_skb_cb,
711
712 /* Not implemented for A2MP */
Gustavo Padovan7e1af8a2012-05-29 13:19:26 -0300713 .new_connection = l2cap_chan_no_new_connection,
714 .teardown = l2cap_chan_no_teardown,
715 .ready = l2cap_chan_no_ready,
Gustavo Padovan2dc4e512012-10-12 19:35:24 +0800716 .defer = l2cap_chan_no_defer,
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300717};
718
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300719static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300720{
721 struct l2cap_chan *chan;
722 int err;
723
724 chan = l2cap_chan_create();
725 if (!chan)
726 return NULL;
727
728 BT_DBG("chan %p", chan);
729
Andrei Emeltchenko416fa752012-05-29 13:59:16 +0300730 chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300731 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
732
733 chan->ops = &a2mp_chan_ops;
734
735 l2cap_chan_set_defaults(chan);
736 chan->remote_max_tx = chan->max_tx;
737 chan->remote_tx_win = chan->tx_win;
738
739 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
740 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
741
742 skb_queue_head_init(&chan->tx_q);
743
744 chan->mode = L2CAP_MODE_ERTM;
745
746 err = l2cap_ertm_init(chan);
747 if (err < 0) {
748 l2cap_chan_del(chan, 0);
749 return NULL;
750 }
751
752 chan->conf_state = 0;
753
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300754 if (locked)
755 __l2cap_chan_add(conn, chan);
756 else
757 l2cap_chan_add(conn, chan);
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300758
759 chan->remote_mps = chan->omtu;
760 chan->mps = chan->omtu;
761
762 chan->state = BT_CONNECTED;
763
764 return chan;
765}
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300766
767/* AMP Manager functions */
Andrei Emeltchenkof706adf2012-10-18 13:16:19 +0300768struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300769{
Andrei Emeltchenkoa0dfe0a2012-07-11 14:43:35 +0300770 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300771
772 kref_get(&mgr->kref);
Andrei Emeltchenkof706adf2012-10-18 13:16:19 +0300773
774 return mgr;
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300775}
776
777static void amp_mgr_destroy(struct kref *kref)
778{
779 struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
780
781 BT_DBG("mgr %p", mgr);
782
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300783 mutex_lock(&amp_mgr_list_lock);
784 list_del(&mgr->list);
785 mutex_unlock(&amp_mgr_list_lock);
786
Andrei Emeltchenko52c0d6e2012-09-27 17:26:12 +0300787 amp_ctrl_list_flush(mgr);
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300788 kfree(mgr);
789}
790
791int amp_mgr_put(struct amp_mgr *mgr)
792{
Andrei Emeltchenkoa0dfe0a2012-07-11 14:43:35 +0300793 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300794
795 return kref_put(&mgr->kref, &amp_mgr_destroy);
796}
797
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300798static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300799{
800 struct amp_mgr *mgr;
801 struct l2cap_chan *chan;
802
803 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
804 if (!mgr)
805 return NULL;
806
807 BT_DBG("conn %p mgr %p", conn, mgr);
808
809 mgr->l2cap_conn = conn;
810
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300811 chan = a2mp_chan_open(conn, locked);
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300812 if (!chan) {
813 kfree(mgr);
814 return NULL;
815 }
816
817 mgr->a2mp_chan = chan;
818 chan->data = mgr;
819
820 conn->hcon->amp_mgr = mgr;
821
822 kref_init(&mgr->kref);
823
Andrei Emeltchenko52c0d6e2012-09-27 17:26:12 +0300824 /* Remote AMP ctrl list initialization */
825 INIT_LIST_HEAD(&mgr->amp_ctrls);
826 mutex_init(&mgr->amp_ctrls_lock);
827
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300828 mutex_lock(&amp_mgr_list_lock);
829 list_add(&mgr->list, &amp_mgr_list);
830 mutex_unlock(&amp_mgr_list_lock);
831
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300832 return mgr;
833}
Andrei Emeltchenko97e8e892012-05-29 13:59:17 +0300834
835struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
836 struct sk_buff *skb)
837{
838 struct amp_mgr *mgr;
839
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300840 mgr = amp_mgr_create(conn, false);
Andrei Emeltchenko97e8e892012-05-29 13:59:17 +0300841 if (!mgr) {
842 BT_ERR("Could not create AMP manager");
843 return NULL;
844 }
845
846 BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
847
848 return mgr->a2mp_chan;
849}
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300850
851struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
852{
853 struct amp_mgr *mgr;
854
855 mutex_lock(&amp_mgr_list_lock);
856 list_for_each_entry(mgr, &amp_mgr_list, list) {
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200857 if (test_and_clear_bit(state, &mgr->state)) {
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300858 amp_mgr_get(mgr);
859 mutex_unlock(&amp_mgr_list_lock);
860 return mgr;
861 }
862 }
863 mutex_unlock(&amp_mgr_list_lock);
864
865 return NULL;
866}
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +0300867
868void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
869{
870 struct amp_mgr *mgr;
871 struct a2mp_info_rsp rsp;
872
873 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
874 if (!mgr)
875 return;
876
877 BT_DBG("%s mgr %p", hdev->name, mgr);
878
879 rsp.id = hdev->id;
880 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
881
Marcel Holtmannece69122013-10-05 11:47:43 -0700882 if (hdev->amp_type != AMP_TYPE_BREDR) {
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +0300883 rsp.status = 0;
884 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
885 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
886 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
887 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
888 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
889 }
890
891 a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
892 amp_mgr_put(mgr);
893}
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300894
895void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
896{
897 struct amp_mgr *mgr;
898 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
899 struct a2mp_amp_assoc_rsp *rsp;
900 size_t len;
901
902 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
903 if (!mgr)
904 return;
905
906 BT_DBG("%s mgr %p", hdev->name, mgr);
907
908 len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
909 rsp = kzalloc(len, GFP_KERNEL);
910 if (!rsp) {
911 amp_mgr_put(mgr);
912 return;
913 }
914
915 rsp->id = hdev->id;
916
917 if (status) {
918 rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
919 } else {
920 rsp->status = A2MP_STATUS_SUCCESS;
921 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
922 }
923
924 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
925 amp_mgr_put(mgr);
926 kfree(rsp);
927}
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300928
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300929void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
930{
931 struct amp_mgr *mgr;
932 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
933 struct a2mp_physlink_req *req;
934 struct l2cap_chan *bredr_chan;
935 size_t len;
936
937 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
938 if (!mgr)
939 return;
940
941 len = sizeof(*req) + loc_assoc->len;
942
943 BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
944
945 req = kzalloc(len, GFP_KERNEL);
946 if (!req) {
947 amp_mgr_put(mgr);
948 return;
949 }
950
951 bredr_chan = mgr->bredr_chan;
952 if (!bredr_chan)
953 goto clean;
954
955 req->local_id = hdev->id;
Andrei Emeltchenkofffadc02012-11-01 15:37:03 +0200956 req->remote_id = bredr_chan->remote_amp_id;
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300957 memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
958
959 a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
960
961clean:
962 amp_mgr_put(mgr);
963 kfree(req);
964}
965
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200966void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
967{
968 struct amp_mgr *mgr;
969 struct a2mp_physlink_rsp rsp;
970 struct hci_conn *hs_hcon;
971
972 mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
973 if (!mgr)
974 return;
975
976 hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
977 if (!hs_hcon) {
978 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
979 } else {
980 rsp.remote_id = hs_hcon->remote_id;
981 rsp.status = A2MP_STATUS_SUCCESS;
982 }
983
984 BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
985 status);
986
987 rsp.local_id = hdev->id;
988 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
989 amp_mgr_put(mgr);
990}
991
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300992void a2mp_discover_amp(struct l2cap_chan *chan)
993{
994 struct l2cap_conn *conn = chan->conn;
995 struct amp_mgr *mgr = conn->hcon->amp_mgr;
996 struct a2mp_discov_req req;
997
998 BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
999
1000 if (!mgr) {
1001 mgr = amp_mgr_create(conn, true);
1002 if (!mgr)
1003 return;
1004 }
1005
1006 mgr->bredr_chan = chan;
1007
1008 req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1009 req.ext_feat = 0;
1010 a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
1011}