cregit-Linux how code gets into the kernel

Release 4.11 net/bluetooth/a2mp.c

Directory: net/bluetooth
/*
   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
   Copyright (c) 2011,2012 Intel Corp.

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License version 2 and
   only version 2 as published by the Free Software Foundation.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
*/

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>

#include "hci_request.h"
#include "a2mp.h"
#include "amp.h"


#define A2MP_FEAT_EXT	0x8000

/* Global AMP Manager list */
static LIST_HEAD(amp_mgr_list);
static DEFINE_MUTEX(amp_mgr_list_lock);

/* A2MP build & send command helper functions */

static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data) { struct a2mp_cmd *cmd; int plen; plen = sizeof(*cmd) + len; cmd = kzalloc(plen, GFP_KERNEL); if (!cmd) return NULL; cmd->code = code; cmd->ident = ident; cmd->len = cpu_to_le16(len); memcpy(cmd->data, data, len); return cmd; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko91100.00%1100.00%
Total91100.00%1100.00%


static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data) { struct l2cap_chan *chan = mgr->a2mp_chan; struct a2mp_cmd *cmd; u16 total_len = len + sizeof(*cmd); struct kvec iv; struct msghdr msg; cmd = __a2mp_build(code, ident, len, data); if (!cmd) return; iv.iov_base = cmd; iv.iov_len = total_len; memset(&msg, 0, sizeof(msg)); iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, &iv, 1, total_len); l2cap_chan_send(chan, &msg, total_len); kfree(cmd); }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko12189.63%125.00%
Al Viro139.63%250.00%
Marcel Holtmann10.74%125.00%
Total135100.00%4100.00%


static u8 __next_ident(struct amp_mgr *mgr) { if (++mgr->ident == 0) mgr->ident = 1; return mgr->ident; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko3096.77%150.00%
Marcel Holtmann13.23%150.00%
Total31100.00%2100.00%


static struct amp_mgr *amp_mgr_lookup_by_state(u8 state) { struct amp_mgr *mgr; mutex_lock(&amp_mgr_list_lock); list_for_each_entry(mgr, &amp_mgr_list, list) { if (test_and_clear_bit(state, &mgr->state)) { amp_mgr_get(mgr); mutex_unlock(&amp_mgr_list_lock); return mgr; } } mutex_unlock(&amp_mgr_list_lock); return NULL; }

Contributors

PersonTokensPropCommitsCommitProp
Marcel Holtmann69100.00%1100.00%
Total69100.00%1100.00%

/* hci_dev_list shall be locked */
static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl) { struct hci_dev *hdev; int i = 1; cl[0].id = AMP_ID_BREDR; cl[0].type = AMP_TYPE_BREDR; cl[0].status = AMP_STATUS_BLUETOOTH_ONLY; list_for_each_entry(hdev, &hci_dev_list, list) { if (hdev->dev_type == HCI_AMP) { cl[i].id = hdev->id; cl[i].type = hdev->amp_type; if (test_bit(HCI_UP, &hdev->flags)) cl[i].status = hdev->amp_status; else cl[i].status = AMP_STATUS_POWERED_DOWN; i++; } } }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko7154.20%120.00%
Marcel Holtmann6045.80%480.00%
Total131100.00%5100.00%

/* Processing A2MP messages */
static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_cmd_rej *rej = (void *) skb->data; if (le16_to_cpu(hdr->len) < sizeof(*rej)) return -EINVAL; BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason)); skb_pull(skb, sizeof(*rej)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko83100.00%1100.00%
Total83100.00%1100.00%


static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_discov_req *req = (void *) skb->data; u16 len = le16_to_cpu(hdr->len); struct a2mp_discov_rsp *rsp; u16 ext_feat; u8 num_ctrl; struct hci_dev *hdev; if (len < sizeof(*req)) return -EINVAL; skb_pull(skb, sizeof(*req)); ext_feat = le16_to_cpu(req->ext_feat); BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat); /* check that packet is not broken for now */ while (ext_feat & A2MP_FEAT_EXT) { if (len < sizeof(ext_feat)) return -EINVAL; ext_feat = get_unaligned_le16(skb->data); BT_DBG("efm 0x%4.4x", ext_feat); len -= sizeof(ext_feat); skb_pull(skb, sizeof(ext_feat)); } read_lock(&hci_dev_list_lock); /* at minimum the BR/EDR needs to be listed */ num_ctrl = 1; list_for_each_entry(hdev, &hci_dev_list, list) { if (hdev->dev_type == HCI_AMP) num_ctrl++; } len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp); rsp = kmalloc(len, GFP_ATOMIC); if (!rsp) { read_unlock(&hci_dev_list_lock); return -ENOMEM; } rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); rsp->ext_feat = 0; __a2mp_add_cl(mgr, rsp->cl); read_unlock(&hci_dev_list_lock); a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp); kfree(rsp); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko26190.00%133.33%
Marcel Holtmann289.66%133.33%
Joe Perches10.34%133.33%
Total290100.00%3100.00%


