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 static struct proto_ctx ctx;
  26
  27 #define PROTO_MAX_LAYERS        16
  28
  29 static struct proto_hdr *headers[PROTO_MAX_LAYERS];
  30 static size_t headers_count;
  31
  32 static struct proto_hdr *registered;
  33
  34 struct proto_hdr *proto_lower_header(struct proto_hdr *hdr)
  35 {
  36         struct proto_hdr *lower = NULL;
  37         size_t i;
  38
  39         if (headers_count == 0)
  40                 return NULL;
  41
  42         for (i = 1, lower = headers[0]; i < headers_count; i++) {
  43                 if (headers[i] == hdr)
  44                         return headers[i - 1];
  45         }
  46
  47         return lower;
  48 }
  49
  50 uint8_t *proto_header_ptr(struct proto_hdr *hdr)
  51 {
  52         return &packet_get(hdr->pkt_id)->payload[hdr->pkt_offset];
  53 }
  54
  55 static struct proto_hdr *proto_header_by_id(enum proto_id id)
  56 {
  57         struct proto_hdr *p = registered;
  58
  59         for (; p; p = p->next)
  60                 if (p->id == id)
  61                         return p;
  62
  63         panic("Can't lookup proto by id %u\n", id);
  64 }
  65
  66 void proto_header_register(struct proto_hdr *hdr)
  67 {
  68         hdr->next = registered;
  69         registered = hdr;
  70
  71         hdr->fields = NULL;
  72         hdr->fields_count = 0;
  73 }
  74
  75 static void proto_fields_realloc(struct proto_hdr *hdr, size_t count)
  76 {
  77         hdr->fields = xrealloc(hdr->fields, count * sizeof(*hdr->fields));
  78         hdr->fields_count = count;
  79 }
  80
  81 void proto_header_fields_add(struct proto_hdr *hdr,
  82                              const struct proto_field *fields, size_t count)
  83 {
  84         struct packet *pkt = packet_get(hdr->pkt_id);
  85         struct proto_field *f;
  86         int i;
  87
  88         if (!hdr->fields)
  89                 hdr->pkt_offset = pkt->len;
  90
  91         proto_fields_realloc(hdr, hdr->fields_count + count);
  92
  93         for (i = 0; count >= 1; count--, i++) {
  94                 f = &hdr->fields[hdr->fields_count - count];
  95
  96                 f->id = fields[i].id;
  97                 f->len = fields[i].len;
  98                 f->is_set = false;
  99                 f->shift = fields[i].shift;
 100                 f->mask = fields[i].mask;
 101                 f->pkt_offset = hdr->pkt_offset + fields[i].offset;
 102                 f->hdr = hdr;
 103
 104                 if (f->pkt_offset + f->len > pkt->len) {
 105                         hdr->len += f->len;
 106                         set_fill(0, (f->pkt_offset + f->len) - pkt->len);
 107                 }
 108         }
 109 }
 110
 111 static struct proto_field *proto_field_by_id(struct proto_hdr *hdr, uint32_t fid)
 112 {
 113         int i;
 114
 115         for (i = 0; i < hdr->fields_count; i++)
 116                 if (hdr->fields[i].id == fid)
 117                         return &hdr->fields[i];
 118
 119         panic("Failed lookup field id %u for proto id %u\n", fid, hdr->id);
 120 }
 121
 122 bool proto_field_is_set(struct proto_hdr *hdr, uint32_t fid)
 123 {
 124         struct proto_field *field = proto_field_by_id(hdr, fid);
 125
 126         return field ? field->is_set : false;
 127 }
 128
 129 struct proto_hdr *proto_header_init(enum proto_id pid)
 130 {
 131         struct proto_hdr *hdr = proto_header_by_id(pid);
 132         struct proto_hdr *new_hdr;
 133
 134         bug_on(headers_count >= PROTO_MAX_LAYERS);
 135
 136         new_hdr = xmalloc(sizeof(*new_hdr));
 137         memcpy(new_hdr, hdr, sizeof(*new_hdr));
 138
 139         new_hdr->pkt_id = current_packet_id();
 140
 141         if (new_hdr->header_init)
 142                 new_hdr->header_init(new_hdr);
 143
 144         headers[headers_count++] = new_hdr;
 145         return new_hdr;
 146 }
 147
 148 void proto_header_finish(struct proto_hdr *hdr)
 149 {
 150         if (hdr && hdr->header_finish)
 151                 hdr->header_finish(hdr);
 152 }
 153
 154 struct proto_hdr *proto_lower_default_add(struct proto_hdr *hdr,
 155                                           enum proto_id pid)
 156 {
 157         struct proto_hdr *current;
 158
 159         if (headers_count > 0) {
 160                 current = headers[headers_count - 1];
 161
 162                 if (current->layer >= proto_header_by_id(pid)->layer)
 163                         goto set_proto;
 164                 if (current->id == pid)
 165                         goto set_proto;
 166         }
 167
 168         current = proto_header_init(pid);
 169
 170 set_proto:
 171         if (current->set_next_proto)
 172                 current->set_next_proto(current, hdr->id);
 173
 174         return current;
 175 }
 176
 177 static void __proto_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 178                                     uint8_t *bytes, bool is_default, bool is_be)
 179 {
 180         struct proto_field *field;
 181         uint8_t *payload, *p8;
 182         uint16_t *p16;
 183         uint32_t *p32;
 184         uint32_t v32;
 185         uint16_t v16;
 186         uint8_t v8;
 187
 188         field = proto_field_by_id(hdr, fid);
 189
 190         if (is_default && field->is_set)
 191                 return;
 192
 193         payload = &packet_get(hdr->pkt_id)->payload[field->pkt_offset];
 194
 195         if (field->len == 1) {
 196                 p8 = payload;
 197                 *p8 = field->mask ? *p8 & ~field->mask : *p8;
 198
 199                 v8 = field_shift_and_mask(field, *bytes);
 200                 v8 = field->mask ? (v8 | *p8) : v8;
 201
 202                 bytes = &v8;
 203         } else if (field->len == 2) {
 204                 p16 = (uint16_t *)payload;
 205                 *p16 = be16_to_cpu(*p16);
 206                 *p16 = cpu_to_be16(field->mask ? *p16 & ~field->mask : *p16);
 207
 208                 v16 = field_shift_and_mask(field, *(uint16_t *)bytes);
 209                 v16 = is_be ? cpu_to_be16(v16) : v16;
 210                 v16 = field->mask ? (v16 | *p16) : v16;
 211
 212                 bytes = (uint8_t *)&v16;
 213         } else if (field->len == 4) {
 214                 p32 = (uint32_t *)payload;
 215                 *p32 = be32_to_cpu(*p32);
 216                 *p32 = cpu_to_be32(field->mask ? *p32 & ~field->mask : *p32);
 217
 218                 v32 = field_shift_and_mask(field, *(uint32_t *)bytes);
 219                 v32 = is_be ? cpu_to_be32(v32) : v32;
 220                 v32 = field->mask ? (v32 | *p32) : v32;
 221
 222                 bytes = (uint8_t *)&v32;
 223         }
 224
 225         memcpy(payload, bytes, field->len);
 226
 227         if (!is_default)
 228                 field->is_set = true;
 229 }
 230
 231 void proto_field_set_bytes(struct proto_hdr *hdr, uint32_t fid, uint8_t *bytes)
 232 {
 233         __proto_field_set_bytes(hdr, fid, bytes, false, false);
 234 }
 235
 236 static uint8_t *__proto_field_get_bytes(struct proto_field *field)
 237 {
 238         return &packet_get(field->hdr->pkt_id)->payload[field->pkt_offset];
 239 }
 240
 241 void proto_field_set_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 242 {
 243         proto_field_set_bytes(hdr, fid, (uint8_t *)&val);
 244 }
 245
 246 uint8_t proto_field_get_u8(struct proto_hdr *hdr, uint32_t fid)
 247 {
 248         struct proto_field *field = proto_field_by_id(hdr, fid);
 249         uint8_t val = *__proto_field_get_bytes(field);
 250
 251         return field_unmask_and_unshift(field, val);
 252 }
 253
 254 void proto_field_set_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 255 {
 256         proto_field_set_bytes(hdr, fid, (uint8_t *)&val);
 257 }
 258
 259 uint16_t proto_field_get_u16(struct proto_hdr *hdr, uint32_t fid)
 260 {
 261         struct proto_field *field = proto_field_by_id(hdr, fid);
 262         uint16_t val = *(uint16_t *)__proto_field_get_bytes(field);
 263
 264         return field_unmask_and_unshift(field, be16_to_cpu(val));
 265 }
 266
 267 void proto_field_set_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 268 {
 269         proto_field_set_bytes(hdr, fid, (uint8_t *)&val);
 270 }
 271
 272 uint32_t proto_field_get_u32(struct proto_hdr *hdr, uint32_t fid)
 273 {
 274         struct proto_field *field = proto_field_by_id(hdr, fid);
 275         uint32_t val = *(uint32_t *)__proto_field_get_bytes(field);
 276
 277         return field_unmask_and_unshift(field, be32_to_cpu(val));
 278 }
 279
 280 void proto_field_set_default_bytes(struct proto_hdr *hdr, uint32_t fid, uint8_t *bytes)
 281 {
 282         __proto_field_set_bytes(hdr, fid, bytes, true, false);
 283 }
 284
 285 void proto_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 286 {
 287         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 288 }
 289
 290 void proto_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 291 {
 292         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 293 }
 294
 295 void proto_field_set_default_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 296 {
 297         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 298 }
 299
 300 void proto_field_set_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 301 {
 302         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, false, true);
 303 }
 304
 305 void proto_field_set_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 306 {
 307         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, false, true);
 308 }
 309
 310 void proto_field_set_default_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 311 {
 312         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, true);
 313 }
 314
 315 void proto_field_set_default_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 316 {
 317         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, true);
 318 }
 319
 320 static void __proto_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
 321                                       bool is_default)
 322 {
 323         uint8_t mac[ETH_ALEN];
 324         int ret;
 325
 326         if (proto_field_is_set(hdr, fid))
 327                 return;
 328
 329         if (!hdr->ctx->dev)
 330                 panic("Device is not specified\n");
 331
 332         ret = device_hw_address(hdr->ctx->dev, mac, sizeof(mac));
 333         if (ret < 0)
 334                 panic("Could not get device hw address\n");
 335
 336         __proto_field_set_bytes(hdr, fid, mac, is_default, false);
 337 }
 338
 339 void proto_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 340 {
 341         __proto_field_set_dev_mac(hdr, fid, false);
 342 }
 343
 344 void proto_field_set_default_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 345 {
 346         __proto_field_set_dev_mac(hdr, fid, true);
 347 }
 348
 349 static void __proto_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 350                                        bool is_default)
 351 {
 352         struct sockaddr_storage ss = { };
 353         struct sockaddr_in *ss4;
 354         int ret;
 355
 356         if (proto_field_is_set(hdr, fid))
 357                 return;
 358
 359         ret = device_address(hdr->ctx->dev, AF_INET, &ss);
 360         if (ret < 0)
 361                 panic("Could not get device IPv4 address\n");
 362
 363         ss4 = (struct sockaddr_in *) &ss;
 364         __proto_field_set_bytes(hdr, fid, (uint8_t *)&ss4->sin_addr.s_addr, is_default, false);
 365 }
 366
 367 void proto_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 368 {
 369         __proto_field_set_dev_ipv4(hdr, fid, false);
 370 }
 371
 372 void proto_field_set_default_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 373 {
 374         __proto_field_set_dev_ipv4(hdr, fid, true);
 375 }
 376
 377 static void __proto_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid,
 378                                        bool is_default)
 379 {
 380         struct sockaddr_storage ss = { };
 381         struct sockaddr_in6 *ss6;
 382         int ret;
 383
 384         if (proto_field_is_set(hdr, fid))
 385                 return;
 386
 387         ret = device_address(hdr->ctx->dev, AF_INET6, &ss);
 388         if (ret < 0)
 389                 panic("Could not get device IPv6 address\n");
 390
 391         ss6 = (struct sockaddr_in6 *) &ss;
 392         __proto_field_set_bytes(hdr, fid, (uint8_t *)&ss6->sin6_addr.s6_addr, is_default, false);
 393 }
 394
 395 void proto_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 396 {
 397         __proto_field_set_dev_ipv6(hdr, fid, false);
 398 }
 399
 400 void proto_field_set_default_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 401 {
 402         __proto_field_set_dev_ipv6(hdr, fid, true);
 403 }
 404
 405 void protos_init(const char *dev)
 406 {
 407         struct proto_hdr *p;
 408
 409         ctx.dev = dev;
 410
 411         protos_l2_init();
 412         protos_l3_init();
 413         protos_l4_init();
 414
 415         for (p = registered; p; p = p->next)
 416                 p->ctx = &ctx;
 417 }
 418
 419 void proto_packet_finish(void)
 420 {
 421         ssize_t i;
 422
 423         /* Go down from upper layers to do last calculations (checksum) */
 424         for (i = headers_count - 1; i >= 0; i--) {
 425                 struct proto_hdr *p = headers[i];
 426
 427                 if (p->packet_finish)
 428                         p->packet_finish(p);
 429         }
 430
 431         for (i = 0; i < headers_count; i++) {
 432                 struct proto_hdr *p = headers[i];
 433
 434                 if (p->fields) {
 435                         xfree(p->fields);
 436                         p->fields_count = 0;
 437                 }
 438
 439                 xfree(headers[i]);
 440         }
 441
 442         headers_count = 0;
 443 }