1  /*
2   * Radiotap parser
3   *
4   * Copyright 2007		Andy Green <andy@warmcat.com>
5   * Copyright 2009		Johannes Berg <johannes@sipsolutions.net>
6   *
7   * This program is free software; you can redistribute it and/or modify
8   * it under the terms of the GNU General Public License version 2 as
9   * published by the Free Software Foundation.
10   *
11   * Alternatively, this software may be distributed under the terms of ISC
12   * license, see COPYING for more details.
13   */
14  #include "platform.h"
15  #include "radiotap_iter.h"
16  
17  /* function prototypes and related defs are in radiotap_iter.h */
18  
19  static const struct radiotap_align_size rtap_namespace_sizes[] = {
20  	[IEEE80211_RADIOTAP_TSFT] = { .align = 8, .size = 8, },
21  	[IEEE80211_RADIOTAP_FLAGS] = { .align = 1, .size = 1, },
22  	[IEEE80211_RADIOTAP_RATE] = { .align = 1, .size = 1, },
23  	[IEEE80211_RADIOTAP_CHANNEL] = { .align = 2, .size = 4, },
24  	[IEEE80211_RADIOTAP_FHSS] = { .align = 2, .size = 2, },
25  	[IEEE80211_RADIOTAP_DBM_ANTSIGNAL] = { .align = 1, .size = 1, },
26  	[IEEE80211_RADIOTAP_DBM_ANTNOISE] = { .align = 1, .size = 1, },
27  	[IEEE80211_RADIOTAP_LOCK_QUALITY] = { .align = 2, .size = 2, },
28  	[IEEE80211_RADIOTAP_TX_ATTENUATION] = { .align = 2, .size = 2, },
29  	[IEEE80211_RADIOTAP_DB_TX_ATTENUATION] = { .align = 2, .size = 2, },
30  	[IEEE80211_RADIOTAP_DBM_TX_POWER] = { .align = 1, .size = 1, },
31  	[IEEE80211_RADIOTAP_ANTENNA] = { .align = 1, .size = 1, },
32  	[IEEE80211_RADIOTAP_DB_ANTSIGNAL] = { .align = 1, .size = 1, },
33  	[IEEE80211_RADIOTAP_DB_ANTNOISE] = { .align = 1, .size = 1, },
34  	[IEEE80211_RADIOTAP_RX_FLAGS] = { .align = 2, .size = 2, },
35  	[IEEE80211_RADIOTAP_TX_FLAGS] = { .align = 2, .size = 2, },
36  	[IEEE80211_RADIOTAP_RTS_RETRIES] = { .align = 1, .size = 1, },
37  	[IEEE80211_RADIOTAP_DATA_RETRIES] = { .align = 1, .size = 1, },
38  	[IEEE80211_RADIOTAP_MCS] = { .align = 1, .size = 3, },
39  	[IEEE80211_RADIOTAP_AMPDU_STATUS] = { .align = 4, .size = 8, },
40  	[IEEE80211_RADIOTAP_VHT] = { .align = 2, .size = 12, },
41  	[IEEE80211_RADIOTAP_TIMESTAMP] = { .align = 8, .size = 12, },
42  	/*
43  	 * add more here as they are defined in radiotap.h
44  	 */
45  };
46  
47  static const struct ieee80211_radiotap_namespace radiotap_ns = {
48  	.n_bits = sizeof(rtap_namespace_sizes) / sizeof(rtap_namespace_sizes[0]),
49  	.align_size = rtap_namespace_sizes,
50  };
51  
52  /**
53   * ieee80211_radiotap_iterator_init - radiotap parser iterator initialization
54   * @iterator: radiotap_iterator to initialize
55   * @radiotap_header: radiotap header to parse
56   * @max_length: total length we can parse into (eg, whole packet length)
57   *
58   * Returns: 0 or a negative error code if there is a problem.
59   *
60   * This function initializes an opaque iterator struct which can then
61   * be passed to ieee80211_radiotap_iterator_next() to visit every radiotap
62   * argument which is present in the header.  It knows about extended
63   * present headers and handles them.
64   *
65   * How to use:
66   * call __ieee80211_radiotap_iterator_init() to init a semi-opaque iterator
67   * struct ieee80211_radiotap_iterator (no need to init the struct beforehand)
68   * checking for a good 0 return code.  Then loop calling
69   * __ieee80211_radiotap_iterator_next()... it returns either 0,
70   * -ENOENT if there are no more args to parse, or -EINVAL if there is a problem.
71   * The iterator's @this_arg member points to the start of the argument
72   * associated with the current argument index that is present, which can be
73   * found in the iterator's @this_arg_index member.  This arg index corresponds
74   * to the IEEE80211_RADIOTAP_... defines.
75   *
76   * Radiotap header length:
77   * You can find the CPU-endian total radiotap header length in
78   * iterator->max_length after executing ieee80211_radiotap_iterator_init()
79   * successfully.
80   *
81   * Alignment Gotcha:
82   * You must take care when dereferencing iterator.this_arg
83   * for multibyte types... the pointer is not aligned.  Use
84   * get_unaligned((type *)iterator.this_arg) to dereference
85   * iterator.this_arg for type "type" safely on all arches.
86   *
87   * Example code: parse.c
88   */
89  
ieee80211_radiotap_iterator_init(struct ieee80211_radiotap_iterator * iterator,struct ieee80211_radiotap_header * radiotap_header,int max_length,const struct ieee80211_radiotap_vendor_namespaces * vns)90  int ieee80211_radiotap_iterator_init(
91  	struct ieee80211_radiotap_iterator *iterator,
92  	struct ieee80211_radiotap_header *radiotap_header,
93  	int max_length, const struct ieee80211_radiotap_vendor_namespaces *vns)
94  {
95  	/* must at least have the radiotap header */
96  	if (max_length < (int)sizeof(struct ieee80211_radiotap_header))
97  		return -EINVAL;
98  
99  	/* Linux only supports version 0 radiotap format */
100  	if (radiotap_header->it_version)
101  		return -EINVAL;
102  
103  	/* sanity check for allowed length and radiotap length field */
104  	if (max_length < get_unaligned_le16(&radiotap_header->it_len))
105  		return -EINVAL;
106  
107  	iterator->_rtheader = radiotap_header;
108  	iterator->_max_length = get_unaligned_le16(&radiotap_header->it_len);
109  	iterator->_arg_index = 0;
110  	iterator->_bitmap_shifter = get_unaligned_le32(&radiotap_header->it_present);
111  	iterator->_arg = (uint8_t *)radiotap_header + sizeof(*radiotap_header);
112  	iterator->_next_ns_data = NULL;
113  	iterator->_reset_on_ext = 0;
114  	iterator->_next_bitmap = (le32 *) (((u8 *) radiotap_header) + offsetof(struct ieee80211_radiotap_header, it_present));
115  	iterator->_next_bitmap++;
116  	iterator->_vns = vns;
117  	iterator->current_namespace = &radiotap_ns;
118  	iterator->is_radiotap_ns = 1;
119  #ifdef RADIOTAP_SUPPORT_OVERRIDES
120  	iterator->n_overrides = 0;
121  	iterator->overrides = NULL;
122  #endif
123  
124  	/* find payload start allowing for extended bitmap(s) */
125  
126  	if (iterator->_bitmap_shifter & BIT(IEEE80211_RADIOTAP_EXT)) {
127  		if ((unsigned long)iterator->_arg -
128  		    (unsigned long)iterator->_rtheader + sizeof(uint32_t) >
129  		    (unsigned long)iterator->_max_length)
130  			return -EINVAL;
131  		while (get_unaligned_le32(iterator->_arg) &
132  		       BIT(IEEE80211_RADIOTAP_EXT)) {
133  			iterator->_arg += sizeof(uint32_t);
134  
135  			/*
136  			 * check for insanity where the present bitmaps
137  			 * keep claiming to extend up to or even beyond the
138  			 * stated radiotap header length
139  			 */
140  
141  			if ((unsigned long)iterator->_arg -
142  			    (unsigned long)iterator->_rtheader +
143  			    sizeof(uint32_t) >
144  			    (unsigned long)iterator->_max_length)
145  				return -EINVAL;
146  		}
147  
148  		iterator->_arg += sizeof(uint32_t);
149  
150  		/*
151  		 * no need to check again for blowing past stated radiotap
152  		 * header length, because ieee80211_radiotap_iterator_next
153  		 * checks it before it is dereferenced
154  		 */
155  	}
156  
157  	iterator->this_arg = iterator->_arg;
158  	iterator->this_arg_index = 0;
159  	iterator->this_arg_size = 0;
160  
161  	/* we are all initialized happily */
162  
163  	return 0;
164  }
165  
find_ns(struct ieee80211_radiotap_iterator * iterator,uint32_t oui,uint8_t subns)166  static void find_ns(struct ieee80211_radiotap_iterator *iterator,
167  		    uint32_t oui, uint8_t subns)
168  {
169  	int i;
170  
171  	iterator->current_namespace = NULL;
172  
173  	if (!iterator->_vns)
174  		return;
175  
176  	for (i = 0; i < iterator->_vns->n_ns; i++) {
177  		if (iterator->_vns->ns[i].oui != oui)
178  			continue;
179  		if (iterator->_vns->ns[i].subns != subns)
180  			continue;
181  
182  		iterator->current_namespace = &iterator->_vns->ns[i];
183  		break;
184  	}
185  }
186  
187  #ifdef RADIOTAP_SUPPORT_OVERRIDES
find_override(struct ieee80211_radiotap_iterator * iterator,int * align,int * size)188  static int find_override(struct ieee80211_radiotap_iterator *iterator,
189  			 int *align, int *size)
190  {
191  	int i;
192  
193  	if (!iterator->overrides)
194  		return 0;
195  
196  	for (i = 0; i < iterator->n_overrides; i++) {
197  		if (iterator->_arg_index == iterator->overrides[i].field) {
198  			*align = iterator->overrides[i].align;
199  			*size = iterator->overrides[i].size;
200  			if (!*align) /* erroneous override */
201  				return 0;
202  			return 1;
203  		}
204  	}
205  
206  	return 0;
207  }
208  #endif
209  
210  
211  /**
212   * ieee80211_radiotap_iterator_next - return next radiotap parser iterator arg
213   * @iterator: radiotap_iterator to move to next arg (if any)
214   *
215   * Returns: 0 if there is an argument to handle,
216   * -ENOENT if there are no more args or -EINVAL
217   * if there is something else wrong.
218   *
219   * This function provides the next radiotap arg index (IEEE80211_RADIOTAP_*)
220   * in @this_arg_index and sets @this_arg to point to the
221   * payload for the field.  It takes care of alignment handling and extended
222   * present fields.  @this_arg can be changed by the caller (eg,
223   * incremented to move inside a compound argument like
224   * IEEE80211_RADIOTAP_CHANNEL).  The args pointed to are in
225   * little-endian format whatever the endianness of your CPU.
226   *
227   * Alignment Gotcha:
228   * You must take care when dereferencing iterator.this_arg
229   * for multibyte types... the pointer is not aligned.  Use
230   * get_unaligned((type *)iterator.this_arg) to dereference
231   * iterator.this_arg for type "type" safely on all arches.
232   */
233  
ieee80211_radiotap_iterator_next(struct ieee80211_radiotap_iterator * iterator)234  int ieee80211_radiotap_iterator_next(
235  	struct ieee80211_radiotap_iterator *iterator)
236  {
237  	while (1) {
238  		int hit = 0;
239  		int pad, align, size, subns;
240  		uint32_t oui;
241  
242  		/* if no more EXT bits, that's it */
243  		if ((iterator->_arg_index % 32) == IEEE80211_RADIOTAP_EXT &&
244  		    !(iterator->_bitmap_shifter & 1))
245  			return -ENOENT;
246  
247  		if (!(iterator->_bitmap_shifter & 1))
248  			goto next_entry; /* arg not present */
249  
250  		/* get alignment/size of data */
251  		switch (iterator->_arg_index % 32) {
252  		case IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE:
253  		case IEEE80211_RADIOTAP_EXT:
254  			align = 1;
255  			size = 0;
256  			break;
257  		case IEEE80211_RADIOTAP_VENDOR_NAMESPACE:
258  			align = 2;
259  			size = 6;
260  			break;
261  		default:
262  #ifdef RADIOTAP_SUPPORT_OVERRIDES
263  			if (find_override(iterator, &align, &size)) {
264  				/* all set */
265  			} else
266  #endif
267  			if (!iterator->current_namespace ||
268  			    iterator->_arg_index >= iterator->current_namespace->n_bits) {
269  				if (iterator->current_namespace == &radiotap_ns)
270  					return -ENOENT;
271  				align = 0;
272  			} else {
273  				align = iterator->current_namespace->align_size[iterator->_arg_index].align;
274  				size = iterator->current_namespace->align_size[iterator->_arg_index].size;
275  			}
276  			if (!align) {
277  				/* skip all subsequent data */
278  				iterator->_arg = iterator->_next_ns_data;
279  				/* give up on this namespace */
280  				iterator->current_namespace = NULL;
281  				goto next_entry;
282  			}
283  			break;
284  		}
285  
286  		/*
287  		 * arg is present, account for alignment padding
288  		 *
289  		 * Note that these alignments are relative to the start
290  		 * of the radiotap header.  There is no guarantee
291  		 * that the radiotap header itself is aligned on any
292  		 * kind of boundary.
293  		 *
294  		 * The above is why get_unaligned() is used to dereference
295  		 * multibyte elements from the radiotap area.
296  		 */
297  
298  		pad = ((unsigned long)iterator->_arg -
299  		       (unsigned long)iterator->_rtheader) & (align - 1);
300  
301  		if (pad)
302  			iterator->_arg += align - pad;
303  
304  		if (iterator->_arg_index % 32 == IEEE80211_RADIOTAP_VENDOR_NAMESPACE) {
305  			int vnslen;
306  
307  			if ((unsigned long)iterator->_arg + size -
308  			    (unsigned long)iterator->_rtheader >
309  			    (unsigned long)iterator->_max_length)
310  				return -EINVAL;
311  
312  			oui = (*iterator->_arg << 16) |
313  				(*(iterator->_arg + 1) << 8) |
314  				*(iterator->_arg + 2);
315  			subns = *(iterator->_arg + 3);
316  
317  			find_ns(iterator, oui, subns);
318  
319  			vnslen = get_unaligned_le16(iterator->_arg + 4);
320  			iterator->_next_ns_data = iterator->_arg + size + vnslen;
321  			if (!iterator->current_namespace)
322  				size += vnslen;
323  		}
324  
325  		/*
326  		 * this is what we will return to user, but we need to
327  		 * move on first so next call has something fresh to test
328  		 */
329  		iterator->this_arg_index = iterator->_arg_index;
330  		iterator->this_arg = iterator->_arg;
331  		iterator->this_arg_size = size;
332  
333  		/* internally move on the size of this arg */
334  		iterator->_arg += size;
335  
336  		/*
337  		 * check for insanity where we are given a bitmap that
338  		 * claims to have more arg content than the length of the
339  		 * radiotap section.  We will normally end up equalling this
340  		 * max_length on the last arg, never exceeding it.
341  		 */
342  
343  		if ((unsigned long)iterator->_arg -
344  		    (unsigned long)iterator->_rtheader >
345  		    (unsigned long)iterator->_max_length)
346  			return -EINVAL;
347  
348  		/* these special ones are valid in each bitmap word */
349  		switch (iterator->_arg_index % 32) {
350  		case IEEE80211_RADIOTAP_VENDOR_NAMESPACE:
351  			iterator->_reset_on_ext = 1;
352  
353  			iterator->is_radiotap_ns = 0;
354  			/*
355  			 * If parser didn't register this vendor
356  			 * namespace with us, allow it to show it
357  			 * as 'raw. Do do that, set argument index
358  			 * to vendor namespace.
359  			 */
360  			iterator->this_arg_index =
361  				IEEE80211_RADIOTAP_VENDOR_NAMESPACE;
362  			if (!iterator->current_namespace)
363  				hit = 1;
364  			goto next_entry;
365  		case IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE:
366  			iterator->_reset_on_ext = 1;
367  			iterator->current_namespace = &radiotap_ns;
368  			iterator->is_radiotap_ns = 1;
369  			goto next_entry;
370  		case IEEE80211_RADIOTAP_EXT:
371  			/*
372  			 * bit 31 was set, there is more
373  			 * -- move to next u32 bitmap
374  			 */
375  			iterator->_bitmap_shifter =
376  				get_unaligned_le32(iterator->_next_bitmap);
377  			iterator->_next_bitmap++;
378  			if (iterator->_reset_on_ext)
379  				iterator->_arg_index = 0;
380  			else
381  				iterator->_arg_index++;
382  			iterator->_reset_on_ext = 0;
383  			break;
384  		default:
385  			/* we've got a hit! */
386  			hit = 1;
387   next_entry:
388  			iterator->_bitmap_shifter >>= 1;
389  			iterator->_arg_index++;
390  		}
391  
392  		/* if we found a valid arg earlier, return it now */
393  		if (hit)
394  			return 0;
395  	}
396  }
397