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