Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003    Copyright (c) 2011,2012 Intel Corp.
0004 
0005 */
0006 
0007 #include <net/bluetooth/bluetooth.h>
0008 #include <net/bluetooth/hci.h>
0009 #include <net/bluetooth/hci_core.h>
0010 #include <crypto/hash.h>
0011 
0012 #include "hci_request.h"
0013 #include "a2mp.h"
0014 #include "amp.h"
0015 
0016 /* Remote AMP Controllers interface */
0017 void amp_ctrl_get(struct amp_ctrl *ctrl)
0018 {
0019     BT_DBG("ctrl %p orig refcnt %d", ctrl,
0020            kref_read(&ctrl->kref));
0021 
0022     kref_get(&ctrl->kref);
0023 }
0024 
0025 static void amp_ctrl_destroy(struct kref *kref)
0026 {
0027     struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
0028 
0029     BT_DBG("ctrl %p", ctrl);
0030 
0031     kfree(ctrl->assoc);
0032     kfree(ctrl);
0033 }
0034 
0035 int amp_ctrl_put(struct amp_ctrl *ctrl)
0036 {
0037     BT_DBG("ctrl %p orig refcnt %d", ctrl,
0038            kref_read(&ctrl->kref));
0039 
0040     return kref_put(&ctrl->kref, &amp_ctrl_destroy);
0041 }
0042 
0043 struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id)
0044 {
0045     struct amp_ctrl *ctrl;
0046 
0047     ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
0048     if (!ctrl)
0049         return NULL;
0050 
0051     kref_init(&ctrl->kref);
0052     ctrl->id = id;
0053 
0054     mutex_lock(&mgr->amp_ctrls_lock);
0055     list_add(&ctrl->list, &mgr->amp_ctrls);
0056     mutex_unlock(&mgr->amp_ctrls_lock);
0057 
0058     BT_DBG("mgr %p ctrl %p", mgr, ctrl);
0059 
0060     return ctrl;
0061 }
0062 
0063 void amp_ctrl_list_flush(struct amp_mgr *mgr)
0064 {
0065     struct amp_ctrl *ctrl, *n;
0066 
0067     BT_DBG("mgr %p", mgr);
0068 
0069     mutex_lock(&mgr->amp_ctrls_lock);
0070     list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
0071         list_del(&ctrl->list);
0072         amp_ctrl_put(ctrl);
0073     }
0074     mutex_unlock(&mgr->amp_ctrls_lock);
0075 }
0076 
0077 struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
0078 {
0079     struct amp_ctrl *ctrl;
0080 
0081     BT_DBG("mgr %p id %u", mgr, id);
0082 
0083     mutex_lock(&mgr->amp_ctrls_lock);
0084     list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
0085         if (ctrl->id == id) {
0086             amp_ctrl_get(ctrl);
0087             mutex_unlock(&mgr->amp_ctrls_lock);
0088             return ctrl;
0089         }
0090     }
0091     mutex_unlock(&mgr->amp_ctrls_lock);
0092 
0093     return NULL;
0094 }
0095 
0096 /* Physical Link interface */
0097 static u8 __next_handle(struct amp_mgr *mgr)
0098 {
0099     if (++mgr->handle == 0)
0100         mgr->handle = 1;
0101 
0102     return mgr->handle;
0103 }
0104 
0105 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
0106                  u8 remote_id, bool out)
0107 {
0108     bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst;
0109     struct hci_conn *hcon;
0110     u8 role = out ? HCI_ROLE_MASTER : HCI_ROLE_SLAVE;
0111 
0112     hcon = hci_conn_add(hdev, AMP_LINK, dst, role);
0113     if (!hcon)
0114         return NULL;
0115 
0116     BT_DBG("hcon %p dst %pMR", hcon, dst);
0117 
0118     hcon->state = BT_CONNECT;
0119     hcon->attempt++;
0120     hcon->handle = __next_handle(mgr);
0121     hcon->remote_id = remote_id;
0122     hcon->amp_mgr = amp_mgr_get(mgr);
0123 
0124     return hcon;
0125 }
0126 
0127 /* AMP crypto key generation interface */
0128 static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
0129 {
0130     struct crypto_shash *tfm;
0131     struct shash_desc *shash;
0132     int ret;
0133 
0134     if (!ksize)
0135         return -EINVAL;
0136 
0137     tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
0138     if (IS_ERR(tfm)) {
0139         BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
0140         return PTR_ERR(tfm);
0141     }
0142 
0143     ret = crypto_shash_setkey(tfm, key, ksize);
0144     if (ret) {
0145         BT_DBG("crypto_ahash_setkey failed: err %d", ret);
0146         goto failed;
0147     }
0148 
0149     shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm),
0150             GFP_KERNEL);
0151     if (!shash) {
0152         ret = -ENOMEM;
0153         goto failed;
0154     }
0155 
0156     shash->tfm = tfm;
0157 
0158     ret = crypto_shash_digest(shash, plaintext, psize, output);
0159 
0160     kfree(shash);
0161 
0162 failed:
0163     crypto_free_shash(tfm);
0164     return ret;
0165 }
0166 
0167 int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
0168 {
0169     struct hci_dev *hdev = conn->hdev;
0170     struct link_key *key;
0171     u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
0172     u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
0173     int err;
0174 
0175     if (!hci_conn_check_link_mode(conn))
0176         return -EACCES;
0177 
0178     BT_DBG("conn %p key_type %d", conn, conn->key_type);
0179 
0180     /* Legacy key */
0181     if (conn->key_type < 3) {
0182         bt_dev_err(hdev, "legacy key type %u", conn->key_type);
0183         return -EACCES;
0184     }
0185 
0186     *type = conn->key_type;
0187     *len = HCI_AMP_LINK_KEY_SIZE;
0188 
0189     key = hci_find_link_key(hdev, &conn->dst);
0190     if (!key) {
0191         BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst);
0192         return -EACCES;
0193     }
0194 
0195     /* BR/EDR Link Key concatenated together with itself */
0196     memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
0197     memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
0198 
0199     /* Derive Generic AMP Link Key (gamp) */
0200     err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
0201     if (err) {
0202         bt_dev_err(hdev, "could not derive Generic AMP Key: err %d", err);
0203         return err;
0204     }
0205 
0206     if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
0207         BT_DBG("Use Generic AMP Key (gamp)");
0208         memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
0209         return err;
0210     }
0211 
0212     /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
0213     return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
0214 }
0215 
0216 static void read_local_amp_assoc_complete(struct hci_dev *hdev, u8 status,
0217                       u16 opcode, struct sk_buff *skb)
0218 {
0219     struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
0220     struct amp_assoc *assoc = &hdev->loc_assoc;
0221     size_t rem_len, frag_len;
0222 
0223     BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
0224 
0225     if (rp->status)
0226         goto send_rsp;
0227 
0228     frag_len = skb->len - sizeof(*rp);
0229     rem_len = __le16_to_cpu(rp->rem_len);
0230 
0231     if (rem_len > frag_len) {
0232         BT_DBG("frag_len %zu rem_len %zu", frag_len, rem_len);
0233 
0234         memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
0235         assoc->offset += frag_len;
0236 
0237         /* Read other fragments */
0238         amp_read_loc_assoc_frag(hdev, rp->phy_handle);
0239 
0240         return;
0241     }
0242 
0243     memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
0244     assoc->len = assoc->offset + rem_len;
0245     assoc->offset = 0;
0246 
0247 send_rsp:
0248     /* Send A2MP Rsp when all fragments are received */
0249     a2mp_send_getampassoc_rsp(hdev, rp->status);
0250     a2mp_send_create_phy_link_req(hdev, rp->status);
0251 }
0252 
0253 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
0254 {
0255     struct hci_cp_read_local_amp_assoc cp;
0256     struct amp_assoc *loc_assoc = &hdev->loc_assoc;
0257     struct hci_request req;
0258     int err;
0259 
0260     BT_DBG("%s handle %u", hdev->name, phy_handle);
0261 
0262     cp.phy_handle = phy_handle;
0263     cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
0264     cp.len_so_far = cpu_to_le16(loc_assoc->offset);
0265 
0266     hci_req_init(&req, hdev);
0267     hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
0268     err = hci_req_run_skb(&req, read_local_amp_assoc_complete);
0269     if (err < 0)
0270         a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
0271 }
0272 
0273 void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
0274 {
0275     struct hci_cp_read_local_amp_assoc cp;
0276     struct hci_request req;
0277     int err;
0278 
0279     memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
0280     memset(&cp, 0, sizeof(cp));
0281 
0282     cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
0283 
0284     set_bit(READ_LOC_AMP_ASSOC, &mgr->state);
0285     hci_req_init(&req, hdev);
0286     hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
0287     err = hci_req_run_skb(&req, read_local_amp_assoc_complete);
0288     if (err < 0)
0289         a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
0290 }
0291 
0292 void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
0293                    struct hci_conn *hcon)
0294 {
0295     struct hci_cp_read_local_amp_assoc cp;
0296     struct amp_mgr *mgr = hcon->amp_mgr;
0297     struct hci_request req;
0298     int err;
0299 
0300     if (!mgr)
0301         return;
0302 
0303     cp.phy_handle = hcon->handle;
0304     cp.len_so_far = cpu_to_le16(0);
0305     cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
0306 
0307     set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state);
0308 
0309     /* Read Local AMP Assoc final link information data */
0310     hci_req_init(&req, hdev);
0311     hci_req_add(&req, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
0312     err = hci_req_run_skb(&req, read_local_amp_assoc_complete);
0313     if (err < 0)
0314         a2mp_send_getampassoc_rsp(hdev, A2MP_STATUS_INVALID_CTRL_ID);
0315 }
0316 
0317 static void write_remote_amp_assoc_complete(struct hci_dev *hdev, u8 status,
0318                         u16 opcode, struct sk_buff *skb)
0319 {
0320     struct hci_rp_write_remote_amp_assoc *rp = (void *)skb->data;
0321 
0322     BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
0323            hdev->name, rp->status, rp->phy_handle);
0324 
0325     if (rp->status)
0326         return;
0327 
0328     amp_write_rem_assoc_continue(hdev, rp->phy_handle);
0329 }
0330 
0331 /* Write AMP Assoc data fragments, returns true with last fragment written*/
0332 static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
0333                      struct hci_conn *hcon)
0334 {
0335     struct hci_cp_write_remote_amp_assoc *cp;
0336     struct amp_mgr *mgr = hcon->amp_mgr;
0337     struct amp_ctrl *ctrl;
0338     struct hci_request req;
0339     u16 frag_len, len;
0340 
0341     ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
0342     if (!ctrl)
0343         return false;
0344 
0345     if (!ctrl->assoc_rem_len) {
0346         BT_DBG("all fragments are written");
0347         ctrl->assoc_rem_len = ctrl->assoc_len;
0348         ctrl->assoc_len_so_far = 0;
0349 
0350         amp_ctrl_put(ctrl);
0351         return true;
0352     }
0353 
0354     frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
0355     len = frag_len + sizeof(*cp);
0356 
0357     cp = kzalloc(len, GFP_KERNEL);
0358     if (!cp) {
0359         amp_ctrl_put(ctrl);
0360         return false;
0361     }
0362 
0363     BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
0364            hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
0365 
0366     cp->phy_handle = hcon->handle;
0367     cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
0368     cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
0369     memcpy(cp->frag, ctrl->assoc, frag_len);
0370 
0371     ctrl->assoc_len_so_far += frag_len;
0372     ctrl->assoc_rem_len -= frag_len;
0373 
0374     amp_ctrl_put(ctrl);
0375 
0376     hci_req_init(&req, hdev);
0377     hci_req_add(&req, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
0378     hci_req_run_skb(&req, write_remote_amp_assoc_complete);
0379 
0380     kfree(cp);
0381 
0382     return false;
0383 }
0384 
0385 void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
0386 {
0387     struct hci_conn *hcon;
0388 
0389     BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
0390 
0391     hcon = hci_conn_hash_lookup_handle(hdev, handle);
0392     if (!hcon)
0393         return;
0394 
0395     /* Send A2MP create phylink rsp when all fragments are written */
0396     if (amp_write_rem_assoc_frag(hdev, hcon))
0397         a2mp_send_create_phy_link_rsp(hdev, 0);
0398 }
0399 
0400 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
0401 {
0402     struct hci_conn *hcon;
0403 
0404     BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
0405 
0406     hcon = hci_conn_hash_lookup_handle(hdev, handle);
0407     if (!hcon)
0408         return;
0409 
0410     BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
0411 
0412     amp_write_rem_assoc_frag(hdev, hcon);
0413 }
0414 
0415 static void create_phylink_complete(struct hci_dev *hdev, u8 status,
0416                     u16 opcode)
0417 {
0418     struct hci_cp_create_phy_link *cp;
0419 
0420     BT_DBG("%s status 0x%2.2x", hdev->name, status);
0421 
0422     cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
0423     if (!cp)
0424         return;
0425 
0426     hci_dev_lock(hdev);
0427 
0428     if (status) {
0429         struct hci_conn *hcon;
0430 
0431         hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle);
0432         if (hcon)
0433             hci_conn_del(hcon);
0434     } else {
0435         amp_write_remote_assoc(hdev, cp->phy_handle);
0436     }
0437 
0438     hci_dev_unlock(hdev);
0439 }
0440 
0441 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
0442             struct hci_conn *hcon)
0443 {
0444     struct hci_cp_create_phy_link cp;
0445     struct hci_request req;
0446 
0447     cp.phy_handle = hcon->handle;
0448 
0449     BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
0450            hcon->handle);
0451 
0452     if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
0453                 &cp.key_type)) {
0454         BT_DBG("Cannot create link key");
0455         return;
0456     }
0457 
0458     hci_req_init(&req, hdev);
0459     hci_req_add(&req, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
0460     hci_req_run(&req, create_phylink_complete);
0461 }
0462 
0463 static void accept_phylink_complete(struct hci_dev *hdev, u8 status,
0464                     u16 opcode)
0465 {
0466     struct hci_cp_accept_phy_link *cp;
0467 
0468     BT_DBG("%s status 0x%2.2x", hdev->name, status);
0469 
0470     if (status)
0471         return;
0472 
0473     cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
0474     if (!cp)
0475         return;
0476 
0477     amp_write_remote_assoc(hdev, cp->phy_handle);
0478 }
0479 
0480 void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
0481             struct hci_conn *hcon)
0482 {
0483     struct hci_cp_accept_phy_link cp;
0484     struct hci_request req;
0485 
0486     cp.phy_handle = hcon->handle;
0487 
0488     BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
0489            hcon->handle);
0490 
0491     if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
0492                 &cp.key_type)) {
0493         BT_DBG("Cannot create link key");
0494         return;
0495     }
0496 
0497     hci_req_init(&req, hdev);
0498     hci_req_add(&req, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
0499     hci_req_run(&req, accept_phylink_complete);
0500 }
0501 
0502 void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon)
0503 {
0504     struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev);
0505     struct amp_mgr *mgr = hs_hcon->amp_mgr;
0506     struct l2cap_chan *bredr_chan;
0507 
0508     BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr);
0509 
0510     if (!bredr_hdev || !mgr || !mgr->bredr_chan)
0511         return;
0512 
0513     bredr_chan = mgr->bredr_chan;
0514 
0515     l2cap_chan_lock(bredr_chan);
0516 
0517     set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags);
0518     bredr_chan->remote_amp_id = hs_hcon->remote_id;
0519     bredr_chan->local_amp_id = hs_hcon->hdev->id;
0520     bredr_chan->hs_hcon = hs_hcon;
0521     bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu;
0522 
0523     __l2cap_physical_cfm(bredr_chan, 0);
0524 
0525     l2cap_chan_unlock(bredr_chan);
0526 
0527     hci_dev_put(bredr_hdev);
0528 }
0529 
0530 void amp_create_logical_link(struct l2cap_chan *chan)
0531 {
0532     struct hci_conn *hs_hcon = chan->hs_hcon;
0533     struct hci_cp_create_accept_logical_link cp;
0534     struct hci_dev *hdev;
0535 
0536     BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon,
0537            &chan->conn->hcon->dst);
0538 
0539     if (!hs_hcon)
0540         return;
0541 
0542     hdev = hci_dev_hold(chan->hs_hcon->hdev);
0543     if (!hdev)
0544         return;
0545 
0546     cp.phy_handle = hs_hcon->handle;
0547 
0548     cp.tx_flow_spec.id = chan->local_id;
0549     cp.tx_flow_spec.stype = chan->local_stype;
0550     cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu);
0551     cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
0552     cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat);
0553     cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to);
0554 
0555     cp.rx_flow_spec.id = chan->remote_id;
0556     cp.rx_flow_spec.stype = chan->remote_stype;
0557     cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu);
0558     cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime);
0559     cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat);
0560     cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to);
0561 
0562     if (hs_hcon->out)
0563         hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp),
0564                  &cp);
0565     else
0566         hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp),
0567                  &cp);
0568 
0569     hci_dev_put(hdev);
0570 }
0571 
0572 void amp_disconnect_logical_link(struct hci_chan *hchan)
0573 {
0574     struct hci_conn *hcon = hchan->conn;
0575     struct hci_cp_disconn_logical_link cp;
0576 
0577     if (hcon->state != BT_CONNECTED) {
0578         BT_DBG("hchan %p not connected", hchan);
0579         return;
0580     }
0581 
0582     cp.log_handle = cpu_to_le16(hchan->handle);
0583     hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp);
0584 }
0585 
0586 void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason)
0587 {
0588     BT_DBG("hchan %p", hchan);
0589 
0590     hci_chan_del(hchan);
0591 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!