| /* |
| * Ultra Wide Band Radio Control |
| * Event Size Tables management |
| * |
| * Copyright (C) 2005-2006 Intel Corporation |
| * Inaky Perez-Gonzalez <inaky.perez-gonzalez@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 |
| * |
| * Infrastructure, code and data tables for guessing the size of |
| * events received on the notification endpoints of UWB radio |
| * controllers. |
| * |
| * You define a table of events and for each, its size and how to get |
| * the extra size. |
| * |
| * ENTRY POINTS: |
| * |
| * uwb_est_{init/destroy}(): To initialize/release the EST subsystem. |
| * |
| * uwb_est_[u]register(): To un/register event size tables |
| * uwb_est_grow() |
| * |
| * uwb_est_find_size(): Get the size of an event |
| * uwb_est_get_size() |
| */ |
| #include <linux/spinlock.h> |
| #define D_LOCAL 0 |
| #include <linux/uwb/debug.h> |
| #include "uwb-internal.h" |
| |
| |
| struct uwb_est { |
| u16 type_event_high; |
| u16 vendor, product; |
| u8 entries; |
| const struct uwb_est_entry *entry; |
| }; |
| |
| |
| static struct uwb_est *uwb_est; |
| static u8 uwb_est_size; |
| static u8 uwb_est_used; |
| static DEFINE_RWLOCK(uwb_est_lock); |
| |
| /** |
| * WUSB Standard Event Size Table, HWA-RC interface |
| * |
| * Sizes for events and notifications type 0 (general), high nibble 0. |
| */ |
| static |
| struct uwb_est_entry uwb_est_00_00xx[] = { |
| [UWB_RC_EVT_IE_RCV] = { |
| .size = sizeof(struct uwb_rc_evt_ie_rcv), |
| .offset = 1 + offsetof(struct uwb_rc_evt_ie_rcv, wIELength), |
| }, |
| [UWB_RC_EVT_BEACON] = { |
| .size = sizeof(struct uwb_rc_evt_beacon), |
| .offset = 1 + offsetof(struct uwb_rc_evt_beacon, wBeaconInfoLength), |
| }, |
| [UWB_RC_EVT_BEACON_SIZE] = { |
| .size = sizeof(struct uwb_rc_evt_beacon_size), |
| }, |
| [UWB_RC_EVT_BPOIE_CHANGE] = { |
| .size = sizeof(struct uwb_rc_evt_bpoie_change), |
| .offset = 1 + offsetof(struct uwb_rc_evt_bpoie_change, |
| wBPOIELength), |
| }, |
| [UWB_RC_EVT_BP_SLOT_CHANGE] = { |
| .size = sizeof(struct uwb_rc_evt_bp_slot_change), |
| }, |
| [UWB_RC_EVT_BP_SWITCH_IE_RCV] = { |
| .size = sizeof(struct uwb_rc_evt_bp_switch_ie_rcv), |
| .offset = 1 + offsetof(struct uwb_rc_evt_bp_switch_ie_rcv, wIELength), |
| }, |
| [UWB_RC_EVT_DEV_ADDR_CONFLICT] = { |
| .size = sizeof(struct uwb_rc_evt_dev_addr_conflict), |
| }, |
| [UWB_RC_EVT_DRP_AVAIL] = { |
| .size = sizeof(struct uwb_rc_evt_drp_avail) |
| }, |
| [UWB_RC_EVT_DRP] = { |
| .size = sizeof(struct uwb_rc_evt_drp), |
| .offset = 1 + offsetof(struct uwb_rc_evt_drp, ie_length), |
| }, |
| [UWB_RC_EVT_BP_SWITCH_STATUS] = { |
| .size = sizeof(struct uwb_rc_evt_bp_switch_status), |
| }, |
| [UWB_RC_EVT_CMD_FRAME_RCV] = { |
| .size = sizeof(struct uwb_rc_evt_cmd_frame_rcv), |
| .offset = 1 + offsetof(struct uwb_rc_evt_cmd_frame_rcv, dataLength), |
| }, |
| [UWB_RC_EVT_CHANNEL_CHANGE_IE_RCV] = { |
| .size = sizeof(struct uwb_rc_evt_channel_change_ie_rcv), |
| .offset = 1 + offsetof(struct uwb_rc_evt_channel_change_ie_rcv, wIELength), |
| }, |
| [UWB_RC_CMD_CHANNEL_CHANGE] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_DEV_ADDR_MGMT] = { |
| .size = sizeof(struct uwb_rc_evt_dev_addr_mgmt) }, |
| [UWB_RC_CMD_GET_IE] = { |
| .size = sizeof(struct uwb_rc_evt_get_ie), |
| .offset = 1 + offsetof(struct uwb_rc_evt_get_ie, wIELength), |
| }, |
| [UWB_RC_CMD_RESET] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SCAN] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SET_BEACON_FILTER] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SET_DRP_IE] = { |
| .size = sizeof(struct uwb_rc_evt_set_drp_ie), |
| }, |
| [UWB_RC_CMD_SET_IE] = { |
| .size = sizeof(struct uwb_rc_evt_set_ie), |
| }, |
| [UWB_RC_CMD_SET_NOTIFICATION_FILTER] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SET_TX_POWER] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SLEEP] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_START_BEACON] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_STOP_BEACON] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_BP_MERGE] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SEND_COMMAND_FRAME] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| [UWB_RC_CMD_SET_ASIE_NOTIF] = { |
| .size = sizeof(struct uwb_rc_evt_confirm), |
| }, |
| }; |
| |
| static |
| struct uwb_est_entry uwb_est_01_00xx[] = { |
| [UWB_RC_DAA_ENERGY_DETECTED] = { |
| .size = sizeof(struct uwb_rc_evt_daa_energy_detected), |
| }, |
| [UWB_RC_SET_DAA_ENERGY_MASK] = { |
| .size = sizeof(struct uwb_rc_evt_set_daa_energy_mask), |
| }, |
| [UWB_RC_SET_NOTIFICATION_FILTER_EX] = { |
| .size = sizeof(struct uwb_rc_evt_set_notification_filter_ex), |
| }, |
| }; |
| |
| /** |
| * Initialize the EST subsystem |
| * |
| * Register the standard tables also. |
| * |
| * FIXME: tag init |
| */ |
| int uwb_est_create(void) |
| { |
| int result; |
| |
| uwb_est_size = 2; |
| uwb_est_used = 0; |
| uwb_est = kzalloc(uwb_est_size * sizeof(uwb_est[0]), GFP_KERNEL); |
| if (uwb_est == NULL) |
| return -ENOMEM; |
| |
| result = uwb_est_register(UWB_RC_CET_GENERAL, 0, 0xffff, 0xffff, |
| uwb_est_00_00xx, ARRAY_SIZE(uwb_est_00_00xx)); |
| if (result < 0) |
| goto out; |
| result = uwb_est_register(UWB_RC_CET_EX_TYPE_1, 0, 0xffff, 0xffff, |
| uwb_est_01_00xx, ARRAY_SIZE(uwb_est_01_00xx)); |
| out: |
| return result; |
| } |
| |
| |
| /** Clean it up */ |
| void uwb_est_destroy(void) |
| { |
| kfree(uwb_est); |
| uwb_est = NULL; |
| uwb_est_size = uwb_est_used = 0; |
| } |
| |
| |
| /** |
| * Double the capacity of the EST table |
| * |
| * @returns 0 if ok, < 0 errno no error. |
| */ |
| static |
| int uwb_est_grow(void) |
| { |
| size_t actual_size = uwb_est_size * sizeof(uwb_est[0]); |
| void *new = kmalloc(2 * actual_size, GFP_ATOMIC); |
| if (new == NULL) |
| return -ENOMEM; |
| memcpy(new, uwb_est, actual_size); |
| memset(new + actual_size, 0, actual_size); |
| kfree(uwb_est); |
| uwb_est = new; |
| uwb_est_size *= 2; |
| return 0; |
| } |
| |
| |
| /** |
| * Register an event size table |
| * |
| * Makes room for it if the table is full, and then inserts it in the |
| * right position (entries are sorted by type, event_high, vendor and |
| * then product). |
| * |
| * @vendor: vendor code for matching against the device (0x0000 and |
| * 0xffff mean any); use 0x0000 to force all to match without |
| * checking possible vendor specific ones, 0xfffff to match |
| * after checking vendor specific ones. |
| * |
| * @product: product code from that vendor; same matching rules, use |
| * 0x0000 for not allowing vendor specific matches, 0xffff |
| * for allowing. |
| * |
| * This arragement just makes the tables sort differenty. Because the |
| * table is sorted by growing type-event_high-vendor-product, a zero |
| * vendor will match before than a 0x456a vendor, that will match |
| * before a 0xfffff vendor. |
| * |
| * @returns 0 if ok, < 0 errno on error (-ENOENT if not found). |
| */ |
| /* FIXME: add bus type to vendor/product code */ |
| int uwb_est_register(u8 type, u8 event_high, u16 vendor, u16 product, |
| const struct uwb_est_entry *entry, size_t entries) |
| { |
| unsigned long flags; |
| unsigned itr; |
| u16 type_event_high; |
| int result = 0; |
| |
| write_lock_irqsave(&uwb_est_lock, flags); |
| if (uwb_est_used == uwb_est_size) { |
| result = uwb_est_grow(); |
| if (result < 0) |
| goto out; |
| } |
| /* Find the right spot to insert it in */ |
| type_event_high = type << 8 | event_high; |
| for (itr = 0; itr < uwb_est_used; itr++) |
| if (uwb_est[itr].type_event_high < type |
| && uwb_est[itr].vendor < vendor |
| && uwb_est[itr].product < product) |
| break; |
| |
| /* Shift others to make room for the new one? */ |
| if (itr < uwb_est_used) |
| memmove(&uwb_est[itr+1], &uwb_est[itr], uwb_est_used - itr); |
| uwb_est[itr].type_event_high = type << 8 | event_high; |
| uwb_est[itr].vendor = vendor; |
| uwb_est[itr].product = product; |
| uwb_est[itr].entry = entry; |
| uwb_est[itr].entries = entries; |
| uwb_est_used++; |
| out: |
| write_unlock_irqrestore(&uwb_est_lock, flags); |
| return result; |
| } |
| EXPORT_SYMBOL_GPL(uwb_est_register); |
| |
| |
| /** |
| * Unregister an event size table |
| * |
| * This just removes the specified entry and moves the ones after it |
| * to fill in the gap. This is needed to keep the list sorted; no |
| * reallocation is done to reduce the size of the table. |
| * |
| * We unregister by all the data we used to register instead of by |
| * pointer to the @entry array because we might have used the same |
| * table for a bunch of IDs (for example). |
| * |
| * @returns 0 if ok, < 0 errno on error (-ENOENT if not found). |
| */ |
| int uwb_est_unregister(u8 type, u8 event_high, u16 vendor, u16 product, |
| const struct uwb_est_entry *entry, size_t entries) |
| { |
| unsigned long flags; |
| unsigned itr; |
| struct uwb_est est_cmp = { |
| .type_event_high = type << 8 | event_high, |
| .vendor = vendor, |
| .product = product, |
| .entry = entry, |
| .entries = entries |
| }; |
| write_lock_irqsave(&uwb_est_lock, flags); |
| for (itr = 0; itr < uwb_est_used; itr++) |
| if (!memcmp(&uwb_est[itr], &est_cmp, sizeof(est_cmp))) |
| goto found; |
| write_unlock_irqrestore(&uwb_est_lock, flags); |
| return -ENOENT; |
| |
| found: |
| if (itr < uwb_est_used - 1) /* Not last one? move ones above */ |
| memmove(&uwb_est[itr], &uwb_est[itr+1], uwb_est_used - itr - 1); |
| uwb_est_used--; |
| write_unlock_irqrestore(&uwb_est_lock, flags); |
| return 0; |
| } |
| EXPORT_SYMBOL_GPL(uwb_est_unregister); |
| |
| |
| /** |
| * Get the size of an event from a table |
| * |
| * @rceb: pointer to the buffer with the event |
| * @rceb_size: size of the area pointed to by @rceb in bytes. |
| * @returns: > 0 Size of the event |
| * -ENOSPC An area big enough was not provided to look |
| * ahead into the event's guts and guess the size. |
| * -EINVAL Unknown event code (wEvent). |
| * |
| * This will look at the received RCEB and guess what is the total |
| * size. For variable sized events, it will look further ahead into |
| * their length field to see how much data should be read. |
| * |
| * Note this size is *not* final--the neh (Notification/Event Handle) |
| * might specificy an extra size to add. |
| */ |
| static |
| ssize_t uwb_est_get_size(struct uwb_rc *uwb_rc, struct uwb_est *est, |
| u8 event_low, const struct uwb_rceb *rceb, |
| size_t rceb_size) |
| { |
| unsigned offset; |
| ssize_t size; |
| struct device *dev = &uwb_rc->uwb_dev.dev; |
| const struct uwb_est_entry *entry; |
| |
| size = -ENOENT; |
| if (event_low >= est->entries) { /* in range? */ |
| if (printk_ratelimit()) |
| dev_err(dev, "EST %p 0x%04x/%04x/%04x[%u]: " |
| "event %u out of range\n", |
| est, est->type_event_high, est->vendor, |
| est->product, est->entries, |
| event_low); |
| goto out; |
| } |
| size = -ENOENT; |
| entry = &est->entry[event_low]; |
| if (entry->size == 0 && entry->offset == 0) { /* unknown? */ |
| if (printk_ratelimit()) |
| dev_err(dev, "EST %p 0x%04x/%04x/%04x[%u]: " |
| "event %u unknown\n", |
| est, est->type_event_high, est->vendor, |
| est->product, est->entries, event_low); |
| goto out; |
| } |
| offset = entry->offset; /* extra fries with that? */ |
| if (offset == 0) |
| size = entry->size; |
| else { |
| /* Ops, got an extra size field at 'offset'--read it */ |
| const void *ptr = rceb; |
| size_t type_size = 0; |
| offset--; |
| size = -ENOSPC; /* enough data for more? */ |
| switch (entry->type) { |
| case UWB_EST_16: type_size = sizeof(__le16); break; |
| case UWB_EST_8: type_size = sizeof(u8); break; |
| default: BUG(); |
| } |
| if (offset + type_size >= rceb_size) { |
| if (printk_ratelimit()) |
| dev_err(dev, "EST %p 0x%04x/%04x/%04x[%u]: " |
| "not enough data to read extra size\n", |
| est, est->type_event_high, est->vendor, |
| est->product, est->entries); |
| goto out; |
| } |
| size = entry->size; |
| ptr += offset; |
| switch (entry->type) { |
| case UWB_EST_16: size += le16_to_cpu(*(__le16 *)ptr); break; |
| case UWB_EST_8: size += *(u8 *)ptr; break; |
| default: BUG(); |
| } |
| } |
| out: |
| return size; |
| } |
| |
| |
| /** |
| * Guesses the size of a WA event |
| * |
| * @rceb: pointer to the buffer with the event |
| * @rceb_size: size of the area pointed to by @rceb in bytes. |
| * @returns: > 0 Size of the event |
| * -ENOSPC An area big enough was not provided to look |
| * ahead into the event's guts and guess the size. |
| * -EINVAL Unknown event code (wEvent). |
| * |
| * This will look at the received RCEB and guess what is the total |
| * size by checking all the tables registered with |
| * uwb_est_register(). For variable sized events, it will look further |
| * ahead into their length field to see how much data should be read. |
| * |
| * Note this size is *not* final--the neh (Notification/Event Handle) |
| * might specificy an extra size to add or replace. |
| */ |
| ssize_t uwb_est_find_size(struct uwb_rc *rc, const struct uwb_rceb *rceb, |
| size_t rceb_size) |
| { |
| /* FIXME: add vendor/product data */ |
| ssize_t size; |
| struct device *dev = &rc->uwb_dev.dev; |
| unsigned long flags; |
| unsigned itr; |
| u16 type_event_high, event; |
| u8 *ptr = (u8 *) rceb; |
| |
| read_lock_irqsave(&uwb_est_lock, flags); |
| d_printf(2, dev, "Size query for event 0x%02x/%04x/%02x," |
| " buffer size %ld\n", |
| (unsigned) rceb->bEventType, |
| (unsigned) le16_to_cpu(rceb->wEvent), |
| (unsigned) rceb->bEventContext, |
| (long) rceb_size); |
| size = -ENOSPC; |
| if (rceb_size < sizeof(*rceb)) |
| goto out; |
| event = le16_to_cpu(rceb->wEvent); |
| type_event_high = rceb->bEventType << 8 | (event & 0xff00) >> 8; |
| for (itr = 0; itr < uwb_est_used; itr++) { |
| d_printf(3, dev, "Checking EST 0x%04x/%04x/%04x\n", |
| uwb_est[itr].type_event_high, uwb_est[itr].vendor, |
| uwb_est[itr].product); |
| if (uwb_est[itr].type_event_high != type_event_high) |
| continue; |
| size = uwb_est_get_size(rc, &uwb_est[itr], |
| event & 0x00ff, rceb, rceb_size); |
| /* try more tables that might handle the same type */ |
| if (size != -ENOENT) |
| goto out; |
| } |
| /* FIXME: downgrade to _dbg() */ |
| if (printk_ratelimit()) |
| dev_err(dev, "event 0x%02x/%04x/%02x: no handlers available; " |
| "RCEB %02x %02x %02x %02x\n", |
| (unsigned) rceb->bEventType, |
| (unsigned) le16_to_cpu(rceb->wEvent), |
| (unsigned) rceb->bEventContext, |
| ptr[0], ptr[1], ptr[2], ptr[3]); |
| size = -ENOENT; |
| out: |
| read_unlock_irqrestore(&uwb_est_lock, flags); |
| return size; |
| } |
| EXPORT_SYMBOL_GPL(uwb_est_find_size); |