net/bluetooth/mgmt_util.c

   1 /*
   2    BlueZ - Bluetooth protocol stack for Linux
   3
   4    Copyright (C) 2015  Intel Corporation
   5
   6    This program is free software; you can redistribute it and/or modify
   7    it under the terms of the GNU General Public License version 2 as
   8    published by the Free Software Foundation;
   9
  10    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  11    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  12    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  13    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  14    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  15    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  16    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  17    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  18
  19    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  20    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  21    SOFTWARE IS DISCLAIMED.
  22 */
  23
  24 #include <asm/unaligned.h>
  25
  26 #include <net/bluetooth/bluetooth.h>
  27 #include <net/bluetooth/hci_core.h>
  28 #include <net/bluetooth/hci_mon.h>
  29 #include <net/bluetooth/mgmt.h>
  30
  31 #include "mgmt_util.h"
  32
  33 static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie,
  34                                                  u16 opcode, u16 len, void *buf)
  35 {
  36         struct hci_mon_hdr *hdr;
  37         struct sk_buff *skb;
  38
  39         skb = bt_skb_alloc(6 + len, GFP_ATOMIC);
  40         if (!skb)
  41                 return NULL;
  42
  43         put_unaligned_le32(cookie, skb_put(skb, 4));
  44         put_unaligned_le16(opcode, skb_put(skb, 2));
  45
  46         if (buf)
  47                 memcpy(skb_put(skb, len), buf, len);
  48
  49         __net_timestamp(skb);
  50
  51         hdr = (void *)skb_push(skb, HCI_MON_HDR_SIZE);
  52         hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);
  53         hdr->index = index;
  54         hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);
  55
  56         return skb;
  57 }
  58
  59 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
  60                     void *data, u16 data_len, int flag, struct sock *skip_sk)
  61 {
  62         struct sk_buff *skb;
  63         struct mgmt_hdr *hdr;
  64
  65         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL);
  66         if (!skb)
  67                 return -ENOMEM;
  68
  69         hdr = (void *) skb_put(skb, sizeof(*hdr));
  70         hdr->opcode = cpu_to_le16(event);
  71         if (hdev)
  72                 hdr->index = cpu_to_le16(hdev->id);
  73         else
  74                 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
  75         hdr->len = cpu_to_le16(data_len);
  76
  77         if (data)
  78                 memcpy(skb_put(skb, data_len), data, data_len);
  79
  80         /* Time stamp */
  81         __net_timestamp(skb);
  82
  83         hci_send_to_channel(channel, skb, flag, skip_sk);
  84
  85         if (channel == HCI_CHANNEL_CONTROL)
  86                 hci_send_monitor_ctrl_event(hdev, event, data, data_len,
  87                                             skb_get_ktime(skb), flag, skip_sk);
  88
  89         kfree_skb(skb);
  90         return 0;
  91 }
  92
  93 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
  94 {
  95         struct sk_buff *skb, *mskb;
  96         struct mgmt_hdr *hdr;
  97         struct mgmt_ev_cmd_status *ev;
  98         int err;
  99
 100         BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
 101
 102         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
 103         if (!skb)
 104                 return -ENOMEM;
 105
 106         hdr = (void *) skb_put(skb, sizeof(*hdr));
 107
 108         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
 109         hdr->index = cpu_to_le16(index);
 110         hdr->len = cpu_to_le16(sizeof(*ev));
 111
 112         ev = (void *) skb_put(skb, sizeof(*ev));
 113         ev->status = status;
 114         ev->opcode = cpu_to_le16(cmd);
 115
 116         mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
 117                                          MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
 118         if (mskb)
 119                 skb->tstamp = mskb->tstamp;
 120         else
 121                 __net_timestamp(skb);
 122
 123         err = sock_queue_rcv_skb(sk, skb);
 124         if (err < 0)
 125                 kfree_skb(skb);
 126
 127         if (mskb) {
 128                 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
 129                                     HCI_SOCK_TRUSTED, NULL);
 130                 kfree_skb(mskb);
 131         }
 132
 133         return err;
 134 }
 135
 136 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
 137                       void *rp, size_t rp_len)
 138 {
 139         struct sk_buff *skb, *mskb;
 140         struct mgmt_hdr *hdr;
 141         struct mgmt_ev_cmd_complete *ev;
 142         int err;
 143
 144         BT_DBG("sock %p", sk);
 145
 146         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
 147         if (!skb)
 148                 return -ENOMEM;
 149
 150         hdr = (void *) skb_put(skb, sizeof(*hdr));
 151
 152         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 153         hdr->index = cpu_to_le16(index);
 154         hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
 155
 156         ev = (void *) skb_put(skb, sizeof(*ev) + rp_len);
 157         ev->opcode = cpu_to_le16(cmd);
 158         ev->status = status;
 159
 160         if (rp)
 161                 memcpy(ev->data, rp, rp_len);
 162
 163         mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
 164                                          MGMT_EV_CMD_COMPLETE,
 165                                          sizeof(*ev) + rp_len, ev);
 166         if (mskb)
 167                 skb->tstamp = mskb->tstamp;
 168         else
 169                 __net_timestamp(skb);
 170
 171         err = sock_queue_rcv_skb(sk, skb);
 172         if (err < 0)
 173                 kfree_skb(skb);
 174
 175         if (mskb) {
 176                 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
 177                                     HCI_SOCK_TRUSTED, NULL);
 178                 kfree_skb(mskb);
 179         }
 180
 181         return err;
 182 }
 183
 184 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
 185                                            struct hci_dev *hdev)
 186 {
 187         struct mgmt_pending_cmd *cmd;
 188
 189         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 190                 if (hci_sock_get_channel(cmd->sk) != channel)
 191                         continue;
 192                 if (cmd->opcode == opcode)
 193                         return cmd;
 194         }
 195
 196         return NULL;
 197 }
 198
 199 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
 200                                                 u16 opcode,
 201                                                 struct hci_dev *hdev,
 202                                                 const void *data)
 203 {
 204         struct mgmt_pending_cmd *cmd;
 205
 206         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 207                 if (cmd->user_data != data)
 208                         continue;
 209                 if (cmd->opcode == opcode)
 210                         return cmd;
 211         }
 212
 213         return NULL;
 214 }
 215
 216 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
 217                           void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
 218                           void *data)
 219 {
 220         struct mgmt_pending_cmd *cmd, *tmp;
 221
 222         list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
 223                 if (opcode > 0 && cmd->opcode != opcode)
 224                         continue;
 225
 226                 cb(cmd, data);
 227         }
 228 }
 229
 230 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
 231                                           struct hci_dev *hdev,
 232                                           void *data, u16 len)
 233 {
 234         struct mgmt_pending_cmd *cmd;
 235
 236         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 237         if (!cmd)
 238                 return NULL;
 239
 240         cmd->opcode = opcode;
 241         cmd->index = hdev->id;
 242
 243         cmd->param = kmemdup(data, len, GFP_KERNEL);
 244         if (!cmd->param) {
 245                 kfree(cmd);
 246                 return NULL;
 247         }
 248
 249         cmd->param_len = len;
 250
 251         cmd->sk = sk;
 252         sock_hold(sk);
 253
 254         list_add(&cmd->list, &hdev->mgmt_pending);
 255
 256         return cmd;
 257 }
 258
 259 void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
 260 {
 261         sock_put(cmd->sk);
 262         kfree(cmd->param);
 263         kfree(cmd);
 264 }
 265
 266 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
 267 {
 268         list_del(&cmd->list);
 269         mgmt_pending_free(cmd);
 270 }