GNU Linux-libre 4.19.211-gnu1
[releases.git] / net / bluetooth / amp.c
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>
17 #include <crypto/hash.h>
18
19 #include "hci_request.h"
20 #include "a2mp.h"
21 #include "amp.h"
22
23 /* Remote AMP Controllers interface */
24 void amp_ctrl_get(struct amp_ctrl *ctrl)
25 {
26         BT_DBG("ctrl %p orig refcnt %d", ctrl,
27                kref_read(&ctrl->kref));
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                kref_read(&ctrl->kref));
46
47         return kref_put(&ctrl->kref, &amp_ctrl_destroy);
48 }
49
50 struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id)
51 {
52         struct amp_ctrl *ctrl;
53
54         ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
55         if (!ctrl)
56                 return NULL;
57
58         kref_init(&ctrl->kref);
59         ctrl->id = id;
60
61         mutex_lock(&mgr->amp_ctrls_lock);
62         list_add(&ctrl->list, &mgr->amp_ctrls);
63         mutex_unlock(&mgr->amp_ctrls_lock);
64
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
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,
113                              u8 remote_id, bool out)
114 {
115         bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst;
116         struct hci_conn *hcon;
117         u8 role = out ? HCI_ROLE_MASTER : HCI_ROLE_SLAVE;
118
119         hcon = hci_conn_add(hdev, AMP_LINK, dst, role);
120         if (!hcon)
121                 return NULL;
122
123         BT_DBG("hcon %p dst %pMR", hcon, dst);
124
125         hcon->state = BT_CONNECT;
126         hcon->attempt++;
127         hcon->handle = __next_handle(mgr);
128         hcon->remote_id = remote_id;
129         hcon->amp_mgr = amp_mgr_get(mgr);
130
131         return hcon;
132 }
133
134 /* AMP crypto key generation interface */
135 static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
136 {
137         struct crypto_shash *tfm;
138         struct shash_desc *shash;
139         int ret;
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);
153                 goto failed;
154         }
155
156         shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm),
157                         GFP_KERNEL);
158         if (!shash) {
159                 ret = -ENOMEM;
160                 goto failed;
161         }
162
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:
171         crypto_free_shash(tfm);
172         return ret;
173 }
174
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_dev_err(hdev, "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);
198         if (!key) {
199                 BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst);
200                 return -EACCES;
201         }
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_dev_err(hdev, "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
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
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;
265         struct hci_request req;
266         int err;
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
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);
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;
284         struct hci_request req;
285         int err;
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
292         set_bit(READ_LOC_AMP_ASSOC, &mgr->state);
293         hci_req_init(&req, hdev);
294         hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
295         err = 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);
298 }
299
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;
305         struct hci_request req;
306         int err;
307
308         if (!mgr)
309                 return;
310
311         cp.phy_handle = hcon->handle;
312         cp.len_so_far = cpu_to_le16(0);
313         cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
314
315         set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state);
316
317         /* Read Local AMP Assoc final link information data */
318         hci_req_init(&req, hdev);
319         hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
320         err = hci_req_run_skb(&req, read_local_amp_assoc_complete);
321         if (err < 0)
322                 a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
323 }
324
325 static void write_remote_amp_assoc_complete(struct hci_dev *hdev, u8 status,
326                                             u16 opcode, struct sk_buff *skb)
327 {
328         struct hci_rp_write_remote_amp_assoc *rp = (void *)skb->data;
329
330         BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
331                hdev->name, rp->status, rp->phy_handle);
332
333         if (rp->status)
334                 return;
335
336         amp_write_rem_assoc_continue(hdev, rp->phy_handle);
337 }
338
339 /* Write AMP Assoc data fragments, returns true with last fragment written*/
340 static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
341                                      struct hci_conn *hcon)
342 {
343         struct hci_cp_write_remote_amp_assoc *cp;
344         struct amp_mgr *mgr = hcon->amp_mgr;
345         struct amp_ctrl *ctrl;
346         struct hci_request req;
347         u16 frag_len, len;
348
349         ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
350         if (!ctrl)
351                 return false;
352
353         if (!ctrl->assoc_rem_len) {
354                 BT_DBG("all fragments are written");
355                 ctrl->assoc_rem_len = ctrl->assoc_len;
356                 ctrl->assoc_len_so_far = 0;
357
358                 amp_ctrl_put(ctrl);
359                 return true;
360         }
361
362         frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
363         len = frag_len + sizeof(*cp);
364
365         cp = kzalloc(len, GFP_KERNEL);
366         if (!cp) {
367                 amp_ctrl_put(ctrl);
368                 return false;
369         }
370
371         BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
372                hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
373
374         cp->phy_handle = hcon->handle;
375         cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
376         cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
377         memcpy(cp->frag, ctrl->assoc, frag_len);
378
379         ctrl->assoc_len_so_far += frag_len;
380         ctrl->assoc_rem_len -= frag_len;
381
382         amp_ctrl_put(ctrl);
383
384         hci_req_init(&req, hdev);
385         hci_req_add(&req, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
386         hci_req_run_skb(&req, write_remote_amp_assoc_complete);
387
388         kfree(cp);
389
390         return false;
391 }
392
393 void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
394 {
395         struct hci_conn *hcon;
396
397         BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
398
399         hcon = hci_conn_hash_lookup_handle(hdev, handle);
400         if (!hcon)
401                 return;
402
403         /* Send A2MP create phylink rsp when all fragments are written */
404         if (amp_write_rem_assoc_frag(hdev, hcon))
405                 a2mp_send_create_phy_link_rsp(hdev, 0);
406 }
407
408 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
409 {
410         struct hci_conn *hcon;
411
412         BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
413
414         hcon = hci_conn_hash_lookup_handle(hdev, handle);
415         if (!hcon)
416                 return;
417
418         BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
419
420         amp_write_rem_assoc_frag(hdev, hcon);
421 }
422
423 static void create_phylink_complete(struct hci_dev *hdev, u8 status,
424                                     u16 opcode)
425 {
426         struct hci_cp_create_phy_link *cp;
427
428         BT_DBG("%s status 0x%2.2x", hdev->name, status);
429
430         cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
431         if (!cp)
432                 return;
433
434         hci_dev_lock(hdev);
435
436         if (status) {
437                 struct hci_conn *hcon;
438
439                 hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle);
440                 if (hcon)
441                         hci_conn_del(hcon);
442         } else {
443                 amp_write_remote_assoc(hdev, cp->phy_handle);
444         }
445
446         hci_dev_unlock(hdev);
447 }
448
449 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
450                         struct hci_conn *hcon)
451 {
452         struct hci_cp_create_phy_link cp;
453         struct hci_request req;
454
455         cp.phy_handle = hcon->handle;
456
457         BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
458                hcon->handle);
459
460         if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
461                             &cp.key_type)) {
462                 BT_DBG("Cannot create link key");
463                 return;
464         }
465
466         hci_req_init(&req, hdev);
467         hci_req_add(&req, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
468         hci_req_run(&req, create_phylink_complete);
469 }
470
471 static void accept_phylink_complete(struct hci_dev *hdev, u8 status,
472                                     u16 opcode)
473 {
474         struct hci_cp_accept_phy_link *cp;
475
476         BT_DBG("%s status 0x%2.2x", hdev->name, status);
477
478         if (status)
479                 return;
480
481         cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
482         if (!cp)
483                 return;
484
485         amp_write_remote_assoc(hdev, cp->phy_handle);
486 }
487
488 void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
489                         struct hci_conn *hcon)
490 {
491         struct hci_cp_accept_phy_link cp;
492         struct hci_request req;
493
494         cp.phy_handle = hcon->handle;
495
496         BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
497                hcon->handle);
498
499         if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
500                             &cp.key_type)) {
501                 BT_DBG("Cannot create link key");
502                 return;
503         }
504
505         hci_req_init(&req, hdev);
506         hci_req_add(&req, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
507         hci_req_run(&req, accept_phylink_complete);
508 }
509
510 void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon)
511 {
512         struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev);
513         struct amp_mgr *mgr = hs_hcon->amp_mgr;
514         struct l2cap_chan *bredr_chan;
515
516         BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr);
517
518         if (!bredr_hdev || !mgr || !mgr->bredr_chan)
519                 return;
520
521         bredr_chan = mgr->bredr_chan;
522
523         l2cap_chan_lock(bredr_chan);
524
525         set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags);
526         bredr_chan->remote_amp_id = hs_hcon->remote_id;
527         bredr_chan->local_amp_id = hs_hcon->hdev->id;
528         bredr_chan->hs_hcon = hs_hcon;
529         bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu;
530
531         __l2cap_physical_cfm(bredr_chan, 0);
532
533         l2cap_chan_unlock(bredr_chan);
534
535         hci_dev_put(bredr_hdev);
536 }
537
538 void amp_create_logical_link(struct l2cap_chan *chan)
539 {
540         struct hci_conn *hs_hcon = chan->hs_hcon;
541         struct hci_cp_create_accept_logical_link cp;
542         struct hci_dev *hdev;
543
544         BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon,
545                &chan->conn->hcon->dst);
546
547         if (!hs_hcon)
548                 return;
549
550         hdev = hci_dev_hold(chan->hs_hcon->hdev);
551         if (!hdev)
552                 return;
553
554         cp.phy_handle = hs_hcon->handle;
555
556         cp.tx_flow_spec.id = chan->local_id;
557         cp.tx_flow_spec.stype = chan->local_stype;
558         cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu);
559         cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
560         cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat);
561         cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to);
562
563         cp.rx_flow_spec.id = chan->remote_id;
564         cp.rx_flow_spec.stype = chan->remote_stype;
565         cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu);
566         cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime);
567         cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat);
568         cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to);
569
570         if (hs_hcon->out)
571                 hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp),
572                              &cp);
573         else
574                 hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp),
575                              &cp);
576
577         hci_dev_put(hdev);
578 }
579
580 void amp_disconnect_logical_link(struct hci_chan *hchan)
581 {
582         struct hci_conn *hcon = hchan->conn;
583         struct hci_cp_disconn_logical_link cp;
584
585         if (hcon->state != BT_CONNECTED) {
586                 BT_DBG("hchan %p not connected", hchan);
587                 return;
588         }
589
590         cp.log_handle = cpu_to_le16(hchan->handle);
591         hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp);
592 }
593
594 void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason)
595 {
596         BT_DBG("hchan %p", hchan);
597
598         hci_chan_del(hchan);
599 }