uwb: order IEs by element ID

ECMA-368 requires that IEs in a beacon must be sorted by element ID.  Most
hardware uses the ordering in the Set IE URC command so get the ordering
right on the host.

Also refactor the IE management code:
  - use uwb_ie_next() instead of uwb_ie_for_each().
  - remove unnecessary functions.
  - API is now only uwb_rc_ie_add() and uwb_rc_ie_rm().

Signed-off-by: David Vrabel <david.vrabel@csr.com>

1 files changed, 150 insertions, 313 deletions

diff --git a/drivers/uwb/ie.c b/drivers/uwb/ie.c
index cf6f3d152b9d..ab976686175b 100644
--- a/drivers/uwb/ie.c
+++ b/drivers/uwb/ie.c
@@ -25,8 +25,6 @@
  */
 
 #include "uwb-internal.h"
-#define D_LOCAL 0
-#include <linux/uwb/debug.h>
 
 /**
  * uwb_ie_next - get the next IE in a buffer
@@ -61,6 +59,42 @@ struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len)
 EXPORT_SYMBOL_GPL(uwb_ie_next);
 
 /**
+ * uwb_ie_dump_hex - print IEs to a character buffer
+ * @ies: the IEs to print.
+ * @len: length of all the IEs.
+ * @buf: the destination buffer.
+ * @size: size of @buf.
+ *
+ * Returns the number of characters written.
+ */
+int uwb_ie_dump_hex(const struct uwb_ie_hdr *ies, size_t len,
+                    char *buf, size_t size)
+{
+        void *ptr;
+        const struct uwb_ie_hdr *ie;
+        int r = 0;
+        u8 *d;
+
+        ptr = (void *)ies;
+        for (;;) {
+                ie = uwb_ie_next(&ptr, &len);
+                if (!ie)
+                        break;
+
+                r += scnprintf(buf + r, size - r, "%02x %02x",
+                               (unsigned)ie->element_id,
+                               (unsigned)ie->length);
+                d = (uint8_t *)ie + sizeof(struct uwb_ie_hdr);
+                while (d != ptr && r < size)
+                        r += scnprintf(buf + r, size - r, " %02x", (unsigned)*d++);
+                if (r < size)
+                        buf[r++] = '\n';
+        };
+
+        return r;
+}
+
+/**
  * Get the IEs that a radio controller is sending in its beacon
  *
  * @uwb_rc:  UWB Radio Controller
@@ -70,6 +104,7 @@ EXPORT_SYMBOL_GPL(uwb_ie_next);
  * anything. Once done with the iedata buffer, call
  * uwb_rc_ie_release(iedata). Don't call kfree on it.
  */
+static
 ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie)
 {
         ssize_t result;
@@ -78,148 +113,35 @@ ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie)
         struct uwb_rceb *reply = NULL;
         struct uwb_rc_evt_get_ie *get_ie;
 
-        d_fnstart(3, dev, "(%p, %p)\n", uwb_rc, pget_ie);
-        result = -ENOMEM;
         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
         if (cmd == NULL)
-                goto error_kzalloc;
+                return -ENOMEM;
+
         cmd->bCommandType = UWB_RC_CET_GENERAL;
         cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE);
         result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd),
                              UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE,
                              &reply);
+        kfree(cmd);
         if (result < 0)
-                goto error_cmd;
+                return result;
+
         get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb);
         if (result < sizeof(*get_ie)) {
                 dev_err(dev, "not enough data returned for decoding GET IE "
                         "(%zu bytes received vs %zu needed)\n",
                         result, sizeof(*get_ie));
-                result = -EINVAL;
+                return -EINVAL;
         } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) {
                 dev_err(dev, "not enough data returned for decoding GET IE "
                         "payload (%zu bytes received vs %zu needed)\n", result,
                         sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength));
