[deliverable/linux.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->connectable = test_bit(HCI_PSCAN, &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)) {
		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;
}

static int set_connectable(struct sock *sk, unsigned char *data, u16 len)
{
	struct mgmt_cp_set_connectable *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_CONNECTABLE, ENODEV);

	hci_dev_lock_bh(hdev);

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

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

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

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

	if (cp->connectable)
		scan = SCAN_PAGE;
	else
		scan = 0;

	err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
	if (err < 0)
		mgmt_pending_remove(MGMT_OP_SET_CONNECTABLE, 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;
	case MGMT_OP_SET_CONNECTABLE:
		err = set_connectable(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;
}

static void connectable_rsp(struct pending_cmd *cmd, void *data)
{
	struct mgmt_cp_set_connectable *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_connectable *rp;

	if (cp->connectable != 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_CONNECTABLE, &ev->opcode);

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

	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_connectable(u16 index, u8 connectable)
{
	struct mgmt_ev_connectable ev;
	struct cmd_lookup match = { connectable, NULL };
	int ret;

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

	mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, index,
						connectable_rsp, &match);

	ret = mgmt_event(MGMT_EV_CONNECTABLE, &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);
9fbcbb45	207	rp->connectable = test_bit(HCI_PSCAN, &hdev->flags);
f7b64e69 JH	208	rp->discoverable = test_bit(HCI_ISCAN, &hdev->flags);
	209	rp->pairable = test_bit(HCI_PSCAN, &hdev->flags);
	210
	211	if (test_bit(HCI_AUTH, &hdev->flags))
	212	rp->sec_mode = 3;
	213	else if (hdev->ssp_mode > 0)
	214	rp->sec_mode = 4;
	215	else
	216	rp->sec_mode = 2;
	217
	218	bacpy(&rp->bdaddr, &hdev->bdaddr);
	219	memcpy(rp->features, hdev->features, 8);
	220	memcpy(rp->dev_class, hdev->dev_class, 3);
	221	put_unaligned_le16(hdev->manufacturer, &rp->manufacturer);
	222	rp->hci_ver = hdev->hci_ver;
	223	put_unaligned_le16(hdev->hci_rev, &rp->hci_rev);
	224
	225	hci_dev_unlock_bh(hdev);
	226	hci_dev_put(hdev);
0381101f JH	227
	228	if (sock_queue_rcv_skb(sk, skb) < 0)
	229	kfree_skb(skb);
e41d8b4e JH	230
e41d8b4e JH	231	return 0;
0381101f JH	232	}
0381101f JH	233
eec8d2bc JH	234	static void mgmt_pending_free(struct pending_cmd *cmd)
	235	{
	236	sock_put(cmd->sk);
	237	kfree(cmd->cmd);
	238	kfree(cmd);
	239	}
	240
	241	static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
	242	void *data, u16 len)
	243	{
	244	struct pending_cmd *cmd;
	245
	246	cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
	247	if (!cmd)
	248	return -ENOMEM;
	249
	250	cmd->opcode = opcode;
	251	cmd->index = index;
	252
	253	cmd->cmd = kmalloc(len, GFP_ATOMIC);
	254	if (!cmd->cmd) {
	255	kfree(cmd);
	256	return -ENOMEM;
	257	}
	258
	259	memcpy(cmd->cmd, data, len);
	260
	261	cmd->sk = sk;
	262	sock_hold(sk);
	263
	264	list_add(&cmd->list, &cmd_list);
	265
	266	return 0;
	267	}
	268
	269	static void mgmt_pending_foreach(u16 opcode, int index,
	270	void (cb)(struct pending_cmd cmd, void *data),
	271	void *data)
	272	{
	273	struct list_head p, n;
	274
	275	list_for_each_safe(p, n, &cmd_list) {
	276	struct pending_cmd *cmd;
	277
	278	cmd = list_entry(p, struct pending_cmd, list);
	279
	280	if (cmd->opcode != opcode)
	281	continue;
	282
	283	if (index >= 0 && cmd->index != index)
	284	continue;
	285
	286	cb(cmd, data);
	287	}
	288	}
	289
	290	static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
	291	{
	292	struct list_head *p;
	293
	294	list_for_each(p, &cmd_list) {
	295	struct pending_cmd *cmd;
	296
	297	cmd = list_entry(p, struct pending_cmd, list);
298
299	if (cmd->opcode != opcode)
300	continue;
301
302	if (index >= 0 && cmd->index != index)
303	continue;
304
305	return cmd;
306	}
307
308	return NULL;
309	}
310
73f22f62 JH	311	static void mgmt_pending_remove(u16 opcode, int index)
	312	{
	313	struct pending_cmd *cmd;
	314
	315	cmd = mgmt_pending_find(opcode, index);
	316	if (cmd == NULL)
	317	return;
	318
	319	list_del(&cmd->list);
	320	mgmt_pending_free(cmd);
	321	}
	322
