[linux-block.git] / net / bluetooth / mgmt_util.c

/*
   BlueZ - Bluetooth protocol stack for Linux

   Copyright (C) 2015  Intel Corporation

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

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
   IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
   CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
   ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
   OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
   SOFTWARE IS DISCLAIMED.
*/

#include <asm/unaligned.h>

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/hci_mon.h>
#include <net/bluetooth/mgmt.h>

#include "mgmt_util.h"

static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie,
						 u16 opcode, u16 len, void *buf)
{
	struct hci_mon_hdr *hdr;
	struct sk_buff *skb;

	skb = bt_skb_alloc(6 + len, GFP_ATOMIC);
	if (!skb)
		return NULL;

	put_unaligned_le32(cookie, skb_put(skb, 4));
	put_unaligned_le16(opcode, skb_put(skb, 2));

	if (buf)
		skb_put_data(skb, buf, len);

	__net_timestamp(skb);

	hdr = skb_push(skb, HCI_MON_HDR_SIZE);
	hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);
	hdr->index = index;
	hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);

	return skb;
}

struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode,
			       unsigned int size)
{
	struct sk_buff *skb;

	skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL);
	if (!skb)
		return skb;

	skb_reserve(skb, sizeof(struct mgmt_hdr));
	bt_cb(skb)->mgmt.hdev = hdev;
	bt_cb(skb)->mgmt.opcode = opcode;

	return skb;
}

int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag,
			struct sock *skip_sk)
{
	struct hci_dev *hdev;
	struct mgmt_hdr *hdr;
	int len = skb->len;

	if (!skb)
		return -EINVAL;

	hdev = bt_cb(skb)->mgmt.hdev;

	/* Time stamp */
	__net_timestamp(skb);

	/* Send just the data, without headers, to the monitor */
	if (channel == HCI_CHANNEL_CONTROL)
		hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode,
					    skb->data, skb->len,
					    skb_get_ktime(skb), flag, skip_sk);

	hdr = skb_push(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode);
	if (hdev)
		hdr->index = cpu_to_le16(hdev->id);
	else
		hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
	hdr->len = cpu_to_le16(len);

	hci_send_to_channel(channel, skb, flag, skip_sk);

	kfree_skb(skb);
	return 0;
}

int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
		    void *data, u16 data_len, int flag, struct sock *skip_sk)
{
	struct sk_buff *skb;

	skb = mgmt_alloc_skb(hdev, event, data_len);
	if (!skb)
		return -ENOMEM;

	if (data)
		skb_put_data(skb, data, data_len);

	return mgmt_send_event_skb(channel, skb, flag, skip_sk);
}

int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
{
	struct sk_buff *skb, *mskb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_status *ev;
	int err;

	BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);

	skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
	if (!skb)
		return -ENOMEM;

	hdr = skb_put(skb, sizeof(*hdr));

	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
	hdr->index = cpu_to_le16(index);
	hdr->len = cpu_to_le16(sizeof(*ev));

	ev = skb_put(skb, sizeof(*ev));
	ev->status = status;
	ev->opcode = cpu_to_le16(cmd);

	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
					 MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
	if (mskb)
		skb->tstamp = mskb->tstamp;
	else
		__net_timestamp(skb);

	err = sock_queue_rcv_skb(sk, skb);
	if (err < 0)
		kfree_skb(skb);

	if (mskb) {
		hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
				    HCI_SOCK_TRUSTED, NULL);
		kfree_skb(mskb);
	}

	return err;
}

int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
		      void *rp, size_t rp_len)
{
	struct sk_buff *skb, *mskb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_complete *ev;
	int err;

	BT_DBG("sock %p", sk);

	skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
	if (!skb)
		return -ENOMEM;

	hdr = skb_put(skb, sizeof(*hdr));

	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	hdr->index = cpu_to_le16(index);
	hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);

	ev = skb_put(skb, sizeof(*ev) + rp_len);
	ev->opcode = cpu_to_le16(cmd);
	ev->status = status;

	if (rp)
		memcpy(ev->data, rp, rp_len);

	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
					 MGMT_EV_CMD_COMPLETE,
					 sizeof(*ev) + rp_len, ev);
	if (mskb)
		skb->tstamp = mskb->tstamp;
	else
		__net_timestamp(skb);

	err = sock_queue_rcv_skb(sk, skb);
	if (err < 0)
		kfree_skb(skb);

	if (mskb) {
		hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
				    HCI_SOCK_TRUSTED, NULL);
		kfree_skb(mskb);
	}

	return err;
}

struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
					   struct hci_dev *hdev)
{
	struct mgmt_pending_cmd *cmd;

	list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
		if (hci_sock_get_channel(cmd->sk) != channel)
			continue;
		if (cmd->opcode == opcode)
			return cmd;
	}

	return NULL;
}

struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
						u16 opcode,
						struct hci_dev *hdev,
						const void *data)
{
	struct mgmt_pending_cmd *cmd;

	list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
		if (cmd->user_data != data)
			continue;
		if (cmd->opcode == opcode)
			return cmd;
	}

	return NULL;
}

void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
			  void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
			  void *data)
{
	struct mgmt_pending_cmd *cmd, *tmp;

	list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
		if (opcode > 0 && cmd->opcode != opcode)
			continue;

		cb(cmd, data);
	}
}

struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode,
					  struct hci_dev *hdev,
					  void *data, u16 len)
{
	struct mgmt_pending_cmd *cmd;

	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
	if (!cmd)
		return NULL;

	cmd->opcode = opcode;
	cmd->index = hdev->id;

	cmd->param = kmemdup(data, len, GFP_KERNEL);
	if (!cmd->param) {
		kfree(cmd);
		return NULL;
	}

	cmd->param_len = len;

	cmd->sk = sk;
	sock_hold(sk);

	return cmd;
}

struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
					  struct hci_dev *hdev,
					  void *data, u16 len)
{
	struct mgmt_pending_cmd *cmd;

	cmd = mgmt_pending_new(sk, opcode, hdev, data, len);
	if (!cmd)
		return NULL;

	list_add(&cmd->list, &hdev->mgmt_pending);

	return cmd;
}

void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
{
	sock_put(cmd->sk);
	kfree(cmd->param);
	kfree(cmd);
}

void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
{
	list_del(&cmd->list);
	mgmt_pending_free(cmd);
}
Commit	Line	Data
a380b6cf JH	1	/*
	2	BlueZ - Bluetooth protocol stack for Linux
	3
	4	Copyright (C) 2015 Intel Corporation
	5
	6	This program is free software; you can redistribute it and/or modify
	7	it under the terms of the GNU General Public License version 2 as
	8	published by the Free Software Foundation;
	9
	10	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
	11	OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	12	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
	13	IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
	14	CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
	15	WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
	16	ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
	17	OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
	18
	19	ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
	20	COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
	21	SOFTWARE IS DISCLAIMED.
	22	*/
	23
38ceaa00 MH	24	#include <asm/unaligned.h>
38ceaa00 MH	25
a380b6cf JH	26	#include <net/bluetooth/bluetooth.h>
a380b6cf JH	27	#include <net/bluetooth/hci_core.h>
38ceaa00	28	#include <net/bluetooth/hci_mon.h>
a380b6cf JH	29	#include <net/bluetooth/mgmt.h>
	30
	31	#include "mgmt_util.h"
	32
38ceaa00 MH	33	static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie,
	34	u16 opcode, u16 len, void *buf)
	35	{
	36	struct hci_mon_hdr *hdr;
	37	struct sk_buff *skb;
	38
	39	skb = bt_skb_alloc(6 + len, GFP_ATOMIC);
	40	if (!skb)
	41	return NULL;
	42
	43	put_unaligned_le32(cookie, skb_put(skb, 4));
	44	put_unaligned_le16(opcode, skb_put(skb, 2));
	45
	46	if (buf)
59ae1d12	47	skb_put_data(skb, buf, len);
38ceaa00 MH	48
	49	__net_timestamp(skb);
	50
d58ff351	51	hdr = skb_push(skb, HCI_MON_HDR_SIZE);
38ceaa00 MH	52	hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);
	53	hdr->index = index;
	54	hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);
	55
	56	return skb;
	57	}
	58
