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_hdr_field_set_default_u8(hdr
, IP4_VER
, 4);
42 proto_hdr_field_set_default_u8(hdr
, IP4_IHL
, 5);
45 static void ipv4_field_changed(struct proto_field
*field
)
47 field
->hdr
->is_csum_valid
= false;
49 if (field
->id
== IP4_SADDR
|| field
->id
== IP4_DADDR
) {
50 struct proto_hdr
*upper
= proto_upper_header(field
->hdr
);
52 if (upper
&& (upper
->ops
->id
== PROTO_UDP
|| upper
->ops
->id
== PROTO_TCP
))
53 upper
->is_csum_valid
= false;
57 static void ipv4_csum_update(struct proto_hdr
*hdr
)
62 if (hdr
->is_csum_valid
)
64 if (proto_hdr_field_is_set(hdr
, IP4_CSUM
))
67 pkt
= packet_get(hdr
->pkt_id
);
69 proto_hdr_field_set_default_u16(hdr
, IP4_CSUM
, 0);
70 csum
= htons(calc_csum(&pkt
->payload
[hdr
->pkt_offset
], hdr
->len
));
71 proto_hdr_field_set_default_u16(hdr
, IP4_CSUM
, bswap_16(csum
));
73 hdr
->is_csum_valid
= true;
76 static void ipv4_packet_finish(struct proto_hdr
*hdr
)
78 struct packet
*pkt
= current_packet();
81 total_len
= pkt
->len
- hdr
->pkt_offset
;
82 proto_hdr_field_set_default_be16(hdr
, IP4_LEN
, total_len
);
83 proto_hdr_field_set_default_dev_ipv4(hdr
, IP4_SADDR
);
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_hdr_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_hdr_field_set_default_be32(hdr
, IP6_VER
, 6);
145 static void ipv6_field_changed(struct proto_field
*field
)
147 if (field
->id
== IP6_SADDR
|| field
->id
== IP6_DADDR
) {
148 struct proto_hdr
*upper
= proto_upper_header(field
->hdr
);
150 if (upper
&& (upper
->ops
->id
== PROTO_UDP
|| upper
->ops
->id
== PROTO_TCP
))
151 upper
->is_csum_valid
= false;
155 #define IPV6_HDR_LEN 40
157 static void ipv6_packet_finish(struct proto_hdr
*hdr
)
159 struct packet
*pkt
= current_packet();
160 uint16_t total_len
= pkt
->len
- hdr
->pkt_offset
- IPV6_HDR_LEN
;
162 proto_hdr_field_set_default_be16(hdr
, IP6_LEN
, total_len
);
163 proto_hdr_field_set_default_dev_ipv6(hdr
, IP6_SADDR
);
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_hdr_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
);