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