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 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 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 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