1 /*
2 * DHD debugability packet logging support
3 *
4 * Copyright (C) 2020, Broadcom.
5 *
6 * Unless you and Broadcom execute a separate written software license
7 * agreement governing use of this software, this software is licensed to you
8 * under the terms of the GNU General Public License version 2 (the "GPL"),
9 * available at http://www.broadcom.com/licenses/GPLv2.php, with the
10 * following added to such license:
11 *
12 * As a special exception, the copyright holders of this software give you
13 * permission to link this software with independent modules, and to copy and
14 * distribute the resulting executable under terms of your choice, provided that
15 * you also meet, for each linked independent module, the terms and conditions of
16 * the license of that module. An independent module is a module which is not
17 * derived from this software. The special exception does not apply to any
18 * modifications of the software.
19 *
20 *
21 * <<Broadcom-WL-IPTag/Open:>>
22 *
23 * $Id$
24 */
25
26 #include <typedefs.h>
27 #include <osl.h>
28 #include <bcmutils.h>
29 #include <bcmstdlib_s.h>
30 #include <dngl_stats.h>
31 #include <dhd.h>
32 #include <dhd_dbg.h>
33 #include <dhd_pktlog.h>
34 #include <dhd_wlfc.h>
35
36 #ifdef DHD_COMPACT_PKT_LOG
37 #include <bcmip.h>
38 #include <bcmudp.h>
39 #include <bcmdhcp.h>
40 #include <bcmarp.h>
41 #include <bcmicmp.h>
42 #include <bcmtlv.h>
43 #include <802.11.h>
44 #include <eap.h>
45 #include <eapol.h>
46 #include <bcmendian.h>
47 #include <bcm_l2_filter.h>
48 #include <dhd_bitpack.h>
49 #include <bcmipv6.h>
50 #endif /* DHD_COMPACT_PKT_LOG */
51
52 #ifdef DHD_PKT_LOGGING
53 #ifndef strtoul
54 #define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
55 #endif /* strtoul */
56 extern int wl_pattern_atoh(char *src, char *dst);
57 extern int pattern_atoh_len(char *src, char *dst, int len);
58 extern wifi_tx_packet_fate __dhd_dbg_map_tx_status_to_pkt_fate(uint16 status);
59
60 #ifdef DHD_COMPACT_PKT_LOG
61 #define CPKT_LOG_BITS_PER_BYTE 8
62
63 #define CPKT_LOG_BIT_LEN_TYPE 4
64
65 #define CPKT_LOG_BIT_OFFSET_TS 0
66 #define CPKT_LOG_BIT_OFFSET_DIR 5
67 #define CPKT_LOG_BIT_OFFSET_TYPE 6
68 #define CPKT_LOG_BIT_OFFSET_SUBTYPE 10
69 #define CPKT_LOG_BIT_OFFSET_PKT_FATE 18
70
71 #define CPKT_LOG_BIT_MASK_TS 0x1f
72 #define CPKT_LOG_BIT_MASK_DIR 0x01
73 #define CPKT_LOG_BIT_MASK_TYPE 0x0f
74 #define CPKT_LOG_BIT_MASK_SUBTYPE 0xff
75 #define CPKT_LOG_BIT_MASK_PKT_FATE 0x0f
76
77 #define CPKT_LOG_DNS_PORT_CLIENT 53
78 #define CPKT_LOG_MDNS_PORT_CLIENT 5353
79
80 #define CPKT_LOG_TYPE_DNS 0x0
81 #define CPKT_LOG_TYPE_ARP 0x1
82 #define CPKT_LOG_TYPE_ICMP_REQ 0x2
83 #define CPKT_LOG_TYPE_ICMP_RES 0x3
84 #define CPKT_LOG_TYPE_ICMP_UNREACHABLE 0x4
85 #define CPKT_LOG_TYPE_DHCP 0x5
86 #define CPKT_LOG_TYPE_802_1X 0x6
87 #define CPKT_LOG_TYPE_ICMPv6 0x7
88 #define CPKT_LOG_TYPE_OTHERS 0xf
89
90 #define CPKT_LOG_802_1X_SUBTYPE_IDENTITY 0x0
91 #define CPKT_LOG_802_1X_SUBTYPE_TLS 0x1
92 #define CPKT_LOG_802_1X_SUBTYPE_TTLS 0x2
93 #define CPKT_LOG_802_1X_SUBTYPE_PEAP 0x3
94 #define CPKT_LOG_802_1X_SUBTYPE_FAST 0x4
95 #define CPKT_LOG_802_1X_SUBTYPE_LEAP 0x5
96 #define CPKT_LOG_802_1X_SUBTYPE_PWD 0x6
97 #define CPKT_LOG_802_1X_SUBTYPE_SIM 0x7
98 #define CPKT_LOG_802_1X_SUBTYPE_AKA 0x8
99 #define CPKT_LOG_802_1X_SUBTYPE_AKAP 0x9
100 #define CPKT_LOG_802_1X_SUBTYPE_SUCCESS 0xA
101 #define CPKT_LOG_802_1X_SUBTYPE_4WAY_M1 0xB
102 #define CPKT_LOG_802_1X_SUBTYPE_4WAY_M2 0xC
103 #define CPKT_LOG_802_1X_SUBTYPE_4WAY_M3 0xD
104 #define CPKT_LOG_802_1X_SUBTYPE_4WAY_M4 0xE
105 #define CPKT_LOG_802_1X_SUBTYPE_OTHERS 0xF
106
107 #define CPKT_LOG_DHCP_MAGIC_COOKIE_LEN 4
108
109 #define CPKT_LOG_ICMP_TYPE_DEST_UNREACHABLE 3
110 #define CPKT_LOG_ICMP_TYPE_DEST_UNREACHABLE_IPV4_OFFSET 4
111
112 typedef struct dhd_cpkt_log_ts_node {
113 struct rb_node rb;
114
115 uint64 ts_diff; /* key, usec */
116 int idx;
117 } dhd_cpkt_log_ts_node_t;
118
119 /* Compact Packet Log Timestamp values, unit: uSec */
120 const uint64 dhd_cpkt_log_tt_idx[] = {
121 10000, 50000, 100000, 150000, 300000, 500000, 750000, 1000000, 3000000, 5000000, 7500000,
122 10000000, 12500000, 15000000, 17500000, 20000000, 22500000, 25000000, 27500000, 30000000,
123 32500000, 35000000, 37500000, 40000000, 50000000, 75000000, 150000000, 300000000, 400000000,
124 500000000, 600000000
125 };
126 #define CPKT_LOG_TT_IDX_ARR_SZ ARRAYSIZE(dhd_cpkt_log_tt_idx)
127
128 static int dhd_cpkt_log_init_tt(dhd_pub_t *dhdp);
129 static void dhd_cpkt_log_deinit_tt(dhd_pub_t *dhdp);
130 #endif /* DHD_COMPACT_PKT_LOG */
131
132 int
dhd_os_attach_pktlog(dhd_pub_t * dhdp)133 dhd_os_attach_pktlog(dhd_pub_t *dhdp)
134 {
135 dhd_pktlog_t *pktlog;
136
137 if (!dhdp) {
138 DHD_ERROR(("%s(): dhdp is NULL\n", __FUNCTION__));
139 return -EINVAL;
140 }
141
142 pktlog = (dhd_pktlog_t *)MALLOCZ(dhdp->osh, sizeof(dhd_pktlog_t));
143 if (unlikely(!pktlog)) {
144 DHD_ERROR(("%s(): could not allocate memory for - "
145 "dhd_pktlog_t\n", __FUNCTION__));
146 return BCME_ERROR;
147 }
148
149 dhdp->pktlog = pktlog;
150 pktlog->dhdp = dhdp;
151
152 OSL_ATOMIC_INIT(dhdp->osh, &pktlog->pktlog_status);
153
154 /* pktlog ring */
155 dhdp->pktlog->pktlog_ring = dhd_pktlog_ring_init(dhdp, MIN_PKTLOG_LEN);
156 dhdp->pktlog->pktlog_filter = dhd_pktlog_filter_init(MAX_DHD_PKTLOG_FILTER_LEN);
157 #ifdef DHD_COMPACT_PKT_LOG
158 dhd_cpkt_log_init_tt(dhdp);
159 #endif
160
161 DHD_ERROR(("%s(): dhd_os_attach_pktlog attach\n", __FUNCTION__));
162
163 return BCME_OK;
164 }
165
166 int
dhd_os_detach_pktlog(dhd_pub_t * dhdp)167 dhd_os_detach_pktlog(dhd_pub_t *dhdp)
168 {
169 if (!dhdp || !dhdp->pktlog) {
170 DHD_PKT_LOG(("%s(): dhdp=%p pktlog=%p\n",
171 __FUNCTION__, dhdp, (dhdp ? dhdp->pktlog : NULL)));
172 return -EINVAL;
173 }
174
175 dhd_pktlog_ring_deinit(dhdp, dhdp->pktlog->pktlog_ring);
176 dhd_pktlog_filter_deinit(dhdp->pktlog->pktlog_filter);
177 #ifdef DHD_COMPACT_PKT_LOG
178 dhd_cpkt_log_deinit_tt(dhdp);
179 #endif /* DHD_COMPACT_PKT_LOG */
180
181 DHD_ERROR(("%s(): dhd_os_attach_pktlog detach\n", __FUNCTION__));
182
183 MFREE(dhdp->osh, dhdp->pktlog, sizeof(dhd_pktlog_t));
184
185 return BCME_OK;
186 }
187
188 dhd_pktlog_ring_t*
dhd_pktlog_ring_init(dhd_pub_t * dhdp,int size)189 dhd_pktlog_ring_init(dhd_pub_t *dhdp, int size)
190 {
191 dhd_pktlog_ring_t *ring;
192 int i = 0;
193
194 if (!dhdp) {
195 DHD_ERROR(("%s(): dhdp is NULL\n", __FUNCTION__));
196 return NULL;
197 }
198
199 ring = (dhd_pktlog_ring_t *)MALLOCZ(dhdp->osh, sizeof(dhd_pktlog_ring_t));
200 if (unlikely(!ring)) {
201 DHD_ERROR(("%s(): could not allocate memory for - "
202 "dhd_pktlog_ring_t\n", __FUNCTION__));
203 goto fail;
204 }
205
206 dll_init(&ring->ring_info_head);
207 dll_init(&ring->ring_info_free);
208
209 ring->ring_info_mem = (dhd_pktlog_ring_info_t *)MALLOCZ(dhdp->osh,
210 sizeof(dhd_pktlog_ring_info_t) * size);
211 if (unlikely(!ring->ring_info_mem)) {
212 DHD_ERROR(("%s(): could not allocate memory for - "
213 "dhd_pktlog_ring_info_t\n", __FUNCTION__));
214 goto fail;
215 }
216
217 /* initialize free ring_info linked list */
218 for (i = 0; i < size; i++) {
219 dll_append(&ring->ring_info_free, (dll_t *)&ring->ring_info_mem[i].p_info);
220 }
221
222 OSL_ATOMIC_SET(dhdp->osh, &ring->start, TRUE);
223 ring->pktlog_minmize = FALSE;
224 ring->pktlog_len = size;
225 ring->pktcount = 0;
226 ring->dhdp = dhdp;
227 ring->pktlog_ring_lock = osl_spin_lock_init(dhdp->osh);
228
229 DHD_ERROR(("%s(): pktlog ring init success\n", __FUNCTION__));
230
231 return ring;
232 fail:
233 if (ring) {
234 MFREE(dhdp->osh, ring, sizeof(dhd_pktlog_ring_t));
235 }
236
237 return NULL;
238 }
239
240 /* Maximum wait counts */
241 #define DHD_PKTLOG_WAIT_MAXCOUNT 1000
242 int
dhd_pktlog_ring_deinit(dhd_pub_t * dhdp,dhd_pktlog_ring_t * ring)243 dhd_pktlog_ring_deinit(dhd_pub_t *dhdp, dhd_pktlog_ring_t *ring)
244 {
245 int ret = BCME_OK;
246 dhd_pktlog_ring_info_t *ring_info;
247 dll_t *item, *next_p;
248 int waitcounts = 0;
249
250 if (!ring) {
251 DHD_ERROR(("%s(): ring is NULL\n", __FUNCTION__));
252 return -EINVAL;
253 }
254
255 if (!ring->dhdp) {
256 DHD_ERROR(("%s(): dhdp is NULL\n", __FUNCTION__));
257 return -EINVAL;
258 }
259
260 /* stop pkt log */
261 OSL_ATOMIC_SET(dhdp->osh, &ring->start, FALSE);
262
263 /* waiting TX/RX/TXS context is done, max timeout 1 second */
264 while ((waitcounts++ < DHD_PKTLOG_WAIT_MAXCOUNT)) {
265 if (!OSL_ATOMIC_READ(dhdp->osh, &dhdp->pktlog->pktlog_status))
266 break;
267 OSL_SLEEP(1);
268 }
269
270 if (waitcounts >= DHD_PKTLOG_WAIT_MAXCOUNT) {
271 DHD_ERROR(("%s(): pktlog wait timeout pktlog_status : 0x%x \n",
272 __FUNCTION__,
273 OSL_ATOMIC_READ(dhdp->osh, &dhdp->pktlog->pktlog_status)));
274 ASSERT(0);
275 return -EINVAL;
276 }
277
278 /* free ring_info->info.pkt */
279 for (item = dll_head_p(&ring->ring_info_head); !dll_end(&ring->ring_info_head, item);
280 item = next_p) {
281 next_p = dll_next_p(item);
282
283 ring_info = (dhd_pktlog_ring_info_t *)item;
284
285 if (ring_info->info.pkt) {
286 PKTFREE(ring->dhdp->osh, ring_info->info.pkt, TRUE);
287 DHD_PKT_LOG(("%s(): pkt free pos %p\n",
288 __FUNCTION__, ring_info->info.pkt));
289 }
290 }
291
292 if (ring->ring_info_mem) {
293 MFREE(ring->dhdp->osh, ring->ring_info_mem,
294 sizeof(dhd_pktlog_ring_info_t) * ring->pktlog_len);
295 }
296
297 if (ring->pktlog_ring_lock) {
298 osl_spin_lock_deinit(ring->dhdp->osh, ring->pktlog_ring_lock);
299 }
300
301 MFREE(dhdp->osh, ring, sizeof(dhd_pktlog_ring_t));
302
303 DHD_ERROR(("%s(): pktlog ring deinit\n", __FUNCTION__));
304
305 return ret;
306 }
307
308 /*
309 * dhd_pktlog_ring_add_pkts : add filtered packets into pktlog ring
310 * pktid : incase of rx, pktid is not used (pass DHD_INVALID_PKID)
311 * direction : 1 - TX / 0 - RX / 2 - RX Wakeup Packet
312 */
313 int
dhd_pktlog_ring_add_pkts(dhd_pub_t * dhdp,void * pkt,void * pktdata,uint32 pktid,uint32 direction)314 dhd_pktlog_ring_add_pkts(dhd_pub_t *dhdp, void *pkt, void *pktdata, uint32 pktid, uint32 direction)
315 {
316 dhd_pktlog_ring_info_t *pkts;
317 dhd_pktlog_ring_t *pktlog_ring;
318 dhd_pktlog_filter_t *pktlog_filter;
319 u64 ts_nsec;
320 uint32 pktlog_case = 0;
321 unsigned long rem_nsec;
322 unsigned long flags = 0;
323
324 /*
325 * dhdp, dhdp->pktlog, dhd->pktlog_ring, pktlog_ring->start
326 * are validated from the DHD_PKTLOG_TX macro
327 */
328
329 pktlog_ring = dhdp->pktlog->pktlog_ring;
330 pktlog_filter = dhdp->pktlog->pktlog_filter;
331
332 if (direction == PKT_TX) {
333 pktlog_case = PKTLOG_TXPKT_CASE;
334 } else if ((direction == PKT_RX) || (direction == PKT_WAKERX)) {
335 pktlog_case = PKTLOG_RXPKT_CASE;
336 }
337
338 if ((direction != PKT_WAKERX) &&
339 dhd_pktlog_filter_matched(pktlog_filter, pktdata, pktlog_case)
340 == FALSE) {
341 return BCME_OK;
342 }
343
344 if (direction == PKT_TX && pktid == DHD_INVALID_PKTID) {
345 DHD_ERROR(("%s : Invalid PKTID \n", __FUNCTION__));
346 return BCME_ERROR;
347 }
348
349 /* get free ring_info and insert to ring_info_head */
350 DHD_PKT_LOG_LOCK(pktlog_ring->pktlog_ring_lock, flags);
351 /* if free_list is empty, use the oldest ring_info */
352 if (dll_empty(&pktlog_ring->ring_info_free)) {
353 pkts = (dhd_pktlog_ring_info_t *)dll_head_p(&pktlog_ring->ring_info_head);
354 dll_delete((dll_t *)pkts);
355 /* free the oldest packet */
356 PKTFREE(pktlog_ring->dhdp->osh, pkts->info.pkt, TRUE);
357 pktlog_ring->pktcount--;
358 } else {
359 pkts = (dhd_pktlog_ring_info_t *)dll_tail_p(&pktlog_ring->ring_info_free);
360 dll_delete((dll_t *)pkts);
361 }
362
363 /* Update packet information */
364 ts_nsec = local_clock();
365 rem_nsec = do_div(ts_nsec, NSEC_PER_SEC);
366
367 pkts->info.pkt = PKTDUP(dhdp->osh, pkt);
368 pkts->info.pkt_len = PKTLEN(dhdp->osh, pkt);
369 pkts->info.driver_ts_sec = (uint32)ts_nsec;
370 pkts->info.driver_ts_usec = (uint32)(rem_nsec/NSEC_PER_USEC);
371 pkts->info.firmware_ts = 0U;
372 pkts->info.payload_type = FRAME_TYPE_ETHERNET_II;
373 pkts->info.direction = direction;
374
375 if (direction == PKT_TX) {
376 pkts->info.pkt_hash = __dhd_dbg_pkt_hash((uintptr_t)pkt, pktid);
377 pkts->tx_fate = TX_PKT_FATE_DRV_QUEUED;
378 } else if (direction == PKT_RX) {
379 pkts->info.pkt_hash = 0U;
380 pkts->rx_fate = RX_PKT_FATE_SUCCESS;
381 } else if (direction == PKT_WAKERX) {
382 pkts->info.pkt_hash = 0U;
383 pkts->rx_fate = RX_PKT_FATE_WAKE_PKT;
384 }
385
386 DHD_PKT_LOG(("%s(): pkt hash %d\n", __FUNCTION__, pkts->info.pkt_hash));
387 DHD_PKT_LOG(("%s(): sec %d usec %d\n", __FUNCTION__,
388 pkts->info.driver_ts_sec, pkts->info.driver_ts_usec));
389
390 /* insert tx_pkts to the pktlog_ring->ring_info_head */
391 dll_append(&pktlog_ring->ring_info_head, (dll_t *)pkts);
392 pktlog_ring->pktcount++;
393 DHD_PKT_LOG_UNLOCK(pktlog_ring->pktlog_ring_lock, flags);
394 return BCME_OK;
395 }
396
397 int
dhd_pktlog_ring_tx_status(dhd_pub_t * dhdp,void * pkt,void * pktdata,uint32 pktid,uint16 status)398 dhd_pktlog_ring_tx_status(dhd_pub_t *dhdp, void *pkt, void *pktdata, uint32 pktid,
399 uint16 status)
400 {
401 dhd_pktlog_ring_info_t *tx_pkt;
402 wifi_tx_packet_fate pkt_fate;
403 uint32 pkt_hash, temp_hash;
404 dhd_pktlog_ring_t *pktlog_ring;
405 dhd_pktlog_filter_t *pktlog_filter;
406 dll_t *item_p, *next_p;
407 unsigned long flags = 0;
408
409 #ifdef BDC
410 struct bdc_header *h;
411 BCM_REFERENCE(h);
412 #endif /* BDC */
413 /*
414 * dhdp, dhdp->pktlog, dhd->pktlog_ring, pktlog_ring->start
415 * are validated from the DHD_PKTLOG_TXS macro
416 */
417
418 pktlog_ring = dhdp->pktlog->pktlog_ring;
419 pktlog_filter = dhdp->pktlog->pktlog_filter;
420
421 if (dhd_pktlog_filter_matched(pktlog_filter, pktdata,
422 PKTLOG_TXSTATUS_CASE) == FALSE) {
423 return BCME_OK;
424 }
425
426 pkt_hash = __dhd_dbg_pkt_hash((uintptr_t)pkt, pktid);
427 pkt_fate = __dhd_dbg_map_tx_status_to_pkt_fate(status);
428
429 /* find the sent tx packet and adding pkt_fate info */
430 DHD_PKT_LOG_LOCK(pktlog_ring->pktlog_ring_lock, flags);
431 /* Inverse traverse from the last packets */
432 for (item_p = dll_tail_p(&pktlog_ring->ring_info_head);
433 !dll_end(&pktlog_ring->ring_info_head, item_p);
434 item_p = next_p)
435 {
436 if (dll_empty(item_p)) {
437 break;
438 }
439 next_p = dll_prev_p(item_p);
440 tx_pkt = (dhd_pktlog_ring_info_t *)item_p;
441 temp_hash = tx_pkt->info.pkt_hash;
442 if (temp_hash == pkt_hash) {
443 tx_pkt->tx_fate = pkt_fate;
444 #ifdef BDC
445 h = (struct bdc_header *)PKTDATA(dhdp->osh, tx_pkt->info.pkt);
446 PKTPULL(dhdp->osh, tx_pkt->info.pkt, BDC_HEADER_LEN);
447 PKTPULL(dhdp->osh, tx_pkt->info.pkt, (h->dataOffset << DHD_WORD_TO_LEN_SHIFT));
448 #endif /* BDC */
449 DHD_PKT_LOG(("%s(): Found pkt hash in prev pos\n", __FUNCTION__));
450 break;
451 }
452 }
453 DHD_PKT_LOG_UNLOCK(pktlog_ring->pktlog_ring_lock, flags);
454 return BCME_OK;
455 }
456
457 dhd_pktlog_filter_t*
dhd_pktlog_filter_init(int size)458 dhd_pktlog_filter_init(int size)
459 {
460 int i;
461 gfp_t kflags;
462 uint32 alloc_len;
463 dhd_pktlog_filter_t *filter;
464 dhd_pktlog_filter_info_t *filter_info = NULL;
465
466 kflags = in_atomic() ? GFP_ATOMIC : GFP_KERNEL;
467
468 /* allocate and initialze pktmon filter */
469 alloc_len = sizeof(dhd_pktlog_filter_t);
470 filter = (dhd_pktlog_filter_t *)kzalloc(alloc_len, kflags);
471 if (unlikely(!filter)) {
472 DHD_ERROR(("%s(): could not allocate memory for - "
473 "dhd_pktlog_filter_t\n", __FUNCTION__));
474 goto fail;
475 }
476
477 alloc_len = (sizeof(dhd_pktlog_filter_info_t) * size);
478 filter_info = (dhd_pktlog_filter_info_t *)kzalloc(alloc_len, kflags);
479 if (unlikely(!filter_info)) {
480 DHD_ERROR(("%s(): could not allocate memory for - "
481 "dhd_pktlog_filter_info_t\n", __FUNCTION__));
482 goto fail;
483 }
484
485 filter->info = filter_info;
486 filter->list_cnt = 0;
487
488 for (i = 0; i < MAX_DHD_PKTLOG_FILTER_LEN; i++) {
489 filter->info[i].id = 0;
490 }
491
492 filter->enable = PKTLOG_TXPKT_CASE | PKTLOG_TXSTATUS_CASE | PKTLOG_RXPKT_CASE;
493
494 DHD_ERROR(("%s(): pktlog filter init success\n", __FUNCTION__));
495
496 return filter;
497 fail:
498 if (filter) {
499 kfree(filter);
500 }
501
502 return NULL;
503 }
504
505 int
dhd_pktlog_filter_deinit(dhd_pktlog_filter_t * filter)506 dhd_pktlog_filter_deinit(dhd_pktlog_filter_t *filter)
507 {
508 int ret = BCME_OK;
509
510 if (!filter) {
511 DHD_ERROR(("%s(): filter is NULL\n", __FUNCTION__));
512 return -EINVAL;
513 }
514
515 if (filter->info) {
516 kfree(filter->info);
517 }
518 kfree(filter);
519
520 DHD_ERROR(("%s(): pktlog filter deinit\n", __FUNCTION__));
521
522 return ret;
523 }
524
525 bool
dhd_pktlog_filter_existed(dhd_pktlog_filter_t * filter,char * arg,uint32 * id)526 dhd_pktlog_filter_existed(dhd_pktlog_filter_t *filter, char *arg, uint32 *id)
527 {
528 char filter_pattern[MAX_FILTER_PATTERN_LEN];
529 char *p;
530 int i, j;
531 int nchar;
532 int len;
533
534 if (!filter || !arg) {
535 DHD_ERROR(("%s(): filter=%p arg=%p\n", __FUNCTION__, filter, arg));
536 return TRUE;
537 }
538
539 for (i = 0; i < filter->list_cnt; i++) {
540 p = filter_pattern;
541 len = sizeof(filter_pattern);
542
543 nchar = snprintf(p, len, "%d ", filter->info[i].offset);
544 p += nchar;
545 len -= nchar;
546
547 nchar = snprintf(p, len, "0x");
548 p += nchar;
549 len -= nchar;
550
551 for (j = 0; j < filter->info[i].size_bytes; j++) {
552 nchar = snprintf(p, len, "%02x", filter->info[i].mask[j]);
553 p += nchar;
554 len -= nchar;
555 }
556
557 nchar = snprintf(p, len, " 0x");
558 p += nchar;
559 len -= nchar;
560
561 for (j = 0; j < filter->info[i].size_bytes; j++) {
562 nchar = snprintf(p, len, "%02x", filter->info[i].pattern[j]);
563 p += nchar;
564 len -= nchar;
565 }
566
567 if (strlen(arg) < strlen(filter_pattern)) {
568 continue;
569 }
570
571 DHD_PKT_LOG(("%s(): Pattern %s\n", __FUNCTION__, filter_pattern));
572
573 if (strncmp(filter_pattern, arg, strlen(filter_pattern)) == 0) {
574 *id = filter->info[i].id;
575 DHD_ERROR(("%s(): This pattern is existed\n", __FUNCTION__));
576 DHD_ERROR(("%s(): arg %s\n", __FUNCTION__, arg));
577 return TRUE;
578 }
579 }
580
581 return FALSE;
582 }
583
584 int
dhd_pktlog_filter_add(dhd_pktlog_filter_t * filter,char * arg)585 dhd_pktlog_filter_add(dhd_pktlog_filter_t *filter, char *arg)
586 {
587 int32 mask_size, pattern_size;
588 char *offset, *bitmask, *pattern;
589 uint32 id = 0;
590
591 if (!filter || !arg) {
592 DHD_ERROR(("%s(): pktlog_filter =%p arg =%p\n", __FUNCTION__, filter, arg));
593 return BCME_ERROR;
594 }
595
596 DHD_PKT_LOG(("%s(): arg %s\n", __FUNCTION__, arg));
597
598 if (dhd_pktlog_filter_existed(filter, arg, &id) == TRUE) {
599 DHD_PKT_LOG(("%s(): This pattern id %d is existed\n", __FUNCTION__, id));
600 return BCME_OK;
601 }
602
603 if (filter->list_cnt >= MAX_DHD_PKTLOG_FILTER_LEN) {
604 DHD_ERROR(("%s(): pktlog filter full\n", __FUNCTION__));
605 return BCME_ERROR;
606 }
607
608 if ((offset = bcmstrtok(&arg, " ", 0)) == NULL) {
609 DHD_ERROR(("%s(): offset not found\n", __FUNCTION__));
610 return BCME_ERROR;
611 }
612
613 if ((bitmask = bcmstrtok(&arg, " ", 0)) == NULL) {
614 DHD_ERROR(("%s(): bitmask not found\n", __FUNCTION__));
615 return BCME_ERROR;
616 }
617
618 if ((pattern = bcmstrtok(&arg, " ", 0)) == NULL) {
619 DHD_ERROR(("%s(): pattern not found\n", __FUNCTION__));
620 return BCME_ERROR;
621 }
622
623 /* parse filter bitmask */
624 mask_size = pattern_atoh_len(bitmask,
625 (char *) &filter->info[filter->list_cnt].mask[0],
626 MAX_MASK_PATTERN_FILTER_LEN);
627 if (mask_size == -1) {
628 DHD_ERROR(("Rejecting: %s\n", bitmask));
629 return BCME_ERROR;
630 }
631
632 /* parse filter pattern */
633 pattern_size = pattern_atoh_len(pattern,
634 (char *) &filter->info[filter->list_cnt].pattern[0],
635 MAX_MASK_PATTERN_FILTER_LEN);
636 if (pattern_size == -1) {
637 DHD_ERROR(("Rejecting: %s\n", pattern));
638 return BCME_ERROR;
639 }
640
641 prhex("mask", (char *)&filter->info[filter->list_cnt].mask[0],
642 mask_size);
643 prhex("pattern", (char *)&filter->info[filter->list_cnt].pattern[0],
644 pattern_size);
645
646 if (mask_size != pattern_size) {
647 DHD_ERROR(("%s(): Mask and pattern not the same size\n", __FUNCTION__));
648 return BCME_ERROR;
649 }
650
651 filter->info[filter->list_cnt].offset = strtoul(offset, NULL, 0);
652 filter->info[filter->list_cnt].size_bytes = mask_size;
653 filter->info[filter->list_cnt].id = filter->list_cnt + 1;
654 filter->info[filter->list_cnt].enable = TRUE;
655
656 filter->list_cnt++;
657
658 return BCME_OK;
659 }
660
661 int
dhd_pktlog_filter_del(dhd_pktlog_filter_t * filter,char * arg)662 dhd_pktlog_filter_del(dhd_pktlog_filter_t *filter, char *arg)
663 {
664 uint32 id = 0;
665
666 if (!filter || !arg) {
667 DHD_ERROR(("%s(): pktlog_filter =%p arg =%p\n", __FUNCTION__, filter, arg));
668 return BCME_ERROR;
669 }
670
671 DHD_PKT_LOG(("%s(): arg %s\n", __FUNCTION__, arg));
672
673 if (dhd_pktlog_filter_existed(filter, arg, &id) != TRUE) {
674 DHD_PKT_LOG(("%s(): This pattern id %d doesn't existed\n", __FUNCTION__, id));
675 return BCME_OK;
676 }
677
678 dhd_pktlog_filter_pull_forward(filter, id, filter->list_cnt);
679
680 filter->list_cnt--;
681
682 return BCME_OK;
683 }
684
685 int
dhd_pktlog_filter_enable(dhd_pktlog_filter_t * filter,uint32 pktmon_case,uint32 enable)686 dhd_pktlog_filter_enable(dhd_pktlog_filter_t *filter, uint32 pktmon_case, uint32 enable)
687 {
688 if (!filter) {
689 DHD_ERROR(("%s(): filter is NULL\n", __FUNCTION__));
690 return BCME_ERROR;
691 }
692
693 DHD_PKT_LOG(("%s(): pktlog_case %d enable %d\n", __FUNCTION__, pktmon_case, enable));
694
695 if (enable) {
696 filter->enable |= pktmon_case;
697 } else {
698 filter->enable &= ~pktmon_case;
699 }
700
701 return BCME_OK;
702 }
703
704 int
dhd_pktlog_filter_pattern_enable(dhd_pktlog_filter_t * filter,char * arg,uint32 enable)705 dhd_pktlog_filter_pattern_enable(dhd_pktlog_filter_t *filter, char *arg, uint32 enable)
706 {
707 uint32 id = 0;
708
709 if (!filter || !arg) {
710 DHD_ERROR(("%s(): pktlog_filter =%p arg =%p\n", __FUNCTION__, filter, arg));
711 return BCME_ERROR;
712 }
713
714 if (dhd_pktlog_filter_existed(filter, arg, &id) == TRUE) {
715 if (id > 0) {
716 filter->info[id-1].enable = enable;
717 DHD_ERROR(("%s(): This pattern id %d is %s\n",
718 __FUNCTION__, id, (enable ? "enabled" : "disabled")));
719 }
720 } else {
721 DHD_ERROR(("%s(): This pattern is not existed\n", __FUNCTION__));
722 DHD_ERROR(("%s(): arg %s\n", __FUNCTION__, arg));
723 }
724
725 return BCME_OK;
726 }
727
728 int
dhd_pktlog_filter_info(dhd_pktlog_filter_t * filter)729 dhd_pktlog_filter_info(dhd_pktlog_filter_t *filter)
730 {
731 char filter_pattern[MAX_FILTER_PATTERN_LEN];
732 char *p;
733 int i, j;
734 int nchar;
735 int len;
736
737 if (!filter) {
738 DHD_ERROR(("%s(): pktlog_filter is NULL\n", __FUNCTION__));
739 return BCME_ERROR;
740 }
741
742 DHD_ERROR(("---- PKTLOG FILTER INFO ----\n\n"));
743
744 DHD_ERROR(("Filter list cnt %d Filter is %s\n",
745 filter->list_cnt, (filter->enable ? "enabled" : "disabled")));
746
747 for (i = 0; i < filter->list_cnt; i++) {
748 p = filter_pattern;
749 len = sizeof(filter_pattern);
750
751 nchar = snprintf(p, len, "%d ", filter->info[i].offset);
752 p += nchar;
753 len -= nchar;
754
755 nchar = snprintf(p, len, "0x");
756 p += nchar;
757 len -= nchar;
758
759 for (j = 0; j < filter->info[i].size_bytes; j++) {
760 nchar = snprintf(p, len, "%02x", filter->info[i].mask[j]);
761 p += nchar;
762 len -= nchar;
763 }
764
765 nchar = snprintf(p, len, " 0x");
766 p += nchar;
767 len -= nchar;
768
769 for (j = 0; j < filter->info[i].size_bytes; j++) {
770 nchar = snprintf(p, len, "%02x", filter->info[i].pattern[j]);
771 p += nchar;
772 len -= nchar;
773 }
774
775 DHD_ERROR(("ID:%d is %s\n",
776 filter->info[i].id, (filter->info[i].enable ? "enabled" : "disabled")));
777 DHD_ERROR(("Pattern %s\n", filter_pattern));
778 }
779
780 DHD_ERROR(("---- PKTLOG FILTER END ----\n"));
781
782 return BCME_OK;
783 }
784 bool
dhd_pktlog_filter_matched(dhd_pktlog_filter_t * filter,char * data,uint32 pktlog_case)785 dhd_pktlog_filter_matched(dhd_pktlog_filter_t *filter, char *data, uint32 pktlog_case)
786 {
787 uint16 szbts; /* pattern size */
788 uint16 offset; /* pattern offset */
789 int i, j;
790 uint8 *mask = NULL; /* bitmask */
791 uint8 *pattern = NULL;
792 uint8 *pkt_offset = NULL; /* packet offset */
793 bool matched;
794
795 if (!filter || !data) {
796 DHD_PKT_LOG(("%s(): filter=%p data=%p\n",
797 __FUNCTION__, filter, data));
798 return TRUE;
799 }
800
801 if (!(pktlog_case & filter->enable)) {
802 DHD_PKT_LOG(("%s(): pktlog_case %d return TRUE filter is disabled\n",
803 __FUNCTION__, pktlog_case));
804 return TRUE;
805 }
806
807 for (i = 0; i < filter->list_cnt; i++) {
808 if (&filter->info[i] && filter->info[i].id && filter->info[i].enable) {
809 szbts = filter->info[i].size_bytes;
810 offset = filter->info[i].offset;
811 mask = &filter->info[i].mask[0];
812 pkt_offset = &data[offset];
813 pattern = &filter->info[i].pattern[0];
814
815 matched = TRUE;
816 for (j = 0; j < szbts; j++) {
817 if ((mask[j] & pkt_offset[j]) != pattern[j]) {
818 matched = FALSE;
819 break;
820 }
821 }
822
823 if (matched) {
824 DHD_PKT_LOG(("%s(): pktlog_filter return TRUE id %d\n",
825 __FUNCTION__, filter->info[i].id));
826 return TRUE;
827 }
828 } else {
829 DHD_PKT_LOG(("%s(): filter ino is null %p\n",
830 __FUNCTION__, &filter->info[i]));
831 }
832 }
833
834 return FALSE;
835 }
836
837 /* Ethernet Type MAC Header 12 bytes + Frame payload 10 bytes */
838 #define PKTLOG_MINIMIZE_REPORT_LEN 22
839
840 static char pktlog_minmize_mask_table[] = {
841 0xff, 0x00, 0x00, 0x00, 0xff, 0x0f, /* Ethernet Type MAC Header - Destination MAC Address */
842 0xff, 0x00, 0x00, 0x00, 0xff, 0x0f, /* Ethernet Type MAC Header - Source MAC Address */
843 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* Ethernet Type MAC Header - Ether Type - 2 bytes */
844 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* Frame payload */
845 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
846 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, /* UDP port number offset - bytes as 0xff */
847 0xff, 0xff,
848 };
849
850 static inline void
dhd_pktlog_minimize_report(char * pkt,uint32 frame_len,void * file,const void * user_buf,void * pos)851 dhd_pktlog_minimize_report(char *pkt, uint32 frame_len,
852 void *file, const void *user_buf, void *pos)
853 {
854 int i;
855 int ret = 0;
856 int table_len;
857 int report_len;
858 char *p_table;
859 char *mem_buf = NULL;
860
861 table_len = sizeof(pktlog_minmize_mask_table);
862 report_len = table_len;
863 p_table = &pktlog_minmize_mask_table[0];
864
865 if (frame_len < PKTLOG_MINIMIZE_REPORT_LEN) {
866 DHD_ERROR(("%s : frame_len is samller than min\n", __FUNCTION__));
867 return;
868 }
869
870 mem_buf = vmalloc(frame_len);
871 if (!mem_buf) {
872 DHD_ERROR(("%s : failed to alloc membuf\n", __FUNCTION__));
873 return;
874 }
875
876 bzero(mem_buf, frame_len);
877
878 if (frame_len < table_len) {
879 report_len = PKTLOG_MINIMIZE_REPORT_LEN;
880 }
881
882 for (i = 0; i < report_len; i++) {
883 mem_buf[i] = pkt[i] & p_table[i];
884 }
885
886 ret = dhd_export_debug_data(mem_buf,
887 file, user_buf, frame_len, pos);
888 if (ret < 0) {
889 DHD_ERROR(("%s : Write minimize report\n", __FUNCTION__));
890 }
891 vfree(mem_buf);
892 }
893
894 dhd_pktlog_ring_t*
dhd_pktlog_ring_change_size(dhd_pktlog_ring_t * ringbuf,int size)895 dhd_pktlog_ring_change_size(dhd_pktlog_ring_t *ringbuf, int size)
896 {
897 uint32 alloc_len;
898 uint32 pktlog_minmize;
899 dhd_pktlog_ring_t *pktlog_ring = NULL;
900 dhd_pub_t *dhdp;
901
902 if (!ringbuf) {
903 DHD_ERROR(("%s(): ringbuf is NULL\n", __FUNCTION__));
904 return NULL;
905 }
906
907 alloc_len = size;
908 if (alloc_len < MIN_PKTLOG_LEN) {
909 alloc_len = MIN_PKTLOG_LEN;
910 }
911 if (alloc_len > MAX_PKTLOG_LEN) {
912 alloc_len = MAX_PKTLOG_LEN;
913 }
914 DHD_ERROR(("ring size requested: %d alloc: %d\n", size, alloc_len));
915
916 /* backup variable */
917 pktlog_minmize = ringbuf->pktlog_minmize;
918 dhdp = ringbuf->dhdp;
919
920 /* free ring_info */
921 dhd_pktlog_ring_deinit(dhdp, ringbuf);
922
923 /* alloc ring_info */
924 pktlog_ring = dhd_pktlog_ring_init(dhdp, alloc_len);
925
926 /* restore variable */
927 if (pktlog_ring) {
928 OSL_ATOMIC_SET(dhdp->osh, &pktlog_ring->start, TRUE);
929 pktlog_ring->pktlog_minmize = pktlog_minmize;
930 }
931
932 return pktlog_ring;
933 }
934
935 void
dhd_pktlog_filter_pull_forward(dhd_pktlog_filter_t * filter,uint32 del_filter_id,uint32 list_cnt)936 dhd_pktlog_filter_pull_forward(dhd_pktlog_filter_t *filter, uint32 del_filter_id, uint32 list_cnt)
937 {
938 int ret = 0;
939 int pos = 0;
940 int move_list_cnt = 0;
941 int move_bytes = 0;
942
943 if ((del_filter_id > list_cnt) ||
944 (list_cnt > MAX_DHD_PKTLOG_FILTER_LEN)) {
945 DHD_ERROR(("Wrong id %d cnt %d tried to remove\n", del_filter_id, list_cnt));
946 return;
947 }
948
949 move_list_cnt = list_cnt - del_filter_id;
950
951 pos = del_filter_id -1;
952 move_bytes = sizeof(dhd_pktlog_filter_info_t) * move_list_cnt;
953 if (move_list_cnt) {
954 ret = memmove_s(&filter->info[pos], move_bytes + sizeof(dhd_pktlog_filter_info_t),
955 &filter->info[pos+1], move_bytes);
956 if (ret) {
957 DHD_ERROR(("filter moving failed\n"));
958 return;
959 }
960 for (; pos < list_cnt -1; pos++) {
961 filter->info[pos].id -= 1;
962 }
963 }
964 bzero(&filter->info[list_cnt-1], sizeof(dhd_pktlog_filter_info_t));
965 }
966
dhd_pktlog_get_filename(dhd_pub_t * dhdp,char * dump_path,int len)967 void dhd_pktlog_get_filename(dhd_pub_t *dhdp, char *dump_path, int len)
968 {
969 /* Init file name */
970 bzero(dump_path, len);
971 clear_debug_dump_time(dhdp->debug_dump_time_pktlog_str);
972 get_debug_dump_time(dhdp->debug_dump_time_pktlog_str);
973
974 if (dhdp->memdump_type == DUMP_TYPE_BY_SYSDUMP) {
975 if (dhdp->debug_dump_subcmd == CMD_UNWANTED) {
976 snprintf(dump_path, len, "%s",
977 DHD_PKTLOG_DUMP_PATH DHD_PKTLOG_DUMP_TYPE
978 DHD_DUMP_SUBSTR_UNWANTED);
979 } else if (dhdp->debug_dump_subcmd == CMD_DISCONNECTED) {
980 snprintf(dump_path, len, "%s",
981 DHD_PKTLOG_DUMP_PATH DHD_PKTLOG_DUMP_TYPE
982 DHD_DUMP_SUBSTR_DISCONNECTED);
983 } else {
984 snprintf(dump_path, len, "%s",
985 DHD_PKTLOG_DUMP_PATH DHD_PKTLOG_DUMP_TYPE);
986 }
987 } else {
988 if (dhdp->pktlog_debug) {
989 snprintf(dump_path, len, "%s",
990 DHD_PKTLOG_DUMP_PATH DHD_PKTLOG_DEBUG_DUMP_TYPE);
991 } else {
992 snprintf(dump_path, len, "%s",
993 DHD_PKTLOG_DUMP_PATH DHD_PKTLOG_DUMP_TYPE);
994 }
995
996 }
997
998 snprintf(dump_path, len, "%s_%s.pcap", dump_path,
999 dhdp->debug_dump_time_pktlog_str);
1000 DHD_ERROR(("%s: pktlog path = %s%s\n", __FUNCTION__, dump_path, FILE_NAME_HAL_TAG));
1001 clear_debug_dump_time(dhdp->debug_dump_time_pktlog_str);
1002 }
1003
1004 uint32
dhd_pktlog_get_item_length(dhd_pktlog_ring_info_t * report_ptr)1005 dhd_pktlog_get_item_length(dhd_pktlog_ring_info_t *report_ptr)
1006 {
1007 uint32 len = 0;
1008 char buf[DHD_PKTLOG_FATE_INFO_STR_LEN];
1009 int bytes_user_data = 0;
1010 uint32 write_frame_len;
1011 uint32 frame_len;
1012
1013 len += (uint32)sizeof(report_ptr->info.driver_ts_sec);
1014 len += (uint32)sizeof(report_ptr->info.driver_ts_usec);
1015
1016 if (report_ptr->info.payload_type == FRAME_TYPE_ETHERNET_II) {
1017 frame_len = (uint32)min(report_ptr->info.pkt_len, (size_t)MAX_FRAME_LEN_ETHERNET);
1018 } else {
1019 frame_len = (uint32)min(report_ptr->info.pkt_len, (size_t)MAX_FRAME_LEN_80211_MGMT);
1020 }
1021
1022 bytes_user_data = sprintf(buf, "%s:%s:%02d\n", DHD_PKTLOG_FATE_INFO_FORMAT,
1023 (report_ptr->tx_fate ? "Failure" : "Succeed"), report_ptr->tx_fate);
1024 write_frame_len = frame_len + bytes_user_data;
1025
1026 /* pcap pkt head has incl_len and orig_len */
1027 len += (uint32)sizeof(write_frame_len);
1028 len += (uint32)sizeof(write_frame_len);
1029 len += frame_len;
1030 len += bytes_user_data;
1031
1032 return len;
1033 }
1034
1035 uint32
dhd_pktlog_get_dump_length(dhd_pub_t * dhdp)1036 dhd_pktlog_get_dump_length(dhd_pub_t *dhdp)
1037 {
1038 dhd_pktlog_ring_info_t *report_ptr;
1039 dhd_pktlog_ring_t *pktlog_ring;
1040 uint32 len;
1041 dll_t *item_p, *next_p;
1042
1043 if (!dhdp || !dhdp->pktlog) {
1044 DHD_PKT_LOG(("%s(): dhdp=%p pktlog=%p\n",
1045 __FUNCTION__, dhdp, (dhdp ? dhdp->pktlog : NULL)));
1046 return -EINVAL;
1047 }
1048
1049 if (!dhdp->pktlog->pktlog_ring) {
1050 DHD_PKT_LOG(("%s(): pktlog_ring =%p\n",
1051 __FUNCTION__, dhdp->pktlog->pktlog_ring));
1052 return -EINVAL;
1053 }
1054
1055 pktlog_ring = dhdp->pktlog->pktlog_ring;
1056 OSL_ATOMIC_SET(dhdp->osh, &pktlog_ring->start, FALSE);
1057
1058 len = sizeof(dhd_pktlog_pcap_hdr_t);
1059
1060 for (item_p = dll_head_p(&pktlog_ring->ring_info_head);
1061 !dll_end(&pktlog_ring->ring_info_head, item_p);
1062 item_p = next_p) {
1063 next_p = dll_next_p(item_p);
1064 report_ptr = (dhd_pktlog_ring_info_t *)item_p;
1065 len += dhd_pktlog_get_item_length(report_ptr);
1066 }
1067 OSL_ATOMIC_SET(dhdp->osh, &pktlog_ring->start, TRUE);
1068 DHD_PKT_LOG(("calcuated pkt log dump len:%d\n", len));
1069
1070 return len;
1071 }
1072
1073 int
dhd_pktlog_dump_write(dhd_pub_t * dhdp,void * file,const void * user_buf,uint32 size)1074 dhd_pktlog_dump_write(dhd_pub_t *dhdp, void *file, const void *user_buf, uint32 size)
1075 {
1076 dhd_pktlog_ring_info_t *report_ptr;
1077 dhd_pktlog_ring_t *pktlog_ring;
1078 char buf[DHD_PKTLOG_FATE_INFO_STR_LEN];
1079 dhd_pktlog_pcap_hdr_t pcap_h;
1080 uint32 write_frame_len;
1081 uint32 frame_len;
1082 ulong len;
1083 int bytes_user_data = 0;
1084 loff_t pos = 0;
1085 int ret = BCME_OK;
1086 dll_t *item_p, *next_p;
1087
1088 if (!dhdp || !dhdp->pktlog) {
1089 DHD_PKT_LOG(("%s(): dhdp=%p pktlog=%p\n",
1090 __FUNCTION__, dhdp, (dhdp ? dhdp->pktlog : NULL)));
1091 return -EINVAL;
1092 }
1093
1094 if (!dhdp->pktlog->pktlog_ring) {
1095 DHD_PKT_LOG(("%s(): pktlog_ring =%p\n",
1096 __FUNCTION__, dhdp->pktlog->pktlog_ring));
1097 return -EINVAL;
1098 }
1099
1100 if (file && !user_buf && (size == 0)) {
1101 DHD_ERROR(("Local file pktlog dump requested\n"));
1102 } else if (!file && user_buf && (size > 0)) {
1103 DHD_ERROR(("HAL file pktlog dump %d bytes requested\n", size));
1104 } else {
1105 DHD_ERROR(("Wrong type pktlog dump requested\n"));
1106 return -EINVAL;
1107 }
1108
1109 pktlog_ring = dhdp->pktlog->pktlog_ring;
1110 OSL_ATOMIC_SET(dhdp->osh, &pktlog_ring->start, FALSE);
1111
1112 pcap_h.magic_number = PKTLOG_PCAP_MAGIC_NUM;
1113 pcap_h.version_major = PKTLOG_PCAP_MAJOR_VER;
1114 pcap_h.version_minor = PKTLOG_PCAP_MINOR_VER;
1115 pcap_h.thiszone = 0x0;
1116 pcap_h.sigfigs = 0x0;
1117 pcap_h.snaplen = PKTLOG_PCAP_SNAP_LEN;
1118 pcap_h.network = PKTLOG_PCAP_NETWORK_TYPE;
1119
1120 ret = dhd_export_debug_data((char *)&pcap_h, file, user_buf, sizeof(pcap_h), &pos);
1121 len = sizeof(pcap_h);
1122
1123 for (item_p = dll_head_p(&pktlog_ring->ring_info_head);
1124 !dll_end(&pktlog_ring->ring_info_head, item_p);
1125 item_p = next_p) {
1126
1127 next_p = dll_next_p(item_p);
1128 report_ptr = (dhd_pktlog_ring_info_t *)item_p;
1129
1130 if ((file == NULL) &&
1131 (len + dhd_pktlog_get_item_length(report_ptr) > size)) {
1132 DHD_ERROR(("overflowed pkt logs are dropped\n"));
1133 break;
1134 }
1135
1136 ret = dhd_export_debug_data((char*)&report_ptr->info.driver_ts_sec, file,
1137 user_buf, sizeof(report_ptr->info.driver_ts_sec), &pos);
1138 len += sizeof(report_ptr->info.driver_ts_sec);
1139
1140 ret = dhd_export_debug_data((char*)&report_ptr->info.driver_ts_usec, file,
1141 user_buf, sizeof(report_ptr->info.driver_ts_usec), &pos);
1142 len += sizeof(report_ptr->info.driver_ts_usec);
1143
1144 if (report_ptr->info.payload_type == FRAME_TYPE_ETHERNET_II) {
1145 frame_len = (uint32)min(report_ptr->info.pkt_len,
1146 (size_t)MAX_FRAME_LEN_ETHERNET);
1147
1148 } else {
1149 frame_len = (uint32)min(report_ptr->info.pkt_len,
1150 (size_t)MAX_FRAME_LEN_80211_MGMT);
1151 }
1152
1153 bytes_user_data = sprintf(buf, "%s:%s:%02d\n", DHD_PKTLOG_FATE_INFO_FORMAT,
1154 (report_ptr->tx_fate ? "Failure" : "Succeed"), report_ptr->tx_fate);
1155 write_frame_len = frame_len + bytes_user_data;
1156
1157 /* pcap pkt head has incl_len and orig_len */
1158 ret = dhd_export_debug_data((char*)&write_frame_len, file, user_buf,
1159 sizeof(write_frame_len), &pos);
1160 len += sizeof(write_frame_len);
1161
1162 ret = dhd_export_debug_data((char*)&write_frame_len, file, user_buf,
1163 sizeof(write_frame_len), &pos);
1164 len += sizeof(write_frame_len);
1165
1166 if (pktlog_ring->pktlog_minmize) {
1167 dhd_pktlog_minimize_report(PKTDATA(pktlog_ring->dhdp->osh,
1168 report_ptr->info.pkt), frame_len, file, user_buf, &pos);
1169 } else {
1170 ret = dhd_export_debug_data(PKTDATA(pktlog_ring->dhdp->osh,
1171 report_ptr->info.pkt), file, user_buf, frame_len, &pos);
1172 }
1173 len += frame_len;
1174
1175 ret = dhd_export_debug_data(buf, file, user_buf, bytes_user_data, &pos);
1176 len += bytes_user_data;
1177 }
1178 OSL_ATOMIC_SET(dhdp->osh, &pktlog_ring->start, TRUE);
1179
1180 return ret;
1181 }
1182
1183 int
dhd_pktlog_dump_write_memory(dhd_pub_t * dhdp,const void * user_buf,uint32 size)1184 dhd_pktlog_dump_write_memory(dhd_pub_t *dhdp, const void *user_buf, uint32 size)
1185 {
1186 int ret = dhd_pktlog_dump_write(dhdp, NULL, user_buf, size);
1187 if (ret < 0) {
1188 DHD_ERROR(("dhd_pktlog_dump_write_memory error\n"));
1189 }
1190 return ret;
1191 }
1192
1193 int
dhd_pktlog_dump_write_file(dhd_pub_t * dhdp)1194 dhd_pktlog_dump_write_file(dhd_pub_t *dhdp)
1195 {
1196 struct file *w_pcap_fp = NULL;
1197 uint32 file_mode;
1198 mm_segment_t old_fs;
1199 char pktlogdump_path[128];
1200 int ret = BCME_OK;
1201
1202 dhd_pktlog_get_filename(dhdp, pktlogdump_path, 128);
1203 old_fs = get_fs();
1204 set_fs(KERNEL_DS);
1205 file_mode = O_CREAT | O_WRONLY;
1206
1207 w_pcap_fp = filp_open(pktlogdump_path, file_mode, 0664);
1208 if (IS_ERR(w_pcap_fp)) {
1209 DHD_ERROR(("%s: Couldn't open file '%s' err %ld\n",
1210 __FUNCTION__, pktlogdump_path, PTR_ERR(w_pcap_fp)));
1211 ret = BCME_ERROR;
1212 goto fail;
1213 }
1214
1215 dhd_pktlog_dump_write(dhdp, w_pcap_fp, NULL, 0);
1216 if (ret < 0) {
1217 DHD_ERROR(("dhd_pktlog_dump_write error\n"));
1218 goto fail;
1219 }
1220
1221 /* Sync file from filesystem to physical media */
1222 ret = vfs_fsync(w_pcap_fp, 0);
1223 if (ret < 0) {
1224 DHD_ERROR(("%s(): sync pcap file error, err = %d\n", __FUNCTION__, ret));
1225 goto fail;
1226 }
1227 fail:
1228 if (!IS_ERR(w_pcap_fp)) {
1229 filp_close(w_pcap_fp, NULL);
1230 }
1231
1232 set_fs(old_fs);
1233
1234 #ifdef DHD_DUMP_MNGR
1235 if (ret >= 0) {
1236 dhd_dump_file_manage_enqueue(dhdp, pktlogdump_path, DHD_PKTLOG_DUMP_TYPE);
1237 }
1238 #endif /* DHD_DUMP_MNGR */
1239 return ret;
1240 }
1241
1242 #ifdef DHD_COMPACT_PKT_LOG
1243 static uint64
dhd_cpkt_log_calc_time_diff(dhd_pktlog_ring_info_t * pkt_info,uint64 curr_ts_nsec)1244 dhd_cpkt_log_calc_time_diff(dhd_pktlog_ring_info_t *pkt_info, uint64 curr_ts_nsec)
1245 {
1246 uint64 pkt_ts_nsec = pkt_info->info.driver_ts_sec * NSEC_PER_SEC +
1247 pkt_info->info.driver_ts_usec * NSEC_PER_USEC;
1248
1249 return (curr_ts_nsec - pkt_ts_nsec) / NSEC_PER_USEC;
1250 }
1251
1252 static int
dhd_cpkt_log_get_ts_idx(dhd_pktlog_t * pktlog,dhd_pktlog_ring_info_t * pkt_info,u64 curr_ts_nsec)1253 dhd_cpkt_log_get_ts_idx(dhd_pktlog_t *pktlog, dhd_pktlog_ring_info_t *pkt_info, u64 curr_ts_nsec)
1254 {
1255 struct rb_node *n = pktlog->cpkt_log_tt_rbt.rb_node;
1256 dhd_cpkt_log_ts_node_t *node = NULL;
1257
1258 uint64 ts_diff = dhd_cpkt_log_calc_time_diff(pkt_info, curr_ts_nsec);
1259
1260 if (ts_diff > dhd_cpkt_log_tt_idx[CPKT_LOG_TT_IDX_ARR_SZ - 1])
1261 return CPKT_LOG_TT_IDX_ARR_SZ;
1262
1263 while (n) {
1264 node = rb_entry(n, dhd_cpkt_log_ts_node_t, rb);
1265
1266 if (ts_diff < node->ts_diff)
1267 n = n->rb_left;
1268 else if (ts_diff > node->ts_diff)
1269 n = n->rb_right;
1270 else
1271 break;
1272 }
1273
1274 if (node != NULL) {
1275 if (node->idx && ts_diff < node->ts_diff)
1276 return node->idx - 1;
1277 return node->idx;
1278 }
1279
1280 return BCME_NOTFOUND;
1281 }
1282
1283 static int
dhd_cpkt_log_get_direction(dhd_pktlog_ring_info_t * pkt_info)1284 dhd_cpkt_log_get_direction(dhd_pktlog_ring_info_t *pkt_info)
1285 {
1286 return pkt_info->info.direction == PKTLOG_TXPKT_CASE ? PKT_TX : PKT_RX;
1287 }
1288
1289 static int
dhd_cpkt_log_get_802_1x_subtype(eapol_header_t * eapol)1290 dhd_cpkt_log_get_802_1x_subtype(eapol_header_t *eapol)
1291 {
1292 int subtype;
1293 eap_header_t *eap;
1294 eapol_wpa_key_header_t *ek;
1295
1296 uint16 key_info;
1297 int pair, ack, mic, kerr, req, sec, install;
1298
1299 subtype = CPKT_LOG_802_1X_SUBTYPE_OTHERS;
1300 if (eapol->type != EAPOL_KEY) {
1301 eap = (eap_header_t *)eapol->body;
1302
1303 switch (eap->type) {
1304 case EAP_IDENTITY:
1305 subtype = CPKT_LOG_802_1X_SUBTYPE_IDENTITY;
1306 break;
1307 case REALM_EAP_TLS:
1308 subtype = CPKT_LOG_802_1X_SUBTYPE_TLS;
1309 break;
1310 case REALM_EAP_TTLS:
1311 subtype = CPKT_LOG_802_1X_SUBTYPE_TTLS;
1312 break;
1313 case REALM_EAP_FAST:
1314 subtype = CPKT_LOG_802_1X_SUBTYPE_FAST;
1315 break;
1316 case REALM_EAP_LEAP:
1317 subtype = CPKT_LOG_802_1X_SUBTYPE_LEAP;
1318 break;
1319 case REALM_EAP_PSK:
1320 subtype = CPKT_LOG_802_1X_SUBTYPE_PWD;
1321 break;
1322 case REALM_EAP_SIM:
1323 subtype = CPKT_LOG_802_1X_SUBTYPE_SIM;
1324 break;
1325 case REALM_EAP_AKA:
1326 subtype = CPKT_LOG_802_1X_SUBTYPE_AKA;
1327 break;
1328 case REALM_EAP_AKAP:
1329 subtype = CPKT_LOG_802_1X_SUBTYPE_AKAP;
1330 break;
1331 default:
1332 break;
1333 }
1334 if (eap->code == EAP_SUCCESS)
1335 subtype = CPKT_LOG_802_1X_SUBTYPE_SUCCESS;
1336 } else {
1337 /* in case of 4 way handshake */
1338 ek = (eapol_wpa_key_header_t *)(eapol->body);
1339
1340 if (ek->type == EAPOL_WPA2_KEY || ek->type == EAPOL_WPA_KEY) {
1341 key_info = ntoh16_ua(&ek->key_info);
1342
1343 pair = 0 != (key_info & WPA_KEY_PAIRWISE);
1344 ack = 0 != (key_info & WPA_KEY_ACK);
1345 mic = 0 != (key_info & WPA_KEY_MIC);
1346 kerr = 0 != (key_info & WPA_KEY_ERROR);
1347 req = 0 != (key_info & WPA_KEY_REQ);
1348 sec = 0 != (key_info & WPA_KEY_SECURE);
1349 install = 0 != (key_info & WPA_KEY_INSTALL);
1350
1351 if (!sec && !mic && ack && !install && pair && !kerr && !req)
1352 subtype = CPKT_LOG_802_1X_SUBTYPE_4WAY_M1;
1353 else if (pair && !install && !ack && mic && !sec && !kerr && !req)
1354 subtype = CPKT_LOG_802_1X_SUBTYPE_4WAY_M2;
1355 else if (pair && ack && mic && sec && !kerr && !req)
1356 subtype = CPKT_LOG_802_1X_SUBTYPE_4WAY_M3;
1357 else if (pair && !install && !ack && mic && sec && !req && !kerr)
1358 subtype = CPKT_LOG_802_1X_SUBTYPE_4WAY_M4;
1359 }
1360 }
1361
1362 return subtype;
1363 }
1364
1365 static int
dhd_cpkt_log_get_pkt_info(dhd_pktlog_t * pktlog,dhd_pktlog_ring_info_t * pkt_info)1366 dhd_cpkt_log_get_pkt_info(dhd_pktlog_t *pktlog, dhd_pktlog_ring_info_t *pkt_info)
1367 {
1368 int type;
1369 int subtype = 0;
1370
1371 uint8 prot;
1372 uint16 src_port, dst_port;
1373 int len, offset;
1374
1375 uint8 *pdata;
1376 uint8 *pkt_data;
1377
1378 uint16 eth_type;
1379 struct bcmarp *arp;
1380 struct bcmicmp_hdr *icmp;
1381 struct ipv4_hdr *ipv4;
1382 struct ether_header *eth_hdr;
1383 bcm_tlv_t *dhcp_opt;
1384
1385 struct ipv6_hdr *ipv6;
1386 struct icmp6_hdr *icmpv6_hdr;
1387
1388 pkt_data = (uint8 *)PKTDATA(pktlog->dhdp->osh, pkt_info->info.pkt);
1389
1390 eth_hdr = (struct ether_header *)pkt_data;
1391 eth_type = ntoh16(eth_hdr->ether_type);
1392
1393 type = CPKT_LOG_TYPE_OTHERS;
1394 switch (eth_type) {
1395 case ETHER_TYPE_IP:
1396 if (get_pkt_ip_type(pktlog->dhdp->osh, pkt_info->info.pkt,
1397 &pdata, &len, &prot) != 0) {
1398 DHD_PKT_LOG(("%s: fail to get pkt ip type\n", __FUNCTION__));
1399 return BCME_ERROR;
1400 }
1401
1402 if (prot == IP_PROT_ICMP) {
1403 icmp = (struct bcmicmp_hdr *)(pdata);
1404 if (!(icmp->type == ICMP_TYPE_ECHO_REQUEST ||
1405 icmp->type == ICMP_TYPE_ECHO_REPLY ||
1406 icmp->type == CPKT_LOG_ICMP_TYPE_DEST_UNREACHABLE)) {
1407 return BCME_ERROR;
1408 }
1409
1410 if (icmp->type == ICMP_TYPE_ECHO_REQUEST) {
1411 type = CPKT_LOG_TYPE_ICMP_REQ;
1412 /* Subtype = Last 8 bits of identifier */
1413 subtype = ntoh16_ua(pdata + sizeof(*icmp)) & 0xFF;
1414 } else if (icmp->type == ICMP_TYPE_ECHO_REPLY) {
1415 type = CPKT_LOG_TYPE_ICMP_RES;
1416 /* Subtype = Last 8 bits of identifier */
1417 subtype = ntoh16_ua(pdata + sizeof(*icmp)) & 0xFF;
1418 } else if (icmp->type == CPKT_LOG_ICMP_TYPE_DEST_UNREACHABLE) {
1419 type = CPKT_LOG_TYPE_ICMP_UNREACHABLE;
1420 /* Subtype = Last 8 bits of identifier */
1421 ipv4 = (struct ipv4_hdr *)(pdata + sizeof(*icmp) +
1422 CPKT_LOG_ICMP_TYPE_DEST_UNREACHABLE_IPV4_OFFSET);
1423 subtype = ipv4->id & 0xFF;
1424 }
1425
1426 DHD_PKT_LOG(("%s: type = ICMP(%d), subtype = %x \n",
1427 __FUNCTION__, type, subtype));
1428 } else if (prot == IP_PROT_UDP) {
1429 if (len < UDP_HDR_LEN)
1430 return BCME_ERROR;
1431
1432 src_port = ntoh16_ua(pdata);
1433 dst_port = ntoh16_ua(pdata + UDP_DEST_PORT_OFFSET);
1434
1435 if (src_port == DHCP_PORT_SERVER || src_port == DHCP_PORT_CLIENT) {
1436 type = CPKT_LOG_TYPE_DHCP;
1437 /* Subtype = DHCP message type */
1438 offset = DHCP_OPT_OFFSET + CPKT_LOG_DHCP_MAGIC_COOKIE_LEN;
1439 if ((UDP_HDR_LEN + offset) >= len)
1440 return BCME_ERROR;
1441 len -= (UDP_HDR_LEN - offset);
1442
1443 dhcp_opt = bcm_parse_tlvs(pdata + UDP_HDR_LEN + offset,
1444 len, DHCP_OPT_MSGTYPE);
1445 if (dhcp_opt == NULL)
1446 return BCME_NOTFOUND;
1447 subtype = dhcp_opt->data[0];
1448
1449 DHD_PKT_LOG(("%s: type = DHCP(%d), subtype = %x \n",
1450 __FUNCTION__, type, subtype));
1451 } else if (src_port == CPKT_LOG_DNS_PORT_CLIENT ||
1452 dst_port == CPKT_LOG_DNS_PORT_CLIENT ||
1453 dst_port == CPKT_LOG_MDNS_PORT_CLIENT) {
1454 type = CPKT_LOG_TYPE_DNS;
1455 /* Subtype = Last 8 bits of DNS Transaction ID */
1456 subtype = ntoh16_ua(pdata + UDP_HDR_LEN) & 0xFF;
1457
1458 DHD_PKT_LOG(("%s: type = DNS(%d), subtype = %x \n",
1459 __FUNCTION__, type, subtype));
1460 } else {
1461 DHD_PKT_LOG(("%s: unsupported ports num (src:%d, dst:%d)\n",
1462 __FUNCTION__, src_port, dst_port));
1463 }
1464 } else {
1465 DHD_PKT_LOG(("%s: prot = %x\n", __FUNCTION__, prot));
1466 }
1467
1468 break;
1469 case ETHER_TYPE_ARP:
1470 type = CPKT_LOG_TYPE_ARP;
1471 /* Subtype = Last 8 bits of target IP address */
1472 arp = (struct bcmarp *)(pkt_data + ETHER_HDR_LEN);
1473 subtype = arp->dst_ip[IPV4_ADDR_LEN - 1];
1474
1475 DHD_PKT_LOG(("%s: type = ARP(%d), subtype = %x\n",
1476 __FUNCTION__, type, subtype));
1477
1478 break;
1479 case ETHER_TYPE_802_1X:
1480 type = CPKT_LOG_TYPE_802_1X;
1481 /* EAPOL for 802.3/Ethernet */
1482 subtype = dhd_cpkt_log_get_802_1x_subtype((eapol_header_t *)pkt_data);
1483
1484 DHD_PKT_LOG(("%s: type = 802.1x(%d), subtype = %x\n",
1485 __FUNCTION__, type, subtype));
1486
1487 break;
1488 case ETHER_TYPE_IPV6:
1489 ipv6 = (struct ipv6_hdr *)(pkt_data + ETHER_HDR_LEN);
1490 if (ipv6->nexthdr == ICMPV6_HEADER_TYPE) {
1491 type = CPKT_LOG_TYPE_ICMPv6;
1492 icmpv6_hdr =
1493 (struct icmp6_hdr *)(pkt_data + ETHER_HDR_LEN + sizeof(*ipv6));
1494 subtype = icmpv6_hdr->icmp6_type;
1495
1496 DHD_PKT_LOG(("%s: type = ICMPv6(%x), subtype = %x\n",
1497 __FUNCTION__, type, subtype));
1498 } else {
1499 DHD_ERROR(("%s: unsupported ipv6 next header\n", __FUNCTION__));
1500 }
1501
1502 break;
1503 default:
1504 DHD_ERROR(("%s: Invalid eth type (%x)\n", __FUNCTION__, eth_hdr->ether_type));
1505 break;
1506 }
1507
1508 return (subtype << CPKT_LOG_BIT_LEN_TYPE) | type;
1509 }
1510
1511 static int
dhd_cpkt_log_get_pkt_fate(dhd_pktlog_ring_info_t * pktlog_info)1512 dhd_cpkt_log_get_pkt_fate(dhd_pktlog_ring_info_t *pktlog_info)
1513 {
1514 return pktlog_info->fate;
1515 }
1516
1517 /*
1518 * dhd_cpkt_log_build: prepare 22 bits of data as compact packet log format to report to big data
1519 *
1520 * pkt_info: one packet data from packet log
1521 * curr_ts_nsec: current time (nano seconds)
1522 * cpkt: pointer for output(22 bits compact packet log)
1523 *
1524 */
1525 static int
dhd_cpkt_log_build(dhd_pktlog_t * pktlog,dhd_pktlog_ring_info_t * pkt_info,u64 curr_ts_nsec,int * cpkt)1526 dhd_cpkt_log_build(dhd_pktlog_t *pktlog, dhd_pktlog_ring_info_t *pkt_info,
1527 u64 curr_ts_nsec, int *cpkt)
1528 {
1529 int ret;
1530 int mask;
1531 int temp = 0;
1532
1533 /* Timestamp index */
1534 ret = dhd_cpkt_log_get_ts_idx(pktlog, pkt_info, curr_ts_nsec);
1535 if (ret < 0) {
1536 DHD_ERROR(("%s: Invalid cpktlog ts, err = %d\n", __FUNCTION__, ret));
1537 return ret;
1538 }
1539 mask = CPKT_LOG_BIT_MASK_TS;
1540 temp |= ((ret & mask) << CPKT_LOG_BIT_OFFSET_TS);
1541
1542 /* Direction: Tx/Rx */
1543 ret = dhd_cpkt_log_get_direction(pkt_info);
1544 mask = CPKT_LOG_BIT_MASK_DIR;
1545 temp |= ((ret & mask) << CPKT_LOG_BIT_OFFSET_DIR);
1546
1547 /* Info = Packet Type & Packet Subtype */
1548 ret = dhd_cpkt_log_get_pkt_info(pktlog, pkt_info);
1549 if (ret < 0) {
1550 DHD_ERROR(("%s: Invalid cpktlog info, err = %d\n", __FUNCTION__, ret));
1551 return ret;
1552 }
1553 mask = CPKT_LOG_BIT_MASK_SUBTYPE << CPKT_LOG_BIT_LEN_TYPE | CPKT_LOG_BIT_MASK_TYPE;
1554 temp |= ((ret & mask) << CPKT_LOG_BIT_OFFSET_TYPE);
1555
1556 /* Packet Fate */
1557 ret = dhd_cpkt_log_get_pkt_fate(pkt_info);
1558 mask = CPKT_LOG_BIT_MASK_PKT_FATE;
1559 temp |= ((ret & mask) << CPKT_LOG_BIT_OFFSET_PKT_FATE);
1560
1561 *cpkt = temp;
1562
1563 return BCME_OK;
1564 }
1565
1566 int
dhd_cpkt_log_proc(dhd_pub_t * dhdp,char * buf,int buf_len,int bit_offset,int req_pkt_num)1567 dhd_cpkt_log_proc(dhd_pub_t *dhdp, char *buf, int buf_len, int bit_offset, int req_pkt_num)
1568 {
1569 int ret;
1570 int cpkt;
1571 int offset = bit_offset;
1572 dll_t *item_p, *prev_p;
1573
1574 uint8 pkt_cnt;
1575 u64 curr_ts_nsec;
1576
1577 dhd_pktlog_t *pktlog;
1578 dhd_pktlog_ring_t *pktlog_rbuf;
1579
1580 if (!dhdp || !dhdp->pktlog) {
1581 DHD_ERROR(("%s: dhdp or pktlog is NULL\n", __FUNCTION__));
1582 return BCME_ERROR;
1583 }
1584
1585 if (!dhdp->pktlog->pktlog_ring) {
1586 DHD_ERROR(("%s: pktlog_ring is NULL\n", __FUNCTION__));
1587 return BCME_ERROR;
1588 }
1589
1590 DHD_PKT_LOG(("%s: start cpkt log\n", __FUNCTION__));
1591
1592 pktlog = dhdp->pktlog;
1593 pktlog_rbuf = pktlog->pktlog_ring;
1594
1595 req_pkt_num = req_pkt_num > CPKT_LOG_MAX_NUM ?
1596 CPKT_LOG_MAX_NUM : req_pkt_num;
1597
1598 pkt_cnt = 0;
1599 curr_ts_nsec = local_clock();
1600 for (item_p = dll_tail_p(&pktlog_rbuf->ring_info_head);
1601 !dll_end(&pktlog_rbuf->ring_info_head, item_p);
1602 item_p = prev_p) {
1603 prev_p = dll_prev_p(item_p);
1604 if (prev_p == NULL)
1605 break;
1606
1607 ret = dhd_cpkt_log_build(pktlog, (dhd_pktlog_ring_info_t *)item_p,
1608 curr_ts_nsec, &cpkt);
1609 if (ret < 0)
1610 continue;
1611
1612 offset = dhd_bit_pack(buf, buf_len, offset, cpkt, CPKT_LOG_BIT_SIZE);
1613
1614 pkt_cnt++;
1615 if (pkt_cnt >= req_pkt_num)
1616 break;
1617 }
1618
1619 return offset;
1620 }
1621
1622 static void
dhd_cpkt_log_insert_ts(dhd_cpkt_log_ts_node_t * node,struct rb_root * root)1623 dhd_cpkt_log_insert_ts(dhd_cpkt_log_ts_node_t *node, struct rb_root *root)
1624 {
1625 struct rb_node **new = &root->rb_node, *parent = NULL;
1626 u64 ts_diff = node->ts_diff;
1627
1628 while (*new) {
1629 parent = *new;
1630 if (ts_diff < rb_entry(parent, dhd_cpkt_log_ts_node_t, rb)->ts_diff)
1631 new = &parent->rb_left;
1632 else
1633 new = &parent->rb_right;
1634 }
1635
1636 rb_link_node(&node->rb, parent, new);
1637 rb_insert_color(&node->rb, root);
1638 }
1639
1640 static void
dhd_cpkt_log_deinit_tt(dhd_pub_t * dhdp)1641 dhd_cpkt_log_deinit_tt(dhd_pub_t *dhdp)
1642 {
1643 struct rb_node *n;
1644 dhd_pktlog_t *pktlog = dhdp->pktlog;
1645
1646 dhd_cpkt_log_ts_node_t *node;
1647
1648 while ((n = rb_first(&pktlog->cpkt_log_tt_rbt))) {
1649 node = rb_entry(n, dhd_cpkt_log_ts_node_t, rb);
1650 rb_erase(&node->rb, &pktlog->cpkt_log_tt_rbt);
1651 MFREE(dhdp->osh, node, sizeof(*node));
1652 }
1653 }
1654
1655 static int
dhd_cpkt_log_init_tt(dhd_pub_t * dhdp)1656 dhd_cpkt_log_init_tt(dhd_pub_t *dhdp)
1657 {
1658 int i;
1659 int ret = BCME_OK;
1660
1661 dhd_pktlog_t *pktlog = dhdp->pktlog;
1662
1663 dhd_cpkt_log_ts_node_t *node;
1664
1665 for (i = 0; i < ARRAYSIZE(dhd_cpkt_log_tt_idx); i++) {
1666 node = (dhd_cpkt_log_ts_node_t *)MALLOCZ(dhdp->osh, sizeof(*node));
1667 if (!node) {
1668 ret = BCME_NOMEM;
1669 goto exit;
1670 }
1671 node->ts_diff = dhd_cpkt_log_tt_idx[i];
1672 node->idx = i;
1673
1674 dhd_cpkt_log_insert_ts(node, &pktlog->cpkt_log_tt_rbt);
1675 }
1676
1677 return BCME_OK;
1678 exit:
1679 dhd_cpkt_log_deinit_tt(dhdp);
1680
1681 return ret;
1682 }
1683 #endif /* DHD_COMPACT_PKT_LOG */
1684 #endif /* DHD_PKT_LOGGING */
1685