ie.c revision 22d203ecef9b0cc1fa8d8f64c935b451ca7d1022
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 */ 42struct 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} 61EXPORT_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 */ 73ssize_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; 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; 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 */ 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; 221} 222EXPORT_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 */ 231int 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; 255error_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 */ 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 */ 324void 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 */ 340ssize_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; 369error_parse: 370error_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 */ 378void 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 386static 387int __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 */ 420int 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); 472error_alloc: 473 mutex_unlock(&uwb_rc->ies_mutex); 474 return result; 475} 476EXPORT_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 */ 491static 492int 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 */ 522int 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} 541EXPORT_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 */ 550int 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} 571