[deliverable/linux.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)
		memcpy(skb_put(skb, len), buf, len);

	__net_timestamp(skb);

	hdr = (void *)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;
}

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;
	struct mgmt_hdr *hdr;

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

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(event);
	if (hdev)
		hdr->index = cpu_to_le16(hdev->id);
	else
		hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
	hdr->len = cpu_to_le16(data_len);

	if (data)
		memcpy(skb_put(skb, data_len), data, data_len);

	/* Time stamp */
	__net_timestamp(skb);

	hci_send_to_channel(channel, skb, flag, skip_sk);

	if (channel == HCI_CHANNEL_CONTROL)
		hci_send_monitor_ctrl_event(hdev, event, data, data_len,
					    skb_get_ktime(skb), flag, skip_sk);

	kfree_skb(skb);
	return 0;
}

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 = (void *) 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 = (void *) 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 = (void *) 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 = (void *) 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_add(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);

	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
185338ab MH	24	#include <asm/unaligned.h>
185338ab MH	25
a380b6cf JH	26	#include <net/bluetooth/bluetooth.h>
a380b6cf JH	27	#include <net/bluetooth/hci_core.h>
185338ab	28	#include <net/bluetooth/hci_mon.h>
a380b6cf JH	29	#include <net/bluetooth/mgmt.h>
	30
	31	#include "mgmt_util.h"
	32
185338ab 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)
	47	memcpy(skb_put(skb, len), buf, len);
	48
	49	__net_timestamp(skb);
	50
	51	hdr = (void *)skb_push(skb, HCI_MON_HDR_SIZE);
	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
a380b6cf JH	59	int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
	60	void data, u16 data_len, int flag, struct sock skip_sk)
	61	{
	62	struct sk_buff *skb;
	63	struct mgmt_hdr *hdr;
	64
	65	skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL);
	66	if (!skb)
	67	return -ENOMEM;
	68
	69	hdr = (void ) skb_put(skb, sizeof(hdr));
	70	hdr->opcode = cpu_to_le16(event);
	71	if (hdev)
	72	hdr->index = cpu_to_le16(hdev->id);
	73	else
	74	hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
	75	hdr->len = cpu_to_le16(data_len);
	76
	77	if (data)
	78	memcpy(skb_put(skb, data_len), data, data_len);
	79
	80	/* Time stamp */
	81	__net_timestamp(skb);
	82
	83	hci_send_to_channel(channel, skb, flag, skip_sk);
a380b6cf	84
185338ab MH	85	if (channel == HCI_CHANNEL_CONTROL)
	86	hci_send_monitor_ctrl_event(hdev, event, data, data_len,
	87	skb_get_ktime(skb), flag, skip_sk);
	88
	89	kfree_skb(skb);
a380b6cf JH	90	return 0;
	91	}
	92
	93	int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
	94	{
185338ab	95	struct sk_buff skb, mskb;
a380b6cf JH	96	struct mgmt_hdr *hdr;
	97	struct mgmt_ev_cmd_status *ev;
	98	int err;
	99
	100	BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
	101
	102	skb = alloc_skb(sizeof(hdr) + sizeof(ev), GFP_KERNEL);
	103	if (!skb)
	104	return -ENOMEM;
	105
	106	hdr = (void ) skb_put(skb, sizeof(hdr));
	107
	108	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
	109	hdr->index = cpu_to_le16(index);
	110	hdr->len = cpu_to_le16(sizeof(*ev));
	111
	112	ev = (void ) skb_put(skb, sizeof(ev));
	113	ev->status = status;
	114	ev->opcode = cpu_to_le16(cmd);
	115
185338ab MH	116	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
	117	MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
	118	if (mskb)
	119	skb->tstamp = mskb->tstamp;
	120	else
	121	__net_timestamp(skb);
	122
a380b6cf JH	123	err = sock_queue_rcv_skb(sk, skb);
	124	if (err < 0)
	125	kfree_skb(skb);
	126
185338ab MH	127	if (mskb) {
	128	hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
	129	HCI_SOCK_TRUSTED, NULL);
	130	kfree_skb(mskb);
	131	}
	132
