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 <net/bluetooth/bluetooth.h>
  25 #include <net/bluetooth/hci_core.h>
  26 #include <net/bluetooth/mgmt.h>
  27
  28 #include "mgmt_util.h"
  29
  30 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
  31                     void *data, u16 data_len, int flag, struct sock *skip_sk)
  32 {
  33         struct sk_buff *skb;
  34         struct mgmt_hdr *hdr;
  35
  36         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL);
  37         if (!skb)
  38                 return -ENOMEM;
  39
  40         hdr = (void *) skb_put(skb, sizeof(*hdr));
  41         hdr->opcode = cpu_to_le16(event);
  42         if (hdev)
  43                 hdr->index = cpu_to_le16(hdev->id);
  44         else
  45                 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
  46         hdr->len = cpu_to_le16(data_len);
  47
  48         if (data)
  49                 memcpy(skb_put(skb, data_len), data, data_len);
  50
  51         /* Time stamp */
  52         __net_timestamp(skb);
  53
  54         hci_send_to_channel(channel, skb, flag, skip_sk);
  55         kfree_skb(skb);
  56
  57         return 0;
  58 }
  59
  60 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
  61 {
  62         struct sk_buff *skb;
  63         struct mgmt_hdr *hdr;
  64         struct mgmt_ev_cmd_status *ev;
  65         int err;
  66
  67         BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
  68
  69         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
  70         if (!skb)
  71                 return -ENOMEM;
  72
  73         hdr = (void *) skb_put(skb, sizeof(*hdr));
  74
  75         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
  76         hdr->index = cpu_to_le16(index);
  77         hdr->len = cpu_to_le16(sizeof(*ev));
  78
  79         ev = (void *) skb_put(skb, sizeof(*ev));
  80         ev->status = status;
  81         ev->opcode = cpu_to_le16(cmd);
  82
  83         err = sock_queue_rcv_skb(sk, skb);
  84         if (err < 0)
  85                 kfree_skb(skb);
  86
  87         return err;
  88 }
  89
  90 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
  91                       void *rp, size_t rp_len)
  92 {
  93         struct sk_buff *skb;
  94         struct mgmt_hdr *hdr;
  95         struct mgmt_ev_cmd_complete *ev;
  96         int err;
  97
  98         BT_DBG("sock %p", sk);
  99
 100         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
 101         if (!skb)
 102                 return -ENOMEM;
 103
 104         hdr = (void *) skb_put(skb, sizeof(*hdr));
 105
 106         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 107         hdr->index = cpu_to_le16(index);
 108         hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
 109
 110         ev = (void *) skb_put(skb, sizeof(*ev) + rp_len);
 111         ev->opcode = cpu_to_le16(cmd);
 112         ev->status = status;
 113
 114         if (rp)
 115                 memcpy(ev->data, rp, rp_len);
 116
 117         err = sock_queue_rcv_skb(sk, skb);
 118         if (err < 0)
 119                 kfree_skb(skb);
 120
 121         return err;
 122 }
 123
 124 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
 125                                            struct hci_dev *hdev)
 126 {
 127         struct mgmt_pending_cmd *cmd;
 128
 129         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 130                 if (hci_sock_get_channel(cmd->sk) != channel)
 131                         continue;
 132                 if (cmd->opcode == opcode)
 133                         return cmd;
 134         }
 135
 136         return NULL;
 137 }
 138
 139 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
 140                                                 u16 opcode,
 141                                                 struct hci_dev *hdev,
 142                                                 const void *data)
 143 {
 144         struct mgmt_pending_cmd *cmd;
 145
 146         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 147                 if (cmd->user_data != data)
 148                         continue;
 149                 if (cmd->opcode == opcode)
 150                         return cmd;
 151         }
 152
 153         return NULL;
 154 }
 155
 156 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
 157                           void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
 158                           void *data)
 159 {
 160         struct mgmt_pending_cmd *cmd, *tmp;
 161
 162         list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
 163                 if (opcode > 0 && cmd->opcode != opcode)
 164                         continue;
 165
 166                 cb(cmd, data);
 167         }
 168 }
 169
 170 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
 171                                           struct hci_dev *hdev,
 172                                           void *data, u16 len)
 173 {
 174         struct mgmt_pending_cmd *cmd;
 175
 176         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 177         if (!cmd)
 178                 return NULL;
 179
 180         cmd->opcode = opcode;
 181         cmd->index = hdev->id;
 182
 183         cmd->param = kmemdup(data, len, GFP_KERNEL);
 184         if (!cmd->param) {
 185                 kfree(cmd);
 186                 return NULL;
 187         }
 188
 189         cmd->param_len = len;
 190
 191         cmd->sk = sk;
 192         sock_hold(sk);
 193
 194         list_add(&cmd->list, &hdev->mgmt_pending);
 195
 196         return cmd;
 197 }
 198
 199 void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
 200 {
 201         sock_put(cmd->sk);
 202         kfree(cmd->param);
 203         kfree(cmd);
 204 }
 205
 206 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
 207 {
 208         list_del(&cmd->list);
 209         mgmt_pending_free(cmd);
 210 }