-                result = -EINVAL;
-        } else
-                *pget_ie = get_ie;
-error_cmd:
-        kfree(cmd);
-error_kzalloc:
-        d_fnend(3, dev, "(%p, %p) = %d\n", uwb_rc, pget_ie, (int)result);
-        return result;
-}
-EXPORT_SYMBOL_GPL(uwb_rc_get_ie);
-
-
-/*
- * Given a pointer to an IE, print it in ASCII/hex followed by a new line
- *
- * @ie_hdr: pointer to the IE header. Length is in there, and it is
- *          guaranteed that the ie_hdr->length bytes following it are
- *          safely accesible.
- *
- * @_data: context data passed from uwb_ie_for_each(), an struct output_ctx
- */
-int uwb_ie_dump_hex(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr,
-                    size_t offset, void *_ctx)
-{
-        struct uwb_buf_ctx *ctx = _ctx;
-        const u8 *pl = (void *)(ie_hdr + 1);
-        u8 pl_itr;
-
-        ctx->bytes += scnprintf(ctx->buf + ctx->bytes, ctx->size - ctx->bytes,
-                                "%02x %02x ", (unsigned) ie_hdr->element_id,
-                                (unsigned) ie_hdr->length);
-        pl_itr = 0;
-        while (pl_itr < ie_hdr->length && ctx->bytes < ctx->size)
-                ctx->bytes += scnprintf(ctx->buf + ctx->bytes,
-                                        ctx->size - ctx->bytes,
-                                        "%02x ", (unsigned) pl[pl_itr++]);
-        if (ctx->bytes < ctx->size)
-                ctx->buf[ctx->bytes++] = '\n';
-        return 0;
-}
-EXPORT_SYMBOL_GPL(uwb_ie_dump_hex);
-
-
-/**
- * Verify that a pointer in a buffer points to valid IE
- *
- * @start: pointer to start of buffer in which IE appears
- * @itr:   pointer to IE inside buffer that will be verified
- * @top:   pointer to end of buffer
- *
- * @returns: 0 if IE is valid, <0 otherwise
- *
- * Verification involves checking that the buffer can contain a
- * header and the amount of data reported in the IE header can be found in
- * the buffer.
- */
-static
-int uwb_rc_ie_verify(struct uwb_dev *uwb_dev, const void *start,
-                     const void *itr, const void *top)
-{
-        struct device *dev = &uwb_dev->dev;
-        const struct uwb_ie_hdr *ie_hdr;
-
-        if (top - itr < sizeof(*ie_hdr)) {
-                dev_err(dev, "Bad IE: no data to decode header "
-                        "(%zu bytes left vs %zu needed) at offset %zu\n",
-                        top - itr, sizeof(*ie_hdr), itr - start);
-                return -EINVAL;
-        }
-        ie_hdr = itr;
-        itr += sizeof(*ie_hdr);
-        if (top - itr < ie_hdr->length) {
-                dev_err(dev, "Bad IE: not enough data for payload "
-                        "(%zu bytes left vs %zu needed) at offset %zu\n",
-                        top - itr, (size_t)ie_hdr->length,
-                        (void *)ie_hdr - start);
                 return -EINVAL;
         }
-        return 0;
-}
 
-
-/**
- * Walk a buffer filled with consecutive IE's a buffer
- *
- * @uwb_dev: UWB device this IEs belong to (for err messages mainly)
- *
- * @fn: function to call with each IE; if it returns 0, we keep
- *      traversing the buffer. If it returns !0, we'll stop and return
- *      that value.
- *
- * @data: pointer passed to @fn
- *
- * @buf: buffer where the consecutive IEs are located
- *
- * @size: size of @buf
- *
- * Each IE is checked for basic correctness (there is space left for
- * the header and the payload). If that test is failed, we stop
- * processing. For every good IE, @fn is called.
- */
-ssize_t uwb_ie_for_each(struct uwb_dev *uwb_dev, uwb_ie_f fn, void *data,
-                        const void *buf, size_t size)
-{
-        ssize_t result = 0;
-        const struct uwb_ie_hdr *ie_hdr;
-        const void *itr = buf, *top = itr + size;
-
-        while (itr < top) {
-                if (uwb_rc_ie_verify(uwb_dev, buf, itr, top) != 0)
-                        break;
-                ie_hdr = itr;
-                itr += sizeof(*ie_hdr) + ie_hdr->length;
-                result = fn(uwb_dev, ie_hdr, itr - buf, data);
-                if (result != 0)
-                        break;
-        }
+        *pget_ie = get_ie;
         return result;
 }
