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 <linux/slab.h>
  28 #include "uwb-internal.h"
  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
  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
  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  */
 108 static
 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
 117         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 118         if (cmd == NULL)
 119                 return -ENOMEM;
 120
 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);
 126         kfree(cmd);
 127         if (result < 0)
 128                 return result;
 129
 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));
 135                 return -EINVAL;
 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));
 140                 return -EINVAL;
 141         }
 142
 143         *pget_ie = get_ie;
 144         return result;
 145 }
 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
 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 /**
 190  * uwb_rc_ie_setup - setup a radio controller's IE manager
 191  * @uwb_rc: the radio controller.
 192  *
 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).
 196  *
 197  * Returns 0 on success; -ve on an error.
 198  */
 199 int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
 200 {
 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;
 207
 208         mutex_lock(&uwb_rc->ies_mutex);
 209
 210         uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
 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;
 214
 215         mutex_unlock(&uwb_rc->ies_mutex);
 216
 217         return 0;
 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
 230 static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
 231 {
 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
 262         return 0;
 263 }
 264
 265 /**
 266  * uwb_rc_ie_add - add new IEs to the radio controller's beacon
 267  * @uwb_rc: the radio controller.
 268  * @ies: the buffer containing the new IE or IEs to be added to
 269  *       the device's beacon.
 270  * @size: length of all the IEs.
 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
 279  * we take the current IEs being transmitted by the device, insert the
 280  * new one, and call SET IE with all the IEs needed.
 281  *
 282  * Returns 0 on success; or -ENOMEM.
 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;
 288         void *ptr;
 289         const struct uwb_ie_hdr *ie;
 290
 291         mutex_lock(&uwb_rc->ies_mutex);
 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;
 302         }
 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);
 307                 } else
 308                         result = -EINVAL;
 309         }
 310
 311         mutex_unlock(&uwb_rc->ies_mutex);
 312
 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
 331 void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
 332 {
 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;
 348                 }
 349         }
 350         uwb_rc->ies->wIELength = cpu_to_le16(len);
 351 }
 352
 353
 354 /**
 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.
 358  *
 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.
 363  */
 364 int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
 365 {
 366         int result = 0;
 367
 368         mutex_lock(&uwb_rc->ies_mutex);
 369
 370         uwb_rc_ie_cache_rm(uwb_rc, element_id);
 371
 372         if (uwb_rc->beaconing != -1)
 373                 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
 374
 375         mutex_unlock(&uwb_rc->ies_mutex);
 376
 377         return result;
 378 }
 379 EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);