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 | ||
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 | ||
30 | int 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 | /* Time stamp */ | |
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 | ||
60 | int 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 | ||
90 | int 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 | ||
124 | struct 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 | ||
139 | struct 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 | ||
156 | void 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 | ||
170 | struct 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 | ||
199 | void mgmt_pending_free(struct mgmt_pending_cmd *cmd) | |
200 | { | |
201 | sock_put(cmd->sk); | |
202 | kfree(cmd->param); | |
203 | kfree(cmd); | |
204 | } | |
205 | ||
206 | void mgmt_pending_remove(struct mgmt_pending_cmd *cmd) | |
207 | { | |
208 | list_del(&cmd->list); | |
209 | mgmt_pending_free(cmd); | |
210 | } |