eec8d2bc JH	323	static int set_powered(struct sock sk, unsigned char data, u16 len)
	324	{
	325	struct mgmt_cp_set_powered *cp;
	326	struct hci_dev *hdev;
	327	u16 dev_id;
	328	int ret, up;
	329
	330	cp = (void *) data;
	331	dev_id = get_unaligned_le16(&cp->index);
	332
	333	BT_DBG("request for hci%u", dev_id);
	334
	335	hdev = hci_dev_get(dev_id);
	336	if (!hdev)
	337	return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
	338
	339	hci_dev_lock_bh(hdev);
	340
	341	up = test_bit(HCI_UP, &hdev->flags);
	342	if ((cp->powered && up) \|\| (!cp->powered && !up)) {
	343	ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
	344	goto failed;
	345	}
	346
	347	if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
	348	ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
	349	goto failed;
	350	}
	351
	352	ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
	353	if (ret < 0)
	354	goto failed;
	355
	356	if (cp->powered)
	357	queue_work(hdev->workqueue, &hdev->power_on);
	358	else
	359	queue_work(hdev->workqueue, &hdev->power_off);
	360
	361	ret = 0;
	362
	363	failed:
	364	hci_dev_unlock_bh(hdev);
	365	hci_dev_put(hdev);
	366	return ret;
	367	}
	368
73f22f62 JH	369	static int set_discoverable(struct sock sk, unsigned char data, u16 len)
	370	{
	371	struct mgmt_cp_set_discoverable *cp;
	372	struct hci_dev *hdev;
	373	u16 dev_id;
	374	u8 scan;
	375	int err;
	376
	377	cp = (void *) data;
	378	dev_id = get_unaligned_le16(&cp->index);
	379
	380	BT_DBG("request for hci%u", dev_id);
	381
	382	hdev = hci_dev_get(dev_id);
	383	if (!hdev)
	384	return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);
	385
	386	hci_dev_lock_bh(hdev);
	387
	388	if (!test_bit(HCI_UP, &hdev->flags)) {
	389	err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
	390	goto failed;
	391	}
	392
	393	if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) \|\|
9fbcbb45	394	mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id)) {
73f22f62 JH	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
9fbcbb45 JH	425	static int set_connectable(struct sock sk, unsigned char data, u16 len)
	426	{
	427	struct mgmt_cp_set_connectable *cp;
	428	struct hci_dev *hdev;
	429	u16 dev_id;
	430	u8 scan;
	431	int err;
	432
	433	cp = (void *) data;
	434	dev_id = get_unaligned_le16(&cp->index);
	435
	436	BT_DBG("request for hci%u", dev_id);
	437
	438	hdev = hci_dev_get(dev_id);
	439	if (!hdev)
	440	return cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENODEV);
	441
	442	hci_dev_lock_bh(hdev);
	443
	444	if (!test_bit(HCI_UP, &hdev->flags)) {
	445	err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENETDOWN);
	446	goto failed;
	447	}
	448
	449	if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) \|\|
	450	mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id)) {
	451	err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EBUSY);
	452	goto failed;
	453	}
	454
	455	if (cp->connectable == test_bit(HCI_PSCAN, &hdev->flags)) {
	456	err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EALREADY);
	457	goto failed;
	458	}
	459
	460	err = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, dev_id, data, len);
	461	if (err < 0)
	462	goto failed;
	463
	464	if (cp->connectable)
	465	scan = SCAN_PAGE;
	466	else
	467	scan = 0;
	468
	469	err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
	470	if (err < 0)
	471	mgmt_pending_remove(MGMT_OP_SET_CONNECTABLE, dev_id);
	472
	473	failed:
	474	hci_dev_unlock_bh(hdev);
	475	hci_dev_put(hdev);
	476
	477	return err;
	478	}
	479
