1214501Srpaulo/* 2214501Srpaulo * Radiotap parser 3214501Srpaulo * 4214501Srpaulo * Copyright 2007 Andy Green <andy@warmcat.com> 5214501Srpaulo * 6214501Srpaulo * This program is free software; you can redistribute it and/or modify 7214501Srpaulo * it under the terms of the GNU General Public License version 2 as 8214501Srpaulo * published by the Free Software Foundation. 9214501Srpaulo * 10214501Srpaulo * Alternatively, this software may be distributed under the terms of BSD 11214501Srpaulo * license. 12214501Srpaulo * 13214501Srpaulo * See README and COPYING for more details. 14214501Srpaulo * 15214501Srpaulo * 16214501Srpaulo * Modified for userspace by Johannes Berg <johannes@sipsolutions.net> 17214501Srpaulo * I only modified some things on top to ease syncing should bugs be found. 18214501Srpaulo */ 19214501Srpaulo 20214501Srpaulo#include "includes.h" 21214501Srpaulo 22214501Srpaulo#include "common.h" 23214501Srpaulo#include "radiotap_iter.h" 24214501Srpaulo 25214501Srpaulo#define le16_to_cpu le_to_host16 26214501Srpaulo#define le32_to_cpu le_to_host32 27214501Srpaulo#define __le32 uint32_t 28214501Srpaulo#define ulong unsigned long 29214501Srpaulo#define unlikely(cond) (cond) 30214501Srpaulo#define get_unaligned(p) \ 31214501Srpaulo({ \ 32214501Srpaulo struct packed_dummy_struct { \ 33214501Srpaulo typeof(*(p)) __val; \ 34214501Srpaulo } __attribute__((packed)) *__ptr = (void *) (p); \ 35214501Srpaulo \ 36214501Srpaulo __ptr->__val; \ 37214501Srpaulo}) 38214501Srpaulo 39214501Srpaulo/* function prototypes and related defs are in radiotap_iter.h */ 40214501Srpaulo 41214501Srpaulo/** 42214501Srpaulo * ieee80211_radiotap_iterator_init - radiotap parser iterator initialization 43214501Srpaulo * @iterator: radiotap_iterator to initialize 44214501Srpaulo * @radiotap_header: radiotap header to parse 45214501Srpaulo * @max_length: total length we can parse into (eg, whole packet length) 46214501Srpaulo * 47214501Srpaulo * Returns: 0 or a negative error code if there is a problem. 48214501Srpaulo * 49214501Srpaulo * This function initializes an opaque iterator struct which can then 50214501Srpaulo * be passed to ieee80211_radiotap_iterator_next() to visit every radiotap 51214501Srpaulo * argument which is present in the header. It knows about extended 52214501Srpaulo * present headers and handles them. 53214501Srpaulo * 54214501Srpaulo * How to use: 55214501Srpaulo * call __ieee80211_radiotap_iterator_init() to init a semi-opaque iterator 56214501Srpaulo * struct ieee80211_radiotap_iterator (no need to init the struct beforehand) 57214501Srpaulo * checking for a good 0 return code. Then loop calling 58214501Srpaulo * __ieee80211_radiotap_iterator_next()... it returns either 0, 59214501Srpaulo * -ENOENT if there are no more args to parse, or -EINVAL if there is a problem. 60214501Srpaulo * The iterator's @this_arg member points to the start of the argument 61214501Srpaulo * associated with the current argument index that is present, which can be 62214501Srpaulo * found in the iterator's @this_arg_index member. This arg index corresponds 63214501Srpaulo * to the IEEE80211_RADIOTAP_... defines. 64214501Srpaulo * 65214501Srpaulo * Radiotap header length: 66214501Srpaulo * You can find the CPU-endian total radiotap header length in 67214501Srpaulo * iterator->max_length after executing ieee80211_radiotap_iterator_init() 68214501Srpaulo * successfully. 69214501Srpaulo * 70214501Srpaulo * Alignment Gotcha: 71214501Srpaulo * You must take care when dereferencing iterator.this_arg 72214501Srpaulo * for multibyte types... the pointer is not aligned. Use 73214501Srpaulo * get_unaligned((type *)iterator.this_arg) to dereference 74214501Srpaulo * iterator.this_arg for type "type" safely on all arches. 75214501Srpaulo * 76214501Srpaulo * Example code: 77214501Srpaulo * See Documentation/networking/radiotap-headers.txt 78214501Srpaulo */ 79214501Srpaulo 80214501Srpauloint ieee80211_radiotap_iterator_init( 81214501Srpaulo struct ieee80211_radiotap_iterator *iterator, 82214501Srpaulo struct ieee80211_radiotap_header *radiotap_header, 83214501Srpaulo int max_length) 84214501Srpaulo{ 85214501Srpaulo /* Linux only supports version 0 radiotap format */ 86214501Srpaulo if (radiotap_header->it_version) 87214501Srpaulo return -EINVAL; 88214501Srpaulo 89214501Srpaulo /* sanity check for allowed length and radiotap length field */ 90214501Srpaulo if (max_length < le16_to_cpu(get_unaligned(&radiotap_header->it_len))) 91214501Srpaulo return -EINVAL; 92214501Srpaulo 93214501Srpaulo iterator->rtheader = radiotap_header; 94214501Srpaulo iterator->max_length = le16_to_cpu(get_unaligned( 95214501Srpaulo &radiotap_header->it_len)); 96214501Srpaulo iterator->arg_index = 0; 97214501Srpaulo iterator->bitmap_shifter = le32_to_cpu(get_unaligned( 98214501Srpaulo &radiotap_header->it_present)); 99214501Srpaulo iterator->arg = (u8 *)radiotap_header + sizeof(*radiotap_header); 100214501Srpaulo iterator->this_arg = NULL; 101214501Srpaulo 102214501Srpaulo /* find payload start allowing for extended bitmap(s) */ 103214501Srpaulo 104214501Srpaulo if (unlikely(iterator->bitmap_shifter & (1<<IEEE80211_RADIOTAP_EXT))) { 105214501Srpaulo while (le32_to_cpu(get_unaligned((__le32 *)iterator->arg)) & 106214501Srpaulo (1<<IEEE80211_RADIOTAP_EXT)) { 107214501Srpaulo iterator->arg += sizeof(u32); 108214501Srpaulo 109214501Srpaulo /* 110214501Srpaulo * check for insanity where the present bitmaps 111214501Srpaulo * keep claiming to extend up to or even beyond the 112214501Srpaulo * stated radiotap header length 113214501Srpaulo */ 114214501Srpaulo 115214501Srpaulo if (((ulong)iterator->arg - (ulong)iterator->rtheader) 116214501Srpaulo > (ulong)iterator->max_length) 117214501Srpaulo return -EINVAL; 118214501Srpaulo } 119214501Srpaulo 120214501Srpaulo iterator->arg += sizeof(u32); 121214501Srpaulo 122214501Srpaulo /* 123214501Srpaulo * no need to check again for blowing past stated radiotap 124214501Srpaulo * header length, because ieee80211_radiotap_iterator_next 125214501Srpaulo * checks it before it is dereferenced 126214501Srpaulo */ 127214501Srpaulo } 128214501Srpaulo 129214501Srpaulo /* we are all initialized happily */ 130214501Srpaulo 131214501Srpaulo return 0; 132214501Srpaulo} 133214501Srpaulo 134214501Srpaulo 135214501Srpaulo/** 136214501Srpaulo * ieee80211_radiotap_iterator_next - return next radiotap parser iterator arg 137214501Srpaulo * @iterator: radiotap_iterator to move to next arg (if any) 138214501Srpaulo * 139214501Srpaulo * Returns: 0 if there is an argument to handle, 140214501Srpaulo * -ENOENT if there are no more args or -EINVAL 141214501Srpaulo * if there is something else wrong. 142214501Srpaulo * 143214501Srpaulo * This function provides the next radiotap arg index (IEEE80211_RADIOTAP_*) 144214501Srpaulo * in @this_arg_index and sets @this_arg to point to the 145214501Srpaulo * payload for the field. It takes care of alignment handling and extended 146214501Srpaulo * present fields. @this_arg can be changed by the caller (eg, 147214501Srpaulo * incremented to move inside a compound argument like 148214501Srpaulo * IEEE80211_RADIOTAP_CHANNEL). The args pointed to are in 149214501Srpaulo * little-endian format whatever the endianess of your CPU. 150214501Srpaulo * 151214501Srpaulo * Alignment Gotcha: 152214501Srpaulo * You must take care when dereferencing iterator.this_arg 153214501Srpaulo * for multibyte types... the pointer is not aligned. Use 154214501Srpaulo * get_unaligned((type *)iterator.this_arg) to dereference 155214501Srpaulo * iterator.this_arg for type "type" safely on all arches. 156214501Srpaulo */ 157214501Srpaulo 158214501Srpauloint ieee80211_radiotap_iterator_next( 159214501Srpaulo struct ieee80211_radiotap_iterator *iterator) 160214501Srpaulo{ 161214501Srpaulo 162214501Srpaulo /* 163214501Srpaulo * small length lookup table for all radiotap types we heard of 164214501Srpaulo * starting from b0 in the bitmap, so we can walk the payload 165214501Srpaulo * area of the radiotap header 166214501Srpaulo * 167214501Srpaulo * There is a requirement to pad args, so that args 168214501Srpaulo * of a given length must begin at a boundary of that length 169214501Srpaulo * -- but note that compound args are allowed (eg, 2 x u16 170214501Srpaulo * for IEEE80211_RADIOTAP_CHANNEL) so total arg length is not 171214501Srpaulo * a reliable indicator of alignment requirement. 172214501Srpaulo * 173214501Srpaulo * upper nybble: content alignment for arg 174214501Srpaulo * lower nybble: content length for arg 175214501Srpaulo */ 176214501Srpaulo 177214501Srpaulo static const u8 rt_sizes[] = { 178214501Srpaulo [IEEE80211_RADIOTAP_TSFT] = 0x88, 179214501Srpaulo [IEEE80211_RADIOTAP_FLAGS] = 0x11, 180214501Srpaulo [IEEE80211_RADIOTAP_RATE] = 0x11, 181214501Srpaulo [IEEE80211_RADIOTAP_CHANNEL] = 0x24, 182214501Srpaulo [IEEE80211_RADIOTAP_FHSS] = 0x22, 183214501Srpaulo [IEEE80211_RADIOTAP_DBM_ANTSIGNAL] = 0x11, 184214501Srpaulo [IEEE80211_RADIOTAP_DBM_ANTNOISE] = 0x11, 185214501Srpaulo [IEEE80211_RADIOTAP_LOCK_QUALITY] = 0x22, 186214501Srpaulo [IEEE80211_RADIOTAP_TX_ATTENUATION] = 0x22, 187214501Srpaulo [IEEE80211_RADIOTAP_DB_TX_ATTENUATION] = 0x22, 188214501Srpaulo [IEEE80211_RADIOTAP_DBM_TX_POWER] = 0x11, 189214501Srpaulo [IEEE80211_RADIOTAP_ANTENNA] = 0x11, 190214501Srpaulo [IEEE80211_RADIOTAP_DB_ANTSIGNAL] = 0x11, 191214501Srpaulo [IEEE80211_RADIOTAP_DB_ANTNOISE] = 0x11, 192214501Srpaulo [IEEE80211_RADIOTAP_RX_FLAGS] = 0x22, 193214501Srpaulo [IEEE80211_RADIOTAP_TX_FLAGS] = 0x22, 194214501Srpaulo [IEEE80211_RADIOTAP_RTS_RETRIES] = 0x11, 195214501Srpaulo [IEEE80211_RADIOTAP_DATA_RETRIES] = 0x11, 196214501Srpaulo /* 197214501Srpaulo * add more here as they are defined in 198214501Srpaulo * include/net/ieee80211_radiotap.h 199214501Srpaulo */ 200214501Srpaulo }; 201214501Srpaulo 202214501Srpaulo /* 203214501Srpaulo * for every radiotap entry we can at 204214501Srpaulo * least skip (by knowing the length)... 205214501Srpaulo */ 206214501Srpaulo 207214501Srpaulo while (iterator->arg_index < (int) sizeof(rt_sizes)) { 208214501Srpaulo int hit = 0; 209214501Srpaulo int pad; 210214501Srpaulo 211214501Srpaulo if (!(iterator->bitmap_shifter & 1)) 212214501Srpaulo goto next_entry; /* arg not present */ 213214501Srpaulo 214214501Srpaulo /* 215214501Srpaulo * arg is present, account for alignment padding 216214501Srpaulo * 8-bit args can be at any alignment 217214501Srpaulo * 16-bit args must start on 16-bit boundary 218214501Srpaulo * 32-bit args must start on 32-bit boundary 219214501Srpaulo * 64-bit args must start on 64-bit boundary 220214501Srpaulo * 221214501Srpaulo * note that total arg size can differ from alignment of 222214501Srpaulo * elements inside arg, so we use upper nybble of length 223214501Srpaulo * table to base alignment on 224214501Srpaulo * 225214501Srpaulo * also note: these alignments are ** relative to the 226214501Srpaulo * start of the radiotap header **. There is no guarantee 227214501Srpaulo * that the radiotap header itself is aligned on any 228214501Srpaulo * kind of boundary. 229214501Srpaulo * 230214501Srpaulo * the above is why get_unaligned() is used to dereference 231214501Srpaulo * multibyte elements from the radiotap area 232214501Srpaulo */ 233214501Srpaulo 234214501Srpaulo pad = (((ulong)iterator->arg) - 235214501Srpaulo ((ulong)iterator->rtheader)) & 236214501Srpaulo ((rt_sizes[iterator->arg_index] >> 4) - 1); 237214501Srpaulo 238214501Srpaulo if (pad) 239214501Srpaulo iterator->arg += 240214501Srpaulo (rt_sizes[iterator->arg_index] >> 4) - pad; 241214501Srpaulo 242214501Srpaulo /* 243214501Srpaulo * this is what we will return to user, but we need to 244214501Srpaulo * move on first so next call has something fresh to test 245214501Srpaulo */ 246214501Srpaulo iterator->this_arg_index = iterator->arg_index; 247214501Srpaulo iterator->this_arg = iterator->arg; 248214501Srpaulo hit = 1; 249214501Srpaulo 250214501Srpaulo /* internally move on the size of this arg */ 251214501Srpaulo iterator->arg += rt_sizes[iterator->arg_index] & 0x0f; 252214501Srpaulo 253214501Srpaulo /* 254214501Srpaulo * check for insanity where we are given a bitmap that 255214501Srpaulo * claims to have more arg content than the length of the 256214501Srpaulo * radiotap section. We will normally end up equalling this 257214501Srpaulo * max_length on the last arg, never exceeding it. 258214501Srpaulo */ 259214501Srpaulo 260214501Srpaulo if (((ulong)iterator->arg - (ulong)iterator->rtheader) > 261214501Srpaulo (ulong) iterator->max_length) 262214501Srpaulo return -EINVAL; 263214501Srpaulo 264214501Srpaulo next_entry: 265214501Srpaulo iterator->arg_index++; 266214501Srpaulo if (unlikely((iterator->arg_index & 31) == 0)) { 267214501Srpaulo /* completed current u32 bitmap */ 268214501Srpaulo if (iterator->bitmap_shifter & 1) { 269214501Srpaulo /* b31 was set, there is more */ 270214501Srpaulo /* move to next u32 bitmap */ 271214501Srpaulo iterator->bitmap_shifter = le32_to_cpu( 272214501Srpaulo get_unaligned(iterator->next_bitmap)); 273214501Srpaulo iterator->next_bitmap++; 274214501Srpaulo } else 275214501Srpaulo /* no more bitmaps: end */ 276214501Srpaulo iterator->arg_index = sizeof(rt_sizes); 277214501Srpaulo } else /* just try the next bit */ 278214501Srpaulo iterator->bitmap_shifter >>= 1; 279214501Srpaulo 280214501Srpaulo /* if we found a valid arg earlier, return it now */ 281214501Srpaulo if (hit) 282214501Srpaulo return 0; 283214501Srpaulo } 284214501Srpaulo 285214501Srpaulo /* we don't know how to handle any more args, we're done */ 286214501Srpaulo return -ENOENT; 287214501Srpaulo} 288