Commit | Line | Data |
---|---|---|
22d203ec IPG |
1 | /* |
2 | * Ultra Wide Band | |
3 | * Information Element Handling | |
4 | * | |
5 | * Copyright (C) 2005-2006 Intel Corporation | |
6 | * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com> | |
7 | * Reinette Chatre <reinette.chatre@intel.com> | |
8 | * | |
9 | * This program is free software; you can redistribute it and/or | |
10 | * modify it under the terms of the GNU General Public License version | |
11 | * 2 as published by the Free Software Foundation. | |
12 | * | |
13 | * This program is distributed in the hope that it will be useful, | |
14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
16 | * GNU General Public License for more details. | |
17 | * | |
18 | * You should have received a copy of the GNU General Public License | |
19 | * along with this program; if not, write to the Free Software | |
20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA | |
21 | * 02110-1301, USA. | |
22 | * | |
23 | * | |
24 | * FIXME: docs | |
25 | */ | |
26 | ||
5a0e3ad6 | 27 | #include <linux/slab.h> |
22d203ec | 28 | #include "uwb-internal.h" |
22d203ec IPG |
29 | |
30 | /** | |
31 | * uwb_ie_next - get the next IE in a buffer | |
32 | * @ptr: start of the buffer containing the IE data | |
33 | * @len: length of the buffer | |
34 | * | |
35 | * Both @ptr and @len are updated so subsequent calls to uwb_ie_next() | |
36 | * will get the next IE. | |
37 | * | |
38 | * NULL is returned (and @ptr and @len will not be updated) if there | |
39 | * are no more IEs in the buffer or the buffer is too short. | |
40 | */ | |
41 | struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len) | |
42 | { | |
43 | struct uwb_ie_hdr *hdr; | |
44 | size_t ie_len; | |
45 | ||
46 | if (*len < sizeof(struct uwb_ie_hdr)) | |
47 | return NULL; | |
48 | ||
49 | hdr = *ptr; | |
50 | ie_len = sizeof(struct uwb_ie_hdr) + hdr->length; | |
51 | ||
52 | if (*len < ie_len) | |
53 | return NULL; | |
54 | ||
55 | *ptr += ie_len; | |
56 | *len -= ie_len; | |
57 | ||
58 | return hdr; | |
59 | } | |
60 | EXPORT_SYMBOL_GPL(uwb_ie_next); | |
61 | ||
1cde7f68 DV |
62 | /** |
63 | * uwb_ie_dump_hex - print IEs to a character buffer | |
64 | * @ies: the IEs to print. | |
65 | * @len: length of all the IEs. | |
66 | * @buf: the destination buffer. | |
67 | * @size: size of @buf. | |
68 | * | |
69 | * Returns the number of characters written. | |
70 | */ | |
71 | int uwb_ie_dump_hex(const struct uwb_ie_hdr *ies, size_t len, | |
72 | char *buf, size_t size) | |
73 | { | |
74 | void *ptr; | |
75 | const struct uwb_ie_hdr *ie; | |
76 | int r = 0; | |
77 | u8 *d; | |
78 | ||
79 | ptr = (void *)ies; | |
80 | for (;;) { | |
81 | ie = uwb_ie_next(&ptr, &len); | |
82 | if (!ie) | |
83 | break; | |
84 | ||
85 | r += scnprintf(buf + r, size - r, "%02x %02x", | |
86 | (unsigned)ie->element_id, | |
87 | (unsigned)ie->length); | |
88 | d = (uint8_t *)ie + sizeof(struct uwb_ie_hdr); | |
89 | while (d != ptr && r < size) | |
90 | r += scnprintf(buf + r, size - r, " %02x", (unsigned)*d++); | |
91 | if (r < size) | |
92 | buf[r++] = '\n'; | |
93 | }; | |
94 | ||
95 | return r; | |
96 | } | |
97 | ||
22d203ec IPG |
98 | /** |
99 | * Get the IEs that a radio controller is sending in its beacon | |
100 | * | |
101 | * @uwb_rc: UWB Radio Controller | |
102 | * @returns: Size read from the system | |
103 | * | |
104 | * We don't need to lock the uwb_rc's mutex because we don't modify | |
105 | * anything. Once done with the iedata buffer, call | |
106 | * uwb_rc_ie_release(iedata). Don't call kfree on it. | |
107 | */ | |
1cde7f68 | 108 | static |
22d203ec IPG |
109 | ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie) |
110 | { | |
111 | ssize_t result; | |
112 | struct device *dev = &uwb_rc->uwb_dev.dev; | |
113 | struct uwb_rccb *cmd = NULL; | |
114 | struct uwb_rceb *reply = NULL; | |
115 | struct uwb_rc_evt_get_ie *get_ie; | |
116 | ||
22d203ec IPG |
117 | cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); |
118 | if (cmd == NULL) | |
1cde7f68 DV |
119 | return -ENOMEM; |
120 | ||
22d203ec IPG |
121 | cmd->bCommandType = UWB_RC_CET_GENERAL; |
122 | cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE); | |
123 | result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd), | |
124 | UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE, | |
125 | &reply); | |
1cde7f68 | 126 | kfree(cmd); |
22d203ec | 127 | if (result < 0) |
1cde7f68 DV |
128 | return result; |
129 | ||
22d203ec IPG |
130 | get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb); |
131 | if (result < sizeof(*get_ie)) { | |
132 | dev_err(dev, "not enough data returned for decoding GET IE " | |
133 | "(%zu bytes received vs %zu needed)\n", | |
134 | result, sizeof(*get_ie)); | |
1cde7f68 | 135 | return -EINVAL; |
22d203ec IPG |
136 | } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) { |
137 | dev_err(dev, "not enough data returned for decoding GET IE " | |
138 | "payload (%zu bytes received vs %zu needed)\n", result, | |
139 | sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)); | |
22d203ec IPG |
140 | return -EINVAL; |
141 | } | |
22d203ec | 142 | |
1cde7f68 | 143 | *pget_ie = get_ie; |
22d203ec IPG |
144 | return result; |
145 | } | |
22d203ec IPG |
146 | |
147 | ||
148 | /** | |
149 | * Replace all IEs currently being transmitted by a device | |
150 | * | |
151 | * @cmd: pointer to the SET-IE command with the IEs to set | |
152 | * @size: size of @buf | |
153 | */ | |
154 | int uwb_rc_set_ie(struct uwb_rc *rc, struct uwb_rc_cmd_set_ie *cmd) | |
155 | { | |
156 | int result; | |
157 | struct device *dev = &rc->uwb_dev.dev; | |
158 | struct uwb_rc_evt_set_ie reply; | |
159 | ||
160 | reply.rceb.bEventType = UWB_RC_CET_GENERAL; | |
161 | reply.rceb.wEvent = UWB_RC_CMD_SET_IE; | |
162 | result = uwb_rc_cmd(rc, "SET-IE", &cmd->rccb, | |
163 | sizeof(*cmd) + le16_to_cpu(cmd->wIELength), | |
164 | &reply.rceb, sizeof(reply)); | |
165 | if (result < 0) | |
166 | goto error_cmd; | |
167 | else if (result != sizeof(reply)) { | |
168 | dev_err(dev, "SET-IE: not enough data to decode reply " | |
169 | "(%d bytes received vs %zu needed)\n", | |
170 | result, sizeof(reply)); | |
171 | result = -EIO; | |
172 | } else if (reply.bResultCode != UWB_RC_RES_SUCCESS) { | |
173 | dev_err(dev, "SET-IE: command execution failed: %s (%d)\n", | |
174 | uwb_rc_strerror(reply.bResultCode), reply.bResultCode); | |
175 | result = -EIO; | |
176 | } else | |
177 | result = 0; | |
178 | error_cmd: | |
179 | return result; | |
180 | } | |
181 | ||
22d203ec IPG |
182 | /* Cleanup the whole IE management subsystem */ |
183 | void uwb_rc_ie_init(struct uwb_rc *uwb_rc) | |
184 | { | |
185 | mutex_init(&uwb_rc->ies_mutex); | |
186 | } | |
187 | ||
188 | ||
189 | /** | |
1cde7f68 DV |
190 | * uwb_rc_ie_setup - setup a radio controller's IE manager |
191 | * @uwb_rc: the radio controller. | |
22d203ec | 192 | * |
1cde7f68 DV |
193 | * The current set of IEs are obtained from the hardware with a GET-IE |
194 | * command (since the radio controller is not yet beaconing this will | |
195 | * be just the hardware's MAC and PHY Capability IEs). | |
22d203ec | 196 | * |
1cde7f68 | 197 | * Returns 0 on success; -ve on an error. |
22d203ec | 198 | */ |
1cde7f68 | 199 | int uwb_rc_ie_setup(struct uwb_rc *uwb_rc) |
22d203ec | 200 | { |
1cde7f68 DV |
201 | struct uwb_rc_evt_get_ie *ie_info = NULL; |
202 | int capacity; | |
203 | ||
204 | capacity = uwb_rc_get_ie(uwb_rc, &ie_info); | |
205 | if (capacity < 0) | |
206 | return capacity; | |
22d203ec | 207 | |
22d203ec | 208 | mutex_lock(&uwb_rc->ies_mutex); |
1cde7f68 DV |
209 | |
210 | uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info; | |
22d203ec IPG |
211 | uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL; |
212 | uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE); | |
213 | uwb_rc->ies_capacity = capacity; | |
1cde7f68 | 214 | |
22d203ec | 215 | mutex_unlock(&uwb_rc->ies_mutex); |
1cde7f68 DV |
216 | |
217 | return 0; | |
22d203ec IPG |
218 | } |
219 | ||
220 | ||
221 | /* Cleanup the whole IE management subsystem */ | |
222 | void uwb_rc_ie_release(struct uwb_rc *uwb_rc) | |
223 | { | |
224 | kfree(uwb_rc->ies); | |
225 | uwb_rc->ies = NULL; | |
226 | uwb_rc->ies_capacity = 0; | |
227 | } | |
228 | ||
229 | ||
1cde7f68 | 230 | static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie) |
22d203ec | 231 | { |
1cde7f68 DV |
232 | struct uwb_rc_cmd_set_ie *new_ies; |
233 | void *ptr, *prev_ie; | |
234 | struct uwb_ie_hdr *ie; | |
235 | size_t length, new_ie_len, new_capacity, size, prev_size; | |
236 | ||
237 | length = le16_to_cpu(rc->ies->wIELength); | |
238 | new_ie_len = sizeof(struct uwb_ie_hdr) + new_ie->length; | |
239 | new_capacity = sizeof(struct uwb_rc_cmd_set_ie) + length + new_ie_len; | |
240 | ||
241 | if (new_capacity > rc->ies_capacity) { | |
242 | new_ies = krealloc(rc->ies, new_capacity, GFP_KERNEL); | |
243 | if (!new_ies) | |
244 | return -ENOMEM; | |
245 | rc->ies = new_ies; | |
246 | } | |
247 | ||
248 | ptr = rc->ies->IEData; | |
249 | size = length; | |
250 | for (;;) { | |
251 | prev_ie = ptr; | |
252 | prev_size = size; | |
253 | ie = uwb_ie_next(&ptr, &size); | |
254 | if (!ie || ie->element_id > new_ie->element_id) | |
255 | break; | |
256 | } | |
257 | ||
258 | memmove(prev_ie + new_ie_len, prev_ie, prev_size); | |
259 | memcpy(prev_ie, new_ie, new_ie_len); | |
260 | rc->ies->wIELength = cpu_to_le16(length + new_ie_len); | |
261 | ||
22d203ec IPG |
262 | return 0; |
263 | } | |
264 | ||
22d203ec | 265 | /** |
1cde7f68 DV |
266 | * uwb_rc_ie_add - add new IEs to the radio controller's beacon |
267 | * @uwb_rc: the radio controller. | |
22d203ec | 268 | * @ies: the buffer containing the new IE or IEs to be added to |
1cde7f68 DV |
269 | * the device's beacon. |
270 | * @size: length of all the IEs. | |
22d203ec IPG |
271 | * |
272 | * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB | |
273 | * after the device sent the first beacon that includes the IEs specified | |
274 | * in the SET IE command. We thus cannot send this command if the device is | |
275 | * not beaconing. Instead, a SET IE command will be sent later right after | |
276 | * we start beaconing. | |
277 | * | |
278 | * Setting an IE on the device will overwrite all current IEs in device. So | |
1cde7f68 | 279 | * we take the current IEs being transmitted by the device, insert the |
22d203ec IPG |
280 | * new one, and call SET IE with all the IEs needed. |
281 | * | |
1cde7f68 | 282 | * Returns 0 on success; or -ENOMEM. |
22d203ec IPG |
283 | */ |
284 | int uwb_rc_ie_add(struct uwb_rc *uwb_rc, | |
285 | const struct uwb_ie_hdr *ies, size_t size) | |
286 | { | |
287 | int result = 0; | |
1cde7f68 DV |
288 | void *ptr; |
289 | const struct uwb_ie_hdr *ie; | |
290 | ||
22d203ec | 291 | mutex_lock(&uwb_rc->ies_mutex); |
1cde7f68 DV |
292 | |
293 | ptr = (void *)ies; | |
294 | for (;;) { | |
295 | ie = uwb_ie_next(&ptr, &size); | |
296 | if (!ie) | |
297 | break; | |
298 | ||
299 | result = uwb_rc_ie_add_one(uwb_rc, ie); | |
300 | if (result < 0) | |
301 | break; | |
22d203ec | 302 | } |
1cde7f68 DV |
303 | if (result >= 0) { |
304 | if (size == 0) { | |
305 | if (uwb_rc->beaconing != -1) | |
306 | result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); | |
22d203ec | 307 | } else |
1cde7f68 | 308 | result = -EINVAL; |
22d203ec | 309 | } |
1cde7f68 | 310 | |
22d203ec | 311 | mutex_unlock(&uwb_rc->ies_mutex); |
1cde7f68 | 312 | |
22d203ec IPG |
313 | return result; |
314 | } | |
315 | EXPORT_SYMBOL_GPL(uwb_rc_ie_add); | |
316 | ||
317 | ||
318 | /* | |
319 | * Remove an IE from internal cache | |
320 | * | |
321 | * We are dealing with our internal IE cache so no need to verify that the | |
322 | * IEs are valid (it has been done already). | |
323 | * | |
324 | * Should be called with ies_mutex held | |
325 | * | |
326 | * We do not break out once an IE is found in the cache. It is currently | |
327 | * possible to have more than one IE with the same ID included in the | |
328 | * beacon. We don't reallocate, we just mark the size smaller. | |
329 | */ | |
330 | static | |
1cde7f68 | 331 | void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove) |
22d203ec | 332 | { |
1cde7f68 DV |
333 | struct uwb_ie_hdr *ie; |
334 | size_t len = le16_to_cpu(uwb_rc->ies->wIELength); | |
335 | void *ptr; | |
336 | size_t size; | |
337 | ||
338 | ptr = uwb_rc->ies->IEData; | |
339 | size = len; | |
340 | for (;;) { | |
341 | ie = uwb_ie_next(&ptr, &size); | |
342 | if (!ie) | |
343 | break; | |
344 | if (ie->element_id == to_remove) { | |
345 | len -= sizeof(struct uwb_ie_hdr) + ie->length; | |
346 | memmove(ie, ptr, size); | |
347 | ptr = ie; | |
22d203ec IPG |
348 | } |
349 | } | |
1cde7f68 | 350 | uwb_rc->ies->wIELength = cpu_to_le16(len); |
22d203ec IPG |
351 | } |
352 | ||
353 | ||
354 | /** | |
1cde7f68 DV |
355 | * uwb_rc_ie_rm - remove an IE from the radio controller's beacon |
356 | * @uwb_rc: the radio controller. | |
357 | * @element_id: the element ID of the IE to remove. | |
22d203ec | 358 | * |
1cde7f68 DV |
359 | * Only IEs previously added with uwb_rc_ie_add() may be removed. |
360 | * | |
361 | * Returns 0 on success; or -ve the SET-IE command to the radio | |
362 | * controller failed. | |
22d203ec IPG |
363 | */ |
364 | int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id) | |
365 | { | |
1cde7f68 | 366 | int result = 0; |
22d203ec | 367 | |
22d203ec | 368 | mutex_lock(&uwb_rc->ies_mutex); |
1cde7f68 DV |
369 | |
370 | uwb_rc_ie_cache_rm(uwb_rc, element_id); | |
371 | ||
372 | if (uwb_rc->beaconing != -1) | |
22d203ec | 373 | result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); |
1cde7f68 | 374 | |
22d203ec | 375 | mutex_unlock(&uwb_rc->ies_mutex); |
1cde7f68 | 376 | |
22d203ec IPG |
377 | return result; |
378 | } | |
379 | EXPORT_SYMBOL_GPL(uwb_rc_ie_rm); |