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)
25 static struct proto_ctx ctx
;
27 #define PROTO_MAX_LAYERS 16
29 static struct proto_hdr
*headers
[PROTO_MAX_LAYERS
];
30 static size_t headers_count
;
32 static struct proto_hdr
*registered
;
34 struct proto_hdr
*proto_lower_header(struct proto_hdr
*hdr
)
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 ¤t_packet()->payload
[hdr
->pkt_offset
];
55 static struct proto_hdr
*proto_header_by_id(enum proto_id id
)
57 struct proto_hdr
*p
= registered
;
59 for (; p
; p
= p
->next
)
63 panic("Can't lookup proto by id %u\n", id
);
66 void proto_header_register(struct proto_hdr
*hdr
)
68 hdr
->next
= registered
;
72 hdr
->fields_count
= 0;
75 static void proto_fields_realloc(struct proto_hdr
*hdr
, size_t count
)
77 hdr
->fields
= xrealloc(hdr
->fields
, count
* sizeof(*hdr
->fields
));
78 hdr
->fields_count
= count
;
81 void proto_header_fields_add(struct proto_hdr
*hdr
,
82 const struct proto_field
*fields
, size_t count
)
84 struct packet
*pkt
= current_packet();
85 struct proto_field
*f
;
89 hdr
->pkt_offset
= pkt
->len
;
91 proto_fields_realloc(hdr
, hdr
->fields_count
+ count
);
93 for (i
= 0; count
>= 1; count
--, i
++) {
94 f
= &hdr
->fields
[hdr
->fields_count
- count
];
97 f
->len
= fields
[i
].len
;
99 f
->shift
= fields
[i
].shift
;
100 f
->mask
= fields
[i
].mask
;
101 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 static struct proto_field
*proto_field_by_id(struct proto_hdr
*hdr
, uint32_t fid
)
114 for (i
= 0; i
< hdr
->fields_count
; i
++)
115 if (hdr
->fields
[i
].id
== fid
)
116 return &hdr
->fields
[i
];
118 panic("Failed lookup field id %u for proto id %u\n", fid
, hdr
->id
);
121 bool proto_field_is_set(struct proto_hdr
*hdr
, uint32_t fid
)
123 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
125 return field
? field
->is_set
: false;
128 struct proto_hdr
*proto_header_init(enum proto_id pid
)
130 struct proto_hdr
*hdr
= proto_header_by_id(pid
);
131 struct proto_hdr
*new_hdr
;
133 bug_on(headers_count
>= PROTO_MAX_LAYERS
);
135 new_hdr
= xmalloc(sizeof(*new_hdr
));
136 memcpy(new_hdr
, hdr
, sizeof(*new_hdr
));
138 if (new_hdr
->header_init
)
139 new_hdr
->header_init(new_hdr
);
141 headers
[headers_count
++] = new_hdr
;
145 void proto_header_finish(struct proto_hdr
*hdr
)
147 if (hdr
&& hdr
->header_finish
)
148 hdr
->header_finish(hdr
);
151 struct proto_hdr
*proto_lower_default_add(struct proto_hdr
*hdr
,
154 struct proto_hdr
*current
;
156 if (headers_count
> 0) {
157 current
= headers
[headers_count
- 1];
159 if (current
->layer
>= proto_header_by_id(pid
)->layer
)
161 if (current
->id
== pid
)
165 current
= proto_header_init(pid
);
168 if (current
->set_next_proto
)
169 current
->set_next_proto(current
, hdr
->id
);
174 static void __proto_field_set_bytes(struct proto_hdr
*hdr
, uint32_t fid
,
175 uint8_t *bytes
, bool is_default
, bool is_be
)
177 struct proto_field
*field
;
178 uint8_t *payload
, *p8
;
185 field
= proto_field_by_id(hdr
, fid
);
187 if (is_default
&& field
->is_set
)
190 payload
= ¤t_packet()->payload
[field
->pkt_offset
];
192 if (field
->len
== 1) {
194 *p8
= field
->mask
? *p8
& ~field
->mask
: *p8
;
196 v8
= field_shift_and_mask(field
, *bytes
);
197 v8
= field
->mask
? (v8
| *p8
) : v8
;
200 } else if (field
->len
== 2) {
201 p16
= (uint16_t *)payload
;
202 *p16
= be16_to_cpu(*p16
);
203 *p16
= cpu_to_be16(field
->mask
? *p16
& ~field
->mask
: *p16
);
205 v16
= field_shift_and_mask(field
, *(uint16_t *)bytes
);
206 v16
= is_be
? cpu_to_be16(v16
) : v16
;
207 v16
= field
->mask
? (v16
| *p16
) : v16
;
209 bytes
= (uint8_t *)&v16
;
210 } else if (field
->len
== 4) {
211 p32
= (uint32_t *)payload
;
212 *p32
= be32_to_cpu(*p32
);
213 *p32
= cpu_to_be32(field
->mask
? *p32
& ~field
->mask
: *p32
);
215 v32
= field_shift_and_mask(field
, *(uint32_t *)bytes
);
216 v32
= is_be
? cpu_to_be32(v32
) : v32
;
217 v32
= field
->mask
? (v32
| *p32
) : v32
;
219 bytes
= (uint8_t *)&v32
;
222 memcpy(payload
, bytes
, field
->len
);
225 field
->is_set
= true;
228 void proto_field_set_bytes(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t *bytes
)
230 __proto_field_set_bytes(hdr
, fid
, bytes
, false, false);
233 static uint8_t *__proto_field_get_bytes(struct proto_field
*field
)
235 struct packet
*pkt
= current_packet();
237 return &pkt
->payload
[field
->pkt_offset
];
240 void proto_field_set_u8(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t val
)
242 proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
245 uint8_t proto_field_get_u8(struct proto_hdr
*hdr
, uint32_t fid
)
247 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
248 uint8_t val
= *__proto_field_get_bytes(field
);
250 return field_unmask_and_unshift(field
, val
);
253 void proto_field_set_u16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
255 proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
258 uint16_t proto_field_get_u16(struct proto_hdr
*hdr
, uint32_t fid
)
260 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
261 uint16_t val
= *(uint16_t *)__proto_field_get_bytes(field
);
263 return field_unmask_and_unshift(field
, be16_to_cpu(val
));
266 void proto_field_set_u32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
268 proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
);
271 uint32_t proto_field_get_u32(struct proto_hdr
*hdr
, uint32_t fid
)
273 struct proto_field
*field
= proto_field_by_id(hdr
, fid
);
274 uint32_t val
= *(uint32_t *)__proto_field_get_bytes(field
);
276 return field_unmask_and_unshift(field
, be32_to_cpu(val
));
279 void proto_field_set_default_bytes(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t *bytes
)
281 __proto_field_set_bytes(hdr
, fid
, bytes
, true, false);
284 void proto_field_set_default_u8(struct proto_hdr
*hdr
, uint32_t fid
, uint8_t val
)
286 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, false);
289 void proto_field_set_default_u16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
291 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, false);
294 void proto_field_set_default_u32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
296 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, false);
299 void proto_field_set_be16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
301 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, false, true);
304 void proto_field_set_be32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
306 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, false, true);
309 void proto_field_set_default_be16(struct proto_hdr
*hdr
, uint32_t fid
, uint16_t val
)
311 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, true);
314 void proto_field_set_default_be32(struct proto_hdr
*hdr
, uint32_t fid
, uint32_t val
)
316 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&val
, true, true);
319 static void __proto_field_set_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
,
322 uint8_t mac
[ETH_ALEN
];
325 if (proto_field_is_set(hdr
, fid
))
329 panic("Device is not specified\n");
331 ret
= device_hw_address(hdr
->ctx
->dev
, mac
, sizeof(mac
));
333 panic("Could not get device hw address\n");
335 __proto_field_set_bytes(hdr
, fid
, mac
, is_default
, false);
338 void proto_field_set_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
)
340 __proto_field_set_dev_mac(hdr
, fid
, false);
343 void proto_field_set_default_dev_mac(struct proto_hdr
*hdr
, uint32_t fid
)
345 __proto_field_set_dev_mac(hdr
, fid
, true);
348 static void __proto_field_set_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
,
351 struct sockaddr_storage ss
= { };
352 struct sockaddr_in
*ss4
;
355 if (proto_field_is_set(hdr
, fid
))
358 ret
= device_address(hdr
->ctx
->dev
, AF_INET
, &ss
);
360 panic("Could not get device IPv4 address\n");
362 ss4
= (struct sockaddr_in
*) &ss
;
363 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&ss4
->sin_addr
.s_addr
, is_default
, false);
366 void proto_field_set_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
)
368 __proto_field_set_dev_ipv4(hdr
, fid
, false);
371 void proto_field_set_default_dev_ipv4(struct proto_hdr
*hdr
, uint32_t fid
)
373 __proto_field_set_dev_ipv4(hdr
, fid
, true);
376 static void __proto_field_set_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
,
379 struct sockaddr_storage ss
= { };
380 struct sockaddr_in6
*ss6
;
383 if (proto_field_is_set(hdr
, fid
))
386 ret
= device_address(hdr
->ctx
->dev
, AF_INET6
, &ss
);
388 panic("Could not get device IPv6 address\n");
390 ss6
= (struct sockaddr_in6
*) &ss
;
391 __proto_field_set_bytes(hdr
, fid
, (uint8_t *)&ss6
->sin6_addr
.s6_addr
, is_default
, false);
394 void proto_field_set_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
)
396 __proto_field_set_dev_ipv6(hdr
, fid
, false);
399 void proto_field_set_default_dev_ipv6(struct proto_hdr
*hdr
, uint32_t fid
)
401 __proto_field_set_dev_ipv6(hdr
, fid
, true);
404 void protos_init(const char *dev
)
414 for (p
= registered
; p
; p
= p
->next
)
418 void proto_packet_finish(void)
422 /* Go down from upper layers to do last calculations (checksum) */
423 for (i
= headers_count
- 1; i
>= 0; i
--) {
424 struct proto_hdr
*p
= headers
[i
];
426 if (p
->packet_finish
)
430 for (i
= 0; i
< headers_count
; i
++) {
431 struct proto_hdr
*p
= headers
[i
];