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 proto_hdr **headers = current_packet()->headers;
  35         size_t headers_count = current_packet()->headers_count;
  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 const struct proto_ops *proto_ops_by_id(enum proto_id id)
  56 {
  57         const struct proto_ops *ops = registered_ops[id];
  58
  59         bug_on(ops->id != id);
  60         return ops;
  61 }
  62
  63 void proto_ops_register(const struct proto_ops *ops)
  64 {
  65         bug_on(ops->id >= __PROTO_MAX);
  66         registered_ops[ops->id] = ops;
  67 }
  68
  69 static void proto_fields_realloc(struct proto_hdr *hdr, size_t count)
  70 {
  71         hdr->fields = xrealloc(hdr->fields, count * sizeof(*hdr->fields));
  72         hdr->fields_count = count;
  73 }
  74
  75 void proto_header_fields_add(struct proto_hdr *hdr,
  76                              const struct proto_field *fields, size_t count)
  77 {
  78         struct packet *pkt = packet_get(hdr->pkt_id);
  79         struct proto_field *f;
  80         int i;
  81
  82         if (!hdr->fields)
  83                 hdr->pkt_offset = pkt->len;
  84
  85         proto_fields_realloc(hdr, hdr->fields_count + count);
  86
  87         for (i = 0; count >= 1; count--, i++) {
  88                 f = &hdr->fields[hdr->fields_count - count];
  89
  90                 f->id = fields[i].id;
  91                 f->len = fields[i].len;
  92                 f->is_set = false;
  93                 f->shift = fields[i].shift;
  94                 f->mask = fields[i].mask;
  95                 f->pkt_offset = hdr->pkt_offset + fields[i].offset;
  96                 f->hdr = hdr;
  97
  98                 if (f->pkt_offset + f->len > pkt->len) {
  99                         hdr->len += f->len;
 100                         set_fill(0, (f->pkt_offset + f->len) - pkt->len);
 101                 }
 102         }
 103 }
 104
 105 static struct proto_field *proto_field_by_id(struct proto_hdr *hdr, uint32_t fid)
 106 {
 107         /* Assume the fields are stored in the same order as the respective
 108          * enum, so the index can be used for faster lookup here.
 109          */
 110         bug_on(hdr->fields[fid].id != fid);
 111
 112         return &hdr->fields[fid];
 113 }
 114
 115 bool proto_field_is_set(struct proto_hdr *hdr, uint32_t fid)
 116 {
 117         struct proto_field *field = proto_field_by_id(hdr, fid);
 118
 119         return field ? field->is_set : false;
 120 }
 121
 122 struct proto_hdr *proto_header_push(enum proto_id pid)
 123 {
 124         struct proto_hdr **headers = current_packet()->headers;
 125         const struct proto_ops *ops = proto_ops_by_id(pid);
 126         struct proto_hdr *hdr;
 127
 128         bug_on(current_packet()->headers_count >= PROTO_MAX_LAYERS);
 129
 130         hdr = xzmalloc(sizeof(*hdr));
 131         hdr->ops = ops;
 132         hdr->pkt_id = current_packet_id();
 133
 134         if (ops && ops->header_init)
 135                 ops->header_init(hdr);
 136
 137         headers[current_packet()->headers_count++] = hdr;
 138
 139         return hdr;
 140 }
 141
 142 void proto_header_finish(struct proto_hdr *hdr)
 143 {
 144         if (hdr && hdr->ops && hdr->ops->header_finish)
 145                 hdr->ops->header_finish(hdr);
 146 }
 147
 148 struct proto_hdr *proto_lower_default_add(struct proto_hdr *upper,
 149                                           enum proto_id pid)
 150 {
 151         struct proto_hdr *current;
 152         size_t headers_count = current_packet()->headers_count;
 153         const struct proto_ops *ops;
 154
 155         if (headers_count > 0) {
 156                 current = current_packet()->headers[headers_count - 1];
 157                 ops = current->ops;
 158
 159                 if (ops->layer >= proto_ops_by_id(pid)->layer)
 160                         goto set_proto;
 161                 if (ops->id == pid)
 162                         goto set_proto;
 163         }
 164
 165         current = proto_header_push(pid);
 166         ops = current->ops;
 167
 168 set_proto:
 169         if (ops && ops->set_next_proto)
 170                 ops->set_next_proto(current, upper->ops->id);
 171
 172         return current;
 173 }
 174
 175 static void __proto_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 176                                     const uint8_t *bytes, bool is_default,
 177                                     bool is_be)
 178 {
 179         struct proto_field *field;
 180         uint8_t *payload, *p8;
 181         uint16_t *p16;
 182         uint32_t *p32;
 183         uint32_t v32;
 184         uint16_t v16;
 185         uint8_t v8;
 186
 187         field = proto_field_by_id(hdr, fid);
 188
 189         if (is_default && field->is_set)
 190                 return;
 191
 192         payload = &packet_get(hdr->pkt_id)->payload[field->pkt_offset];
 193
 194         if (field->len == 1) {
 195                 p8 = payload;
 196                 *p8 = field->mask ? *p8 & ~field->mask : *p8;
 197
 198                 v8 = field_shift_and_mask(field, *bytes);
 199                 v8 = field->mask ? (v8 | *p8) : v8;
 200
 201                 bytes = &v8;
 202         } else if (field->len == 2) {
 203                 p16 = (uint16_t *)payload;
 204                 *p16 = be16_to_cpu(*p16);
 205                 *p16 = cpu_to_be16(field->mask ? *p16 & ~field->mask : *p16);
 206
 207                 v16 = field_shift_and_mask(field, *(uint16_t *)bytes);
 208                 v16 = is_be ? cpu_to_be16(v16) : v16;
 209                 v16 = field->mask ? (v16 | *p16) : v16;
 210
 211                 bytes = (uint8_t *)&v16;
 212         } else if (field->len == 4) {
 213                 p32 = (uint32_t *)payload;
 214                 *p32 = be32_to_cpu(*p32);
 215                 *p32 = cpu_to_be32(field->mask ? *p32 & ~field->mask : *p32);
 216
 217                 v32 = field_shift_and_mask(field, *(uint32_t *)bytes);
 218                 v32 = is_be ? cpu_to_be32(v32) : v32;
 219                 v32 = field->mask ? (v32 | *p32) : v32;
 220
 221                 bytes = (uint8_t *)&v32;
 222         }
 223
 224         memcpy(payload, bytes, field->len);
 225
 226         if (!is_default)
 227                 field->is_set = true;
 228 }
 229
 230 void proto_field_set_bytes(struct proto_hdr *hdr, uint32_t fid,
 231                            const 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,
 281                                    const uint8_t *bytes)
 282 {
 283         __proto_field_set_bytes(hdr, fid, bytes, true, false);
 284 }
 285
 286 void proto_field_set_default_u8(struct proto_hdr *hdr, uint32_t fid, uint8_t val)
 287 {
 288         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 289 }
 290
 291 void proto_field_set_default_u16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 292 {
 293         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 294 }
 295
 296 void proto_field_set_default_u32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 297 {
 298         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, false);
 299 }
 300
 301 void proto_field_set_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 302 {
 303         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, false, true);
 304 }
 305
 306 void proto_field_set_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 307 {
 308         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, false, true);
 309 }
 310
 311 void proto_field_set_default_be16(struct proto_hdr *hdr, uint32_t fid, uint16_t val)
 312 {
 313         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, true);
 314 }
 315
 316 void proto_field_set_default_be32(struct proto_hdr *hdr, uint32_t fid, uint32_t val)
 317 {
 318         __proto_field_set_bytes(hdr, fid, (uint8_t *)&val, true, true);
 319 }
 320
 321 static void __proto_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid,
 322                                       bool is_default)
 323 {
 324         uint8_t mac[ETH_ALEN];
 325         int ret;
 326
 327         if (proto_field_is_set(hdr, fid))
 328                 return;
 329
 330         ret = device_hw_address(ctx.dev, mac, sizeof(mac));
 331         if (ret < 0)
 332                 panic("Could not get device hw address\n");
 333
 334         __proto_field_set_bytes(hdr, fid, mac, is_default, false);
 335 }
 336
 337 void proto_field_set_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 338 {
 339         __proto_field_set_dev_mac(hdr, fid, false);
 340 }
 341
 342 void proto_field_set_default_dev_mac(struct proto_hdr *hdr, uint32_t fid)
 343 {
 344         __proto_field_set_dev_mac(hdr, fid, true);
 345 }
 346
 347 static void __proto_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid,
 348                                        bool is_default)
 349 {
 350         struct sockaddr_storage ss = { };
 351         struct sockaddr_in *ss4;
 352         int ret;
 353
 354         if (proto_field_is_set(hdr, fid))
 355                 return;
 356
 357         ret = device_address(ctx.dev, AF_INET, &ss);
 358         if (ret < 0)
 359                 panic("Could not get device IPv4 address\n");
 360
 361         ss4 = (struct sockaddr_in *) &ss;
 362         __proto_field_set_bytes(hdr, fid, (uint8_t *)&ss4->sin_addr.s_addr, is_default, false);
 363 }
 364
 365 void proto_field_set_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 366 {
 367         __proto_field_set_dev_ipv4(hdr, fid, false);
 368 }
 369
 370 void proto_field_set_default_dev_ipv4(struct proto_hdr *hdr, uint32_t fid)
 371 {
 372         __proto_field_set_dev_ipv4(hdr, fid, true);
 373 }
 374
 375 static void __proto_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid,
 376                                        bool is_default)
 377 {
 378         struct sockaddr_storage ss = { };
 379         struct sockaddr_in6 *ss6;
 380         int ret;
 381
 382         if (proto_field_is_set(hdr, fid))
 383                 return;
 384
 385         ret = device_address(ctx.dev, AF_INET6, &ss);
 386         if (ret < 0)
 387                 panic("Could not get device IPv6 address\n");
 388
 389         ss6 = (struct sockaddr_in6 *) &ss;
 390         __proto_field_set_bytes(hdr, fid, (uint8_t *)&ss6->sin6_addr.s6_addr, is_default, false);
 391 }
 392
 393 void proto_field_set_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 394 {
 395         __proto_field_set_dev_ipv6(hdr, fid, false);
 396 }
 397
 398 void proto_field_set_default_dev_ipv6(struct proto_hdr *hdr, uint32_t fid)
 399 {
 400         __proto_field_set_dev_ipv6(hdr, fid, true);
 401 }
 402
 403 void protos_init(const char *dev)
 404 {
 405         ctx.dev = dev;
 406
 407         protos_l2_init();
 408         protos_l3_init();
 409         protos_l4_init();
 410 }
 411
 412 void proto_packet_finish(void)
 413 {
 414         struct proto_hdr **headers = current_packet()->headers;
 415         size_t headers_count = current_packet()->headers_count;
 416         ssize_t i;
 417
 418         /* Go down from upper layers to do last calculations (checksum) */
 419         for (i = headers_count - 1; i >= 0; i--) {
 420                 struct proto_hdr *hdr = headers[i];
 421                 const struct proto_ops *ops = hdr->ops;
 422
 423                 if (ops && ops->packet_finish)
 424                         ops->packet_finish(hdr);
 425         }
 426 }