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