wlantest/writepcap.c

   1 /*
   2  * PCAP capture file writer
   3  * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
   4  *
   5  * This software may be distributed under the terms of the BSD license.
   6  * See README for more details.
   7  */
   8
   9 #include "utils/includes.h"
  10 #include <pcap.h>
  11 #include <pcap-bpf.h>
  12
  13 #include "utils/common.h"
  14 #include "wlantest.h"
  15 #include "common/qca-vendor.h"
  16
  17
  18 int write_pcap_init(struct wlantest *wt, const char *fname)
  19 {
  20         wt->write_pcap = pcap_open_dead(DLT_IEEE802_11_RADIO, 4000);
  21         if (wt->write_pcap == NULL)
  22                 return -1;
  23         wt->write_pcap_dumper = pcap_dump_open(wt->write_pcap, fname);
  24         if (wt->write_pcap_dumper == NULL) {
  25                 pcap_close(wt->write_pcap);
  26                 wt->write_pcap = NULL;
  27                 return -1;
  28         }
  29
  30         wpa_printf(MSG_DEBUG, "Writing PCAP dump to '%s'", fname);
  31
  32         return 0;
  33 }
  34
  35
  36 void write_pcap_deinit(struct wlantest *wt)
  37 {
  38         if (wt->write_pcap_dumper) {
  39                 pcap_dump_close(wt->write_pcap_dumper);
  40                 wt->write_pcap_dumper = NULL;
  41         }
  42         if (wt->write_pcap) {
  43                 pcap_close(wt->write_pcap);
  44                 wt->write_pcap = NULL;
  45         }
  46 }
  47
  48
  49 void write_pcap_captured(struct wlantest *wt, const u8 *buf, size_t len)
  50 {
  51         struct pcap_pkthdr h;
  52
  53         if (!wt->write_pcap_dumper)
  54                 return;
  55
  56         os_memset(&h, 0, sizeof(h));
  57         gettimeofday(&wt->write_pcap_time, NULL);
  58         h.ts = wt->write_pcap_time;
  59         h.caplen = len;
  60         h.len = len;
  61         pcap_dump(wt->write_pcap_dumper, &h, buf);
  62         if (wt->pcap_no_buffer)
  63                 pcap_dump_flush(wt->write_pcap_dumper);
  64 }
  65
  66
  67 void write_pcap_decrypted(struct wlantest *wt, const u8 *buf1, size_t len1,
  68                           const u8 *buf2, size_t len2)
  69 {
  70         struct pcap_pkthdr h;
  71         u8 rtap[] = {
  72                 0x00 /* rev */,
  73                 0x00 /* pad */,
  74                 0x0e, 0x00, /* header len */
  75                 0x00, 0x00, 0x00, 0x40, /* present flags */
  76                 0x00, 0x13, 0x74, QCA_RADIOTAP_VID_WLANTEST,
  77                 0x00, 0x00
  78         };
  79         u8 *buf;
  80         size_t len;
  81
  82         if (!wt->write_pcap_dumper && !wt->pcapng)
  83                 return;
  84
  85         os_free(wt->decrypted);
  86         len = sizeof(rtap) + len1 + len2;
  87         wt->decrypted = buf = os_malloc(len);
  88         if (buf == NULL)
  89                 return;
  90         wt->decrypted_len = len;
  91         os_memcpy(buf, rtap, sizeof(rtap));
  92         if (buf1) {
  93                 os_memcpy(buf + sizeof(rtap), buf1, len1);
  94                 buf[sizeof(rtap) + 1] &= ~0x40; /* Clear Protected flag */
  95         }
  96         if (buf2)
  97                 os_memcpy(buf + sizeof(rtap) + len1, buf2, len2);
  98
  99         if (!wt->write_pcap_dumper)
 100                 return;
 101
 102         os_memset(&h, 0, sizeof(h));
 103         h.ts = wt->write_pcap_time;
 104         h.caplen = len;
 105         h.len = len;
 106         pcap_dump(wt->write_pcap_dumper, &h, buf);
 107         if (wt->pcap_no_buffer)
 108                 pcap_dump_flush(wt->write_pcap_dumper);
 109 }
 110
 111
 112 struct pcapng_section_header {
 113         u32 block_type; /* 0x0a0d0d0a */
 114         u32 block_total_len;
 115         u32 byte_order_magic;
 116         u16 major_version;
 117         u16 minor_version;
 118         u64 section_len;
 119         u32 block_total_len2;
 120 } STRUCT_PACKED;
 121
 122 struct pcapng_interface_description {
 123         u32 block_type; /* 0x00000001 */
 124         u32 block_total_len;
 125         u16 link_type;
 126         u16 reserved;
 127         u32 snap_len;
 128         u32 block_total_len2;
 129 } STRUCT_PACKED;
 130
 131 struct pcapng_enhanced_packet {
 132         u32 block_type; /* 0x00000006 */
 133         u32 block_total_len;
 134         u32 interface_id;
 135         u32 timestamp_high;
 136         u32 timestamp_low;
 137         u32 captured_len;
 138         u32 packet_len;
 139         /* Packet data - aligned to 32 bits */
 140         /* Options (variable) */
 141         /* Block Total Length copy */
 142 } STRUCT_PACKED;
 143
 144 #define PCAPNG_BYTE_ORDER_MAGIC 0x1a2b3c4d
 145 #define PCAPNG_BLOCK_IFACE_DESC 0x00000001
 146 #define PCAPNG_BLOCK_PACKET 0x00000002
 147 #define PCAPNG_BLOCK_SIMPLE_PACKET 0x00000003
 148 #define PCAPNG_BLOCK_NAME_RESOLUTION 0x00000004
 149 #define PCAPNG_BLOCK_INTERFACE_STATISTICS 0x00000005
 150 #define PCAPNG_BLOCK_ENHANCED_PACKET 0x00000006
 151 #define PCAPNG_BLOCK_SECTION_HEADER 0x0a0d0d0a
 152
 153 #define LINKTYPE_IEEE802_11 105
 154 #define LINKTYPE_IEEE802_11_RADIO 127
 155
 156 #define PAD32(a) ((4 - ((a) & 3)) & 3)
 157 #define ALIGN32(a) ((a) + PAD32((a)))
 158
 159
 160 int write_pcapng_init(struct wlantest *wt, const char *fname)
 161 {
 162         struct pcapng_section_header hdr;
 163         struct pcapng_interface_description desc;
 164
 165         wt->pcapng = fopen(fname, "wb");
 166         if (wt->pcapng == NULL)
 167                 return -1;
 168
 169         wpa_printf(MSG_DEBUG, "Writing PCAPNG dump to '%s'", fname);
 170
 171         os_memset(&hdr, 0, sizeof(hdr));
 172         hdr.block_type = PCAPNG_BLOCK_SECTION_HEADER;
 173         hdr.block_total_len = sizeof(hdr);
 174         hdr.byte_order_magic = PCAPNG_BYTE_ORDER_MAGIC;
 175         hdr.major_version = 1;
 176         hdr.minor_version = 0;
 177         hdr.section_len = -1;
 178         hdr.block_total_len2 = hdr.block_total_len;
 179         fwrite(&hdr, sizeof(hdr), 1, wt->pcapng);
 180
 181         os_memset(&desc, 0, sizeof(desc));
 182         desc.block_type = PCAPNG_BLOCK_IFACE_DESC;
 183         desc.block_total_len = sizeof(desc);
 184         desc.block_total_len2 = desc.block_total_len;
 185         desc.link_type = LINKTYPE_IEEE802_11_RADIO;
 186         desc.snap_len = 65535;
 187         fwrite(&desc, sizeof(desc), 1, wt->pcapng);
 188         if (wt->pcap_no_buffer)
 189                 fflush(wt->pcapng);
 190
 191         return 0;
 192 }
 193
 194
 195 void write_pcapng_deinit(struct wlantest *wt)
 196 {
 197         if (wt->pcapng) {
 198                 fclose(wt->pcapng);
 199                 wt->pcapng = NULL;
 200         }
 201 }
 202
 203
 204 static u8 * pcapng_add_comments(struct wlantest *wt, u8 *pos)
 205 {
 206         size_t i;
 207         u16 *len;
 208
 209         if (!wt->num_notes)
 210                 return pos;
 211
 212         *((u16 *) pos) = 1 /* opt_comment */;
 213         pos += 2;
 214         len = (u16 *) pos /* length to be filled in */;
 215         pos += 2;
 216
 217         for (i = 0; i < wt->num_notes; i++) {
 218                 size_t nlen = os_strlen(wt->notes[i]);
 219                 if (i > 0)
 220                         *pos++ = '\n';
 221                 os_memcpy(pos, wt->notes[i], nlen);
 222                 pos += nlen;
 223         }
 224         *len = pos - (u8 *) len - 2;
 225         pos += PAD32(*len);
 226
 227         *((u16 *) pos) = 0 /* opt_endofopt */;
 228         pos += 2;
 229         *((u16 *) pos) = 0;
 230         pos += 2;
 231
 232         return pos;
 233 }
 234
 235
 236 static void write_pcapng_decrypted(struct wlantest *wt)
 237 {
 238         size_t len;
 239         struct pcapng_enhanced_packet *pkt;
 240         u8 *pos;
 241         u32 *block_len;
 242
 243         if (!wt->pcapng || wt->decrypted == NULL)
 244                 return;
 245
 246         add_note(wt, MSG_EXCESSIVE, "decrypted version of the previous frame");
 247
 248         len = sizeof(*pkt) + wt->decrypted_len + 100 + notes_len(wt, 32);
 249         pkt = os_zalloc(len);
 250         if (pkt == NULL)
 251                 return;
 252
 253         pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
 254         pkt->interface_id = 0;
 255         pkt->timestamp_high = wt->write_pcapng_time_high;
 256         pkt->timestamp_low = wt->write_pcapng_time_low;
 257         pkt->captured_len = wt->decrypted_len;
 258         pkt->packet_len = wt->decrypted_len;
 259
 260         pos = (u8 *) (pkt + 1);
 261
 262         os_memcpy(pos, wt->decrypted, wt->decrypted_len);
 263         pos += ALIGN32(wt->decrypted_len);
 264
 265         pos = pcapng_add_comments(wt, pos);
 266
 267         block_len = (u32 *) pos;
 268         pos += 4;
 269         *block_len = pkt->block_total_len = pos - (u8 *) pkt;
 270
 271         fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);
 272         if (wt->pcap_no_buffer)
 273                 fflush(wt->pcapng);
 274
 275         os_free(pkt);
 276 }
 277
 278
 279 void write_pcapng_write_read(struct wlantest *wt, int dlt,
 280                              struct pcap_pkthdr *hdr, const u8 *data)
 281 {
 282         struct pcapng_enhanced_packet *pkt;
 283         u8 *pos;
 284         u32 *block_len;
 285         u64 timestamp;
 286         size_t len, datalen = hdr->caplen;
 287         u8 rtap[] = {
 288                 0x00 /* rev */,
 289                 0x00 /* pad */,
 290                 0x0a, 0x00, /* header len */
 291                 0x02, 0x00, 0x00, 0x00, /* present flags */
 292                 0x00, /* flags */
 293                 0x00 /* pad */
 294         };
 295
 296         if (wt->assume_fcs)
 297                 rtap[8] |= 0x10;
 298
 299         if (!wt->pcapng)
 300                 return;
 301
 302         len = sizeof(*pkt) + hdr->len + 100 + notes_len(wt, 32) + sizeof(rtap);
 303         pkt = os_zalloc(len);
 304         if (pkt == NULL)
 305                 return;
 306
 307         pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
 308         pkt->interface_id = 0;
 309         timestamp = 1000000 * hdr->ts.tv_sec + hdr->ts.tv_usec;
 310         pkt->timestamp_high = timestamp >> 32;
 311         pkt->timestamp_low = timestamp & 0xffffffff;
 312         wt->write_pcapng_time_high = pkt->timestamp_high;
 313         wt->write_pcapng_time_low = pkt->timestamp_low;
 314         pkt->captured_len = hdr->caplen;
 315         pkt->packet_len = hdr->len;
 316
 317         pos = (u8 *) (pkt + 1);
 318
 319         switch (dlt) {
 320         case DLT_IEEE802_11_RADIO:
 321                 break;
 322         case DLT_PRISM_HEADER:
 323                 /* remove prism header (could be kept ... lazy) */
 324                 pkt->captured_len -= WPA_GET_LE32(data + 4);
 325                 pkt->packet_len -= WPA_GET_LE32(data + 4);
 326                 datalen -= WPA_GET_LE32(data + 4);
 327                 data += WPA_GET_LE32(data + 4);
 328                 /* fall through */
 329         case DLT_IEEE802_11:
 330                 pkt->captured_len += sizeof(rtap);
 331                 pkt->packet_len += sizeof(rtap);
 332                 os_memcpy(pos, &rtap, sizeof(rtap));
 333                 pos += sizeof(rtap);
 334                 break;
 335         default:
 336                 return;
 337         }
 338
 339         os_memcpy(pos, data, datalen);
 340         pos += datalen + PAD32(pkt->captured_len);
 341         pos = pcapng_add_comments(wt, pos);
 342
 343         block_len = (u32 *) pos;
 344         pos += 4;
 345         *block_len = pkt->block_total_len = pos - (u8 *) pkt;
 346
 347         fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);
 348         if (wt->pcap_no_buffer)
 349                 fflush(wt->pcapng);
 350
 351         os_free(pkt);
 352
 353         write_pcapng_decrypted(wt);
 354 }
 355
 356
 357 void write_pcapng_captured(struct wlantest *wt, const u8 *buf, size_t len)
 358 {
 359         struct pcap_pkthdr h;
 360
 361         if (!wt->pcapng)
 362                 return;
 363
 364         os_memset(&h, 0, sizeof(h));
 365         gettimeofday(&h.ts, NULL);
 366         h.caplen = len;
 367         h.len = len;
 368         write_pcapng_write_read(wt, DLT_IEEE802_11_RADIO, &h, buf);
 369 }