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         struct dev_io *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         if (dev_io_is_netdev(ctx.dev)) {
 512                 ret = device_hw_address(dev_io_name_get(ctx.dev), mac, sizeof(mac));
 513                 if (ret < 0)
 514                         panic("Could not get device hw address\n");
 515
 516                 __proto_field_set_bytes(field, mac, 6, is_default, false);
 517         }
 518 }
 519
 520 void proto_hdr_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 521 {
 522         __proto_hdr_field_set_dev_mac(hdr, fid, false);
 523 }
 524
 525 void proto_hdr_field_set_default_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 526 {
 527         __proto_hdr_field_set_dev_mac(hdr, fid, true);
 528 }
 529
 530 static void __proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 531                                            bool is_default)
 532 {
 533         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 534         struct sockaddr_storage ss = { };
 535         struct sockaddr_in *ss4;
 536         int ret;
 537
 538         if (proto_hdr_field_is_set(hdr, fid))
 539                 return;
 540
 541         if (dev_io_is_netdev(ctx.dev)) {
 542                 ret = device_address(dev_io_name_get(ctx.dev), AF_INET, &ss);
 543                 if (ret < 0) {
 544                         fprintf(stderr, "Warning: Could not get device IPv4 address for %s\n",
 545                                 dev_io_name_get(ctx.dev));
 546                         return;
 547                 }
 548
 549                 ss4 = (struct sockaddr_in *) &ss;
 550                 __proto_field_set_bytes(field, (uint8_t *)&ss4->sin_addr.s_addr, 4, is_default, false);
 551         }
 552 }
 553
 554 void proto_hdr_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 555 {
 556         __proto_hdr_field_set_dev_ipv4(hdr, fid, false);
 557 }
 558
 559 void proto_hdr_field_set_default_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 560 {
 561         __proto_hdr_field_set_dev_ipv4(hdr, fid, true);
 562 }
 563
 564 static void __proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid,
 565                                            bool is_default)
 566 {
 567         struct proto_field *field = proto_hdr_field_by_id(hdr, fid);
 568         struct sockaddr_storage ss = { };
 569         struct sockaddr_in6 *ss6;
 570         int ret;
 571
 572         if (proto_hdr_field_is_set(hdr, fid))
 573                 return;
 574
 575         if (dev_io_is_netdev(ctx.dev)) {
 576                 ret = device_address(dev_io_name_get(ctx.dev), AF_INET6, &ss);
 577                 if (ret < 0) {
 578                         fprintf(stderr, "Warning: Could not get device IPv6 address for %s\n",
 579                                 dev_io_name_get(ctx.dev));
 580                         return;
 581                 }
 582
 583                 ss6 = (struct sockaddr_in6 *) &ss;
 584                 __proto_field_set_bytes(field, (uint8_t *)&ss6->sin6_addr.s6_addr, 16, is_default, false);
 585         }
 586 }
 587
 588 void proto_hdr_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 589 {
 590         __proto_hdr_field_set_dev_ipv6(hdr, fid, false);
 591 }
 592
 593 void proto_hdr_field_set_default_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 594 {
 595         __proto_hdr_field_set_dev_ipv6(hdr, fid, true);
 596 }
 597
 598 void proto_hdr_field_set_string(struct proto_hdr *hdr, uint32_t fid, const char *str)
 599 {
 600         proto_hdr_field_set_bytes(hdr, fid, (uint8_t *)str, strlen(str) + 1);
 601 }
 602
 603 void proto_hdr_field_set_default_string(struct proto_hdr *hdr, uint32_t fid, const char *str)
 604 {
 605         proto_hdr_field_set_default_bytes(hdr, fid, (uint8_t *)str, strlen(str) + 1);
 606 }
 607
 608 void proto_field_set_u8(struct proto_field *field, uint8_t val)
 609 {
 610         __proto_field_set_bytes(field, &val, 1, false, false);
 611 }
 612
 613 uint8_t proto_field_get_u8(struct proto_field *field)
 614 {
 615         uint8_t val = *__proto_field_get_bytes(field);
 616
 617         return field_unmask_and_unshift(field, val);
 618 }
 619
 620 void proto_field_set_u16(struct proto_field *field, uint16_t val)
 621 {
 622         __proto_field_set_bytes(field, (uint8_t *)&val, 2, false, false);
 623 }
 624
 625 uint16_t proto_field_get_u16(struct proto_field *field)
 626 {
 627         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 628
 629         return field_unmask_and_unshift(field, be16_to_cpu(val));
 630 }
 631
 632 void proto_field_set_u32(struct proto_field *field, uint32_t val)
 633 {
 634         __proto_field_set_bytes(field, (uint8_t *)&val, 4, false, false);
 635 }
 636
 637 uint32_t proto_field_get_u32(struct proto_field *field)
 638 {
 639         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 640
 641         return field_unmask_and_unshift(field, be32_to_cpu(val));
 642 }
 643
 644 void proto_field_set_be16(struct proto_field *field, uint16_t val)
 645 {
 646         __proto_field_set_bytes(field, (uint8_t *)&val, 2, false, true);
 647 }
 648
 649 void proto_field_set_be32(struct proto_field *field, uint32_t val)
 650 {
 651         __proto_field_set_bytes(field, (uint8_t *)&val, 4, false, true);
 652 }
 653
 654 void proto_field_set_bytes(struct proto_field *field, const uint8_t *bytes, size_t len)
 655 {
 656         __proto_field_set_bytes(field, bytes, len, false, false);
 657 }
 658
 659 void proto_field_set_string(struct proto_field *field, const char *str)
 660 {
 661         proto_field_set_bytes(field, (uint8_t *)str, strlen(str) + 1);
 662 }
 663
 664 void proto_field_set_default_string(struct proto_field *field, const char *str)
 665 {
 666         __proto_field_set_bytes(field, (uint8_t *)str, strlen(str) + 1, true, false);
 667 }
 668
 669 void protos_init(struct dev_io *dev)
 670 {
 671         ctx.dev = dev;
 672
 673         protos_l2_init();
 674         protos_l3_init();
 675         protos_l4_init();
 676         protos_l7_init();
 677 }
 678
 679 void proto_packet_update(uint32_t idx)
 680 {
 681         struct packet *pkt = packet_get(idx);
 682         ssize_t i;
 683
 684         for (i = pkt->headers_count - 1; i >= 0; i--) {
 685                 struct proto_hdr *hdr = pkt->headers[i];
 686
 687                 if (hdr->ops->packet_update)
 688                         hdr->ops->packet_update(hdr);
 689         }
 690 }
 691
 692 void proto_packet_finish(void)
 693 {
 694         struct proto_hdr **headers = current_packet()->headers;
 695         size_t headers_count = current_packet()->headers_count;
 696         ssize_t i;
 697
 698         /* Go down from upper layers to do last calculations (checksum) */
 699         for (i = headers_count - 1; i >= 0; i--) {
 700                 struct proto_hdr *hdr = headers[i];
 701                 const struct proto_ops *ops = hdr->ops;
 702
 703                 if (ops && ops->packet_finish)
 704                         ops->packet_finish(hdr);
 705         }
 706 }
 707
 708 static inline uint32_t field_inc(struct proto_field *field)
 709 {
 710         uint32_t min = field->func.min;
 711         uint32_t max = field->func.max;
 712         uint32_t val = field->func.val;
 713         uint32_t inc = field->func.inc;
 714         uint32_t next;
 715
 716         next = (val + inc) % (max + 1);
 717         field->func.val = max(next, min);
 718
 719         return val;
 720 }
 721
 722 static void field_inc_func(struct proto_field *field)
 723 {
 724         if (field->len == 1) {
 725                 proto_field_set_u8(field, field_inc(field));
 726         } else if (field->len == 2) {
 727                 proto_field_set_be16(field, field_inc(field));
 728         } else if (field->len == 4) {
 729                 proto_field_set_be32(field, field_inc(field));
 730         } else if (field->len > 4) {
 731                 uint8_t *bytes = __proto_field_get_bytes(field);
 732
 733                 bytes += field->len - 4;
 734
 735                 *(uint32_t *)bytes = bswap_32(field_inc(field));
 736         }
 737 }
 738
 739 static inline uint32_t field_rand(struct proto_field *field)
 740 {
 741         return field->func.min + (rand() % ((field->func.max - field->func.min) + 1));
 742 }
 743
 744 static void field_rnd_func(struct proto_field *field)
 745 {
 746         if (field->len == 1) {
 747                 proto_field_set_u8(field, (uint8_t) field_rand(field));
 748         } else if (field->len == 2) {
 749                 proto_field_set_be16(field, (uint16_t) field_rand(field));
 750         } else if (field->len == 4) {
 751                 proto_field_set_be32(field, (uint32_t) field_rand(field));
 752         } else if (field->len > 4) {
 753                 uint8_t *bytes = __proto_field_get_bytes(field);
 754                 uint32_t i;
 755
 756                 for (i = 0; i < field->len; i++)
 757                         bytes[i] = (uint8_t) field_rand(field);
 758         }
 759 }
 760
 761 void proto_field_func_add(struct proto_field *field,
 762                           struct proto_field_func *func)
 763 {
 764         bug_on(!func);
 765
 766         field->func.update_field = func->update_field;
 767         field->func.type = func->type;
 768         field->func.max = func->max ?: UINT32_MAX - 1;
 769         field->func.min = func->min;
 770         field->func.inc = func->inc;
 771
 772         if (func->type & PROTO_FIELD_FUNC_INC) {
 773                 if (func->type & PROTO_FIELD_FUNC_MIN)
 774                         field->func.val = func->min;
 775                 else if (field->len == 1)
 776                         field->func.val = proto_field_get_u8(field);
 777                 else if (field->len == 2)
 778                         field->func.val = proto_field_get_u16(field);
 779                 else if (field->len == 4)
 780                         field->func.val = proto_field_get_u32(field);
 781                 else if (field->len > 4) {
 782                         uint8_t *bytes = __proto_field_get_bytes(field);
 783
 784                         bytes += field->len - 4;
 785                         field->func.val = bswap_32(*(uint32_t *)bytes);
 786                 }
 787
 788                 field->func.update_field = field_inc_func;
 789         } else if (func->type & PROTO_FIELD_FUNC_RND) {
 790                 field->func.update_field = field_rnd_func;
 791         }
 792 }
 793
 794 void proto_field_dyn_apply(struct proto_field *field)
 795 {
 796         if (field->func.update_field)
 797                 field->func.update_field(field);
 798
 799         if (field->hdr->ops->field_changed)
 800                 field->hdr->ops->field_changed(field);
 801 }
 802
 803 struct dev_io *proto_dev_get(void)
 804 {
 805         return ctx.dev;
 806 }