-EXPORT_SYMBOL_GPL(uwb_ie_for_each);
 
 
 /**
@@ -256,70 +178,6 @@ error_cmd:
         return result;
 }
 
-/**
- * Determine by IE id if IE is host settable
- * WUSB 1.0 [8.6.2.8 Table 8.85]
- *
- * EXCEPTION:
- * All but UWB_IE_WLP appears in Table 8.85 from WUSB 1.0. Setting this IE
- * is required for the WLP substack to perform association with its WSS so
- * we hope that the WUSB spec will be changed to reflect this.
- */
-static
-int uwb_rc_ie_is_host_settable(enum uwb_ie element_id)
-{
-        if (element_id == UWB_PCA_AVAILABILITY ||
-            element_id == UWB_BP_SWITCH_IE ||
-            element_id == UWB_MAC_CAPABILITIES_IE ||
-            element_id == UWB_PHY_CAPABILITIES_IE ||
-            element_id == UWB_APP_SPEC_PROBE_IE ||
-            element_id == UWB_IDENTIFICATION_IE ||
-            element_id == UWB_MASTER_KEY_ID_IE ||
-            element_id == UWB_IE_WLP ||
-            element_id == UWB_APP_SPEC_IE)
-                return 1;
-        return 0;
-}
-
-
-/**
- * Extract Host Settable IEs from IE
- *
- * @ie_data: pointer to buffer containing all IEs
- * @size:    size of buffer
- *
- * @returns: length of buffer that only includes host settable IEs
- *
- * Given a buffer of IEs we move all Host Settable IEs to front of buffer
- * by overwriting the IEs that are not Host Settable.
- * Buffer length is adjusted accordingly.
- */
-static
-ssize_t uwb_rc_parse_host_settable_ie(struct uwb_dev *uwb_dev,
-                                      void *ie_data, size_t size)
-{
-        size_t new_len = size;
-        struct uwb_ie_hdr *ie_hdr;
-        size_t ie_length;
-        void *itr = ie_data, *top = itr + size;
-
-        while (itr < top) {
-                if (uwb_rc_ie_verify(uwb_dev, ie_data, itr, top) != 0)
-                        break;
-                ie_hdr = itr;
-                ie_length = sizeof(*ie_hdr) + ie_hdr->length;
-                if (uwb_rc_ie_is_host_settable(ie_hdr->element_id)) {
-                        itr += ie_length;
-                } else {
-                        memmove(itr, itr + ie_length, top - (itr + ie_length));
-                        new_len -= ie_length;
-                        top -= ie_length;
-                }
-        }
-        return new_len;
-}
-
-
 /* Cleanup the whole IE management subsystem */
 void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
 {
@@ -328,49 +186,34 @@ void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
 
 
 /**
- * Set up cache for host settable IEs currently being transmitted
+ * uwb_rc_ie_setup - setup a radio controller's IE manager
+ * @uwb_rc: the radio controller.
  *
- * First we just call GET-IE to get the current IEs being transmitted
- * (or we workaround and pretend we did) and (because the format is
- * the same) reuse that as the IE cache (with the command prefix, as
- * explained in 'struct uwb_rc').
+ * The current set of IEs are obtained from the hardware with a GET-IE
+ * command (since the radio controller is not yet beaconing this will
+ * be just the hardware's MAC and PHY Capability IEs).
  *
- * @returns: size of cache created
+ * Returns 0 on success; -ve on an error.
  */
-ssize_t uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
+int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
 {
-        struct device *dev = &uwb_rc->uwb_dev.dev;
-        ssize_t result;
-        size_t capacity;
-        struct uwb_rc_evt_get_ie *ie_info;
+        struct uwb_rc_evt_get_ie *ie_info = NULL;
+        int capacity;
+
+        capacity = uwb_rc_get_ie(uwb_rc, &ie_info);
+        if (capacity < 0)
+                return capacity;
 
-        d_fnstart(3, dev, "(%p)\n", uwb_rc);
         mutex_lock(&uwb_rc->ies_mutex);
-        result = uwb_rc_get_ie(uwb_rc, &ie_info);
-        if (result < 0)
-                goto error_get_ie;
-        capacity = result;
-        d_printf(5, dev, "Got IEs %zu bytes (%zu long at %p)\n", result,
-                 (size_t)le16_to_cpu(ie_info->wIELength), ie_info);
-
-        /* Remove IEs that host should not set. */
-        result = uwb_rc_parse_host_settable_ie(&uwb_rc->uwb_dev,
-                        ie_info->IEData, le16_to_cpu(ie_info->wIELength));
-        if (result < 0)
-                goto error_parse;
-        d_printf(5, dev, "purged non-settable IEs to %zu bytes\n", result);
-        uwb_rc->ies = (void *) ie_info;
+
+        uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
         uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL;
         uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE);
         uwb_rc->ies_capacity = capacity;
-        d_printf(5, dev, "IE cache at %p %zu bytes, %zu capacity\n",
-                 ie_info, result, capacity);
-        result = 0;
-error_parse:
-error_get_ie:
+
         mutex_unlock(&uwb_rc->ies_mutex);
-        d_fnend(3, dev, "(%p) = %zu\n", uwb_rc, result);
-        return result;
+
+        return 0;
 }
 
 
@@ -383,26 +226,47 @@ void uwb_rc_ie_release(struct uwb_rc *uwb_rc)
 }
 
 
