diff options
Diffstat (limited to 'src/transport/wlan/radiotap-parser.c')
-rw-r--r-- | src/transport/wlan/radiotap-parser.c | 251 |
1 files changed, 251 insertions, 0 deletions
diff --git a/src/transport/wlan/radiotap-parser.c b/src/transport/wlan/radiotap-parser.c new file mode 100644 index 000000000..46ea6d542 --- /dev/null +++ b/src/transport/wlan/radiotap-parser.c | |||
@@ -0,0 +1,251 @@ | |||
1 | /* | ||
2 | * Copyright (c) 2007, 2008, Andy Green <andy@warmcat.com> | ||
3 | * | ||
4 | * This program is free software; you can redistribute it and/or modify | ||
5 | * it under the terms of the GNU General Public License as published by | ||
6 | * the Free Software Foundation; either version 2 of the License, or | ||
7 | * (at your option) any later version. | ||
8 | * | ||
9 | * This program is distributed in the hope that it will be useful, | ||
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
12 | * GNU General Public License for more details. | ||
13 | * | ||
14 | * You should have received a copy of the GNU General Public License | ||
15 | * along with this program; if not, write to the Free Software | ||
16 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA | ||
17 | */ | ||
18 | |||
19 | #include <sys/types.h> | ||
20 | #include <stdio.h> | ||
21 | #include <errno.h> | ||
22 | |||
23 | #include "radiotap-parser.h" | ||
24 | |||
25 | |||
26 | /* | ||
27 | * Radiotap header iteration | ||
28 | * implemented in src/radiotap-parser.c | ||
29 | * | ||
30 | * call __ieee80211_radiotap_iterator_init() to init a semi-opaque iterator | ||
31 | * struct ieee80211_radiotap_iterator (no need to init the struct beforehand) | ||
32 | * then loop calling __ieee80211_radiotap_iterator_next()... it returns -1 | ||
33 | * if there are no more args in the header, or the next argument type index | ||
34 | * that is present. The iterator's this_arg member points to the start of the | ||
35 | * argument associated with the current argument index that is present, | ||
36 | * which can be found in the iterator's this_arg_index member. This arg | ||
37 | * index corresponds to the IEEE80211_RADIOTAP_... defines. | ||
38 | */ | ||
39 | |||
40 | |||
41 | int ieee80211_radiotap_iterator_init( | ||
42 | struct ieee80211_radiotap_iterator * iterator, | ||
43 | struct ieee80211_radiotap_header * radiotap_header, | ||
44 | int max_length) | ||
45 | { | ||
46 | if(iterator == NULL) | ||
47 | return (-EINVAL); | ||
48 | |||
49 | if(radiotap_header == NULL) | ||
50 | return (-EINVAL); | ||
51 | /* Linux only supports version 0 radiotap format */ | ||
52 | |||
53 | if (radiotap_header->it_version) | ||
54 | return (-EINVAL); | ||
55 | |||
56 | /* sanity check for allowed length and radiotap length field */ | ||
57 | |||
58 | if (max_length < (le16_to_cpu(radiotap_header->it_len))) | ||
59 | return (-EINVAL); | ||
60 | |||
61 | iterator->rtheader = radiotap_header; | ||
62 | iterator->max_length = le16_to_cpu(radiotap_header->it_len); | ||
63 | iterator->arg_index = 0; | ||
64 | iterator->bitmap_shifter = le32_to_cpu(radiotap_header->it_present); | ||
65 | iterator->arg = ((u8 *)radiotap_header) + | ||
66 | sizeof (struct ieee80211_radiotap_header); | ||
67 | iterator->this_arg = 0; | ||
68 | |||
69 | /* find payload start allowing for extended bitmap(s) */ | ||
70 | |||
71 | if (unlikely(iterator->bitmap_shifter & | ||
72 | IEEE80211_RADIOTAP_PRESENT_EXTEND_MASK)) { | ||
73 | while (le32_to_cpu(*((u32 *)iterator->arg)) & | ||
74 | IEEE80211_RADIOTAP_PRESENT_EXTEND_MASK) { | ||
75 | iterator->arg += sizeof (u32); | ||
76 | |||
77 | /* | ||
78 | * check for insanity where the present bitmaps | ||
79 | * keep claiming to extend up to or even beyond the | ||
80 | * stated radiotap header length | ||
81 | */ | ||
82 | |||
83 | if ((((void*)iterator->arg) - ((void*)iterator->rtheader)) > | ||
84 | iterator->max_length) | ||
85 | return (-EINVAL); | ||
86 | |||
87 | } | ||
88 | |||
89 | iterator->arg += sizeof (u32); | ||
90 | |||
91 | /* | ||
92 | * no need to check again for blowing past stated radiotap | ||
93 | * header length, becuase ieee80211_radiotap_iterator_next | ||
94 | * checks it before it is dereferenced | ||
95 | */ | ||
96 | |||
97 | } | ||
98 | |||
99 | /* we are all initialized happily */ | ||
100 | |||
101 | return (0); | ||
102 | } | ||
103 | |||
104 | |||
105 | /** | ||
106 | * ieee80211_radiotap_iterator_next - return next radiotap parser iterator arg | ||
107 | * @iterator: radiotap_iterator to move to next arg (if any) | ||
108 | * | ||
109 | * Returns: next present arg index on success or negative if no more or error | ||
110 | * | ||
111 | * This function returns the next radiotap arg index (IEEE80211_RADIOTAP_...) | ||
112 | * and sets iterator->this_arg to point to the payload for the arg. It takes | ||
113 | * care of alignment handling and extended present fields. interator->this_arg | ||
114 | * can be changed by the caller. The args pointed to are in little-endian | ||
115 | * format. | ||
116 | */ | ||
117 | |||
118 | int ieee80211_radiotap_iterator_next( | ||
119 | struct ieee80211_radiotap_iterator * iterator) | ||
120 | { | ||
121 | |||
122 | /* | ||
123 | * small length lookup table for all radiotap types we heard of | ||
124 | * starting from b0 in the bitmap, so we can walk the payload | ||
125 | * area of the radiotap header | ||
126 | * | ||
127 | * There is a requirement to pad args, so that args | ||
128 | * of a given length must begin at a boundary of that length | ||
129 | * -- but note that compound args are allowed (eg, 2 x u16 | ||
130 | * for IEEE80211_RADIOTAP_CHANNEL) so total arg length is not | ||
131 | * a reliable indicator of alignment requirement. | ||
132 | * | ||
133 | * upper nybble: content alignment for arg | ||
134 | * lower nybble: content length for arg | ||
135 | */ | ||
136 | |||
137 | static const u8 rt_sizes[] = { | ||
138 | [IEEE80211_RADIOTAP_TSFT] = 0x88, | ||
139 | [IEEE80211_RADIOTAP_FLAGS] = 0x11, | ||
140 | [IEEE80211_RADIOTAP_RATE] = 0x11, | ||
141 | [IEEE80211_RADIOTAP_CHANNEL] = 0x24, | ||
142 | [IEEE80211_RADIOTAP_FHSS] = 0x22, | ||
143 | [IEEE80211_RADIOTAP_DBM_ANTSIGNAL] = 0x11, | ||
144 | [IEEE80211_RADIOTAP_DBM_ANTNOISE] = 0x11, | ||
145 | [IEEE80211_RADIOTAP_LOCK_QUALITY] = 0x22, | ||
146 | [IEEE80211_RADIOTAP_TX_ATTENUATION] = 0x22, | ||
147 | [IEEE80211_RADIOTAP_DB_TX_ATTENUATION] = 0x22, | ||
148 | [IEEE80211_RADIOTAP_DBM_TX_POWER] = 0x11, | ||
149 | [IEEE80211_RADIOTAP_ANTENNA] = 0x11, | ||
150 | [IEEE80211_RADIOTAP_DB_ANTSIGNAL] = 0x11, | ||
151 | [IEEE80211_RADIOTAP_DB_ANTNOISE] = 0x11, | ||
152 | [IEEE80211_RADIOTAP_TX_FLAGS] = 0x22, | ||
153 | [IEEE80211_RADIOTAP_RX_FLAGS] = 0x22, | ||
154 | [IEEE80211_RADIOTAP_RTS_RETRIES] = 0x11, | ||
155 | [IEEE80211_RADIOTAP_DATA_RETRIES] = 0x11 | ||
156 | /* | ||
157 | * add more here as they are defined in | ||
158 | * include/net/ieee80211_radiotap.h | ||
159 | */ | ||
160 | }; | ||
161 | |||
162 | /* | ||
163 | * for every radiotap entry we can at | ||
164 | * least skip (by knowing the length)... | ||
165 | */ | ||
166 | |||
167 | while (iterator->arg_index < (int)sizeof (rt_sizes)) { | ||
168 | int hit = 0; | ||
169 | |||
170 | if (!(iterator->bitmap_shifter & 1)) | ||
171 | goto next_entry; /* arg not present */ | ||
172 | |||
173 | /* | ||
174 | * arg is present, account for alignment padding | ||
175 | * 8-bit args can be at any alignment | ||
176 | * 16-bit args must start on 16-bit boundary | ||
177 | * 32-bit args must start on 32-bit boundary | ||
178 | * 64-bit args must start on 64-bit boundary | ||
179 | * | ||
180 | * note that total arg size can differ from alignment of | ||
181 | * elements inside arg, so we use upper nybble of length | ||
182 | * table to base alignment on | ||
183 | * | ||
184 | * also note: these alignments are ** relative to the | ||
185 | * start of the radiotap header **. There is no guarantee | ||
186 | * that the radiotap header itself is aligned on any | ||
187 | * kind of boundary. | ||
188 | */ | ||
189 | |||
190 | if ((((void*)iterator->arg)-((void*)iterator->rtheader)) & | ||
191 | ((rt_sizes[iterator->arg_index] >> 4) - 1)) | ||
192 | iterator->arg_index += | ||
193 | (rt_sizes[iterator->arg_index] >> 4) - | ||
194 | ((((void*)iterator->arg) - | ||
195 | ((void*)iterator->rtheader)) & | ||
196 | ((rt_sizes[iterator->arg_index] >> 4) - 1)); | ||
197 | |||
198 | /* | ||
199 | * this is what we will return to user, but we need to | ||
200 | * move on first so next call has something fresh to test | ||
201 | */ | ||
202 | |||
203 | iterator->this_arg_index = iterator->arg_index; | ||
204 | iterator->this_arg = iterator->arg; | ||
205 | hit = 1; | ||
206 | |||
207 | /* internally move on the size of this arg */ | ||
208 | |||
209 | iterator->arg += rt_sizes[iterator->arg_index] & 0x0f; | ||
210 | |||
211 | /* | ||
212 | * check for insanity where we are given a bitmap that | ||
213 | * claims to have more arg content than the length of the | ||
214 | * radiotap section. We will normally end up equalling this | ||
215 | * max_length on the last arg, never exceeding it. | ||
216 | */ | ||
217 | |||
218 | if ((((void*)iterator->arg) - ((void*)iterator->rtheader)) > | ||
219 | iterator->max_length) | ||
220 | return (-EINVAL); | ||
221 | |||
222 | next_entry: | ||
223 | |||
224 | iterator->arg_index++; | ||
225 | if (unlikely((iterator->arg_index & 31) == 0)) { | ||
226 | /* completed current u32 bitmap */ | ||
227 | if (iterator->bitmap_shifter & 1) { | ||
228 | /* b31 was set, there is more */ | ||
229 | /* move to next u32 bitmap */ | ||
230 | iterator->bitmap_shifter = le32_to_cpu( | ||
231 | *iterator->next_bitmap); | ||
232 | iterator->next_bitmap++; | ||
233 | } else { | ||
234 | /* no more bitmaps: end */ | ||
235 | iterator->arg_index = sizeof (rt_sizes); | ||
236 | } | ||
237 | } else { /* just try the next bit */ | ||
238 | iterator->bitmap_shifter >>= 1; | ||
239 | } | ||
240 | |||
241 | /* if we found a valid arg earlier, return it now */ | ||
242 | |||
243 | if (hit) | ||
244 | return (iterator->this_arg_index); | ||
245 | |||
246 | } | ||
247 | |||
248 | /* we don't know how to handle any more args, we're done */ | ||
249 | |||
250 | return (-1); | ||
251 | } | ||