[mirror_ubuntu-artful-kernel.git] / net / bluetooth / mgmt.c

/*
   BlueZ - Bluetooth protocol stack for Linux
   Copyright (C) 2010  Nokia 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.
*/

/* Bluetooth HCI Management interface */

#include <asm/uaccess.h>
#include <asm/unaligned.h>

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

#define MGMT_VERSION	0
#define MGMT_REVISION	1

struct pending_cmd {
	struct list_head list;
	__u16 opcode;
	int index;
	void *cmd;
	struct sock *sk;
};

LIST_HEAD(cmd_list);

static int cmd_status(struct sock *sk, u16 cmd, u8 status)
{
	struct sk_buff *skb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_status *ev;

	BT_DBG("sock %p", sk);

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

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

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

	ev = (void *) skb_put(skb, sizeof(*ev));
	ev->status = status;
	put_unaligned_le16(cmd, &ev->opcode);

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

	return 0;
}

static int read_version(struct sock *sk)
{
	struct sk_buff *skb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_complete *ev;
	struct mgmt_rp_read_version *rp;

	BT_DBG("sock %p", sk);

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

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));

	ev = (void *) skb_put(skb, sizeof(*ev));
	put_unaligned_le16(MGMT_OP_READ_VERSION, &ev->opcode);

	rp = (void *) skb_put(skb, sizeof(*rp));
	rp->version = MGMT_VERSION;
	put_unaligned_le16(MGMT_REVISION, &rp->revision);

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

	return 0;
}

static int read_index_list(struct sock *sk)
{
	struct sk_buff *skb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_complete *ev;
	struct mgmt_rp_read_index_list *rp;
	struct list_head *p;
	size_t body_len;
	u16 count;
	int i;

	BT_DBG("sock %p", sk);

	read_lock(&hci_dev_list_lock);

	count = 0;
	list_for_each(p, &hci_dev_list) {
		count++;
	}

	body_len = sizeof(*ev) + sizeof(*rp) + (2 * count);
	skb = alloc_skb(sizeof(*hdr) + body_len, GFP_ATOMIC);
	if (!skb) {
		read_unlock(&hci_dev_list_lock);
		return -ENOMEM;
	}

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	hdr->len = cpu_to_le16(body_len);

	ev = (void *) skb_put(skb, sizeof(*ev));
	put_unaligned_le16(MGMT_OP_READ_INDEX_LIST, &ev->opcode);

	rp = (void *) skb_put(skb, sizeof(*rp) + (2 * count));
	put_unaligned_le16(count, &rp->num_controllers);

	i = 0;
	list_for_each(p, &hci_dev_list) {
		struct hci_dev *d = list_entry(p, struct hci_dev, list);

		hci_del_off_timer(d);

		if (test_bit(HCI_SETUP, &d->flags))
			continue;

		put_unaligned_le16(d->id, &rp->index[i++]);
		BT_DBG("Added hci%u", d->id);
	}

	read_unlock(&hci_dev_list_lock);

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

	return 0;
}

static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
{
	struct sk_buff *skb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_complete *ev;
	struct mgmt_rp_read_info *rp;
	struct mgmt_cp_read_info *cp;
	struct hci_dev *hdev;
	u16 dev_id;

	BT_DBG("sock %p", sk);

	if (len != 2)
		return cmd_status(sk, MGMT_OP_READ_INFO, EINVAL);

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

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));

	ev = (void *) skb_put(skb, sizeof(*ev));
	put_unaligned_le16(MGMT_OP_READ_INFO, &ev->opcode);

	rp = (void *) skb_put(skb, sizeof(*rp));

	cp = (void *) data;
	dev_id = get_unaligned_le16(&cp->index);

	BT_DBG("request for hci%u", dev_id);

	hdev = hci_dev_get(dev_id);
	if (!hdev) {
		kfree_skb(skb);
		return cmd_status(sk, MGMT_OP_READ_INFO, ENODEV);
	}

	hci_del_off_timer(hdev);

	hci_dev_lock_bh(hdev);

	put_unaligned_le16(hdev->id, &rp->index);
	rp->type = hdev->dev_type;

	rp->powered = test_bit(HCI_UP, &hdev->flags);
	rp->discoverable = test_bit(HCI_ISCAN, &hdev->flags);
	rp->pairable = test_bit(HCI_PSCAN, &hdev->flags);

	if (test_bit(HCI_AUTH, &hdev->flags))
		rp->sec_mode = 3;
	else if (hdev->ssp_mode > 0)
		rp->sec_mode = 4;
	else
		rp->sec_mode = 2;

	bacpy(&rp->bdaddr, &hdev->bdaddr);
	memcpy(rp->features, hdev->features, 8);
	memcpy(rp->dev_class, hdev->dev_class, 3);
	put_unaligned_le16(hdev->manufacturer, &rp->manufacturer);
	rp->hci_ver = hdev->hci_ver;
	put_unaligned_le16(hdev->hci_rev, &rp->hci_rev);

	hci_dev_unlock_bh(hdev);
	hci_dev_put(hdev);

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

	return 0;
}

