aboutsummaryrefslogtreecommitdiffstats
path: root/drivers/uwb/ie.c
diff options
context:
space:
mode:
authorDavid Vrabel <david.vrabel@csr.com>2008-10-27 12:48:09 -0400
committerDavid Vrabel <david.vrabel@csr.com>2008-10-28 08:09:17 -0400
commit1cde7f68ced8d10a20dd2370e9d1d22ab3c1ea5c (patch)
tree2fb65bbb93eae73e02fa1e9608212536259f8802 /drivers/uwb/ie.c
parent4d2bea4ca0adb4cebfbf89d34869c74081c42577 (diff)
uwb: order IEs by element ID
ECMA-368 requires that IEs in a beacon must be sorted by element ID. Most hardware uses the ordering in the Set IE URC command so get the ordering right on the host. Also refactor the IE management code: - use uwb_ie_next() instead of uwb_ie_for_each(). - remove unnecessary functions. - API is now only uwb_rc_ie_add() and uwb_rc_ie_rm(). Signed-off-by: David Vrabel <david.vrabel@csr.com>
Diffstat (limited to 'drivers/uwb/ie.c')
-rw-r--r--drivers/uwb/ie.c463
1 files changed, 150 insertions, 313 deletions
diff --git a/drivers/uwb/ie.c b/drivers/uwb/ie.c
index cf6f3d152b9d..ab976686175b 100644
--- a/drivers/uwb/ie.c
+++ b/drivers/uwb/ie.c
@@ -25,8 +25,6 @@
25 */ 25 */
26 26
27#include "uwb-internal.h" 27#include "uwb-internal.h"
28#define D_LOCAL 0
29#include <linux/uwb/debug.h>
30 28
31/** 29/**
32 * uwb_ie_next - get the next IE in a buffer 30 * uwb_ie_next - get the next IE in a buffer
@@ -61,6 +59,42 @@ struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len)
61EXPORT_SYMBOL_GPL(uwb_ie_next); 59EXPORT_SYMBOL_GPL(uwb_ie_next);
62 60
63/** 61/**
62 * uwb_ie_dump_hex - print IEs to a character buffer
63 * @ies: the IEs to print.
64 * @len: length of all the IEs.
65 * @buf: the destination buffer.
66 * @size: size of @buf.
67 *
68 * Returns the number of characters written.
69 */
70int uwb_ie_dump_hex(const struct uwb_ie_hdr *ies, size_t len,
71 char *buf, size_t size)
72{
73 void *ptr;
74 const struct uwb_ie_hdr *ie;
75 int r = 0;
76 u8 *d;
77
78 ptr = (void *)ies;
79 for (;;) {
80 ie = uwb_ie_next(&ptr, &len);
81 if (!ie)
82 break;
83
84 r += scnprintf(buf + r, size - r, "%02x %02x",
85 (unsigned)ie->element_id,
86 (unsigned)ie->length);
87 d = (uint8_t *)ie + sizeof(struct uwb_ie_hdr);
88 while (d != ptr && r < size)
89 r += scnprintf(buf + r, size - r, " %02x", (unsigned)*d++);
90 if (r < size)
91 buf[r++] = '\n';
92 };
93
94 return r;
95}
96
97/**
64 * Get the IEs that a radio controller is sending in its beacon 98 * Get the IEs that a radio controller is sending in its beacon
65 * 99 *
66 * @uwb_rc: UWB Radio Controller 100 * @uwb_rc: UWB Radio Controller
@@ -70,6 +104,7 @@ EXPORT_SYMBOL_GPL(uwb_ie_next);
70 * anything. Once done with the iedata buffer, call 104 * anything. Once done with the iedata buffer, call
71 * uwb_rc_ie_release(iedata). Don't call kfree on it. 105 * uwb_rc_ie_release(iedata). Don't call kfree on it.
72 */ 106 */
107static
73ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie) 108ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie)
74{ 109{
75 ssize_t result; 110 ssize_t result;
@@ -78,148 +113,35 @@ ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie)
78 struct uwb_rceb *reply = NULL; 113 struct uwb_rceb *reply = NULL;
79 struct uwb_rc_evt_get_ie *get_ie; 114 struct uwb_rc_evt_get_ie *get_ie;
80 115
81 d_fnstart(3, dev, "(%p, %p)\n", uwb_rc, pget_ie);
82 result = -ENOMEM;
83 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); 116 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
84 if (cmd == NULL) 117 if (cmd == NULL)
85 goto error_kzalloc; 118 return -ENOMEM;
119
86 cmd->bCommandType = UWB_RC_CET_GENERAL; 120 cmd->bCommandType = UWB_RC_CET_GENERAL;
87 cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE); 121 cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE);
88 result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd), 122 result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd),
89 UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE, 123 UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE,
90 &reply); 124 &reply);
125 kfree(cmd);
91 if (result < 0) 126 if (result < 0)
92 goto error_cmd; 127 return result;
128
93 get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb); 129 get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb);
94 if (result < sizeof(*get_ie)) { 130 if (result < sizeof(*get_ie)) {
95 dev_err(dev, "not enough data returned for decoding GET IE " 131 dev_err(dev, "not enough data returned for decoding GET IE "
96 "(%zu bytes received vs %zu needed)\n", 132 "(%zu bytes received vs %zu needed)\n",
97 result, sizeof(*get_ie)); 133 result, sizeof(*get_ie));
98 result = -EINVAL; 134 return -EINVAL;
99 } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) { 135 } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) {
100 dev_err(dev, "not enough data returned for decoding GET IE " 136 dev_err(dev, "not enough data returned for decoding GET IE "
101 "payload (%zu bytes received vs %zu needed)\n", result, 137 "payload (%zu bytes received vs %zu needed)\n", result,
102 sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)); 138 sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength));
103 result = -EINVAL;
104 } else
105 *pget_ie = get_ie;
106error_cmd:
107 kfree(cmd);
108error_kzalloc:
109 d_fnend(3, dev, "(%p, %p) = %d\n", uwb_rc, pget_ie, (int)result);
110 return result;
111}
112EXPORT_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 */
124int 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}
143EXPORT_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 */
159static
160int 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; 139 return -EINVAL;
180 } 140 }
181 return 0;
182}
183 141
184 142 *pget_ie = get_ie;
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 */
204ssize_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; 143 return result;
221} 144}
222EXPORT_SYMBOL_GPL(uwb_ie_for_each);
223 145
224 146
225/** 147/**
@@ -256,70 +178,6 @@ error_cmd:
256 return result; 178 return result;
257} 179}
258 180
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 */
268static
269int 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 */
297static
298ssize_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 */ 181/* Cleanup the whole IE management subsystem */
324void uwb_rc_ie_init(struct uwb_rc *uwb_rc) 182void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
325{ 183{
@@ -328,49 +186,34 @@ void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
328 186
329 187
330/** 188/**
331 * Set up cache for host settable IEs currently being transmitted 189 * uwb_rc_ie_setup - setup a radio controller's IE manager
190 * @uwb_rc: the radio controller.
332 * 191 *
333 * First we just call GET-IE to get the current IEs being transmitted 192 * The current set of IEs are obtained from the hardware with a GET-IE
334 * (or we workaround and pretend we did) and (because the format is 193 * command (since the radio controller is not yet beaconing this will
335 * the same) reuse that as the IE cache (with the command prefix, as 194 * be just the hardware's MAC and PHY Capability IEs).
336 * explained in 'struct uwb_rc').
337 * 195 *
338 * @returns: size of cache created 196 * Returns 0 on success; -ve on an error.
339 */ 197 */
340ssize_t uwb_rc_ie_setup(struct uwb_rc *uwb_rc) 198int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
341{ 199{
342 struct device *dev = &uwb_rc->uwb_dev.dev; 200 struct uwb_rc_evt_get_ie *ie_info = NULL;
343 ssize_t result; 201 int capacity;
344 size_t capacity; 202
345 struct uwb_rc_evt_get_ie *ie_info; 203 capacity = uwb_rc_get_ie(uwb_rc, &ie_info);
204 if (capacity < 0)
205 return capacity;
346 206
347 d_fnstart(3, dev, "(%p)\n", uwb_rc);
348 mutex_lock(&uwb_rc->ies_mutex); 207 mutex_lock(&uwb_rc->ies_mutex);
349 result = uwb_rc_get_ie(uwb_rc, &ie_info); 208
350 if (result < 0) 209 uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
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; 210 uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL;
364 uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE); 211 uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE);
365 uwb_rc->ies_capacity = capacity; 212 uwb_rc->ies_capacity = capacity;
366 d_printf(5, dev, "IE cache at %p %zu bytes, %zu capacity\n", 213
367 ie_info, result, capacity);
368 result = 0;
369error_parse:
370error_get_ie:
371 mutex_unlock(&uwb_rc->ies_mutex); 214 mutex_unlock(&uwb_rc->ies_mutex);
372 d_fnend(3, dev, "(%p) = %zu\n", uwb_rc, result); 215
373 return result; 216 return 0;
374} 217}
375 218
376 219
@@ -383,26 +226,47 @@ void uwb_rc_ie_release(struct uwb_rc *uwb_rc)
383} 226}
384 227
385 228
386static 229static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
387int __acc_size(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr,
388 size_t offset, void *_ctx)
389{ 230{
390 size_t *acc_size = _ctx; 231 struct uwb_rc_cmd_set_ie *new_ies;
391 *acc_size += sizeof(*ie_hdr) + ie_hdr->length; 232 void *ptr, *prev_ie;
392 d_printf(6, &uwb_dev->dev, "new acc size %zu\n", *acc_size); 233 struct uwb_ie_hdr *ie;
234 size_t length, new_ie_len, new_capacity, size, prev_size;
235
236 length = le16_to_cpu(rc->ies->wIELength);
237 new_ie_len = sizeof(struct uwb_ie_hdr) + new_ie->length;
238 new_capacity = sizeof(struct uwb_rc_cmd_set_ie) + length + new_ie_len;
239
240 if (new_capacity > rc->ies_capacity) {
241 new_ies = krealloc(rc->ies, new_capacity, GFP_KERNEL);
242 if (!new_ies)
243 return -ENOMEM;
244 rc->ies = new_ies;
245 }
246
247 ptr = rc->ies->IEData;
248 size = length;
249 for (;;) {
250 prev_ie = ptr;
251 prev_size = size;
252 ie = uwb_ie_next(&ptr, &size);
253 if (!ie || ie->element_id > new_ie->element_id)
254 break;
255 }
256
257 memmove(prev_ie + new_ie_len, prev_ie, prev_size);
258 memcpy(prev_ie, new_ie, new_ie_len);
259 rc->ies->wIELength = cpu_to_le16(length + new_ie_len);
260
393 return 0; 261 return 0;
394} 262}
395 263
396
397/** 264/**
398 * Add a new IE to IEs currently being transmitted by device 265 * uwb_rc_ie_add - add new IEs to the radio controller's beacon
399 * 266 * @uwb_rc: the radio controller.
400 * @ies: the buffer containing the new IE or IEs to be added to 267 * @ies: the buffer containing the new IE or IEs to be added to
401 * the device's beacon. The buffer will be verified for 268 * the device's beacon.
402 * consistence (meaning the headers should be right) and 269 * @size: length of all the IEs.
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 * 270 *
407 * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB 271 * 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 272 * after the device sent the first beacon that includes the IEs specified
@@ -411,66 +275,40 @@ int __acc_size(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr,
411 * we start beaconing. 275 * we start beaconing.
412 * 276 *
413 * Setting an IE on the device will overwrite all current IEs in device. So 277 * 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 278 * we take the current IEs being transmitted by the device, insert the
415 * new one, and call SET IE with all the IEs needed. 279 * new one, and call SET IE with all the IEs needed.
416 * 280 *
417 * The local IE cache will only be updated with the new IE if SET IE 281 * Returns 0 on success; or -ENOMEM.
418 * completed successfully.
419 */ 282 */
420int uwb_rc_ie_add(struct uwb_rc *uwb_rc, 283int uwb_rc_ie_add(struct uwb_rc *uwb_rc,
421 const struct uwb_ie_hdr *ies, size_t size) 284 const struct uwb_ie_hdr *ies, size_t size)
422{ 285{
423 int result = 0; 286 int result = 0;
424 struct device *dev = &uwb_rc->uwb_dev.dev; 287 void *ptr;
425 struct uwb_rc_cmd_set_ie *new_ies; 288 const struct uwb_ie_hdr *ie;
426 size_t ies_size, total_size, acc_size = 0; 289
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); 290 mutex_lock(&uwb_rc->ies_mutex);
439 ies_size = le16_to_cpu(uwb_rc->ies->wIELength); 291
440 total_size = sizeof(*uwb_rc->ies) + ies_size; 292 ptr = (void *)ies;
441 if (total_size + size > uwb_rc->ies_capacity) { 293 for (;;) {
442 d_printf(4, dev, "Reallocating IE cache from %p capacity %zu " 294 ie = uwb_ie_next(&ptr, &size);
443 "to capacity %zu\n", uwb_rc->ies, uwb_rc->ies_capacity, 295 if (!ie)
444 total_size + size); 296 break;
445 new_ies = kzalloc(total_size + size, GFP_KERNEL); 297
446 if (new_ies == NULL) { 298 result = uwb_rc_ie_add_one(uwb_rc, ie);
447 dev_err(dev, "No memory for adding new IE\n"); 299 if (result < 0)
448 result = -ENOMEM; 300 break;
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 } 301 }
458 memcpy((void *)uwb_rc->ies + total_size, ies, size); 302 if (result >= 0) {
459 uwb_rc->ies->wIELength = cpu_to_le16(ies_size + size); 303 if (size == 0) {
460 if (uwb_rc->beaconing != -1) { 304 if (uwb_rc->beaconing != -1)
461 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); 305 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 306 } else
467 result = 0; 307 result = -EINVAL;
468 } 308 }
469 d_printf(4, dev, "IEs now occupy %hu bytes of %zu capacity at %p\n", 309
470 le16_to_cpu(uwb_rc->ies->wIELength), uwb_rc->ies_capacity,
471 uwb_rc->ies);
472error_alloc:
473 mutex_unlock(&uwb_rc->ies_mutex); 310 mutex_unlock(&uwb_rc->ies_mutex);
311
474 return result; 312 return result;
475} 313}
476EXPORT_SYMBOL_GPL(uwb_rc_ie_add); 314EXPORT_SYMBOL_GPL(uwb_rc_ie_add);
@@ -489,53 +327,52 @@ EXPORT_SYMBOL_GPL(uwb_rc_ie_add);
489 * beacon. We don't reallocate, we just mark the size smaller. 327 * beacon. We don't reallocate, we just mark the size smaller.
490 */ 328 */
491static 329static
492int uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove) 330void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
493{ 331{
494 struct uwb_ie_hdr *ie_hdr; 332 struct uwb_ie_hdr *ie;
495 size_t new_len = le16_to_cpu(uwb_rc->ies->wIELength); 333 size_t len = le16_to_cpu(uwb_rc->ies->wIELength);
496 void *itr = uwb_rc->ies->IEData; 334 void *ptr;
497 void *top = itr + new_len; 335 size_t size;
498 336
499 while (itr < top) { 337 ptr = uwb_rc->ies->IEData;
500 ie_hdr = itr; 338 size = len;
501 if (ie_hdr->element_id != to_remove) { 339 for (;;) {
502 itr += sizeof(*ie_hdr) + ie_hdr->length; 340 ie = uwb_ie_next(&ptr, &size);
503 } else { 341 if (!ie)
504 int ie_length; 342 break;
505 ie_length = sizeof(*ie_hdr) + ie_hdr->length; 343 if (ie->element_id == to_remove) {
506 if (top - itr != ie_length) 344 len -= sizeof(struct uwb_ie_hdr) + ie->length;
507 memmove(itr, itr + ie_length, top - itr + ie_length); 345 memmove(ie, ptr, size);
508 top -= ie_length; 346 ptr = ie;
509 new_len -= ie_length;
510 } 347 }
511 } 348 }
512 uwb_rc->ies->wIELength = cpu_to_le16(new_len); 349 uwb_rc->ies->wIELength = cpu_to_le16(len);
513 return 0;
514} 350}
515 351
516 352
517/** 353/**
518 * Remove an IE currently being transmitted by device 354 * uwb_rc_ie_rm - remove an IE from the radio controller's beacon
355 * @uwb_rc: the radio controller.
356 * @element_id: the element ID of the IE to remove.
519 * 357 *
520 * @element_id: id of IE to be removed from device's beacon 358 * Only IEs previously added with uwb_rc_ie_add() may be removed.
359 *
360 * Returns 0 on success; or -ve the SET-IE command to the radio
361 * controller failed.
521 */ 362 */
522int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id) 363int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
523{ 364{
524 struct device *dev = &uwb_rc->uwb_dev.dev; 365 int result = 0;
525 int result;
526 366
527 if (uwb_rc->ies == NULL)
528 return -ESHUTDOWN;
529 mutex_lock(&uwb_rc->ies_mutex); 367 mutex_lock(&uwb_rc->ies_mutex);
530 result = uwb_rc_ie_cache_rm(uwb_rc, element_id); 368
531 if (result < 0) 369 uwb_rc_ie_cache_rm(uwb_rc, element_id);
532 dev_err(dev, "Cannot remove IE from cache.\n"); 370
533 if (uwb_rc->beaconing != -1) { 371 if (uwb_rc->beaconing != -1)
534 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); 372 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
535 if (result < 0) 373
536 dev_err(dev, "Cannot set new IE on device.\n");
537 }
538 mutex_unlock(&uwb_rc->ies_mutex); 374 mutex_unlock(&uwb_rc->ies_mutex);
375
539 return result; 376 return result;
540} 377}
541EXPORT_SYMBOL_GPL(uwb_rc_ie_rm); 378EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);