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 <net/bluetooth/bluetooth.h>
25#include <net/bluetooth/hci_core.h>
26#include <net/bluetooth/mgmt.h>
27
28#include "mgmt_util.h"
29
30int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
31 void *data, u16 data_len, int flag, struct sock *skip_sk)
32{
33 struct sk_buff *skb;
34 struct mgmt_hdr *hdr;
35
36 skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL);
37 if (!skb)
38 return -ENOMEM;
39
40 hdr = (void *) skb_put(skb, sizeof(*hdr));
41 hdr->opcode = cpu_to_le16(event);
42 if (hdev)
43 hdr->index = cpu_to_le16(hdev->id);
44 else
45 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
46 hdr->len = cpu_to_le16(data_len);
47
48 if (data)
49 memcpy(skb_put(skb, data_len), data, data_len);
50
51
52 __net_timestamp(skb);
53
54 hci_send_to_channel(channel, skb, flag, skip_sk);
55 kfree_skb(skb);
56
57 return 0;
58}
59
60int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
61{
62 struct sk_buff *skb;
63 struct mgmt_hdr *hdr;
64 struct mgmt_ev_cmd_status *ev;
65 int err;
66
67 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
68
69 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
70 if (!skb)
71 return -ENOMEM;
72
73 hdr = (void *) skb_put(skb, sizeof(*hdr));
74
75 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
76 hdr->index = cpu_to_le16(index);
77 hdr->len = cpu_to_le16(sizeof(*ev));
78
79 ev = (void *) skb_put(skb, sizeof(*ev));
80 ev->status = status;
81 ev->opcode = cpu_to_le16(cmd);
82
83 err = sock_queue_rcv_skb(sk, skb);
84 if (err < 0)
85 kfree_skb(skb);
86
87 return err;
88}
89
90int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
91 void *rp, size_t rp_len)
92{
93 struct sk_buff *skb;
94 struct mgmt_hdr *hdr;
95 struct mgmt_ev_cmd_complete *ev;
96 int err;
97
98 BT_DBG("sock %p", sk);
99
100 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
101 if (!skb)
102 return -ENOMEM;
103
104 hdr = (void *) skb_put(skb, sizeof(*hdr));
105
106 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
107 hdr->index = cpu_to_le16(index);
108 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
109
110 ev = (void *) skb_put(skb, sizeof(*ev) + rp_len);
111 ev->opcode = cpu_to_le16(cmd);
112 ev->status = status;
113
114 if (rp)
115 memcpy(ev->data, rp, rp_len);
116
117 err = sock_queue_rcv_skb(sk, skb);
118 if (err < 0)
119 kfree_skb(skb);
120
121 return err;
122}
123
124struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
125 struct hci_dev *hdev)
126{
127 struct mgmt_pending_cmd *cmd;
128
129 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
130 if (hci_sock_get_channel(cmd->sk) != channel)
131 continue;
132 if (cmd->opcode == opcode)
133 return cmd;
134 }
135
136 return NULL;
137}
138
139struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
140 u16 opcode,
141 struct hci_dev *hdev,
142 const void *data)
143{
144 struct mgmt_pending_cmd *cmd;
145
146 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
147 if (cmd->user_data != data)
148 continue;
149 if (cmd->opcode == opcode)
150 return cmd;
151 }
152
153 return NULL;
154}
155
156void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
157 void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
158 void *data)
159{
160 struct mgmt_pending_cmd *cmd, *tmp;
161
162 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
163 if (opcode > 0 && cmd->opcode != opcode)
164 continue;
165
166 cb(cmd, data);
167 }
168}
169
170struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
171 struct hci_dev *hdev,
172 void *data, u16 len)
173{
174 struct mgmt_pending_cmd *cmd;
175
176 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
177 if (!cmd)
178 return NULL;
179
180 cmd->opcode = opcode;
181 cmd->index = hdev->id;
182
183 cmd->param = kmemdup(data, len, GFP_KERNEL);
184 if (!cmd->param) {
185 kfree(cmd);
186 return NULL;
187 }
188
189 cmd->param_len = len;
190
191 cmd->sk = sk;
192 sock_hold(sk);
193
194 list_add(&cmd->list, &hdev->mgmt_pending);
195
196 return cmd;
197}
198
199void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
200{
201 sock_put(cmd->sk);
202 kfree(cmd->param);
203 kfree(cmd);
204}
205
206void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
207{
208 list_del(&cmd->list);
209 mgmt_pending_free(cmd);
210}
211