← Back to branch summary

~ubuntu-branches/ubuntu/precise/linux-lowlatency/precise

« back to all changes in this revision

Viewing changes to drivers/uwb/ie.c

  • Committer: Package Import Robot
  • Author(s): Alessio Igor Bogani
  • Date: 2011-10-26 11:13:05 UTC
  • Revision ID: package-import@ubuntu.com-20111026111305-tz023xykf0i6eosh
Tags: upstream-3.2.0
Import upstream version 3.2.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
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
 */
 
42
struct 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
}
 
61
EXPORT_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
 */
 
72
int 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
 */
 
109
static
 
110
ssize_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
 */
 
155
int 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;
 
179
error_cmd:
 
180
        return result;
 
181
}
 
182
 
 
183
/* Cleanup the whole IE management subsystem */
 
184
void 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
 */
 
200
int 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 */
 
223
void 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
 
 
231
static 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
 */
 
285
int 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
}
 
316
EXPORT_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
 */
 
331
static
 
332
void 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
 */
 
365
int 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
}
 
380
EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);