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