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 | ||
27 | #include "uwb-internal.h" | |
28 | #define D_LOCAL 0 | |
29 | #include <linux/uwb/debug.h> | |
30 | ||
31 | /** | |
32 | * uwb_ie_next - get the next IE in a buffer | |
33 | * @ptr: start of the buffer containing the IE data | |
34 | * @len: length of the buffer | |
35 | * | |
36 | * Both @ptr and @len are updated so subsequent calls to uwb_ie_next() | |
37 | * will get the next IE. | |
38 | * | |
39 | * NULL is returned (and @ptr and @len will not be updated) if there | |
40 | * are no more IEs in the buffer or the buffer is too short. | |
41 | */ | |
42 | struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len) | |
43 | { | |
44 | struct uwb_ie_hdr *hdr; | |
45 | size_t ie_len; | |
46 | ||
47 | if (*len < sizeof(struct uwb_ie_hdr)) | |
48 | return NULL; | |
49 | ||
50 | hdr = *ptr; | |
51 | ie_len = sizeof(struct uwb_ie_hdr) + hdr->length; | |
52 | ||
53 | if (*len < ie_len) | |
54 | return NULL; | |
55 | ||
56 | *ptr += ie_len; | |
57 | *len -= ie_len; | |
58 | ||
59 | return hdr; | |
60 | } | |
61 | EXPORT_SYMBOL_GPL(uwb_ie_next); | |
62 | ||
63 | /** | |
64 | * Get the IEs that a radio controller is sending in its beacon | |
65 | * | |
66 | * @uwb_rc: UWB Radio Controller | |
67 | * @returns: Size read from the system | |
68 | * | |
69 | * We don't need to lock the uwb_rc's mutex because we don't modify | |
70 | * anything. Once done with the iedata buffer, call | |
71 | * uwb_rc_ie_release(iedata). Don't call kfree on it. | |
72 | */ | |
73 | ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie) | |
74 | { | |
75 | ssize_t result; | |
76 | struct device *dev = &uwb_rc->uwb_dev.dev; | |
77 | struct uwb_rccb *cmd = NULL; | |
78 | struct uwb_rceb *reply = NULL; | |
79 | struct uwb_rc_evt_get_ie *get_ie; | |
80 | ||
81 | d_fnstart(3, dev, "(%p, %p)\n", uwb_rc, pget_ie); | |
82 | result = -ENOMEM; | |
83 | cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); | |
84 | if (cmd == NULL) | |
85 | goto error_kzalloc; | |
86 | cmd->bCommandType = UWB_RC_CET_GENERAL; | |
87 | cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE); | |
88 | result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd), | |
89 | UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE, | |
90 | &reply); | |
91 | if (result < 0) | |
92 | goto error_cmd; | |
93 | get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb); | |
94 | if (result < sizeof(*get_ie)) { | |
95 | dev_err(dev, "not enough data returned for decoding GET IE " | |
96 | "(%zu bytes received vs %zu needed)\n", | |
97 | result, sizeof(*get_ie)); | |
98 | result = -EINVAL; | |
99 | } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) { | |
100 | dev_err(dev, "not enough data returned for decoding GET IE " | |
101 | "payload (%zu bytes received vs %zu needed)\n", result, | |
102 | sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)); | |
103 | result = -EINVAL; | |
104 | } else | |
105 | *pget_ie = get_ie; | |
106 | error_cmd: | |
107 | kfree(cmd); | |
108 | error_kzalloc: | |
109 | d_fnend(3, dev, "(%p, %p) = %d\n", uwb_rc, pget_ie, (int)result); | |
110 | return result; | |
111 | } | |
112 | EXPORT_SYMBOL_GPL(uwb_rc_get_ie); | |
113 | ||
114 | ||
115 | /* | |
116 | * Given a pointer to an IE, print it in ASCII/hex followed by a new line | |
117 | * | |
118 | * @ie_hdr: pointer to the IE header. Length is in there, and it is | |
119 | * guaranteed that the ie_hdr->length bytes following it are | |
120 | * safely accesible. | |
121 | * | |
122 | * @_data: context data passed from uwb_ie_for_each(), an struct output_ctx | |
123 | */ | |
124 | int uwb_ie_dump_hex(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr, | |
125 | size_t offset, void *_ctx) | |
126 | { | |
127 | struct uwb_buf_ctx *ctx = _ctx; | |
128 | const u8 *pl = (void *)(ie_hdr + 1); | |
129 | u8 pl_itr; | |
130 | ||
131 | ctx->bytes += scnprintf(ctx->buf + ctx->bytes, ctx->size - ctx->bytes, | |
132 | "%02x %02x ", (unsigned) ie_hdr->element_id, | |
133 | (unsigned) ie_hdr->length); | |
134 | pl_itr = 0; | |
135 | while (pl_itr < ie_hdr->length && ctx->bytes < ctx->size) | |
136 | ctx->bytes += scnprintf(ctx->buf + ctx->bytes, | |
137 | ctx->size - ctx->bytes, | |
138 | "%02x ", (unsigned) pl[pl_itr++]); | |
139 | if (ctx->bytes < ctx->size) | |
140 | ctx->buf[ctx->bytes++] = '\n'; | |
141 | return 0; | |
142 | } | |
143 | EXPORT_SYMBOL_GPL(uwb_ie_dump_hex); | |
144 | ||
145 | ||
146 | /** | |
147 | * Verify that a pointer in a buffer points to valid IE | |
148 | * | |
149 | * @start: pointer to start of buffer in which IE appears | |
150 | * @itr: pointer to IE inside buffer that will be verified | |
151 | * @top: pointer to end of buffer | |
152 | * | |
153 | * @returns: 0 if IE is valid, <0 otherwise | |
154 | * | |
155 | * Verification involves checking that the buffer can contain a | |
156 | * header and the amount of data reported in the IE header can be found in | |
157 | * the buffer. | |
158 | */ | |
159 | static | |
160 | int uwb_rc_ie_verify(struct uwb_dev *uwb_dev, const void *start, | |
161 | const void *itr, const void *top) | |
162 | { | |
163 | struct device *dev = &uwb_dev->dev; | |
164 | const struct uwb_ie_hdr *ie_hdr; | |
165 | ||
166 | if (top - itr < sizeof(*ie_hdr)) { | |
167 | dev_err(dev, "Bad IE: no data to decode header " | |
168 | "(%zu bytes left vs %zu needed) at offset %zu\n", | |
169 | top - itr, sizeof(*ie_hdr), itr - start); | |
170 | return -EINVAL; | |
171 | } | |
172 | ie_hdr = itr; | |
173 | itr += sizeof(*ie_hdr); | |
174 | if (top - itr < ie_hdr->length) { | |
175 | dev_err(dev, "Bad IE: not enough data for payload " | |
176 | "(%zu bytes left vs %zu needed) at offset %zu\n", | |
177 | top - itr, (size_t)ie_hdr->length, | |
178 | (void *)ie_hdr - start); | |
179 | return -EINVAL; | |
180 | } | |
181 | return 0; | |
182 | } | |
183 | ||
184 | ||
185 | /** | |
186 | * Walk a buffer filled with consecutive IE's a buffer | |
187 | * | |
188 | * @uwb_dev: UWB device this IEs belong to (for err messages mainly) | |
189 | * | |
190 | * @fn: function to call with each IE; if it returns 0, we keep | |
191 | * traversing the buffer. If it returns !0, we'll stop and return | |
192 | * that value. | |
193 | * | |
194 | * @data: pointer passed to @fn | |
195 | * | |
196 | * @buf: buffer where the consecutive IEs are located | |
197 | * | |
198 | * @size: size of @buf | |
199 | * | |
200 | * Each IE is checked for basic correctness (there is space left for | |
201 | * the header and the payload). If that test is failed, we stop | |
202 | * processing. For every good IE, @fn is called. | |
203 | */ | |
204 | ssize_t uwb_ie_for_each(struct uwb_dev *uwb_dev, uwb_ie_f fn, void *data, | |
205 | const void *buf, size_t size) | |
206 | { | |
207 | ssize_t result = 0; | |
208 | const struct uwb_ie_hdr *ie_hdr; | |
209 | const void *itr = buf, *top = itr + size; | |
210 | ||
211 | while (itr < top) { | |
212 | if (uwb_rc_ie_verify(uwb_dev, buf, itr, top) != 0) | |
213 | break; | |
214 | ie_hdr = itr; | |
215 | itr += sizeof(*ie_hdr) + ie_hdr->length; | |
216 | result = fn(uwb_dev, ie_hdr, itr - buf, data); | |
217 | if (result != 0) | |
218 | break; | |
219 | } | |
220 | return result; | |
221 | } | |
222 | EXPORT_SYMBOL_GPL(uwb_ie_for_each); | |
223 | ||
224 | ||
225 | /** | |
226 | * Replace all IEs currently being transmitted by a device | |
227 | * | |
228 | * @cmd: pointer to the SET-IE command with the IEs to set | |
229 | * @size: size of @buf | |
230 | */ | |
231 | int uwb_rc_set_ie(struct uwb_rc *rc, struct uwb_rc_cmd_set_ie *cmd) | |
232 | { | |
233 | int result; | |
234 | struct device *dev = &rc->uwb_dev.dev; | |
235 | struct uwb_rc_evt_set_ie reply; | |
236 | ||
237 | reply.rceb.bEventType = UWB_RC_CET_GENERAL; | |
238 | reply.rceb.wEvent = UWB_RC_CMD_SET_IE; | |
239 | result = uwb_rc_cmd(rc, "SET-IE", &cmd->rccb, | |
240 | sizeof(*cmd) + le16_to_cpu(cmd->wIELength), | |
241 | &reply.rceb, sizeof(reply)); | |
242 | if (result < 0) | |
243 | goto error_cmd; | |
244 | else if (result != sizeof(reply)) { | |
245 | dev_err(dev, "SET-IE: not enough data to decode reply " | |
246 | "(%d bytes received vs %zu needed)\n", | |
247 | result, sizeof(reply)); | |
248 | result = -EIO; | |
249 | } else if (reply.bResultCode != UWB_RC_RES_SUCCESS) { | |
250 | dev_err(dev, "SET-IE: command execution failed: %s (%d)\n", | |
251 | uwb_rc_strerror(reply.bResultCode), reply.bResultCode); | |
252 | result = -EIO; | |
253 | } else | |
254 | result = 0; | |
255 | error_cmd: | |
256 | return result; | |
257 | } | |
258 | ||
259 | /** | |
260 | * Determine by IE id if IE is host settable | |
261 | * WUSB 1.0 [8.6.2.8 Table 8.85] | |
262 | * | |
263 | * EXCEPTION: | |
264 | * All but UWB_IE_WLP appears in Table 8.85 from WUSB 1.0. Setting this IE | |
265 | * is required for the WLP substack to perform association with its WSS so | |
266 | * we hope that the WUSB spec will be changed to reflect this. | |
267 | */ | |
268 | static | |
269 | int uwb_rc_ie_is_host_settable(enum uwb_ie element_id) | |
270 | { | |
271 | if (element_id == UWB_PCA_AVAILABILITY || | |
272 | element_id == UWB_BP_SWITCH_IE || | |
273 | element_id == UWB_MAC_CAPABILITIES_IE || | |
274 | element_id == UWB_PHY_CAPABILITIES_IE || | |
275 | element_id == UWB_APP_SPEC_PROBE_IE || | |
276 | element_id == UWB_IDENTIFICATION_IE || | |
277 | element_id == UWB_MASTER_KEY_ID_IE || | |
278 | element_id == UWB_IE_WLP || | |
279 | element_id == UWB_APP_SPEC_IE) | |
280 | return 1; | |
281 | return 0; | |
282 | } | |
283 | ||
284 | ||
285 | /** | |
286 | * Extract Host Settable IEs from IE | |
287 | * | |
288 | * @ie_data: pointer to buffer containing all IEs | |
289 | * @size: size of buffer | |
290 | * | |
291 | * @returns: length of buffer that only includes host settable IEs | |
292 | * | |
293 | * Given a buffer of IEs we move all Host Settable IEs to front of buffer | |
294 | * by overwriting the IEs that are not Host Settable. | |
295 | * Buffer length is adjusted accordingly. | |
296 | */ | |
297 | static | |
298 | ssize_t uwb_rc_parse_host_settable_ie(struct uwb_dev *uwb_dev, | |
299 | void *ie_data, size_t size) | |
300 | { | |
301 | size_t new_len = size; | |
302 | struct uwb_ie_hdr *ie_hdr; | |
303 | size_t ie_length; | |
304 | void *itr = ie_data, *top = itr + size; | |
305 | ||
306 | while (itr < top) { | |
307 | if (uwb_rc_ie_verify(uwb_dev, ie_data, itr, top) != 0) | |
308 | break; | |
309 | ie_hdr = itr; | |
310 | ie_length = sizeof(*ie_hdr) + ie_hdr->length; | |
311 | if (uwb_rc_ie_is_host_settable(ie_hdr->element_id)) { | |
312 | itr += ie_length; | |
313 | } else { | |
314 | memmove(itr, itr + ie_length, top - (itr + ie_length)); | |
315 | new_len -= ie_length; | |
316 | top -= ie_length; | |
317 | } | |
318 | } | |
319 | return new_len; | |
320 | } | |
321 | ||
322 | ||
323 | /* Cleanup the whole IE management subsystem */ | |
324 | void uwb_rc_ie_init(struct uwb_rc *uwb_rc) | |
325 | { | |
326 | mutex_init(&uwb_rc->ies_mutex); | |
327 | } | |
328 | ||
329 | ||
330 | /** | |
331 | * Set up cache for host settable IEs currently being transmitted | |
332 | * | |
333 | * First we just call GET-IE to get the current IEs being transmitted | |
334 | * (or we workaround and pretend we did) and (because the format is | |
335 | * the same) reuse that as the IE cache (with the command prefix, as | |
336 | * explained in 'struct uwb_rc'). | |
337 | * | |
338 | * @returns: size of cache created | |
339 | */ | |
340 | ssize_t uwb_rc_ie_setup(struct uwb_rc *uwb_rc) | |
341 | { | |
342 | struct device *dev = &uwb_rc->uwb_dev.dev; | |
343 | ssize_t result; | |
344 | size_t capacity; | |
345 | struct uwb_rc_evt_get_ie *ie_info; | |
346 | ||
347 | d_fnstart(3, dev, "(%p)\n", uwb_rc); | |
348 | mutex_lock(&uwb_rc->ies_mutex); | |
349 | result = uwb_rc_get_ie(uwb_rc, &ie_info); | |
350 | if (result < 0) | |
351 | goto error_get_ie; | |
352 | capacity = result; | |
353 | d_printf(5, dev, "Got IEs %zu bytes (%zu long at %p)\n", result, | |
354 | (size_t)le16_to_cpu(ie_info->wIELength), ie_info); | |
355 | ||
356 | /* Remove IEs that host should not set. */ | |
357 | result = uwb_rc_parse_host_settable_ie(&uwb_rc->uwb_dev, | |
358 | ie_info->IEData, le16_to_cpu(ie_info->wIELength)); | |
359 | if (result < 0) | |
360 | goto error_parse; | |
361 | d_printf(5, dev, "purged non-settable IEs to %zu bytes\n", result); | |
362 | uwb_rc->ies = (void *) ie_info; | |
363 | uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL; | |
364 | uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE); | |
365 | uwb_rc->ies_capacity = capacity; | |
366 | d_printf(5, dev, "IE cache at %p %zu bytes, %zu capacity\n", | |
367 | ie_info, result, capacity); | |
368 | result = 0; | |
369 | error_parse: | |
370 | error_get_ie: | |
371 | mutex_unlock(&uwb_rc->ies_mutex); | |
372 | d_fnend(3, dev, "(%p) = %zu\n", uwb_rc, result); | |
373 | return result; | |
374 | } | |
375 | ||
376 | ||
377 | /* Cleanup the whole IE management subsystem */ | |
378 | void uwb_rc_ie_release(struct uwb_rc *uwb_rc) | |
379 | { | |
380 | kfree(uwb_rc->ies); | |
381 | uwb_rc->ies = NULL; | |
382 | uwb_rc->ies_capacity = 0; | |
383 | } | |
384 | ||
385 | ||
386 | static | |
387 | int __acc_size(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr, | |
388 | size_t offset, void *_ctx) | |
389 | { | |
390 | size_t *acc_size = _ctx; | |
391 | *acc_size += sizeof(*ie_hdr) + ie_hdr->length; | |
392 | d_printf(6, &uwb_dev->dev, "new acc size %zu\n", *acc_size); | |
393 | return 0; | |
394 | } | |
395 | ||
396 | ||
397 | /** | |
398 | * Add a new IE to IEs currently being transmitted by device | |
399 | * | |
400 | * @ies: the buffer containing the new IE or IEs to be added to | |
401 | * the device's beacon. The buffer will be verified for | |
402 | * consistence (meaning the headers should be right) and | |
403 | * consistent with the buffer size. | |
404 | * @size: size of @ies (in bytes, total buffer size) | |
405 | * @returns: 0 if ok, <0 errno code on error | |
406 | * | |
407 | * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB | |
408 | * after the device sent the first beacon that includes the IEs specified | |
409 | * in the SET IE command. We thus cannot send this command if the device is | |
410 | * not beaconing. Instead, a SET IE command will be sent later right after | |
411 | * we start beaconing. | |
412 | * | |
413 | * Setting an IE on the device will overwrite all current IEs in device. So | |
414 | * we take the current IEs being transmitted by the device, append the | |
415 | * new one, and call SET IE with all the IEs needed. | |
416 | * | |
417 | * The local IE cache will only be updated with the new IE if SET IE | |
418 | * completed successfully. | |
419 | */ | |
420 | int uwb_rc_ie_add(struct uwb_rc *uwb_rc, | |
421 | const struct uwb_ie_hdr *ies, size_t size) | |
422 | { | |
423 | int result = 0; | |
424 | struct device *dev = &uwb_rc->uwb_dev.dev; | |
425 | struct uwb_rc_cmd_set_ie *new_ies; | |
426 | size_t ies_size, total_size, acc_size = 0; | |
427 | ||
428 | if (uwb_rc->ies == NULL) | |
429 | return -ESHUTDOWN; | |
430 | uwb_ie_for_each(&uwb_rc->uwb_dev, __acc_size, &acc_size, ies, size); | |
431 | if (acc_size != size) { | |
432 | dev_err(dev, "BUG: bad IEs, misconstructed headers " | |
433 | "[%zu bytes reported vs %zu calculated]\n", | |
434 | size, acc_size); | |
435 | WARN_ON(1); | |
436 | return -EINVAL; | |
437 | } | |
438 | mutex_lock(&uwb_rc->ies_mutex); | |
439 | ies_size = le16_to_cpu(uwb_rc->ies->wIELength); | |
440 | total_size = sizeof(*uwb_rc->ies) + ies_size; | |
441 | if (total_size + size > uwb_rc->ies_capacity) { | |
442 | d_printf(4, dev, "Reallocating IE cache from %p capacity %zu " | |
443 | "to capacity %zu\n", uwb_rc->ies, uwb_rc->ies_capacity, | |
444 | total_size + size); | |
445 | new_ies = kzalloc(total_size + size, GFP_KERNEL); | |
446 | if (new_ies == NULL) { | |
447 | dev_err(dev, "No memory for adding new IE\n"); | |
448 | result = -ENOMEM; | |
449 | goto error_alloc; | |
450 | } | |
451 | memcpy(new_ies, uwb_rc->ies, total_size); | |
452 | uwb_rc->ies_capacity = total_size + size; | |
453 | kfree(uwb_rc->ies); | |
454 | uwb_rc->ies = new_ies; | |
455 | d_printf(4, dev, "New IE cache at %p capacity %zu\n", | |
456 | uwb_rc->ies, uwb_rc->ies_capacity); | |
457 | } | |
458 | memcpy((void *)uwb_rc->ies + total_size, ies, size); | |
459 | uwb_rc->ies->wIELength = cpu_to_le16(ies_size + size); | |
460 | if (uwb_rc->beaconing != -1) { | |
461 | result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); | |
462 | if (result < 0) { | |
463 | dev_err(dev, "Cannot set new IE on device: %d\n", | |
464 | result); | |
465 | uwb_rc->ies->wIELength = cpu_to_le16(ies_size); | |
466 | } else | |
467 | result = 0; | |
468 | } | |
469 | d_printf(4, dev, "IEs now occupy %hu bytes of %zu capacity at %p\n", | |
470 | le16_to_cpu(uwb_rc->ies->wIELength), uwb_rc->ies_capacity, | |
471 | uwb_rc->ies); | |
472 | error_alloc: | |
473 | mutex_unlock(&uwb_rc->ies_mutex); | |
474 | return result; | |
475 | } | |
476 | EXPORT_SYMBOL_GPL(uwb_rc_ie_add); | |
477 | ||
478 | ||
479 | /* | |
480 | * Remove an IE from internal cache | |
481 | * | |
482 | * We are dealing with our internal IE cache so no need to verify that the | |
483 | * IEs are valid (it has been done already). | |
484 | * | |
485 | * Should be called with ies_mutex held | |
486 | * | |
487 | * We do not break out once an IE is found in the cache. It is currently | |
488 | * possible to have more than one IE with the same ID included in the | |
489 | * beacon. We don't reallocate, we just mark the size smaller. | |
490 | */ | |
491 | static | |
492 | int uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove) | |
493 | { | |
494 | struct uwb_ie_hdr *ie_hdr; | |
495 | size_t new_len = le16_to_cpu(uwb_rc->ies->wIELength); | |
496 | void *itr = uwb_rc->ies->IEData; | |
497 | void *top = itr + new_len; | |
498 | ||
499 | while (itr < top) { | |
500 | ie_hdr = itr; | |
501 | if (ie_hdr->element_id != to_remove) { | |
502 | itr += sizeof(*ie_hdr) + ie_hdr->length; | |
503 | } else { | |
504 | int ie_length; | |
505 | ie_length = sizeof(*ie_hdr) + ie_hdr->length; | |
506 | if (top - itr != ie_length) | |
507 | memmove(itr, itr + ie_length, top - itr + ie_length); | |
508 | top -= ie_length; | |
509 | new_len -= ie_length; | |
510 | } | |
511 | } | |
512 | uwb_rc->ies->wIELength = cpu_to_le16(new_len); | |
513 | return 0; | |
514 | } | |
515 | ||
516 | ||
517 | /** | |
518 | * Remove an IE currently being transmitted by device | |
519 | * | |
520 | * @element_id: id of IE to be removed from device's beacon | |
521 | */ | |
522 | int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id) | |
523 | { | |
524 | struct device *dev = &uwb_rc->uwb_dev.dev; | |
525 | int result; | |
526 | ||
527 | if (uwb_rc->ies == NULL) | |
528 | return -ESHUTDOWN; | |
529 | mutex_lock(&uwb_rc->ies_mutex); | |
530 | result = uwb_rc_ie_cache_rm(uwb_rc, element_id); | |
531 | if (result < 0) | |
532 | dev_err(dev, "Cannot remove IE from cache.\n"); | |
533 | if (uwb_rc->beaconing != -1) { | |
534 | result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); | |
535 | if (result < 0) | |
536 | dev_err(dev, "Cannot set new IE on device.\n"); | |
537 | } | |
538 | mutex_unlock(&uwb_rc->ies_mutex); | |
539 | return result; | |
540 | } | |
541 | EXPORT_SYMBOL_GPL(uwb_rc_ie_rm); | |
542 | ||
543 | ||
544 | /** | |
545 | * Create and set new Identification IE | |
546 | * | |
547 | * Currently only sets the Vendor ID. The Vendor ID is set from the OUI, | |
548 | * which is obtained from the first three bytes from the MAC address. | |
549 | */ | |
550 | int uwb_rc_set_identification_ie(struct uwb_rc *uwb_rc) | |
551 | { | |
552 | struct { | |
553 | struct uwb_identification_ie id_ie; | |
554 | struct uwb_dev_info dev_info; | |
555 | struct uwb_vendor_id vendor_id; | |
556 | } ie_data; | |
557 | ||
558 | ie_data.id_ie.hdr.element_id = UWB_IDENTIFICATION_IE; | |
559 | ie_data.id_ie.hdr.length = sizeof(struct uwb_dev_info) + | |
560 | sizeof(struct uwb_vendor_id); | |
561 | ||
562 | ie_data.dev_info.type = UWB_DEV_INFO_VENDOR_ID; | |
563 | ie_data.dev_info.length = sizeof(struct uwb_vendor_id); | |
564 | ||
565 | ie_data.vendor_id.data[0] = uwb_rc->uwb_dev.mac_addr.data[0]; | |
566 | ie_data.vendor_id.data[1] = uwb_rc->uwb_dev.mac_addr.data[1]; | |
567 | ie_data.vendor_id.data[2] = uwb_rc->uwb_dev.mac_addr.data[2]; | |
568 | ||
569 | return uwb_rc_ie_add(uwb_rc, &ie_data.id_ie.hdr, sizeof(ie_data)); | |
570 | } |