static void mgmt_pending_free(struct pending_cmd *cmd)
{
	sock_put(cmd->sk);
	kfree(cmd->cmd);
	kfree(cmd);
}

static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
							void *data, u16 len)
{
	struct pending_cmd *cmd;

	cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
	if (!cmd)
		return -ENOMEM;

	cmd->opcode = opcode;
	cmd->index = index;

	cmd->cmd = kmalloc(len, GFP_ATOMIC);
	if (!cmd->cmd) {
		kfree(cmd);
		return -ENOMEM;
	}

	memcpy(cmd->cmd, data, len);

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

	list_add(&cmd->list, &cmd_list);

	return 0;
}

static void mgmt_pending_foreach(u16 opcode, int index,
				void (*cb)(struct pending_cmd *cmd, void *data),
				void *data)
{
	struct list_head *p, *n;

	list_for_each_safe(p, n, &cmd_list) {
		struct pending_cmd *cmd;

		cmd = list_entry(p, struct pending_cmd, list);

		if (cmd->opcode != opcode)
			continue;

		if (index >= 0 && cmd->index != index)
			continue;

		cb(cmd, data);
	}
}

static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
{
	struct list_head *p;

	list_for_each(p, &cmd_list) {
		struct pending_cmd *cmd;

		cmd = list_entry(p, struct pending_cmd, list);

		if (cmd->opcode != opcode)
			continue;

		if (index >= 0 && cmd->index != index)
			continue;

		return cmd;
	}

	return NULL;
}

static void mgmt_pending_remove(u16 opcode, int index)
{
	struct pending_cmd *cmd;

	cmd = mgmt_pending_find(opcode, index);
	if (cmd == NULL)
		return;

	list_del(&cmd->list);
	mgmt_pending_free(cmd);
}

static int set_powered(struct sock *sk, unsigned char *data, u16 len)
{
	struct mgmt_cp_set_powered *cp;
	struct hci_dev *hdev;
	u16 dev_id;
	int ret, up;

	cp = (void *) data;
	dev_id = get_unaligned_le16(&cp->index);

	BT_DBG("request for hci%u", dev_id);

	hdev = hci_dev_get(dev_id);
	if (!hdev)
		return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);

	hci_dev_lock_bh(hdev);

	up = test_bit(HCI_UP, &hdev->flags);
	if ((cp->powered && up) || (!cp->powered && !up)) {
		ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
		goto failed;
	}

	if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
		ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
		goto failed;
	}

	ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
	if (ret < 0)
		goto failed;

	if (cp->powered)
		queue_work(hdev->workqueue, &hdev->power_on);
	else
		queue_work(hdev->workqueue, &hdev->power_off);

	ret = 0;

failed:
	hci_dev_unlock_bh(hdev);
	hci_dev_put(hdev);
	return ret;
}

static int set_discoverable(struct sock *sk, unsigned char *data, u16 len)
{
	struct mgmt_cp_set_discoverable *cp;
	struct hci_dev *hdev;
	u16 dev_id;
	u8 scan;
	int err;

	cp = (void *) data;
	dev_id = get_unaligned_le16(&cp->index);

	BT_DBG("request for hci%u", dev_id);

	hdev = hci_dev_get(dev_id);
	if (!hdev)
		return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);

	hci_dev_lock_bh(hdev);

	if (!test_bit(HCI_UP, &hdev->flags)) {
		err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
		goto failed;
	}

	if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) ||
			mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id) ||
			hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE)) {
		err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EBUSY);
		goto failed;
	}

	if (cp->discoverable == test_bit(HCI_ISCAN, &hdev->flags) &&
					test_bit(HCI_PSCAN, &hdev->flags)) {
		err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EALREADY);
		goto failed;
	}

	err = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, dev_id, data, len);
	if (err < 0)
		goto failed;

	scan = SCAN_PAGE;

	if (cp->discoverable)
		scan |= SCAN_INQUIRY;

	err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
	if (err < 0)
		mgmt_pending_remove(MGMT_OP_SET_DISCOVERABLE, dev_id);

