lib-pcap.c

   1 #include <stdio.h>
   2 #include <stdint.h>
   3 #include <string.h>
   4 #include <libubox/utils.h>
   5 #include <libubox/blobmsg.h>
   6 #include "udebug-pcap.h"
   7 #include "priv.h"
   8
   9 static char pcap_buf[65536];
  10 static struct pcap_block_hdr *pcap_hdr = (struct pcap_block_hdr *)pcap_buf;
  11
  12 struct pcap_block_hdr {
  13         uint32_t type;
  14         uint32_t len;
  15 };
  16
  17 struct pcap_shb_hdr {
  18         uint32_t endian;
  19         uint16_t major;
  20         uint16_t minor;
  21         uint64_t section_len;
  22 };
  23
  24 struct pcap_idb_hdr {
  25         uint16_t link_type;
  26         uint16_t _pad;
  27         uint32_t snap_len;
  28 };
  29
  30 struct pcap_epb_hdr {
  31         uint32_t iface;
  32         uint32_t ts_hi;
  33         uint32_t ts_lo;
  34         uint32_t cap_len;
  35         uint32_t pkt_len;
  36 };
  37
  38 struct pcap_opt_hdr {
  39         uint16_t id;
  40         uint16_t len;
  41 };
  42
  43 #define PCAPNG_BTYPE_SHB        0x0a0d0d0a
  44 #define PCAPNG_BTYPE_IDB        1
  45 #define PCAPNG_BTYPE_EPB        6
  46
  47 #define PCAPNG_ENDIAN           0x1a2b3c4d
  48
  49 static void *
  50 pcap_block_start(uint32_t type, uint32_t len)
  51 {
  52         struct pcap_block_hdr *b = pcap_hdr;
  53
  54         b->type = type;
  55         b->len = len + sizeof(*b);
  56         memset(b + 1, 0, len);
  57
  58         return b + 1;
  59 }
  60
  61 static void *
  62 pcap_block_append(int len)
  63 {
  64         struct pcap_block_hdr *b = pcap_hdr;
  65         void *data = &pcap_buf[b->len];
  66
  67         memset(data, 0, len);
  68         b->len += len;
  69
  70         return data;
  71 }
  72
  73 static void *
  74 pcap_opt_append(int id, int len)
  75 {
  76         struct pcap_opt_hdr *opt;
  77
  78         len = (len + 3) & ~3;
  79         opt = pcap_block_append(sizeof(*opt) + len);
  80         opt->id = id;
  81         opt->len = len;
  82
  83         return opt + 1;
  84 }
  85
  86 static void
  87 pcap_opt_str_add(int id, const char *val)
  88 {
  89         int len;
  90
  91         if (!val)
  92                  return;
  93
  94         len = strlen(val) + 1;
  95         memcpy(pcap_opt_append(id, len), val, len);
  96 }
  97
  98 static void
  99 pcap_opt_u8_add(uint16_t id, uint8_t val)
 100 {
 101         *(uint8_t *)pcap_opt_append(id, 1) = val;
 102 }
 103
 104 static void
 105 pcap_opt_end(void)
 106 {
 107         pcap_block_append(4);
 108 }
 109
 110 static uint32_t __pcap_block_align(int offset, int val)
 111 {
 112         struct pcap_block_hdr *b = pcap_hdr;
 113         uint32_t cur_len = b->len - offset;
 114         uint32_t aligned_len = (cur_len + (val - 1)) & ~(val - 1);
 115         uint32_t pad = aligned_len - cur_len;
 116
 117         if (pad)
 118                 pcap_block_append(pad);
 119
 120         return pad;
 121 }
 122
 123 static uint32_t pcap_block_align(int val)
 124 {
 125         return __pcap_block_align(0, val);
 126 }
 127
 128 static int
 129 pcap_block_end(void)
 130 {
 131         struct pcap_block_hdr *b = (struct pcap_block_hdr *)pcap_buf;
 132         uint32_t *len;
 133
 134         pcap_block_align(4);
 135         len = (uint32_t *)&pcap_buf[b->len];
 136         b->len += 4;
 137         *len = b->len;
 138
 139         return *len;
 140 }
 141
 142
 143 int pcap_init(struct pcap_context *p, struct pcap_meta *meta)
 144 {
 145         struct pcap_shb_hdr *shb;
 146
 147         shb = pcap_block_start(PCAPNG_BTYPE_SHB, sizeof(*shb));
 148         shb->endian = PCAPNG_ENDIAN;
 149         shb->major = 1;
 150         shb->section_len = ~0ULL;
 151         pcap_opt_str_add(2, meta->hw);
 152         pcap_opt_str_add(3, meta->os);
 153         pcap_opt_str_add(4, meta->app);
 154         pcap_opt_end();
 155         pcap_block_end();
 156
 157         return 0;
 158 }
 159
 160 int pcap_interface_init(struct pcap_context *p, uint32_t *id,
 161                         struct pcap_interface_meta *meta)
 162 {
 163         struct pcap_idb_hdr *idb;
 164
 165         *id = p->iface_id++;
 166         idb = pcap_block_start(PCAPNG_BTYPE_IDB, sizeof(*idb));
 167         idb->link_type = meta->link_type;
 168         idb->snap_len = 0xffff;
 169         pcap_opt_str_add(2, meta->name);
 170         pcap_opt_str_add(3, meta->description);
 171         pcap_opt_u8_add(9, meta->time_res);
 172         pcap_opt_end();
 173         pcap_block_end();
 174
 175         return 0;
 176 }
 177
 178 void pcap_packet_init(uint32_t iface, uint64_t ts)
 179 {
 180         struct pcap_epb_hdr *epb;
 181
 182         epb = pcap_block_start(PCAPNG_BTYPE_EPB, sizeof(*epb));
 183         epb->iface = iface;
 184         epb->ts_hi = ts >> 32;
 185         epb->ts_lo = (uint32_t)ts;
 186 }
 187
 188 void *pcap_packet_append(const void *data, size_t len)
 189 {
 190         void *buf;
 191
 192         buf = pcap_block_append(len);
 193         if (data)
 194                 memcpy(buf, data, len);
 195
 196         return buf;
 197 }
 198
 199 void pcap_packet_done(void)
 200 {
 201         struct pcap_epb_hdr *epb = (struct pcap_epb_hdr *)&pcap_hdr[1];
 202         unsigned int len;
 203
 204         len = pcap_hdr->len - sizeof(*pcap_hdr) - sizeof(*epb);
 205         epb->cap_len = epb->pkt_len = len;
 206         pcap_block_align(4);
 207         pcap_block_end();
 208 }
 209
 210 int pcap_interface_rbuf_init(struct pcap_context *p, struct udebug_remote_buf *rb)
 211 {
 212         const struct udebug_packet_info *meta = rb->meta;
 213         struct pcap_interface_meta if_meta = {
 214                 .time_res = 6,
 215                 .name = meta->attr[UDEBUG_META_IFACE_NAME],
 216                 .description = meta->attr[UDEBUG_META_IFACE_DESC],
 217         };
 218
 219         if (rb->buf.hdr->format == UDEBUG_FORMAT_PACKET)
 220                 if_meta.link_type = rb->buf.hdr->sub_format;
 221         else if (rb->buf.hdr->format == UDEBUG_FORMAT_STRING)
 222                 if_meta.link_type = 147;
 223
 224         return pcap_interface_init(p, &rb->pcap_iface, &if_meta);
 225 }
 226
 227 int pcap_snapshot_packet_init(struct udebug *ctx, struct udebug_iter *it)
 228 {
 229         struct udebug_remote_buf *rb;
 230         struct udebug_snapshot *s = it->s;
 231
 232         rb = udebug_remote_buf_get(ctx, s->rbuf_idx);
 233         if (!rb)
 234                 return -1;
 235
 236         pcap_packet_init(rb->pcap_iface, it->timestamp);
 237
 238         switch (s->format) {
 239         case UDEBUG_FORMAT_PACKET:
 240         case UDEBUG_FORMAT_STRING:
 241                 pcap_packet_append(it->data, it->len);
 242                 break;
 243         case UDEBUG_FORMAT_BLOBMSG:
 244                 break;
 245         default:
 246                 return -1;
 247         }
 248
 249         pcap_packet_done();
 250
 251         return 0;
 252 }
 253
 254 void pcap_block_write_file(FILE *f)
 255 {
 256         fwrite(pcap_buf, pcap_hdr->len, 1, f);
 257         fflush(f);
 258 }
 259
 260 void *pcap_block_get(size_t *len)
 261 {
 262         *len = pcap_hdr->len;
 263
 264         return pcap_buf;
 265 }