Commit | Line | Data |
---|---|---|
a380b6cf JH |
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 | ||
38ceaa00 MH |
24 | #include <asm/unaligned.h> |
25 | ||
a380b6cf JH |
26 | #include <net/bluetooth/bluetooth.h> |
27 | #include <net/bluetooth/hci_core.h> | |
38ceaa00 | 28 | #include <net/bluetooth/hci_mon.h> |
a380b6cf JH |
29 | #include <net/bluetooth/mgmt.h> |
30 | ||
31 | #include "mgmt_util.h" | |
32 | ||
38ceaa00 MH |
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) | |
59ae1d12 | 47 | skb_put_data(skb, buf, len); |
38ceaa00 MH |
48 | |
49 | __net_timestamp(skb); | |
50 | ||
51 | hdr = (void *)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 | ||
a380b6cf JH |
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 = (void *) 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) | |
59ae1d12 | 78 | skb_put_data(skb, data, data_len); |
a380b6cf JH |
79 | |
80 | /* Time stamp */ | |
81 | __net_timestamp(skb); | |
82 | ||
83 | hci_send_to_channel(channel, skb, flag, skip_sk); | |
a380b6cf | 84 | |
38ceaa00 MH |
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); | |
a380b6cf JH |
90 | return 0; |
91 | } | |
92 | ||
93 | int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) | |
94 | { | |
38ceaa00 | 95 | struct sk_buff *skb, *mskb; |
a380b6cf JH |
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 = (void *) 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 = (void *) skb_put(skb, sizeof(*ev)); | |
113 | ev->status = status; | |
114 | ev->opcode = cpu_to_le16(cmd); | |
115 | ||
38ceaa00 MH |
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 | ||
a380b6cf JH |
123 | err = sock_queue_rcv_skb(sk, skb); |
124 | if (err < 0) | |
125 | kfree_skb(skb); | |
126 | ||
38ceaa00 MH |
127 | if (mskb) { |
128 | hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, | |
129 | HCI_SOCK_TRUSTED, NULL); | |
130 | kfree_skb(mskb); | |
131 | } | |
132 | ||
a380b6cf JH |
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 | { | |
38ceaa00 | 139 | struct sk_buff *skb, *mskb; |
a380b6cf JH |
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 = (void *) 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 = (void *) 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 | ||
38ceaa00 MH |
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 | ||
a380b6cf JH |
171 | err = sock_queue_rcv_skb(sk, skb); |
172 | if (err < 0) | |
173 | kfree_skb(skb); | |
174 | ||
38ceaa00 MH |
175 | if (mskb) { |
176 | hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, | |
177 | HCI_SOCK_TRUSTED, NULL); | |
178 | kfree_skb(mskb); | |
179 | } | |
180 | ||
a380b6cf JH |
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 | } |