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