4 files changed, 137 insertions, 2 deletions

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 746f8dc8aad1..2197a099a2b7 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -716,6 +716,7 @@ int mgmt_connectable(u16 index, u8 connectable);
 int mgmt_new_key(u16 index, struct link_key *key, u8 old_key_type);
 int mgmt_connected(u16 index, bdaddr_t *bdaddr);
 int mgmt_disconnected(u16 index, bdaddr_t *bdaddr);
+int mgmt_disconnect_failed(u16 index);
 
 /* HCI info for socket */
 #define hci_pi(sk) ((struct hci_pinfo *) sk)
diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index 6719e9a36613..2c47601b6e63 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -120,6 +120,16 @@ struct mgmt_cp_remove_key {
         __u8 disconnect;
 } __packed;
 
+#define MGMT_OP_DISCONNECT              0x000F
+struct mgmt_cp_disconnect {
+        __le16 index;
+        bdaddr_t bdaddr;
+} __packed;
+struct mgmt_rp_disconnect {
+        __le16 index;
+        bdaddr_t bdaddr;
+} __packed;
+
 #define MGMT_EV_CMD_COMPLETE            0x0001
 struct mgmt_ev_cmd_complete {
         __le16 opcode;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 46ddb029912b..335c60bad96c 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1264,8 +1264,10 @@ static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 
-        if (ev->status)
+        if (ev->status) {
+                mgmt_disconnect_failed(hdev->id);
                 return;
+        }
 
         hci_dev_lock(hdev);
 
@@ -1680,6 +1682,11 @@ static inline void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cs_exit_sniff_mode(hdev, ev->status);
                 break;
 
+        case HCI_OP_DISCONNECT:
+                if (ev->status != 0)
+                        mgmt_disconnect_failed(hdev->id);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%x", hdev->name, opcode);
                 break;
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 7cf1968157d8..48f266a64caf 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -887,6 +887,60 @@ unlock:
         return err;
 }
 
+static int disconnect(struct sock *sk, unsigned char *data, u16 len)
+{
+        struct hci_dev *hdev;
+        struct mgmt_cp_disconnect *cp;
+        struct hci_cp_disconnect dc;
+        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 (!hdev)
+                return cmd_status(sk, MGMT_OP_DISCONNECT, ENODEV);
+
+        hci_dev_lock_bh(hdev);
+
+        if (!test_bit(HCI_UP, &hdev->flags)) {
+                err = cmd_status(sk, MGMT_OP_DISCONNECT, ENETDOWN);
+                goto failed;
+        }
+
+        if (mgmt_pending_find(MGMT_OP_DISCONNECT, dev_id)) {
+                err = cmd_status(sk, 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);
+                goto failed;
+        }
+
+        err = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, dev_id, data, len);
+        if (err < 0)
+                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);
+
+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;
@@ -957,6 +1011,9 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
         case MGMT_OP_REMOVE_KEY:
                 err = remove_key(sk, buf + sizeof(*hdr), len);
                 break;
+        case MGMT_OP_DISCONNECT:
+                err = disconnect(sk, buf + sizeof(*hdr), len);
+                break;
         default:
                 BT_DBG("Unknown op %u", opcode);
                 err = cmd_status(sk, opcode, 0x01);
@@ -1101,12 +1158,72 @@ int mgmt_connected(u16 index, bdaddr_t *bdaddr)
         return mgmt_event(MGMT_EV_CONNECTED, &ev, sizeof(ev), NULL);
 }
 
+static void disconnect_rsp(struct pending_cmd *cmd, void *data)
+{
+        struct mgmt_cp_disconnect *cp = cmd->cmd;
+        struct sock **sk = data;
+        struct sk_buff *skb;
+        struct mgmt_hdr *hdr;
+        struct mgmt_ev_cmd_complete *ev;
+        struct mgmt_rp_disconnect *rp;
+
+        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_DISCONNECT, &ev->opcode);
+
+        rp = (void *) skb_put(skb, sizeof(*rp));
+        put_unaligned_le16(cmd->index, &rp->index);
+        bacpy(&rp->bdaddr, &cp->bdaddr);
+
+        if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
+                kfree_skb(skb);
+
+        *sk = cmd->sk;
+        sock_hold(*sk);
+
+        list_del(&cmd->list);
+        mgmt_pending_free(cmd);
+}
+
 int mgmt_disconnected(u16 index, bdaddr_t *bdaddr)
 {
         struct mgmt_ev_disconnected ev;
+        struct sock *sk = NULL;
+        int err;
+
+        mgmt_pending_foreach(MGMT_OP_DISCONNECT, index, disconnect_rsp, &sk);
 
         put_unaligned_le16(index, &ev.index);
         bacpy(&ev.bdaddr, bdaddr);
 
-        return mgmt_event(MGMT_EV_DISCONNECTED, &ev, sizeof(ev), NULL);
+        err = mgmt_event(MGMT_EV_DISCONNECTED, &ev, sizeof(ev), sk);
+
+        if (sk)
+                sock_put(sk);
+
+        return err;
+}
+
+int mgmt_disconnect_failed(u16 index)
+{
+        struct pending_cmd *cmd;
+        int err;
+
+        cmd = mgmt_pending_find(MGMT_OP_DISCONNECT, index);
+        if (!cmd)
+                return -ENOENT;
+
+        err = cmd_status(cmd->sk, MGMT_OP_DISCONNECT, EIO);
+
+        list_del(&cmd->list);
+        mgmt_pending_free(cmd);
+
+        return err;
 }