GNU Linux-libre 4.19.211-gnu1
[releases.git] / 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 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 = 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                 skb_put_data(skb, 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 = 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 = 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 = 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 = 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 }