trafgen_proto.c

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