drm/i915: sync hdmi detection by hdmi identifier with 2D
[linux-2.6] / 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 "uwb-internal.h"
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
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
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  */
107 static
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
116         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
117         if (cmd == NULL)
118                 return -ENOMEM;
119
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);
125         kfree(cmd);
126         if (result < 0)
127                 return result;
128
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));
134                 return -EINVAL;
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));
139                 return -EINVAL;
140         }
141
142         *pget_ie = get_ie;
143         return result;
144 }
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
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 /**
189  * uwb_rc_ie_setup - setup a radio controller's IE manager
190  * @uwb_rc: the radio controller.
191  *
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).
195  *
196  * Returns 0 on success; -ve on an error.
197  */
198 int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
199 {
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;
206
207         mutex_lock(&uwb_rc->ies_mutex);
208
209         uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
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;
213
214         mutex_unlock(&uwb_rc->ies_mutex);
215
216         return 0;
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
229 static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
230 {
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
261         return 0;
262 }
263
264 /**
265  * uwb_rc_ie_add - add new IEs to the radio controller's beacon
266  * @uwb_rc: the radio controller.
267  * @ies: the buffer containing the new IE or IEs to be added to
268  *       the device's beacon.
269  * @size: length of all the IEs.
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
278  * we take the current IEs being transmitted by the device, insert the
279  * new one, and call SET IE with all the IEs needed.
280  *
281  * Returns 0 on success; or -ENOMEM.
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;
287         void *ptr;
288         const struct uwb_ie_hdr *ie;
289
290         mutex_lock(&uwb_rc->ies_mutex);
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;
301         }
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);
306                 } else
307                         result = -EINVAL;
308         }
309
310         mutex_unlock(&uwb_rc->ies_mutex);
311
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
330 void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
331 {
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;
347                 }
348         }
349         uwb_rc->ies->wIELength = cpu_to_le16(len);
350 }
351
352
353 /**
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.
357  *
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.
362  */
363 int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
364 {
365         int result = 0;
366
367         mutex_lock(&uwb_rc->ies_mutex);
368
369         uwb_rc_ie_cache_rm(uwb_rc, element_id);
370
371         if (uwb_rc->beaconing != -1)
372                 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
373
374         mutex_unlock(&uwb_rc->ies_mutex);
375
376         return result;
377 }
378 EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);