failed:
	hci_dev_unlock_bh(hdev);
	hci_dev_put(hdev);

	return err;
}

int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
{
	unsigned char *buf;
	struct mgmt_hdr *hdr;
	u16 opcode, len;
	int err;

	BT_DBG("got %zu bytes", msglen);

	if (msglen < sizeof(*hdr))
		return -EINVAL;

	buf = kmalloc(msglen, GFP_ATOMIC);
	if (!buf)
		return -ENOMEM;

	if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
		err = -EFAULT;
		goto done;
	}

	hdr = (struct mgmt_hdr *) buf;
	opcode = get_unaligned_le16(&hdr->opcode);
	len = get_unaligned_le16(&hdr->len);

	if (len != msglen - sizeof(*hdr)) {
		err = -EINVAL;
		goto done;
	}

	switch (opcode) {
	case MGMT_OP_READ_VERSION:
		err = read_version(sk);
		break;
	case MGMT_OP_READ_INDEX_LIST:
		err = read_index_list(sk);
		break;
	case MGMT_OP_READ_INFO:
		err = read_controller_info(sk, buf + sizeof(*hdr), len);
		break;
	case MGMT_OP_SET_POWERED:
		err = set_powered(sk, buf + sizeof(*hdr), len);
		break;
	case MGMT_OP_SET_DISCOVERABLE:
		err = set_discoverable(sk, buf + sizeof(*hdr), len);
		break;
	default:
		BT_DBG("Unknown op %u", opcode);
		err = cmd_status(sk, opcode, 0x01);
		break;
	}

	if (err < 0)
		goto done;

	err = msglen;

done:
	kfree(buf);
	return err;
}

static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
{
	struct sk_buff *skb;
	struct mgmt_hdr *hdr;

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

	bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(event);
	hdr->len = cpu_to_le16(data_len);

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

	hci_send_to_sock(NULL, skb, skip_sk);
	kfree_skb(skb);

	return 0;
}

int mgmt_index_added(u16 index)
{
	struct mgmt_ev_index_added ev;

	put_unaligned_le16(index, &ev.index);

	return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
}

int mgmt_index_removed(u16 index)
{
	struct mgmt_ev_index_added ev;

	put_unaligned_le16(index, &ev.index);

	return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
}

struct cmd_lookup {
	u8 value;
	struct sock *sk;
};

static void power_rsp(struct pending_cmd *cmd, void *data)
{
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_complete *ev;
	struct mgmt_rp_set_powered *rp;
	struct mgmt_cp_set_powered *cp = cmd->cmd;
	struct sk_buff *skb;
	struct cmd_lookup *match = data;

	if (cp->powered != match->value)
		return;

	skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
	if (!skb)
		return;

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));

	ev = (void *) skb_put(skb, sizeof(*ev));
	put_unaligned_le16(cmd->opcode, &ev->opcode);

	rp = (void *) skb_put(skb, sizeof(*rp));
	put_unaligned_le16(cmd->index, &rp->index);
	rp->powered = cp->powered;

	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
		kfree_skb(skb);

	list_del(&cmd->list);

	if (match->sk == NULL) {
		match->sk = cmd->sk;
		sock_hold(match->sk);
	}

	mgmt_pending_free(cmd);
}

int mgmt_powered(u16 index, u8 powered)
{
	struct mgmt_ev_powered ev;
	struct cmd_lookup match = { powered, NULL };
	int ret;

	put_unaligned_le16(index, &ev.index);
	ev.powered = powered;

	mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, power_rsp, &match);

	ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);

	if (match.sk)
		sock_put(match.sk);

	return ret;
}

static void discoverable_rsp(struct pending_cmd *cmd, void *data)
{
	struct mgmt_cp_set_discoverable *cp = cmd->cmd;
	struct cmd_lookup *match = data;
	struct sk_buff *skb;
	struct mgmt_hdr *hdr;
	struct mgmt_ev_cmd_complete *ev;
	struct mgmt_rp_set_discoverable *rp;

	if (cp->discoverable != match->value)
		return;

	skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
	if (!skb)
		return;

	hdr = (void *) skb_put(skb, sizeof(*hdr));
	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));

	ev = (void *) skb_put(skb, sizeof(*ev));
	put_unaligned_le16(MGMT_OP_SET_DISCOVERABLE, &ev->opcode);

	rp = (void *) skb_put(skb, sizeof(*rp));
	put_unaligned_le16(cmd->index, &rp->index);
	rp->discoverable = cp->discoverable;

	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
		kfree_skb(skb);

	list_del(&cmd->list);

	if (match->sk == NULL) {
		match->sk = cmd->sk;
		sock_hold(match->sk);
	}

	mgmt_pending_free(cmd);
}

