Root/drivers/uwb/ie.c

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

Archive Download this file

Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master

Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9



interactive