release/src-rt-6.x.4708/linux/linux-2.6.36/drivers/uwb/ie.c

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