1179f831bSAndy Green /* 2179f831bSAndy Green * Radiotap parser 3179f831bSAndy Green * 4179f831bSAndy Green * Copyright 2007 Andy Green <andy@warmcat.com> 533e5a2f7SJohannes Berg * Copyright 2009 Johannes Berg <johannes@sipsolutions.net> 633e5a2f7SJohannes Berg * 733e5a2f7SJohannes Berg * This program is free software; you can redistribute it and/or modify 833e5a2f7SJohannes Berg * it under the terms of the GNU General Public License version 2 as 933e5a2f7SJohannes Berg * published by the Free Software Foundation. 1033e5a2f7SJohannes Berg * 1133e5a2f7SJohannes Berg * Alternatively, this software may be distributed under the terms of BSD 1233e5a2f7SJohannes Berg * license. 1333e5a2f7SJohannes Berg * 1433e5a2f7SJohannes Berg * See COPYING for more details. 15179f831bSAndy Green */ 16179f831bSAndy Green 1794262316SNikitas Angelinas #include <linux/kernel.h> 18179f831bSAndy Green #include <net/cfg80211.h> 19179f831bSAndy Green #include <net/ieee80211_radiotap.h> 20179f831bSAndy Green #include <asm/unaligned.h> 21179f831bSAndy Green 22179f831bSAndy Green /* function prototypes and related defs are in include/net/cfg80211.h */ 23179f831bSAndy Green 2433e5a2f7SJohannes Berg static const struct radiotap_align_size rtap_namespace_sizes[] = { 2533e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_TSFT] = { .align = 8, .size = 8, }, 2633e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_FLAGS] = { .align = 1, .size = 1, }, 2733e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_RATE] = { .align = 1, .size = 1, }, 2833e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_CHANNEL] = { .align = 2, .size = 4, }, 2933e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_FHSS] = { .align = 2, .size = 2, }, 3033e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DBM_ANTSIGNAL] = { .align = 1, .size = 1, }, 3133e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DBM_ANTNOISE] = { .align = 1, .size = 1, }, 3233e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_LOCK_QUALITY] = { .align = 2, .size = 2, }, 3333e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_TX_ATTENUATION] = { .align = 2, .size = 2, }, 3433e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DB_TX_ATTENUATION] = { .align = 2, .size = 2, }, 3533e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DBM_TX_POWER] = { .align = 1, .size = 1, }, 3633e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_ANTENNA] = { .align = 1, .size = 1, }, 3733e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DB_ANTSIGNAL] = { .align = 1, .size = 1, }, 3833e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DB_ANTNOISE] = { .align = 1, .size = 1, }, 3933e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_RX_FLAGS] = { .align = 2, .size = 2, }, 4033e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_TX_FLAGS] = { .align = 2, .size = 2, }, 4133e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_RTS_RETRIES] = { .align = 1, .size = 1, }, 4233e5a2f7SJohannes Berg [IEEE80211_RADIOTAP_DATA_RETRIES] = { .align = 1, .size = 1, }, 4333e5a2f7SJohannes Berg /* 4433e5a2f7SJohannes Berg * add more here as they are defined in radiotap.h 4533e5a2f7SJohannes Berg */ 4633e5a2f7SJohannes Berg }; 4733e5a2f7SJohannes Berg 4833e5a2f7SJohannes Berg static const struct ieee80211_radiotap_namespace radiotap_ns = { 4994262316SNikitas Angelinas .n_bits = ARRAY_SIZE(rtap_namespace_sizes), 5033e5a2f7SJohannes Berg .align_size = rtap_namespace_sizes, 5133e5a2f7SJohannes Berg }; 5233e5a2f7SJohannes Berg 53179f831bSAndy Green /** 54179f831bSAndy Green * ieee80211_radiotap_iterator_init - radiotap parser iterator initialization 55179f831bSAndy Green * @iterator: radiotap_iterator to initialize 56179f831bSAndy Green * @radiotap_header: radiotap header to parse 57179f831bSAndy Green * @max_length: total length we can parse into (eg, whole packet length) 58179f831bSAndy Green * 59179f831bSAndy Green * Returns: 0 or a negative error code if there is a problem. 60179f831bSAndy Green * 61179f831bSAndy Green * This function initializes an opaque iterator struct which can then 62179f831bSAndy Green * be passed to ieee80211_radiotap_iterator_next() to visit every radiotap 63179f831bSAndy Green * argument which is present in the header. It knows about extended 64179f831bSAndy Green * present headers and handles them. 65179f831bSAndy Green * 66179f831bSAndy Green * How to use: 67179f831bSAndy Green * call __ieee80211_radiotap_iterator_init() to init a semi-opaque iterator 68179f831bSAndy Green * struct ieee80211_radiotap_iterator (no need to init the struct beforehand) 69179f831bSAndy Green * checking for a good 0 return code. Then loop calling 70179f831bSAndy Green * __ieee80211_radiotap_iterator_next()... it returns either 0, 71179f831bSAndy Green * -ENOENT if there are no more args to parse, or -EINVAL if there is a problem. 72179f831bSAndy Green * The iterator's @this_arg member points to the start of the argument 73179f831bSAndy Green * associated with the current argument index that is present, which can be 74179f831bSAndy Green * found in the iterator's @this_arg_index member. This arg index corresponds 75179f831bSAndy Green * to the IEEE80211_RADIOTAP_... defines. 76179f831bSAndy Green * 77179f831bSAndy Green * Radiotap header length: 78179f831bSAndy Green * You can find the CPU-endian total radiotap header length in 79179f831bSAndy Green * iterator->max_length after executing ieee80211_radiotap_iterator_init() 80179f831bSAndy Green * successfully. 81179f831bSAndy Green * 82179f831bSAndy Green * Alignment Gotcha: 83179f831bSAndy Green * You must take care when dereferencing iterator.this_arg 84179f831bSAndy Green * for multibyte types... the pointer is not aligned. Use 85179f831bSAndy Green * get_unaligned((type *)iterator.this_arg) to dereference 86179f831bSAndy Green * iterator.this_arg for type "type" safely on all arches. 87179f831bSAndy Green * 88179f831bSAndy Green * Example code: 89179f831bSAndy Green * See Documentation/networking/radiotap-headers.txt 90179f831bSAndy Green */ 91179f831bSAndy Green 92179f831bSAndy Green int ieee80211_radiotap_iterator_init( 93179f831bSAndy Green struct ieee80211_radiotap_iterator *iterator, 94179f831bSAndy Green struct ieee80211_radiotap_header *radiotap_header, 9533e5a2f7SJohannes Berg int max_length, const struct ieee80211_radiotap_vendor_namespaces *vns) 96179f831bSAndy Green { 97179f831bSAndy Green /* Linux only supports version 0 radiotap format */ 98179f831bSAndy Green if (radiotap_header->it_version) 99179f831bSAndy Green return -EINVAL; 100179f831bSAndy Green 101179f831bSAndy Green /* sanity check for allowed length and radiotap length field */ 102ae7245cbSHarvey Harrison if (max_length < get_unaligned_le16(&radiotap_header->it_len)) 103179f831bSAndy Green return -EINVAL; 104179f831bSAndy Green 10533e5a2f7SJohannes Berg iterator->_rtheader = radiotap_header; 10633e5a2f7SJohannes Berg iterator->_max_length = get_unaligned_le16(&radiotap_header->it_len); 10733e5a2f7SJohannes Berg iterator->_arg_index = 0; 10833e5a2f7SJohannes Berg iterator->_bitmap_shifter = get_unaligned_le32(&radiotap_header->it_present); 10933e5a2f7SJohannes Berg iterator->_arg = (uint8_t *)radiotap_header + sizeof(*radiotap_header); 11033e5a2f7SJohannes Berg iterator->_reset_on_ext = 0; 11133e5a2f7SJohannes Berg iterator->_next_bitmap = &radiotap_header->it_present; 11233e5a2f7SJohannes Berg iterator->_next_bitmap++; 11333e5a2f7SJohannes Berg iterator->_vns = vns; 11433e5a2f7SJohannes Berg iterator->current_namespace = &radiotap_ns; 11533e5a2f7SJohannes Berg iterator->is_radiotap_ns = 1; 116179f831bSAndy Green 117179f831bSAndy Green /* find payload start allowing for extended bitmap(s) */ 118179f831bSAndy Green 11933e5a2f7SJohannes Berg if (iterator->_bitmap_shifter & (1<<IEEE80211_RADIOTAP_EXT)) { 12033e5a2f7SJohannes Berg while (get_unaligned_le32(iterator->_arg) & 121179f831bSAndy Green (1 << IEEE80211_RADIOTAP_EXT)) { 12233e5a2f7SJohannes Berg iterator->_arg += sizeof(uint32_t); 123179f831bSAndy Green 124179f831bSAndy Green /* 125179f831bSAndy Green * check for insanity where the present bitmaps 126179f831bSAndy Green * keep claiming to extend up to or even beyond the 127179f831bSAndy Green * stated radiotap header length 128179f831bSAndy Green */ 129179f831bSAndy Green 13033e5a2f7SJohannes Berg if ((unsigned long)iterator->_arg - 13133e5a2f7SJohannes Berg (unsigned long)iterator->_rtheader > 13233e5a2f7SJohannes Berg (unsigned long)iterator->_max_length) 133179f831bSAndy Green return -EINVAL; 134179f831bSAndy Green } 135179f831bSAndy Green 13633e5a2f7SJohannes Berg iterator->_arg += sizeof(uint32_t); 137179f831bSAndy Green 138179f831bSAndy Green /* 139179f831bSAndy Green * no need to check again for blowing past stated radiotap 140179f831bSAndy Green * header length, because ieee80211_radiotap_iterator_next 141179f831bSAndy Green * checks it before it is dereferenced 142179f831bSAndy Green */ 143179f831bSAndy Green } 144179f831bSAndy Green 14533e5a2f7SJohannes Berg iterator->this_arg = iterator->_arg; 14633e5a2f7SJohannes Berg 147179f831bSAndy Green /* we are all initialized happily */ 148179f831bSAndy Green 149179f831bSAndy Green return 0; 150179f831bSAndy Green } 151179f831bSAndy Green EXPORT_SYMBOL(ieee80211_radiotap_iterator_init); 152179f831bSAndy Green 15333e5a2f7SJohannes Berg static void find_ns(struct ieee80211_radiotap_iterator *iterator, 15433e5a2f7SJohannes Berg uint32_t oui, uint8_t subns) 15533e5a2f7SJohannes Berg { 15633e5a2f7SJohannes Berg int i; 15733e5a2f7SJohannes Berg 15833e5a2f7SJohannes Berg iterator->current_namespace = NULL; 15933e5a2f7SJohannes Berg 16033e5a2f7SJohannes Berg if (!iterator->_vns) 16133e5a2f7SJohannes Berg return; 16233e5a2f7SJohannes Berg 16333e5a2f7SJohannes Berg for (i = 0; i < iterator->_vns->n_ns; i++) { 16433e5a2f7SJohannes Berg if (iterator->_vns->ns[i].oui != oui) 16533e5a2f7SJohannes Berg continue; 16633e5a2f7SJohannes Berg if (iterator->_vns->ns[i].subns != subns) 16733e5a2f7SJohannes Berg continue; 16833e5a2f7SJohannes Berg 16933e5a2f7SJohannes Berg iterator->current_namespace = &iterator->_vns->ns[i]; 17033e5a2f7SJohannes Berg break; 17133e5a2f7SJohannes Berg } 17233e5a2f7SJohannes Berg } 17333e5a2f7SJohannes Berg 17433e5a2f7SJohannes Berg 175179f831bSAndy Green 176179f831bSAndy Green /** 177179f831bSAndy Green * ieee80211_radiotap_iterator_next - return next radiotap parser iterator arg 178179f831bSAndy Green * @iterator: radiotap_iterator to move to next arg (if any) 179179f831bSAndy Green * 180179f831bSAndy Green * Returns: 0 if there is an argument to handle, 181179f831bSAndy Green * -ENOENT if there are no more args or -EINVAL 182179f831bSAndy Green * if there is something else wrong. 183179f831bSAndy Green * 184179f831bSAndy Green * This function provides the next radiotap arg index (IEEE80211_RADIOTAP_*) 185179f831bSAndy Green * in @this_arg_index and sets @this_arg to point to the 186179f831bSAndy Green * payload for the field. It takes care of alignment handling and extended 187179f831bSAndy Green * present fields. @this_arg can be changed by the caller (eg, 188179f831bSAndy Green * incremented to move inside a compound argument like 189179f831bSAndy Green * IEEE80211_RADIOTAP_CHANNEL). The args pointed to are in 190179f831bSAndy Green * little-endian format whatever the endianess of your CPU. 191179f831bSAndy Green * 192179f831bSAndy Green * Alignment Gotcha: 193179f831bSAndy Green * You must take care when dereferencing iterator.this_arg 194179f831bSAndy Green * for multibyte types... the pointer is not aligned. Use 195179f831bSAndy Green * get_unaligned((type *)iterator.this_arg) to dereference 196179f831bSAndy Green * iterator.this_arg for type "type" safely on all arches. 197179f831bSAndy Green */ 198179f831bSAndy Green 199179f831bSAndy Green int ieee80211_radiotap_iterator_next( 200179f831bSAndy Green struct ieee80211_radiotap_iterator *iterator) 201179f831bSAndy Green { 20233e5a2f7SJohannes Berg while (1) { 203179f831bSAndy Green int hit = 0; 204*9ebad4abSJohannes Berg int pad, align, size, subns; 20533e5a2f7SJohannes Berg uint32_t oui; 206179f831bSAndy Green 20733e5a2f7SJohannes Berg /* if no more EXT bits, that's it */ 20833e5a2f7SJohannes Berg if ((iterator->_arg_index % 32) == IEEE80211_RADIOTAP_EXT && 20933e5a2f7SJohannes Berg !(iterator->_bitmap_shifter & 1)) 21033e5a2f7SJohannes Berg return -ENOENT; 21133e5a2f7SJohannes Berg 21233e5a2f7SJohannes Berg if (!(iterator->_bitmap_shifter & 1)) 213179f831bSAndy Green goto next_entry; /* arg not present */ 214179f831bSAndy Green 21533e5a2f7SJohannes Berg /* get alignment/size of data */ 21633e5a2f7SJohannes Berg switch (iterator->_arg_index % 32) { 21733e5a2f7SJohannes Berg case IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE: 21833e5a2f7SJohannes Berg case IEEE80211_RADIOTAP_EXT: 21933e5a2f7SJohannes Berg align = 1; 22033e5a2f7SJohannes Berg size = 0; 22133e5a2f7SJohannes Berg break; 22233e5a2f7SJohannes Berg case IEEE80211_RADIOTAP_VENDOR_NAMESPACE: 22333e5a2f7SJohannes Berg align = 2; 22433e5a2f7SJohannes Berg size = 6; 22533e5a2f7SJohannes Berg break; 22633e5a2f7SJohannes Berg default: 22733e5a2f7SJohannes Berg if (!iterator->current_namespace || 22833e5a2f7SJohannes Berg iterator->_arg_index >= iterator->current_namespace->n_bits) { 22933e5a2f7SJohannes Berg if (iterator->current_namespace == &radiotap_ns) 23033e5a2f7SJohannes Berg return -ENOENT; 23133e5a2f7SJohannes Berg align = 0; 23233e5a2f7SJohannes Berg } else { 23333e5a2f7SJohannes Berg align = iterator->current_namespace->align_size[iterator->_arg_index].align; 23433e5a2f7SJohannes Berg size = iterator->current_namespace->align_size[iterator->_arg_index].size; 23533e5a2f7SJohannes Berg } 23633e5a2f7SJohannes Berg if (!align) { 23733e5a2f7SJohannes Berg /* skip all subsequent data */ 23833e5a2f7SJohannes Berg iterator->_arg = iterator->_next_ns_data; 23933e5a2f7SJohannes Berg /* give up on this namespace */ 24033e5a2f7SJohannes Berg iterator->current_namespace = NULL; 24133e5a2f7SJohannes Berg goto next_entry; 24233e5a2f7SJohannes Berg } 24333e5a2f7SJohannes Berg break; 24433e5a2f7SJohannes Berg } 24533e5a2f7SJohannes Berg 246179f831bSAndy Green /* 247179f831bSAndy Green * arg is present, account for alignment padding 248179f831bSAndy Green * 24933e5a2f7SJohannes Berg * Note that these alignments are relative to the start 25033e5a2f7SJohannes Berg * of the radiotap header. There is no guarantee 251179f831bSAndy Green * that the radiotap header itself is aligned on any 252179f831bSAndy Green * kind of boundary. 253179f831bSAndy Green * 25433e5a2f7SJohannes Berg * The above is why get_unaligned() is used to dereference 25533e5a2f7SJohannes Berg * multibyte elements from the radiotap area. 256179f831bSAndy Green */ 257179f831bSAndy Green 25833e5a2f7SJohannes Berg pad = ((unsigned long)iterator->_arg - 25933e5a2f7SJohannes Berg (unsigned long)iterator->_rtheader) & (align - 1); 260179f831bSAndy Green 261179f831bSAndy Green if (pad) 26233e5a2f7SJohannes Berg iterator->_arg += align - pad; 263179f831bSAndy Green 264*9ebad4abSJohannes Berg if (iterator->_arg_index % 32 == IEEE80211_RADIOTAP_VENDOR_NAMESPACE) { 265*9ebad4abSJohannes Berg int vnslen; 266*9ebad4abSJohannes Berg 267*9ebad4abSJohannes Berg if ((unsigned long)iterator->_arg + size - 268*9ebad4abSJohannes Berg (unsigned long)iterator->_rtheader > 269*9ebad4abSJohannes Berg (unsigned long)iterator->_max_length) 270*9ebad4abSJohannes Berg return -EINVAL; 271*9ebad4abSJohannes Berg 272*9ebad4abSJohannes Berg oui = (*iterator->_arg << 16) | 273*9ebad4abSJohannes Berg (*(iterator->_arg + 1) << 8) | 274*9ebad4abSJohannes Berg *(iterator->_arg + 2); 275*9ebad4abSJohannes Berg subns = *(iterator->_arg + 3); 276*9ebad4abSJohannes Berg 277*9ebad4abSJohannes Berg find_ns(iterator, oui, subns); 278*9ebad4abSJohannes Berg 279*9ebad4abSJohannes Berg vnslen = get_unaligned_le16(iterator->_arg + 4); 280*9ebad4abSJohannes Berg iterator->_next_ns_data = iterator->_arg + size + vnslen; 281*9ebad4abSJohannes Berg if (!iterator->current_namespace) 282*9ebad4abSJohannes Berg size += vnslen; 283*9ebad4abSJohannes Berg } 284*9ebad4abSJohannes Berg 285179f831bSAndy Green /* 286179f831bSAndy Green * this is what we will return to user, but we need to 287179f831bSAndy Green * move on first so next call has something fresh to test 288179f831bSAndy Green */ 28933e5a2f7SJohannes Berg iterator->this_arg_index = iterator->_arg_index; 29033e5a2f7SJohannes Berg iterator->this_arg = iterator->_arg; 29133e5a2f7SJohannes Berg iterator->this_arg_size = size; 292179f831bSAndy Green 293179f831bSAndy Green /* internally move on the size of this arg */ 29433e5a2f7SJohannes Berg iterator->_arg += size; 295179f831bSAndy Green 296179f831bSAndy Green /* 297179f831bSAndy Green * check for insanity where we are given a bitmap that 298179f831bSAndy Green * claims to have more arg content than the length of the 299179f831bSAndy Green * radiotap section. We will normally end up equalling this 300179f831bSAndy Green * max_length on the last arg, never exceeding it. 301179f831bSAndy Green */ 302179f831bSAndy Green 30333e5a2f7SJohannes Berg if ((unsigned long)iterator->_arg - 30433e5a2f7SJohannes Berg (unsigned long)iterator->_rtheader > 30533e5a2f7SJohannes Berg (unsigned long)iterator->_max_length) 306179f831bSAndy Green return -EINVAL; 307179f831bSAndy Green 30833e5a2f7SJohannes Berg /* these special ones are valid in each bitmap word */ 30933e5a2f7SJohannes Berg switch (iterator->_arg_index % 32) { 31033e5a2f7SJohannes Berg case IEEE80211_RADIOTAP_VENDOR_NAMESPACE: 31133e5a2f7SJohannes Berg iterator->_reset_on_ext = 1; 31233e5a2f7SJohannes Berg 31333e5a2f7SJohannes Berg iterator->is_radiotap_ns = 0; 314*9ebad4abSJohannes Berg /* 315*9ebad4abSJohannes Berg * If parser didn't register this vendor 316*9ebad4abSJohannes Berg * namespace with us, allow it to show it 317*9ebad4abSJohannes Berg * as 'raw. Do do that, set argument index 318*9ebad4abSJohannes Berg * to vendor namespace. 319*9ebad4abSJohannes Berg */ 32033e5a2f7SJohannes Berg iterator->this_arg_index = 32133e5a2f7SJohannes Berg IEEE80211_RADIOTAP_VENDOR_NAMESPACE; 322*9ebad4abSJohannes Berg if (!iterator->current_namespace) 32333e5a2f7SJohannes Berg hit = 1; 324*9ebad4abSJohannes Berg goto next_entry; 32533e5a2f7SJohannes Berg case IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE: 32633e5a2f7SJohannes Berg iterator->_reset_on_ext = 1; 32733e5a2f7SJohannes Berg iterator->current_namespace = &radiotap_ns; 32833e5a2f7SJohannes Berg iterator->is_radiotap_ns = 1; 329*9ebad4abSJohannes Berg goto next_entry; 33033e5a2f7SJohannes Berg case IEEE80211_RADIOTAP_EXT: 33133e5a2f7SJohannes Berg /* 33233e5a2f7SJohannes Berg * bit 31 was set, there is more 33333e5a2f7SJohannes Berg * -- move to next u32 bitmap 33433e5a2f7SJohannes Berg */ 33533e5a2f7SJohannes Berg iterator->_bitmap_shifter = 33633e5a2f7SJohannes Berg get_unaligned_le32(iterator->_next_bitmap); 33733e5a2f7SJohannes Berg iterator->_next_bitmap++; 33833e5a2f7SJohannes Berg if (iterator->_reset_on_ext) 33933e5a2f7SJohannes Berg iterator->_arg_index = 0; 34033e5a2f7SJohannes Berg else 34133e5a2f7SJohannes Berg iterator->_arg_index++; 34233e5a2f7SJohannes Berg iterator->_reset_on_ext = 0; 34333e5a2f7SJohannes Berg break; 34433e5a2f7SJohannes Berg default: 34533e5a2f7SJohannes Berg /* we've got a hit! */ 34633e5a2f7SJohannes Berg hit = 1; 347179f831bSAndy Green next_entry: 34833e5a2f7SJohannes Berg iterator->_bitmap_shifter >>= 1; 34933e5a2f7SJohannes Berg iterator->_arg_index++; 35033e5a2f7SJohannes Berg } 351179f831bSAndy Green 352179f831bSAndy Green /* if we found a valid arg earlier, return it now */ 353179f831bSAndy Green if (hit) 354179f831bSAndy Green return 0; 355179f831bSAndy Green } 356179f831bSAndy Green } 357179f831bSAndy Green EXPORT_SYMBOL(ieee80211_radiotap_iterator_next); 358