Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/padovan/bluetooth-next-2.6

7 files changed, 585 insertions, 269 deletions

diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index 88af9eb9aa48..8add9b499912 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -550,10 +550,8 @@ static int __init bt_init(void)
 		goto error;
 
 	err = l2cap_init();
-	if (err < 0) {
-		hci_sock_cleanup();
+	if (err < 0)
 		goto sock_err;
-	}
 
 	err = sco_init();
 	if (err < 0) {
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index a050a6984901..7a6f56b2f49d 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -286,6 +286,7 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 	conn->state = BT_OPEN;
 	conn->auth_type = HCI_AT_GENERAL_BONDING;
 	conn->io_capability = hdev->io_capability;
+	conn->remote_auth = 0xff;
 
 	conn->power_save = 1;
 	conn->disc_timeout = HCI_DISCONN_TIMEOUT;
@@ -429,10 +430,11 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst, __u8
 
 	if (type == LE_LINK) {
 		le = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst);
+		if (le)
+			return ERR_PTR(-EBUSY);
+		le = hci_conn_add(hdev, LE_LINK, dst);
 		if (!le)
-			le = hci_conn_add(hdev, LE_LINK, dst);
-		if (!le)
-			return NULL;
+			return ERR_PTR(-ENOMEM);
 		if (le->state == BT_OPEN)
 			hci_le_connect(le);
 
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 98b5764e4315..3fbfa50c2bff 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -796,6 +796,29 @@ static void hci_cc_le_read_buffer_size(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_LE_READ_BUFFER_SIZE, rp->status);
 }
 
+static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct hci_rp_user_confirm_reply *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (test_bit(HCI_MGMT, &hdev->flags))
+		mgmt_user_confirm_reply_complete(hdev->id, &rp->bdaddr,
+								rp->status);
+}
+
+static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev,
+							struct sk_buff *skb)
+{
+	struct hci_rp_user_confirm_reply *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (test_bit(HCI_MGMT, &hdev->flags))
+		mgmt_user_confirm_neg_reply_complete(hdev->id, &rp->bdaddr,
+								rp->status);
+}
+
 static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%x", hdev->name, status);
@@ -1401,8 +1424,10 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s
 		if (!ev->status) {
 			conn->link_mode |= HCI_LM_AUTH;
 			conn->sec_level = conn->pending_sec_level;
-		} else
+		} else {
+			mgmt_auth_failed(hdev->id, &conn->dst, ev->status);
 			conn->sec_level = BT_SECURITY_LOW;
+		}
 
 		clear_bit(HCI_CONN_AUTH_PEND, &conn->pend);
 
@@ -1728,6 +1753,14 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_le_read_buffer_size(hdev, skb);
 		break;
 
+	case HCI_OP_USER_CONFIRM_REPLY:
+		hci_cc_user_confirm_reply(hdev, skb);
+		break;
+
+	case HCI_OP_USER_CONFIRM_NEG_REPLY:
+		hci_cc_user_confirm_neg_reply(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%x", hdev->name, opcode);
 		break;
@@ -2362,6 +2395,21 @@ unlock:
 	hci_dev_unlock(hdev);
 }
 
+static inline void hci_user_confirm_request_evt(struct hci_dev *hdev,
+							struct sk_buff *skb)
+{
+	struct hci_ev_user_confirm_req *ev = (void *) skb->data;
+
+	BT_DBG("%s", hdev->name);
+
+	hci_dev_lock(hdev);
+
+	if (test_bit(HCI_MGMT, &hdev->flags))
+		mgmt_user_confirm_request(hdev->id, &ev->bdaddr, ev->passkey);
+
+	hci_dev_unlock(hdev);
+}
+
 static inline void hci_simple_pair_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_ev_simple_pair_complete *ev = (void *) skb->data;
@@ -2372,9 +2420,20 @@ static inline void hci_simple_pair_complete_evt(struct hci_dev *hdev, struct sk_
 	hci_dev_lock(hdev);
 
 	conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr);
-	if (conn)
-		hci_conn_put(conn);
+	if (!conn)
+		goto unlock;
 
+	/* To avoid duplicate auth_failed events to user space we check
+	 * the HCI_CONN_AUTH_PEND flag which will be set if we
+	 * initiated the authentication. A traditional auth_complete
+	 * event gets always produced as initiator and is also mapped to
+	 * the mgmt_auth_failed event */
+	if (!test_bit(HCI_CONN_AUTH_PEND, &conn->pend) && ev->status != 0)
+		mgmt_auth_failed(hdev->id, &conn->dst, ev->status);
+
+	hci_conn_put(conn);
+
+unlock:
 	hci_dev_unlock(hdev);
 }
 
@@ -2580,6 +2639,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_io_capa_reply_evt(hdev, skb);
 		break;
 
+	case HCI_EV_USER_CONFIRM_REQUEST:
+		hci_user_confirm_request_evt(hdev, skb);
+		break;
+
 	case HCI_EV_SIMPLE_PAIR_COMPLETE:
 		hci_simple_pair_complete_evt(hdev, skb);
 		break;
diff --git a/net/bluetooth/hci_sock.c b/net/bluetooth/hci_sock.c
index d50e96136608..295e4a88fff8 100644
--- a/net/bluetooth/hci_sock.c
+++ b/net/bluetooth/hci_sock.c
@@ -861,7 +861,7 @@ error:
 	return err;
 }
 
-void __exit hci_sock_cleanup(void)
+void hci_sock_cleanup(void)
 {
 	if (bt_sock_unregister(BTPROTO_HCI) < 0)
 		BT_ERR("HCI socket unregistration failed");
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index efcef0dc1259..c9f9cecca527 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -852,8 +852,6 @@ int l2cap_do_connect(struct sock *sk)
 
 	hci_dev_lock_bh(hdev);
 
-	err = -ENOMEM;
-
 	auth_type = l2cap_get_auth_type(sk);
 
 	if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA)
@@ -863,17 +861,18 @@ int l2cap_do_connect(struct sock *sk)
 		hcon = hci_connect(hdev, ACL_LINK, dst,
 					l2cap_pi(sk)->sec_level, auth_type);
 
-	if (!hcon)
+	if (IS_ERR(hcon)) {
+		err = PTR_ERR(hcon);
 		goto done;
+	}
 
 	conn = l2cap_conn_add(hcon, 0);
 	if (!conn) {
 		hci_conn_put(hcon);
+		err = -ENOMEM;
 		goto done;
 	}
 
