[net-next-2.6.git] / drivers / uwb / ie.c

/*
 * Ultra Wide Band
 * Information Element Handling
 *
 * Copyright (C) 2005-2006 Intel Corporation
 * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
 * Reinette Chatre <reinette.chatre@intel.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License version
 * 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301, USA.
 *
 *
 * FIXME: docs
 */

#include <linux/slab.h>
#include "uwb-internal.h"

/**
 * uwb_ie_next - get the next IE in a buffer
 * @ptr: start of the buffer containing the IE data
 * @len: length of the buffer
 *
 * Both @ptr and @len are updated so subsequent calls to uwb_ie_next()
 * will get the next IE.
 *
 * NULL is returned (and @ptr and @len will not be updated) if there
 * are no more IEs in the buffer or the buffer is too short.
 */
struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len)
{
	struct uwb_ie_hdr *hdr;
	size_t ie_len;

	if (*len < sizeof(struct uwb_ie_hdr))
		return NULL;

	hdr = *ptr;
	ie_len = sizeof(struct uwb_ie_hdr) + hdr->length;

	if (*len < ie_len)
		return NULL;

	*ptr += ie_len;
	*len -= ie_len;

	return hdr;
}
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
 * @returns: Size read from the system
 *
 * We don't need to lock the uwb_rc's mutex because we don't modify
 * 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 device *dev = &uwb_rc->uwb_dev.dev;
	struct uwb_rccb *cmd = NULL;
	struct uwb_rceb *reply = NULL;
	struct uwb_rc_evt_get_ie *get_ie;

	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
	if (cmd == NULL)
		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)
		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));
		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));
		return -EINVAL;
	}

	*pget_ie = get_ie;
	return result;
}


/**
 * Replace all IEs currently being transmitted by a device
 *
 * @cmd:    pointer to the SET-IE command with the IEs to set
 * @size:   size of @buf
 */
int uwb_rc_set_ie(struct uwb_rc *rc, struct uwb_rc_cmd_set_ie *cmd)
{
	int result;
	struct device *dev = &rc->uwb_dev.dev;
	struct uwb_rc_evt_set_ie reply;

	reply.rceb.bEventType = UWB_RC_CET_GENERAL;
	reply.rceb.wEvent = UWB_RC_CMD_SET_IE;
	result = uwb_rc_cmd(rc, "SET-IE", &cmd->rccb,
			    sizeof(*cmd) + le16_to_cpu(cmd->wIELength),
			    &reply.rceb, sizeof(reply));
	if (result < 0)
		goto error_cmd;
	else if (result != sizeof(reply)) {
		dev_err(dev, "SET-IE: not enough data to decode reply "
			"(%d bytes received vs %zu needed)\n",
			result, sizeof(reply));
		result = -EIO;
	} else if (reply.bResultCode != UWB_RC_RES_SUCCESS) {
		dev_err(dev, "SET-IE: command execution failed: %s (%d)\n",
			uwb_rc_strerror(reply.bResultCode), reply.bResultCode);
		result = -EIO;
	} else
		result = 0;
error_cmd:
	return result;
}

/* Cleanup the whole IE management subsystem */
void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
{
	mutex_init(&uwb_rc->ies_mutex);
}


/**
 * uwb_rc_ie_setup - setup a radio controller's IE manager
 * @uwb_rc: the radio controller.
 *
 * 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 0 on success; -ve on an error.
 */
int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
{
	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;

	mutex_lock(&uwb_rc->ies_mutex);

	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;

	mutex_unlock(&uwb_rc->ies_mutex);

	return 0;
}


/* Cleanup the whole IE management subsystem */
void uwb_rc_ie_release(struct uwb_rc *uwb_rc)
{
	kfree(uwb_rc->ies);
	uwb_rc->ies = NULL;
	uwb_rc->ies_capacity = 0;
}


static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
{
	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;
}

/**
 * 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.
 * @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
 * in the SET IE command. We thus cannot send this command if the device is
 * not beaconing. Instead, a SET IE command will be sent later right after
 * 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, insert the
 * new one, and call SET IE with all the IEs needed.
 *
 * 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;
	void *ptr;
	const struct uwb_ie_hdr *ie;

	mutex_lock(&uwb_rc->ies_mutex);

	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;
	}
	if (result >= 0) {
		if (size == 0) {
			if (uwb_rc->beaconing != -1)
				result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
		} else
			result = -EINVAL;
	}

	mutex_unlock(&uwb_rc->ies_mutex);

	return result;
}
EXPORT_SYMBOL_GPL(uwb_rc_ie_add);


/*
 * Remove an IE from internal cache
 *
 * We are dealing with our internal IE cache so no need to verify that the
 * IEs are valid (it has been done already).
 *
 * Should be called with ies_mutex held
 *
 * We do not break out once an IE is found in the cache. It is currently
 * possible to have more than one IE with the same ID included in the
 * beacon. We don't reallocate, we just mark the size smaller.
 */
static
void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
{
	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(len);
}


/**
 * 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.
 *
 * 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)
{
	int result = 0;

	mutex_lock(&uwb_rc->ies_mutex);

	uwb_rc_ie_cache_rm(uwb_rc, element_id);

	if (uwb_rc->beaconing != -1)
		result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);

	mutex_unlock(&uwb_rc->ies_mutex);

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