Commit | Line | Data |
---|---|---|
903e4541 AE |
1 | /* |
2 | Copyright (c) 2011,2012 Intel Corp. | |
3 | ||
4 | This program is free software; you can redistribute it and/or modify | |
5 | it under the terms of the GNU General Public License version 2 and | |
6 | only version 2 as published by the Free Software Foundation. | |
7 | ||
8 | This program is distributed in the hope that it will be useful, | |
9 | but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | GNU General Public License for more details. | |
12 | */ | |
13 | ||
14 | #include <net/bluetooth/bluetooth.h> | |
15 | #include <net/bluetooth/hci.h> | |
16 | #include <net/bluetooth/hci_core.h> | |
ba221bba | 17 | #include <crypto/hash.h> |
903e4541 | 18 | |
b3d39140 | 19 | #include "hci_request.h" |
7024728e | 20 | #include "a2mp.h" |
7ef9fbf0 MH |
21 | #include "amp.h" |
22 | ||
52c0d6e5 | 23 | /* Remote AMP Controllers interface */ |
0b26ab9d | 24 | void amp_ctrl_get(struct amp_ctrl *ctrl) |
52c0d6e5 AE |
25 | { |
26 | BT_DBG("ctrl %p orig refcnt %d", ctrl, | |
27 | atomic_read(&ctrl->kref.refcount)); | |
28 | ||
29 | kref_get(&ctrl->kref); | |
30 | } | |
31 | ||
32 | static void amp_ctrl_destroy(struct kref *kref) | |
33 | { | |
34 | struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref); | |
35 | ||
36 | BT_DBG("ctrl %p", ctrl); | |
37 | ||
38 | kfree(ctrl->assoc); | |
39 | kfree(ctrl); | |
40 | } | |
41 | ||
42 | int amp_ctrl_put(struct amp_ctrl *ctrl) | |
43 | { | |
44 | BT_DBG("ctrl %p orig refcnt %d", ctrl, | |
45 | atomic_read(&ctrl->kref.refcount)); | |
46 | ||
47 | return kref_put(&ctrl->kref, &_ctrl_destroy); | |
48 | } | |
49 | ||
fa4ebc66 | 50 | struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id) |
52c0d6e5 AE |
51 | { |
52 | struct amp_ctrl *ctrl; | |
53 | ||
54 | ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL); | |
55 | if (!ctrl) | |
56 | return NULL; | |
57 | ||
fa4ebc66 AE |
58 | kref_init(&ctrl->kref); |
59 | ctrl->id = id; | |
60 | ||
52c0d6e5 AE |
61 | mutex_lock(&mgr->amp_ctrls_lock); |
62 | list_add(&ctrl->list, &mgr->amp_ctrls); | |
63 | mutex_unlock(&mgr->amp_ctrls_lock); | |
64 | ||
52c0d6e5 AE |
65 | BT_DBG("mgr %p ctrl %p", mgr, ctrl); |
66 | ||
67 | return ctrl; | |
68 | } | |
69 | ||
70 | void amp_ctrl_list_flush(struct amp_mgr *mgr) | |
71 | { | |
72 | struct amp_ctrl *ctrl, *n; | |
73 | ||
74 | BT_DBG("mgr %p", mgr); | |
75 | ||
76 | mutex_lock(&mgr->amp_ctrls_lock); | |
77 | list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) { | |
78 | list_del(&ctrl->list); | |
79 | amp_ctrl_put(ctrl); | |
80 | } | |
81 | mutex_unlock(&mgr->amp_ctrls_lock); | |
82 | } | |
83 | ||
84 | struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id) | |
85 | { | |
86 | struct amp_ctrl *ctrl; | |
87 | ||
88 | BT_DBG("mgr %p id %d", mgr, id); | |
89 | ||
90 | mutex_lock(&mgr->amp_ctrls_lock); | |
91 | list_for_each_entry(ctrl, &mgr->amp_ctrls, list) { | |
92 | if (ctrl->id == id) { | |
93 | amp_ctrl_get(ctrl); | |
94 | mutex_unlock(&mgr->amp_ctrls_lock); | |
95 | return ctrl; | |
96 | } | |
97 | } | |
98 | mutex_unlock(&mgr->amp_ctrls_lock); | |
99 | ||
100 | return NULL; | |
101 | } | |
102 | ||
3161ae1c AE |
103 | /* Physical Link interface */ |
104 | static u8 __next_handle(struct amp_mgr *mgr) | |
105 | { | |
106 | if (++mgr->handle == 0) | |
107 | mgr->handle = 1; | |
108 | ||
109 | return mgr->handle; | |
110 | } | |
111 | ||
112 | struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr, | |
a0c234fe | 113 | u8 remote_id, bool out) |
3161ae1c | 114 | { |
bdc8ead2 | 115 | bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst; |
3161ae1c | 116 | struct hci_conn *hcon; |
a5c4e309 | 117 | u8 role = out ? HCI_ROLE_MASTER : HCI_ROLE_SLAVE; |
3161ae1c | 118 | |
a5c4e309 | 119 | hcon = hci_conn_add(hdev, AMP_LINK, dst, role); |
3161ae1c AE |
120 | if (!hcon) |
121 | return NULL; | |
122 | ||
a0c234fe AE |
123 | BT_DBG("hcon %p dst %pMR", hcon, dst); |
124 | ||
3161ae1c | 125 | hcon->state = BT_CONNECT; |
3161ae1c AE |
126 | hcon->attempt++; |
127 | hcon->handle = __next_handle(mgr); | |
128 | hcon->remote_id = remote_id; | |
f706adfe | 129 | hcon->amp_mgr = amp_mgr_get(mgr); |
3161ae1c AE |
130 | |
131 | return hcon; | |
132 | } | |
133 | ||
ba221bba DK |
134 | /* AMP crypto key generation interface */ |
135 | static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output) | |
136 | { | |
ba221bba | 137 | struct crypto_shash *tfm; |
252670c4 | 138 | struct shash_desc *shash; |
0a961a44 | 139 | int ret; |
ba221bba DK |
140 | |
141 | if (!ksize) | |
142 | return -EINVAL; | |
143 | ||
144 | tfm = crypto_alloc_shash("hmac(sha256)", 0, 0); | |
145 | if (IS_ERR(tfm)) { | |
146 | BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm)); | |
147 | return PTR_ERR(tfm); | |
148 | } | |
149 | ||
150 | ret = crypto_shash_setkey(tfm, key, ksize); | |
151 | if (ret) { | |
152 | BT_DBG("crypto_ahash_setkey failed: err %d", ret); | |
252670c4 JH |
153 | goto failed; |
154 | } | |
ba221bba | 155 | |
252670c4 JH |
156 | shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm), |
157 | GFP_KERNEL); | |
158 | if (!shash) { | |
159 | ret = -ENOMEM; | |
160 | goto failed; | |
ba221bba DK |
161 | } |
162 | ||
252670c4 JH |
163 | shash->tfm = tfm; |
164 | shash->flags = CRYPTO_TFM_REQ_MAY_SLEEP; | |
165 | ||
166 | ret = crypto_shash_digest(shash, plaintext, psize, output); | |
167 | ||
168 | kfree(shash); | |
169 | ||
170 | failed: | |
ba221bba DK |
171 | crypto_free_shash(tfm); |
172 | return ret; | |
173 | } | |
174 | ||
5a349186 AE |
175 | int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type) |
176 | { | |
177 | struct hci_dev *hdev = conn->hdev; | |
178 | struct link_key *key; | |
179 | u8 keybuf[HCI_AMP_LINK_KEY_SIZE]; | |
180 | u8 gamp_key[HCI_AMP_LINK_KEY_SIZE]; | |
181 | int err; | |
182 | ||
183 | if (!hci_conn_check_link_mode(conn)) | |
184 | return -EACCES; | |
185 | ||
186 | BT_DBG("conn %p key_type %d", conn, conn->key_type); | |
187 | ||
188 | /* Legacy key */ | |
189 | if (conn->key_type < 3) { | |
190 | BT_ERR("Legacy key type %d", conn->key_type); | |
191 | return -EACCES; | |
192 | } | |
193 | ||
194 | *type = conn->key_type; | |
195 | *len = HCI_AMP_LINK_KEY_SIZE; | |
196 | ||
197 | key = hci_find_link_key(hdev, &conn->dst); | |
079db0c6 AE |
198 | if (!key) { |
199 | BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst); | |
200 | return -EACCES; | |
201 | } | |
5a349186 AE |
202 | |
203 | /* BR/EDR Link Key concatenated together with itself */ | |
204 | memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE); | |
205 | memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE); | |
206 | ||
207 | /* Derive Generic AMP Link Key (gamp) */ | |
208 | err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key); | |
209 | if (err) { | |
210 | BT_ERR("Could not derive Generic AMP Key: err %d", err); | |
211 | return err; | |
212 | } | |
213 | ||
214 | if (conn->key_type == HCI_LK_DEBUG_COMBINATION) { | |
215 | BT_DBG("Use Generic AMP Key (gamp)"); | |
216 | memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE); | |
217 | return err; | |
218 | } | |
219 | ||
220 | /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */ | |
221 | return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data); | |
222 | } | |
223 | ||
b3d39140 AW |
224 | static void read_local_amp_assoc_complete(struct hci_dev *hdev, u8 status, |
225 | u16 opcode, struct sk_buff *skb) | |
226 | { | |
227 | struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data; | |
228 | struct amp_assoc *assoc = &hdev->loc_assoc; | |
229 | size_t rem_len, frag_len; | |
230 | ||
231 | BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); | |
232 | ||
233 | if (rp->status) | |
234 | goto send_rsp; | |
235 | ||
236 | frag_len = skb->len - sizeof(*rp); | |
237 | rem_len = __le16_to_cpu(rp->rem_len); | |
238 | ||
239 | if (rem_len > frag_len) { | |
240 | BT_DBG("frag_len %zu rem_len %zu", frag_len, rem_len); | |
241 | ||
242 | memcpy(assoc->data + assoc->offset, rp->frag, frag_len); | |
243 | assoc->offset += frag_len; | |
244 | ||
245 | /* Read other fragments */ | |
246 | amp_read_loc_assoc_frag(hdev, rp->phy_handle); | |
247 | ||
248 | return; | |
249 | } | |
250 | ||
251 | memcpy(assoc->data + assoc->offset, rp->frag, rem_len); | |
252 | assoc->len = assoc->offset + rem_len; | |
253 | assoc->offset = 0; | |
254 | ||
255 | send_rsp: | |
256 | /* Send A2MP Rsp when all fragments are received */ | |
257 | a2mp_send_getampassoc_rsp(hdev, rp->status); | |
258 | a2mp_send_create_phy_link_req(hdev, rp->status); | |
259 | } | |
260 | ||
903e4541 AE |
261 | void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle) |
262 | { | |
263 | struct hci_cp_read_local_amp_assoc cp; | |
264 | struct amp_assoc *loc_assoc = &hdev->loc_assoc; | |
b3d39140 AW |
265 | struct hci_request req; |
266 | int err = 0; | |
903e4541 AE |
267 | |
268 | BT_DBG("%s handle %d", hdev->name, phy_handle); | |
269 | ||
270 | cp.phy_handle = phy_handle; | |
271 | cp.max_len = cpu_to_le16(hdev->amp_assoc_size); | |
272 | cp.len_so_far = cpu_to_le16(loc_assoc->offset); | |
273 | ||
b3d39140 AW |
274 | hci_req_init(&req, hdev); |
275 | hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); | |
276 | err = hci_req_run_skb(&req, read_local_amp_assoc_complete); | |
277 | if (err < 0) | |
278 | a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID); | |
903e4541 AE |
279 | } |
280 | ||
281 | void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr) | |
282 | { | |
283 | struct hci_cp_read_local_amp_assoc cp; | |
b3d39140 AW |
284 | struct hci_request req; |
285 | int err = 0; | |
903e4541 AE |
286 | |
287 | memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc)); | |
288 | memset(&cp, 0, sizeof(cp)); | |
289 | ||
290 | cp.max_len = cpu_to_le16(hdev->amp_assoc_size); | |
291 | ||
cb6801c6 | 292 | set_bit(READ_LOC_AMP_ASSOC, &mgr->state); |
b3d39140 AW |
293 | hci_req_init(&req, hdev); |
294 | hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); | |
295 | hci_req_run_skb(&req, read_local_amp_assoc_complete); | |
296 | if (err < 0) | |
297 | a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID); | |
903e4541 | 298 | } |
a02226d6 | 299 | |
9495b2ee AE |
300 | void amp_read_loc_assoc_final_data(struct hci_dev *hdev, |
301 | struct hci_conn *hcon) | |
302 | { | |
303 | struct hci_cp_read_local_amp_assoc cp; | |
304 | struct amp_mgr *mgr = hcon->amp_mgr; | |
b3d39140 AW |
305 | struct hci_request req; |
306 | int err = 0; | |
9495b2ee AE |
307 | |
308 | cp.phy_handle = hcon->handle; | |
309 | cp.len_so_far = cpu_to_le16(0); | |
310 | cp.max_len = cpu_to_le16(hdev->amp_assoc_size); | |
311 | ||
cb6801c6 | 312 | set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state); |
9495b2ee AE |
313 | |
314 | /* Read Local AMP Assoc final link information data */ | |
b3d39140 AW |
315 | hci_req_init(&req, hdev); |
316 | hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); | |
317 | hci_req_run_skb(&req, read_local_amp_assoc_complete); | |
318 | if (err < 0) | |
319 | a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID); | |
320 | } | |
321 | ||
322 | static void write_remote_amp_assoc_complete(struct hci_dev *hdev, u8 status, | |
323 | u16 opcode, struct sk_buff *skb) | |
324 | { | |
325 | struct hci_rp_write_remote_amp_assoc *rp = (void *)skb->data; | |
326 | ||
327 | BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x", | |
328 | hdev->name, rp->status, rp->phy_handle); | |
329 | ||
330 | if (rp->status) | |
331 | return; | |
332 | ||
333 | amp_write_rem_assoc_continue(hdev, rp->phy_handle); | |
9495b2ee | 334 | } |
93c284ee AE |
335 | |
336 | /* Write AMP Assoc data fragments, returns true with last fragment written*/ | |
337 | static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, | |
338 | struct hci_conn *hcon) | |
339 | { | |
340 | struct hci_cp_write_remote_amp_assoc *cp; | |
341 | struct amp_mgr *mgr = hcon->amp_mgr; | |
342 | struct amp_ctrl *ctrl; | |
b3d39140 | 343 | struct hci_request req; |
93c284ee AE |
344 | u16 frag_len, len; |
345 | ||
346 | ctrl = amp_ctrl_lookup(mgr, hcon->remote_id); | |
347 | if (!ctrl) | |
348 | return false; | |
349 | ||
350 | if (!ctrl->assoc_rem_len) { | |
351 | BT_DBG("all fragments are written"); | |
352 | ctrl->assoc_rem_len = ctrl->assoc_len; | |
353 | ctrl->assoc_len_so_far = 0; | |
354 | ||
355 | amp_ctrl_put(ctrl); | |
356 | return true; | |
357 | } | |
358 | ||
359 | frag_len = min_t(u16, 248, ctrl->assoc_rem_len); | |
360 | len = frag_len + sizeof(*cp); | |
361 | ||
362 | cp = kzalloc(len, GFP_KERNEL); | |
363 | if (!cp) { | |
364 | amp_ctrl_put(ctrl); | |
365 | return false; | |
366 | } | |
367 | ||
368 | BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u", | |
369 | hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len); | |
370 | ||
371 | cp->phy_handle = hcon->handle; | |
372 | cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far); | |
373 | cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len); | |
374 | memcpy(cp->frag, ctrl->assoc, frag_len); | |
375 | ||
376 | ctrl->assoc_len_so_far += frag_len; | |
377 | ctrl->assoc_rem_len -= frag_len; | |
378 | ||
379 | amp_ctrl_put(ctrl); | |
380 | ||
b3d39140 | 381 | hci_req_init(&req, hdev); |
0208bc88 | 382 | hci_req_add(&req, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp); |
b3d39140 | 383 | hci_req_run_skb(&req, write_remote_amp_assoc_complete); |
93c284ee AE |
384 | |
385 | kfree(cp); | |
386 | ||
387 | return false; | |
388 | } | |
389 | ||
390 | void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle) | |
391 | { | |
392 | struct hci_conn *hcon; | |
393 | ||
394 | BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle); | |
395 | ||
396 | hcon = hci_conn_hash_lookup_handle(hdev, handle); | |
397 | if (!hcon) | |
398 | return; | |
399 | ||
8e05e3ba AE |
400 | /* Send A2MP create phylink rsp when all fragments are written */ |
401 | if (amp_write_rem_assoc_frag(hdev, hcon)) | |
402 | a2mp_send_create_phy_link_rsp(hdev, 0); | |
93c284ee AE |
403 | } |
404 | ||
405 | void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle) | |
406 | { | |
407 | struct hci_conn *hcon; | |
408 | ||
409 | BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle); | |
410 | ||
411 | hcon = hci_conn_hash_lookup_handle(hdev, handle); | |
412 | if (!hcon) | |
413 | return; | |
414 | ||
415 | BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon); | |
416 | ||
417 | amp_write_rem_assoc_frag(hdev, hcon); | |
418 | } | |
419 | ||
df9b89c7 AW |
420 | static void create_phylink_complete(struct hci_dev *hdev, u8 status, |
421 | u16 opcode) | |
422 | { | |
423 | struct hci_cp_create_phy_link *cp; | |
424 | ||
425 | BT_DBG("%s status 0x%2.2x", hdev->name, status); | |
426 | ||
427 | cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK); | |
428 | if (!cp) | |
429 | return; | |
430 | ||
431 | hci_dev_lock(hdev); | |
432 | ||
433 | if (status) { | |
434 | struct hci_conn *hcon; | |
435 | ||
436 | hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle); | |
437 | if (hcon) | |
438 | hci_conn_del(hcon); | |
439 | } else { | |
440 | amp_write_remote_assoc(hdev, cp->phy_handle); | |
441 | } | |
442 | ||
443 | hci_dev_unlock(hdev); | |
444 | } | |
445 | ||
a02226d6 AE |
446 | void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, |
447 | struct hci_conn *hcon) | |
448 | { | |
449 | struct hci_cp_create_phy_link cp; | |
df9b89c7 | 450 | struct hci_request req; |
a02226d6 AE |
451 | |
452 | cp.phy_handle = hcon->handle; | |
453 | ||
454 | BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon, | |
455 | hcon->handle); | |
456 | ||
457 | if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len, | |
458 | &cp.key_type)) { | |
459 | BT_DBG("Cannot create link key"); | |
460 | return; | |
461 | } | |
462 | ||
df9b89c7 AW |
463 | hci_req_init(&req, hdev); |
464 | hci_req_add(&req, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp); | |
465 | hci_req_run(&req, create_phylink_complete); | |
466 | } | |
467 | ||
468 | static void accept_phylink_complete(struct hci_dev *hdev, u8 status, | |
469 | u16 opcode) | |
470 | { | |
471 | struct hci_cp_accept_phy_link *cp; | |
472 | ||
473 | BT_DBG("%s status 0x%2.2x", hdev->name, status); | |
474 | ||
475 | if (status) | |
476 | return; | |
477 | ||
478 | cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK); | |
479 | if (!cp) | |
480 | return; | |
481 | ||
482 | amp_write_remote_assoc(hdev, cp->phy_handle); | |
a02226d6 | 483 | } |
dffa3871 AE |
484 | |
485 | void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, | |
486 | struct hci_conn *hcon) | |
487 | { | |
488 | struct hci_cp_accept_phy_link cp; | |
df9b89c7 | 489 | struct hci_request req; |
dffa3871 AE |
490 | |
491 | cp.phy_handle = hcon->handle; | |
492 | ||
493 | BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon, | |
494 | hcon->handle); | |
495 | ||
496 | if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len, | |
497 | &cp.key_type)) { | |
498 | BT_DBG("Cannot create link key"); | |
499 | return; | |
500 | } | |
501 | ||
df9b89c7 AW |
502 | hci_req_init(&req, hdev); |
503 | hci_req_add(&req, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp); | |
504 | hci_req_run(&req, accept_phylink_complete); | |
dffa3871 | 505 | } |
5ce66b59 | 506 | |
cf70ff22 AE |
507 | void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon) |
508 | { | |
509 | struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev); | |
510 | struct amp_mgr *mgr = hs_hcon->amp_mgr; | |
511 | struct l2cap_chan *bredr_chan; | |
512 | ||
513 | BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr); | |
514 | ||
515 | if (!bredr_hdev || !mgr || !mgr->bredr_chan) | |
516 | return; | |
517 | ||
518 | bredr_chan = mgr->bredr_chan; | |
519 | ||
a514b17f AE |
520 | l2cap_chan_lock(bredr_chan); |
521 | ||
cf70ff22 | 522 | set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags); |
fffadc08 | 523 | bredr_chan->remote_amp_id = hs_hcon->remote_id; |
37295996 | 524 | bredr_chan->local_amp_id = hs_hcon->hdev->id; |
cf70ff22 AE |
525 | bredr_chan->hs_hcon = hs_hcon; |
526 | bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu; | |
cf70ff22 | 527 | |
a514b17f AE |
528 | __l2cap_physical_cfm(bredr_chan, 0); |
529 | ||
530 | l2cap_chan_unlock(bredr_chan); | |
cf70ff22 AE |
531 | |
532 | hci_dev_put(bredr_hdev); | |
533 | } | |
534 | ||
5ce66b59 AE |
535 | void amp_create_logical_link(struct l2cap_chan *chan) |
536 | { | |
7a9898c6 | 537 | struct hci_conn *hs_hcon = chan->hs_hcon; |
5ce66b59 | 538 | struct hci_cp_create_accept_logical_link cp; |
5ce66b59 AE |
539 | struct hci_dev *hdev; |
540 | ||
bdc8ead2 MH |
541 | BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon, |
542 | &chan->conn->hcon->dst); | |
5ce66b59 | 543 | |
7a9898c6 | 544 | if (!hs_hcon) |
5ce66b59 AE |
545 | return; |
546 | ||
547 | hdev = hci_dev_hold(chan->hs_hcon->hdev); | |
548 | if (!hdev) | |
549 | return; | |
550 | ||
7a9898c6 | 551 | cp.phy_handle = hs_hcon->handle; |
5ce66b59 AE |
552 | |
553 | cp.tx_flow_spec.id = chan->local_id; | |
554 | cp.tx_flow_spec.stype = chan->local_stype; | |
555 | cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu); | |
556 | cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime); | |
557 | cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat); | |
558 | cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to); | |
559 | ||
560 | cp.rx_flow_spec.id = chan->remote_id; | |
561 | cp.rx_flow_spec.stype = chan->remote_stype; | |
562 | cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu); | |
563 | cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime); | |
564 | cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat); | |
565 | cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to); | |
566 | ||
7a9898c6 | 567 | if (hs_hcon->out) |
5ce66b59 AE |
568 | hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp), |
569 | &cp); | |
570 | else | |
571 | hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp), | |
572 | &cp); | |
573 | ||
5ce66b59 AE |
574 | hci_dev_put(hdev); |
575 | } | |
606e2a10 | 576 | |
419e08c1 AE |
577 | void amp_disconnect_logical_link(struct hci_chan *hchan) |
578 | { | |
579 | struct hci_conn *hcon = hchan->conn; | |
580 | struct hci_cp_disconn_logical_link cp; | |
581 | ||
582 | if (hcon->state != BT_CONNECTED) { | |
583 | BT_DBG("hchan %p not connected", hchan); | |
584 | return; | |
585 | } | |
586 | ||
587 | cp.log_handle = cpu_to_le16(hchan->handle); | |
588 | hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp); | |
589 | } | |
590 | ||
606e2a10 AE |
591 | void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason) |
592 | { | |
593 | BT_DBG("hchan %p", hchan); | |
594 | ||
595 | hci_chan_del(hchan); | |
596 | } |