int mgmt_discoverable(u16 index, u8 discoverable)
{
	struct mgmt_ev_discoverable ev;
	struct cmd_lookup match = { discoverable, NULL };
	int ret;

	put_unaligned_le16(index, &ev.index);
	ev.discoverable = discoverable;

	mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
						discoverable_rsp, &match);

	ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);

	if (match.sk)
		sock_put(match.sk);

	return ret;
}
Commit	Line	Data
0381101f JH	1	/*
	2	BlueZ - Bluetooth protocol stack for Linux
	3	Copyright (C) 2010 Nokia Corporation
	4
	5	This program is free software; you can redistribute it and/or modify
	6	it under the terms of the GNU General Public License version 2 as
	7	published by the Free Software Foundation;
	8
	9	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
	10	OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	11	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
	12	IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
	13	CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
	14	WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
	15	ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
	16	OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
	17
	18	ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
	19	COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
	20	SOFTWARE IS DISCLAIMED.
	21	*/
	22
	23	/* Bluetooth HCI Management interface */
	24
	25	#include <asm/uaccess.h>
	26	#include <asm/unaligned.h>
	27
	28	#include <net/bluetooth/bluetooth.h>
	29	#include <net/bluetooth/hci_core.h>
	30	#include <net/bluetooth/mgmt.h>
	31
02d98129 JH	32	#define MGMT_VERSION 0
	33	#define MGMT_REVISION 1
	34
eec8d2bc JH	35	struct pending_cmd {
	36	struct list_head list;
	37	__u16 opcode;
	38	int index;
	39	void *cmd;
	40	struct sock *sk;
	41	};
	42
	43	LIST_HEAD(cmd_list);
	44
f7b64e69 JH	45	static int cmd_status(struct sock *sk, u16 cmd, u8 status)
	46	{
	47	struct sk_buff *skb;
	48	struct mgmt_hdr *hdr;
	49	struct mgmt_ev_cmd_status *ev;
	50
	51	BT_DBG("sock %p", sk);
	52
	53	skb = alloc_skb(sizeof(hdr) + sizeof(ev), GFP_ATOMIC);
	54	if (!skb)
	55	return -ENOMEM;
	56
	57	hdr = (void ) skb_put(skb, sizeof(hdr));
	58
	59	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
	60	hdr->len = cpu_to_le16(sizeof(*ev));
	61
	62	ev = (void ) skb_put(skb, sizeof(ev));
	63	ev->status = status;
	64	put_unaligned_le16(cmd, &ev->opcode);
	65
	66	if (sock_queue_rcv_skb(sk, skb) < 0)
	67	kfree_skb(skb);
	68
	69	return 0;
	70	}
	71
02d98129 JH	72	static int read_version(struct sock *sk)
	73	{
	74	struct sk_buff *skb;
	75	struct mgmt_hdr *hdr;
	76	struct mgmt_ev_cmd_complete *ev;
	77	struct mgmt_rp_read_version *rp;
	78
	79	BT_DBG("sock %p", sk);
	80
	81	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
	82	if (!skb)
	83	return -ENOMEM;
	84
	85	hdr = (void ) skb_put(skb, sizeof(hdr));
	86	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	87	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
	88
	89	ev = (void ) skb_put(skb, sizeof(ev));
	90	put_unaligned_le16(MGMT_OP_READ_VERSION, &ev->opcode);
	91
	92	rp = (void ) skb_put(skb, sizeof(rp));
	93	rp->version = MGMT_VERSION;
	94	put_unaligned_le16(MGMT_REVISION, &rp->revision);
	95
	96	if (sock_queue_rcv_skb(sk, skb) < 0)
	97	kfree_skb(skb);
	98
	99	return 0;
	100	}
	101