static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_discov_rsp *rsp = (void *) skb->data; u16 len = le16_to_cpu(hdr->len); struct a2mp_cl *cl; u16 ext_feat; bool found = false; if (len < sizeof(*rsp)) return -EINVAL; len -= sizeof(*rsp); skb_pull(skb, sizeof(*rsp)); ext_feat = le16_to_cpu(rsp->ext_feat); BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat); /* check that packet is not broken for now */ while (ext_feat & A2MP_FEAT_EXT) { if (len < sizeof(ext_feat)) return -EINVAL; ext_feat = get_unaligned_le16(skb->data); BT_DBG("efm 0x%4.4x", ext_feat); len -= sizeof(ext_feat); skb_pull(skb, sizeof(ext_feat)); } cl = (void *) skb->data; while (len >= sizeof(*cl)) { BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type, cl->status); if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) { struct a2mp_info_req req; found = true; req.id = cl->id; a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr), sizeof(req), &req); } len -= sizeof(*cl); cl = (void *) skb_pull(skb, sizeof(*cl)); } /* Fall back to L2CAP init sequence */ if (!found) { struct l2cap_conn *conn = mgr->l2cap_conn; struct l2cap_chan *chan; mutex_lock(&conn->chan_lock); list_for_each_entry(chan, &conn->chan_l, list) { BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); if (chan->scid == L2CAP_CID_A2MP) continue; l2cap_chan_lock(chan); if (chan->state == BT_CONNECT) l2cap_send_conn_req(chan); l2cap_chan_unlock(chan); } mutex_unlock(&conn->chan_lock); } return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko37698.69%240.00%
Marcel Holtmann30.79%240.00%
Johan Hedberg20.52%120.00%
Total381100.00%5100.00%


static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_cl *cl = (void *) skb->data; while (skb->len >= sizeof(*cl)) { BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, cl->status); cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); } /* TODO send A2MP_CHANGE_RSP */ return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko87100.00%1100.00%
Total87100.00%1100.00%


static void read_local_amp_info_complete(struct hci_dev *hdev, u8 status, u16 opcode) { BT_DBG("%s status 0x%2.2x", hdev->name, status); a2mp_send_getinfo_rsp(hdev); }

Contributors

PersonTokensPropCommitsCommitProp
Arron Wang33100.00%1100.00%
Total33100.00%1100.00%


static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_info_req *req = (void *) skb->data; struct hci_dev *hdev; struct hci_request hreq; int err = 0; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("id %d", req->id); hdev = hci_dev_get(req->id); if (!hdev || hdev->dev_type != HCI_AMP) { struct a2mp_info_rsp rsp; rsp.id = req->id; rsp.status = A2MP_STATUS_INVALID_CTRL_ID; a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp); goto done; } set_bit(READ_LOC_AMP_INFO, &mgr->state); hci_req_init(&hreq, hdev); hci_req_add(&hreq, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL); err = hci_req_run(&hreq, read_local_amp_info_complete); if (err < 0) a2mp_send_getinfo_rsp(hdev); done: if (hdev) hci_dev_put(hdev); skb_pull(skb, sizeof(*req)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko17481.31%480.00%
Arron Wang4018.69%120.00%
Total214100.00%5100.00%


static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data; struct a2mp_amp_assoc_req req; struct amp_ctrl *ctrl; if (le16_to_cpu(hdr->len) < sizeof(*rsp)) return -EINVAL; BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status); if (rsp->status) return -EINVAL; ctrl = amp_ctrl_add(mgr, rsp->id); if (!ctrl) return -ENOMEM; req.id = rsp->id; a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req), &req); skb_pull(skb, sizeof(*rsp)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko148100.00%2100.00%
Total148100.00%2100.00%


static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_amp_assoc_req *req = (void *) skb->data; struct hci_dev *hdev; struct amp_mgr *tmp; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("id %d", req->id); /* Make sure that other request is not processed */ tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); hdev = hci_dev_get(req->id); if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) { struct a2mp_amp_assoc_rsp rsp; rsp.id = req->id; if (tmp) { rsp.status = A2MP_STATUS_COLLISION_OCCURED; amp_mgr_put(tmp); } else { rsp.status = A2MP_STATUS_INVALID_CTRL_ID; } a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), &rsp); goto done; } amp_read_loc_assoc(hdev, mgr); done: if (hdev) hci_dev_put(hdev); skb_pull(skb, sizeof(*req)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko19599.49%266.67%
Marcel Holtmann10.51%133.33%
Total196100.00%3100.00%


static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data; u16 len = le16_to_cpu(hdr->len); struct hci_dev *hdev; struct amp_ctrl *ctrl; struct hci_conn *hcon; size_t assoc_len; if (len < sizeof(*rsp)) return -EINVAL; assoc_len = len - sizeof(*rsp); BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status, assoc_len); if (rsp->status) return -EINVAL; /* Save remote ASSOC data */ ctrl = amp_ctrl_lookup(mgr, rsp->id); if (ctrl) { u8 *assoc; assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL); if (!assoc) { amp_ctrl_put(ctrl); return -ENOMEM; } ctrl->assoc = assoc; ctrl->assoc_len = assoc_len; ctrl->assoc_rem_len = assoc_len; ctrl->assoc_len_so_far = 0; amp_ctrl_put(ctrl); } /* Create Phys Link */ hdev = hci_dev_get(rsp->id); if (!hdev) return -EINVAL; hcon = phylink_add(hdev, mgr, rsp->id, true); if (!hcon) goto done; BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id); mgr->bredr_chan->remote_amp_id = rsp->id; amp_create_phylink(hdev, mgr, hcon); done: hci_dev_put(hdev); skb_pull(skb, len); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko27998.24%685.71%
Alexandru Gheorghiu51.76%114.29%
Total284100.00%7100.00%


