GNU Linux-libre 4.14.259-gnu1
[releases.git] / 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);