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_l7.h"
  18 #include "trafgen_proto.h"
  19
  20 #define field_shift_and_mask(f, v) (((v) << (f)->shift) & \
  21                 ((f)->mask ? (f)->mask : (0xffffffff)))
  22
  23 #define field_unmask_and_unshift(f, v) (((v) & \
  24                 ((f)->mask ? (f)->mask : (0xffffffff))) >> (f)->shift)
  25
  26 struct ctx {
  27         const char *dev;
  28 };
  29 static struct ctx ctx;
  30
  31 static const struct proto_ops *registered_ops[__PROTO_MAX];
  32
  33 struct proto_hdr *proto_lower_header(struct proto_hdr *hdr)
  34 {
  35         struct packet *pkt = packet_get(hdr->pkt_id);
  36         struct proto_hdr **headers = &pkt->headers[0];
  37
  38         if (hdr->index == 0)
  39                 return NULL;
  40
  41         return headers[hdr->index - 1];
  42 }
  43
  44 struct proto_hdr *proto_upper_header(struct proto_hdr *hdr)
  45 {
  46         struct packet *pkt = packet_get(hdr->pkt_id);
  47         struct proto_hdr **headers = &pkt->headers[0];
  48         size_t headers_count = pkt->headers_count;
  49
  50         if (hdr->index == headers_count - 1)
  51                 return NULL;
  52
  53         return headers[hdr->index + 1];
  54 }
  55
  56 uint8_t *proto_header_ptr(struct proto_hdr *hdr)
  57 {
  58         return &packet_get(hdr->pkt_id)->payload[hdr->pkt_offset];
  59 }
  60
  61 static const struct proto_ops *proto_ops_by_id(enum proto_id id)
  62 {
  63         const struct proto_ops *ops = registered_ops[id];
  64
  65         bug_on(ops->id != id);
  66         return ops;
  67 }
  68
  69 void proto_ops_register(const struct proto_ops *ops)
  70 {
  71         bug_on(ops->id >= __PROTO_MAX);
  72         registered_ops[ops->id] = ops;
  73 }
  74
  75 static void proto_fields_realloc(struct proto_hdr *hdr, size_t count)
  76 {
  77         hdr->fields = xrealloc(hdr->fields, count * sizeof(*hdr->fields));
  78         hdr->fields_count = count;
  79 }
  80
  81 void proto_header_fields_add(struct proto_hdr *hdr,
  82                              const struct proto_field *fields, size_t count)
  83 {
  84         struct packet *pkt = packet_get(hdr->pkt_id);
  85         struct proto_field *f;
  86         int i;
  87
  88         if (!hdr->fields)
  89                 hdr->pkt_offset = pkt->len;
  90
  91         proto_fields_realloc(hdr, hdr->fields_count + count);
  92
  93         for (i = 0; count >= 1; count--, i++) {
  94                 f = &hdr->fields[hdr->fields_count - count];
  95
  96                 f->id = fields[i].id;
  97                 f->len = fields[i].len;
  98                 f->is_set = false;
  99                 f->shift = fields[i].shift;
 100                 f->mask = fields[i].mask;
 101                 f->pkt_offset = hdr->pkt_offset + fields[i].offset;
 102                 f->hdr = hdr;
 103
 104                 if (!f->len)
 105                         continue;
 106
 107                 if (f->pkt_offset + f->len > pkt->len) {
 108                         hdr->len += f->len;
 109                         set_fill(0, (f->pkt_offset + f->len) - pkt->len);
 110                 }
 111         }
 112 }
 113
 114 struct proto_field *proto_hdr_field_by_id(struct proto_hdr *hdr, uint32_t fid)
 115 {
 116         /* Assume the fields are stored in the same order as the respective
 117          * enum, so the index can be used for faster lookup here.
 118          */
 119         bug_on(hdr->fields[fid].id != fid);
 120
 121         return &hdr->fields[fid];
 122 }
 123
 124 bool proto_hdr_field_is_set(struct proto_hdr *hdr, uint32_t fid)
 125 {
 126         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 127
 128         return field ? field->is_set : false;
 129 }
 130
 131 struct proto_hdr *proto_header_push(enum proto_id pid)
 132 {
 133         struct packet *pkt = current_packet();
 134         struct proto_hdr **headers = &pkt->headers[0];
 135         const struct proto_ops *ops = proto_ops_by_id(pid);
 136         struct proto_hdr *hdr;
 137
 138         bug_on(pkt->headers_count >= PROTO_MAX_LAYERS);
 139
 140         hdr = xzmalloc(sizeof(*hdr));
 141         hdr->ops = ops;
 142         hdr->pkt_id = current_packet_id();
 143
 144         if (ops && ops->header_init)
 145                 ops->header_init(hdr);
 146
 147         /* This is very important to have it after header_init as
 148          * pkt->headers_count might be changed by adding default lower headers */
 149         hdr->index = pkt->headers_count;
 150
 151         headers[pkt->headers_count++] = hdr;
 152         return hdr;
 153 }
 154
 155 void proto_header_finish(struct proto_hdr *hdr)
 156 {
 157         if (hdr && hdr->ops && hdr->ops->header_finish)
 158                 hdr->ops->header_finish(hdr);
 159 }
 160
 161 struct proto_hdr *proto_hdr_push_sub_header(struct proto_hdr *hdr, int id)
 162 {
 163         struct proto_hdr *sub_hdr;
 164
 165         sub_hdr = xzmalloc(sizeof(struct proto_hdr));
 166         sub_hdr->index = hdr->sub_headers_count;
 167         sub_hdr->parent = hdr;
 168         sub_hdr->id = id;
 169
 170         hdr->sub_headers_count++;
 171         hdr->sub_headers = xrealloc(hdr->sub_headers,
 172                                     hdr->sub_headers_count * sizeof(struct proto_hdr *));
 173
 174         hdr->sub_headers[hdr->sub_headers_count - 1] = sub_hdr;
 175
 176         if (hdr->ops->push_sub_header)
 177                 hdr->ops->push_sub_header(hdr, sub_hdr);
 178
 179         if (sub_hdr->ops->header_init)
 180                 sub_hdr->ops->header_init(sub_hdr);
 181
 182         return sub_hdr;
 183 }
 184
 185 static void __proto_hdr_set_offset(struct proto_hdr *hdr, uint16_t pkt_offset)
 186 {
 187         size_t i;
 188
 189         hdr->pkt_offset = pkt_offset;
 190
 191         for (i = 0; i < hdr->fields_count; i++) {
 192                 struct proto_field *f = &hdr->fields[i];
 193
 194                 f->pkt_offset = pkt_offset + f->offset;
 195         }
 196 }
 197
 198 void proto_hdr_move_sub_header(struct proto_hdr *hdr, struct proto_hdr *from,
 199                                struct proto_hdr *to)
 200 {
 201         struct proto_hdr *src_hdr, *dst_hdr, *tmp;
 202         uint8_t *src_ptr, *dst_ptr;
 203         uint16_t to_pkt_offset;
 204         uint16_t to_index;
 205         uint16_t pkt_offset;
 206         int idx_shift;
 207         size_t len = 0;
 208         uint8_t *buf;
 209         int i;
 210
 211         if (hdr->sub_headers_count < 2)
 212                 return;
 213         if (from->index == to->index)
 214                 return;
 215
 216         buf = xmemdupz(proto_header_ptr(from), from->len);
 217
 218         to_pkt_offset = to->pkt_offset;
 219         to_index = to->index;
 220
 221         if (from->index < to->index) {
 222                 src_hdr = hdr->sub_headers[from->index + 1];
 223                 dst_hdr = to;
 224
 225                 src_ptr = proto_header_ptr(src_hdr);
 226                 dst_ptr = proto_header_ptr(from);
 227                 len = (to->pkt_offset + to->len) - src_hdr->pkt_offset;
 228
 229                 pkt_offset = from->pkt_offset;
 230                 idx_shift = 1;
 231         } else {
 232                 src_hdr = to;
 233                 dst_hdr = hdr->sub_headers[from->index - 1];
 234
 235                 src_ptr = proto_header_ptr(src_hdr);
 236                 dst_ptr = src_ptr + from->len;
 237                 len = from->pkt_offset - to->pkt_offset;
 238
 239                 pkt_offset = to->pkt_offset + from->len;
 240                 idx_shift = -1;
 241         }
 242
 243         hdr->sub_headers[from->index] = to;
 244         hdr->sub_headers[to->index] = from;
 245
 246         for (i = src_hdr->index; i <= dst_hdr->index; i++) {
 247                 tmp = hdr->sub_headers[i];
 248
 249                 __proto_hdr_set_offset(tmp, pkt_offset);
 250                 pkt_offset += tmp->len;
 251         }
 252
 253         for (i = src_hdr->index; i <= dst_hdr->index; i++)
 254                 hdr->sub_headers[i]->index = i + idx_shift;
 255
 256         memmove(dst_ptr, src_ptr, len);
 257
 258         from->pkt_offset = to_pkt_offset;
 259         from->index = to_index;
 260
 261         memcpy(proto_header_ptr(from), buf, from->len);
 262
 263         xfree(buf);
 264 }
 265
 266 struct proto_hdr *proto_lower_default_add(struct proto_hdr *upper,
 267                                           enum proto_id pid)
 268 {
 269         struct proto_hdr *current;
 270         size_t headers_count = current_packet()->headers_count;
 271         const struct proto_ops *ops;
 272
 273         if (headers_count > 0) {
 274                 current = current_packet()->headers[headers_count - 1];
 275                 ops = current->ops;
 276
 277                 if (ops->layer >= proto_ops_by_id(pid)->layer)
 278                         goto set_proto;
 279                 if (ops->id == pid)
 280                         goto set_proto;
 281         }
 282
 283         current = proto_header_push(pid);
 284         ops = current->ops;
 285
 286 set_proto:
 287         if (ops && ops->set_next_proto)
 288                 ops->set_next_proto(current, upper->ops->id);
 289
 290         return current;
 291 }
 292
 293 static void __proto_field_relocate(struct proto_field *field)
 294 {
 295         struct proto_hdr *hdr = field->hdr;
 296         struct packet *pkt = packet_get(hdr->pkt_id);
 297         uint8_t *from, *to;
 298         int i;
 299
 300         /* If this is a last field then just calculate 'pkt_offset' */
 301         if (field->id == hdr->fields_count - 1) {
 302                 field->pkt_offset = hdr->pkt_offset + hdr->len - field->len;
 303                 return;
 304         }
 305
 306         /* Use 'pkt_offset' from the 1st real (len > 0) field after the
 307          * 'target' one */
 308         for (i = field->id + 1; i < hdr->fields_count; i++) {
 309                 if (hdr->fields[i].len == 0)
 310                         continue;
 311
 312                 field->pkt_offset = hdr->fields[i].pkt_offset;
 313                 break;
 314         }
 315
 316         /* Move payload of overlapped fields (each after the 'target' field) */
 317         from = &pkt->payload[field->pkt_offset];
 318         to = &pkt->payload[field->pkt_offset + field->len];
 319         memcpy(to, from, hdr->len - field->len);
 320
 321         /* Recalculate 'pkt_offset' of the rest fields */
 322         for (; i < hdr->fields_count; i++) {
 323                 struct proto_field *tmp = &hdr->fields[i];
 324
 325                 if (tmp->len == 0)
 326                         continue;
 327
 328                 tmp->pkt_offset += field->len;
 329         }
 330 }
 331
 332 static void __proto_field_set_bytes(struct proto_field *field,
 333                                     const uint8_t *bytes, size_t len,
 334                                     bool is_default, bool is_be)
 335 {
 336         uint8_t *payload, *p8;
 337         uint16_t *p16;
 338         uint32_t *p32;
 339         uint32_t v32;
 340         uint16_t v16;
 341         uint8_t v8;
 342
 343         if (is_default && field->is_set)
 344                 return;
 345
 346         if (field->len == 0) {
 347                 field->hdr->len += len;
 348                 field->len = len;
 349                 set_fill(0, len);
 350
 351                 __proto_field_relocate(field);
 352         }
 353
 354         payload = &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 355
 356         if (field->len == 1) {
 357                 p8 = payload;
 358                 *p8 = field->mask ? *p8 & ~field->mask : *p8;
 359
 360                 v8 = field_shift_and_mask(field, *bytes);
 361                 v8 = field->mask ? (v8 | *p8) : v8;
 362
 363                 bytes = &v8;
 364         } else if (field->len == 2) {
 365                 p16 = (uint16_t *)payload;
 366                 *p16 = be16_to_cpu(*p16);
 367                 *p16 = cpu_to_be16(field->mask ? *p16 & ~field->mask : *p16);
 368
 369                 v16 = field_shift_and_mask(field, *(const uint16_t *)bytes);
 370                 v16 = is_be ? cpu_to_be16(v16) : v16;
 371                 v16 = field->mask ? (v16 | *p16) : v16;
 372
 373                 bytes = (uint8_t *)&v16;
 374         } else if (field->len == 4) {
 375                 p32 = (uint32_t *)payload;
 376                 *p32 = be32_to_cpu(*p32);
 377                 *p32 = cpu_to_be32(field->mask ? *p32 & ~field->mask : *p32);
 378
 379                 v32 = field_shift_and_mask(field, *(const uint32_t *)bytes);
 380                 v32 = is_be ? cpu_to_be32(v32) : v32;
 381                 v32 = field->mask ? (v32 | *p32) : v32;
 382
 383                 bytes = (uint8_t *)&v32;
 384         }
 385
 386         memcpy(payload, bytes, field->len);
 387
 388         if (!is_default)
 389                 field->is_set = true;
 390 }
 391
 392 void proto_hdr_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 393                                const uint8_t *bytes, size_t len)
 394 {
 395         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 396
 397         __proto_field_set_bytes(field, bytes, len, false, false);
 398 }
 399
 400 static uint8_t *__proto_field_get_bytes(struct proto_field *field)
 401 {
 402         return &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 403 }
 404
 405 void proto_hdr_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 406 {
 407         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 1);
 408 }
 409
 410 uint8_t proto_hdr_field_get_u8(struct proto_hdr *hdr, uint32_t fid)
 411 {
 412         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 413         uint8_t val = *__proto_field_get_bytes(field);
 414
 415         return field_unmask_and_unshift(field, val);
 416 }
 417
 418 void proto_hdr_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 419 {
 420         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 2);
 421 }
 422
 423 uint16_t proto_hdr_field_get_u16(struct proto_hdr *hdr, uint32_t fid)
 424 {
 425         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 426         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 427
 428         return field_unmask_and_unshift(field, be16_to_cpu(val));
 429 }
 430
 431 void proto_hdr_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 432 {
 433         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)&val, 4);
 434 }
 435
 436 uint32_t proto_hdr_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
 437 {
 438         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 439         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 440
 441         return field_unmask_and_unshift(field, be32_to_cpu(val));
 442 }
 443
 444 void proto_hdr_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid,
 445                                        const uint8_t *bytes, size_t len)
 446 {
 447         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 448
 449         __proto_field_set_bytes(field, bytes, len, true, false);
 450 }
 451
 452 void proto_hdr_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 453 {
 454         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 455
 456         __proto_field_set_bytes(field, (uint8_t *)&val, 1, true, false);
 457 }
 458
 459 void proto_hdr_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 460 {
 461         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 462
 463         __proto_field_set_bytes(field, (uint8_t *)&val, 2, true, false);
 464 }
 465
 466 void proto_hdr_field_set_default_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 467 {
 468         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 469
 470         __proto_field_set_bytes(field, (uint8_t *)&val, 4, true, false);
 471 }
 472
 473 void proto_hdr_field_set_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 474 {
 475         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 476
 477         __proto_field_set_bytes(field, (uint8_t *)&val, 2, false, true);
 478 }
 479
 480 void proto_hdr_field_set_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 481 {
 482         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 483
 484         __proto_field_set_bytes(field, (uint8_t *)&val, 4, false, true);
 485 }
 486
 487 void proto_hdr_field_set_default_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 488 {
 489         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 490
 491         __proto_field_set_bytes(field, (uint8_t *)&val, 2, true, true);
 492 }
 493
 494 void proto_hdr_field_set_default_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 495 {
 496         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 497
 498         __proto_field_set_bytes(field, (uint8_t *)&val, 4, true, true);
 499 }
 500
 501 static void __proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
 502                                           bool is_default)
 503 {
 504         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 505         uint8_t mac[ETH_ALEN];
 506         int ret;
 507
 508         if (proto_hdr_field_is_set(hdr, fid))
 509                 return;
 510
 511         ret = device_hw_address(ctx.dev, mac, sizeof(mac));
 512         if (ret < 0)
 513                 panic("Could not get device hw address\n");
 514
 515         __proto_field_set_bytes(field, mac, 6, is_default, false);
 516 }
 517
 518 void proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 519 {
 520         __proto_hdr_field_set_dev_mac(hdr, fid, false);
 521 }
 522
 523 void proto_hdr_field_set_default_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 524 {
 525         __proto_hdr_field_set_dev_mac(hdr, fid, true);
 526 }
 527
 528 static void __proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 529                                            bool is_default)
 530 {
 531         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 532         struct sockaddr_storage ss = { };
 533         struct sockaddr_in *ss4;
 534         int ret;
 535
 536         if (proto_hdr_field_is_set(hdr, fid))
 537                 return;
 538
 539         ret = device_address(ctx.dev, AF_INET, &ss);
 540         if (ret < 0) {
 541                 fprintf(stderr, "Warning: Could not get device IPv4 address for %s\n", ctx.dev);
 542                 return;
 543         }
 544
 545         ss4 = (struct sockaddr_in *) &ss;
 546         __proto_field_set_bytes(field, (uint8_t *)&ss4->sin_addr.s_addr, 4, is_default, false);
 547 }
 548
 549 void proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 550 {
 551         __proto_hdr_field_set_dev_ipv4(hdr, fid, false);
 552 }
 553
 554 void proto_hdr_field_set_default_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 555 {
 556         __proto_hdr_field_set_dev_ipv4(hdr, fid, true);
 557 }
 558
 559 static void __proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid,
 560                                            bool is_default)
 561 {
 562         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 563         struct sockaddr_storage ss = { };
 564         struct sockaddr_in6 *ss6;
 565         int ret;
 566
 567         if (proto_hdr_field_is_set(hdr, fid))
 568                 return;
 569
 570         ret = device_address(ctx.dev, AF_INET6, &ss);
 571         if (ret < 0) {
 572                 fprintf(stderr, "Warning: Could not get device IPv6 address for %s\n", ctx.dev);
 573                 return;
 574         }
 575
 576         ss6 = (struct sockaddr_in6 *) &ss;
 577         __proto_field_set_bytes(field, (uint8_t *)&ss6->sin6_addr.s6_addr, 16, is_default, false);
 578 }
 579
 580 void proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 581 {
 582         __proto_hdr_field_set_dev_ipv6(hdr, fid, false);
 583 }
 584
 585 void proto_hdr_field_set_default_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 586 {
 587         __proto_hdr_field_set_dev_ipv6(hdr, fid, true);
 588 }
 589
 590 void proto_hdr_field_set_string(struct proto_hdr *hdr, uint32_t fid, const char *str)
 591 {
 592         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)str, strlen(str) + 1);
 593 }
 594
 595 void proto_hdr_field_set_default_string(struct proto_hdr *hdr, uint32_t fid, const char *str)
 596 {
 597         proto_hdr_field_set_default_bytes(hdr, fid, (uint8_t *)str, strlen(str) + 1);
 598 }
 599
 600 void proto_field_set_u8(struct proto_field *field, uint8_t val)
 601 {
 602         __proto_field_set_bytes(field, &val, 1, false, false);
 603 }
 604
 605 uint8_t proto_field_get_u8(struct proto_field *field)
 606 {
 607         uint8_t val = *__proto_field_get_bytes(field);
 608
 609         return field_unmask_and_unshift(field, val);
 610 }
 611
 612 void proto_field_set_u16(struct proto_field *field, uint16_t val)
 613 {
 614         __proto_field_set_bytes(field, (uint8_t *)&val, 2, false, false);
 615 }
 616
 617 uint16_t proto_field_get_u16(struct proto_field *field)
 618 {
 619         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 620
 621         return field_unmask_and_unshift(field, be16_to_cpu(val));
 622 }
 623
 624 void proto_field_set_u32(struct proto_field *field, uint32_t val)
 625 {
 626         __proto_field_set_bytes(field, (uint8_t *)&val, 4, false, false);
 627 }
 628
 629 uint32_t proto_field_get_u32(struct proto_field *field)
 630 {
 631         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 632
 633         return field_unmask_and_unshift(field, be32_to_cpu(val));
 634 }
 635
 636 void proto_field_set_be16(struct proto_field *field, uint16_t val)
 637 {
 638         __proto_field_set_bytes(field, (uint8_t *)&val, 2, false, true);
 639 }
 640
 641 void proto_field_set_be32(struct proto_field *field, uint32_t val)
 642 {
 643         __proto_field_set_bytes(field, (uint8_t *)&val, 4, false, true);
 644 }
 645
 646 void proto_field_set_bytes(struct proto_field *field, const uint8_t *bytes, size_t len)
 647 {
 648         __proto_field_set_bytes(field, bytes, len, false, false);
 649 }
 650
 651 void proto_field_set_string(struct proto_field *field, const char *str)
 652 {
 653         proto_field_set_bytes(field, (uint8_t *)str, strlen(str) + 1);
 654 }
 655
 656 void proto_field_set_default_string(struct proto_field *field, const char *str)
 657 {
 658         __proto_field_set_bytes(field, (uint8_t *)str, strlen(str) + 1, true, false);
 659 }
 660
 661 void protos_init(const char *dev)
 662 {
 663         ctx.dev = dev;
 664
 665         protos_l2_init();
 666         protos_l3_init();
 667         protos_l4_init();
 668         protos_l7_init();
 669 }
 670
 671 void proto_packet_update(uint32_t idx)
 672 {
 673         struct packet *pkt = packet_get(idx);
 674         ssize_t i;
 675
 676         for (i = pkt->headers_count - 1; i >= 0; i--) {
 677                 struct proto_hdr *hdr = pkt->headers[i];
 678
 679                 if (hdr->ops->packet_update)
 680                         hdr->ops->packet_update(hdr);
 681         }
 682 }
 683
 684 void proto_packet_finish(void)
 685 {
 686         struct proto_hdr **headers = current_packet()->headers;
 687         size_t headers_count = current_packet()->headers_count;
 688         ssize_t i;
 689
 690         /* Go down from upper layers to do last calculations (checksum) */
 691         for (i = headers_count - 1; i >= 0; i--) {
 692                 struct proto_hdr *hdr = headers[i];
 693                 const struct proto_ops *ops = hdr->ops;
 694
 695                 if (ops && ops->packet_finish)
 696                         ops->packet_finish(hdr);
 697         }
 698 }
 699
 700 static inline uint32_t field_inc(struct proto_field *field)
 701 {
 702         uint32_t min = field->func.min;
 703         uint32_t max = field->func.max;
 704         uint32_t val = field->func.val;
 705         uint32_t inc = field->func.inc;
 706         uint32_t next;
 707
 708         next = (val + inc) % (max + 1);
 709         field->func.val = max(next, min);
 710
 711         return val;
 712 }
 713
 714 static void field_inc_func(struct proto_field *field)
 715 {
 716         if (field->len == 1) {
 717                 proto_field_set_u8(field, field_inc(field));
 718         } else if (field->len == 2) {
 719                 proto_field_set_be16(field, field_inc(field));
 720         } else if (field->len == 4) {
 721                 proto_field_set_be32(field, field_inc(field));
 722         } else if (field->len > 4) {
 723                 uint8_t *bytes = __proto_field_get_bytes(field);
 724
 725                 bytes += field->len - 4;
 726
 727                 *(uint32_t *)bytes = bswap_32(field_inc(field));
 728         }
 729 }
 730
 731 static inline uint32_t field_rand(struct proto_field *field)
 732 {
 733         return field->func.min + (rand() % ((field->func.max - field->func.min) + 1));
 734 }
 735
 736 static void field_rnd_func(struct proto_field *field)
 737 {
 738         if (field->len == 1) {
 739                 proto_field_set_u8(field, (uint8_t) field_rand(field));
 740         } else if (field->len == 2) {
 741                 proto_field_set_be16(field, (uint16_t) field_rand(field));
 742         } else if (field->len == 4) {
 743                 proto_field_set_be32(field, (uint32_t) field_rand(field));
 744         } else if (field->len > 4) {
 745                 uint8_t *bytes = __proto_field_get_bytes(field);
 746                 uint32_t i;
 747
 748                 for (i = 0; i < field->len; i++)
 749                         bytes[i] = (uint8_t) field_rand(field);
 750         }
 751 }
 752
 753 void proto_field_func_add(struct proto_field *field,
 754                           struct proto_field_func *func)
 755 {
 756         bug_on(!func);
 757
 758         field->func.update_field = func->update_field;
 759         field->func.type = func->type;
 760         field->func.max = func->max ?: UINT32_MAX - 1;
 761         field->func.min = func->min;
 762         field->func.inc = func->inc;
 763
 764         if (func->type & PROTO_FIELD_FUNC_INC) {
 765                 if (func->type & PROTO_FIELD_FUNC_MIN)
 766                         field->func.val = func->min;
 767                 else if (field->len == 1)
 768                         field->func.val = proto_field_get_u8(field);
 769                 else if (field->len == 2)
 770                         field->func.val = proto_field_get_u16(field);
 771                 else if (field->len == 4)
 772                         field->func.val = proto_field_get_u32(field);
 773                 else if (field->len > 4) {
 774                         uint8_t *bytes = __proto_field_get_bytes(field);
 775
 776                         bytes += field->len - 4;
 777                         field->func.val = bswap_32(*(uint32_t *)bytes);
 778                 }
 779
 780                 field->func.update_field = field_inc_func;
 781         } else if (func->type & PROTO_FIELD_FUNC_RND) {
 782                 field->func.update_field = field_rnd_func;
 783         }
 784 }
 785
 786 void proto_field_dyn_apply(struct proto_field *field)
 787 {
 788         if (field->func.update_field)
 789                 field->func.update_field(field);
 790
 791         if (field->hdr->ops->field_changed)
 792                 field->hdr->ops->field_changed(field);
 793 }
 794
 795 const char *proto_dev_get(void)
 796 {
 797         return ctx.dev;
 798 }