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                 skb_put_data(skb, buf, len);
  48
  49         __net_timestamp(skb);
  50
  51         hdr = 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 struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode,
  60                                unsigned int size)
  61 {
  62         struct sk_buff *skb;
  63
  64         skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL);
  65         if (!skb)
  66                 return skb;
  67
  68         skb_reserve(skb, sizeof(struct mgmt_hdr));
  69         bt_cb(skb)->mgmt.hdev = hdev;
  70         bt_cb(skb)->mgmt.opcode = opcode;
  71
  72         return skb;
  73 }
  74
  75 int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag,
  76                         struct sock *skip_sk)
  77 {
  78         struct hci_dev *hdev;
  79         struct mgmt_hdr *hdr;
  80         int len = skb->len;
  81
  82         if (!skb)
  83                 return -EINVAL;
  84
  85         hdev = bt_cb(skb)->mgmt.hdev;
  86
  87         /* Time stamp */
  88         __net_timestamp(skb);
  89
  90         /* Send just the data, without headers, to the monitor */
  91         if (channel == HCI_CHANNEL_CONTROL)
  92                 hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode,
  93                                             skb->data, skb->len,
  94                                             skb_get_ktime(skb), flag, skip_sk);
  95
  96         hdr = skb_push(skb, sizeof(*hdr));
  97         hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode);
  98         if (hdev)
  99                 hdr->index = cpu_to_le16(hdev->id);
 100         else
 101                 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
 102         hdr->len = cpu_to_le16(len);
 103
 104         hci_send_to_channel(channel, skb, flag, skip_sk);
 105
 106         kfree_skb(skb);
 107         return 0;
 108 }
 109
 110 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
 111                     void *data, u16 data_len, int flag, struct sock *skip_sk)
 112 {
 113         struct sk_buff *skb;
 114
 115         skb = mgmt_alloc_skb(hdev, event, data_len);
 116         if (!skb)
 117                 return -ENOMEM;
 118
 119         if (data)
 120                 skb_put_data(skb, data, data_len);
 121
 122         return mgmt_send_event_skb(channel, skb, flag, skip_sk);
 123 }
 124
 125 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
 126 {
 127         struct sk_buff *skb, *mskb;
 128         struct mgmt_hdr *hdr;
 129         struct mgmt_ev_cmd_status *ev;
 130         int err;
 131
 132         BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
 133
 134         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
 135         if (!skb)
 136                 return -ENOMEM;
 137
 138         hdr = skb_put(skb, sizeof(*hdr));
 139
 140         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
 141         hdr->index = cpu_to_le16(index);
 142         hdr->len = cpu_to_le16(sizeof(*ev));
 143
 144         ev = skb_put(skb, sizeof(*ev));
 145         ev->status = status;
 146         ev->opcode = cpu_to_le16(cmd);
 147
 148         mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
 149                                          MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
 150         if (mskb)
 151                 skb->tstamp = mskb->tstamp;
 152         else
 153                 __net_timestamp(skb);
 154
 155         err = sock_queue_rcv_skb(sk, skb);
 156         if (err < 0)
 157                 kfree_skb(skb);
 158
 159         if (mskb) {
 160                 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
 161                                     HCI_SOCK_TRUSTED, NULL);
 162                 kfree_skb(mskb);
 163         }
 164
 165         return err;
 166 }
 167
 168 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
 169                       void *rp, size_t rp_len)
 170 {
 171         struct sk_buff *skb, *mskb;
 172         struct mgmt_hdr *hdr;
 173         struct mgmt_ev_cmd_complete *ev;
 174         int err;
 175
 176         BT_DBG("sock %p", sk);
 177
 178         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
 179         if (!skb)
 180                 return -ENOMEM;
 181
 182         hdr = skb_put(skb, sizeof(*hdr));
 183
 184         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 185         hdr->index = cpu_to_le16(index);
 186         hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
 187
 188         ev = skb_put(skb, sizeof(*ev) + rp_len);
 189         ev->opcode = cpu_to_le16(cmd);
 190         ev->status = status;
 191
 192         if (rp)
 193                 memcpy(ev->data, rp, rp_len);
 194
 195         mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
 196                                          MGMT_EV_CMD_COMPLETE,
 197                                          sizeof(*ev) + rp_len, ev);
 198         if (mskb)
 199                 skb->tstamp = mskb->tstamp;
 200         else
 201                 __net_timestamp(skb);
 202
 203         err = sock_queue_rcv_skb(sk, skb);
 204         if (err < 0)
 205                 kfree_skb(skb);
 206
 207         if (mskb) {
 208                 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
 209                                     HCI_SOCK_TRUSTED, NULL);
 210                 kfree_skb(mskb);
 211         }
 212
 213         return err;
 214 }
 215
 216 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
 217                                            struct hci_dev *hdev)
 218 {
 219         struct mgmt_pending_cmd *cmd;
 220
 221         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 222                 if (hci_sock_get_channel(cmd->sk) != channel)
 223                         continue;
 224                 if (cmd->opcode == opcode)
 225                         return cmd;
 226         }
 227
 228         return NULL;
 229 }
 230
 231 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
 232                                                 u16 opcode,
 233                                                 struct hci_dev *hdev,
 234                                                 const void *data)
 235 {
 236         struct mgmt_pending_cmd *cmd;
 237
 238         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 239                 if (cmd->user_data != data)
 240                         continue;
 241                 if (cmd->opcode == opcode)
 242                         return cmd;
 243         }
 244
 245         return NULL;
 246 }
 247
 248 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
 249                           void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
 250                           void *data)
 251 {
 252         struct mgmt_pending_cmd *cmd, *tmp;
 253
 254         list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
 255                 if (opcode > 0 && cmd->opcode != opcode)
 256                         continue;
 257
 258                 cb(cmd, data);
 259         }
 260 }
 261
 262 struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode,
 263                                           struct hci_dev *hdev,
 264                                           void *data, u16 len)
 265 {
 266         struct mgmt_pending_cmd *cmd;
 267
 268         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 269         if (!cmd)
 270                 return NULL;
 271
 272         cmd->opcode = opcode;
 273         cmd->index = hdev->id;
 274
 275         cmd->param = kmemdup(data, len, GFP_KERNEL);
 276         if (!cmd->param) {
 277                 kfree(cmd);
 278                 return NULL;
 279         }
 280
 281         cmd->param_len = len;
 282
 283         cmd->sk = sk;
 284         sock_hold(sk);
 285
 286         return cmd;
 287 }
 288
 289 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
 290                                           struct hci_dev *hdev,
 291                                           void *data, u16 len)
 292 {
 293         struct mgmt_pending_cmd *cmd;
 294
 295         cmd = mgmt_pending_new(sk, opcode, hdev, data, len);
 296         if (!cmd)
 297                 return NULL;
 298
 299         list_add(&cmd->list, &hdev->mgmt_pending);
 300
 301         return cmd;
 302 }
 303
 304 void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
 305 {
 306         sock_put(cmd->sk);
 307         kfree(cmd->param);
 308         kfree(cmd);
 309 }
 310
 311 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
 312 {
 313         list_del(&cmd->list);
 314         mgmt_pending_free(cmd);
 315 }