00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "includes.h"
00021
00022 #include "common.h"
00023 #include "radiotap_iter.h"
00024
00025 #define le16_to_cpu le_to_host16
00026 #define le32_to_cpu le_to_host32
00027 #define __le32 uint32_t
00028 #define ulong unsigned long
00029 #define unlikely(cond) (cond)
00030 #define get_unaligned(p) \
00031 ({ \
00032 struct packed_dummy_struct { \
00033 typeof(*(p)) __val; \
00034 } __attribute__((packed)) *__ptr = (void *) (p); \
00035 \
00036 __ptr->__val; \
00037 })
00038
00039
00040
00080 int ieee80211_radiotap_iterator_init(
00081 struct ieee80211_radiotap_iterator *iterator,
00082 struct ieee80211_radiotap_header *radiotap_header,
00083 int max_length)
00084 {
00085
00086 if (radiotap_header->it_version)
00087 return -EINVAL;
00088
00089
00090 if (max_length < le16_to_cpu(get_unaligned(&radiotap_header->it_len)))
00091 return -EINVAL;
00092
00093 iterator->rtheader = radiotap_header;
00094 iterator->max_length = le16_to_cpu(get_unaligned(
00095 &radiotap_header->it_len));
00096 iterator->arg_index = 0;
00097 iterator->bitmap_shifter = le32_to_cpu(get_unaligned(
00098 &radiotap_header->it_present));
00099 iterator->arg = (u8 *)radiotap_header + sizeof(*radiotap_header);
00100 iterator->this_arg = NULL;
00101
00102
00103
00104 if (unlikely(iterator->bitmap_shifter & (1<<IEEE80211_RADIOTAP_EXT))) {
00105 while (le32_to_cpu(get_unaligned((__le32 *)iterator->arg)) &
00106 (1<<IEEE80211_RADIOTAP_EXT)) {
00107 iterator->arg += sizeof(u32);
00108
00109
00110
00111
00112
00113
00114
00115 if (((ulong)iterator->arg - (ulong)iterator->rtheader)
00116 > (ulong)iterator->max_length)
00117 return -EINVAL;
00118 }
00119
00120 iterator->arg += sizeof(u32);
00121
00122
00123
00124
00125
00126
00127 }
00128
00129
00130
00131 return 0;
00132 }
00133
00134
00158 int ieee80211_radiotap_iterator_next(
00159 struct ieee80211_radiotap_iterator *iterator)
00160 {
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177 static const u8 rt_sizes[] = {
00178 [IEEE80211_RADIOTAP_TSFT] = 0x88,
00179 [IEEE80211_RADIOTAP_FLAGS] = 0x11,
00180 [IEEE80211_RADIOTAP_RATE] = 0x11,
00181 [IEEE80211_RADIOTAP_CHANNEL] = 0x24,
00182 [IEEE80211_RADIOTAP_FHSS] = 0x22,
00183 [IEEE80211_RADIOTAP_DBM_ANTSIGNAL] = 0x11,
00184 [IEEE80211_RADIOTAP_DBM_ANTNOISE] = 0x11,
00185 [IEEE80211_RADIOTAP_LOCK_QUALITY] = 0x22,
00186 [IEEE80211_RADIOTAP_TX_ATTENUATION] = 0x22,
00187 [IEEE80211_RADIOTAP_DB_TX_ATTENUATION] = 0x22,
00188 [IEEE80211_RADIOTAP_DBM_TX_POWER] = 0x11,
00189 [IEEE80211_RADIOTAP_ANTENNA] = 0x11,
00190 [IEEE80211_RADIOTAP_DB_ANTSIGNAL] = 0x11,
00191 [IEEE80211_RADIOTAP_DB_ANTNOISE] = 0x11,
00192 [IEEE80211_RADIOTAP_RX_FLAGS] = 0x22,
00193 [IEEE80211_RADIOTAP_TX_FLAGS] = 0x22,
00194 [IEEE80211_RADIOTAP_RTS_RETRIES] = 0x11,
00195 [IEEE80211_RADIOTAP_DATA_RETRIES] = 0x11,
00196
00197
00198
00199
00200 };
00201
00202
00203
00204
00205
00206
00207 while (iterator->arg_index < (int) sizeof(rt_sizes)) {
00208 int hit = 0;
00209 int pad;
00210
00211 if (!(iterator->bitmap_shifter & 1))
00212 goto next_entry;
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234 pad = (((ulong)iterator->arg) -
00235 ((ulong)iterator->rtheader)) &
00236 ((rt_sizes[iterator->arg_index] >> 4) - 1);
00237
00238 if (pad)
00239 iterator->arg +=
00240 (rt_sizes[iterator->arg_index] >> 4) - pad;
00241
00242
00243
00244
00245
00246 iterator->this_arg_index = iterator->arg_index;
00247 iterator->this_arg = iterator->arg;
00248 hit = 1;
00249
00250
00251 iterator->arg += rt_sizes[iterator->arg_index] & 0x0f;
00252
00253
00254
00255
00256
00257
00258
00259
00260 if (((ulong)iterator->arg - (ulong)iterator->rtheader) >
00261 (ulong) iterator->max_length)
00262 return -EINVAL;
00263
00264 next_entry:
00265 iterator->arg_index++;
00266 if (unlikely((iterator->arg_index & 31) == 0)) {
00267
00268 if (iterator->bitmap_shifter & 1) {
00269
00270
00271 iterator->bitmap_shifter = le32_to_cpu(
00272 get_unaligned(iterator->next_bitmap));
00273 iterator->next_bitmap++;
00274 } else
00275
00276 iterator->arg_index = sizeof(rt_sizes);
00277 } else
00278 iterator->bitmap_shifter >>= 1;
00279
00280
00281 if (hit)
00282 return 0;
00283 }
00284
00285
00286 return -ENOENT;
00287 }