a380b6cf JH	133	return err;
	134	}
	135
	136	int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
	137	void *rp, size_t rp_len)
	138	{
185338ab	139	struct sk_buff skb, mskb;
a380b6cf JH	140	struct mgmt_hdr *hdr;
	141	struct mgmt_ev_cmd_complete *ev;
	142	int err;
	143
	144	BT_DBG("sock %p", sk);
	145
	146	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + rp_len, GFP_KERNEL);
	147	if (!skb)
	148	return -ENOMEM;
	149
	150	hdr = (void ) skb_put(skb, sizeof(hdr));
	151
	152	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	153	hdr->index = cpu_to_le16(index);
	154	hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
	155
	156	ev = (void ) skb_put(skb, sizeof(ev) + rp_len);
	157	ev->opcode = cpu_to_le16(cmd);
	158	ev->status = status;
	159
	160	if (rp)
	161	memcpy(ev->data, rp, rp_len);
	162
185338ab MH	163	mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
	164	MGMT_EV_CMD_COMPLETE,
	165	sizeof(*ev) + rp_len, ev);
	166	if (mskb)
	167	skb->tstamp = mskb->tstamp;
	168	else
	169	__net_timestamp(skb);
	170
a380b6cf JH	171	err = sock_queue_rcv_skb(sk, skb);
	172	if (err < 0)
	173	kfree_skb(skb);
	174
185338ab MH	175	if (mskb) {
	176	hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
	177	HCI_SOCK_TRUSTED, NULL);
	178	kfree_skb(mskb);
	179	}
	180
a380b6cf JH	181	return err;
	182	}
	183
	184	struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
	185	struct hci_dev *hdev)
	186	{
	187	struct mgmt_pending_cmd *cmd;
	188
	189	list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
	190	if (hci_sock_get_channel(cmd->sk) != channel)
	191	continue;
	192	if (cmd->opcode == opcode)
	193	return cmd;
	194	}
	195
	196	return NULL;
	197	}
	198
	199	struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
	200	u16 opcode,
	201	struct hci_dev *hdev,
	202	const void *data)
	203	{
	204	struct mgmt_pending_cmd *cmd;
	205
	206	list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
	207	if (cmd->user_data != data)
	208	continue;
	209	if (cmd->opcode == opcode)
	210	return cmd;
	211	}
	212
	213	return NULL;
	214	}
	215
	216	void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
	217	void (cb)(struct mgmt_pending_cmd cmd, void *data),
	218	void *data)
	219	{
	220	struct mgmt_pending_cmd cmd, tmp;
	221
	222	list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
	223	if (opcode > 0 && cmd->opcode != opcode)
	224	continue;
	225
	226	cb(cmd, data);
	227	}
	228	}
	229
	230	struct mgmt_pending_cmd mgmt_pending_add(struct sock sk, u16 opcode,
	231	struct hci_dev *hdev,
	232	void *data, u16 len)
	233	{
	234	struct mgmt_pending_cmd *cmd;
	235
	236	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
	237	if (!cmd)
	238	return NULL;
	239
	240	cmd->opcode = opcode;
	241	cmd->index = hdev->id;
	242
	243	cmd->param = kmemdup(data, len, GFP_KERNEL);
	244	if (!cmd->param) {
245	kfree(cmd);
246	return NULL;
247	}
248
249	cmd->param_len = len;
250
251	cmd->sk = sk;
252	sock_hold(sk);
253
254	list_add(&cmd->list, &hdev->mgmt_pending);
255
256	return cmd;
257	}
258
259	void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
260	{
261	sock_put(cmd->sk);
262	kfree(cmd->param);
263	kfree(cmd);
264	}
265
266	void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
267	{
268	list_del(&cmd->list);
269	mgmt_pending_free(cmd);
270	}