Release 4.11 net/bluetooth/a2mp.c

/*
   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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
91
100.00%
1
100.00%
Total
91
100.00%
1
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
121
89.63%
1
25.00%
Al Viro
13
9.63%
2
50.00%
Marcel Holtmann
1
0.74%
1
25.00%
Total
135
100.00%
4
100.00%



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

	return mgr->ident;
}

Contributors
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
30
96.77%
1
50.00%
Marcel Holtmann
1
3.23%
1
50.00%
Total
31
100.00%
2
100.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
Person
Tokens
Prop
Commits
CommitProp
Marcel Holtmann
69
100.00%
1
100.00%
Total
69
100.00%
1
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
71
54.20%
1
20.00%
Marcel Holtmann
60
45.80%
4
80.00%
Total
131
100.00%
5
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
83
100.00%
1
100.00%
Total
83
100.00%
1
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
261
90.00%
1
33.33%
Marcel Holtmann
28
9.66%
1
33.33%
Joe Perches
1
0.34%
1
33.33%
Total
290
100.00%
3
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
376
98.69%
2
40.00%
Marcel Holtmann
3
0.79%
2
40.00%
Johan Hedberg
2
0.52%
1
20.00%
Total
381
100.00%
5
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
87
100.00%
1
100.00%
Total
87
100.00%
1
100.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
Person
Tokens
Prop
Commits
CommitProp
Arron Wang
33
100.00%
1
100.00%
Total
33
100.00%
1
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
174
81.31%
4
80.00%
Arron Wang
40
18.69%
1
20.00%
Total
214
100.00%
5
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
148
100.00%
2
100.00%
Total
148
100.00%
2
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
195
99.49%
2
66.67%
Marcel Holtmann
1
0.51%
1
33.33%
Total
196
100.00%
3
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
279
98.24%
6
85.71%
Alexandru Gheorghiu
5
1.76%
1
14.29%
Total
284
100.00%
7
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
383
98.21%
9
81.82%
Alexandru Gheorghiu
5
1.28%
1
9.09%
Marcel Holtmann
2
0.51%
1
9.09%
Total
390
100.00%
11
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
206
98.56%
2
66.67%
Marcel Holtmann
3
1.44%
1
33.33%
Total
209
100.00%
3
100.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
Person
Tokens
Prop
Commits
CommitProp
Andrei Emeltchenko
50
100.00%
2
100.00%
Total
50
100.00%
2
100.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