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