net/bluetooth/mgmt_util.c

0001 /*
0002    BlueZ - Bluetooth protocol stack for Linux
0003
0004    Copyright (C) 2015  Intel Corporation
0005
0006    This program is free software; you can redistribute it and/or modify
0007    it under the terms of the GNU General Public License version 2 as
0008    published by the Free Software Foundation;
0009
0010    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
0011    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
0012    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
0013    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
0014    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
0015    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
0016    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
0017    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
0018
0019    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
0020    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
0021    SOFTWARE IS DISCLAIMED.
0022 */
0023
0024 #include <asm/unaligned.h>
0025
0026 #include <net/bluetooth/bluetooth.h>
0027 #include <net/bluetooth/hci_core.h>
0028 #include <net/bluetooth/hci_mon.h>
0029 #include <net/bluetooth/mgmt.h>
0030
0031 #include "mgmt_util.h"
0032
0033 static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie,
0034                          u16 opcode, u16 len, void *buf)
0035 {
0036     struct hci_mon_hdr *hdr;
0037     struct sk_buff *skb;
0038
0039     skb = bt_skb_alloc(6 + len, GFP_ATOMIC);
0040     if (!skb)
0041         return NULL;
0042
0043     put_unaligned_le32(cookie, skb_put(skb, 4));
0044     put_unaligned_le16(opcode, skb_put(skb, 2));
0045
0046     if (buf)
0047         skb_put_data(skb, buf, len);
0048
0049     __net_timestamp(skb);
0050
0051     hdr = skb_push(skb, HCI_MON_HDR_SIZE);
0052     hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);
0053     hdr->index = index;
0054     hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);
0055
0056     return skb;
0057 }
0058
0059 struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode,
0060                    unsigned int size)
0061 {
0062     struct sk_buff *skb;
0063
0064     skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL);
0065     if (!skb)
0066         return skb;
0067
0068     skb_reserve(skb, sizeof(struct mgmt_hdr));
0069     bt_cb(skb)->mgmt.hdev = hdev;
0070     bt_cb(skb)->mgmt.opcode = opcode;
0071
0072     return skb;
0073 }
0074
0075 int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag,
0076             struct sock *skip_sk)
0077 {
0078     struct hci_dev *hdev;
0079     struct mgmt_hdr *hdr;
0080     int len;
0081
0082     if (!skb)
0083         return -EINVAL;
0084
0085     len = skb->len;
0086     hdev = bt_cb(skb)->mgmt.hdev;
0087
0088     /* Time stamp */
0089     __net_timestamp(skb);
0090
0091     /* Send just the data, without headers, to the monitor */
0092     if (channel == HCI_CHANNEL_CONTROL)
0093         hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode,
0094                         skb->data, skb->len,
0095                         skb_get_ktime(skb), flag, skip_sk);
0096
0097     hdr = skb_push(skb, sizeof(*hdr));
0098     hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode);
0099     if (hdev)
0100         hdr->index = cpu_to_le16(hdev->id);
0101     else
0102         hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
0103     hdr->len = cpu_to_le16(len);
0104
0105     hci_send_to_channel(channel, skb, flag, skip_sk);
0106
0107     kfree_skb(skb);
0108     return 0;
0109 }
0110
0111 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
0112             void *data, u16 data_len, int flag, struct sock *skip_sk)
0113 {
0114     struct sk_buff *skb;
0115
0116     skb = mgmt_alloc_skb(hdev, event, data_len);
0117     if (!skb)
0118         return -ENOMEM;
0119
0120     if (data)
0121         skb_put_data(skb, data, data_len);
0122
0123     return mgmt_send_event_skb(channel, skb, flag, skip_sk);
0124 }
0125
0126 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
0127 {
0128     struct sk_buff *skb, *mskb;
0129     struct mgmt_hdr *hdr;
0130     struct mgmt_ev_cmd_status *ev;
0131     int err;
0132
0133     BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
0134
0135     skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
0136     if (!skb)
0137         return -ENOMEM;
0138
0139     hdr = skb_put(skb, sizeof(*hdr));
0140
0141     hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
0142     hdr->index = cpu_to_le16(index);
0143     hdr->len = cpu_to_le16(sizeof(*ev));
0144
0145     ev = skb_put(skb, sizeof(*ev));
0146     ev->status = status;
0147     ev->opcode = cpu_to_le16(cmd);
0148
0149     mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
0150                      MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
0151     if (mskb)
0152         skb->tstamp = mskb->tstamp;
0153     else
0154         __net_timestamp(skb);
0155
0156     err = sock_queue_rcv_skb(sk, skb);
0157     if (err < 0)
0158         kfree_skb(skb);
0159
0160     if (mskb) {
0161         hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
0162                     HCI_SOCK_TRUSTED, NULL);
0163         kfree_skb(mskb);
0164     }
0165
0166     return err;
0167 }
0168
0169 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
0170               void *rp, size_t rp_len)
0171 {
0172     struct sk_buff *skb, *mskb;
0173     struct mgmt_hdr *hdr;
0174     struct mgmt_ev_cmd_complete *ev;
0175     int err;
0176
0177     BT_DBG("sock %p", sk);
0178
0179     skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
0180     if (!skb)
0181         return -ENOMEM;
0182
0183     hdr = skb_put(skb, sizeof(*hdr));
0184
0185     hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
0186     hdr->index = cpu_to_le16(index);
0187     hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
0188
0189     ev = skb_put(skb, sizeof(*ev) + rp_len);
0190     ev->opcode = cpu_to_le16(cmd);
0191     ev->status = status;
0192
0193     if (rp)
0194         memcpy(ev->data, rp, rp_len);
0195
0196     mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
0197                      MGMT_EV_CMD_COMPLETE,
0198                      sizeof(*ev) + rp_len, ev);
0199     if (mskb)
0200         skb->tstamp = mskb->tstamp;
0201     else
0202         __net_timestamp(skb);
0203
0204     err = sock_queue_rcv_skb(sk, skb);
0205     if (err < 0)
0206         kfree_skb(skb);
0207
0208     if (mskb) {
0209         hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
0210                     HCI_SOCK_TRUSTED, NULL);
0211         kfree_skb(mskb);
0212     }
0213
0214     return err;
0215 }
0216
0217 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
0218                        struct hci_dev *hdev)
0219 {
0220     struct mgmt_pending_cmd *cmd;
0221
0222     list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
0223         if (hci_sock_get_channel(cmd->sk) != channel)
0224             continue;
0225         if (cmd->opcode == opcode)
0226             return cmd;
0227     }
0228
0229     return NULL;
0230 }
0231
0232 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
0233                         u16 opcode,
0234                         struct hci_dev *hdev,
0235                         const void *data)
0236 {
0237     struct mgmt_pending_cmd *cmd;
0238
0239     list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
0240         if (cmd->user_data != data)
0241             continue;
0242         if (cmd->opcode == opcode)
0243             return cmd;
0244     }
0245
0246     return NULL;
0247 }
0248
0249 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
0250               void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
0251               void *data)
0252 {
0253     struct mgmt_pending_cmd *cmd, *tmp;
0254
0255     list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
0256         if (opcode > 0 && cmd->opcode != opcode)
0257             continue;
0258
0259         cb(cmd, data);
0260     }
0261 }
0262
0263 struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode,
0264                       struct hci_dev *hdev,
0265                       void *data, u16 len)
0266 {
0267     struct mgmt_pending_cmd *cmd;
0268
0269     cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
0270     if (!cmd)
0271         return NULL;
0272
0273     cmd->opcode = opcode;
0274     cmd->index = hdev->id;
0275
0276     cmd->param = kmemdup(data, len, GFP_KERNEL);
0277     if (!cmd->param) {
0278         kfree(cmd);
0279         return NULL;
0280     }
0281
0282     cmd->param_len = len;
0283
0284     cmd->sk = sk;
0285     sock_hold(sk);
0286
0287     return cmd;
0288 }
0289
0290 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
0291                       struct hci_dev *hdev,
0292                       void *data, u16 len)
0293 {
0294     struct mgmt_pending_cmd *cmd;
0295
0296     cmd = mgmt_pending_new(sk, opcode, hdev, data, len);
0297     if (!cmd)
0298         return NULL;
0299
0300     list_add_tail(&cmd->list, &hdev->mgmt_pending);
0301
0302     return cmd;
0303 }
0304
0305 void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
0306 {
0307     sock_put(cmd->sk);
0308     kfree(cmd->param);
0309     kfree(cmd);
0310 }
0311
0312 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
0313 {
0314     list_del(&cmd->list);
0315     mgmt_pending_free(cmd);
0316 }