Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003    Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
0004    Copyright (c) 2011,2012 Intel Corp.
0005 
0006 */
0007 
0008 #include <net/bluetooth/bluetooth.h>
0009 #include <net/bluetooth/hci_core.h>
0010 #include <net/bluetooth/l2cap.h>
0011 
0012 #include "hci_request.h"
0013 #include "a2mp.h"
0014 #include "amp.h"
0015 
0016 #define A2MP_FEAT_EXT   0x8000
0017 
0018 /* Global AMP Manager list */
0019 static LIST_HEAD(amp_mgr_list);
0020 static DEFINE_MUTEX(amp_mgr_list_lock);
0021 
0022 /* A2MP build & send command helper functions */
0023 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
0024 {
0025     struct a2mp_cmd *cmd;
0026     int plen;
0027 
0028     plen = sizeof(*cmd) + len;
0029     cmd = kzalloc(plen, GFP_KERNEL);
0030     if (!cmd)
0031         return NULL;
0032 
0033     cmd->code = code;
0034     cmd->ident = ident;
0035     cmd->len = cpu_to_le16(len);
0036 
0037     memcpy(cmd->data, data, len);
0038 
0039     return cmd;
0040 }
0041 
0042 static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
0043 {
0044     struct l2cap_chan *chan = mgr->a2mp_chan;
0045     struct a2mp_cmd *cmd;
0046     u16 total_len = len + sizeof(*cmd);
0047     struct kvec iv;
0048     struct msghdr msg;
0049 
0050     cmd = __a2mp_build(code, ident, len, data);
0051     if (!cmd)
0052         return;
0053 
0054     iv.iov_base = cmd;
0055     iv.iov_len = total_len;
0056 
0057     memset(&msg, 0, sizeof(msg));
0058 
0059     iov_iter_kvec(&msg.msg_iter, WRITE, &iv, 1, total_len);
0060 
0061     l2cap_chan_send(chan, &msg, total_len);
0062 
0063     kfree(cmd);
0064 }
0065 
0066 static u8 __next_ident(struct amp_mgr *mgr)
0067 {
0068     if (++mgr->ident == 0)
0069         mgr->ident = 1;
0070 
0071     return mgr->ident;
0072 }
0073 
0074 static struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
0075 {
0076     struct amp_mgr *mgr;
0077 
0078     mutex_lock(&amp_mgr_list_lock);
0079     list_for_each_entry(mgr, &amp_mgr_list, list) {
0080         if (test_and_clear_bit(state, &mgr->state)) {
0081             amp_mgr_get(mgr);
0082             mutex_unlock(&amp_mgr_list_lock);
0083             return mgr;
0084         }
0085     }
0086     mutex_unlock(&amp_mgr_list_lock);
0087 
0088     return NULL;
0089 }
0090 
0091 /* hci_dev_list shall be locked */
0092 static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl)
0093 {
0094     struct hci_dev *hdev;
0095     int i = 1;
0096 
0097     cl[0].id = AMP_ID_BREDR;
0098     cl[0].type = AMP_TYPE_BREDR;
0099     cl[0].status = AMP_STATUS_BLUETOOTH_ONLY;
0100 
0101     list_for_each_entry(hdev, &hci_dev_list, list) {
0102         if (hdev->dev_type == HCI_AMP) {
0103             cl[i].id = hdev->id;
0104             cl[i].type = hdev->amp_type;
0105             if (test_bit(HCI_UP, &hdev->flags))
0106                 cl[i].status = hdev->amp_status;
0107             else
0108                 cl[i].status = AMP_STATUS_POWERED_DOWN;
0109             i++;
0110         }
0111     }
0112 }
0113 
0114 /* Processing A2MP messages */
0115 static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
0116                 struct a2mp_cmd *hdr)
0117 {
0118     struct a2mp_cmd_rej *rej = (void *) skb->data;
0119 
0120     if (le16_to_cpu(hdr->len) < sizeof(*rej))
0121         return -EINVAL;
0122 
0123     BT_DBG("ident %u reason %d", hdr->ident, le16_to_cpu(rej->reason));
0124 
0125     skb_pull(skb, sizeof(*rej));
0126 
0127     return 0;
0128 }
0129 
0130 static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
0131                  struct a2mp_cmd *hdr)
0132 {
0133     struct a2mp_discov_req *req = (void *) skb->data;
0134     u16 len = le16_to_cpu(hdr->len);
0135     struct a2mp_discov_rsp *rsp;
0136     u16 ext_feat;
0137     u8 num_ctrl;
0138     struct hci_dev *hdev;
0139 
0140     if (len < sizeof(*req))
0141         return -EINVAL;
0142 
0143     skb_pull(skb, sizeof(*req));
0144 
0145     ext_feat = le16_to_cpu(req->ext_feat);
0146 
0147     BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
0148 
0149     /* check that packet is not broken for now */
0150     while (ext_feat & A2MP_FEAT_EXT) {
0151         if (len < sizeof(ext_feat))
0152             return -EINVAL;
0153 
0154         ext_feat = get_unaligned_le16(skb->data);
0155         BT_DBG("efm 0x%4.4x", ext_feat);
0156         len -= sizeof(ext_feat);
0157         skb_pull(skb, sizeof(ext_feat));
0158     }
0159 
0160     read_lock(&hci_dev_list_lock);
0161 
0162     /* at minimum the BR/EDR needs to be listed */
0163     num_ctrl = 1;
0164 
0165     list_for_each_entry(hdev, &hci_dev_list, list) {
0166         if (hdev->dev_type == HCI_AMP)
0167             num_ctrl++;
0168     }
0169 
0170     len = struct_size(rsp, cl, num_ctrl);
0171     rsp = kmalloc(len, GFP_ATOMIC);
0172     if (!rsp) {
0173         read_unlock(&hci_dev_list_lock);
0174         return -ENOMEM;
0175     }
0176 
0177     rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
0178     rsp->ext_feat = 0;
0179 
0180     __a2mp_add_cl(mgr, rsp->cl);
0181 
0182     read_unlock(&hci_dev_list_lock);
0183 
0184     a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
0185 
0186     kfree(rsp);
0187     return 0;
0188 }
0189 
0190 static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
0191                  struct a2mp_cmd *hdr)
0192 {
0193     struct a2mp_discov_rsp *rsp = (void *) skb->data;
0194     u16 len = le16_to_cpu(hdr->len);
0195     struct a2mp_cl *cl;
0196     u16 ext_feat;
0197     bool found = false;
0198 
0199     if (len < sizeof(*rsp))
0200         return -EINVAL;
0201 
0202     len -= sizeof(*rsp);
0203     skb_pull(skb, sizeof(*rsp));
0204 
0205     ext_feat = le16_to_cpu(rsp->ext_feat);
0206 
0207     BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
0208 
0209     /* check that packet is not broken for now */
0210     while (ext_feat & A2MP_FEAT_EXT) {
0211         if (len < sizeof(ext_feat))
0212             return -EINVAL;
0213 
0214         ext_feat = get_unaligned_le16(skb->data);
0215         BT_DBG("efm 0x%4.4x", ext_feat);
0216         len -= sizeof(ext_feat);
0217         skb_pull(skb, sizeof(ext_feat));
0218     }
0219 
0220     cl = (void *) skb->data;
0221     while (len >= sizeof(*cl)) {
0222         BT_DBG("Remote AMP id %u type %u status %u", cl->id, cl->type,
0223                cl->status);
0224 
0225         if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) {
0226             struct a2mp_info_req req;
0227 
0228             found = true;
0229 
0230             memset(&req, 0, sizeof(req));
0231 
0232             req.id = cl->id;
0233             a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
0234                   sizeof(req), &req);
0235         }
0236 
0237         len -= sizeof(*cl);
0238         cl = skb_pull(skb, sizeof(*cl));
0239     }
0240 
0241     /* Fall back to L2CAP init sequence */
0242     if (!found) {
0243         struct l2cap_conn *conn = mgr->l2cap_conn;
0244         struct l2cap_chan *chan;
0245 
0246         mutex_lock(&conn->chan_lock);
0247 
0248         list_for_each_entry(chan, &conn->chan_l, list) {
0249 
0250             BT_DBG("chan %p state %s", chan,
0251                    state_to_string(chan->state));
0252 
0253             if (chan->scid == L2CAP_CID_A2MP)
0254                 continue;
0255 
0256             l2cap_chan_lock(chan);
0257 
0258             if (chan->state == BT_CONNECT)
0259                 l2cap_send_conn_req(chan);
0260 
0261             l2cap_chan_unlock(chan);
0262         }
0263 
0264         mutex_unlock(&conn->chan_lock);
0265     }
0266 
0267     return 0;
0268 }
0269 
0270 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
0271                   struct a2mp_cmd *hdr)
0272 {
0273     struct a2mp_cl *cl = (void *) skb->data;
0274 
0275     while (skb->len >= sizeof(*cl)) {
0276         BT_DBG("Controller id %u type %u status %u", cl->id, cl->type,
0277                cl->status);
0278         cl = skb_pull(skb, sizeof(*cl));
0279     }
0280 
0281     /* TODO send A2MP_CHANGE_RSP */
0282 
0283     return 0;
0284 }
0285 
0286 static void read_local_amp_info_complete(struct hci_dev *hdev, u8 status,
0287                      u16 opcode)
0288 {
0289     BT_DBG("%s status 0x%2.2x", hdev->name, status);
0290 
0291     a2mp_send_getinfo_rsp(hdev);
0292 }
0293 
0294 static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
0295                 struct a2mp_cmd *hdr)
0296 {
0297     struct a2mp_info_req *req  = (void *) skb->data;
0298     struct hci_dev *hdev;
0299     struct hci_request hreq;
0300     int err = 0;
0301 
0302     if (le16_to_cpu(hdr->len) < sizeof(*req))
0303         return -EINVAL;
0304 
0305     BT_DBG("id %u", req->id);
0306 
0307     hdev = hci_dev_get(req->id);
0308     if (!hdev || hdev->dev_type != HCI_AMP) {
0309         struct a2mp_info_rsp rsp;
0310 
0311         memset(&rsp, 0, sizeof(rsp));
0312 
0313         rsp.id = req->id;
0314         rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
0315 
0316         a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
0317               &rsp);
0318 
0319         goto done;
0320     }
0321 
0322     set_bit(READ_LOC_AMP_INFO, &mgr->state);
0323     hci_req_init(&hreq, hdev);
0324     hci_req_add(&hreq, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
0325     err = hci_req_run(&hreq, read_local_amp_info_complete);
0326     if (err < 0)
0327         a2mp_send_getinfo_rsp(hdev);
0328 
0329 done:
0330     if (hdev)
0331         hci_dev_put(hdev);
0332 
0333     skb_pull(skb, sizeof(*req));
0334     return 0;
0335 }
0336 
0337 static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
0338                 struct a2mp_cmd *hdr)
0339 {
0340     struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
0341     struct a2mp_amp_assoc_req req;
0342     struct amp_ctrl *ctrl;
0343 
0344     if (le16_to_cpu(hdr->len) < sizeof(*rsp))
0345         return -EINVAL;
0346 
0347     BT_DBG("id %u status 0x%2.2x", rsp->id, rsp->status);
0348 
0349     if (rsp->status)
0350         return -EINVAL;
0351 
0352     ctrl = amp_ctrl_add(mgr, rsp->id);
0353     if (!ctrl)
0354         return -ENOMEM;
0355 
0356     memset(&req, 0, sizeof(req));
0357 
0358     req.id = rsp->id;
0359     a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
0360           &req);
0361 
0362     skb_pull(skb, sizeof(*rsp));
0363     return 0;
0364 }
0365 
0366 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
0367                 struct a2mp_cmd *hdr)
0368 {
0369     struct a2mp_amp_assoc_req *req = (void *) skb->data;
0370     struct hci_dev *hdev;
0371     struct amp_mgr *tmp;
0372 
0373     if (le16_to_cpu(hdr->len) < sizeof(*req))
0374         return -EINVAL;
0375 
0376     BT_DBG("id %u", req->id);
0377 
0378     /* Make sure that other request is not processed */
0379     tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
0380 
0381     hdev = hci_dev_get(req->id);
0382     if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) {
0383         struct a2mp_amp_assoc_rsp rsp;
0384 
0385         memset(&rsp, 0, sizeof(rsp));
0386         rsp.id = req->id;
0387 
0388         if (tmp) {
0389             rsp.status = A2MP_STATUS_COLLISION_OCCURED;
0390             amp_mgr_put(tmp);
0391         } else {
0392             rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
0393         }
0394 
0395         a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
0396               &rsp);
0397 
0398         goto done;
0399     }
0400 
0401     amp_read_loc_assoc(hdev, mgr);
0402 
0403 done:
0404     if (hdev)
0405         hci_dev_put(hdev);
0406 
0407     skb_pull(skb, sizeof(*req));
0408     return 0;
0409 }
0410 
0411 static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
0412                 struct a2mp_cmd *hdr)
0413 {
0414     struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
0415     u16 len = le16_to_cpu(hdr->len);
0416     struct hci_dev *hdev;
0417     struct amp_ctrl *ctrl;
0418     struct hci_conn *hcon;
0419     size_t assoc_len;
0420 
0421     if (len < sizeof(*rsp))
0422         return -EINVAL;
0423 
0424     assoc_len = len - sizeof(*rsp);
0425 
0426     BT_DBG("id %u status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
0427            assoc_len);
0428 
0429     if (rsp->status)
0430         return -EINVAL;
0431 
0432     /* Save remote ASSOC data */
0433     ctrl = amp_ctrl_lookup(mgr, rsp->id);
0434     if (ctrl) {
0435         u8 *assoc;
0436 
0437         assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
0438         if (!assoc) {
0439             amp_ctrl_put(ctrl);
0440             return -ENOMEM;
0441         }
0442 
0443         ctrl->assoc = assoc;
0444         ctrl->assoc_len = assoc_len;
0445         ctrl->assoc_rem_len = assoc_len;
0446         ctrl->assoc_len_so_far = 0;
0447 
0448         amp_ctrl_put(ctrl);
0449     }
0450 
0451     /* Create Phys Link */
0452     hdev = hci_dev_get(rsp->id);
0453     if (!hdev)
0454         return -EINVAL;
0455 
0456     hcon = phylink_add(hdev, mgr, rsp->id, true);
0457     if (!hcon)
0458         goto done;
0459 
0460     BT_DBG("Created hcon %p: loc:%u -> rem:%u", hcon, hdev->id, rsp->id);
0461 
0462     mgr->bredr_chan->remote_amp_id = rsp->id;
0463 
0464     amp_create_phylink(hdev, mgr, hcon);
0465 
0466 done:
0467     hci_dev_put(hdev);
0468     skb_pull(skb, len);
0469     return 0;
0470 }
0471 
0472 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
0473                    struct a2mp_cmd *hdr)
0474 {
0475     struct a2mp_physlink_req *req = (void *) skb->data;
0476     struct a2mp_physlink_rsp rsp;
0477     struct hci_dev *hdev;
0478     struct hci_conn *hcon;
0479     struct amp_ctrl *ctrl;
0480 
0481     if (le16_to_cpu(hdr->len) < sizeof(*req))
0482         return -EINVAL;
0483 
0484     BT_DBG("local_id %u, remote_id %u", req->local_id, req->remote_id);
0485 
0486     memset(&rsp, 0, sizeof(rsp));
0487 
0488     rsp.local_id = req->remote_id;
0489     rsp.remote_id = req->local_id;
0490 
0491     hdev = hci_dev_get(req->remote_id);
0492     if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) {
0493         rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
0494         goto send_rsp;
0495     }
0496 
0497     ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
0498     if (!ctrl) {
0499         ctrl = amp_ctrl_add(mgr, rsp.remote_id);
0500         if (ctrl) {
0501             amp_ctrl_get(ctrl);
0502         } else {
0503             rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
0504             goto send_rsp;
0505         }
0506     }
0507 
0508     if (ctrl) {
0509         size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
0510         u8 *assoc;
0511 
0512         assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
0513         if (!assoc) {
0514             amp_ctrl_put(ctrl);
0515             hci_dev_put(hdev);
0516             return -ENOMEM;
0517         }
0518 
0519         ctrl->assoc = assoc;
0520         ctrl->assoc_len = assoc_len;
0521         ctrl->assoc_rem_len = assoc_len;
0522         ctrl->assoc_len_so_far = 0;
0523 
0524         amp_ctrl_put(ctrl);
0525     }
0526 
0527     hcon = phylink_add(hdev, mgr, req->local_id, false);
0528     if (hcon) {
0529         amp_accept_phylink(hdev, mgr, hcon);
0530         rsp.status = A2MP_STATUS_SUCCESS;
0531     } else {
0532         rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
0533     }
0534 
0535 send_rsp:
0536     if (hdev)
0537         hci_dev_put(hdev);
0538 
0539     /* Reply error now and success after HCI Write Remote AMP Assoc
0540        command complete with success status
0541      */
0542     if (rsp.status != A2MP_STATUS_SUCCESS) {
0543         a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
0544               sizeof(rsp), &rsp);
0545     } else {
0546         set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
0547         mgr->ident = hdr->ident;
0548     }
0549 
0550     skb_pull(skb, le16_to_cpu(hdr->len));
0551     return 0;
0552 }
0553 
0554 static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
0555                  struct a2mp_cmd *hdr)
0556 {
0557     struct a2mp_physlink_req *req = (void *) skb->data;
0558     struct a2mp_physlink_rsp rsp;
0559     struct hci_dev *hdev;
0560     struct hci_conn *hcon;
0561 
0562     if (le16_to_cpu(hdr->len) < sizeof(*req))
0563         return -EINVAL;
0564 
0565     BT_DBG("local_id %u remote_id %u", req->local_id, req->remote_id);
0566 
0567     memset(&rsp, 0, sizeof(rsp));
0568 
0569     rsp.local_id = req->remote_id;
0570     rsp.remote_id = req->local_id;
0571     rsp.status = A2MP_STATUS_SUCCESS;
0572 
0573     hdev = hci_dev_get(req->remote_id);
0574     if (!hdev) {
0575         rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
0576         goto send_rsp;
0577     }
0578 
0579     hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
0580                        &mgr->l2cap_conn->hcon->dst);
0581     if (!hcon) {
0582         bt_dev_err(hdev, "no phys link exist");
0583         rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
0584         goto clean;
0585     }
0586 
0587     /* TODO Disconnect Phys Link here */
0588 
0589 clean:
0590     hci_dev_put(hdev);
0591 
0592 send_rsp:
0593     a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
0594 
0595     skb_pull(skb, sizeof(*req));
0596     return 0;
0597 }
0598 
0599 static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
0600                    struct a2mp_cmd *hdr)
0601 {
0602     BT_DBG("ident %u code 0x%2.2x", hdr->ident, hdr->code);
0603 
0604     skb_pull(skb, le16_to_cpu(hdr->len));
0605     return 0;
0606 }
0607 
0608 /* Handle A2MP signalling */
0609 static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
0610 {
0611     struct a2mp_cmd *hdr;
0612     struct amp_mgr *mgr = chan->data;
0613     int err = 0;
0614 
0615     amp_mgr_get(mgr);
0616 
0617     while (skb->len >= sizeof(*hdr)) {
0618         u16 len;
0619 
0620         hdr = (void *) skb->data;
0621         len = le16_to_cpu(hdr->len);
0622 
0623         BT_DBG("code 0x%2.2x id %u len %u", hdr->code, hdr->ident, len);
0624 
0625         skb_pull(skb, sizeof(*hdr));
0626 
0627         if (len > skb->len || !hdr->ident) {
0628             err = -EINVAL;
0629             break;
0630         }
0631 
0632         mgr->ident = hdr->ident;
0633 
0634         switch (hdr->code) {
0635         case A2MP_COMMAND_REJ:
0636             a2mp_command_rej(mgr, skb, hdr);
0637             break;
0638 
0639         case A2MP_DISCOVER_REQ:
0640             err = a2mp_discover_req(mgr, skb, hdr);
0641             break;
0642 
0643         case A2MP_CHANGE_NOTIFY:
0644             err = a2mp_change_notify(mgr, skb, hdr);
0645             break;
0646 
0647         case A2MP_GETINFO_REQ:
0648             err = a2mp_getinfo_req(mgr, skb, hdr);
0649             break;
0650 
0651         case A2MP_GETAMPASSOC_REQ:
0652             err = a2mp_getampassoc_req(mgr, skb, hdr);
0653             break;
0654 
0655         case A2MP_CREATEPHYSLINK_REQ:
0656             err = a2mp_createphyslink_req(mgr, skb, hdr);
0657             break;
0658 
0659         case A2MP_DISCONNPHYSLINK_REQ:
0660             err = a2mp_discphyslink_req(mgr, skb, hdr);
0661             break;
0662 
0663         case A2MP_DISCOVER_RSP:
0664             err = a2mp_discover_rsp(mgr, skb, hdr);
0665             break;
0666 
0667         case A2MP_GETINFO_RSP:
0668             err = a2mp_getinfo_rsp(mgr, skb, hdr);
0669             break;
0670 
0671         case A2MP_GETAMPASSOC_RSP:
0672             err = a2mp_getampassoc_rsp(mgr, skb, hdr);
0673             break;
0674 
0675         case A2MP_CHANGE_RSP:
0676         case A2MP_CREATEPHYSLINK_RSP:
0677         case A2MP_DISCONNPHYSLINK_RSP:
0678             err = a2mp_cmd_rsp(mgr, skb, hdr);
0679             break;
0680 
0681         default:
0682             BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
0683             err = -EINVAL;
0684             break;
0685         }
0686     }
0687 
0688     if (err) {
0689         struct a2mp_cmd_rej rej;
0690 
0691         memset(&rej, 0, sizeof(rej));
0692 
0693         rej.reason = cpu_to_le16(0);
0694         hdr = (void *) skb->data;
0695 
0696         BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
0697 
0698         a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
0699               &rej);
0700     }
0701 
0702     /* Always free skb and return success error code to prevent
0703        from sending L2CAP Disconnect over A2MP channel */
0704     kfree_skb(skb);
0705 
0706     amp_mgr_put(mgr);
0707 
0708     return 0;
0709 }
0710 
0711 static void a2mp_chan_close_cb(struct l2cap_chan *chan)
0712 {
0713     l2cap_chan_put(chan);
0714 }
0715 
0716 static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state,
0717                       int err)
0718 {
0719     struct amp_mgr *mgr = chan->data;
0720 
0721     if (!mgr)
0722         return;
0723 
0724     BT_DBG("chan %p state %s", chan, state_to_string(state));
0725 
0726     chan->state = state;
0727 
0728     switch (state) {
0729     case BT_CLOSED:
0730         if (mgr)
0731             amp_mgr_put(mgr);
0732         break;
0733     }
0734 }
0735 
0736 static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
0737                           unsigned long hdr_len,
0738                           unsigned long len, int nb)
0739 {
0740     struct sk_buff *skb;
0741 
0742     skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL);
0743     if (!skb)
0744         return ERR_PTR(-ENOMEM);
0745 
0746     return skb;
0747 }
0748 
0749 static const struct l2cap_ops a2mp_chan_ops = {
0750     .name = "L2CAP A2MP channel",
0751     .recv = a2mp_chan_recv_cb,
0752     .close = a2mp_chan_close_cb,
0753     .state_change = a2mp_chan_state_change_cb,
0754     .alloc_skb = a2mp_chan_alloc_skb_cb,
0755 
0756     /* Not implemented for A2MP */
0757     .new_connection = l2cap_chan_no_new_connection,
0758     .teardown = l2cap_chan_no_teardown,
0759     .ready = l2cap_chan_no_ready,
0760     .defer = l2cap_chan_no_defer,
0761     .resume = l2cap_chan_no_resume,
0762     .set_shutdown = l2cap_chan_no_set_shutdown,
0763     .get_sndtimeo = l2cap_chan_no_get_sndtimeo,
0764 };
0765 
0766 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
0767 {
0768     struct l2cap_chan *chan;
0769     int err;
0770 
0771     chan = l2cap_chan_create();
0772     if (!chan)
0773         return NULL;
0774 
0775     BT_DBG("chan %p", chan);
0776 
0777     chan->chan_type = L2CAP_CHAN_FIXED;
0778     chan->scid = L2CAP_CID_A2MP;
0779     chan->dcid = L2CAP_CID_A2MP;
0780     chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
0781     chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
0782     chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
0783 
0784     chan->ops = &a2mp_chan_ops;
0785 
0786     l2cap_chan_set_defaults(chan);
0787     chan->remote_max_tx = chan->max_tx;
0788     chan->remote_tx_win = chan->tx_win;
0789 
0790     chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
0791     chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
0792 
0793     skb_queue_head_init(&chan->tx_q);
0794 
0795     chan->mode = L2CAP_MODE_ERTM;
0796 
0797     err = l2cap_ertm_init(chan);
0798     if (err < 0) {
0799         l2cap_chan_del(chan, 0);
0800         return NULL;
0801     }
0802 
0803     chan->conf_state = 0;
0804 
0805     if (locked)
0806         __l2cap_chan_add(conn, chan);
0807     else
0808         l2cap_chan_add(conn, chan);
0809 
0810     chan->remote_mps = chan->omtu;
0811     chan->mps = chan->omtu;
0812 
0813     chan->state = BT_CONNECTED;
0814 
0815     return chan;
0816 }
0817 
0818 /* AMP Manager functions */
0819 struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
0820 {
0821     BT_DBG("mgr %p orig refcnt %d", mgr, kref_read(&mgr->kref));
0822 
0823     kref_get(&mgr->kref);
0824 
0825     return mgr;
0826 }
0827 
0828 static void amp_mgr_destroy(struct kref *kref)
0829 {
0830     struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
0831 
0832     BT_DBG("mgr %p", mgr);
0833 
0834     mutex_lock(&amp_mgr_list_lock);
0835     list_del(&mgr->list);
0836     mutex_unlock(&amp_mgr_list_lock);
0837 
0838     amp_ctrl_list_flush(mgr);
0839     kfree(mgr);
0840 }
0841 
0842 int amp_mgr_put(struct amp_mgr *mgr)
0843 {
0844     BT_DBG("mgr %p orig refcnt %d", mgr, kref_read(&mgr->kref));
0845 
0846     return kref_put(&mgr->kref, &amp_mgr_destroy);
0847 }
0848 
0849 static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
0850 {
0851     struct amp_mgr *mgr;
0852     struct l2cap_chan *chan;
0853 
0854     mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
0855     if (!mgr)
0856         return NULL;
0857 
0858     BT_DBG("conn %p mgr %p", conn, mgr);
0859 
0860     mgr->l2cap_conn = conn;
0861 
0862     chan = a2mp_chan_open(conn, locked);
0863     if (!chan) {
0864         kfree(mgr);
0865         return NULL;
0866     }
0867 
0868     mgr->a2mp_chan = chan;
0869     chan->data = mgr;
0870 
0871     conn->hcon->amp_mgr = mgr;
0872 
0873     kref_init(&mgr->kref);
0874 
0875     /* Remote AMP ctrl list initialization */
0876     INIT_LIST_HEAD(&mgr->amp_ctrls);
0877     mutex_init(&mgr->amp_ctrls_lock);
0878 
0879     mutex_lock(&amp_mgr_list_lock);
0880     list_add(&mgr->list, &amp_mgr_list);
0881     mutex_unlock(&amp_mgr_list_lock);
0882 
0883     return mgr;
0884 }
0885 
0886 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
0887                        struct sk_buff *skb)
0888 {
0889     struct amp_mgr *mgr;
0890 
0891     if (conn->hcon->type != ACL_LINK)
0892         return NULL;
0893 
0894     mgr = amp_mgr_create(conn, false);
0895     if (!mgr) {
0896         BT_ERR("Could not create AMP manager");
0897         return NULL;
0898     }
0899 
0900     BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
0901 
0902     return mgr->a2mp_chan;
0903 }
0904 
0905 void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
0906 {
0907     struct amp_mgr *mgr;
0908     struct a2mp_info_rsp rsp;
0909 
0910     mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
0911     if (!mgr)
0912         return;
0913 
0914     BT_DBG("%s mgr %p", hdev->name, mgr);
0915 
0916     memset(&rsp, 0, sizeof(rsp));
0917 
0918     rsp.id = hdev->id;
0919     rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
0920 
0921     if (hdev->amp_type != AMP_TYPE_BREDR) {
0922         rsp.status = 0;
0923         rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
0924         rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
0925         rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
0926         rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
0927         rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
0928     }
0929 
0930     a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
0931     amp_mgr_put(mgr);
0932 }
0933 
0934 void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
0935 {
0936     struct amp_mgr *mgr;
0937     struct amp_assoc *loc_assoc = &hdev->loc_assoc;
0938     struct a2mp_amp_assoc_rsp *rsp;
0939     size_t len;
0940 
0941     mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
0942     if (!mgr)
0943         return;
0944 
0945     BT_DBG("%s mgr %p", hdev->name, mgr);
0946 
0947     len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
0948     rsp = kzalloc(len, GFP_KERNEL);
0949     if (!rsp) {
0950         amp_mgr_put(mgr);
0951         return;
0952     }
0953 
0954     rsp->id = hdev->id;
0955 
0956     if (status) {
0957         rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
0958     } else {
0959         rsp->status = A2MP_STATUS_SUCCESS;
0960         memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
0961     }
0962 
0963     a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
0964     amp_mgr_put(mgr);
0965     kfree(rsp);
0966 }
0967 
0968 void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
0969 {
0970     struct amp_mgr *mgr;
0971     struct amp_assoc *loc_assoc = &hdev->loc_assoc;
0972     struct a2mp_physlink_req *req;
0973     struct l2cap_chan *bredr_chan;
0974     size_t len;
0975 
0976     mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
0977     if (!mgr)
0978         return;
0979 
0980     len = sizeof(*req) + loc_assoc->len;
0981 
0982     BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
0983 
0984     req = kzalloc(len, GFP_KERNEL);
0985     if (!req) {
0986         amp_mgr_put(mgr);
0987         return;
0988     }
0989 
0990     bredr_chan = mgr->bredr_chan;
0991     if (!bredr_chan)
0992         goto clean;
0993 
0994     req->local_id = hdev->id;
0995     req->remote_id = bredr_chan->remote_amp_id;
0996     memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
0997 
0998     a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
0999 
1000 clean:
1001     amp_mgr_put(mgr);
1002     kfree(req);
1003 }
1004 
1005 void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
1006 {
1007     struct amp_mgr *mgr;
1008     struct a2mp_physlink_rsp rsp;
1009     struct hci_conn *hs_hcon;
1010 
1011     mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
1012     if (!mgr)
1013         return;
1014 
1015     memset(&rsp, 0, sizeof(rsp));
1016 
1017     hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
1018     if (!hs_hcon) {
1019         rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
1020     } else {
1021         rsp.remote_id = hs_hcon->remote_id;
1022         rsp.status = A2MP_STATUS_SUCCESS;
1023     }
1024 
1025     BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
1026            status);
1027 
1028     rsp.local_id = hdev->id;
1029     a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
1030     amp_mgr_put(mgr);
1031 }
1032 
1033 void a2mp_discover_amp(struct l2cap_chan *chan)
1034 {
1035     struct l2cap_conn *conn = chan->conn;
1036     struct amp_mgr *mgr = conn->hcon->amp_mgr;
1037     struct a2mp_discov_req req;
1038 
1039     BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
1040 
1041     if (!mgr) {
1042         mgr = amp_mgr_create(conn, true);
1043         if (!mgr)
1044             return;
1045     }
1046 
1047     mgr->bredr_chan = chan;
1048 
1049     memset(&req, 0, sizeof(req));
1050 
1051     req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1052     req.ext_feat = 0;
1053     a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
1054 }
[ 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!