faba42eb JH	102	static int read_index_list(struct sock *sk)
	103	{
	104	struct sk_buff *skb;
	105	struct mgmt_hdr *hdr;
	106	struct mgmt_ev_cmd_complete *ev;
	107	struct mgmt_rp_read_index_list *rp;
	108	struct list_head *p;
	109	size_t body_len;
	110	u16 count;
	111	int i;
	112
	113	BT_DBG("sock %p", sk);
	114
	115	read_lock(&hci_dev_list_lock);
	116
	117	count = 0;
	118	list_for_each(p, &hci_dev_list) {
	119	count++;
	120	}
	121
	122	body_len = sizeof(ev) + sizeof(rp) + (2 * count);
	123	skb = alloc_skb(sizeof(*hdr) + body_len, GFP_ATOMIC);
b2c60d42 JJ	124	if (!skb) {
b2c60d42 JJ	125	read_unlock(&hci_dev_list_lock);
faba42eb	126	return -ENOMEM;
b2c60d42	127	}
faba42eb JH	128
	129	hdr = (void ) skb_put(skb, sizeof(hdr));
	130	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	131	hdr->len = cpu_to_le16(body_len);
	132
	133	ev = (void ) skb_put(skb, sizeof(ev));
	134	put_unaligned_le16(MGMT_OP_READ_INDEX_LIST, &ev->opcode);
	135
	136	rp = (void ) skb_put(skb, sizeof(rp) + (2 * count));
	137	put_unaligned_le16(count, &rp->num_controllers);
	138
	139	i = 0;
	140	list_for_each(p, &hci_dev_list) {
	141	struct hci_dev *d = list_entry(p, struct hci_dev, list);
ab81cbf9 JH	142
	143	hci_del_off_timer(d);
	144
	145	if (test_bit(HCI_SETUP, &d->flags))
	146	continue;
	147
faba42eb JH	148	put_unaligned_le16(d->id, &rp->index[i++]);
	149	BT_DBG("Added hci%u", d->id);
	150	}
	151
	152	read_unlock(&hci_dev_list_lock);
	153
	154	if (sock_queue_rcv_skb(sk, skb) < 0)
	155	kfree_skb(skb);
	156
	157	return 0;
	158	}
	159