-	err = 0;
-
 	/* Update source addr of the socket */
 	bacpy(src, conn->src);
 
@@ -892,6 +891,8 @@ int l2cap_do_connect(struct sock *sk)
 			l2cap_do_start(sk);
 	}
 
+	err = 0;
+
 done:
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
@@ -4033,8 +4034,6 @@ int __init l2cap_init(void)
 			BT_ERR("Failed to create L2CAP debug file");
 	}
 
-	BT_INFO("L2CAP socket layer initialized");
-
 	return 0;
 
 error:
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index f5ef7a3374c7..0054c74e27b7 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -38,17 +38,18 @@ struct pending_cmd {
 	int index;
 	void *cmd;
 	struct sock *sk;
+	void *user_data;
 };
 
 LIST_HEAD(cmd_list);
 
-static int cmd_status(struct sock *sk, u16 cmd, u8 status)
+static int cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
 {
 	struct sk_buff *skb;
 	struct mgmt_hdr *hdr;
 	struct mgmt_ev_cmd_status *ev;
 
-	BT_DBG("sock %p", sk);
+	BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
 
 	skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_ATOMIC);
 	if (!skb)
@@ -57,6 +58,7 @@ static int cmd_status(struct sock *sk, u16 cmd, u8 status)
 	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));
@@ -69,7 +71,8 @@ static int cmd_status(struct sock *sk, u16 cmd, u8 status)
 	return 0;
 }
 
-static int cmd_complete(struct sock *sk, u16 cmd, void *rp, size_t rp_len)
+static int cmd_complete(struct sock *sk, u16 index, u16 cmd, void *rp,
+								size_t rp_len)
 {
 	struct sk_buff *skb;
 	struct mgmt_hdr *hdr;
@@ -84,11 +87,14 @@ static int cmd_complete(struct sock *sk, u16 cmd, void *rp, size_t rp_len)
 	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);
 	put_unaligned_le16(cmd, &ev->opcode);
-	memcpy(ev->data, rp, rp_len);
+
+	if (rp)
+		memcpy(ev->data, rp, rp_len);
 
 	if (sock_queue_rcv_skb(sk, skb) < 0)
 		kfree_skb(skb);
@@ -105,7 +111,8 @@ static int read_version(struct sock *sk)
 	rp.version = MGMT_VERSION;
 	put_unaligned_le16(MGMT_REVISION, &rp.revision);
 
-	return cmd_complete(sk, MGMT_OP_READ_VERSION, &rp, sizeof(rp));
+	return cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_VERSION, &rp,
+								sizeof(rp));
 }
 
 static int read_index_list(struct sock *sk)
@@ -151,32 +158,24 @@ static int read_index_list(struct sock *sk)
 
 	read_unlock(&hci_dev_list_lock);
 
-	err = cmd_complete(sk, MGMT_OP_READ_INDEX_LIST, rp, rp_len);
+	err = cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_INDEX_LIST, rp,
+									rp_len);
 
 	kfree(rp);
 
 	return err;
 }
 
-static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
+static int read_controller_info(struct sock *sk, u16 index)
 {
 	struct mgmt_rp_read_info rp;
-	struct mgmt_cp_read_info *cp = (void *) data;
 	struct hci_dev *hdev;
-	u16 dev_id;
-
-	BT_DBG("sock %p", sk);
 
-	if (len != 2)
-		return cmd_status(sk, MGMT_OP_READ_INFO, EINVAL);
+	BT_DBG("sock %p hci%u", sk, index);
 
-	dev_id = get_unaligned_le16(&cp->index);
-
-	BT_DBG("request for hci%u", dev_id);
-
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_READ_INFO, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_READ_INFO, ENODEV);
 
 	hci_del_off_timer(hdev);
 
@@ -184,7 +183,6 @@ static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
 
 	set_bit(HCI_MGMT, &hdev->flags);
 
-	put_unaligned_le16(hdev->id, &rp.index);
 	rp.type = hdev->dev_type;
 
 	rp.powered = test_bit(HCI_UP, &hdev->flags);
@@ -209,7 +207,7 @@ static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
 
-	return cmd_complete(sk, MGMT_OP_READ_INFO, &rp, sizeof(rp));
+	return cmd_complete(sk, index, MGMT_OP_READ_INFO, &rp, sizeof(rp));
 }
 
 static void mgmt_pending_free(struct pending_cmd *cmd)
@@ -219,14 +217,14 @@ static void mgmt_pending_free(struct pending_cmd *cmd)
 	kfree(cmd);
 }
 
-static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
-							void *data, u16 len)
+static struct pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
+						u16 index, void *data, u16 len)
 {
 	struct pending_cmd *cmd;
 
 	cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
 	if (!cmd)
-		return -ENOMEM;
+		return NULL;
 
 	cmd->opcode = opcode;
 	cmd->index = index;
@@ -234,7 +232,7 @@ static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
 	cmd->cmd = kmalloc(len, GFP_ATOMIC);
 	if (!cmd->cmd) {
 		kfree(cmd);
-		return -ENOMEM;
+		return NULL;
 	}
 
 	memcpy(cmd->cmd, data, len);
@@ -244,7 +242,7 @@ static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
 
 	list_add(&cmd->list, &cmd_list);
 
-	return 0;
+	return cmd;
 }
 
 static void mgmt_pending_foreach(u16 opcode, int index,
@@ -289,103 +287,106 @@ static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
 	return NULL;
 }
 
-static void mgmt_pending_remove(u16 opcode, int index)
+static void mgmt_pending_remove(struct pending_cmd *cmd)
 {
-	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)
+static int set_powered(struct sock *sk, u16 index, unsigned char *data, u16 len)
 {
 	struct mgmt_mode *cp;
 	struct hci_dev *hdev;
-	u16 dev_id;
-	int ret, up;
+	struct pending_cmd *cmd;
+	int err, up;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_POWERED, EINVAL);
 
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_SET_POWERED, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
 	up = test_bit(HCI_UP, &hdev->flags);
 	if ((cp->val && up) || (!cp->val && !up)) {
-		ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
+		err = cmd_status(sk, index, 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);
+	if (mgmt_pending_find(MGMT_OP_SET_POWERED, index)) {
+		err = cmd_status(sk, index, MGMT_OP_SET_POWERED, EBUSY);
 		goto failed;
 	}
 
-	ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
-	if (ret < 0)
+	cmd = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
 	if (cp->val)
 		queue_work(hdev->workqueue, &hdev->power_on);
 	else
 		queue_work(hdev->workqueue, &hdev->power_off);
 
-	ret = 0;
+	err = 0;
 
 failed:
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
-	return ret;
+	return err;
 }
 
