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 struct ctx {
  26         const char *dev;
  27 };
  28 static struct ctx ctx;
  29
  30 static const struct proto_ops *registered_ops[__PROTO_MAX];
  31
  32 struct proto_hdr *proto_lower_header(struct proto_hdr *hdr)
  33 {
  34         struct packet *pkt = packet_get(hdr->pkt_id);
  35         struct proto_hdr **headers = &pkt->headers[0];
  36
  37         if (hdr->index == 0)
  38                 return NULL;
  39
  40         return headers[hdr->index - 1];
  41 }
  42
  43 struct proto_hdr *proto_upper_header(struct proto_hdr *hdr)
  44 {
  45         struct packet *pkt = packet_get(hdr->pkt_id);
  46         struct proto_hdr **headers = &pkt->headers[0];
  47         size_t headers_count = pkt->headers_count;
  48
  49         if (hdr->index == headers_count - 1)
  50                 return NULL;
  51
  52         return headers[hdr->index + 1];
  53 }
  54
  55 uint8_t *proto_header_ptr(struct proto_hdr *hdr)
  56 {
  57         return &packet_get(hdr->pkt_id)->payload[hdr->pkt_offset];
  58 }
  59
  60 static const struct proto_ops *proto_ops_by_id(enum proto_id id)
  61 {
  62         const struct proto_ops *ops = registered_ops[id];
  63
  64         bug_on(ops->id != id);
  65         return ops;
  66 }
  67
  68 void proto_ops_register(const struct proto_ops *ops)
  69 {
  70         bug_on(ops->id >= __PROTO_MAX);
  71         registered_ops[ops->id] = ops;
  72 }
  73
  74 static void proto_fields_realloc(struct proto_hdr *hdr, size_t count)
  75 {
  76         hdr->fields = xrealloc(hdr->fields, count * sizeof(*hdr->fields));
  77         hdr->fields_count = count;
  78 }
  79
  80 void proto_header_fields_add(struct proto_hdr *hdr,
  81                              const struct proto_field *fields, size_t count)
  82 {
  83         struct packet *pkt = packet_get(hdr->pkt_id);
  84         struct proto_field *f;
  85         int i;
  86
  87         if (!hdr->fields)
  88                 hdr->pkt_offset = pkt->len;
  89
  90         proto_fields_realloc(hdr, hdr->fields_count + count);
  91
  92         for (i = 0; count >= 1; count--, i++) {
  93                 f = &hdr->fields[hdr->fields_count - count];
  94
  95                 f->id = fields[i].id;
  96                 f->len = fields[i].len;
  97                 f->is_set = false;
  98                 f->shift = fields[i].shift;
  99                 f->mask = fields[i].mask;
 100                 f->pkt_offset = hdr->pkt_offset + fields[i].offset;
 101                 f->hdr = hdr;
 102
 103                 if (f->pkt_offset + f->len > pkt->len) {
 104                         hdr->len += f->len;
 105                         set_fill(0, (f->pkt_offset + f->len) - pkt->len);
 106                 }
 107         }
 108 }
 109
 110 struct proto_field *proto_hdr_field_by_id(struct proto_hdr *hdr, uint32_t fid)
 111 {
 112         /* Assume the fields are stored in the same order as the respective
 113          * enum, so the index can be used for faster lookup here.
 114          */
 115         bug_on(hdr->fields[fid].id != fid);
 116
 117         return &hdr->fields[fid];
 118 }
 119
 120 bool proto_hdr_field_is_set(struct proto_hdr *hdr, uint32_t fid)
 121 {
 122         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 123
 124         return field ? field->is_set : false;
 125 }
 126
 127 struct proto_hdr *proto_header_push(enum proto_id pid)
 128 {
 129         struct packet *pkt = current_packet();
 130         struct proto_hdr **headers = &pkt->headers[0];
 131         const struct proto_ops *ops = proto_ops_by_id(pid);
 132         struct proto_hdr *hdr;
 133
 134         bug_on(pkt->headers_count >= PROTO_MAX_LAYERS);
 135
 136         hdr = xzmalloc(sizeof(*hdr));
 137         hdr->ops = ops;
 138         hdr->pkt_id = current_packet_id();
 139
 140         if (ops && ops->header_init)
 141                 ops->header_init(hdr);
 142
 143         /* This is very important to have it after header_init as
 144          * pkt->headers_count might be changed by adding default lower headers */
 145         hdr->index = pkt->headers_count;
 146
 147         headers[pkt->headers_count++] = hdr;
 148         return hdr;
 149 }
 150
 151 void proto_header_finish(struct proto_hdr *hdr)
 152 {
 153         if (hdr && hdr->ops && hdr->ops->header_finish)
 154                 hdr->ops->header_finish(hdr);
 155 }
 156
 157 struct proto_hdr *proto_lower_default_add(struct proto_hdr *upper,
 158                                           enum proto_id pid)
 159 {
 160         struct proto_hdr *current;
 161         size_t headers_count = current_packet()->headers_count;
 162         const struct proto_ops *ops;
 163
 164         if (headers_count > 0) {
 165                 current = current_packet()->headers[headers_count - 1];
 166                 ops = current->ops;
 167
 168                 if (ops->layer >= proto_ops_by_id(pid)->layer)
 169                         goto set_proto;
 170                 if (ops->id == pid)
 171                         goto set_proto;
 172         }
 173
 174         current = proto_header_push(pid);
 175         ops = current->ops;
 176
 177 set_proto:
 178         if (ops && ops->set_next_proto)
 179                 ops->set_next_proto(current, upper->ops->id);
 180
 181         return current;
 182 }
 183
 184 static void __proto_field_set_bytes(struct proto_field *field,
 185                                     const uint8_t *bytes, bool is_default,
 186                                     bool is_be)
 187 {
 188         uint8_t *payload, *p8;
 189         uint16_t *p16;
 190         uint32_t *p32;
 191         uint32_t v32;
 192         uint16_t v16;
 193         uint8_t v8;
 194
 195         if (is_default && field->is_set)
 196                 return;
 197
 198         payload = &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 199
 200         if (field->len == 1) {
 201                 p8 = payload;
 202                 *p8 = field->mask ? *p8 & ~field->mask : *p8;
 203
 204                 v8 = field_shift_and_mask(field, *bytes);
 205                 v8 = field->mask ? (v8 | *p8) : v8;
 206
 207                 bytes = &v8;
 208         } else if (field->len == 2) {
 209                 p16 = (uint16_t *)payload;
 210                 *p16 = be16_to_cpu(*p16);
 211                 *p16 = cpu_to_be16(field->mask ? *p16 & ~field->mask : *p16);
 212
 213                 v16 = field_shift_and_mask(field, *(const uint16_t *)bytes);
 214                 v16 = is_be ? cpu_to_be16(v16) : v16;
 215                 v16 = field->mask ? (v16 | *p16) : v16;
 216
 217                 bytes = (uint8_t *)&v16;
 218         } else if (field->len == 4) {
 219                 p32 = (uint32_t *)payload;
 220                 *p32 = be32_to_cpu(*p32);
 221                 *p32 = cpu_to_be32(field->mask ? *p32 & ~field->mask : *p32);
 222
 223                 v32 = field_shift_and_mask(field, *(const uint32_t *)bytes);
 224                 v32 = is_be ? cpu_to_be32(v32) : v32;
 225                 v32 = field->mask ? (v32 | *p32) : v32;
 226
 227                 bytes = (uint8_t *)&v32;
 228         }
 229
 230         memcpy(payload, bytes, field->len);
 231
 232         if (!is_default)
 233                 field->is_set = true;
 234 }
 235
 236 void proto_hdr_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 237                                const uint8_t *bytes)
 238 {
 239         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 240
 241         __proto_field_set_bytes(field, bytes, false, false);
 242 }
 243
 244 static uint8_t *__proto_field_get_bytes(struct proto_field *field)
 245 {
 246         return &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 247 }
 248
 249 void proto_hdr_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 250 {
 251         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val);
 252 }
 253
 254 uint8_t proto_hdr_field_get_u8(struct proto_hdr *hdr, uint32_t fid)
 255 {
 256         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 257         uint8_t val = *__proto_field_get_bytes(field);
 258
 259         return field_unmask_and_unshift(field, val);
 260 }
 261
 262 void proto_hdr_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 263 {
 264         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val);
 265 }
 266
 267 uint16_t proto_hdr_field_get_u16(struct proto_hdr *hdr, uint32_t fid)
 268 {
 269         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 270         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 271
 272         return field_unmask_and_unshift(field, be16_to_cpu(val));
 273 }
 274
 275 void proto_hdr_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 276 {
 277         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val);
 278 }
 279
 280 uint32_t proto_hdr_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
 281 {
 282         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 283         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 284
 285         return field_unmask_and_unshift(field, be32_to_cpu(val));
 286 }
 287
 288 void proto_hdr_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid,
 289                                        const uint8_t *bytes)
 290 {
 291         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 292
 293         __proto_field_set_bytes(field, bytes, true, false);
 294 }
 295
 296 void proto_hdr_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 297 {
 298         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 299
 300         __proto_field_set_bytes(field, (uint8_t *)&val, true, false);
 301 }
 302
 303 void proto_hdr_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 304 {
 305         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 306
 307         __proto_field_set_bytes(field, (uint8_t *)&val, true, false);
 308 }
 309
 310 void proto_hdr_field_set_default_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 311 {
 312         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 313
 314         __proto_field_set_bytes(field, (uint8_t *)&val, true, false);
 315 }
 316
 317 void proto_hdr_field_set_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 318 {
 319         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 320
 321         __proto_field_set_bytes(field, (uint8_t *)&val, false, true);
 322 }
 323
 324 void proto_hdr_field_set_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 325 {
 326         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 327
 328         __proto_field_set_bytes(field, (uint8_t *)&val, false, true);
 329 }
 330
 331 void proto_hdr_field_set_default_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 332 {
 333         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 334
 335         __proto_field_set_bytes(field, (uint8_t *)&val, true, true);
 336 }
 337
 338 void proto_hdr_field_set_default_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 339 {
 340         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 341
 342         __proto_field_set_bytes(field, (uint8_t *)&val, true, true);
 343 }
 344
 345 static void __proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
 346                                           bool is_default)
 347 {
 348         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 349         uint8_t mac[ETH_ALEN];
 350         int ret;
 351
 352         if (proto_hdr_field_is_set(hdr, fid))
 353                 return;
 354
 355         ret = device_hw_address(ctx.dev, mac, sizeof(mac));
 356         if (ret < 0)
 357                 panic("Could not get device hw address\n");
 358
 359         __proto_field_set_bytes(field, mac, is_default, false);
 360 }
 361
 362 void proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 363 {
 364         __proto_hdr_field_set_dev_mac(hdr, fid, false);
 365 }
 366
 367 void proto_hdr_field_set_default_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 368 {
 369         __proto_hdr_field_set_dev_mac(hdr, fid, true);
 370 }
 371
 372 static void __proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 373                                            bool is_default)
 374 {
 375         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 376         struct sockaddr_storage ss = { };
 377         struct sockaddr_in *ss4;
 378         int ret;
 379
 380         if (proto_hdr_field_is_set(hdr, fid))
 381                 return;
 382
 383         ret = device_address(ctx.dev, AF_INET, &ss);
 384         if (ret < 0) {
 385                 fprintf(stderr, "Warning: Could not get device IPv4 address for %s\n", ctx.dev);
 386                 return;
 387         }
 388
 389         ss4 = (struct sockaddr_in *) &ss;
 390         __proto_field_set_bytes(field, (uint8_t *)&ss4->sin_addr.s_addr, is_default, false);
 391 }
 392
 393 void proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 394 {
 395         __proto_hdr_field_set_dev_ipv4(hdr, fid, false);
 396 }
 397
 398 void proto_hdr_field_set_default_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 399 {
 400         __proto_hdr_field_set_dev_ipv4(hdr, fid, true);
 401 }
 402
 403 static void __proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid,
 404                                            bool is_default)
 405 {
 406         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 407         struct sockaddr_storage ss = { };
 408         struct sockaddr_in6 *ss6;
 409         int ret;
 410
 411         if (proto_hdr_field_is_set(hdr, fid))
 412                 return;
 413
 414         ret = device_address(ctx.dev, AF_INET6, &ss);
 415         if (ret < 0) {
 416                 fprintf(stderr, "Warning: Could not get device IPv6 address for %s\n", ctx.dev);
 417                 return;
 418         }
 419
 420         ss6 = (struct sockaddr_in6 *) &ss;
 421         __proto_field_set_bytes(field, (uint8_t *)&ss6->sin6_addr.s6_addr, is_default, false);
 422 }
 423
 424 void proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 425 {
 426         __proto_hdr_field_set_dev_ipv6(hdr, fid, false);
 427 }
 428
 429 void proto_hdr_field_set_default_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 430 {
 431         __proto_hdr_field_set_dev_ipv6(hdr, fid, true);
 432 }
 433
 434 void proto_field_set_u8(struct proto_field *field, uint8_t val)
 435 {
 436         __proto_field_set_bytes(field, &val, false, false);
 437 }
 438
 439 uint8_t proto_field_get_u8(struct proto_field *field)
 440 {
 441         uint8_t val = *__proto_field_get_bytes(field);
 442
 443         return field_unmask_and_unshift(field, val);
 444 }
 445
 446 void proto_field_set_u16(struct proto_field *field, uint16_t val)
 447 {
 448         __proto_field_set_bytes(field, (uint8_t *)&val, false, false);
 449 }
 450
 451 uint16_t proto_field_get_u16(struct proto_field *field)
 452 {
 453         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 454
 455         return field_unmask_and_unshift(field, be16_to_cpu(val));
 456 }
 457
 458 void proto_field_set_u32(struct proto_field *field, uint32_t val)
 459 {
 460         __proto_field_set_bytes(field, (uint8_t *)&val, false, false);
 461 }
 462
 463 uint32_t proto_field_get_u32(struct proto_field *field)
 464 {
 465         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 466
 467         return field_unmask_and_unshift(field, be32_to_cpu(val));
 468 }
 469
 470 void proto_field_set_be16(struct proto_field *field, uint16_t val)
 471 {
 472         __proto_field_set_bytes(field, (uint8_t *)&val, false, true);
 473 }
 474
 475 void proto_field_set_be32(struct proto_field *field, uint32_t val)
 476 {
 477         __proto_field_set_bytes(field, (uint8_t *)&val, false, true);
 478 }
 479
 480 void protos_init(const char *dev)
 481 {
 482         ctx.dev = dev;
 483
 484         protos_l2_init();
 485         protos_l3_init();
 486         protos_l4_init();
 487 }
 488
 489 void proto_packet_update(uint32_t idx)
 490 {
 491         struct packet *pkt = packet_get(idx);
 492         ssize_t i;
 493
 494         for (i = pkt->headers_count - 1; i >= 0; i--) {
 495                 struct proto_hdr *hdr = pkt->headers[i];
 496
 497                 if (hdr->ops->packet_update)
 498                         hdr->ops->packet_update(hdr);
 499         }
 500 }
 501
 502 void proto_packet_finish(void)
 503 {
 504         struct proto_hdr **headers = current_packet()->headers;
 505         size_t headers_count = current_packet()->headers_count;
 506         ssize_t i;
 507
 508         /* Go down from upper layers to do last calculations (checksum) */
 509         for (i = headers_count - 1; i >= 0; i--) {
 510                 struct proto_hdr *hdr = headers[i];
 511                 const struct proto_ops *ops = hdr->ops;
 512
 513                 if (ops && ops->packet_finish)
 514                         ops->packet_finish(hdr);
 515         }
 516 }
 517
 518 static inline uint32_t field_inc(struct proto_field *field)
 519 {
 520         uint32_t min = field->func.min;
 521         uint32_t max = field->func.max;
 522         uint32_t val = field->func.val;
 523         uint32_t inc = field->func.inc;
 524         uint32_t next;
 525
 526         next = (val + inc) % (max + 1);
 527         field->func.val = max(next, min);
 528
 529         return val;
 530 }
 531
 532 static void field_inc_func(struct proto_field *field)
 533 {
 534         if (field->len == 1) {
 535                 proto_field_set_u8(field, field_inc(field));
 536         } else if (field->len == 2) {
 537                 proto_field_set_be16(field, field_inc(field));
 538         } else if (field->len == 4) {
 539                 proto_field_set_be32(field, field_inc(field));
 540         } else if (field->len > 4) {
 541                 uint8_t *bytes = __proto_field_get_bytes(field);
 542
 543                 bytes += field->len - 4;
 544
 545                 *(uint32_t *)bytes = bswap_32(field_inc(field));
 546         }
 547 }
 548
 549 static inline uint32_t field_rand(struct proto_field *field)
 550 {
 551         return field->func.min + (rand() % ((field->func.max - field->func.min) + 1));
 552 }
 553
 554 static void field_rnd_func(struct proto_field *field)
 555 {
 556         if (field->len == 1) {
 557                 proto_field_set_u8(field, (uint8_t) field_rand(field));
 558         } else if (field->len == 2) {
 559                 proto_field_set_be16(field, (uint16_t) field_rand(field));
 560         } else if (field->len == 4) {
 561                 proto_field_set_be32(field, (uint32_t) field_rand(field));
 562         } else if (field->len > 4) {
 563                 uint8_t *bytes = __proto_field_get_bytes(field);
 564                 uint32_t i;
 565
 566                 for (i = 0; i < field->len; i++)
 567                         bytes[i] = (uint8_t) field_rand(field);
 568         }
 569 }
 570
 571 void proto_field_func_add(struct proto_field *field,
 572                           struct proto_field_func *func)
 573 {
 574         bug_on(!func);
 575
 576         field->func.update_field = func->update_field;
 577         field->func.type = func->type;
 578         field->func.max = func->max ?: UINT32_MAX - 1;
 579         field->func.min = func->min;
 580         field->func.inc = func->inc;
 581
 582         if (func->type & PROTO_FIELD_FUNC_INC) {
 583                 if (func->type & PROTO_FIELD_FUNC_MIN)
 584                         field->func.val = func->min;
 585                 else if (field->len == 1)
 586                         field->func.val = proto_field_get_u8(field);
 587                 else if (field->len == 2)
 588                         field->func.val = proto_field_get_u16(field);
 589                 else if (field->len == 4)
 590                         field->func.val = proto_field_get_u32(field);
 591                 else if (field->len > 4) {
 592                         uint8_t *bytes = __proto_field_get_bytes(field);
 593
 594                         bytes += field->len - 4;
 595                         field->func.val = bswap_32(*(uint32_t *)bytes);
 596                 }
 597
 598                 field->func.update_field = field_inc_func;
 599         } else if (func->type & PROTO_FIELD_FUNC_RND) {
 600                 field->func.update_field = field_rnd_func;
 601         }
 602 }
 603
 604 void proto_field_dyn_apply(struct proto_field *field)
 605 {
 606         if (field->func.update_field)
 607                 field->func.update_field(field);
 608
 609         if (field->hdr->ops->field_changed)
 610                 field->hdr->ops->field_changed(field);
 611 }
 612
 613 const char *proto_dev_get(void)
 614 {
 615         return ctx.dev;
 616 }