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