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