-static
-int __acc_size(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr,
-               size_t offset, void *_ctx)
+static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
 {
-        size_t *acc_size = _ctx;
-        *acc_size += sizeof(*ie_hdr) + ie_hdr->length;
-        d_printf(6, &uwb_dev->dev, "new acc size %zu\n", *acc_size);
+        struct uwb_rc_cmd_set_ie *new_ies;
+        void *ptr, *prev_ie;
+        struct uwb_ie_hdr *ie;
+        size_t length, new_ie_len, new_capacity, size, prev_size;
+
+        length = le16_to_cpu(rc->ies->wIELength);
+        new_ie_len = sizeof(struct uwb_ie_hdr) + new_ie->length;
+        new_capacity = sizeof(struct uwb_rc_cmd_set_ie) + length + new_ie_len;
+
+        if (new_capacity > rc->ies_capacity) {
+                new_ies = krealloc(rc->ies, new_capacity, GFP_KERNEL);
+                if (!new_ies)
+                        return -ENOMEM;
+                rc->ies = new_ies;
+        }
+
+        ptr = rc->ies->IEData;
+        size = length;
+        for (;;) {
+                prev_ie = ptr;
+                prev_size = size;
+                ie = uwb_ie_next(&ptr, &size);
+                if (!ie || ie->element_id > new_ie->element_id)
+                        break;
+        }
+
+        memmove(prev_ie + new_ie_len, prev_ie, prev_size);
+        memcpy(prev_ie, new_ie, new_ie_len);
+        rc->ies->wIELength = cpu_to_le16(length + new_ie_len);
+
         return 0;
 }
 
