uwb: order IEs by element ID

	ssize_t result = 0;

	struct uwb_rc_evt_beacon *be;

	struct uwb_beacon_frame *bf;

	struct uwb_buf_ctx ctx = {

		.buf = buf,

		.bytes = 0,

		.size = size

	};

	int ies_len;

	struct uwb_ie_hdr *ies;

	mutex_lock(&bce->mutex);

	be = bce->be;

	if (be == NULL)

		goto out;

	bf = (void *) be->BeaconInfo;

	uwb_ie_for_each(uwb_dev, uwb_ie_dump_hex, &ctx,

			bf->IEData, be->wBeaconInfoLength - sizeof(*bf));

	result = ctx.bytes;

out:

	if (be) {

		bf = (struct uwb_beacon_frame *)bce->be->BeaconInfo;

		ies_len = be->wBeaconInfoLength - sizeof(struct uwb_beacon_frame);

		ies = (struct uwb_ie_hdr *)bf->IEData;

		result = uwb_ie_dump_hex(ies, ies_len, buf, size);

	}

	mutex_unlock(&bce->mutex);

	return result;

}

 */

#include "uwb-internal.h"

#define D_LOCAL 0

#include <linux/uwb/debug.h>

/**

 * uwb_ie_next - get the next IE in a buffer

}

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

 *

 * 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;

	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);

/**

	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)

{

/**

 * 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;

}

}

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);

	return 0;

	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

 * 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) {

	if (result >= 0) {

		if (size == 0) {

			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);

		} 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);

 * 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.

 *

 * Only IEs previously added with uwb_rc_ie_add() may be removed.

 *

 * @element_id: id of IE to be removed from device's beacon

 * 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);

	__uwb_rc_put(rc);

}

EXPORT_SYMBOL_GPL(uwb_rc_put);

/*

 *

 *

 */

ssize_t uwb_rc_print_IEs(struct uwb_rc *uwb_rc, char *buf, size_t size)

{

	ssize_t result;

	struct uwb_rc_evt_get_ie *ie_info;

	struct uwb_buf_ctx ctx;

	result = uwb_rc_get_ie(uwb_rc, &ie_info);

	if (result < 0)

		goto error_get_ie;

	ctx.buf = buf;

	ctx.size = size;

	ctx.bytes = 0;

	uwb_ie_for_each(&uwb_rc->uwb_dev, uwb_ie_dump_hex, &ctx,

			ie_info->IEData, result - sizeof(*ie_info));

	result = ctx.bytes;

	kfree(ie_info);

error_get_ie:

	return result;

}