Merge tag 'integrity-v5.17' of git://git.kernel.org/pub/scm/linux/kernel/git/zohar...
[linux-2.6-microblaze.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 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 }