-static int set_discoverable(struct sock *sk, unsigned char *data, u16 len)
+static int set_discoverable(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct mgmt_mode *cp;
 	struct hci_dev *hdev;
-	u16 dev_id;
+	struct pending_cmd *cmd;
 	u8 scan;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, EINVAL);
 
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);
+		return cmd_status(sk, index, 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);
+		err = cmd_status(sk, index, 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);
+	if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, index) ||
+			mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, index)) {
+		err = cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, EBUSY);
 		goto failed;
 	}
 
 	if (cp->val == test_bit(HCI_ISCAN, &hdev->flags) &&
 					test_bit(HCI_PSCAN, &hdev->flags)) {
-		err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EALREADY);
+		err = cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, EALREADY);
 		goto failed;
 	}
 
-	err = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, dev_id, data, len);
-	if (err < 0)
+	cmd = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
 	scan = SCAN_PAGE;
 
@@ -394,7 +395,7 @@ static int set_discoverable(struct sock *sk, unsigned char *data, u16 len)
 
 	err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 	if (err < 0)
-		mgmt_pending_remove(MGMT_OP_SET_DISCOVERABLE, dev_id);
+		mgmt_pending_remove(cmd);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -403,44 +404,49 @@ failed:
 	return err;
 }
 
-static int set_connectable(struct sock *sk, unsigned char *data, u16 len)
+static int set_connectable(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct mgmt_mode *cp;
 	struct hci_dev *hdev;
-	u16 dev_id;
+	struct pending_cmd *cmd;
 	u8 scan;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, EINVAL);
 
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENODEV);
+		return cmd_status(sk, index, 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);
+		err = cmd_status(sk, index, 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);
+	if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, index) ||
+			mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, index)) {
+		err = cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, EBUSY);
 		goto failed;
 	}
 
 	if (cp->val == test_bit(HCI_PSCAN, &hdev->flags)) {
-		err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EALREADY);
+		err = cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, EALREADY);
 		goto failed;
 	}
 
-	err = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, dev_id, data, len);
-	if (err < 0)
+	cmd = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
 	if (cp->val)
 		scan = SCAN_PAGE;
@@ -449,7 +455,7 @@ static int set_connectable(struct sock *sk, unsigned char *data, u16 len)
 
 	err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 	if (err < 0)
-		mgmt_pending_remove(MGMT_OP_SET_CONNECTABLE, dev_id);
+		mgmt_pending_remove(cmd);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -458,7 +464,8 @@ failed:
 	return err;
 }
 
-static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
+static int mgmt_event(u16 event, u16 index, void *data, u16 data_len,
+							struct sock *skip_sk)
 {
 	struct sk_buff *skb;
 	struct mgmt_hdr *hdr;
@@ -471,9 +478,11 @@ static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
 
 	hdr = (void *) skb_put(skb, sizeof(*hdr));
 	hdr->opcode = cpu_to_le16(event);
+	hdr->index = cpu_to_le16(index);
 	hdr->len = cpu_to_le16(data_len);
 
-	memcpy(skb_put(skb, data_len), data, data_len);
+	if (data)
+		memcpy(skb_put(skb, data_len), data, data_len);
 
 	hci_send_to_sock(NULL, skb, skip_sk);
 	kfree_skb(skb);
@@ -485,27 +494,28 @@ static int send_mode_rsp(struct sock *sk, u16 opcode, u16 index, u8 val)
 {
 	struct mgmt_mode rp;
 
-	put_unaligned_le16(index, &rp.index);
 	rp.val = val;
 
-	return cmd_complete(sk, opcode, &rp, sizeof(rp));
+	return cmd_complete(sk, index, opcode, &rp, sizeof(rp));
 }
 
-static int set_pairable(struct sock *sk, unsigned char *data, u16 len)
+static int set_pairable(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct mgmt_mode *cp, ev;
 	struct hci_dev *hdev;
-	u16 dev_id;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_PAIRABLE, EINVAL);
 
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_PAIRABLE, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_SET_PAIRABLE, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
@@ -514,14 +524,13 @@ static int set_pairable(struct sock *sk, unsigned char *data, u16 len)
 	else
 		clear_bit(HCI_PAIRABLE, &hdev->flags);
 
-	err = send_mode_rsp(sk, MGMT_OP_SET_PAIRABLE, dev_id, cp->val);
+	err = send_mode_rsp(sk, MGMT_OP_SET_PAIRABLE, index, cp->val);
 	if (err < 0)
 		goto failed;
 
-	put_unaligned_le16(dev_id, &ev.index);
 	ev.val = cp->val;
 
-	err = mgmt_event(MGMT_EV_PAIRABLE, &ev, sizeof(ev), sk);
+	err = mgmt_event(MGMT_EV_PAIRABLE, index, &ev, sizeof(ev), sk);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -563,22 +572,23 @@ static int update_class(struct hci_dev *hdev)
 	return hci_send_cmd(hdev, HCI_OP_WRITE_CLASS_OF_DEV, sizeof(cod), cod);
 }
 
-static int add_uuid(struct sock *sk, unsigned char *data, u16 len)
+static int add_uuid(struct sock *sk, u16 index, unsigned char *data, u16 len)
 {
 	struct mgmt_cp_add_uuid *cp;
 	struct hci_dev *hdev;
 	struct bt_uuid *uuid;
-	u16 dev_id;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_ADD_UUID, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_ADD_UUID, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_ADD_UUID, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
@@ -597,7 +607,7 @@ static int add_uuid(struct sock *sk, unsigned char *data, u16 len)
 	if (err < 0)
 		goto failed;
 
-	err = cmd_complete(sk, MGMT_OP_ADD_UUID, &dev_id, sizeof(dev_id));
+	err = cmd_complete(sk, index, MGMT_OP_ADD_UUID, NULL, 0);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -606,23 +616,24 @@ failed:
 	return err;
 }
 
-static int remove_uuid(struct sock *sk, unsigned char *data, u16 len)
+static int remove_uuid(struct sock *sk, u16 index, unsigned char *data, u16 len)
 {
 	struct list_head *p, *n;
-	struct mgmt_cp_add_uuid *cp;
+	struct mgmt_cp_remove_uuid *cp;
 	struct hci_dev *hdev;
 	u8 bt_uuid_any[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
-	u16 dev_id;
 	int err, found;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_REMOVE_UUID, EINVAL);
 
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_REMOVE_UUID, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_REMOVE_UUID, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
@@ -644,7 +655,7 @@ static int remove_uuid(struct sock *sk, unsigned char *data, u16 len)
 	}
 
 	if (found == 0) {
-		err = cmd_status(sk, MGMT_OP_REMOVE_UUID, ENOENT);
+		err = cmd_status(sk, index, MGMT_OP_REMOVE_UUID, ENOENT);
 		goto unlock;
 	}
 