f7b64e69	160	static int read_controller_info(struct sock sk, unsigned char data, u16 len)
0381101f JH	161	{
	162	struct sk_buff *skb;
	163	struct mgmt_hdr *hdr;
f7b64e69 JH	164	struct mgmt_ev_cmd_complete *ev;
	165	struct mgmt_rp_read_info *rp;
	166	struct mgmt_cp_read_info *cp;
	167	struct hci_dev *hdev;
	168	u16 dev_id;
0381101f JH	169
	170	BT_DBG("sock %p", sk);
	171
f7b64e69 JH	172	if (len != 2)
	173	return cmd_status(sk, MGMT_OP_READ_INFO, EINVAL);
	174
	175	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
0381101f	176	if (!skb)
e41d8b4e	177	return -ENOMEM;
0381101f JH	178
0381101f JH	179	hdr = (void ) skb_put(skb, sizeof(hdr));
f7b64e69 JH	180	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
f7b64e69 JH	181	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
0381101f JH	182
0381101f JH	183	ev = (void ) skb_put(skb, sizeof(ev));
f7b64e69 JH	184	put_unaligned_le16(MGMT_OP_READ_INFO, &ev->opcode);
	185
	186	rp = (void ) skb_put(skb, sizeof(rp));
	187
	188	cp = (void *) data;
	189	dev_id = get_unaligned_le16(&cp->index);
	190
	191	BT_DBG("request for hci%u", dev_id);
	192
	193	hdev = hci_dev_get(dev_id);
	194	if (!hdev) {
	195	kfree_skb(skb);
	196	return cmd_status(sk, MGMT_OP_READ_INFO, ENODEV);
	197	}
	198
ab81cbf9 JH	199	hci_del_off_timer(hdev);
ab81cbf9 JH	200
f7b64e69 JH	201	hci_dev_lock_bh(hdev);
	202
	203	put_unaligned_le16(hdev->id, &rp->index);
	204	rp->type = hdev->dev_type;
	205
	206	rp->powered = test_bit(HCI_UP, &hdev->flags);
	207	rp->discoverable = test_bit(HCI_ISCAN, &hdev->flags);
	208	rp->pairable = test_bit(HCI_PSCAN, &hdev->flags);
	209
	210	if (test_bit(HCI_AUTH, &hdev->flags))
	211	rp->sec_mode = 3;
	212	else if (hdev->ssp_mode > 0)
	213	rp->sec_mode = 4;
	214	else
	215	rp->sec_mode = 2;
	216
	217	bacpy(&rp->bdaddr, &hdev->bdaddr);
	218	memcpy(rp->features, hdev->features, 8);
	219	memcpy(rp->dev_class, hdev->dev_class, 3);
	220	put_unaligned_le16(hdev->manufacturer, &rp->manufacturer);
	221	rp->hci_ver = hdev->hci_ver;
	222	put_unaligned_le16(hdev->hci_rev, &rp->hci_rev);
	223
	224	hci_dev_unlock_bh(hdev);
	225	hci_dev_put(hdev);
0381101f JH	226
	227	if (sock_queue_rcv_skb(sk, skb) < 0)
	228	kfree_skb(skb);
e41d8b4e JH	229
e41d8b4e JH	230	return 0;
0381101f JH	231	}
0381101f JH	232
eec8d2bc JH	233	static void mgmt_pending_free(struct pending_cmd *cmd)
	234	{
	235	sock_put(cmd->sk);
	236	kfree(cmd->cmd);
	237	kfree(cmd);
	238	}
	239
	240	static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
	241	void *data, u16 len)
	242	{
	243	struct pending_cmd *cmd;
	244
	245	cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
	246	if (!cmd)
	247	return -ENOMEM;
	248
	249	cmd->opcode = opcode;
	250	cmd->index = index;
	251
	252	cmd->cmd = kmalloc(len, GFP_ATOMIC);
	253	if (!cmd->cmd) {
	254	kfree(cmd);
	255	return -ENOMEM;
	256	}
	257
	258	memcpy(cmd->cmd, data, len);
	259
	260	cmd->sk = sk;
	261	sock_hold(sk);
	262
	263	list_add(&cmd->list, &cmd_list);
	264
	265	return 0;
	266	}
	267
	268	static void mgmt_pending_foreach(u16 opcode, int index,
	269	void (cb)(struct pending_cmd cmd, void *data),
	270	void *data)
	271	{
	272	struct list_head p, n;
	273
	274	list_for_each_safe(p, n, &cmd_list) {
	275	struct pending_cmd *cmd;
	276
	277	cmd = list_entry(p, struct pending_cmd, list);
	278
	279	if (cmd->opcode != opcode)
	280	continue;
	281
	282	if (index >= 0 && cmd->index != index)
	283	continue;
	284
	285	cb(cmd, data);
	286	}
	287	}
	288
	289	static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
	290	{
	291	struct list_head *p;
	292
	293	list_for_each(p, &cmd_list) {
	294	struct pending_cmd *cmd;
	295
	296	cmd = list_entry(p, struct pending_cmd, list);
297
298	if (cmd->opcode != opcode)
299	continue;
300
301	if (index >= 0 && cmd->index != index)
302	continue;
303
304	return cmd;
305	}
306
307	return NULL;
308	}
309
73f22f62 JH	310	static void mgmt_pending_remove(u16 opcode, int index)
	311	{
	312	struct pending_cmd *cmd;
	313
	314	cmd = mgmt_pending_find(opcode, index);
	315	if (cmd == NULL)
	316	return;
	317
	318	list_del(&cmd->list);
	319	mgmt_pending_free(cmd);
	320	}
	321
eec8d2bc JH	322	static int set_powered(struct sock sk, unsigned char data, u16 len)
	323	{
	324	struct mgmt_cp_set_powered *cp;
	325	struct hci_dev *hdev;
	326	u16 dev_id;
	327	int ret, up;
	328
	329	cp = (void *) data;
	330	dev_id = get_unaligned_le16(&cp->index);
	331
	332	BT_DBG("request for hci%u", dev_id);
	333
	334	hdev = hci_dev_get(dev_id);
	335	if (!hdev)
	336	return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
	337
	338	hci_dev_lock_bh(hdev);
	339
	340	up = test_bit(HCI_UP, &hdev->flags);
	341	if ((cp->powered && up) \|\| (!cp->powered && !up)) {
	342	ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
	343	goto failed;
	344	}
	345
	346	if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
	347	ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
	348	goto failed;
	349	}
	350
	351	ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
	352	if (ret < 0)
	353	goto failed;
	354
	355	if (cp->powered)
	356	queue_work(hdev->workqueue, &hdev->power_on);
	357	else
	358	queue_work(hdev->workqueue, &hdev->power_off);
	359
	360	ret = 0;
	361
	362	failed:
	363	hci_dev_unlock_bh(hdev);
	364	hci_dev_put(hdev);
	365	return ret;
	366	}
	367
