2 * netsniff-ng - the packet sniffing beast
3 * Subject to the GPL, version 2.
8 #include <netinet/in.h>
9 #include <linux/if_ether.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"
19 #define field_shift_and_mask(f, v) (((v) << (f)->shift) & \
20 ((f)->mask ? (f)->mask : (0xffffffff)))
22 #define field_unmask_and_unshift(f, v) (((v) & \
23 ((f)->mask ? (f)->mask : (0xffffffff))) >> (f)->shift)
28 static struct ctx ctx
;
30 static const struct proto_ops
*registered_ops
[__PROTO_MAX
];
32 struct proto_hdr
*proto_lower_header(struct proto_hdr
*hdr
)
34 struct packet
*pkt
= packet_get(hdr
->pkt_id
);
35 struct proto_hdr
**headers
= &pkt
->headers
[0];
40 return headers
[hdr
->index
- 1];
43 struct proto_hdr
*proto_upper_header(struct proto_hdr
*hdr
)
45 struct packet
*pkt
= packet_get(hdr
->pkt_id
);
46 struct proto_hdr
**headers
= &pkt
->headers
[0];
47 size_t headers_count
= pkt
->headers_count
;
49 if (hdr
->index
== headers_count
- 1)
52 return headers
[hdr
->index
+ 1];
55 uint8_t *proto_header_ptr(struct proto_hdr
*hdr
)
57 return &packet_get(hdr
->pkt_id
)->payload
[hdr
->pkt_offset
];
60 static const struct proto_ops
*proto_ops_by_id(enum proto_id id
)
62 const struct proto_ops
*ops
= registered_ops
[id
];
64 bug_on(ops
->id
!= id
);
68 void proto_ops_register(const struct proto_ops
*ops
)
70 bug_on(ops
->id
>= __PROTO_MAX
);
71 registered_ops
[ops
->id
] = ops
;
74 static void proto_fields_realloc(struct proto_hdr
*hdr
, size_t count
)
76 hdr
->fields
= xrealloc(hdr
->fields
, count
* sizeof(*hdr
->fields
));
77 hdr
->fields_count
= count
;
80 void proto_header_fields_add(struct proto_hdr
*hdr
,
81 const struct proto_field
*fields
, size_t count
)
83 struct packet
*pkt
= packet_get(hdr
->pkt_id
);
84 struct proto_field
*f
;
88 hdr
->pkt_offset
= pkt
->len
;
90 proto_fields_realloc(hdr
, hdr
->fields_count
+ count
);
92 for (i
= 0; count
>= 1; count
--, i
++) {
93 f
= &hdr
->fields
[hdr
->fields_count
- count
];
96 f
->len
= fields
[i
].len
;
98 f
->shift
= fields
[i
].shift
;
99 f
->mask
= fields
[i
].mask
;
100 f
->pkt_offset
= hdr
->pkt_offset
+ fields
[i
].offset
;
103 if (f
->pkt_offset
+ f
->len
> pkt
->len
) {
105 set_fill(0, (f
->pkt_offset
+ f
->len
) - pkt
->len
);
110 struct proto_field
*proto_hdr_field_by_id(struct proto_hdr
*hdr
, uint32_t fid
)
112 /* Assume the fields are stored in the same order as the respective
113 * enum, so the index can be used for faster lookup here.
115 bug_on(hdr
->fields
[fid
].id
!= fid
);
117 return &hdr
->fields
[fid
];
120 bool proto_hdr_field_is_set(struct proto_hdr
*hdr
, uint32_t fid
)
122 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
124 return field
? field
->is_set
: false;
127 struct proto_hdr
*proto_header_push(enum proto_id pid
)
129 struct packet
*pkt
= current_packet();
130 struct proto_hdr
**headers
= &pkt
->headers
[0];
131 const struct proto_ops
*ops
= proto_ops_by_id(pid
);
132 struct proto_hdr
*hdr
;
134 bug_on(pkt
->headers_count
>= PROTO_MAX_LAYERS
);
136 hdr
= xzmalloc(sizeof(*hdr
));
138 hdr
->pkt_id
= current_packet_id();
140 if (ops
&& ops
->header_init
)
141 ops
->header_init(hdr
);
143 /* This is very important to have it after header_init as
144 * pkt->headers_count might be changed by adding default lower headers */
145 hdr
->index
= pkt
->headers_count
;
147 headers
[pkt
->headers_count
++] = hdr
;
151 void proto_header_finish(struct proto_hdr
*hdr
)
153 if (hdr
&& hdr
->ops
&& hdr
->ops
->header_finish
)
154 hdr
->ops
->header_finish(hdr
);
157 struct proto_hdr
*proto_lower_default_add(struct proto_hdr
*upper
,
160 struct proto_hdr
*current
;
161 size_t headers_count
= current_packet()->headers_count
;
162 const struct proto_ops
*ops
;
164 if (headers_count
> 0) {
165 current
= current_packet()->headers
[headers_count
- 1];
168 if (ops
->layer
>= proto_ops_by_id(pid
)->layer
)
174 current
= proto_header_push(pid
);
178 if (ops
&& ops
->set_next_proto
)
179 ops
->set_next_proto(current
, upper
->ops
->id
);
184 static void __proto_field_set_bytes(struct proto_field
*field
,
185 const uint8_t *bytes
, bool is_default
,
188 uint8_t *payload
, *p8
;
195 if (is_default
&& field
->is_set
)
198 payload
= &packet_get(field
->hdr
->pkt_id
)->payload
[field
->pkt_offset
];
200 if (field
->len
== 1) {
202 *p8
= field
->mask
? *p8
& ~field
->mask
: *p8
;
204 v8
= field_shift_and_mask(field
, *bytes
);
205 v8
= field
->mask
? (v8
| *p8
) : v8
;
208 } else if (field
->len
== 2) {
209 p16
= (uint16_t *)payload
;
210 *p16
= be16_to_cpu(*p16
);
211 *p16
= cpu_to_be16(field
->mask
? *p16
& ~field
->mask
: *p16
);
213 v16
= field_shift_and_mask(field
, *(const uint16_t *)bytes
);
214 v16
= is_be
? cpu_to_be16(v16
) : v16
;
215 v16
= field
->mask
? (v16
| *p16
) : v16
;
217 bytes
= (uint8_t *)&v16
;
218 } else if (field
->len
== 4) {
219 p32
= (uint32_t *)payload
;
220 *p32
= be32_to_cpu(*p32
);
221 *p32
= cpu_to_be32(field
->mask
? *p32
& ~field
->mask
: *p32
);
223 v32
= field_shift_and_mask(field
, *(const uint32_t *)bytes
);
224 v32
= is_be
? cpu_to_be32(v32
) : v32
;
225 v32
= field
->mask
? (v32
| *p32
) : v32
;
227 bytes
= (uint8_t *)&v32
;
230 memcpy(payload
, bytes
, field
->len
);
233 field
->is_set
= true;
236 void proto_hdr_field_set_bytes(struct proto_hdr
*hdr
, uint32_t fid
,
237 const uint8_t *bytes
)
239 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
241 __proto_field_set_bytes(field
, bytes
, false, false);
244 static uint8_t *__proto_field_get_bytes(struct proto_field
*field
)
246 return &packet_get(field
->hdr
->pkt_id
)->payload
[field
->pkt_offset
];
249 void proto_hdr_field_set_u8(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t val
)
251 proto_hdr_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
254 uint8_t proto_hdr_field_get_u8(struct proto_hdr
*hdr
, uint32_t fid
)
256 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
257 uint8_t val
= *__proto_field_get_bytes(field
);
259 return field_unmask_and_unshift(field
, val
);
262 void proto_hdr_field_set_u16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
264 proto_hdr_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
267 uint16_t proto_hdr_field_get_u16(struct proto_hdr
*hdr
, uint32_t fid
)
269 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
270 uint16_t val
= *(uint16_t *)__proto_field_get_bytes(field
);
272 return field_unmask_and_unshift(field
, be16_to_cpu(val
));
275 void proto_hdr_field_set_u32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
277 proto_hdr_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
280 uint32_t proto_hdr_field_get_u32(struct proto_hdr
*hdr
, uint32_t fid
)
282 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
283 uint32_t val
= *(uint32_t *)__proto_field_get_bytes(field
);
285 return field_unmask_and_unshift(field
, be32_to_cpu(val
));
288 void proto_hdr_field_set_default_bytes(struct proto_hdr
*hdr
, uint32_t fid
,
289 const uint8_t *bytes
)
291 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
293 __proto_field_set_bytes(field
, bytes
, true, false);
296 void proto_hdr_field_set_default_u8(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t val
)
298 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
300 __proto_field_set_bytes(field
, (uint8_t *)&val
, true, false);
303 void proto_hdr_field_set_default_u16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
305 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
307 __proto_field_set_bytes(field
, (uint8_t *)&val
, true, false);
310 void proto_hdr_field_set_default_u32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
312 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
314 __proto_field_set_bytes(field
, (uint8_t *)&val
, true, false);
317 void proto_hdr_field_set_be16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
319 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
321 __proto_field_set_bytes(field
, (uint8_t *)&val
, false, true);
324 void proto_hdr_field_set_be32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
326 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
328 __proto_field_set_bytes(field
, (uint8_t *)&val
, false, true);
331 void proto_hdr_field_set_default_be16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
333 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
335 __proto_field_set_bytes(field
, (uint8_t *)&val
, true, true);
338 void proto_hdr_field_set_default_be32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
340 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
342 __proto_field_set_bytes(field
, (uint8_t *)&val
, true, true);
345 static void __proto_hdr_field_set_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
,
348 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
349 uint8_t mac
[ETH_ALEN
];
352 if (proto_hdr_field_is_set(hdr
, fid
))
355 ret
= device_hw_address(ctx
.dev
, mac
, sizeof(mac
));
357 panic("Could not get device hw address\n");
359 __proto_field_set_bytes(field
, mac
, is_default
, false);
362 void proto_hdr_field_set_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
)
364 __proto_hdr_field_set_dev_mac(hdr
, fid
, false);
367 void proto_hdr_field_set_default_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
)
369 __proto_hdr_field_set_dev_mac(hdr
, fid
, true);
372 static void __proto_hdr_field_set_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
,
375 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
376 struct sockaddr_storage ss
= { };
377 struct sockaddr_in
*ss4
;
380 if (proto_hdr_field_is_set(hdr
, fid
))
383 ret
= device_address(ctx
.dev
, AF_INET
, &ss
);
385 fprintf(stderr
, "Warning: Could not get device IPv4 address for %s\n", ctx
.dev
);
389 ss4
= (struct sockaddr_in
*) &ss
;
390 __proto_field_set_bytes(field
, (uint8_t *)&ss4
->sin_addr
.s_addr
, is_default
, false);
393 void proto_hdr_field_set_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
)
395 __proto_hdr_field_set_dev_ipv4(hdr
, fid
, false);
398 void proto_hdr_field_set_default_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
)
400 __proto_hdr_field_set_dev_ipv4(hdr
, fid
, true);
403 static void __proto_hdr_field_set_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
,
406 struct proto_field
*field
= proto_hdr_field_by_id(hdr
, fid
);
407 struct sockaddr_storage ss
= { };
408 struct sockaddr_in6
*ss6
;
411 if (proto_hdr_field_is_set(hdr
, fid
))
414 ret
= device_address(ctx
.dev
, AF_INET6
, &ss
);
416 fprintf(stderr
, "Warning: Could not get device IPv6 address for %s\n", ctx
.dev
);
420 ss6
= (struct sockaddr_in6
*) &ss
;
421 __proto_field_set_bytes(field
, (uint8_t *)&ss6
->sin6_addr
.s6_addr
, is_default
, false);
424 void proto_hdr_field_set_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
)
426 __proto_hdr_field_set_dev_ipv6(hdr
, fid
, false);
429 void proto_hdr_field_set_default_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
)
431 __proto_hdr_field_set_dev_ipv6(hdr
, fid
, true);
434 void proto_field_set_u8(struct proto_field
*field
, uint8_t val
)
436 __proto_field_set_bytes(field
, &val
, false, false);
439 uint8_t proto_field_get_u8(struct proto_field
*field
)
441 uint8_t val
= *__proto_field_get_bytes(field
);
443 return field_unmask_and_unshift(field
, val
);
446 void proto_field_set_u16(struct proto_field
*field
, uint16_t val
)
448 __proto_field_set_bytes(field
, (uint8_t *)&val
, false, false);
451 uint16_t proto_field_get_u16(struct proto_field
*field
)
453 uint16_t val
= *(uint16_t *)__proto_field_get_bytes(field
);
455 return field_unmask_and_unshift(field
, be16_to_cpu(val
));
458 void proto_field_set_u32(struct proto_field
*field
, uint32_t val
)
460 __proto_field_set_bytes(field
, (uint8_t *)&val
, false, false);
463 uint32_t proto_field_get_u32(struct proto_field
*field
)
465 uint32_t val
= *(uint32_t *)__proto_field_get_bytes(field
);
467 return field_unmask_and_unshift(field
, be32_to_cpu(val
));
470 void proto_field_set_be16(struct proto_field
*field
, uint16_t val
)
472 __proto_field_set_bytes(field
, (uint8_t *)&val
, false, true);
475 void proto_field_set_be32(struct proto_field
*field
, uint32_t val
)
477 __proto_field_set_bytes(field
, (uint8_t *)&val
, false, true);
480 void protos_init(const char *dev
)
489 void proto_packet_update(uint32_t idx
)
491 struct packet
*pkt
= packet_get(idx
);
494 for (i
= pkt
->headers_count
- 1; i
>= 0; i
--) {
495 struct proto_hdr
*hdr
= pkt
->headers
[i
];
497 if (hdr
->ops
->packet_update
)
498 hdr
->ops
->packet_update(hdr
);
502 void proto_packet_finish(void)
504 struct proto_hdr
**headers
= current_packet()->headers
;
505 size_t headers_count
= current_packet()->headers_count
;
508 /* Go down from upper layers to do last calculations (checksum) */
509 for (i
= headers_count
- 1; i
>= 0; i
--) {
510 struct proto_hdr
*hdr
= headers
[i
];
511 const struct proto_ops
*ops
= hdr
->ops
;
513 if (ops
&& ops
->packet_finish
)
514 ops
->packet_finish(hdr
);
518 static inline uint32_t field_inc(struct proto_field
*field
)
520 uint32_t min
= field
->func
.min
;
521 uint32_t max
= field
->func
.max
;
522 uint32_t val
= field
->func
.val
;
523 uint32_t inc
= field
->func
.inc
;
526 next
= (val
+ inc
) % (max
+ 1);
527 field
->func
.val
= max(next
, min
);
532 static void field_inc_func(struct proto_field
*field
)
534 if (field
->len
== 1) {
535 proto_field_set_u8(field
, field_inc(field
));
536 } else if (field
->len
== 2) {
537 proto_field_set_be16(field
, field_inc(field
));
538 } else if (field
->len
== 4) {
539 proto_field_set_be32(field
, field_inc(field
));
540 } else if (field
->len
> 4) {
541 uint8_t *bytes
= __proto_field_get_bytes(field
);
543 bytes
+= field
->len
- 4;
545 *(uint32_t *)bytes
= bswap_32(field_inc(field
));
549 static inline uint32_t field_rand(struct proto_field
*field
)
551 return field
->func
.min
+ (rand() % ((field
->func
.max
- field
->func
.min
) + 1));
554 static void field_rnd_func(struct proto_field
*field
)
556 if (field
->len
== 1) {
557 proto_field_set_u8(field
, (uint8_t) field_rand(field
));
558 } else if (field
->len
== 2) {
559 proto_field_set_be16(field
, (uint16_t) field_rand(field
));
560 } else if (field
->len
== 4) {
561 proto_field_set_be32(field
, (uint32_t) field_rand(field
));
562 } else if (field
->len
> 4) {
563 uint8_t *bytes
= __proto_field_get_bytes(field
);
566 for (i
= 0; i
< field
->len
; i
++)
567 bytes
[i
] = (uint8_t) field_rand(field
);
571 void proto_field_func_add(struct proto_field
*field
,
572 struct proto_field_func
*func
)
576 field
->func
.update_field
= func
->update_field
;
577 field
->func
.type
= func
->type
;
578 field
->func
.max
= func
->max
?: UINT32_MAX
- 1;
579 field
->func
.min
= func
->min
;
580 field
->func
.inc
= func
->inc
;
582 if (func
->type
& PROTO_FIELD_FUNC_INC
) {
583 if (func
->type
& PROTO_FIELD_FUNC_MIN
)
584 field
->func
.val
= func
->min
;
585 else if (field
->len
== 1)
586 field
->func
.val
= proto_field_get_u8(field
);
587 else if (field
->len
== 2)
588 field
->func
.val
= proto_field_get_u16(field
);
589 else if (field
->len
== 4)
590 field
->func
.val
= proto_field_get_u32(field
);
591 else if (field
->len
> 4) {
592 uint8_t *bytes
= __proto_field_get_bytes(field
);
594 bytes
+= field
->len
- 4;
595 field
->func
.val
= bswap_32(*(uint32_t *)bytes
);
598 field
->func
.update_field
= field_inc_func
;
599 } else if (func
->type
& PROTO_FIELD_FUNC_RND
) {
600 field
->func
.update_field
= field_rnd_func
;
604 void proto_field_dyn_apply(struct proto_field
*field
)
606 if (field
->func
.update_field
)
607 field
->func
.update_field(field
);
609 if (field
->hdr
->ops
->field_changed
)
610 field
->hdr
->ops
->field_changed(field
);
613 const char *proto_dev_get(void)