drivers/uwb/ie.c

   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);