-
 /**
- * Add a new IE to IEs currently being transmitted by device
- *
+ * uwb_rc_ie_add - add new IEs to the radio controller's beacon
+ * @uwb_rc: the radio controller.
  * @ies: the buffer containing the new IE or IEs to be added to
- *       the device's beacon. The buffer will be verified for
- *       consistence (meaning the headers should be right) and
- *       consistent with the buffer size.
- * @size: size of @ies (in bytes, total buffer size)
- * @returns: 0 if ok, <0 errno code on error
+ *       the device's beacon.
+ * @size: length of all the IEs.
  *
  * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB
  * after the device sent the first beacon that includes the IEs specified
@@ -411,66 +275,40 @@ int __acc_size(struct uwb_dev *uwb_dev, const struct uwb_ie_hdr *ie_hdr,
  * we start beaconing.
  *
  * Setting an IE on the device will overwrite all current IEs in device. So
- * we take the current IEs being transmitted by the device, append the
+ * we take the current IEs being transmitted by the device, insert the
  * new one, and call SET IE with all the IEs needed.
  *
- * The local IE cache will only be updated with the new IE if SET IE
- * completed successfully.
+ * Returns 0 on success; or -ENOMEM.
  */
 int uwb_rc_ie_add(struct uwb_rc *uwb_rc,
                   const struct uwb_ie_hdr *ies, size_t size)
 {
         int result = 0;
-        struct device *dev = &uwb_rc->uwb_dev.dev;
-        struct uwb_rc_cmd_set_ie *new_ies;
-        size_t ies_size, total_size, acc_size = 0;
-
-        if (uwb_rc->ies == NULL)
-                return -ESHUTDOWN;
-        uwb_ie_for_each(&uwb_rc->uwb_dev, __acc_size, &acc_size, ies, size);
-        if (acc_size != size) {
-                dev_err(dev, "BUG: bad IEs, misconstructed headers "
-                        "[%zu bytes reported vs %zu calculated]\n",
-                        size, acc_size);
-                WARN_ON(1);
-                return -EINVAL;
-        }
+        void *ptr;
+        const struct uwb_ie_hdr *ie;
+
         mutex_lock(&uwb_rc->ies_mutex);
-        ies_size = le16_to_cpu(uwb_rc->ies->wIELength);
-        total_size = sizeof(*uwb_rc->ies) + ies_size;
-        if (total_size + size > uwb_rc->ies_capacity) {
-                d_printf(4, dev, "Reallocating IE cache from %p capacity %zu "
-                         "to capacity %zu\n", uwb_rc->ies, uwb_rc->ies_capacity,
-                         total_size + size);
-                new_ies = kzalloc(total_size + size, GFP_KERNEL);
-                if (new_ies == NULL) {
-                        dev_err(dev, "No memory for adding new IE\n");
-                        result = -ENOMEM;
-                        goto error_alloc;
-                }
-                memcpy(new_ies, uwb_rc->ies, total_size);
-                uwb_rc->ies_capacity = total_size + size;
-                kfree(uwb_rc->ies);
-                uwb_rc->ies = new_ies;
-                d_printf(4, dev, "New IE cache at %p capacity %zu\n",
-                         uwb_rc->ies, uwb_rc->ies_capacity);
+
+        ptr = (void *)ies;
+        for (;;) {
+                ie = uwb_ie_next(&ptr, &size);
+                if (!ie)
+                        break;
+
+                result = uwb_rc_ie_add_one(uwb_rc, ie);
+                if (result < 0)
+                        break;
         }
-        memcpy((void *)uwb_rc->ies + total_size, ies, size);
-        uwb_rc->ies->wIELength = cpu_to_le16(ies_size + size);
-        if (uwb_rc->beaconing != -1) {
-                result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
-                if (result < 0) {
-                        dev_err(dev, "Cannot set new IE on device: %d\n",
-                                result);
-                        uwb_rc->ies->wIELength = cpu_to_le16(ies_size);
+        if (result >= 0) {
+                if (size == 0) {
+                        if (uwb_rc->beaconing != -1)
+                                result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
                 } else
-                        result = 0;
+                        result = -EINVAL;
         }
-        d_printf(4, dev, "IEs now occupy %hu bytes of %zu capacity at %p\n",
-                 le16_to_cpu(uwb_rc->ies->wIELength), uwb_rc->ies_capacity,
-                 uwb_rc->ies);
-error_alloc:
+
         mutex_unlock(&uwb_rc->ies_mutex);
+
         return result;
 }
 EXPORT_SYMBOL_GPL(uwb_rc_ie_add);
@@ -489,53 +327,52 @@ EXPORT_SYMBOL_GPL(uwb_rc_ie_add);
  * beacon. We don't reallocate, we just mark the size smaller.
  */
 static
-int uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
+void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
 {
-        struct uwb_ie_hdr *ie_hdr;
-        size_t new_len = le16_to_cpu(uwb_rc->ies->wIELength);
-        void *itr = uwb_rc->ies->IEData;
-        void *top = itr + new_len;
-
-        while (itr < top) {
-                ie_hdr = itr;
-                if (ie_hdr->element_id != to_remove) {
-                        itr += sizeof(*ie_hdr) + ie_hdr->length;
-                } else {
-                        int ie_length;
-                        ie_length = sizeof(*ie_hdr) + ie_hdr->length;
-                        if (top - itr != ie_length)
-                                memmove(itr, itr + ie_length, top - itr + ie_length);
-                        top -= ie_length;
-                        new_len -= ie_length;
+        struct uwb_ie_hdr *ie;
+        size_t len = le16_to_cpu(uwb_rc->ies->wIELength);
+        void *ptr;
+        size_t size;
+
+        ptr = uwb_rc->ies->IEData;
+        size = len;
+        for (;;) {
+                ie = uwb_ie_next(&ptr, &size);
+                if (!ie)
+                        break;
+                if (ie->element_id == to_remove) {
+                        len -= sizeof(struct uwb_ie_hdr) + ie->length;
+                        memmove(ie, ptr, size);
+                        ptr = ie;
                 }
         }
-        uwb_rc->ies->wIELength = cpu_to_le16(new_len);
-        return 0;
+        uwb_rc->ies->wIELength = cpu_to_le16(len);
 }
 
 
 /**
- * Remove an IE currently being transmitted by device
+ * uwb_rc_ie_rm - remove an IE from the radio controller's beacon
+ * @uwb_rc: the radio controller.
+ * @element_id: the element ID of the IE to remove.
  *
- * @element_id: id of IE to be removed from device's beacon
+ * Only IEs previously added with uwb_rc_ie_add() may be removed.
+ *
+ * Returns 0 on success; or -ve the SET-IE command to the radio
+ * controller failed.
  */
 int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
 {
-        struct device *dev = &uwb_rc->uwb_dev.dev;
-        int result;
+        int result = 0;
 
-        if (uwb_rc->ies == NULL)
-                return -ESHUTDOWN;
         mutex_lock(&uwb_rc->ies_mutex);
-        result = uwb_rc_ie_cache_rm(uwb_rc, element_id);
-        if (result < 0)
-                dev_err(dev, "Cannot remove IE from cache.\n");
-        if (uwb_rc->beaconing != -1) {
+
+        uwb_rc_ie_cache_rm(uwb_rc, element_id);
+
+        if (uwb_rc->beaconing != -1)
                 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
-                if (result < 0)
-                        dev_err(dev, "Cannot set new IE on device.\n");
-        }
+
         mutex_unlock(&uwb_rc->ies_mutex);
+
         return result;
 }
 EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);