nexmon – Blame information for rev 1

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