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