0381101f JH	480	int mgmt_control(struct sock sk, struct msghdr msg, size_t msglen)
	481	{
	482	unsigned char *buf;
	483	struct mgmt_hdr *hdr;
	484	u16 opcode, len;
	485	int err;
	486
	487	BT_DBG("got %zu bytes", msglen);
	488
	489	if (msglen < sizeof(*hdr))
	490	return -EINVAL;
	491
	492	buf = kmalloc(msglen, GFP_ATOMIC);
	493	if (!buf)
	494	return -ENOMEM;
	495
	496	if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
	497	err = -EFAULT;
	498	goto done;
	499	}
	500
	501	hdr = (struct mgmt_hdr *) buf;
	502	opcode = get_unaligned_le16(&hdr->opcode);
	503	len = get_unaligned_le16(&hdr->len);
	504
	505	if (len != msglen - sizeof(*hdr)) {
	506	err = -EINVAL;
	507	goto done;
	508	}
	509
	510	switch (opcode) {
02d98129 JH	511	case MGMT_OP_READ_VERSION:
	512	err = read_version(sk);
	513	break;
faba42eb JH	514	case MGMT_OP_READ_INDEX_LIST:
	515	err = read_index_list(sk);
	516	break;
f7b64e69 JH	517	case MGMT_OP_READ_INFO:
	518	err = read_controller_info(sk, buf + sizeof(*hdr), len);
	519	break;
eec8d2bc JH	520	case MGMT_OP_SET_POWERED:
	521	err = set_powered(sk, buf + sizeof(*hdr), len);
	522	break;
73f22f62 JH	523	case MGMT_OP_SET_DISCOVERABLE:
	524	err = set_discoverable(sk, buf + sizeof(*hdr), len);
	525	break;
9fbcbb45 JH	526	case MGMT_OP_SET_CONNECTABLE:
	527	err = set_connectable(sk, buf + sizeof(*hdr), len);
	528	break;
0381101f JH	529	default:
0381101f JH	530	BT_DBG("Unknown op %u", opcode);
e41d8b4e	531	err = cmd_status(sk, opcode, 0x01);
0381101f JH	532	break;
	533	}
	534
e41d8b4e JH	535	if (err < 0)
	536	goto done;
	537
0381101f JH	538	err = msglen;
	539
	540	done:
	541	kfree(buf);
	542	return err;
	543	}
c71e97bf	544
eec8d2bc	545	static int mgmt_event(u16 event, void data, u16 data_len, struct sock skip_sk)
c71e97bf JH	546	{
	547	struct sk_buff *skb;
	548	struct mgmt_hdr *hdr;
	549
	550	skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
	551	if (!skb)
	552	return -ENOMEM;
	553
	554	bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
	555
	556	hdr = (void ) skb_put(skb, sizeof(hdr));
	557	hdr->opcode = cpu_to_le16(event);
	558	hdr->len = cpu_to_le16(data_len);
	559
	560	memcpy(skb_put(skb, data_len), data, data_len);
	561
eec8d2bc	562	hci_send_to_sock(NULL, skb, skip_sk);
c71e97bf JH	563	kfree_skb(skb);
	564
	565	return 0;
	566	}
	567
	568	int mgmt_index_added(u16 index)
	569	{
	570	struct mgmt_ev_index_added ev;
	571
	572	put_unaligned_le16(index, &ev.index);
	573
eec8d2bc	574	return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
c71e97bf JH	575	}
	576
	577	int mgmt_index_removed(u16 index)
	578	{
	579	struct mgmt_ev_index_added ev;
	580
	581	put_unaligned_le16(index, &ev.index);
	582
eec8d2bc JH	583	return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
	584	}
	585
73f22f62 JH	586	struct cmd_lookup {
73f22f62 JH	587	u8 value;
eec8d2bc JH	588	struct sock *sk;
	589	};
	590
	591	static void power_rsp(struct pending_cmd cmd, void data)
	592	{
	593	struct mgmt_hdr *hdr;
	594	struct mgmt_ev_cmd_complete *ev;
	595	struct mgmt_rp_set_powered *rp;
	596	struct mgmt_cp_set_powered *cp = cmd->cmd;
	597	struct sk_buff *skb;
73f22f62	598	struct cmd_lookup *match = data;
eec8d2bc	599
73f22f62	600	if (cp->powered != match->value)
eec8d2bc JH	601	return;
	602
	603	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
	604	if (!skb)
	605	return;
	606
	607	hdr = (void ) skb_put(skb, sizeof(hdr));
	608	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	609	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
	610
	611	ev = (void ) skb_put(skb, sizeof(ev));
	612	put_unaligned_le16(cmd->opcode, &ev->opcode);
	613
	614	rp = (void ) skb_put(skb, sizeof(rp));
	615	put_unaligned_le16(cmd->index, &rp->index);
	616	rp->powered = cp->powered;
	617
	618	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
	619	kfree_skb(skb);
	620
	621	list_del(&cmd->list);
	622
	623	if (match->sk == NULL) {
	624	match->sk = cmd->sk;
	625	sock_hold(match->sk);
	626	}
	627
	628	mgmt_pending_free(cmd);
c71e97bf	629	}
5add6af8 JH	630
	631	int mgmt_powered(u16 index, u8 powered)
	632	{
	633	struct mgmt_ev_powered ev;
73f22f62	634	struct cmd_lookup match = { powered, NULL };
eec8d2bc	635	int ret;
5add6af8 JH	636
	637	put_unaligned_le16(index, &ev.index);
	638	ev.powered = powered;
	639
eec8d2bc JH	640	mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, power_rsp, &match);
	641
	642	ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
	643
	644	if (match.sk)
	645	sock_put(match.sk);
	646
	647	return ret;
