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