blob: 17f33a62f6db559824d9dc1b3973acb779fa9162 [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
Alexandru Gheorghiu5ae327f2013-03-17 07:16:50 +0200400 assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300401 if (!assoc) {
402 amp_ctrl_put(ctrl);
403 return -ENOMEM;
404 }
405
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300406 ctrl->assoc = assoc;
407 ctrl->assoc_len = assoc_len;
408 ctrl->assoc_rem_len = assoc_len;
409 ctrl->assoc_len_so_far = 0;
410
411 amp_ctrl_put(ctrl);
412 }
413
414 /* Create Phys Link */
415 hdev = hci_dev_get(rsp->id);
416 if (!hdev)
417 return -EINVAL;
418
Andrei Emeltchenkoa0c234f2012-10-05 16:56:56 +0300419 hcon = phylink_add(hdev, mgr, rsp->id, true);
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300420 if (!hcon)
421 goto done;
422
423 BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
424
Andrei Emeltchenkofffadc02012-11-01 15:37:03 +0200425 mgr->bredr_chan->remote_amp_id = rsp->id;
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300426
Andrei Emeltchenkoa02226d2012-09-27 17:26:19 +0300427 amp_create_phylink(hdev, mgr, hcon);
428
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300429done:
430 hci_dev_put(hdev);
431 skb_pull(skb, len);
432 return 0;
433}
434
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300435static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
436 struct a2mp_cmd *hdr)
437{
438 struct a2mp_physlink_req *req = (void *) skb->data;
439
440 struct a2mp_physlink_rsp rsp;
441 struct hci_dev *hdev;
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300442 struct hci_conn *hcon;
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300443 struct amp_ctrl *ctrl;
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300444
445 if (le16_to_cpu(hdr->len) < sizeof(*req))
446 return -EINVAL;
447
448 BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
449
450 rsp.local_id = req->remote_id;
451 rsp.remote_id = req->local_id;
452
453 hdev = hci_dev_get(req->remote_id);
454 if (!hdev || hdev->amp_type != HCI_AMP) {
455 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
456 goto send_rsp;
457 }
458
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300459 ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
460 if (!ctrl) {
Andrei Emeltchenkofa4ebc62012-10-05 16:56:55 +0300461 ctrl = amp_ctrl_add(mgr, rsp.remote_id);
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300462 if (ctrl) {
463 amp_ctrl_get(ctrl);
464 } else {
465 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
466 goto send_rsp;
467 }
468 }
469
470 if (ctrl) {
Andrei Emeltchenko13465c02012-09-28 16:55:00 +0300471 size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
472 u8 *assoc;
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300473
Alexandru Gheorghiu5ae327f2013-03-17 07:16:50 +0200474 assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300475 if (!assoc) {
476 amp_ctrl_put(ctrl);
477 return -ENOMEM;
478 }
479
Andrei Emeltchenko0b26ab92012-09-27 17:26:24 +0300480 ctrl->assoc = assoc;
481 ctrl->assoc_len = assoc_len;
482 ctrl->assoc_rem_len = assoc_len;
483 ctrl->assoc_len_so_far = 0;
484
485 amp_ctrl_put(ctrl);
486 }
487
Andrei Emeltchenkoa0c234f2012-10-05 16:56:56 +0300488 hcon = phylink_add(hdev, mgr, req->local_id, false);
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300489 if (hcon) {
Andrei Emeltchenkodffa3872012-09-27 17:26:23 +0300490 amp_accept_phylink(hdev, mgr, hcon);
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300491 rsp.status = A2MP_STATUS_SUCCESS;
492 } else {
493 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
494 }
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300495
496send_rsp:
497 if (hdev)
498 hci_dev_put(hdev);
499
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200500 /* Reply error now and success after HCI Write Remote AMP Assoc
501 command complete with success status
502 */
503 if (rsp.status != A2MP_STATUS_SUCCESS) {
504 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
505 sizeof(rsp), &rsp);
506 } else {
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200507 set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200508 mgr->ident = hdr->ident;
509 }
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300510
511 skb_pull(skb, le16_to_cpu(hdr->len));
512 return 0;
513}
514
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300515static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
516 struct a2mp_cmd *hdr)
517{
518 struct a2mp_physlink_req *req = (void *) skb->data;
519 struct a2mp_physlink_rsp rsp;
520 struct hci_dev *hdev;
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300521 struct hci_conn *hcon;
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300522
523 if (le16_to_cpu(hdr->len) < sizeof(*req))
524 return -EINVAL;
525
526 BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
527
528 rsp.local_id = req->remote_id;
529 rsp.remote_id = req->local_id;
530 rsp.status = A2MP_STATUS_SUCCESS;
531
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300532 hdev = hci_dev_get(req->remote_id);
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300533 if (!hdev) {
534 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
535 goto send_rsp;
536 }
537
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300538 hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
539 if (!hcon) {
540 BT_ERR("No phys link exist");
541 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
542 goto clean;
543 }
544
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300545 /* TODO Disconnect Phys Link here */
546
Andrei Emeltchenkocb8488c2012-09-27 17:26:13 +0300547clean:
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300548 hci_dev_put(hdev);
549
550send_rsp:
551 a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
552
553 skb_pull(skb, sizeof(*req));
554 return 0;
555}
556
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300557static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
558 struct a2mp_cmd *hdr)
559{
Andrei Emeltchenko8e8c7e32012-07-19 17:03:41 +0300560 BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300561
562 skb_pull(skb, le16_to_cpu(hdr->len));
563 return 0;
564}
565
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300566/* Handle A2MP signalling */
567static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
568{
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300569 struct a2mp_cmd *hdr;
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300570 struct amp_mgr *mgr = chan->data;
571 int err = 0;
572
573 amp_mgr_get(mgr);
574
575 while (skb->len >= sizeof(*hdr)) {
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300576 u16 len;
577
578 hdr = (void *) skb->data;
579 len = le16_to_cpu(hdr->len);
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300580
Andrei Emeltchenko8e8c7e32012-07-19 17:03:41 +0300581 BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300582
583 skb_pull(skb, sizeof(*hdr));
584
585 if (len > skb->len || !hdr->ident) {
586 err = -EINVAL;
587 break;
588 }
589
590 mgr->ident = hdr->ident;
591
592 switch (hdr->code) {
593 case A2MP_COMMAND_REJ:
Andrei Emeltchenko21dbd2c2012-05-29 13:59:08 +0300594 a2mp_command_rej(mgr, skb, hdr);
595 break;
596
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300597 case A2MP_DISCOVER_REQ:
Andrei Emeltchenko8598d062012-05-29 13:59:09 +0300598 err = a2mp_discover_req(mgr, skb, hdr);
599 break;
600
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300601 case A2MP_CHANGE_NOTIFY:
Andrei Emeltchenko329d81af2012-05-29 13:59:10 +0300602 err = a2mp_change_notify(mgr, skb, hdr);
603 break;
604
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300605 case A2MP_GETINFO_REQ:
Andrei Emeltchenko47f2d972012-05-29 13:59:11 +0300606 err = a2mp_getinfo_req(mgr, skb, hdr);
607 break;
608
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300609 case A2MP_GETAMPASSOC_REQ:
Andrei Emeltchenkoa28381dc2012-05-29 13:59:12 +0300610 err = a2mp_getampassoc_req(mgr, skb, hdr);
611 break;
612
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300613 case A2MP_CREATEPHYSLINK_REQ:
Andrei Emeltchenkoe072f5d2012-05-29 13:59:13 +0300614 err = a2mp_createphyslink_req(mgr, skb, hdr);
615 break;
616
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300617 case A2MP_DISCONNPHYSLINK_REQ:
Andrei Emeltchenko6113f842012-05-29 13:59:14 +0300618 err = a2mp_discphyslink_req(mgr, skb, hdr);
619 break;
620
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300621 case A2MP_DISCOVER_RSP:
Andrei Emeltchenkoaa095372012-09-27 17:26:10 +0300622 err = a2mp_discover_rsp(mgr, skb, hdr);
623 break;
624
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300625 case A2MP_GETINFO_RSP:
Andrei Emeltchenko0d868de2012-09-27 17:26:14 +0300626 err = a2mp_getinfo_rsp(mgr, skb, hdr);
627 break;
628
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300629 case A2MP_GETAMPASSOC_RSP:
Andrei Emeltchenko9a5e94d2012-09-27 17:26:15 +0300630 err = a2mp_getampassoc_rsp(mgr, skb, hdr);
631 break;
632
633 case A2MP_CHANGE_RSP:
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300634 case A2MP_CREATEPHYSLINK_RSP:
635 case A2MP_DISCONNPHYSLINK_RSP:
Andrei Emeltchenkof6410a82012-05-29 13:59:15 +0300636 err = a2mp_cmd_rsp(mgr, skb, hdr);
637 break;
638
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300639 default:
640 BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
641 err = -EINVAL;
642 break;
643 }
644 }
645
646 if (err) {
647 struct a2mp_cmd_rej rej;
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300648
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300649 rej.reason = __constant_cpu_to_le16(0);
Andrei Emeltchenkod9fc1d52012-07-19 17:03:46 +0300650 hdr = (void *) skb->data;
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300651
652 BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
653
654 a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
655 &rej);
656 }
657
658 /* Always free skb and return success error code to prevent
659 from sending L2CAP Disconnect over A2MP channel */
660 kfree_skb(skb);
661
662 amp_mgr_put(mgr);
663
664 return 0;
665}
666
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300667static void a2mp_chan_close_cb(struct l2cap_chan *chan)
668{
Jaganath Kanakkassery4af66c62012-07-13 18:17:55 +0530669 l2cap_chan_put(chan);
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300670}
671
672static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
673{
674 struct amp_mgr *mgr = chan->data;
675
676 if (!mgr)
677 return;
678
679 BT_DBG("chan %p state %s", chan, state_to_string(state));
680
681 chan->state = state;
682
683 switch (state) {
684 case BT_CLOSED:
685 if (mgr)
686 amp_mgr_put(mgr);
687 break;
688 }
689}
690
691static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
692 unsigned long len, int nb)
693{
694 return bt_skb_alloc(len, GFP_KERNEL);
695}
696
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300697static struct l2cap_ops a2mp_chan_ops = {
698 .name = "L2CAP A2MP channel",
Andrei Emeltchenko6b44d9b2012-05-29 13:59:07 +0300699 .recv = a2mp_chan_recv_cb,
Andrei Emeltchenko46d5c902012-05-29 13:59:04 +0300700 .close = a2mp_chan_close_cb,
701 .state_change = a2mp_chan_state_change_cb,
702 .alloc_skb = a2mp_chan_alloc_skb_cb,
703
704 /* Not implemented for A2MP */
Gustavo Padovan7e1af8a2012-05-29 13:19:26 -0300705 .new_connection = l2cap_chan_no_new_connection,
706 .teardown = l2cap_chan_no_teardown,
707 .ready = l2cap_chan_no_ready,
Gustavo Padovan2dc4e512012-10-12 19:35:24 +0800708 .defer = l2cap_chan_no_defer,
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300709};
710
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300711static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300712{
713 struct l2cap_chan *chan;
714 int err;
715
716 chan = l2cap_chan_create();
717 if (!chan)
718 return NULL;
719
720 BT_DBG("chan %p", chan);
721
Andrei Emeltchenko416fa752012-05-29 13:59:16 +0300722 chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300723 chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
724
725 chan->ops = &a2mp_chan_ops;
726
727 l2cap_chan_set_defaults(chan);
728 chan->remote_max_tx = chan->max_tx;
729 chan->remote_tx_win = chan->tx_win;
730
731 chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
732 chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
733
734 skb_queue_head_init(&chan->tx_q);
735
736 chan->mode = L2CAP_MODE_ERTM;
737
738 err = l2cap_ertm_init(chan);
739 if (err < 0) {
740 l2cap_chan_del(chan, 0);
741 return NULL;
742 }
743
744 chan->conf_state = 0;
745
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300746 if (locked)
747 __l2cap_chan_add(conn, chan);
748 else
749 l2cap_chan_add(conn, chan);
Andrei Emeltchenko466f8002012-05-29 13:59:01 +0300750
751 chan->remote_mps = chan->omtu;
752 chan->mps = chan->omtu;
753
754 chan->state = BT_CONNECTED;
755
756 return chan;
757}
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300758
759/* AMP Manager functions */
Andrei Emeltchenkof706adf2012-10-18 13:16:19 +0300760struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300761{
Andrei Emeltchenkoa0dfe0a2012-07-11 14:43:35 +0300762 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300763
764 kref_get(&mgr->kref);
Andrei Emeltchenkof706adf2012-10-18 13:16:19 +0300765
766 return mgr;
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300767}
768
769static void amp_mgr_destroy(struct kref *kref)
770{
771 struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
772
773 BT_DBG("mgr %p", mgr);
774
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300775 mutex_lock(&amp_mgr_list_lock);
776 list_del(&mgr->list);
777 mutex_unlock(&amp_mgr_list_lock);
778
Andrei Emeltchenko52c0d6e2012-09-27 17:26:12 +0300779 amp_ctrl_list_flush(mgr);
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300780 kfree(mgr);
781}
782
783int amp_mgr_put(struct amp_mgr *mgr)
784{
Andrei Emeltchenkoa0dfe0a2012-07-11 14:43:35 +0300785 BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300786
787 return kref_put(&mgr->kref, &amp_mgr_destroy);
788}
789
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300790static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300791{
792 struct amp_mgr *mgr;
793 struct l2cap_chan *chan;
794
795 mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
796 if (!mgr)
797 return NULL;
798
799 BT_DBG("conn %p mgr %p", conn, mgr);
800
801 mgr->l2cap_conn = conn;
802
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300803 chan = a2mp_chan_open(conn, locked);
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300804 if (!chan) {
805 kfree(mgr);
806 return NULL;
807 }
808
809 mgr->a2mp_chan = chan;
810 chan->data = mgr;
811
812 conn->hcon->amp_mgr = mgr;
813
814 kref_init(&mgr->kref);
815
Andrei Emeltchenko52c0d6e2012-09-27 17:26:12 +0300816 /* Remote AMP ctrl list initialization */
817 INIT_LIST_HEAD(&mgr->amp_ctrls);
818 mutex_init(&mgr->amp_ctrls_lock);
819
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300820 mutex_lock(&amp_mgr_list_lock);
821 list_add(&mgr->list, &amp_mgr_list);
822 mutex_unlock(&amp_mgr_list_lock);
823
Andrei Emeltchenko9740e492012-05-29 13:59:02 +0300824 return mgr;
825}
Andrei Emeltchenko97e8e892012-05-29 13:59:17 +0300826
827struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
828 struct sk_buff *skb)
829{
830 struct amp_mgr *mgr;
831
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300832 mgr = amp_mgr_create(conn, false);
Andrei Emeltchenko97e8e892012-05-29 13:59:17 +0300833 if (!mgr) {
834 BT_ERR("Could not create AMP manager");
835 return NULL;
836 }
837
838 BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
839
840 return mgr->a2mp_chan;
841}
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300842
843struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
844{
845 struct amp_mgr *mgr;
846
847 mutex_lock(&amp_mgr_list_lock);
848 list_for_each_entry(mgr, &amp_mgr_list, list) {
Andrei Emeltchenkocb6801c62012-12-07 14:59:08 +0200849 if (test_and_clear_bit(state, &mgr->state)) {
Andrei Emeltchenkof97268f2012-09-27 17:26:07 +0300850 amp_mgr_get(mgr);
851 mutex_unlock(&amp_mgr_list_lock);
852 return mgr;
853 }
854 }
855 mutex_unlock(&amp_mgr_list_lock);
856
857 return NULL;
858}
Andrei Emeltchenko8e2a0d92012-09-27 17:26:08 +0300859
860void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
861{
862 struct amp_mgr *mgr;
863 struct a2mp_info_rsp rsp;
864
865 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
866 if (!mgr)
867 return;
868
869 BT_DBG("%s mgr %p", hdev->name, mgr);
870
871 rsp.id = hdev->id;
872 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
873
874 if (hdev->amp_type != HCI_BREDR) {
875 rsp.status = 0;
876 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
877 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
878 rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
879 rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
880 rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
881 }
882
883 a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
884 amp_mgr_put(mgr);
885}
Andrei Emeltchenko903e4542012-09-27 17:26:09 +0300886
887void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
888{
889 struct amp_mgr *mgr;
890 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
891 struct a2mp_amp_assoc_rsp *rsp;
892 size_t len;
893
894 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
895 if (!mgr)
896 return;
897
898 BT_DBG("%s mgr %p", hdev->name, mgr);
899
900 len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
901 rsp = kzalloc(len, GFP_KERNEL);
902 if (!rsp) {
903 amp_mgr_put(mgr);
904 return;
905 }
906
907 rsp->id = hdev->id;
908
909 if (status) {
910 rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
911 } else {
912 rsp->status = A2MP_STATUS_SUCCESS;
913 memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
914 }
915
916 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
917 amp_mgr_put(mgr);
918 kfree(rsp);
919}
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300920
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300921void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
922{
923 struct amp_mgr *mgr;
924 struct amp_assoc *loc_assoc = &hdev->loc_assoc;
925 struct a2mp_physlink_req *req;
926 struct l2cap_chan *bredr_chan;
927 size_t len;
928
929 mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
930 if (!mgr)
931 return;
932
933 len = sizeof(*req) + loc_assoc->len;
934
935 BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
936
937 req = kzalloc(len, GFP_KERNEL);
938 if (!req) {
939 amp_mgr_put(mgr);
940 return;
941 }
942
943 bredr_chan = mgr->bredr_chan;
944 if (!bredr_chan)
945 goto clean;
946
947 req->local_id = hdev->id;
Andrei Emeltchenkofffadc02012-11-01 15:37:03 +0200948 req->remote_id = bredr_chan->remote_amp_id;
Andrei Emeltchenko9495b2e2012-09-27 17:26:22 +0300949 memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
950
951 a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
952
953clean:
954 amp_mgr_put(mgr);
955 kfree(req);
956}
957
Andrei Emeltchenko8e05e3b2012-12-07 14:59:05 +0200958void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
959{
960 struct amp_mgr *mgr;
961 struct a2mp_physlink_rsp rsp;
962 struct hci_conn *hs_hcon;
963
964 mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
965 if (!mgr)
966 return;
967
968 hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
969 if (!hs_hcon) {
970 rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
971 } else {
972 rsp.remote_id = hs_hcon->remote_id;
973 rsp.status = A2MP_STATUS_SUCCESS;
974 }
975
976 BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
977 status);
978
979 rsp.local_id = hdev->id;
980 a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
981 amp_mgr_put(mgr);
982}
983
Andrei Emeltchenko93c3e8f2012-09-27 17:26:16 +0300984void a2mp_discover_amp(struct l2cap_chan *chan)
985{
986 struct l2cap_conn *conn = chan->conn;
987 struct amp_mgr *mgr = conn->hcon->amp_mgr;
988 struct a2mp_discov_req req;
989
990 BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
991
992 if (!mgr) {
993 mgr = amp_mgr_create(conn, true);
994 if (!mgr)
995 return;
996 }
997
998 mgr->bredr_chan = chan;
999
1000 req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1001 req.ext_feat = 0;
1002 a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
1003}