8aca46f9 LAD	59	struct sk_buff mgmt_alloc_skb(struct hci_dev hdev, u16 opcode,
8aca46f9 LAD	60	unsigned int size)
a380b6cf JH	61	{
a380b6cf JH	62	struct sk_buff *skb;
a380b6cf	63
8aca46f9	64	skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL);
a380b6cf	65	if (!skb)
8aca46f9	66	return skb;
a380b6cf	67
8aca46f9 LAD	68	skb_reserve(skb, sizeof(struct mgmt_hdr));
	69	bt_cb(skb)->mgmt.hdev = hdev;
	70	bt_cb(skb)->mgmt.opcode = opcode;
a380b6cf	71
8aca46f9 LAD	72	return skb;
	73	}
	74
	75	int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag,
	76	struct sock *skip_sk)
	77	{
	78	struct hci_dev *hdev;
	79	struct mgmt_hdr *hdr;
	80	int len = skb->len;
	81
	82	if (!skb)
	83	return -EINVAL;
	84
	85	hdev = bt_cb(skb)->mgmt.hdev;
a380b6cf JH	86
	87	/* Time stamp */
	88	__net_timestamp(skb);
	89
8aca46f9	90	/* Send just the data, without headers, to the monitor */
38ceaa00	91	if (channel == HCI_CHANNEL_CONTROL)
8aca46f9 LAD	92	hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode,
8aca46f9 LAD	93	skb->data, skb->len,
38ceaa00 MH	94	skb_get_ktime(skb), flag, skip_sk);
38ceaa00 MH	95
8aca46f9 LAD	96	hdr = skb_push(skb, sizeof(*hdr));
	97	hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode);
	98	if (hdev)
	99	hdr->index = cpu_to_le16(hdev->id);
	100	else
	101	hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
	102	hdr->len = cpu_to_le16(len);
	103
	104	hci_send_to_channel(channel, skb, flag, skip_sk);
	105
38ceaa00	106	kfree_skb(skb);
a380b6cf JH	107	return 0;
	108	}
	109
8aca46f9 LAD	110	int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
	111	void data, u16 data_len, int flag, struct sock skip_sk)
	112	{
	113	struct sk_buff *skb;
	114
	115	skb = mgmt_alloc_skb(hdev, event, data_len);
	116	if (!skb)
	117	return -ENOMEM;
	118
	119	if (data)
	120	skb_put_data(skb, data, data_len);
	121
	122	return mgmt_send_event_skb(channel, skb, flag, skip_sk);
	123	}
	124
a380b6cf JH	125	int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
a380b6cf JH	126	{
38ceaa00	127	struct sk_buff skb, mskb;
a380b6cf JH	128	struct mgmt_hdr *hdr;
	129	struct mgmt_ev_cmd_status *ev;
	130	int err;
	131
	132	BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
	133
	134	skb = alloc_skb(sizeof(hdr) + sizeof(ev), GFP_KERNEL);
	135	if (!skb)
	136	return -ENOMEM;
	137
4df864c1	138	hdr = skb_put(skb, sizeof(*hdr));
a380b6cf JH	139
	140	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
	141	hdr->index = cpu_to_le16(index);
	142	hdr->len = cpu_to_le16(sizeof(*ev));
	143
4df864c1	144	ev = skb_put(skb, sizeof(*ev));
a380b6cf JH	145	ev->status = status;
	146	ev->opcode = cpu_to_le16(cmd);
	147
38ceaa00 MH	148	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
	149	MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
	150	if (mskb)
	151	skb->tstamp = mskb->tstamp;
	152	else
	153	__net_timestamp(skb);
	154
a380b6cf JH	155	err = sock_queue_rcv_skb(sk, skb);
	156	if (err < 0)
	157	kfree_skb(skb);
	158
38ceaa00 MH	159	if (mskb) {
	160	hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
	161	HCI_SOCK_TRUSTED, NULL);
	162	kfree_skb(mskb);
	163	}
	164
