|
|
|
@ -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
|
|
|
|
@ -60,6 +58,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
|
|
|
|
|
*
|
|
|
|
@ -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
|
|
|
|
|
return -EINVAL;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
*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;
|
|
|
|
|
}
|
|
|
|
|
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;
|
|
|
|
|
void *ptr;
|
|
|
|
|
const struct uwb_ie_hdr *ie;
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
|
|
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(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);
|
|
|
|
|
}
|
|
|
|
|
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);
|
|
|
|
@ -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;
|
|
|
|
|
struct uwb_ie_hdr *ie;
|
|
|
|
|
size_t len = le16_to_cpu(uwb_rc->ies->wIELength);
|
|
|
|
|
void *ptr;
|
|
|
|
|
size_t size;
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
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);
|
|
|
|
|