@@ -652,7 +663,7 @@ static int remove_uuid(struct sock *sk, unsigned char *data, u16 len)
 	if (err < 0)
 		goto unlock;
 
-	err = cmd_complete(sk, MGMT_OP_REMOVE_UUID, &dev_id, sizeof(dev_id));
+	err = cmd_complete(sk, index, MGMT_OP_REMOVE_UUID, NULL, 0);
 
 unlock:
 	hci_dev_unlock_bh(hdev);
@@ -661,21 +672,23 @@ unlock:
 	return err;
 }
 
-static int set_dev_class(struct sock *sk, unsigned char *data, u16 len)
+static int set_dev_class(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_set_dev_class *cp;
-	u16 dev_id;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	BT_DBG("request for hci%u", dev_id);
+	BT_DBG("request for hci%u", index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_DEV_CLASS, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_DEV_CLASS, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_SET_DEV_CLASS, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
@@ -685,8 +698,7 @@ static int set_dev_class(struct sock *sk, unsigned char *data, u16 len)
 	err = update_class(hdev);
 
 	if (err == 0)
-		err = cmd_complete(sk, MGMT_OP_SET_DEV_CLASS, &dev_id,
-							sizeof(dev_id));
+		err = cmd_complete(sk, index, MGMT_OP_SET_DEV_CLASS, NULL, 0);
 
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
@@ -694,23 +706,25 @@ static int set_dev_class(struct sock *sk, unsigned char *data, u16 len)
 	return err;
 }
 
-static int set_service_cache(struct sock *sk, unsigned char *data, u16 len)
+static int set_service_cache(struct sock *sk, u16 index,  unsigned char *data,
+									u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_set_service_cache *cp;
-	u16 dev_id;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_SERVICE_CACHE, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_SERVICE_CACHE, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_SET_SERVICE_CACHE, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
-	BT_DBG("hci%u enable %d", dev_id, cp->enable);
+	BT_DBG("hci%u enable %d", index, cp->enable);
 
 	if (cp->enable) {
 		set_bit(HCI_SERVICE_CACHE, &hdev->flags);
@@ -721,8 +735,8 @@ static int set_service_cache(struct sock *sk, unsigned char *data, u16 len)
 	}
 
 	if (err == 0)
-		err = cmd_complete(sk, MGMT_OP_SET_SERVICE_CACHE, &dev_id,
-							sizeof(dev_id));
+		err = cmd_complete(sk, index, MGMT_OP_SET_SERVICE_CACHE, NULL,
+									0);
 
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
@@ -730,15 +744,18 @@ static int set_service_cache(struct sock *sk, unsigned char *data, u16 len)
 	return err;
 }
 
-static int load_keys(struct sock *sk, unsigned char *data, u16 len)
+static int load_keys(struct sock *sk, u16 index, unsigned char *data, u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_load_keys *cp;
-	u16 dev_id, key_count, expected_len;
+	u16 key_count, expected_len;
 	int i;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
+
+	if (len < sizeof(*cp))
+		return -EINVAL;
+
 	key_count = get_unaligned_le16(&cp->key_count);
 
 	expected_len = sizeof(*cp) + key_count * sizeof(struct mgmt_key_info);
@@ -748,11 +765,11 @@ static int load_keys(struct sock *sk, unsigned char *data, u16 len)
 		return -EINVAL;
 	}
 
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_LOAD_KEYS, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_LOAD_KEYS, ENODEV);
 
-	BT_DBG("hci%u debug_keys %u key_count %u", dev_id, cp->debug_keys,
+	BT_DBG("hci%u debug_keys %u key_count %u", index, cp->debug_keys,
 								key_count);
 
 	hci_dev_lock_bh(hdev);
@@ -779,26 +796,27 @@ static int load_keys(struct sock *sk, unsigned char *data, u16 len)
 	return 0;
 }
 
-static int remove_key(struct sock *sk, unsigned char *data, u16 len)
+static int remove_key(struct sock *sk, u16 index, unsigned char *data, u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_remove_key *cp;
 	struct hci_conn *conn;
-	u16 dev_id;
 	int err;
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_REMOVE_KEY, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_REMOVE_KEY, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_REMOVE_KEY, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
 	err = hci_remove_link_key(hdev, &cp->bdaddr);
 	if (err < 0) {
-		err = cmd_status(sk, MGMT_OP_REMOVE_KEY, -err);
+		err = cmd_status(sk, index, MGMT_OP_REMOVE_KEY, -err);
 		goto unlock;
 	}
 
@@ -823,52 +841,56 @@ unlock:
 	return err;
 }
 
-static int disconnect(struct sock *sk, unsigned char *data, u16 len)
+static int disconnect(struct sock *sk, u16 index, unsigned char *data, u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_disconnect *cp;
 	struct hci_cp_disconnect dc;
+	struct pending_cmd *cmd;
 	struct hci_conn *conn;
-	u16 dev_id;
 	int err;
 
 	BT_DBG("");
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_DISCONNECT, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_DISCONNECT, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_DISCONNECT, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
 	if (!test_bit(HCI_UP, &hdev->flags)) {
-		err = cmd_status(sk, MGMT_OP_DISCONNECT, ENETDOWN);
+		err = cmd_status(sk, index, MGMT_OP_DISCONNECT, ENETDOWN);
 		goto failed;
 	}
 
-	if (mgmt_pending_find(MGMT_OP_DISCONNECT, dev_id)) {
-		err = cmd_status(sk, MGMT_OP_DISCONNECT, EBUSY);
+	if (mgmt_pending_find(MGMT_OP_DISCONNECT, index)) {
+		err = cmd_status(sk, index, MGMT_OP_DISCONNECT, EBUSY);
 		goto failed;
 	}
 
 	conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 	if (!conn) {
-		err = cmd_status(sk, MGMT_OP_DISCONNECT, ENOTCONN);
+		err = cmd_status(sk, index, MGMT_OP_DISCONNECT, ENOTCONN);
 		goto failed;
 	}
 
-	err = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, dev_id, data, len);
-	if (err < 0)
+	cmd = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
 	put_unaligned_le16(conn->handle, &dc.handle);
 	dc.reason = 0x13; /* Remote User Terminated Connection */
 
 	err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc);
 	if (err < 0)
