2 * netsniff-ng - the packet sniffing beast
3 * Subject to the GPL, version 2.
6 #include <linux/if_ether.h>
11 #include "trafgen_l2.h"
12 #include "trafgen_l3.h"
13 #include "trafgen_proto.h"
14 #include "trafgen_conf.h"
16 static struct proto_field ipv4_fields
[] = {
17 { .id
= IP4_VER
, .len
= 1, .offset
= 0, .shift
= 4, .mask
= 0xf0 },
18 { .id
= IP4_IHL
, .len
= 1, .offset
= 0, .shift
= 0, .mask
= 0x0f },
19 { .id
= IP4_DSCP
, .len
= 1, .offset
= 1, .shift
= 2, .mask
= 0xfc },
20 { .id
= IP4_ECN
, .len
= 1, .offset
= 1, .shift
= 0, .mask
= 0x03 },
21 { .id
= IP4_TOS
, .len
= 1, .offset
= 1 },
22 { .id
= IP4_LEN
, .len
= 2, .offset
= 2 },
23 { .id
= IP4_ID
, .len
= 2, .offset
= 4 },
24 { .id
= IP4_FLAGS
, .len
= 2, .offset
= 6, .shift
= 13, .mask
= 0xe000 },
25 { .id
= IP4_MF
, .len
= 2, .offset
= 6, .shift
= 13, .mask
= 0x2000 },
26 { .id
= IP4_DF
, .len
= 2, .offset
= 6, .shift
= 14, .mask
= 0x4000 },
27 { .id
= IP4_FRAG_OFFS
, .len
= 2, .offset
= 6, .shift
= 0, .mask
= 0x1fff },
28 { .id
= IP4_TTL
, .len
= 1, .offset
= 8 },
29 { .id
= IP4_PROTO
, .len
= 1, .offset
= 9 },
30 { .id
= IP4_CSUM
, .len
= 2, .offset
= 10 },
31 { .id
= IP4_SADDR
, .len
= 4, .offset
= 12 },
32 { .id
= IP4_DADDR
, .len
= 4, .offset
= 16 },
35 static void ipv4_header_init(struct proto_hdr
*hdr
)
37 proto_lower_default_add(hdr
, PROTO_ETH
);
39 proto_header_fields_add(hdr
, ipv4_fields
, array_size(ipv4_fields
));
41 proto_field_set_default_u8(hdr
, IP4_VER
, 4);
42 proto_field_set_default_u8(hdr
, IP4_IHL
, 5);
43 proto_field_set_default_dev_ipv4(hdr
, IP4_SADDR
);
46 static void ipv4_field_changed(struct proto_field
*field
)
48 field
->hdr
->is_csum_valid
= false;
50 if (field
->id
== IP4_SADDR
|| field
->id
== IP4_DADDR
) {
51 struct proto_hdr
*upper
= proto_upper_header(field
->hdr
);
53 if (upper
&& (upper
->ops
->id
== PROTO_UDP
|| upper
->ops
->id
== PROTO_TCP
))
54 upper
->is_csum_valid
= false;
58 static void ipv4_csum_update(struct proto_hdr
*hdr
)
63 if (hdr
->is_csum_valid
)
65 if (proto_field_is_set(hdr
, IP4_CSUM
))
68 pkt
= packet_get(hdr
->pkt_id
);
70 proto_field_set_default_u16(hdr
, IP4_CSUM
, 0);
71 csum
= htons(calc_csum(&pkt
->payload
[hdr
->pkt_offset
], hdr
->len
));
72 proto_field_set_default_u16(hdr
, IP4_CSUM
, bswap_16(csum
));
74 hdr
->is_csum_valid
= true;
77 static void ipv4_packet_finish(struct proto_hdr
*hdr
)
79 struct packet
*pkt
= current_packet();
82 total_len
= pkt
->len
- hdr
->pkt_offset
;
83 proto_field_set_default_be16(hdr
, IP4_LEN
, total_len
);
85 ipv4_csum_update(hdr
);
88 static void ipv4_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
94 ip_proto
= IPPROTO_IPIP
;
97 ip_proto
= IPPROTO_IPV6
;
100 ip_proto
= IPPROTO_ICMP
;
103 ip_proto
= IPPROTO_UDP
;
106 ip_proto
= IPPROTO_TCP
;
112 proto_field_set_default_u8(hdr
, IP4_PROTO
, ip_proto
);
115 static const struct proto_ops ipv4_proto_ops
= {
118 .header_init
= ipv4_header_init
,
119 .packet_update
= ipv4_csum_update
,
120 .field_changed
= ipv4_field_changed
,
121 .packet_finish
= ipv4_packet_finish
,
122 .set_next_proto
= ipv4_set_next_proto
,
125 static struct proto_field ipv6_fields
[] = {
126 { .id
= IP6_VER
, .len
= 4, .offset
= 0, .shift
= 28, .mask
= 0xf0000000 },
127 { .id
= IP6_CLASS
, .len
= 4, .offset
= 0, .shift
= 20, .mask
= 0x0ff00000 },
128 { .id
= IP6_FLOW_LBL
, .len
= 4, .offset
= 0, .shift
= 0, .mask
= 0x000fffff },
129 { .id
= IP6_LEN
, .len
= 2, .offset
= 4 },
130 { .id
= IP6_NEXT_HDR
, .len
= 1, .offset
= 6 },
131 { .id
= IP6_HOP_LIMIT
, .len
= 1, .offset
= 7 },
132 { .id
= IP6_SADDR
, .len
= 16, .offset
= 8 },
133 { .id
= IP6_DADDR
, .len
= 16, .offset
= 24 },
136 static void ipv6_header_init(struct proto_hdr
*hdr
)
138 proto_lower_default_add(hdr
, PROTO_ETH
);
140 proto_header_fields_add(hdr
, ipv6_fields
, array_size(ipv6_fields
));
142 proto_field_set_default_be32(hdr
, IP6_VER
, 6);
143 proto_field_set_default_dev_ipv6(hdr
, IP6_SADDR
);
146 static void ipv6_field_changed(struct proto_field
*field
)
148 if (field
->id
== IP6_SADDR
|| field
->id
== IP6_DADDR
) {
149 struct proto_hdr
*upper
= proto_upper_header(field
->hdr
);
151 if (upper
&& (upper
->ops
->id
== PROTO_UDP
|| upper
->ops
->id
== PROTO_TCP
))
152 upper
->is_csum_valid
= false;
156 #define IPV6_HDR_LEN 40
158 static void ipv6_packet_finish(struct proto_hdr
*hdr
)
160 struct packet
*pkt
= current_packet();
161 uint16_t total_len
= pkt
->len
- hdr
->pkt_offset
- IPV6_HDR_LEN
;
163 proto_field_set_default_be16(hdr
, IP6_LEN
, total_len
);
166 static void ipv6_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
172 ip_proto
= IPPROTO_ICMPV6
;
175 ip_proto
= IPPROTO_UDP
;
178 ip_proto
= IPPROTO_TCP
;
184 proto_field_set_default_u8(hdr
, IP6_NEXT_HDR
, ip_proto
);
187 static const struct proto_ops ipv6_proto_ops
= {
190 .header_init
= ipv6_header_init
,
191 .field_changed
= ipv6_field_changed
,
192 .packet_finish
= ipv6_packet_finish
,
193 .set_next_proto
= ipv6_set_next_proto
,
196 void protos_l3_init(void)
198 proto_ops_register(&ipv4_proto_ops
);
199 proto_ops_register(&ipv6_proto_ops
);