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 "uwb-internal.h"
 29
 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/**
 63 * 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/**
 99 * 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 */
108static
109ssize_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
117	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
118	if (cmd == NULL)
119		return -ENOMEM;
120
121	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);
126	kfree(cmd);
127	if (result < 0)
128		return result;
129
130	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));
135		return -EINVAL;
136	} 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));
140		return -EINVAL;
141	}
142
143	*pget_ie = get_ie;
144	return result;
145}
146
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
182/* 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/**
190 * uwb_rc_ie_setup - setup a radio controller's IE manager
191 * @uwb_rc: the radio controller.
192 *
193 * 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).
196 *
197 * Returns 0 on success; -ve on an error.
198 */
199int uwb_rc_ie_setup(struct uwb_rc *uwb_rc)
200{
201	struct uwb_rc_evt_get_ie *ie_info = NULL;
202	int capacity;
203
204	capacity = uwb_rc_get_ie(uwb_rc, &ie_info);
205	if (capacity < 0)
206		return capacity;
207
208	mutex_lock(&uwb_rc->ies_mutex);
209
210	uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info;
211	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;
214
215	mutex_unlock(&uwb_rc->ies_mutex);
216
217	return 0;
218}
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
230static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie)
231{
232	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
262	return 0;
263}
264
265/**
266 * uwb_rc_ie_add - add new IEs to the radio controller's beacon
267 * @uwb_rc: the radio controller.
268 * @ies: the buffer containing the new IE or IEs to be added to
269 *       the device's beacon.
270 * @size: length of all the IEs.
271 *
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
279 * we take the current IEs being transmitted by the device, insert the
280 * new one, and call SET IE with all the IEs needed.
281 *
282 * Returns 0 on success; or -ENOMEM.
283 */
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;
288	void *ptr;
289	const struct uwb_ie_hdr *ie;
290
291	mutex_lock(&uwb_rc->ies_mutex);
292
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;
302	}
303	if (result >= 0) {
304		if (size == 0) {
305			if (uwb_rc->beaconing != -1)
306				result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
307		} else
308			result = -EINVAL;
309	}
310
311	mutex_unlock(&uwb_rc->ies_mutex);
312
313	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
331void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove)
332{
333	struct uwb_ie_hdr *ie;
334	size_t len = le16_to_cpu(uwb_rc->ies->wIELength);
335	void *ptr;
336	size_t size;
337
338	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;
348		}
349	}
350	uwb_rc->ies->wIELength = cpu_to_le16(len);
351}
352
353
354/**
355 * 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.
358 *
359 * 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.
363 */
364int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id)
365{
366	int result = 0;
367
368	mutex_lock(&uwb_rc->ies_mutex);
369
370	uwb_rc_ie_cache_rm(uwb_rc, element_id);
371
372	if (uwb_rc->beaconing != -1)
373		result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies);
374
375	mutex_unlock(&uwb_rc->ies_mutex);
376
377	return result;
378}
379EXPORT_SYMBOL_GPL(uwb_rc_ie_rm);