blob: 30acec740425e48ea48c1e11d32028bbd837d710 [file] [log] [blame]
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +01001/*
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
Tejun Heo5a0e3ad2010-03-24 17:04:11 +090027#include <linux/slab.h>
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +010028#include "uwb-internal.h"
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +010029
30/**
31 * uwb_ie_next - get the next IE in a buffer
32 * @ptr: start of the buffer containing the IE data
33 * @len: length of the buffer
34 *
35 * Both @ptr and @len are updated so subsequent calls to uwb_ie_next()
36 * will get the next IE.
37 *
38 * NULL is returned (and @ptr and @len will not be updated) if there
39 * are no more IEs in the buffer or the buffer is too short.
40 */
41struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len)
42{
43 struct uwb_ie_hdr *hdr;
44 size_t ie_len;
45
46 if (*len < sizeof(struct uwb_ie_hdr))
47 return NULL;
48
49 hdr = *ptr;
50 ie_len = sizeof(struct uwb_ie_hdr) + hdr->length;
51
52 if (*len < ie_len)
53 return NULL;
54
55 *ptr += ie_len;
56 *len -= ie_len;
57
58 return hdr;
59}
60EXPORT_SYMBOL_GPL(uwb_ie_next);
61
62/**
David Vrabel1cde7f62008-10-27 16:48:09 +000063 * uwb_ie_dump_hex - print IEs to a character buffer
64 * @ies: the IEs to print.
65 * @len: length of all the IEs.
66 * @buf: the destination buffer.
67 * @size: size of @buf.
68 *
69 * Returns the number of characters written.
70 */
71int uwb_ie_dump_hex(const struct uwb_ie_hdr *ies, size_t len,
72 char *buf, size_t size)
73{
74 void *ptr;
75 const struct uwb_ie_hdr *ie;
76 int r = 0;
77 u8 *d;
78
79 ptr = (void *)ies;
80 for (;;) {
81 ie = uwb_ie_next(&ptr, &len);
82 if (!ie)
83 break;
84
85 r += scnprintf(buf + r, size - r, "%02x %02x",
86 (unsigned)ie->element_id,
87 (unsigned)ie->length);
88 d = (uint8_t *)ie + sizeof(struct uwb_ie_hdr);
89 while (d != ptr && r < size)
90 r += scnprintf(buf + r, size - r, " %02x", (unsigned)*d++);
91 if (r < size)
92 buf[r++] = '\n';
93 };
94
95 return r;
96}
97
98/**
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +010099 * Get the IEs that a radio controller is sending in its beacon
100 *
101 * @uwb_rc: UWB Radio Controller
102 * @returns: Size read from the system
103 *
104 * We don't need to lock the uwb_rc's mutex because we don't modify
105 * anything. Once done with the iedata buffer, call
106 * uwb_rc_ie_release(iedata). Don't call kfree on it.
107 */
David Vrabel1cde7f62008-10-27 16:48:09 +0000108static
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100109ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie)
110{
111 ssize_t result;
112 struct device *dev = &uwb_rc->uwb_dev.dev;
113 struct uwb_rccb *cmd = NULL;
114 struct uwb_rceb *reply = NULL;
115 struct uwb_rc_evt_get_ie *get_ie;
116
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100117 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
118 if (cmd == NULL)
David Vrabel1cde7f62008-10-27 16:48:09 +0000119 return -ENOMEM;
120
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100121 cmd->bCommandType = UWB_RC_CET_GENERAL;
122 cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE);
123 result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd),
124 UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE,
125 &reply);
David Vrabel1cde7f62008-10-27 16:48:09 +0000126 kfree(cmd);
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100127 if (result < 0)
David Vrabel1cde7f62008-10-27 16:48:09 +0000128 return result;
129
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100130 get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb);
131 if (result < sizeof(*get_ie)) {
132 dev_err(dev, "not enough data returned for decoding GET IE "
133 "(%zu bytes received vs %zu needed)\n",
134 result, sizeof(*get_ie));
David Vrabel1cde7f62008-10-27 16:48:09 +0000135 return -EINVAL;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100136 } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) {
137 dev_err(dev, "not enough data returned for decoding GET IE "
138 "payload (%zu bytes received vs %zu needed)\n", result,
139 sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength));
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100140 return -EINVAL;
141 }
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100142
David Vrabel1cde7f62008-10-27 16:48:09 +0000143 *pget_ie = get_ie;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100144 return result;
145}
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100146
147
148/**
149 * Replace all IEs currently being transmitted by a device
150 *
151 * @cmd: pointer to the SET-IE command with the IEs to set
152 * @size: size of @buf
153 */
154int uwb_rc_set_ie(struct uwb_rc *rc, struct uwb_rc_cmd_set_ie *cmd)
155{
156 int result;
157 struct device *dev = &rc->uwb_dev.dev;
158 struct uwb_rc_evt_set_ie reply;
159
160 reply.rceb.bEventType = UWB_RC_CET_GENERAL;
161 reply.rceb.wEvent = UWB_RC_CMD_SET_IE;
162 result = uwb_rc_cmd(rc, "SET-IE", &cmd->rccb,
163 sizeof(*cmd) + le16_to_cpu(cmd->wIELength),
164 &reply.rceb, sizeof(reply));
165 if (result < 0)
166 goto error_cmd;
167 else if (result != sizeof(reply)) {
168 dev_err(dev, "SET-IE: not enough data to decode reply "
169 "(%d bytes received vs %zu needed)\n",
170 result, sizeof(reply));
171 result = -EIO;
172 } else if (reply.bResultCode != UWB_RC_RES_SUCCESS) {
173 dev_err(dev, "SET-IE: command execution failed: %s (%d)\n",
174 uwb_rc_strerror(reply.bResultCode), reply.bResultCode);
175 result = -EIO;
176 } else
177 result = 0;
178error_cmd:
179 return result;
180}
181
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100182/* Cleanup the whole IE management subsystem */
183void uwb_rc_ie_init(struct uwb_rc *uwb_rc)
184{
185 mutex_init(&uwb_rc->ies_mutex);
186}
187
188
189/**
David Vrabel1cde7f62008-10-27 16:48:09 +0000190 * uwb_rc_ie_setup - setup a radio controller's IE manager
191 * @uwb_rc: the radio controller.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100192 *
David Vrabel1cde7f62008-10-27 16:48:09 +0000193 * The current set of IEs are obtained from the hardware with a GET-IE
194 * command (since the radio controller is not yet beaconing this will
195 * be just the hardware's MAC and PHY Capability IEs).
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100196 *
David Vrabel1cde7f62008-10-27 16:48:09 +0000197 * Returns 0 on success; -ve on an error.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100198 */
David Vrabel1cde7f62008-10-27 16:48:09 +0000199int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100200{
David Vrabel1cde7f62008-10-27 16:48:09 +0000201 struct uwb_rc_evt_get_ie *ie_info = NULL;
202 int capacity;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100203
David Vrabel1cde7f62008-10-27 16:48:09 +0000204 capacity = uwb_rc_get_ie(uwb_rc, &ie_info);
205 if (capacity < 0)
206 return capacity;
207
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100208 mutex_lock(&uwb_rc->ies_mutex);
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100209
David Vrabel1cde7f62008-10-27 16:48:09 +0000210 uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100211 uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL;
212 uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE);
213 uwb_rc->ies_capacity = capacity;
David Vrabel1cde7f62008-10-27 16:48:09 +0000214
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100215 mutex_unlock(&uwb_rc->ies_mutex);
David Vrabel1cde7f62008-10-27 16:48:09 +0000216
217 return 0;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100218}
219
220
221/* Cleanup the whole IE management subsystem */
222void uwb_rc_ie_release(struct uwb_rc *uwb_rc)
223{
224 kfree(uwb_rc->ies);
225 uwb_rc->ies = NULL;
226 uwb_rc->ies_capacity = 0;
227}
228
229
David Vrabel1cde7f62008-10-27 16:48:09 +0000230static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100231{
David Vrabel1cde7f62008-10-27 16:48:09 +0000232 struct uwb_rc_cmd_set_ie *new_ies;
233 void *ptr, *prev_ie;
234 struct uwb_ie_hdr *ie;
235 size_t length, new_ie_len, new_capacity, size, prev_size;
236
237 length = le16_to_cpu(rc->ies->wIELength);
238 new_ie_len = sizeof(struct uwb_ie_hdr) + new_ie->length;
239 new_capacity = sizeof(struct uwb_rc_cmd_set_ie) + length + new_ie_len;
240
241 if (new_capacity > rc->ies_capacity) {
242 new_ies = krealloc(rc->ies, new_capacity, GFP_KERNEL);
243 if (!new_ies)
244 return -ENOMEM;
245 rc->ies = new_ies;
246 }
247
248 ptr = rc->ies->IEData;
249 size = length;
250 for (;;) {
251 prev_ie = ptr;
252 prev_size = size;
253 ie = uwb_ie_next(&ptr, &size);
254 if (!ie || ie->element_id > new_ie->element_id)
255 break;
256 }
257
258 memmove(prev_ie + new_ie_len, prev_ie, prev_size);
259 memcpy(prev_ie, new_ie, new_ie_len);
260 rc->ies->wIELength = cpu_to_le16(length + new_ie_len);
261
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100262 return 0;
263}
264
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100265/**
David Vrabel1cde7f62008-10-27 16:48:09 +0000266 * uwb_rc_ie_add - add new IEs to the radio controller's beacon
267 * @uwb_rc: the radio controller.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100268 * @ies: the buffer containing the new IE or IEs to be added to
David Vrabel1cde7f62008-10-27 16:48:09 +0000269 * the device's beacon.
270 * @size: length of all the IEs.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100271 *
272 * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB
273 * after the device sent the first beacon that includes the IEs specified
274 * in the SET IE command. We thus cannot send this command if the device is
275 * not beaconing. Instead, a SET IE command will be sent later right after
276 * we start beaconing.
277 *
278 * Setting an IE on the device will overwrite all current IEs in device. So
David Vrabel1cde7f62008-10-27 16:48:09 +0000279 * we take the current IEs being transmitted by the device, insert the
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100280 * new one, and call SET IE with all the IEs needed.
281 *
David Vrabel1cde7f62008-10-27 16:48:09 +0000282 * Returns 0 on success; or -ENOMEM.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100283 */
284int uwb_rc_ie_add(struct uwb_rc *uwb_rc,
285 const struct uwb_ie_hdr *ies, size_t size)
286{
287 int result = 0;
David Vrabel1cde7f62008-10-27 16:48:09 +0000288 void *ptr;
289 const struct uwb_ie_hdr *ie;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100290
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100291 mutex_lock(&uwb_rc->ies_mutex);
David Vrabel1cde7f62008-10-27 16:48:09 +0000292
293 ptr = (void *)ies;
294 for (;;) {
295 ie = uwb_ie_next(&ptr, &size);
296 if (!ie)
297 break;
298
299 result = uwb_rc_ie_add_one(uwb_rc, ie);
300 if (result < 0)
301 break;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100302 }
David Vrabel1cde7f62008-10-27 16:48:09 +0000303 if (result >= 0) {
304 if (size == 0) {
305 if (uwb_rc->beaconing != -1)
306 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100307 } else
David Vrabel1cde7f62008-10-27 16:48:09 +0000308 result = -EINVAL;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100309 }
David Vrabel1cde7f62008-10-27 16:48:09 +0000310
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100311 mutex_unlock(&uwb_rc->ies_mutex);
David Vrabel1cde7f62008-10-27 16:48:09 +0000312
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100313 return result;
314}
315EXPORT_SYMBOL_GPL(uwb_rc_ie_add);
316
317
318/*
319 * Remove an IE from internal cache
320 *
321 * We are dealing with our internal IE cache so no need to verify that the
322 * IEs are valid (it has been done already).
323 *
324 * Should be called with ies_mutex held
325 *
326 * We do not break out once an IE is found in the cache. It is currently
327 * possible to have more than one IE with the same ID included in the
328 * beacon. We don't reallocate, we just mark the size smaller.
329 */
330static
David Vrabel1cde7f62008-10-27 16:48:09 +0000331void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100332{
David Vrabel1cde7f62008-10-27 16:48:09 +0000333 struct uwb_ie_hdr *ie;
334 size_t len = le16_to_cpu(uwb_rc->ies->wIELength);
335 void *ptr;
336 size_t size;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100337
David Vrabel1cde7f62008-10-27 16:48:09 +0000338 ptr = uwb_rc->ies->IEData;
339 size = len;
340 for (;;) {
341 ie = uwb_ie_next(&ptr, &size);
342 if (!ie)
343 break;
344 if (ie->element_id == to_remove) {
345 len -= sizeof(struct uwb_ie_hdr) + ie->length;
346 memmove(ie, ptr, size);
347 ptr = ie;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100348 }
349 }
David Vrabel1cde7f62008-10-27 16:48:09 +0000350 uwb_rc->ies->wIELength = cpu_to_le16(len);
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100351}
352
353
354/**
David Vrabel1cde7f62008-10-27 16:48:09 +0000355 * uwb_rc_ie_rm - remove an IE from the radio controller's beacon
356 * @uwb_rc: the radio controller.
357 * @element_id: the element ID of the IE to remove.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100358 *
David Vrabel1cde7f62008-10-27 16:48:09 +0000359 * Only IEs previously added with uwb_rc_ie_add() may be removed.
360 *
361 * Returns 0 on success; or -ve the SET-IE command to the radio
362 * controller failed.
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100363 */
364int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
365{
David Vrabel1cde7f62008-10-27 16:48:09 +0000366 int result = 0;
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100367
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100368 mutex_lock(&uwb_rc->ies_mutex);
David Vrabel1cde7f62008-10-27 16:48:09 +0000369
370 uwb_rc_ie_cache_rm(uwb_rc, element_id);
371
372 if (uwb_rc->beaconing != -1)
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100373 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
David Vrabel1cde7f62008-10-27 16:48:09 +0000374
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100375 mutex_unlock(&uwb_rc->ies_mutex);
David Vrabel1cde7f62008-10-27 16:48:09 +0000376
Inaky Perez-Gonzalez22d203e2008-09-17 16:34:08 +0100377 return result;
378}
379EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);