1 /*
2 * RadioTap utility routines for WL
3 * This file housing the functions use by
4 * wl driver.
5 *
6 * Copyright (C) 2020, Broadcom.
7 *
8 * Unless you and Broadcom execute a separate written software license
9 * agreement governing use of this software, this software is licensed to you
10 * under the terms of the GNU General Public License version 2 (the "GPL"),
11 * available at http://www.broadcom.com/licenses/GPLv2.php, with the
12 * following added to such license:
13 *
14 * As a special exception, the copyright holders of this software give you
15 * permission to link this software with independent modules, and to copy and
16 * distribute the resulting executable under terms of your choice, provided that
17 * you also meet, for each linked independent module, the terms and conditions of
18 * the license of that module. An independent module is a module which is not
19 * derived from this software. The special exception does not apply to any
20 * modifications of the software.
21 *
22 *
23 * <<Broadcom-WL-IPTag/Dual:>>
24 */
25
26 #include <bcmutils.h>
27 #include <bcmendian.h>
28 #include <bcmwifi_channels.h>
29 #include <hndd11.h>
30 #include <bcmwifi_radiotap.h>
31
32 const struct rtap_field rtap_parse_info[] = {
33 {8, 8}, /* 0: IEEE80211_RADIOTAP_TSFT */
34 {1, 1}, /* 1: IEEE80211_RADIOTAP_FLAGS */
35 {1, 1}, /* 2: IEEE80211_RADIOTAP_RATE */
36 {4, 2}, /* 3: IEEE80211_RADIOTAP_CHANNEL */
37 {2, 2}, /* 4: IEEE80211_RADIOTAP_FHSS */
38 {1, 1}, /* 5: IEEE80211_RADIOTAP_DBM_ANTSIGNAL */
39 {1, 1}, /* 6: IEEE80211_RADIOTAP_DBM_ANTNOISE */
40 {2, 2}, /* 7: IEEE80211_RADIOTAP_LOCK_QUALITY */
41 {2, 2}, /* 8: IEEE80211_RADIOTAP_TX_ATTENUATION */
42 {2, 2}, /* 9: IEEE80211_RADIOTAP_DB_TX_ATTENUATION */
43 {1, 1}, /* 10: IEEE80211_RADIOTAP_DBM_TX_POWER */
44 {1, 1}, /* 11: IEEE80211_RADIOTAP_ANTENNA */
45 {1, 1}, /* 12: IEEE80211_RADIOTAP_DB_ANTSIGNAL */
46 {1, 1}, /* 13: IEEE80211_RADIOTAP_DB_ANTNOISE */
47 {0, 0}, /* 14: netbsd */
48 {2, 2}, /* 15: IEEE80211_RADIOTAP_TXFLAGS */
49 {0, 0}, /* 16: missing */
50 {1, 1}, /* 17: IEEE80211_RADIOTAP_RETRIES */
51 {8, 4}, /* 18: IEEE80211_RADIOTAP_XCHANNEL */
52 {3, 1}, /* 19: IEEE80211_RADIOTAP_MCS */
53 {8, 4}, /* 20: IEEE80211_RADIOTAP_AMPDU_STATUS */
54 {12, 2}, /* 21: IEEE80211_RADIOTAP_VHT */
55 {0, 0}, /* 22: */
56 {0, 0}, /* 23: */
57 {0, 0}, /* 24: */
58 {0, 0}, /* 25: */
59 {0, 0}, /* 26: */
60 {0, 0}, /* 27: */
61 {0, 0}, /* 28: */
62 {0, 0}, /* 29: IEEE80211_RADIOTAP_RADIOTAP_NAMESPACE */
63 {6, 2}, /* 30: IEEE80211_RADIOTAP_VENDOR_NAMESPACE */
64 {0, 0} /* 31: IEEE80211_RADIOTAP_EXT */
65 };
66
67 static int bitmap = 0;
68
69 void
70 radiotap_add_vendor_ns(ieee80211_radiotap_header_t *hdr);
71
72 void
73 radiotap_encode_multi_rssi(monitor_pkt_rxsts_t* rxsts, ieee80211_radiotap_header_t *hdr);
74 void
75 radiotap_encode_bw_signaling(uint16 mask, struct wl_rxsts* rxsts, ieee80211_radiotap_header_t *hdr);
76 #ifdef MONITOR_DNGL_CONV
77 void radiotap_encode_alignpad(ieee80211_radiotap_header_t *hdr, uint16 pad_req);
78 #endif
79
80 static const uint8 brcm_oui[] = {0x00, 0x10, 0x18};
81
82 static void
wl_rtapParseReset(radiotap_parse_t * rtap)83 wl_rtapParseReset(radiotap_parse_t *rtap)
84 {
85 rtap->idx = 0; /* reset parse index */
86 rtap->offset = 0; /* reset current field pointer */
87 }
88
89 static void*
wl_rtapParseFindField(radiotap_parse_t * rtap,uint search_idx)90 wl_rtapParseFindField(radiotap_parse_t *rtap, uint search_idx)
91 {
92 uint idx; /* first bit index to parse */
93 uint32 btmap; /* presence bitmap */
94 uint offset, field_offset;
95 uint align, len;
96 void *ptr = NULL;
97
98 if (search_idx > IEEE80211_RADIOTAP_EXT)
99 return ptr;
100
101 if (search_idx < rtap->idx)
102 wl_rtapParseReset(rtap);
103
104 btmap = rtap->hdr->it_present;
105 idx = rtap->idx;
106 offset = rtap->offset;
107
108 /* loop through each field index until we get to the target idx */
109 while (idx <= search_idx) {
110 /* if field 'idx' is present, update the offset and check for a match */
111 if ((1 << idx) & btmap) {
112 /* if we hit a field for which we have no parse info
113 * we need to just bail out
114 */
115 if (rtap_parse_info[idx].align == 0)
116 break;
117
118 /* step past any alignment padding */
119 align = rtap_parse_info[idx].align;
120 len = rtap_parse_info[idx].len;
121
122 /* ROUNDUP */
123 field_offset = ((offset + (align - 1)) / align) * align;
124
125 /* if this field is not in the boulds of the header
126 * just bail out
127 */
128 if (field_offset + len > rtap->fields_len)
129 break;
130
131 /* did we find the field? */
132 if (idx == search_idx)
133 ptr = (uint8*)rtap->fields + field_offset;
134
135 /* step past this field */
136 offset = field_offset + len;
137 }
138
139 idx++;
140 }
141
142 rtap->idx = idx;
143 rtap->offset = offset;
144
145 return ptr;
146 }
147
148 ratespec_t
wl_calcRspecFromRTap(uint8 * rtap_header)149 wl_calcRspecFromRTap(uint8 *rtap_header)
150 {
151 ratespec_t rspec = 0;
152 radiotap_parse_t rtap;
153 uint8 rate = 0;
154 uint8 flags = 0;
155 int flags_present = FALSE;
156 uint8 mcs = 0;
157 uint8 mcs_flags = 0;
158 uint8 mcs_known = 0;
159 int mcs_present = FALSE;
160 void *p;
161
162 wl_rtapParseInit(&rtap, rtap_header);
163
164 p = wl_rtapParseFindField(&rtap, IEEE80211_RADIOTAP_FLAGS);
165 if (p != NULL) {
166 flags_present = TRUE;
167 flags = ((uint8*)p)[0];
168 }
169
170 p = wl_rtapParseFindField(&rtap, IEEE80211_RADIOTAP_RATE);
171 if (p != NULL)
172 rate = ((uint8*)p)[0];
173
174 p = wl_rtapParseFindField(&rtap, IEEE80211_RADIOTAP_MCS);
175 if (p != NULL) {
176 mcs_present = TRUE;
177 mcs_known = ((uint8*)p)[0];
178 mcs_flags = ((uint8*)p)[1];
179 mcs = ((uint8*)p)[2];
180 }
181
182 if (rate != 0) {
183 /* validate the DSSS rates 1,2,5.5,11 */
184 if (rate == 2 || rate == 4 || rate == 11 || rate == 22) {
185 rspec = LEGACY_RSPEC(rate) | WL_RSPEC_OVERRIDE_RATE;
186 if (flags_present && (flags & IEEE80211_RADIOTAP_F_SHORTPRE)) {
187 rspec |= WL_RSPEC_OVERRIDE_MODE | WL_RSPEC_SHORT_PREAMBLE;
188 }
189 }
190 } else if (mcs_present) {
191 /* validate the MCS value */
192 if (mcs <= 23 || mcs == 32) {
193 uint32 override = 0;
194 if (mcs_known &
195 (IEEE80211_RADIOTAP_MCS_HAVE_GI |
196 IEEE80211_RADIOTAP_MCS_HAVE_FMT |
197 IEEE80211_RADIOTAP_MCS_HAVE_FEC)) {
198 override = WL_RSPEC_OVERRIDE_MODE;
199 }
200
201 rspec = HT_RSPEC(mcs) | WL_RSPEC_OVERRIDE_RATE;
202
203 if ((mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_GI) &&
204 (mcs_flags & IEEE80211_RADIOTAP_MCS_SGI))
205 rspec |= WL_RSPEC_SGI;
206 if ((mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_FMT) &&
207 (mcs_flags & IEEE80211_RADIOTAP_MCS_FMT_GF))
208 rspec |= WL_RSPEC_SHORT_PREAMBLE;
209 if ((mcs_known & IEEE80211_RADIOTAP_MCS_HAVE_FEC) &&
210 (mcs_flags & IEEE80211_RADIOTAP_MCS_FEC_LDPC))
211 rspec |= WL_RSPEC_LDPC;
212
213 rspec |= override;
214 }
215 }
216
217 return rspec;
218 }
219
220 bool
wl_rtapFlags(uint8 * rtap_header,uint8 * flags)221 wl_rtapFlags(uint8 *rtap_header, uint8* flags)
222 {
223 radiotap_parse_t rtap;
224 void *p;
225
226 wl_rtapParseInit(&rtap, rtap_header);
227
228 p = wl_rtapParseFindField(&rtap, IEEE80211_RADIOTAP_FLAGS);
229 if (p != NULL) {
230 *flags = ((uint8*)p)[0];
231 }
232
233 return (p != NULL);
234 }
235
236 void
wl_rtapParseInit(radiotap_parse_t * rtap,uint8 * rtap_header)237 wl_rtapParseInit(radiotap_parse_t *rtap, uint8 *rtap_header)
238 {
239 uint rlen;
240 uint32 *present_word;
241 struct ieee80211_radiotap_header *hdr = (struct ieee80211_radiotap_header*)rtap_header;
242
243 bzero(rtap, sizeof(radiotap_parse_t));
244
245 rlen = hdr->it_len; /* total space in rtap_header */
246
247 /* If a precence word has the IEEE80211_RADIOTAP_EXT bit set it indicates
248 * that there is another precence word.
249 * Step over the presence words until we find the end of the list
250 */
251 present_word = &hdr->it_present;
252 /* remaining length in header past it_present */
253 rlen -= sizeof(struct ieee80211_radiotap_header);
254
255 while ((*present_word & (1<<IEEE80211_RADIOTAP_EXT)) && rlen >= 4) {
256 present_word++;
257 rlen -= 4; /* account for 4 bytes of present_word */
258 }
259
260 rtap->hdr = hdr;
261 rtap->fields = (uint8*)(present_word + 1);
262 rtap->fields_len = rlen;
263 wl_rtapParseReset(rtap);
264 }
265
266 uint
wl_radiotap_rx(struct dot11_header * mac_header,wl_rxsts_t * rxsts,bsd_header_rx_t * bsd_header)267 wl_radiotap_rx(struct dot11_header *mac_header, wl_rxsts_t *rxsts, bsd_header_rx_t *bsd_header)
268 {
269 int channel_frequency;
270 uint32 channel_flags;
271 uint8 flags;
272 uint8 *cp;
273 uint pad_len;
274 uint32 field_map;
275 uint16 fc;
276 uint bsd_header_len;
277 uint16 ampdu_flags = 0;
278
279 fc = LTOH16(mac_header->fc);
280 pad_len = 3;
281 field_map = WL_RADIOTAP_PRESENT_BASIC;
282
283 if (CHSPEC_IS2G(rxsts->chanspec)) {
284 channel_flags = IEEE80211_CHAN_2GHZ | IEEE80211_CHAN_DYN;
285 channel_frequency = wf_channel2mhz(wf_chspec_ctlchan(rxsts->chanspec),
286 WF_CHAN_FACTOR_2_4_G);
287 } else if (CHSPEC_IS5G(rxsts->chanspec)) {
288 channel_flags = IEEE80211_CHAN_5GHZ | IEEE80211_CHAN_OFDM;
289 channel_frequency = wf_channel2mhz(wf_chspec_ctlchan(rxsts->chanspec),
290 WF_CHAN_FACTOR_5_G);
291 } else {
292 channel_flags = IEEE80211_CHAN_OFDM;
293 channel_frequency = wf_channel2mhz(wf_chspec_ctlchan(rxsts->chanspec),
294 WF_CHAN_FACTOR_6_G);
295 }
296
297 if ((rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_FIRST) ||
298 (rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_SUB)) {
299
300 ampdu_flags = IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN;
301 }
302
303 flags = IEEE80211_RADIOTAP_F_FCS;
304
305 if (rxsts->preamble == WL_RXS_PREAMBLE_SHORT)
306 flags |= IEEE80211_RADIOTAP_F_SHORTPRE;
307
308 if ((fc & FC_WEP) == FC_WEP)
309 flags |= IEEE80211_RADIOTAP_F_WEP;
310
311 if ((fc & FC_MOREFRAG) == FC_MOREFRAG)
312 flags |= IEEE80211_RADIOTAP_F_FRAG;
313
314 if (rxsts->pkterror & WL_RXS_CRC_ERROR)
315 flags |= IEEE80211_RADIOTAP_F_BADFCS;
316
317 if (rxsts->encoding == WL_RXS_ENCODING_HT)
318 field_map = WL_RADIOTAP_PRESENT_HT;
319 else if (rxsts->encoding == WL_RXS_ENCODING_VHT)
320 field_map = WL_RADIOTAP_PRESENT_VHT;
321
322 bsd_header_len = sizeof(struct wl_radiotap_sna); /* start with sna size */
323 /* Test for signal/noise values and update length and field bitmap */
324 if (rxsts->signal == 0) {
325 field_map &= ~(1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL);
326 pad_len = (pad_len - 1);
327 bsd_header_len--;
328 }
329
330 if (rxsts->noise == 0) {
331 field_map &= ~(1 << IEEE80211_RADIOTAP_DBM_ANTNOISE);
332 pad_len = (pad_len - 1);
333 bsd_header_len--;
334 }
335
336 if (rxsts->encoding == WL_RXS_ENCODING_HT ||
337 rxsts->encoding == WL_RXS_ENCODING_VHT) {
338 struct wl_radiotap_hdr *rtht = &bsd_header->hdr;
339 struct wl_radiotap_ht_tail *tail;
340
341 /*
342 * Header length is complicated due to dynamic
343 * presence of signal and noise fields
344 * and padding for xchannel following
345 * signal/noise/ant.
346 * Start with length of wl_radiotap_ht plus
347 * signal/noise/ant
348 */
349 bsd_header_len += sizeof(struct wl_radiotap_hdr) + pad_len;
350 bsd_header_len += sizeof(struct wl_radiotap_xchan);
351 if (rxsts->nfrmtype == WL_RXS_NFRM_AMPDU_FIRST ||
352 rxsts->nfrmtype == WL_RXS_NFRM_AMPDU_SUB) {
353 bsd_header_len += sizeof(struct wl_radiotap_ampdu);
354 }
355 /* add the length of the tail end of the structure */
356 if (rxsts->encoding == WL_RXS_ENCODING_HT)
357 bsd_header_len += sizeof(struct wl_htmcs);
358 else if (rxsts->encoding == WL_RXS_ENCODING_VHT)
359 bsd_header_len += sizeof(struct wl_vhtmcs);
360 bzero((uint8 *)rtht, sizeof(*rtht));
361
362 rtht->ieee_radiotap.it_version = 0;
363 rtht->ieee_radiotap.it_pad = 0;
364 rtht->ieee_radiotap.it_len = (uint16)HTOL16(bsd_header_len);
365 rtht->ieee_radiotap.it_present = HTOL32(field_map);
366
367 rtht->tsft = HTOL64((uint64)rxsts->mactime);
368 rtht->flags = flags;
369 rtht->channel_freq = (uint16)HTOL16(channel_frequency);
370 rtht->channel_flags = (uint16)HTOL16(channel_flags);
371
372 cp = bsd_header->pad;
373 /* add in signal/noise/ant */
374 if (rxsts->signal != 0) {
375 *cp++ = (int8)rxsts->signal;
376 pad_len--;
377 }
378 if (rxsts->noise != 0) {
379 *cp++ = (int8)rxsts->noise;
380 pad_len--;
381 }
382 *cp++ = (int8)rxsts->antenna;
383 pad_len--;
384
385 tail = (struct wl_radiotap_ht_tail *)(bsd_header->ht);
386 /* Fill in XCHANNEL */
387 if (CHSPEC_IS40(rxsts->chanspec)) {
388 if (CHSPEC_SB_UPPER(rxsts->chanspec))
389 channel_flags |= IEEE80211_CHAN_HT40D;
390 else
391 channel_flags |= IEEE80211_CHAN_HT40U;
392 } else
393 channel_flags |= IEEE80211_CHAN_HT20;
394
395 tail->xc.xchannel_flags = HTOL32(channel_flags);
396 tail->xc.xchannel_freq = (uint16)HTOL16(channel_frequency);
397 tail->xc.xchannel_channel = wf_chspec_ctlchan(rxsts->chanspec);
398 tail->xc.xchannel_maxpower = (17*2);
399 /* fill in A-mpdu Status */
400 tail->ampdu.ref_num = mac_header->seq;
401 tail->ampdu.flags = ampdu_flags;
402 tail->ampdu.delimiter_crc = 0;
403 tail->ampdu.reserved = 0;
404
405 if (rxsts->encoding == WL_RXS_ENCODING_HT) {
406 tail->u.ht.mcs_index = rxsts->mcs;
407 tail->u.ht.mcs_known = (IEEE80211_RADIOTAP_MCS_HAVE_BW |
408 IEEE80211_RADIOTAP_MCS_HAVE_MCS |
409 IEEE80211_RADIOTAP_MCS_HAVE_GI |
410 IEEE80211_RADIOTAP_MCS_HAVE_FEC |
411 IEEE80211_RADIOTAP_MCS_HAVE_FMT);
412 tail->u.ht.mcs_flags = 0;
413
414 switch (rxsts->htflags & WL_RXS_HTF_BW_MASK) {
415 case WL_RXS_HTF_20L:
416 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_20L;
417 break;
418 case WL_RXS_HTF_20U:
419 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_20U;
420 break;
421 case WL_RXS_HTF_40:
422 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_40;
423 break;
424 default:
425 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_20;
426 break;
427 }
428
429 if (rxsts->htflags & WL_RXS_HTF_SGI)
430 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_SGI;
431 if (rxsts->preamble & WL_RXS_PREAMBLE_HT_GF)
432 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_FMT_GF;
433 if (rxsts->htflags & WL_RXS_HTF_LDPC)
434 tail->u.ht.mcs_flags |= IEEE80211_RADIOTAP_MCS_FEC_LDPC;
435 } else if (rxsts->encoding == WL_RXS_ENCODING_VHT) {
436 tail->u.vht.vht_known = (IEEE80211_RADIOTAP_VHT_HAVE_STBC |
437 IEEE80211_RADIOTAP_VHT_HAVE_TXOP_PS |
438 IEEE80211_RADIOTAP_VHT_HAVE_GI |
439 IEEE80211_RADIOTAP_VHT_HAVE_SGI_NSYM_DA |
440 IEEE80211_RADIOTAP_VHT_HAVE_LDPC_EXTRA |
441 IEEE80211_RADIOTAP_VHT_HAVE_BF |
442 IEEE80211_RADIOTAP_VHT_HAVE_BW |
443 IEEE80211_RADIOTAP_VHT_HAVE_GID |
444 IEEE80211_RADIOTAP_VHT_HAVE_PAID);
445
446 tail->u.vht.vht_flags = (uint8)HTOL16(rxsts->vhtflags);
447
448 switch (rxsts->bw) {
449 case WL_RXS_VHT_BW_20:
450 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20;
451 break;
452 case WL_RXS_VHT_BW_40:
453 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_40;
454 break;
455 case WL_RXS_VHT_BW_20L:
456 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20L;
457 break;
458 case WL_RXS_VHT_BW_20U:
459 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20U;
460 break;
461 case WL_RXS_VHT_BW_80:
462 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_80;
463 break;
464 case WL_RXS_VHT_BW_40L:
465 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_40L;
466 break;
467 case WL_RXS_VHT_BW_40U:
468 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_40U;
469 break;
470 case WL_RXS_VHT_BW_20LL:
471 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20LL;
472 break;
473 case WL_RXS_VHT_BW_20LU:
474 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20LU;
475 break;
476 case WL_RXS_VHT_BW_20UL:
477 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20UL;
478 break;
479 case WL_RXS_VHT_BW_20UU:
480 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20UU;
481 break;
482 default:
483 tail->u.vht.vht_bw = IEEE80211_RADIOTAP_VHT_BW_20;
484 break;
485 }
486
487 tail->u.vht.vht_mcs_nss[0] = (rxsts->mcs << 4) |
488 (rxsts->nss & IEEE80211_RADIOTAP_VHT_NSS);
489 tail->u.vht.vht_mcs_nss[1] = 0;
490 tail->u.vht.vht_mcs_nss[2] = 0;
491 tail->u.vht.vht_mcs_nss[3] = 0;
492
493 tail->u.vht.vht_coding = rxsts->coding;
494 tail->u.vht.vht_group_id = rxsts->gid;
495 tail->u.vht.vht_partial_aid = HTOL16(rxsts->aid);
496 }
497 } else {
498 struct wl_radiotap_hdr *rtl = &bsd_header->hdr;
499
500 /*
501 * Header length is complicated due to dynamic presence of signal and noise fields
502 * Start with length of wl_radiotap_legacy plus signal/noise/ant
503 */
504 bsd_header_len = sizeof(struct wl_radiotap_hdr) + pad_len;
505 bzero((uint8 *)rtl, sizeof(*rtl));
506
507 rtl->ieee_radiotap.it_version = 0;
508 rtl->ieee_radiotap.it_pad = 0;
509 rtl->ieee_radiotap.it_len = (uint16)HTOL16(bsd_header_len);
510 rtl->ieee_radiotap.it_present = HTOL32(field_map);
511
512 rtl->tsft = HTOL64((uint64)rxsts->mactime);
513 rtl->flags = flags;
514 rtl->u.rate = (uint8)rxsts->datarate;
515 rtl->channel_freq = (uint16)HTOL16(channel_frequency);
516 rtl->channel_flags = (uint16)HTOL16(channel_flags);
517
518 /* add in signal/noise/ant */
519 cp = bsd_header->pad;
520 if (rxsts->signal != 0)
521 *cp++ = (int8)rxsts->signal;
522 if (rxsts->noise != 0)
523 *cp++ = (int8)rxsts->noise;
524 *cp++ = (int8)rxsts->antenna;
525 }
526 return bsd_header_len;
527 }
528
529 static int
wl_radiotap_rx_channel_frequency(wl_rxsts_t * rxsts)530 wl_radiotap_rx_channel_frequency(wl_rxsts_t *rxsts)
531 {
532 if (CHSPEC_IS2G(rxsts->chanspec)) {
533 return wf_channel2mhz(wf_chspec_ctlchan(rxsts->chanspec),
534 WF_CHAN_FACTOR_2_4_G);
535 } else if (CHSPEC_IS5G(rxsts->chanspec)) {
536 return wf_channel2mhz(wf_chspec_ctlchan(rxsts->chanspec),
537 WF_CHAN_FACTOR_5_G);
538 } else {
539 return wf_channel2mhz(wf_chspec_ctlchan(rxsts->chanspec),
540 WF_CHAN_FACTOR_6_G);
541 }
542 }
543
544 static uint16
wl_radiotap_rx_channel_flags(wl_rxsts_t * rxsts)545 wl_radiotap_rx_channel_flags(wl_rxsts_t *rxsts)
546 {
547 if (CHSPEC_IS2G(rxsts->chanspec)) {
548 return (IEEE80211_CHAN_2GHZ | IEEE80211_CHAN_DYN);
549 } else if (CHSPEC_IS5G(rxsts->chanspec)) {
550 return (IEEE80211_CHAN_5GHZ | IEEE80211_CHAN_OFDM);
551 } else {
552 return (IEEE80211_CHAN_OFDM);
553 }
554 }
555
556 static uint8
wl_radiotap_rx_flags(struct dot11_header * mac_header,wl_rxsts_t * rxsts)557 wl_radiotap_rx_flags(struct dot11_header *mac_header, wl_rxsts_t *rxsts)
558 {
559 uint8 flags;
560 uint16 fc;
561
562 fc = ltoh16(mac_header->fc);
563
564 flags = IEEE80211_RADIOTAP_F_FCS;
565
566 if (rxsts->preamble == WL_RXS_PREAMBLE_SHORT)
567 flags |= IEEE80211_RADIOTAP_F_SHORTPRE;
568
569 if (fc & FC_WEP)
570 flags |= IEEE80211_RADIOTAP_F_WEP;
571
572 if (fc & FC_MOREFRAG)
573 flags |= IEEE80211_RADIOTAP_F_FRAG;
574
575 return flags;
576 }
577
578 uint
wl_radiotap_rx_legacy(struct dot11_header * mac_header,wl_rxsts_t * rxsts,ieee80211_radiotap_header_t * rtap_hdr)579 wl_radiotap_rx_legacy(struct dot11_header *mac_header,
580 wl_rxsts_t *rxsts, ieee80211_radiotap_header_t *rtap_hdr)
581 {
582 int channel_frequency;
583 uint16 channel_flags;
584 uint8 flags;
585 uint16 rtap_len = LTOH16(rtap_hdr->it_len);
586 wl_radiotap_legacy_t *rtl = (wl_radiotap_legacy_t *)((uint8*)rtap_hdr + rtap_len);
587
588 rtap_len += sizeof(wl_radiotap_legacy_t);
589 rtap_hdr->it_len = HTOL16(rtap_len);
590 rtap_hdr->it_present |= HTOL32(WL_RADIOTAP_PRESENT_LEGACY);
591
592 channel_frequency = (uint16)wl_radiotap_rx_channel_frequency(rxsts);
593 channel_flags = wl_radiotap_rx_channel_flags(rxsts);
594 flags = wl_radiotap_rx_flags(mac_header, rxsts);
595
596 rtl->basic.tsft_l = HTOL32(rxsts->mactime);
597 rtl->basic.tsft_h = 0;
598 rtl->basic.flags = flags;
599 rtl->basic.rate = (uint8)rxsts->datarate;
600 rtl->basic.channel_freq = (uint16)HTOL16(channel_frequency);
601 rtl->basic.channel_flags = HTOL16(channel_flags);
602 rtl->basic.signal = (int8)rxsts->signal;
603 rtl->basic.noise = (int8)rxsts->noise;
604 rtl->basic.antenna = (int8)rxsts->antenna;
605
606 return 0;
607 }
608
609 uint
wl_radiotap_rx_ht(struct dot11_header * mac_header,wl_rxsts_t * rxsts,ieee80211_radiotap_header_t * rtap_hdr)610 wl_radiotap_rx_ht(struct dot11_header *mac_header,
611 wl_rxsts_t *rxsts, ieee80211_radiotap_header_t *rtap_hdr)
612 {
613 int channel_frequency;
614 uint16 channel_flags;
615 uint32 xchannel_flags;
616 uint8 flags;
617
618 uint16 rtap_len = LTOH16(rtap_hdr->it_len);
619 wl_radiotap_ht_t *rtht = (wl_radiotap_ht_t *)((uint8*)rtap_hdr + rtap_len);
620
621 rtap_len += sizeof(wl_radiotap_ht_t);
622 rtap_hdr->it_len = HTOL16(rtap_len);
623 rtap_hdr->it_present |= HTOL32(WL_RADIOTAP_PRESENT_HT);
624
625 channel_frequency = (uint16)wl_radiotap_rx_channel_frequency(rxsts);
626 channel_flags = wl_radiotap_rx_channel_flags(rxsts);
627 flags = wl_radiotap_rx_flags(mac_header, rxsts);
628
629 rtht->basic.tsft_l = HTOL32(rxsts->mactime);
630 rtht->basic.tsft_h = 0;
631 rtht->basic.flags = flags;
632 rtht->basic.channel_freq = (uint16)HTOL16(channel_frequency);
633 rtht->basic.channel_flags = HTOL16(channel_flags);
634 rtht->basic.signal = (int8)rxsts->signal;
635 rtht->basic.noise = (int8)rxsts->noise;
636 rtht->basic.antenna = (uint8)rxsts->antenna;
637
638 /* xchannel */
639 xchannel_flags = (uint32)channel_flags;
640 if (CHSPEC_IS40(rxsts->chanspec)) {
641 if (CHSPEC_SB_UPPER(rxsts->chanspec))
642 xchannel_flags |= IEEE80211_CHAN_HT40D;
643 else {
644 xchannel_flags |= IEEE80211_CHAN_HT40U;
645 }
646 } else {
647 xchannel_flags |= IEEE80211_CHAN_HT20;
648 }
649
650 rtht->xchannel_flags = HTOL32(xchannel_flags);
651 rtht->xchannel_freq = (uint16)HTOL16(channel_frequency);
652 rtht->xchannel_channel = wf_chspec_ctlchan(rxsts->chanspec);
653 rtht->xchannel_maxpower = (17*2);
654
655 /* add standard MCS */
656 rtht->mcs_known = (IEEE80211_RADIOTAP_MCS_HAVE_BW |
657 IEEE80211_RADIOTAP_MCS_HAVE_MCS |
658 IEEE80211_RADIOTAP_MCS_HAVE_GI |
659 IEEE80211_RADIOTAP_MCS_HAVE_FEC |
660 IEEE80211_RADIOTAP_MCS_HAVE_FMT);
661
662 rtht->mcs_flags = 0;
663 switch (rxsts->htflags & WL_RXS_HTF_BW_MASK) {
664 case WL_RXS_HTF_20L:
665 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_20L;
666 break;
667 case WL_RXS_HTF_20U:
668 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_20U;
669 break;
670 case WL_RXS_HTF_40:
671 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_40;
672 break;
673 default:
674 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_BW_20;
675 }
676
677 if (rxsts->htflags & WL_RXS_HTF_SGI) {
678 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_SGI;
679 }
680 if (rxsts->preamble & WL_RXS_PREAMBLE_HT_GF) {
681 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_FMT_GF;
682 }
683 if (rxsts->htflags & WL_RXS_HTF_LDPC) {
684 rtht->mcs_flags |= IEEE80211_RADIOTAP_MCS_FEC_LDPC;
685 }
686 rtht->mcs_index = rxsts->mcs;
687 rtht->ampdu_flags = 0;
688 rtht->ampdu_delim_crc = 0;
689
690 rtht->ampdu_ref_num = rxsts->ampdu_counter;
691
692 if (!(rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_FIRST) &&
693 !(rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_SUB)) {
694 rtht->ampdu_flags |= IEEE80211_RADIOTAP_AMPDU_IS_LAST;
695 } else {
696 rtht->ampdu_flags |= IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN;
697 }
698 return 0;
699 }
700
701 uint
wl_radiotap_rx_vht(struct dot11_header * mac_header,wl_rxsts_t * rxsts,ieee80211_radiotap_header_t * rtap_hdr)702 wl_radiotap_rx_vht(struct dot11_header *mac_header,
703 wl_rxsts_t *rxsts, ieee80211_radiotap_header_t *rtap_hdr)
704 {
705 int channel_frequency;
706 uint16 channel_flags;
707 uint8 flags;
708
709 uint16 rtap_len = LTOH16(rtap_hdr->it_len);
710 wl_radiotap_vht_t *rtvht = (wl_radiotap_vht_t *)((uint8*)rtap_hdr + rtap_len);
711
712 rtap_len += sizeof(wl_radiotap_vht_t);
713 rtap_hdr->it_len = HTOL16(rtap_len);
714 rtap_hdr->it_present |= HTOL32(WL_RADIOTAP_PRESENT_VHT);
715
716 channel_frequency = (uint16)wl_radiotap_rx_channel_frequency(rxsts);
717 channel_flags = wl_radiotap_rx_channel_flags(rxsts);
718 flags = wl_radiotap_rx_flags(mac_header, rxsts);
719
720 rtvht->basic.tsft_l = HTOL32(rxsts->mactime);
721 rtvht->basic.tsft_h = 0;
722 rtvht->basic.flags = flags;
723 rtvht->basic.channel_freq = (uint16)HTOL16(channel_frequency);
724 rtvht->basic.channel_flags = HTOL16(channel_flags);
725 rtvht->basic.signal = (int8)rxsts->signal;
726 rtvht->basic.noise = (int8)rxsts->noise;
727 rtvht->basic.antenna = (uint8)rxsts->antenna;
728
729 rtvht->vht_known = (IEEE80211_RADIOTAP_VHT_HAVE_STBC |
730 IEEE80211_RADIOTAP_VHT_HAVE_TXOP_PS |
731 IEEE80211_RADIOTAP_VHT_HAVE_GI |
732 IEEE80211_RADIOTAP_VHT_HAVE_SGI_NSYM_DA |
733 IEEE80211_RADIOTAP_VHT_HAVE_LDPC_EXTRA |
734 IEEE80211_RADIOTAP_VHT_HAVE_BF |
735 IEEE80211_RADIOTAP_VHT_HAVE_BW |
736 IEEE80211_RADIOTAP_VHT_HAVE_GID |
737 IEEE80211_RADIOTAP_VHT_HAVE_PAID);
738
739 STATIC_ASSERT(WL_RXS_VHTF_STBC ==
740 IEEE80211_RADIOTAP_VHT_STBC);
741 STATIC_ASSERT(WL_RXS_VHTF_TXOP_PS ==
742 IEEE80211_RADIOTAP_VHT_TXOP_PS);
743 STATIC_ASSERT(WL_RXS_VHTF_SGI ==
744 IEEE80211_RADIOTAP_VHT_SGI);
745 STATIC_ASSERT(WL_RXS_VHTF_SGI_NSYM_DA ==
746 IEEE80211_RADIOTAP_VHT_SGI_NSYM_DA);
747 STATIC_ASSERT(WL_RXS_VHTF_LDPC_EXTRA ==
748 IEEE80211_RADIOTAP_VHT_LDPC_EXTRA);
749 STATIC_ASSERT(WL_RXS_VHTF_BF ==
750 IEEE80211_RADIOTAP_VHT_BF);
751
752 rtvht->vht_flags = (uint8)HTOL16(rxsts->vhtflags);
753
754 STATIC_ASSERT(WL_RXS_VHT_BW_20 ==
755 IEEE80211_RADIOTAP_VHT_BW_20);
756 STATIC_ASSERT(WL_RXS_VHT_BW_40 ==
757 IEEE80211_RADIOTAP_VHT_BW_40);
758 STATIC_ASSERT(WL_RXS_VHT_BW_20L ==
759 IEEE80211_RADIOTAP_VHT_BW_20L);
760 STATIC_ASSERT(WL_RXS_VHT_BW_20U ==
761 IEEE80211_RADIOTAP_VHT_BW_20U);
762 STATIC_ASSERT(WL_RXS_VHT_BW_80 ==
763 IEEE80211_RADIOTAP_VHT_BW_80);
764 STATIC_ASSERT(WL_RXS_VHT_BW_40L ==
765 IEEE80211_RADIOTAP_VHT_BW_40L);
766 STATIC_ASSERT(WL_RXS_VHT_BW_40U ==
767 IEEE80211_RADIOTAP_VHT_BW_40U);
768 STATIC_ASSERT(WL_RXS_VHT_BW_20LL ==
769 IEEE80211_RADIOTAP_VHT_BW_20LL);
770 STATIC_ASSERT(WL_RXS_VHT_BW_20LU ==
771 IEEE80211_RADIOTAP_VHT_BW_20LU);
772 STATIC_ASSERT(WL_RXS_VHT_BW_20UL ==
773 IEEE80211_RADIOTAP_VHT_BW_20UL);
774 STATIC_ASSERT(WL_RXS_VHT_BW_20UU ==
775 IEEE80211_RADIOTAP_VHT_BW_20UU);
776
777 rtvht->vht_bw = rxsts->bw;
778
779 rtvht->vht_mcs_nss[0] = (rxsts->mcs << 4) |
780 (rxsts->nss & IEEE80211_RADIOTAP_VHT_NSS);
781 rtvht->vht_mcs_nss[1] = 0;
782 rtvht->vht_mcs_nss[2] = 0;
783 rtvht->vht_mcs_nss[3] = 0;
784
785 STATIC_ASSERT(WL_RXS_VHTF_CODING_LDCP ==
786 IEEE80211_RADIOTAP_VHT_CODING_LDPC);
787
788 rtvht->vht_coding = rxsts->coding;
789 rtvht->vht_group_id = rxsts->gid;
790 rtvht->vht_partial_aid = HTOL16(rxsts->aid);
791
792 rtvht->ampdu_flags = 0;
793 rtvht->ampdu_delim_crc = 0;
794 rtvht->ampdu_ref_num = HTOL32(rxsts->ampdu_counter);
795 if (!(rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_FIRST) &&
796 !(rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_SUB)) {
797 rtvht->ampdu_flags |= HTOL16(IEEE80211_RADIOTAP_AMPDU_IS_LAST);
798 } else {
799 rtvht->ampdu_flags |= HTOL16(IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN);
800 }
801
802 return 0;
803 }
804
805 /* Rx status to radiotap conversion of HE type */
806 uint
wl_radiotap_rx_he(struct dot11_header * mac_header,wl_rxsts_t * rxsts,ieee80211_radiotap_header_t * rtap_hdr)807 wl_radiotap_rx_he(struct dot11_header *mac_header, wl_rxsts_t *rxsts,
808 ieee80211_radiotap_header_t *rtap_hdr)
809 {
810 int channel_frequency;
811 uint16 channel_flags;
812 uint8 flags;
813 uint16 rtap_len = LTOH16(rtap_hdr->it_len);
814 wl_radiotap_he_t *rthe = (wl_radiotap_he_t *)((uint8*)rtap_hdr + rtap_len);
815
816 rtap_len += sizeof(wl_radiotap_he_t);
817 rtap_hdr->it_len = HTOL16(rtap_len);
818 rtap_hdr->it_present |= HTOL32(WL_RADIOTAP_PRESENT_HE);
819
820 channel_frequency = (uint16)wl_radiotap_rx_channel_frequency(rxsts);
821 channel_flags = wl_radiotap_rx_channel_flags(rxsts);
822 flags = wl_radiotap_rx_flags(mac_header, rxsts);
823
824 rthe->basic.tsft_l = HTOL32(rxsts->mactime);
825 rthe->basic.tsft_h = 0;
826 rthe->basic.flags = flags;
827 rthe->basic.channel_freq = (uint16)HTOL16(channel_frequency);
828 rthe->basic.channel_flags = HTOL16(channel_flags);
829 rthe->basic.signal = (int8)rxsts->signal;
830 rthe->basic.noise = (int8)rxsts->noise;
831 rthe->basic.antenna = (uint8)rxsts->antenna;
832
833 rthe->ampdu_flags = 0;
834 rthe->ampdu_delim_crc = 0;
835 rthe->ampdu_ref_num = HTOL32(rxsts->ampdu_counter);
836 if (!(rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_FIRST) &&
837 !(rxsts->nfrmtype & WL_RXS_NFRM_AMPDU_SUB)) {
838 rthe->ampdu_flags |= HTOL16(IEEE80211_RADIOTAP_AMPDU_IS_LAST);
839 } else {
840 rthe->ampdu_flags |= HTOL16(IEEE80211_RADIOTAP_AMPDU_LAST_KNOWN);
841 }
842
843 rthe->data1 = HTOL16(rxsts->data1);
844 rthe->data2 = HTOL16(rxsts->data2);
845 rthe->data3 = HTOL16(rxsts->data3);
846 rthe->data4 = HTOL16(rxsts->data4);
847 rthe->data5 = HTOL16(rxsts->data5);
848 rthe->data6 = HTOL16(rxsts->data6);
849
850 return 0;
851 }
852
853 /* Rx status to radiotap conversion of EHT type */
854 uint
wl_radiotap_rx_eht(struct dot11_header * mac_header,wl_rxsts_t * rxsts,ieee80211_radiotap_header_t * rtap_hdr)855 wl_radiotap_rx_eht(struct dot11_header *mac_header, wl_rxsts_t *rxsts,
856 ieee80211_radiotap_header_t *rtap_hdr)
857 {
858 ASSERT(!"wl_radiotap_rx_eht: not implemented!");
859 return 0;
860 }
861
862 uint16
wl_rxsts_to_rtap(monitor_pkt_rxsts_t * pkt_rxsts,void * payload,uint16 len,void * pout,uint16 pad_req)863 wl_rxsts_to_rtap(monitor_pkt_rxsts_t *pkt_rxsts, void *payload,
864 uint16 len, void* pout, uint16 pad_req)
865 {
866 uint16 rtap_len = 0;
867 struct dot11_header* mac_header;
868 uint8* p = payload;
869 ieee80211_radiotap_header_t* rtap_hdr = (ieee80211_radiotap_header_t*)pout;
870 wl_rxsts_t* rxsts;
871
872 ASSERT(p && pkt_rxsts);
873 rxsts = pkt_rxsts->rxsts;
874 rtap_hdr->it_version = 0;
875 rtap_hdr->it_pad = 0;
876 rtap_hdr->it_len = HTOL16(sizeof(*rtap_hdr));
877 rtap_hdr->it_present = 0;
878 bitmap = 0;
879
880 #ifdef MONITOR_DNGL_CONV
881 if (pad_req) {
882 radiotap_add_vendor_ns(rtap_hdr);
883 }
884 #endif
885
886 #ifdef BCM_MON_QDBM_RSSI
887 /* if per-core RSSI is present, add vendor element */
888 if (pkt_rxsts->corenum != 0) {
889 radiotap_add_vendor_ns(rtap_hdr);
890 }
891 #endif
892 mac_header = (struct dot11_header *)(p);
893
894 if (rxsts->encoding == WL_RXS_ENCODING_EHT) {
895 wl_radiotap_rx_eht(mac_header, rxsts, rtap_hdr);
896 } else if (rxsts->encoding == WL_RXS_ENCODING_HE) {
897 wl_radiotap_rx_he(mac_header, rxsts, rtap_hdr);
898 } else if (rxsts->encoding == WL_RXS_ENCODING_VHT) {
899 wl_radiotap_rx_vht(mac_header, rxsts, rtap_hdr);
900 } else if (rxsts->encoding == WL_RXS_ENCODING_HT) {
901 wl_radiotap_rx_ht(mac_header, rxsts, rtap_hdr);
902 } else {
903 uint16 mask = ltoh16(mac_header->fc) & FC_KIND_MASK;
904 if (mask == FC_RTS || mask == FC_CTS) {
905 radiotap_add_vendor_ns(rtap_hdr);
906 }
907 wl_radiotap_rx_legacy(mac_header, rxsts, rtap_hdr);
908 if (mask == FC_RTS || mask == FC_CTS) {
909 radiotap_encode_bw_signaling(mask, rxsts, rtap_hdr);
910 }
911 }
912 #ifdef BCM_MON_QDBM_RSSI
913 /* if per-core RSSI is present, add vendor element */
914 if (pkt_rxsts->corenum != 0) {
915 radiotap_encode_multi_rssi(pkt_rxsts, rtap_hdr);
916 }
917 #endif
918 #ifdef MONITOR_DNGL_CONV
919 if (pad_req) {
920 radiotap_encode_alignpad(rtap_hdr, pad_req);
921 }
922 #endif
923 rtap_len = LTOH16(rtap_hdr->it_len);
924 len += rtap_len;
925
926 #ifndef MONITOR_DNGL_CONV
927 if (len > MAX_MON_PKT_SIZE) {
928 return 0;
929 }
930 /* copy payload */
931 if (!(rxsts->nfrmtype & WL_RXS_NFRM_AMSDU_FIRST) &&
932 !(rxsts->nfrmtype & WL_RXS_NFRM_AMSDU_SUB)) {
933 memcpy((uint8*)pout + rtap_len, (uint8*)p, len - rtap_len);
934 }
935 #endif
936 #ifdef MONITOR_DNGL_CONV
937 return rtap_len;
938 #else
939 return len;
940 #endif
941 }
942
943 #ifdef BCM_MON_QDBM_RSSI
944 void
radiotap_encode_multi_rssi(monitor_pkt_rxsts_t * rxsts,ieee80211_radiotap_header_t * hdr)945 radiotap_encode_multi_rssi(monitor_pkt_rxsts_t* rxsts, ieee80211_radiotap_header_t *hdr)
946 {
947 uint16 cur_len = LTOH16(hdr->it_len);
948 uint16 len = ROUNDUP(1 + rxsts->corenum * sizeof(monitor_pkt_rssi_t), 4);
949 int i = 0;
950 uint8 *vend_p = (uint8 *)hdr + cur_len;
951 radiotap_vendor_ns_t *vendor_ns = (radiotap_vendor_ns_t*)vend_p;
952 memcpy(vendor_ns->vend_oui, brcm_oui, sizeof(vendor_ns->vend_oui));
953 vendor_ns->sns = 1;
954 vendor_ns->skip_len = HTOL16(len);
955 vend_p += sizeof(*vendor_ns);
956 vend_p[0] = rxsts->corenum;
957 for (i = 0; i < rxsts->corenum; i++) {
958 vend_p[2*i + 1] = rxsts->rxpwr[i].dBm;
959 vend_p[2*i + 2] = rxsts->rxpwr[i].decidBm;
960 }
961 hdr->it_len = HTOL16(cur_len + sizeof(radiotap_vendor_ns_t) + len);
962 }
963 #endif /* BCM_CORE_RSSI */
964
965 #ifdef MONITOR_DNGL_CONV
966 #define AILIGN_4BYTES (4u)
967 void
radiotap_encode_alignpad(ieee80211_radiotap_header_t * hdr,uint16 pad_req)968 radiotap_encode_alignpad(ieee80211_radiotap_header_t *hdr, uint16 pad_req)
969 {
970 uint16 cur_len = LTOH16(hdr->it_len);
971 uint8 *vend_p = (uint8 *)hdr + cur_len;
972 radiotap_vendor_ns_t *vendor_ns = (radiotap_vendor_ns_t*)vend_p;
973 uint16 len;
974 uint16 align_pad = 0;
975
976 memcpy(vendor_ns->vend_oui, brcm_oui, sizeof(vendor_ns->vend_oui));
977 vendor_ns->sns = WL_RADIOTAP_BRCM_PAD_SNS;
978 len = cur_len + sizeof(radiotap_vendor_ns_t);
979 if (len % AILIGN_4BYTES != 0) {
980 align_pad = (AILIGN_4BYTES - (len % AILIGN_4BYTES));
981 }
982 hdr->it_len = HTOL16(len + pad_req + align_pad);
983 vendor_ns->skip_len = HTOL16(pad_req + align_pad);
984 }
985 #endif /* MONITOR_DNGL_CONV */
986
987 void
radiotap_encode_bw_signaling(uint16 mask,struct wl_rxsts * rxsts,ieee80211_radiotap_header_t * hdr)988 radiotap_encode_bw_signaling(uint16 mask,
989 struct wl_rxsts* rxsts, ieee80211_radiotap_header_t *hdr)
990 {
991 uint16 cur_len = LTOH16(hdr->it_len);
992 uint8 *vend_p = (uint8 *)hdr + cur_len;
993 radiotap_vendor_ns_t *vendor_ns = (radiotap_vendor_ns_t *)vend_p;
994 wl_radiotap_nonht_vht_t* nonht_vht;
995
996 memcpy(vendor_ns->vend_oui, brcm_oui, sizeof(vendor_ns->vend_oui));
997 vendor_ns->sns = 0;
998 vendor_ns->skip_len = sizeof(wl_radiotap_nonht_vht_t);
999 nonht_vht = (wl_radiotap_nonht_vht_t *)(vend_p + sizeof(*vendor_ns));
1000
1001 /* VHT b/w signalling */
1002 bzero((uint8 *)nonht_vht, sizeof(wl_radiotap_nonht_vht_t));
1003 nonht_vht->len = WL_RADIOTAP_NONHT_VHT_LEN;
1004 nonht_vht->flags |= WL_RADIOTAP_F_NONHT_VHT_BW;
1005 nonht_vht->bw = (uint8)rxsts->bw_nonht;
1006
1007 if (mask == FC_RTS) {
1008 if (rxsts->vhtflags & WL_RXS_VHTF_DYN_BW_NONHT) {
1009 nonht_vht->flags |= WL_RADIOTAP_F_NONHT_VHT_DYN_BW;
1010 }
1011 }
1012 hdr->it_len = HTOL16(cur_len + sizeof(radiotap_vendor_ns_t) +
1013 sizeof(wl_radiotap_nonht_vht_t));
1014 }
1015
1016 void
radiotap_add_vendor_ns(ieee80211_radiotap_header_t * hdr)1017 radiotap_add_vendor_ns(ieee80211_radiotap_header_t *hdr)
1018 {
1019
1020 uint32 * it_present = &hdr->it_present;
1021 uint16 len = LTOH16(hdr->it_len);
1022
1023 /* if the last bitmap has a vendor ns, add a new one */
1024 if (it_present[bitmap] & (1 << IEEE80211_RADIOTAP_VENDOR_NAMESPACE)) {
1025 it_present[bitmap] |= 1 << IEEE80211_RADIOTAP_EXT;
1026 bitmap++;
1027 /* align to 8 bytes */
1028 if (bitmap%2) {
1029 hdr->it_len = HTOL16(len + 8);
1030 }
1031 it_present[bitmap] = 1 << IEEE80211_RADIOTAP_VENDOR_NAMESPACE;
1032 } else {
1033 it_present[bitmap] |= 1 << IEEE80211_RADIOTAP_VENDOR_NAMESPACE;
1034 }
1035 }
1036