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