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