X-Git-Url: https://gerrit.opnfv.org/gerrit/gitweb?a=blobdiff_plain;f=kernel%2Fdrivers%2Fuwb%2Fie.c;fp=kernel%2Fdrivers%2Fuwb%2Fie.c;h=902b0f2f961ef9b1e3c7b00b928a4509c86c990e;hb=9ca8dbcc65cfc63d6f5ef3312a33184e1d726e00;hp=0000000000000000000000000000000000000000;hpb=98260f3884f4a202f9ca5eabed40b1354c489b29;p=kvmfornfv.git diff --git a/kernel/drivers/uwb/ie.c b/kernel/drivers/uwb/ie.c new file mode 100644 index 000000000..902b0f2f9 --- /dev/null +++ b/kernel/drivers/uwb/ie.c @@ -0,0 +1,380 @@ +/* + * Ultra Wide Band + * Information Element Handling + * + * Copyright (C) 2005-2006 Intel Corporation + * Inaky Perez-Gonzalez + * Reinette Chatre + * + * 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 +#include +#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);