drivers/staging/uwb/ie.c

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