73f22f62 JH	368	static int set_discoverable(struct sock sk, unsigned char data, u16 len)
	369	{
	370	struct mgmt_cp_set_discoverable *cp;
	371	struct hci_dev *hdev;
	372	u16 dev_id;
	373	u8 scan;
	374	int err;
	375
	376	cp = (void *) data;
	377	dev_id = get_unaligned_le16(&cp->index);
	378
	379	BT_DBG("request for hci%u", dev_id);
	380
	381	hdev = hci_dev_get(dev_id);
	382	if (!hdev)
	383	return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);
	384
	385	hci_dev_lock_bh(hdev);
	386
	387	if (!test_bit(HCI_UP, &hdev->flags)) {
	388	err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
	389	goto failed;
	390	}
	391
	392	if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) \|\|
	393	mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id) \|\|
	394	hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE)) {
	395	err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EBUSY);
	396	goto failed;
	397	}
	398
	399	if (cp->discoverable == test_bit(HCI_ISCAN, &hdev->flags) &&
	400	test_bit(HCI_PSCAN, &hdev->flags)) {
	401	err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EALREADY);
	402	goto failed;
	403	}
	404
	405	err = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, dev_id, data, len);
	406	if (err < 0)
	407	goto failed;
	408
	409	scan = SCAN_PAGE;
	410
	411	if (cp->discoverable)
	412	scan \|= SCAN_INQUIRY;
	413
	414	err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
	415	if (err < 0)
	416	mgmt_pending_remove(MGMT_OP_SET_DISCOVERABLE, dev_id);
	417
	418	failed:
	419	hci_dev_unlock_bh(hdev);
	420	hci_dev_put(hdev);
	421
	422	return err;
	423	}
	424
0381101f JH	425	int mgmt_control(struct sock sk, struct msghdr msg, size_t msglen)
	426	{
	427	unsigned char *buf;
	428	struct mgmt_hdr *hdr;
	429	u16 opcode, len;
	430	int err;
	431
	432	BT_DBG("got %zu bytes", msglen);
	433
	434	if (msglen < sizeof(*hdr))
	435	return -EINVAL;
	436
	437	buf = kmalloc(msglen, GFP_ATOMIC);
	438	if (!buf)
	439	return -ENOMEM;
	440
	441	if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
	442	err = -EFAULT;
	443	goto done;
	444	}
	445
	446	hdr = (struct mgmt_hdr *) buf;
	447	opcode = get_unaligned_le16(&hdr->opcode);
	448	len = get_unaligned_le16(&hdr->len);
	449
	450	if (len != msglen - sizeof(*hdr)) {
	451	err = -EINVAL;
	452	goto done;
	453	}
	454
	455	switch (opcode) {
02d98129 JH	456	case MGMT_OP_READ_VERSION:
	457	err = read_version(sk);
	458	break;
faba42eb JH	459	case MGMT_OP_READ_INDEX_LIST:
	460	err = read_index_list(sk);
	461	break;
f7b64e69 JH	462	case MGMT_OP_READ_INFO:
	463	err = read_controller_info(sk, buf + sizeof(*hdr), len);
	464	break;
eec8d2bc JH	465	case MGMT_OP_SET_POWERED:
	466	err = set_powered(sk, buf + sizeof(*hdr), len);
	467	break;
73f22f62 JH	468	case MGMT_OP_SET_DISCOVERABLE:
	469	err = set_discoverable(sk, buf + sizeof(*hdr), len);
	470	break;
0381101f JH	471	default:
0381101f JH	472	BT_DBG("Unknown op %u", opcode);
e41d8b4e	473	err = cmd_status(sk, opcode, 0x01);
0381101f JH	474	break;
	475	}
	476
e41d8b4e JH	477	if (err < 0)
	478	goto done;
	479
0381101f JH	480	err = msglen;
	481
	482	done:
	483	kfree(buf);
	484	return err;
	485	}
c71e97bf	486
eec8d2bc	487	static int mgmt_event(u16 event, void data, u16 data_len, struct sock skip_sk)
c71e97bf JH	488	{
	489	struct sk_buff *skb;
	490	struct mgmt_hdr *hdr;
	491
	492	skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
	493	if (!skb)
	494	return -ENOMEM;
	495
	496	bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
	497
	498	hdr = (void ) skb_put(skb, sizeof(hdr));
	499	hdr->opcode = cpu_to_le16(event);
	500	hdr->len = cpu_to_le16(data_len);
	501
	502	memcpy(skb_put(skb, data_len), data, data_len);
	503
eec8d2bc	504	hci_send_to_sock(NULL, skb, skip_sk);
c71e97bf JH	505	kfree_skb(skb);
	506
	507	return 0;
	508	}
	509
	510	int mgmt_index_added(u16 index)
	511	{
	512	struct mgmt_ev_index_added ev;
	513
	514	put_unaligned_le16(index, &ev.index);
	515
eec8d2bc	516	return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
c71e97bf JH	517	}
	518
	519	int mgmt_index_removed(u16 index)
	520	{
	521	struct mgmt_ev_index_added ev;
	522
	523	put_unaligned_le16(index, &ev.index);
	524
eec8d2bc JH	525	return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
	526	}
	527