-		mgmt_pending_remove(MGMT_OP_DISCONNECT, dev_id);
+		mgmt_pending_remove(cmd);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -877,24 +899,20 @@ failed:
 	return err;
 }
 
-static int get_connections(struct sock *sk, unsigned char *data, u16 len)
+static int get_connections(struct sock *sk, u16 index)
 {
-	struct mgmt_cp_get_connections *cp;
 	struct mgmt_rp_get_connections *rp;
 	struct hci_dev *hdev;
 	struct list_head *p;
 	size_t rp_len;
-	u16 dev_id, count;
+	u16 count;
 	int i, err;
 
 	BT_DBG("");
 
-	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
-
-	hdev = hci_dev_get(dev_id);
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_GET_CONNECTIONS, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_GET_CONNECTIONS, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
@@ -910,7 +928,6 @@ static int get_connections(struct sock *sk, unsigned char *data, u16 len)
 		goto unlock;
 	}
 
-	put_unaligned_le16(dev_id, &rp->index);
 	put_unaligned_le16(count, &rp->conn_count);
 
 	read_lock(&hci_dev_list_lock);
@@ -924,7 +941,7 @@ static int get_connections(struct sock *sk, unsigned char *data, u16 len)
 
 	read_unlock(&hci_dev_list_lock);
 
-	err = cmd_complete(sk, MGMT_OP_GET_CONNECTIONS, rp, rp_len);
+	err = cmd_complete(sk, index, MGMT_OP_GET_CONNECTIONS, rp, rp_len);
 
 unlock:
 	kfree(rp);
@@ -933,33 +950,38 @@ unlock:
 	return err;
 }
 
-static int pin_code_reply(struct sock *sk, unsigned char *data, u16 len)
+static int pin_code_reply(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_pin_code_reply *cp;
 	struct hci_cp_pin_code_reply reply;
-	u16 dev_id;
+	struct pending_cmd *cmd;
 	int err;
 
 	BT_DBG("");
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_DISCONNECT, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
 	if (!test_bit(HCI_UP, &hdev->flags)) {
-		err = cmd_status(sk, MGMT_OP_PIN_CODE_REPLY, ENETDOWN);
+		err = cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, ENETDOWN);
 		goto failed;
 	}
 
-	err = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_REPLY, dev_id, data, len);
-	if (err < 0)
+	cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_REPLY, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
 	bacpy(&reply.bdaddr, &cp->bdaddr);
 	reply.pin_len = cp->pin_len;
@@ -967,7 +989,7 @@ static int pin_code_reply(struct sock *sk, unsigned char *data, u16 len)
 
 	err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_REPLY, sizeof(reply), &reply);
 	if (err < 0)
-		mgmt_pending_remove(MGMT_OP_PIN_CODE_REPLY, dev_id);
+		mgmt_pending_remove(cmd);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -976,38 +998,46 @@ failed:
 	return err;
 }
 
-static int pin_code_neg_reply(struct sock *sk, unsigned char *data, u16 len)
+static int pin_code_neg_reply(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_pin_code_neg_reply *cp;
-	u16 dev_id;
+	struct pending_cmd *cmd;
 	int err;
 
 	BT_DBG("");
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_PIN_CODE_NEG_REPLY,
+									EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_PIN_CODE_NEG_REPLY, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_PIN_CODE_NEG_REPLY,
+									ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
 	if (!test_bit(HCI_UP, &hdev->flags)) {
-		err = cmd_status(sk, MGMT_OP_PIN_CODE_NEG_REPLY, ENETDOWN);
+		err = cmd_status(sk, index, MGMT_OP_PIN_CODE_NEG_REPLY,
+								ENETDOWN);
 		goto failed;
 	}
 
-	err = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_NEG_REPLY, dev_id,
+	cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_NEG_REPLY, index,
 								data, len);
-	if (err < 0)
+	if (!cmd) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
-	err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, sizeof(bdaddr_t),
+	err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, sizeof(cp->bdaddr),
 								&cp->bdaddr);
 	if (err < 0)
-		mgmt_pending_remove(MGMT_OP_PIN_CODE_NEG_REPLY, dev_id);
+		mgmt_pending_remove(cmd);
 
 failed:
 	hci_dev_unlock_bh(hdev);
@@ -1016,40 +1046,217 @@ failed:
 	return err;
 }
 
-static int set_io_capability(struct sock *sk, unsigned char *data, u16 len)
+static int set_io_capability(struct sock *sk, u16 index, unsigned char *data,
+									u16 len)
 {
 	struct hci_dev *hdev;
 	struct mgmt_cp_set_io_capability *cp;
-	u16 dev_id;
 
 	BT_DBG("");
 
 	cp = (void *) data;
-	dev_id = get_unaligned_le16(&cp->index);
 
-	hdev = hci_dev_get(dev_id);
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_SET_IO_CAPABILITY, EINVAL);
+
+	hdev = hci_dev_get(index);
 	if (!hdev)
-		return cmd_status(sk, MGMT_OP_SET_IO_CAPABILITY, ENODEV);
+		return cmd_status(sk, index, MGMT_OP_SET_IO_CAPABILITY, ENODEV);
 
 	hci_dev_lock_bh(hdev);
 
 	hdev->io_capability = cp->io_capability;
 
 	BT_DBG("%s IO capability set to 0x%02x", hdev->name,
-						hdev->io_capability);
+							hdev->io_capability);
 
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
 
