Bluetooth: Add disconnect managment command

This patch adds a disconnect command to the managment interface. Using
this command user space is able to force the disconnection of connected
devices. The command maps directly to the Disconnect HCI command.

Signed-off-by: Johan Hedberg <johan.hedberg@nokia.com>
Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 746f8dc8aad1d4f758f2c17392c3de6c5353ff7c..2197a099a2b7e282e2a616e093b65d69b187823f 100644 (file)
--- 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 6719e9a36613857fc6bfffeed0c5f73ad8afe0dc..2c47601b6e63c4e1bd5c21d60073caf92d57c8d1 100644 (file)
--- 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 46ddb029912b86ca0b38e64e6352622cc82423c8..335c60bad96c24055d2ab0439e1ccc59121cf9fa 100644 (file)
--- 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 7cf1968157d878378ac47d246fbfb4e7c8430c0d..48f266a64caf9edc3d1e938380cc7223e9b3b460 100644 (file)
--- 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;
 }