73f22f62 JH	528	struct cmd_lookup {
73f22f62 JH	529	u8 value;
eec8d2bc JH	530	struct sock *sk;
	531	};
	532
	533	static void power_rsp(struct pending_cmd cmd, void data)
	534	{
	535	struct mgmt_hdr *hdr;
	536	struct mgmt_ev_cmd_complete *ev;
	537	struct mgmt_rp_set_powered *rp;
	538	struct mgmt_cp_set_powered *cp = cmd->cmd;
	539	struct sk_buff *skb;
73f22f62	540	struct cmd_lookup *match = data;
eec8d2bc	541
73f22f62	542	if (cp->powered != match->value)
eec8d2bc JH	543	return;
	544
	545	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
	546	if (!skb)
	547	return;
	548
	549	hdr = (void ) skb_put(skb, sizeof(hdr));
	550	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	551	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
	552
	553	ev = (void ) skb_put(skb, sizeof(ev));
	554	put_unaligned_le16(cmd->opcode, &ev->opcode);
	555
	556	rp = (void ) skb_put(skb, sizeof(rp));
	557	put_unaligned_le16(cmd->index, &rp->index);
	558	rp->powered = cp->powered;
	559
	560	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
	561	kfree_skb(skb);
	562
	563	list_del(&cmd->list);
	564
	565	if (match->sk == NULL) {
	566	match->sk = cmd->sk;
	567	sock_hold(match->sk);
	568	}
	569
	570	mgmt_pending_free(cmd);
c71e97bf	571	}
5add6af8 JH	572
	573	int mgmt_powered(u16 index, u8 powered)
	574	{
	575	struct mgmt_ev_powered ev;
73f22f62	576	struct cmd_lookup match = { powered, NULL };
eec8d2bc	577	int ret;
5add6af8 JH	578
	579	put_unaligned_le16(index, &ev.index);
	580	ev.powered = powered;
	581
eec8d2bc JH	582	mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, power_rsp, &match);
	583
	584	ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
	585
	586	if (match.sk)
	587	sock_put(match.sk);
	588
	589	return ret;
5add6af8	590	}
73f22f62 JH	591
	592	static void discoverable_rsp(struct pending_cmd cmd, void data)
	593	{
	594	struct mgmt_cp_set_discoverable *cp = cmd->cmd;
	595	struct cmd_lookup *match = data;
	596	struct sk_buff *skb;
	597	struct mgmt_hdr *hdr;
	598	struct mgmt_ev_cmd_complete *ev;
	599	struct mgmt_rp_set_discoverable *rp;
	600
	601	if (cp->discoverable != match->value)
	602	return;
	603
	604	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
	605	if (!skb)
	606	return;
	607
	608	hdr = (void ) skb_put(skb, sizeof(hdr));
	609	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	610	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
	611
	612	ev = (void ) skb_put(skb, sizeof(ev));
	613	put_unaligned_le16(MGMT_OP_SET_DISCOVERABLE, &ev->opcode);
	614
	615	rp = (void ) skb_put(skb, sizeof(rp));
	616	put_unaligned_le16(cmd->index, &rp->index);
	617	rp->discoverable = cp->discoverable;
	618
	619	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
	620	kfree_skb(skb);
	621
	622	list_del(&cmd->list);
	623
	624	if (match->sk == NULL) {
	625	match->sk = cmd->sk;
	626	sock_hold(match->sk);
	627	}
	628
	629	mgmt_pending_free(cmd);
	630	}
	631
	632	int mgmt_discoverable(u16 index, u8 discoverable)
	633	{
	634	struct mgmt_ev_discoverable ev;
	635	struct cmd_lookup match = { discoverable, NULL };
	636	int ret;
	637
	638	put_unaligned_le16(index, &ev.index);
	639	ev.discoverable = discoverable;
	640
	641	mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
	642	discoverable_rsp, &match);
	643
	644	ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);
	645
	646	if (match.sk)
	647	sock_put(match.sk);
	648
	649	return ret;
	650	}