-	return cmd_complete(sk, MGMT_OP_SET_IO_CAPABILITY,
-						&dev_id, sizeof(dev_id));
+	return cmd_complete(sk, index, MGMT_OP_SET_IO_CAPABILITY, NULL, 0);
+}
+
+static inline struct pending_cmd *find_pairing(struct hci_conn *conn)
+{
+	struct hci_dev *hdev = conn->hdev;
+	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 != MGMT_OP_PAIR_DEVICE)
+			continue;
+
+		if (cmd->index != hdev->id)
+			continue;
+
+		if (cmd->user_data != conn)
+			continue;
+
+		return cmd;
+	}
+
+	return NULL;
+}
+
+static void pairing_complete(struct pending_cmd *cmd, u8 status)
+{
+	struct mgmt_rp_pair_device rp;
+	struct hci_conn *conn = cmd->user_data;
+
+	bacpy(&rp.bdaddr, &conn->dst);
+	rp.status = status;
+
+	cmd_complete(cmd->sk, cmd->index, MGMT_OP_PAIR_DEVICE, &rp, sizeof(rp));
+
+	/* So we don't get further callbacks for this connection */
+	conn->connect_cfm_cb = NULL;
+	conn->security_cfm_cb = NULL;
+	conn->disconn_cfm_cb = NULL;
+
+	hci_conn_put(conn);
+
+	mgmt_pending_remove(cmd);
+}
+
+static void pairing_complete_cb(struct hci_conn *conn, u8 status)
+{
+	struct pending_cmd *cmd;
+
+	BT_DBG("status %u", status);
+
+	cmd = find_pairing(conn);
+	if (!cmd) {
+		BT_DBG("Unable to find a pending command");
+		return;
+	}
+
+	pairing_complete(cmd, status);
+}
+
+static int pair_device(struct sock *sk, u16 index, unsigned char *data, u16 len)
+{
+	struct hci_dev *hdev;
+	struct mgmt_cp_pair_device *cp;
+	struct pending_cmd *cmd;
+	u8 sec_level, auth_type;
+	struct hci_conn *conn;
+	int err;
+
+	BT_DBG("");
+
+	cp = (void *) data;
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, MGMT_OP_PAIR_DEVICE, EINVAL);
+
+	hdev = hci_dev_get(index);
+	if (!hdev)
+		return cmd_status(sk, index, MGMT_OP_PAIR_DEVICE, ENODEV);
+
+	hci_dev_lock_bh(hdev);
+
+	if (cp->io_cap == 0x03) {
+		sec_level = BT_SECURITY_MEDIUM;
+		auth_type = HCI_AT_DEDICATED_BONDING;
+	} else {
+		sec_level = BT_SECURITY_HIGH;
+		auth_type = HCI_AT_DEDICATED_BONDING_MITM;
+	}
+
+	conn = hci_connect(hdev, ACL_LINK, &cp->bdaddr, sec_level, auth_type);
+	if (IS_ERR(conn)) {
+		err = PTR_ERR(conn);
+		goto unlock;
+	}
+
+	if (conn->connect_cfm_cb) {
+		hci_conn_put(conn);
+		err = cmd_status(sk, index, MGMT_OP_PAIR_DEVICE, EBUSY);
+		goto unlock;
+	}
+
+	cmd = mgmt_pending_add(sk, MGMT_OP_PAIR_DEVICE, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
+		hci_conn_put(conn);
+		goto unlock;
+	}
+
+	conn->connect_cfm_cb = pairing_complete_cb;
+	conn->security_cfm_cb = pairing_complete_cb;
+	conn->disconn_cfm_cb = pairing_complete_cb;
+	conn->io_capability = cp->io_cap;
+	cmd->user_data = conn;
+
+	if (conn->state == BT_CONNECTED &&
+				hci_conn_security(conn, sec_level, auth_type))
+		pairing_complete(cmd, 0);
+
+	err = 0;
+
+unlock:
+	hci_dev_unlock_bh(hdev);
+	hci_dev_put(hdev);
+
+	return err;
+}
+
+static int user_confirm_reply(struct sock *sk, u16 index, unsigned char *data,
+							u16 len, int success)
+{
+	struct mgmt_cp_user_confirm_reply *cp = (void *) data;
+	u16 mgmt_op, hci_op;
+	struct pending_cmd *cmd;
+	struct hci_dev *hdev;
+	int err;
+
+	BT_DBG("");
+
+	if (success) {
+		mgmt_op = MGMT_OP_USER_CONFIRM_REPLY;
+		hci_op = HCI_OP_USER_CONFIRM_REPLY;
+	} else {
+		mgmt_op = MGMT_OP_USER_CONFIRM_NEG_REPLY;
+		hci_op = HCI_OP_USER_CONFIRM_NEG_REPLY;
+	}
+
+	if (len != sizeof(*cp))
+		return cmd_status(sk, index, mgmt_op, EINVAL);
+
+	hdev = hci_dev_get(index);
+	if (!hdev)
+		return cmd_status(sk, index, mgmt_op, ENODEV);
+
+	if (!test_bit(HCI_UP, &hdev->flags)) {
+		err = cmd_status(sk, index, mgmt_op, ENETDOWN);
+		goto failed;
+	}
+
+	cmd = mgmt_pending_add(sk, mgmt_op, index, data, len);
+	if (!cmd) {
+		err = -ENOMEM;
+		goto failed;
+	}
+
+	err = hci_send_cmd(hdev, hci_op, sizeof(cp->bdaddr), &cp->bdaddr);
+	if (err < 0)
+		mgmt_pending_remove(cmd);
+
+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;
+	u16 opcode, index, len;
 	int err;
 
 	BT_DBG("got %zu bytes", msglen);
@@ -1068,6 +1275,7 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 
 	hdr = (struct mgmt_hdr *) buf;
 	opcode = get_unaligned_le16(&hdr->opcode);
+	index = get_unaligned_le16(&hdr->index);
 	len = get_unaligned_le16(&hdr->len);
 
 	if (len != msglen - sizeof(*hdr)) {
@@ -1083,56 +1291,65 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 		err = read_index_list(sk);
 		break;
 	case MGMT_OP_READ_INFO:
-		err = read_controller_info(sk, buf + sizeof(*hdr), len);
+		err = read_controller_info(sk, index);
 		break;
 	case MGMT_OP_SET_POWERED:
-		err = set_powered(sk, buf + sizeof(*hdr), len);
+		err = set_powered(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_SET_DISCOVERABLE:
-		err = set_discoverable(sk, buf + sizeof(*hdr), len);
+		err = set_discoverable(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_SET_CONNECTABLE:
-		err = set_connectable(sk, buf + sizeof(*hdr), len);
+		err = set_connectable(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_SET_PAIRABLE:
-		err = set_pairable(sk, buf + sizeof(*hdr), len);
+		err = set_pairable(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_ADD_UUID:
-		err = add_uuid(sk, buf + sizeof(*hdr), len);
+		err = add_uuid(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_REMOVE_UUID:
-		err = remove_uuid(sk, buf + sizeof(*hdr), len);
+		err = remove_uuid(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_SET_DEV_CLASS:
-		err = set_dev_class(sk, buf + sizeof(*hdr), len);
+		err = set_dev_class(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_SET_SERVICE_CACHE:
-		err = set_service_cache(sk, buf + sizeof(*hdr), len);
+		err = set_service_cache(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_LOAD_KEYS:
-		err = load_keys(sk, buf + sizeof(*hdr), len);
+		err = load_keys(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_REMOVE_KEY:
-		err = remove_key(sk, buf + sizeof(*hdr), len);
+		err = remove_key(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_DISCONNECT:
-		err = disconnect(sk, buf + sizeof(*hdr), len);
+		err = disconnect(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_GET_CONNECTIONS:
-		err = get_connections(sk, buf + sizeof(*hdr), len);
+		err = get_connections(sk, index);
 		break;
 	case MGMT_OP_PIN_CODE_REPLY:
-		err = pin_code_reply(sk, buf + sizeof(*hdr), len);
+		err = pin_code_reply(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_PIN_CODE_NEG_REPLY:
-		err = pin_code_neg_reply(sk, buf + sizeof(*hdr), len);
+		err = pin_code_neg_reply(sk, index, buf + sizeof(*hdr), len);
 		break;
 	case MGMT_OP_SET_IO_CAPABILITY:
-		err = set_io_capability(sk, buf + sizeof(*hdr), len);
+		err = set_io_capability(sk, index, buf + sizeof(*hdr), len);
+		break;
+	case MGMT_OP_PAIR_DEVICE:
+		err = pair_device(sk, index, buf + sizeof(*hdr), len);
+		break;
+	case MGMT_OP_USER_CONFIRM_REPLY:
+		err = user_confirm_reply(sk, index, buf + sizeof(*hdr), len, 1);
+		break;
+	case MGMT_OP_USER_CONFIRM_NEG_REPLY:
+		err = user_confirm_reply(sk, index, buf + sizeof(*hdr), len, 0);
 		break;
 	default:
 		BT_DBG("Unknown op %u", opcode);
-		err = cmd_status(sk, opcode, 0x01);
+		err = cmd_status(sk, index, opcode, 0x01);
 		break;
 	}
 
@@ -1148,20 +1365,12 @@ done:
 
 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);
+	return mgmt_event(MGMT_EV_INDEX_ADDED, index, NULL, 0, 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);
+	return mgmt_event(MGMT_EV_INDEX_REMOVED, index, NULL, 0, NULL);
 }
 
 struct cmd_lookup {
@@ -1197,10 +1406,9 @@ int mgmt_powered(u16 index, u8 powered)
 
 	mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, mode_rsp, &match);
 
-	put_unaligned_le16(index, &ev.index);
 	ev.val = powered;
 
-	ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
+	ret = mgmt_event(MGMT_EV_POWERED, index, &ev, sizeof(ev), match.sk);
 
 	if (match.sk)
 		sock_put(match.sk);
@@ -1214,13 +1422,12 @@ int mgmt_discoverable(u16 index, u8 discoverable)
 	struct cmd_lookup match = { discoverable, NULL };
 	int ret;
 
-	mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
-							mode_rsp, &match);
+	mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index, mode_rsp, &match);
 
-	put_unaligned_le16(index, &ev.index);
 	ev.val = discoverable;
 
-	ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);
+	ret = mgmt_event(MGMT_EV_DISCOVERABLE, index, &ev, sizeof(ev),
+								match.sk);
 
 	if (match.sk)
 		sock_put(match.sk);
@@ -1236,10 +1443,9 @@ int mgmt_connectable(u16 index, u8 connectable)
 
 	mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, index, mode_rsp, &match);
 
-	put_unaligned_le16(index, &ev.index);
 	ev.val = connectable;
 
-	ret = mgmt_event(MGMT_EV_CONNECTABLE, &ev, sizeof(ev), match.sk);
+	ret = mgmt_event(MGMT_EV_CONNECTABLE, index, &ev, sizeof(ev), match.sk);
 
 	if (match.sk)
 		sock_put(match.sk);
@@ -1253,25 +1459,22 @@ int mgmt_new_key(u16 index, struct link_key *key, u8 old_key_type)
 
 	memset(&ev, 0, sizeof(ev));
 
-	put_unaligned_le16(index, &ev.index);
-
 	bacpy(&ev.key.bdaddr, &key->bdaddr);
 	ev.key.type = key->type;
 	memcpy(ev.key.val, key->val, 16);
 	ev.key.pin_len = key->pin_len;
 	ev.old_key_type = old_key_type;
 
-	return mgmt_event(MGMT_EV_NEW_KEY, &ev, sizeof(ev), NULL);
+	return mgmt_event(MGMT_EV_NEW_KEY, index, &ev, sizeof(ev), NULL);
 }
 
 int mgmt_connected(u16 index, bdaddr_t *bdaddr)
 {
 	struct mgmt_ev_connected ev;
 
-	put_unaligned_le16(index, &ev.index);
 	bacpy(&ev.bdaddr, bdaddr);
 
-	return mgmt_event(MGMT_EV_CONNECTED, &ev, sizeof(ev), NULL);
+	return mgmt_event(MGMT_EV_CONNECTED, index, &ev, sizeof(ev), NULL);
 }
 
 static void disconnect_rsp(struct pending_cmd *cmd, void *data)
@@ -1280,16 +1483,14 @@ static void disconnect_rsp(struct pending_cmd *cmd, void *data)
 	struct sock **sk = data;
 	struct mgmt_rp_disconnect rp;
 
-	put_unaligned_le16(cmd->index, &rp.index);
 	bacpy(&rp.bdaddr, &cp->bdaddr);
 
-	cmd_complete(cmd->sk, MGMT_OP_DISCONNECT, &rp, sizeof(rp));
+	cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT, &rp, sizeof(rp));
 
 	*sk = cmd->sk;
 	sock_hold(*sk);
 
-	list_del(&cmd->list);
-	mgmt_pending_free(cmd);
+	mgmt_pending_remove(cmd);
 }
 
 int mgmt_disconnected(u16 index, bdaddr_t *bdaddr)
@@ -1300,10 +1501,9 @@ int mgmt_disconnected(u16 index, bdaddr_t *bdaddr)
 
 	mgmt_pending_foreach(MGMT_OP_DISCONNECT, index, disconnect_rsp, &sk);
 
-	put_unaligned_le16(index, &ev.index);
 	bacpy(&ev.bdaddr, bdaddr);
 
-	err = mgmt_event(MGMT_EV_DISCONNECTED, &ev, sizeof(ev), sk);
+	err = mgmt_event(MGMT_EV_DISCONNECTED, index, &ev, sizeof(ev), sk);
 
 	if (sk)
 		sock_put(sk);
@@ -1320,10 +1520,9 @@ int mgmt_disconnect_failed(u16 index)
 	if (!cmd)
 		return -ENOENT;
 
-	err = cmd_status(cmd->sk, MGMT_OP_DISCONNECT, EIO);
+	err = cmd_status(cmd->sk, index, MGMT_OP_DISCONNECT, EIO);
 
-	list_del(&cmd->list);
-	mgmt_pending_free(cmd);
+	mgmt_pending_remove(cmd);
 
 	return err;
 }
@@ -1332,40 +1531,39 @@ int mgmt_connect_failed(u16 index, bdaddr_t *bdaddr, u8 status)
 {
 	struct mgmt_ev_connect_failed ev;
 
-	put_unaligned_le16(index, &ev.index);
 	bacpy(&ev.bdaddr, bdaddr);
 	ev.status = status;
 
-	return mgmt_event(MGMT_EV_CONNECT_FAILED, &ev, sizeof(ev), NULL);
+	return mgmt_event(MGMT_EV_CONNECT_FAILED, index, &ev, sizeof(ev), NULL);
 }
 
 int mgmt_pin_code_request(u16 index, bdaddr_t *bdaddr)
 {
 	struct mgmt_ev_pin_code_request ev;
 
-	put_unaligned_le16(index, &ev.index);
 	bacpy(&ev.bdaddr, bdaddr);
 
-	return mgmt_event(MGMT_EV_PIN_CODE_REQUEST, &ev, sizeof(ev), NULL);
+	return mgmt_event(MGMT_EV_PIN_CODE_REQUEST, index, &ev, sizeof(ev),
+									NULL);
 }
 
 int mgmt_pin_code_reply_complete(u16 index, bdaddr_t *bdaddr, u8 status)
 {
 	struct pending_cmd *cmd;
+	struct mgmt_rp_pin_code_reply rp;
 	int err;
 
 	cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_REPLY, index);
 	if (!cmd)
 		return -ENOENT;
 
-	if (status != 0)
-		err = cmd_status(cmd->sk, MGMT_OP_PIN_CODE_REPLY, status);
-	else
-		err = cmd_complete(cmd->sk, MGMT_OP_PIN_CODE_REPLY,
-						bdaddr, sizeof(*bdaddr));
+	bacpy(&rp.bdaddr, bdaddr);
+	rp.status = status;
 
-	list_del(&cmd->list);
-	mgmt_pending_free(cmd);
+	err = cmd_complete(cmd->sk, index, MGMT_OP_PIN_CODE_REPLY, &rp,
+								sizeof(rp));
+
+	mgmt_pending_remove(cmd);
 
 	return err;
 }
@@ -1373,20 +1571,75 @@ int mgmt_pin_code_reply_complete(u16 index, bdaddr_t *bdaddr, u8 status)
 int mgmt_pin_code_neg_reply_complete(u16 index, bdaddr_t *bdaddr, u8 status)
 {
 	struct pending_cmd *cmd;
+	struct mgmt_rp_pin_code_reply rp;
 	int err;
 
 	cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_NEG_REPLY, index);
 	if (!cmd)
 		return -ENOENT;
 
-	if (status != 0)
-		err = cmd_status(cmd->sk, MGMT_OP_PIN_CODE_NEG_REPLY, status);
-	else
-		err = cmd_complete(cmd->sk, MGMT_OP_PIN_CODE_NEG_REPLY,
-						bdaddr, sizeof(*bdaddr));
+	bacpy(&rp.bdaddr, bdaddr);
+	rp.status = status;
 
-	list_del(&cmd->list);
-	mgmt_pending_free(cmd);
+	err = cmd_complete(cmd->sk, index, MGMT_OP_PIN_CODE_NEG_REPLY, &rp,
+								sizeof(rp));
+
+	mgmt_pending_remove(cmd);
+
+	return err;
+}
+
+int mgmt_user_confirm_request(u16 index, bdaddr_t *bdaddr, __le32 value)
+{
+	struct mgmt_ev_user_confirm_request ev;
+
+	BT_DBG("hci%u", index);
+
+	bacpy(&ev.bdaddr, bdaddr);
+	put_unaligned_le32(value, &ev.value);
+
+	return mgmt_event(MGMT_EV_USER_CONFIRM_REQUEST, index, &ev, sizeof(ev),
+									NULL);
+}
+
+static int confirm_reply_complete(u16 index, bdaddr_t *bdaddr, u8 status,
+								u8 opcode)
+{
+	struct pending_cmd *cmd;
+	struct mgmt_rp_user_confirm_reply rp;
+	int err;
+
+	cmd = mgmt_pending_find(opcode, index);
+	if (!cmd)
+		return -ENOENT;
+
+	bacpy(&rp.bdaddr, bdaddr);
+	rp.status = status;
+	err = cmd_complete(cmd->sk, index, opcode, &rp, sizeof(rp));
+
+	mgmt_pending_remove(cmd);
 
 	return err;
 }
+
+int mgmt_user_confirm_reply_complete(u16 index, bdaddr_t *bdaddr, u8 status)
+{
+	return confirm_reply_complete(index, bdaddr, status,
+						MGMT_OP_USER_CONFIRM_REPLY);
+}
+
+int mgmt_user_confirm_neg_reply_complete(u16 index, bdaddr_t *bdaddr, u8 status)
+{
+	return confirm_reply_complete(index, bdaddr, status,
+					MGMT_OP_USER_CONFIRM_NEG_REPLY);
+}
+
+int mgmt_auth_failed(u16 index, bdaddr_t *bdaddr, u8 status)
+{
+	struct mgmt_ev_auth_failed ev;
+
+	bacpy(&ev.bdaddr, bdaddr);
+	ev.status = status;
+
+	return mgmt_event(MGMT_EV_AUTH_FAILED, index, &ev, sizeof(ev), NULL);
+}
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index c9348ddda877..42fdffd1d76c 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -190,20 +190,21 @@ static int sco_connect(struct sock *sk)
 
 	hci_dev_lock_bh(hdev);
 
-	err = -ENOMEM;
-
 	if (lmp_esco_capable(hdev) && !disable_esco)
 		type = ESCO_LINK;
 	else
 		type = SCO_LINK;
 
 	hcon = hci_connect(hdev, type, dst, BT_SECURITY_LOW, HCI_AT_NO_BONDING);
-	if (!hcon)
+	if (IS_ERR(hcon)) {
+		err = PTR_ERR(hcon);
 		goto done;
+	}
 
 	conn = sco_conn_add(hcon, 0);
 	if (!conn) {
 		hci_conn_put(hcon);
+		err = -ENOMEM;
 		goto done;
 	}