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 proto_hdr
**headers
= current_packet()->headers
;
35 size_t headers_count
= current_packet()->headers_count
;
36 struct proto_hdr
*lower
= NULL
;
39 if (headers_count
== 0)
42 for (i
= 1, lower
= headers
[0]; i
< headers_count
; i
++) {
43 if (headers
[i
] == hdr
)
44 return headers
[i
- 1];
50 uint8_t *proto_header_ptr(struct proto_hdr
*hdr
)
52 return &packet_get(hdr
->pkt_id
)->payload
[hdr
->pkt_offset
];
55 static const struct proto_ops
*proto_ops_by_id(enum proto_id id
)
57 const struct proto_ops
*ops
= registered_ops
[id
];
59 bug_on(ops
->id
!= id
);
63 void proto_ops_register(const struct proto_ops
*ops
)
65 bug_on(ops
->id
>= __PROTO_MAX
);
66 registered_ops
[ops
->id
] = ops
;
69 static void proto_fields_realloc(struct proto_hdr
*hdr
, size_t count
)
71 hdr
->fields
= xrealloc(hdr
->fields
, count
* sizeof(*hdr
->fields
));
72 hdr
->fields_count
= count
;
75 void proto_header_fields_add(struct proto_hdr
*hdr
,
76 const struct proto_field
*fields
, size_t count
)
78 struct packet
*pkt
= packet_get(hdr
->pkt_id
);
79 struct proto_field
*f
;
83 hdr
->pkt_offset
= pkt
->len
;
85 proto_fields_realloc(hdr
, hdr
->fields_count
+ count
);
87 for (i
= 0; count
>= 1; count
--, i
++) {
88 f
= &hdr
->fields
[hdr
->fields_count
- count
];
91 f
->len
= fields
[i
].len
;
93 f
->shift
= fields
[i
].shift
;
94 f
->mask
= fields
[i
].mask
;
95 f
->pkt_offset
= hdr
->pkt_offset
+ fields
[i
].offset
;
98 if (f
->pkt_offset
+ f
->len
> pkt
->len
) {
100 set_fill(0, (f
->pkt_offset
+ f
->len
) - pkt
->len
);
105 static struct proto_field
*proto_field_by_id(struct proto_hdr
*hdr
, uint32_t fid
)
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.
110 bug_on(hdr
->fields
[fid
].id
!= fid
);
112 return &hdr
->fields
[fid
];
115 bool proto_field_is_set(struct proto_hdr
*hdr
, uint32_t fid
)
117 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
119 return field
? field
->is_set
: false;
122 struct proto_hdr
*proto_header_push(enum proto_id pid
)
124 struct proto_hdr
**headers
= current_packet()->headers
;
125 const struct proto_ops
*ops
= proto_ops_by_id(pid
);
126 struct proto_hdr
*hdr
;
128 bug_on(current_packet()->headers_count
>= PROTO_MAX_LAYERS
);
130 hdr
= xzmalloc(sizeof(*hdr
));
132 hdr
->pkt_id
= current_packet_id();
134 if (ops
&& ops
->header_init
)
135 ops
->header_init(hdr
);
137 headers
[current_packet()->headers_count
++] = hdr
;
142 void proto_header_finish(struct proto_hdr
*hdr
)
144 if (hdr
&& hdr
->ops
&& hdr
->ops
->header_finish
)
145 hdr
->ops
->header_finish(hdr
);
148 struct proto_hdr
*proto_lower_default_add(struct proto_hdr
*upper
,
151 struct proto_hdr
*current
;
152 size_t headers_count
= current_packet()->headers_count
;
153 const struct proto_ops
*ops
;
155 if (headers_count
> 0) {
156 current
= current_packet()->headers
[headers_count
- 1];
159 if (ops
->layer
>= proto_ops_by_id(pid
)->layer
)
165 current
= proto_header_push(pid
);
169 if (ops
&& ops
->set_next_proto
)
170 ops
->set_next_proto(current
, upper
->ops
->id
);
175 static void __proto_field_set_bytes(struct proto_hdr
*hdr
, uint32_t fid
,
176 const uint8_t *bytes
, bool is_default
,
179 struct proto_field
*field
;
180 uint8_t *payload
, *p8
;
187 field
= proto_field_by_id(hdr
, fid
);
189 if (is_default
&& field
->is_set
)
192 payload
= &packet_get(hdr
->pkt_id
)->payload
[field
->pkt_offset
];
194 if (field
->len
== 1) {
196 *p8
= field
->mask
? *p8
& ~field
->mask
: *p8
;
198 v8
= field_shift_and_mask(field
, *bytes
);
199 v8
= field
->mask
? (v8
| *p8
) : 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
);
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
;
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
);
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
;
221 bytes
= (uint8_t *)&v32
;
224 memcpy(payload
, bytes
, field
->len
);
227 field
->is_set
= true;
230 void proto_field_set_bytes(struct proto_hdr
*hdr
, uint32_t fid
,
231 const uint8_t *bytes
)
233 __proto_field_set_bytes(hdr
, fid
, bytes
, false, false);
236 static uint8_t *__proto_field_get_bytes(struct proto_field
*field
)
238 return &packet_get(field
->hdr
->pkt_id
)->payload
[field
->pkt_offset
];
241 void proto_field_set_u8(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t val
)
243 proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
246 uint8_t proto_field_get_u8(struct proto_hdr
*hdr
, uint32_t fid
)
248 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
249 uint8_t val
= *__proto_field_get_bytes(field
);
251 return field_unmask_and_unshift(field
, val
);
254 void proto_field_set_u16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
256 proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
259 uint16_t proto_field_get_u16(struct proto_hdr
*hdr
, uint32_t fid
)
261 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
262 uint16_t val
= *(uint16_t *)__proto_field_get_bytes(field
);
264 return field_unmask_and_unshift(field
, be16_to_cpu(val
));
267 void proto_field_set_u32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
269 proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
272 uint32_t proto_field_get_u32(struct proto_hdr
*hdr
, uint32_t fid
)
274 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
275 uint32_t val
= *(uint32_t *)__proto_field_get_bytes(field
);
277 return field_unmask_and_unshift(field
, be32_to_cpu(val
));
280 void proto_field_set_default_bytes(struct proto_hdr
*hdr
, uint32_t fid
,
281 const uint8_t *bytes
)
283 __proto_field_set_bytes(hdr
, fid
, bytes
, true, false);
286 void proto_field_set_default_u8(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t val
)
288 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, false);
291 void proto_field_set_default_u16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
293 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, false);
296 void proto_field_set_default_u32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
298 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, false);
301 void proto_field_set_be16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
303 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, false, true);
306 void proto_field_set_be32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
308 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, false, true);
311 void proto_field_set_default_be16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
313 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, true);
316 void proto_field_set_default_be32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
318 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, true);
321 static void __proto_field_set_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
,
324 uint8_t mac
[ETH_ALEN
];
327 if (proto_field_is_set(hdr
, fid
))
330 ret
= device_hw_address(ctx
.dev
, mac
, sizeof(mac
));
332 panic("Could not get device hw address\n");
334 __proto_field_set_bytes(hdr
, fid
, mac
, is_default
, false);
337 void proto_field_set_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
)
339 __proto_field_set_dev_mac(hdr
, fid
, false);
342 void proto_field_set_default_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
)
344 __proto_field_set_dev_mac(hdr
, fid
, true);
347 static void __proto_field_set_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
,
350 struct sockaddr_storage ss
= { };
351 struct sockaddr_in
*ss4
;
354 if (proto_field_is_set(hdr
, fid
))
357 ret
= device_address(ctx
.dev
, AF_INET
, &ss
);
359 panic("Could not get device IPv4 address\n");
361 ss4
= (struct sockaddr_in
*) &ss
;
362 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&ss4
->sin_addr
.s_addr
, is_default
, false);
365 void proto_field_set_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
)
367 __proto_field_set_dev_ipv4(hdr
, fid
, false);
370 void proto_field_set_default_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
)
372 __proto_field_set_dev_ipv4(hdr
, fid
, true);
375 static void __proto_field_set_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
,
378 struct sockaddr_storage ss
= { };
379 struct sockaddr_in6
*ss6
;
382 if (proto_field_is_set(hdr
, fid
))
385 ret
= device_address(ctx
.dev
, AF_INET6
, &ss
);
387 panic("Could not get device IPv6 address\n");
389 ss6
= (struct sockaddr_in6
*) &ss
;
390 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&ss6
->sin6_addr
.s6_addr
, is_default
, false);
393 void proto_field_set_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
)
395 __proto_field_set_dev_ipv6(hdr
, fid
, false);
398 void proto_field_set_default_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
)
400 __proto_field_set_dev_ipv6(hdr
, fid
, true);
403 void protos_init(const char *dev
)
412 void proto_packet_update(uint32_t idx
)
414 struct packet
*pkt
= packet_get(idx
);
417 for (i
= pkt
->headers_count
- 1; i
>= 0; i
--) {
418 struct proto_hdr
*hdr
= pkt
->headers
[i
];
420 if (hdr
->ops
->packet_update
)
421 hdr
->ops
->packet_update(hdr
);
425 void proto_packet_finish(void)
427 struct proto_hdr
**headers
= current_packet()->headers
;
428 size_t headers_count
= current_packet()->headers_count
;
431 /* Go down from upper layers to do last calculations (checksum) */
432 for (i
= headers_count
- 1; i
>= 0; i
--) {
433 struct proto_hdr
*hdr
= headers
[i
];
434 const struct proto_ops
*ops
= hdr
->ops
;
436 if (ops
&& ops
->packet_finish
)
437 ops
->packet_finish(hdr
);
441 void proto_field_func_add(struct proto_hdr
*hdr
, uint32_t fid
,
442 struct proto_field_func
*func
)
444 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
448 field
->func
.update_field
= func
->update_field
;
451 void proto_field_dyn_apply(struct proto_field
*field
)
453 if (field
->func
.update_field
)
454 field
->func
.update_field(field
);
456 if (field
->hdr
->ops
->field_changed
)
457 field
->hdr
->ops
->field_changed(field
);