5add6af8	648	}
73f22f62 JH	649
	650	static void discoverable_rsp(struct pending_cmd cmd, void data)
	651	{
	652	struct mgmt_cp_set_discoverable *cp = cmd->cmd;
	653	struct cmd_lookup *match = data;
	654	struct sk_buff *skb;
	655	struct mgmt_hdr *hdr;
	656	struct mgmt_ev_cmd_complete *ev;
	657	struct mgmt_rp_set_discoverable *rp;
	658
	659	if (cp->discoverable != match->value)
	660	return;
	661
	662	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
	663	if (!skb)
	664	return;
	665
	666	hdr = (void ) skb_put(skb, sizeof(hdr));
	667	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	668	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
	669
	670	ev = (void ) skb_put(skb, sizeof(ev));
	671	put_unaligned_le16(MGMT_OP_SET_DISCOVERABLE, &ev->opcode);
	672
	673	rp = (void ) skb_put(skb, sizeof(rp));
	674	put_unaligned_le16(cmd->index, &rp->index);
	675	rp->discoverable = cp->discoverable;
	676
	677	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
	678	kfree_skb(skb);
	679
	680	list_del(&cmd->list);
	681
	682	if (match->sk == NULL) {
	683	match->sk = cmd->sk;
	684	sock_hold(match->sk);
	685	}
	686
	687	mgmt_pending_free(cmd);
	688	}
	689
	690	int mgmt_discoverable(u16 index, u8 discoverable)
	691	{
	692	struct mgmt_ev_discoverable ev;
	693	struct cmd_lookup match = { discoverable, NULL };
	694	int ret;
	695
	696	put_unaligned_le16(index, &ev.index);
	697	ev.discoverable = discoverable;
	698
	699	mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
	700	discoverable_rsp, &match);
	701
	702	ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);
	703
	704	if (match.sk)
	705	sock_put(match.sk);
	706
	707	return ret;
	708	}
9fbcbb45 JH	709
	710	static void connectable_rsp(struct pending_cmd cmd, void data)
	711	{
	712	struct mgmt_cp_set_connectable *cp = cmd->cmd;
	713	struct cmd_lookup *match = data;
	714	struct sk_buff *skb;
	715	struct mgmt_hdr *hdr;
	716	struct mgmt_ev_cmd_complete *ev;
	717	struct mgmt_rp_set_connectable *rp;
	718
	719	if (cp->connectable != match->value)
	720	return;
	721
	722	skb = alloc_skb(sizeof(hdr) + sizeof(ev) + sizeof(*rp), GFP_ATOMIC);
	723	if (!skb)
	724	return;
	725
	726	hdr = (void ) skb_put(skb, sizeof(hdr));
	727	hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
	728	hdr->len = cpu_to_le16(sizeof(ev) + sizeof(rp));
	729
	730	ev = (void ) skb_put(skb, sizeof(ev));
	731	put_unaligned_le16(MGMT_OP_SET_CONNECTABLE, &ev->opcode);
	732
	733	rp = (void ) skb_put(skb, sizeof(rp));
	734	put_unaligned_le16(cmd->index, &rp->index);
	735	rp->connectable = cp->connectable;
	736
	737	if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
	738	kfree_skb(skb);
	739
	740	list_del(&cmd->list);
	741
	742	if (match->sk == NULL) {
	743	match->sk = cmd->sk;
	744	sock_hold(match->sk);
	745	}
	746
	747	mgmt_pending_free(cmd);
	748	}
	749
	750	int mgmt_connectable(u16 index, u8 connectable)
	751	{
	752	struct mgmt_ev_connectable ev;
	753	struct cmd_lookup match = { connectable, NULL };
	754	int ret;
	755
	756	put_unaligned_le16(index, &ev.index);
	757	ev.connectable = connectable;
	758
	759	mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, index,
	760	connectable_rsp, &match);
	761
	762	ret = mgmt_event(MGMT_EV_CONNECTABLE, &ev, sizeof(ev), match.sk);
	763
	764	if (match.sk)
	765	sock_put(match.sk);
	766
	767	return ret;
	768	}