trafgen_proto.c

   1 /*
   2  * netsniff-ng - the packet sniffing beast
   3  * Subject to the GPL, version 2.
   4  */
   5
   6 #include <stddef.h>
   7 #include <string.h>
   8 #include <netinet/in.h>
   9 #include <linux/if_ether.h>
  10
  11 #include "dev.h"
  12 #include "xmalloc.h"
  13 #include "trafgen_conf.h"
  14 #include "trafgen_l2.h"
  15 #include "trafgen_l3.h"
  16 #include "trafgen_l4.h"
  17 #include "trafgen_proto.h"
  18
  19 #define field_shift_and_mask(f, v) (((v) << (f)->shift) & \
  20                 ((f)->mask ? (f)->mask : (0xffffffff)))
  21
  22 #define field_unmask_and_unshift(f, v) (((v) & \
  23                 ((f)->mask ? (f)->mask : (0xffffffff))) >> (f)->shift)
  24
  25 static struct proto_ctx ctx;
  26
  27 #define PROTO_MAX_LAYERS        16
  28
  29 static struct proto_hdr *headers[PROTO_MAX_LAYERS];
  30 static size_t headers_count;
  31
  32 static struct proto_hdr *registered;
  33
  34 static inline struct proto_hdr *proto_current_header(void)
  35 {
  36         if (headers_count > 0)
  37                 return headers[headers_count - 1];
  38
  39         panic("No header was added\n");
  40 }
  41
  42 struct proto_hdr *proto_lower_header(struct proto_hdr *hdr)
  43 {
  44         struct proto_hdr *lower = NULL;
  45         size_t i;
  46
  47         if (headers_count == 0)
  48                 return NULL;
  49
  50         for (i = 1, lower = headers[0]; i < headers_count; i++) {
  51                 if (headers[i] == hdr)
  52                         return headers[i - 1];
  53         }
  54
  55         return lower;
  56 }
  57
  58 uint8_t *proto_header_ptr(struct proto_hdr *hdr)
  59 {
  60         return &current_packet()->payload[hdr->pkt_offset];
  61 }
  62
  63 static struct proto_hdr *proto_header_by_id(enum proto_id id)
  64 {
  65         struct proto_hdr *p = registered;
  66
  67         for (; p; p = p->next)
  68                 if (p->id == id)
  69                         return p;
  70
  71         panic("Can't lookup proto by id %u\n", id);
  72 }
  73
  74 void proto_header_register(struct proto_hdr *hdr)
  75 {
  76         hdr->next = registered;
  77         registered = hdr;
  78
  79         hdr->fields = NULL;
  80         hdr->fields_count = 0;
  81 }
  82
  83 static void proto_fields_realloc(struct proto_hdr *hdr, size_t count)
  84 {
  85         hdr->fields = xrealloc(hdr->fields, count * sizeof(*hdr->fields));
  86         hdr->fields_count = count;
  87 }
  88
  89 void proto_header_fields_add(struct proto_hdr *hdr,
  90                              const struct proto_field *fields, size_t count)
  91 {
  92         struct packet *pkt = current_packet();
  93         struct proto_field *f;
  94         int i;
  95
  96         if (!hdr->fields)
  97                 hdr->pkt_offset = pkt->len;
  98
  99         proto_fields_realloc(hdr, hdr->fields_count + count);
 100
 101         for (i = 0; count >= 1; count--, i++) {
 102                 f = &hdr->fields[hdr->fields_count - count];
 103
 104                 f->id = fields[i].id;
 105                 f->len = fields[i].len;
 106                 f->is_set = false;
 107                 f->shift = fields[i].shift;
 108                 f->mask = fields[i].mask;
 109                 f->pkt_offset = hdr->pkt_offset + fields[i].offset;
 110
 111                 if (f->pkt_offset + f->len > pkt->len)
 112                         set_fill(0, (f->pkt_offset + f->len) - pkt->len);
 113         }
 114 }
 115
 116 static struct proto_field *proto_field_by_id(struct proto_hdr *hdr, uint32_t fid)
 117 {
 118         int i;
 119
 120         for (i = 0; i < hdr->fields_count; i++)
 121                 if (hdr->fields[i].id == fid)
 122                         return &hdr->fields[i];
 123
 124         panic("Failed lookup field id %u for proto id %u\n", fid, hdr->id);
 125 }
 126
 127 bool proto_field_is_set(struct proto_hdr *hdr, uint32_t fid)
 128 {
 129         struct proto_field *field = proto_field_by_id(hdr, fid);
 130
 131         return field ? field->is_set : false;
 132 }
 133
 134 struct proto_hdr *proto_header_init(enum proto_id pid)
 135 {
 136         struct proto_hdr *hdr = proto_header_by_id(pid);
 137         struct proto_hdr *new_hdr;
 138
 139         if (headers_count >= PROTO_MAX_LAYERS)
 140                 panic("Too many proto headers\n");
 141
 142         new_hdr = xmalloc(sizeof(*new_hdr));
 143         memcpy(new_hdr, hdr, sizeof(*new_hdr));
 144
 145         if (new_hdr->header_init)
 146                 new_hdr->header_init(new_hdr);
 147
 148         headers[headers_count++] = new_hdr;
 149         return new_hdr;
 150 }
 151
 152 void proto_header_finish(struct proto_hdr *hdr)
 153 {
 154         if (hdr && hdr->header_finish)
 155                 hdr->header_finish(hdr);
 156 }
 157
 158 struct proto_hdr *proto_lower_default_add(struct proto_hdr *hdr,
 159                                           enum proto_id pid)
 160 {
 161         struct proto_hdr *current;
 162
 163         if (headers_count > 0) {
 164                 current = proto_current_header();
 165
 166                 if (current->layer >= proto_header_by_id(pid)->layer)
 167                         goto set_proto;
 168                 if (current->id == pid)
 169                         goto set_proto;
 170         }
 171
 172         current = proto_header_init(pid);
 173
 174 set_proto:
 175         if (current->set_next_proto)
 176                 current->set_next_proto(current, hdr->id);
 177
 178         return current;
 179 }
 180
 181 static void __proto_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 182                                     uint8_t *bytes, bool is_default, bool is_be)
 183 {
 184         struct proto_field *field;
 185         uint8_t *payload, *p8;
 186         uint16_t *p16;
 187         uint32_t *p32;
 188         uint32_t v32;
 189         uint16_t v16;
 190         uint8_t v8;
 191
 192         field = proto_field_by_id(hdr, fid);
 193
 194         if (is_default && field->is_set)
 195                 return;
 196
 197         payload = &current_packet()->payload[field->pkt_offset];
 198
 199         if (field->len == 1) {
 200                 p8 = payload;
 201                 *p8 = field->mask ? *p8 & ~field->mask : *p8;
 202
 203                 v8 = field_shift_and_mask(field, *bytes);
 204                 v8 = field->mask ? (v8 | *p8) : v8;
 205
 206                 bytes = &v8;
 207         } else if (field->len == 2) {
 208                 p16 = (uint16_t *)payload;
 209                 *p16 = be16_to_cpu(*p16);
 210                 *p16 = cpu_to_be16(field->mask ? *p16 & ~field->mask : *p16);
 211
 212                 v16 = field_shift_and_mask(field, *(uint16_t *)bytes);
 213                 v16 = is_be ? cpu_to_be16(v16) : v16;
 214                 v16 = field->mask ? (v16 | *p16) : v16;
 215
 216                 bytes = (uint8_t *)&v16;
 217         } else if (field->len == 4) {
 218                 p32 = (uint32_t *)payload;
 219                 *p32 = be32_to_cpu(*p32);
 220                 *p32 = cpu_to_be32(field->mask ? *p32 & ~field->mask : *p32);
 221
 222                 v32 = field_shift_and_mask(field, *(uint32_t *)bytes);
 223                 v32 = is_be ? cpu_to_be32(v32) : v32;
 224                 v32 = field->mask ? (v32 | *p32) : v32;
 225
 226                 bytes = (uint8_t *)&v32;
 227         }
 228
 229         memcpy(payload, bytes, field->len);
 230
 231         if (!is_default)
 232                 field->is_set = true;
 233 }
 234
 235 void proto_field_set_bytes(struct proto_hdr *hdr, uint32_t fid, uint8_t *bytes)
 236 {
 237         __proto_field_set_bytes(hdr, fid, bytes, false, false);
 238 }
 239
 240 static uint8_t *__proto_field_get_bytes(struct proto_field *field)
 241 {
 242         struct packet *pkt = current_packet();
 243
 244         return &pkt->payload[field->pkt_offset];
 245 }
 246
 247 void proto_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 248 {
 249         proto_field_set_bytes(hdr, fid, (uint8_t *)&val);
 250 }
 251
 252 uint8_t proto_field_get_u8(struct proto_hdr *hdr, uint32_t fid)
 253 {
 254         struct proto_field *field = proto_field_by_id(hdr, fid);
 255         uint8_t val = *__proto_field_get_bytes(field);
 256
 257         return field_unmask_and_unshift(field, val);
 258 }
 259
 260 void proto_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 261 {
 262         proto_field_set_bytes(hdr, fid, (uint8_t *)&val);
 263 }
 264
 265 uint16_t proto_field_get_u16(struct proto_hdr *hdr, uint32_t fid)
 266 {
 267         struct proto_field *field = proto_field_by_id(hdr, fid);
 268         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 269
 270         return field_unmask_and_unshift(field, be16_to_cpu(val));
 271 }
 272
 273 void proto_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 274 {
 275         proto_field_set_bytes(hdr, fid, (uint8_t *)&val);
 276 }
 277
 278 uint32_t proto_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
 279 {
 280         struct proto_field *field = proto_field_by_id(hdr, fid);
 281         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 282
 283         return field_unmask_and_unshift(field, be32_to_cpu(val));
 284 }
 285
 286 void proto_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid, uint8_t *bytes)
 287 {
 288         __proto_field_set_bytes(hdr, fid, bytes, true, false);
 289 }
 290
 291 void proto_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 292 {
 293         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 294 }
 295
 296 void proto_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 297 {
 298         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 299 }
 300
 301 void proto_field_set_default_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 302 {
 303         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 304 }
 305
 306 void proto_field_set_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 307 {
 308         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, false, true);
 309 }
 310
 311 void proto_field_set_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 312 {
 313         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, false, true);
 314 }
 315
 316 void proto_field_set_default_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 317 {
 318         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, true);
 319 }
 320
 321 void proto_field_set_default_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 322 {
 323         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, true);
 324 }
 325
 326 static void __proto_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
 327                                       bool is_default)
 328 {
 329         uint8_t mac[ETH_ALEN];
 330         int ret;
 331
 332         if (proto_field_is_set(hdr, fid))
 333                 return;
 334
 335         if (!hdr->ctx->dev)
 336                 panic("Device is not specified\n");
 337
 338         ret = device_hw_address(hdr->ctx->dev, mac, sizeof(mac));
 339         if (ret < 0)
 340                 panic("Could not get device hw address\n");
 341
 342         __proto_field_set_bytes(hdr, fid, mac, is_default, false);
 343 }
 344
 345 void proto_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 346 {
 347         __proto_field_set_dev_mac(hdr, fid, false);
 348 }
 349
 350 void proto_field_set_default_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 351 {
 352         __proto_field_set_dev_mac(hdr, fid, true);
 353 }
 354
 355 static void __proto_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 356                                        bool is_default)
 357 {
 358         struct sockaddr_storage ss = { };
 359         struct sockaddr_in *ss4;
 360         uint32_t ip_addr;
 361         int ret;
 362
 363         if (proto_field_is_set(hdr, fid))
 364                 return;
 365
 366         ret = device_address(hdr->ctx->dev, AF_INET, &ss);
 367         if (ret < 0)
 368                 panic("Could not get device IPv4 address\n");
 369
 370         ss4 = (struct sockaddr_in *) &ss;
 371         ip_addr = ss4->sin_addr.s_addr;
 372
 373         __proto_field_set_bytes(hdr, fid, (uint8_t *)&ip_addr, is_default, false);
 374 }
 375
 376 void proto_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 377 {
 378         __proto_field_set_dev_ipv4(hdr, fid, false);
 379 }
 380
 381 void proto_field_set_default_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 382 {
 383         __proto_field_set_dev_ipv4(hdr, fid, true);
 384 }
 385
 386 void protos_init(const char *dev)
 387 {
 388         struct proto_hdr *p;
 389
 390         ctx.dev = dev;
 391
 392         protos_l2_init();
 393         protos_l3_init();
 394         protos_l4_init();
 395
 396         for (p = registered; p; p = p->next)
 397                 p->ctx = &ctx;
 398 }
 399
 400 void proto_packet_finish(void)
 401 {
 402         ssize_t i;
 403
 404         /* Go down from upper layers to do last calculations (checksum) */
 405         for (i = headers_count - 1; i >= 0; i--) {
 406                 struct proto_hdr *p = headers[i];
 407
 408                 if (p->packet_finish)
 409                         p->packet_finish(p);
 410         }
 411
 412         for (i = 0; i < headers_count; i++) {
 413                 struct proto_hdr *p = headers[i];
 414
 415                 if (p->fields) {
 416                         xfree(p->fields);
 417                         p->fields_count = 0;
 418                 }
 419
 420                 xfree(headers[i]);
 421         }
 422
 423         headers_count = 0;
 424 }