1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
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
33static 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
59int 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
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
93int 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
136int 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
184struct 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
199struct 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
216void 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
230struct 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
259void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
260{
261 sock_put(cmd->sk);
262 kfree(cmd->param);
263 kfree(cmd);
264}
265
266void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
267{
268 list_del(&cmd->list);
269 mgmt_pending_free(cmd);
270}
271