a380b6cf JH	165	return err;
	166	}
	167
	168	int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
	169	void *rp, size_t rp_len)
	170	{
38ceaa00	171	struct sk_buff skb, mskb;
a380b6cf JH	172	struct mgmt_hdr *hdr;
	173	struct mgmt_ev_cmd_complete *ev;
	174	int err;
	175
	176	BT_DBG("sock %p", sk);
	177
	178	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + rp_len, GFP_KERNEL);
	179	if (!skb)
	180	return -ENOMEM;
	181
4df864c1	182	hdr = skb_put(skb, sizeof(*hdr));
a380b6cf JH	183
	184	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	185	hdr->index = cpu_to_le16(index);
	186	hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
	187
4df864c1	188	ev = skb_put(skb, sizeof(*ev) + rp_len);
a380b6cf JH	189	ev->opcode = cpu_to_le16(cmd);
	190	ev->status = status;
	191
	192	if (rp)
	193	memcpy(ev->data, rp, rp_len);
	194
38ceaa00 MH	195	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
	196	MGMT_EV_CMD_COMPLETE,
	197	sizeof(*ev) + rp_len, ev);
	198	if (mskb)
	199	skb->tstamp = mskb->tstamp;
	200	else
	201	__net_timestamp(skb);
	202
a380b6cf JH	203	err = sock_queue_rcv_skb(sk, skb);
	204	if (err < 0)
	205	kfree_skb(skb);
	206
38ceaa00 MH	207	if (mskb) {
	208	hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
	209	HCI_SOCK_TRUSTED, NULL);
	210	kfree_skb(mskb);
	211	}
	212
a380b6cf JH	213	return err;
	214	}
	215
	216	struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
	217	struct hci_dev *hdev)
	218	{
	219	struct mgmt_pending_cmd *cmd;
	220
	221	list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
	222	if (hci_sock_get_channel(cmd->sk) != channel)
	223	continue;
	224	if (cmd->opcode == opcode)
	225	return cmd;
	226	}
	227
	228	return NULL;
	229	}
	230
	231	struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
	232	u16 opcode,
	233	struct hci_dev *hdev,
	234	const void *data)
	235	{
	236	struct mgmt_pending_cmd *cmd;
	237
	238	list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
	239	if (cmd->user_data != data)
	240	continue;
	241	if (cmd->opcode == opcode)
	242	return cmd;
	243	}
	244
	245	return NULL;
	246	}
	247
	248	void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
	249	void (cb)(struct mgmt_pending_cmd cmd, void *data),
	250	void *data)
	251	{
	252	struct mgmt_pending_cmd cmd, tmp;
	253
	254	list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
	255	if (opcode > 0 && cmd->opcode != opcode)
	256	continue;
	257
	258	cb(cmd, data);
	259	}
	260	}
	261
161510cc	262	struct mgmt_pending_cmd mgmt_pending_new(struct sock sk, u16 opcode,
a380b6cf JH	263	struct hci_dev *hdev,
	264	void *data, u16 len)
	265	{
	266	struct mgmt_pending_cmd *cmd;
	267
	268	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
	269	if (!cmd)
	270	return NULL;
	271
	272	cmd->opcode = opcode;
	273	cmd->index = hdev->id;
	274
	275	cmd->param = kmemdup(data, len, GFP_KERNEL);
	276	if (!cmd->param) {
	277	kfree(cmd);
	278	return NULL;
	279	}
	280
	281	cmd->param_len = len;
	282
	283	cmd->sk = sk;
	284	sock_hold(sk);
	285
161510cc LAD	286	return cmd;
	287	}
	288
	289	struct mgmt_pending_cmd mgmt_pending_add(struct sock sk, u16 opcode,
	290	struct hci_dev *hdev,
	291	void *data, u16 len)
	292	{
	293	struct mgmt_pending_cmd *cmd;
	294
	295	cmd = mgmt_pending_new(sk, opcode, hdev, data, len);
	296	if (!cmd)
	297	return NULL;
	298
a380b6cf JH	299	list_add(&cmd->list, &hdev->mgmt_pending);
	300
	301	return cmd;
	302	}
	303
	304	void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
	305	{
	306	sock_put(cmd->sk);
	307	kfree(cmd->param);
	308	kfree(cmd);
	309	}
	310
	311	void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
	312	{
	313	list_del(&cmd->list);
	314	mgmt_pending_free(cmd);
	315	}