static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_physlink_req *req = (void *) skb->data; struct a2mp_physlink_rsp rsp; struct hci_dev *hdev; struct hci_conn *hcon; struct amp_ctrl *ctrl; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); rsp.local_id = req->remote_id; rsp.remote_id = req->local_id; hdev = hci_dev_get(req->remote_id); if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) { rsp.status = A2MP_STATUS_INVALID_CTRL_ID; goto send_rsp; } ctrl = amp_ctrl_lookup(mgr, rsp.remote_id); if (!ctrl) { ctrl = amp_ctrl_add(mgr, rsp.remote_id); if (ctrl) { amp_ctrl_get(ctrl); } else { rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; goto send_rsp; } } if (ctrl) { size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req); u8 *assoc; assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL); if (!assoc) { amp_ctrl_put(ctrl); return -ENOMEM; } ctrl->assoc = assoc; ctrl->assoc_len = assoc_len; ctrl->assoc_rem_len = assoc_len; ctrl->assoc_len_so_far = 0; amp_ctrl_put(ctrl); } hcon = phylink_add(hdev, mgr, req->local_id, false); if (hcon) { amp_accept_phylink(hdev, mgr, hcon); rsp.status = A2MP_STATUS_SUCCESS; } else { rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; } send_rsp: if (hdev) hci_dev_put(hdev); /* Reply error now and success after HCI Write Remote AMP Assoc command complete with success status */ if (rsp.status != A2MP_STATUS_SUCCESS) { a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); } else { set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state); mgr->ident = hdr->ident; } skb_pull(skb, le16_to_cpu(hdr->len)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko38398.21%981.82%
Alexandru Gheorghiu51.28%19.09%
Marcel Holtmann20.51%19.09%
Total390100.00%11100.00%


static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { struct a2mp_physlink_req *req = (void *) skb->data; struct a2mp_physlink_rsp rsp; struct hci_dev *hdev; struct hci_conn *hcon; if (le16_to_cpu(hdr->len) < sizeof(*req)) return -EINVAL; BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); rsp.local_id = req->remote_id; rsp.remote_id = req->local_id; rsp.status = A2MP_STATUS_SUCCESS; hdev = hci_dev_get(req->remote_id); if (!hdev) { rsp.status = A2MP_STATUS_INVALID_CTRL_ID; goto send_rsp; } hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, &mgr->l2cap_conn->hcon->dst); if (!hcon) { BT_ERR("No phys link exist"); rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS; goto clean; } /* TODO Disconnect Phys Link here */ clean: hci_dev_put(hdev); send_rsp: a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); skb_pull(skb, sizeof(*req)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko20698.56%266.67%
Marcel Holtmann31.44%133.33%
Total209100.00%3100.00%


static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, struct a2mp_cmd *hdr) { BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code); skb_pull(skb, le16_to_cpu(hdr->len)); return 0; }

Contributors

PersonTokensPropCommitsCommitProp
Andrei Emeltchenko50100.00%2100.00%
Total50100.00%2100.00%

/* Handle A2MP signalling */
static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) { struct a2mp_cmd *hdr; struct amp_mgr *mgr = chan->data; int err = 0; amp_mgr_get(mgr); while (skb->len >= sizeof(*hdr)) { u16 len; hdr = (void *) skb->data; len = le16_to_cpu(hdr->len); BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len); skb_pull(skb, sizeof(*hdr)); if (len > skb->len || !hdr->ident) { err = -EINVAL; break; } mgr->ident = hdr->ident; switch (hdr->code) { case