net/bluetooth/mgmt.c

   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
  32 #define MGMT_VERSION    0
  33 #define MGMT_REVISION   1
  34
  35 static int read_version(struct sock *sk)
  36 {
  37         struct sk_buff *skb;
  38         struct mgmt_hdr *hdr;
  39         struct mgmt_ev_cmd_complete *ev;
  40         struct mgmt_rp_read_version *rp;
  41
  42         BT_DBG("sock %p", sk);
  43
  44         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
  45         if (!skb)
  46                 return -ENOMEM;
  47
  48         hdr = (void *) skb_put(skb, sizeof(*hdr));
  49         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
  50         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
  51
  52         ev = (void *) skb_put(skb, sizeof(*ev));
  53         put_unaligned_le16(MGMT_OP_READ_VERSION, &ev->opcode);
  54
  55         rp = (void *) skb_put(skb, sizeof(*rp));
  56         rp->version = MGMT_VERSION;
  57         put_unaligned_le16(MGMT_REVISION, &rp->revision);
  58
  59         if (sock_queue_rcv_skb(sk, skb) < 0)
  60                 kfree_skb(skb);
  61
  62         return 0;
  63 }
  64
  65 static int read_index_list(struct sock *sk)
  66 {
  67         struct sk_buff *skb;
  68         struct mgmt_hdr *hdr;
  69         struct mgmt_ev_cmd_complete *ev;
  70         struct mgmt_rp_read_index_list *rp;
  71         struct list_head *p;
  72         size_t body_len;
  73         u16 count;
  74         int i;
  75
  76         BT_DBG("sock %p", sk);
  77
  78         read_lock(&hci_dev_list_lock);
  79
  80         count = 0;
  81         list_for_each(p, &hci_dev_list) {
  82                 count++;
  83         }
  84
  85         body_len = sizeof(*ev) + sizeof(*rp) + (2 * count);
  86         skb = alloc_skb(sizeof(*hdr) + body_len, GFP_ATOMIC);
  87         if (!skb)
  88                 return -ENOMEM;
  89
  90         hdr = (void *) skb_put(skb, sizeof(*hdr));
  91         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
  92         hdr->len = cpu_to_le16(body_len);
  93
  94         ev = (void *) skb_put(skb, sizeof(*ev));
  95         put_unaligned_le16(MGMT_OP_READ_INDEX_LIST, &ev->opcode);
  96
  97         rp = (void *) skb_put(skb, sizeof(*rp) + (2 * count));
  98         put_unaligned_le16(count, &rp->num_controllers);
  99
 100         i = 0;
 101         list_for_each(p, &hci_dev_list) {
 102                 struct hci_dev *d = list_entry(p, struct hci_dev, list);
 103                 put_unaligned_le16(d->id, &rp->index[i++]);
 104                 BT_DBG("Added hci%u", d->id);
 105         }
 106
 107         read_unlock(&hci_dev_list_lock);
 108
 109         if (sock_queue_rcv_skb(sk, skb) < 0)
 110                 kfree_skb(skb);
 111
 112         return 0;
 113 }
 114
 115 static int cmd_status(struct sock *sk, u16 cmd, u8 status)
 116 {
 117         struct sk_buff *skb;
 118         struct mgmt_hdr *hdr;
 119         struct mgmt_ev_cmd_status *ev;
 120
 121         BT_DBG("sock %p", sk);
 122
 123         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_ATOMIC);
 124         if (!skb)
 125                 return -ENOMEM;
 126
 127         hdr = (void *) skb_put(skb, sizeof(*hdr));
 128
 129         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
 130         hdr->len = cpu_to_le16(sizeof(*ev));
 131
 132         ev = (void *) skb_put(skb, sizeof(*ev));
 133         ev->status = status;
 134         put_unaligned_le16(cmd, &ev->opcode);
 135
 136         if (sock_queue_rcv_skb(sk, skb) < 0)
 137                 kfree_skb(skb);
 138
 139         return 0;
 140 }
 141
 142 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 143 {
 144         unsigned char *buf;
 145         struct mgmt_hdr *hdr;
 146         u16 opcode, len;
 147         int err;
 148
 149         BT_DBG("got %zu bytes", msglen);
 150
 151         if (msglen < sizeof(*hdr))
 152                 return -EINVAL;
 153
 154         buf = kmalloc(msglen, GFP_ATOMIC);
 155         if (!buf)
 156                 return -ENOMEM;
 157
 158         if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
 159                 err = -EFAULT;
 160                 goto done;
 161         }
 162
 163         hdr = (struct mgmt_hdr *) buf;
 164         opcode = get_unaligned_le16(&hdr->opcode);
 165         len = get_unaligned_le16(&hdr->len);
 166
 167         if (len != msglen - sizeof(*hdr)) {
 168                 err = -EINVAL;
 169                 goto done;
 170         }
 171
 172         switch (opcode) {
 173         case MGMT_OP_READ_VERSION:
 174                 err = read_version(sk);
 175                 break;
 176         case MGMT_OP_READ_INDEX_LIST:
 177                 err = read_index_list(sk);
 178                 break;
 179         default:
 180                 BT_DBG("Unknown op %u", opcode);
 181                 err = cmd_status(sk, opcode, 0x01);
 182                 break;
 183         }
 184
 185         if (err < 0)
 186                 goto done;
 187
 188         err = msglen;
 189
 190 done:
 191         kfree(buf);
 192         return err;
 193 }