linux/drivers/uwb/ie.c
<<
>>
Prefs
   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 <linux/export.h>
  29#include "uwb-internal.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 * uwb_ie_dump_hex - print IEs to a character buffer
  65 * @ies: the IEs to print.
  66 * @len: length of all the IEs.
  67 * @buf: the destination buffer.
  68 * @size: size of @buf.
  69 *
  70 * Returns the number of characters written.
  71 */
  72int uwb_ie_dump_hex(const struct uwb_ie_hdr *ies, size_t len,
  73                    char *buf, size_t size)
  74{
  75        void *ptr;
  76        const struct uwb_ie_hdr *ie;
  77        int r = 0;
  78        u8 *d;
  79
  80        ptr = (void *)ies;
  81        for (;;) {
  82                ie = uwb_ie_next(&ptr, &len);
  83                if (!ie)
  84                        break;
  85
  86                r += scnprintf(buf + r, size - r, "%02x %02x",
  87                               (unsigned)ie->element_id,
  88                               (unsigned)ie->length);
  89                d = (uint8_t *)ie + sizeof(struct uwb_ie_hdr);
  90                while (d != ptr && r < size)
  91                        r += scnprintf(buf + r, size - r, " %02x", (unsigned)*d++);
  92                if (r < size)
  93                        buf[r++] = '\n';
  94        };
  95
  96        return r;
  97}
  98
  99/**
 100 * Get the IEs that a radio controller is sending in its beacon
 101 *
 102 * @uwb_rc:  UWB Radio Controller
 103 * @returns: Size read from the system
 104 *
 105 * We don't need to lock the uwb_rc's mutex because we don't modify
 106 * anything. Once done with the iedata buffer, call
 107 * uwb_rc_ie_release(iedata). Don't call kfree on it.
 108 */
 109static
 110ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie)
 111{
 112        ssize_t result;
 113        struct device *dev = &uwb_rc->uwb_dev.dev;
 114        struct uwb_rccb *cmd = NULL;
 115        struct uwb_rceb *reply = NULL;
 116        struct uwb_rc_evt_get_ie *get_ie;
 117
 118        cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 119        if (cmd == NULL)
 120                return -ENOMEM;
 121
 122        cmd->bCommandType = UWB_RC_CET_GENERAL;
 123        cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE);
 124        result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd),
 125                             UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE,
 126                             &reply);
 127        kfree(cmd);
 128        if (result < 0)
 129                return result;
 130
 131        get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb);
 132        if (result < sizeof(*get_ie)) {
 133                dev_err(dev, "not enough data returned for decoding GET IE "
 134                        "(%zu bytes received vs %zu needed)\n",
 135                        result, sizeof(*get_ie));
 136                return -EINVAL;
 137        } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) {
 138                dev_err(dev, "not enough data returned for decoding GET IE "
 139                        "payload (%zu bytes received vs %zu needed)\n", result,
 140                        sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength));
 141                return -EINVAL;
 142        }
 143
 144        *pget_ie = get_ie;
 145        return result;
 146}
 147
 148
 149/**
 150 * Replace all IEs currently being transmitted by a device
 151 *
 152 * @cmd:    pointer to the SET-IE command with the IEs to set
 153 * @size:   size of @buf
 154 */
 155int uwb_rc_set_ie(struct uwb_rc *rc, struct uwb_rc_cmd_set_ie *cmd)
 156{
 157        int result;
 158        struct device *dev = &rc->uwb_dev.dev;
 159        struct uwb_rc_evt_set_ie reply;
 160
 161        reply.rceb.bEventType = UWB_RC_CET_GENERAL;
 162        reply.rceb.wEvent = UWB_RC_CMD_SET_IE;
 163        result = uwb_rc_cmd(rc, "SET-IE", &cmd->rccb,
 164                            sizeof(*cmd) + le16_to_cpu(cmd->wIELength),
 165                            &reply.rceb, sizeof(reply));
 166        if (result < 0)
 167                goto error_cmd;
 168        else if (result != sizeof(reply)) {
 169                dev_err(dev, "SET-IE: not enough data to decode reply "
 170                        "(%d bytes received vs %zu needed)\n",
 171                        result, sizeof(reply));
 172                result = -EIO;
 173        } else if (reply.bResultCode != UWB_RC_RES_SUCCESS) {
 174                dev_err(dev, "SET-IE: command execution failed: %s (%d)\n",
 175                        uwb_rc_strerror(reply.bResultCode), reply.bResultCode);
 176                result = -EIO;
 177        } else
 178                result = 0;
 179error_cmd:
 180        return result;
 181}
 182
 183/* Cleanup the whole IE management subsystem */
 184void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
 185{
 186        mutex_init(&uwb_rc->ies_mutex);
 187}
 188
 189
 190/**
 191 * uwb_rc_ie_setup - setup a radio controller's IE manager
 192 * @uwb_rc: the radio controller.
 193 *
 194 * The current set of IEs are obtained from the hardware with a GET-IE
 195 * command (since the radio controller is not yet beaconing this will
 196 * be just the hardware's MAC and PHY Capability IEs).
 197 *
 198 * Returns 0 on success; -ve on an error.
 199 */
 200int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
 201{
 202        struct uwb_rc_evt_get_ie *ie_info = NULL;
 203        int capacity;
 204
 205        capacity = uwb_rc_get_ie(uwb_rc, &ie_info);
 206        if (capacity < 0)
 207                return capacity;
 208
 209        mutex_lock(&uwb_rc->ies_mutex);
 210
 211        uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
 212        uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL;
 213        uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE);
 214        uwb_rc->ies_capacity = capacity;
 215
 216        mutex_unlock(&uwb_rc->ies_mutex);
 217
 218        return 0;
 219}
 220
 221
 222/* Cleanup the whole IE management subsystem */
 223void uwb_rc_ie_release(struct uwb_rc *uwb_rc)
 224{
 225        kfree(uwb_rc->ies);
 226        uwb_rc->ies = NULL;
 227        uwb_rc->ies_capacity = 0;
 228}
 229
 230
 231static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
 232{
 233        struct uwb_rc_cmd_set_ie *new_ies;
 234        void *ptr, *prev_ie;
 235        struct uwb_ie_hdr *ie;
 236        size_t length, new_ie_len, new_capacity, size, prev_size;
 237
 238        length = le16_to_cpu(rc->ies->wIELength);
 239        new_ie_len = sizeof(struct uwb_ie_hdr) + new_ie->length;
 240        new_capacity = sizeof(struct uwb_rc_cmd_set_ie) + length + new_ie_len;
 241
 242        if (new_capacity > rc->ies_capacity) {
 243                new_ies = krealloc(rc->ies, new_capacity, GFP_KERNEL);
 244                if (!new_ies)
 245                        return -ENOMEM;
 246                rc->ies = new_ies;
 247        }
 248
 249        ptr = rc->ies->IEData;
 250        size = length;
 251        for (;;) {
 252                prev_ie = ptr;
 253                prev_size = size;
 254                ie = uwb_ie_next(&ptr, &size);
 255                if (!ie || ie->element_id > new_ie->element_id)
 256                        break;
 257        }
 258
 259        memmove(prev_ie + new_ie_len, prev_ie, prev_size);
 260        memcpy(prev_ie, new_ie, new_ie_len);
 261        rc->ies->wIELength = cpu_to_le16(length + new_ie_len);
 262
 263        return 0;
 264}
 265
 266/**
 267 * uwb_rc_ie_add - add new IEs to the radio controller's beacon
 268 * @uwb_rc: the radio controller.
 269 * @ies: the buffer containing the new IE or IEs to be added to
 270 *       the device's beacon.
 271 * @size: length of all the IEs.
 272 *
 273 * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB
 274 * after the device sent the first beacon that includes the IEs specified
 275 * in the SET IE command. We thus cannot send this command if the device is
 276 * not beaconing. Instead, a SET IE command will be sent later right after
 277 * we start beaconing.
 278 *
 279 * Setting an IE on the device will overwrite all current IEs in device. So
 280 * we take the current IEs being transmitted by the device, insert the
 281 * new one, and call SET IE with all the IEs needed.
 282 *
 283 * Returns 0 on success; or -ENOMEM.
 284 */
 285int uwb_rc_ie_add(struct uwb_rc *uwb_rc,
 286                  const struct uwb_ie_hdr *ies, size_t size)
 287{
 288        int result = 0;
 289        void *ptr;
 290        const struct uwb_ie_hdr *ie;
 291
 292        mutex_lock(&uwb_rc->ies_mutex);
 293
 294        ptr = (void *)ies;
 295        for (;;) {
 296                ie = uwb_ie_next(&ptr, &size);
 297                if (!ie)
 298                        break;
 299
 300                result = uwb_rc_ie_add_one(uwb_rc, ie);
 301                if (result < 0)
 302                        break;
 303        }
 304        if (result >= 0) {
 305                if (size == 0) {
 306                        if (uwb_rc->beaconing != -1)
 307                                result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
 308                } else
 309                        result = -EINVAL;
 310        }
 311
 312        mutex_unlock(&uwb_rc->ies_mutex);
 313
 314        return result;
 315}
 316EXPORT_SYMBOL_GPL(uwb_rc_ie_add);
 317
 318
 319/*
 320 * Remove an IE from internal cache
 321 *
 322 * We are dealing with our internal IE cache so no need to verify that the
 323 * IEs are valid (it has been done already).
 324 *
 325 * Should be called with ies_mutex held
 326 *
 327 * We do not break out once an IE is found in the cache. It is currently
 328 * possible to have more than one IE with the same ID included in the
 329 * beacon. We don't reallocate, we just mark the size smaller.
 330 */
 331static
 332void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
 333{
 334        struct uwb_ie_hdr *ie;
 335        size_t len = le16_to_cpu(uwb_rc->ies->wIELength);
 336        void *ptr;
 337        size_t size;
 338
 339        ptr = uwb_rc->ies->IEData;
 340        size = len;
 341        for (;;) {
 342                ie = uwb_ie_next(&ptr, &size);
 343                if (!ie)
 344                        break;
 345                if (ie->element_id == to_remove) {
 346                        len -= sizeof(struct uwb_ie_hdr) + ie->length;
 347                        memmove(ie, ptr, size);
 348                        ptr = ie;
 349                }
 350        }
 351        uwb_rc->ies->wIELength = cpu_to_le16(len);
 352}
 353
 354
 355/**
 356 * uwb_rc_ie_rm - remove an IE from the radio controller's beacon
 357 * @uwb_rc: the radio controller.
 358 * @element_id: the element ID of the IE to remove.
 359 *
 360 * Only IEs previously added with uwb_rc_ie_add() may be removed.
 361 *
 362 * Returns 0 on success; or -ve the SET-IE command to the radio
 363 * controller failed.
 364 */
 365int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
 366{
 367        int result = 0;
 368
 369        mutex_lock(&uwb_rc->ies_mutex);
 370
 371        uwb_rc_ie_cache_rm(uwb_rc, element_id);
 372
 373        if (uwb_rc->beaconing != -1)
 374                result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
 375
 376        mutex_unlock(&uwb_rc->ies_mutex);
 377
 378        return result;
 379}
 380EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);
 381
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.