2 * netsniff-ng - the packet sniffing beast
3 * Subject to the GPL, version 2.
7 #include <netinet/in.h>
12 #include "trafgen_l3.h"
13 #include "trafgen_l4.h"
14 #include "trafgen_conf.h"
15 #include "trafgen_proto.h"
17 static struct proto_field udp_fields
[] = {
18 { .id
= UDP_SPORT
, .len
= 2, .offset
= 0 },
19 { .id
= UDP_DPORT
, .len
= 2, .offset
= 2 },
20 { .id
= UDP_LEN
, .len
= 2, .offset
= 4 },
21 { .id
= UDP_CSUM
, .len
= 2, .offset
= 6 },
24 static void udp_header_init(struct proto_hdr
*hdr
)
26 proto_lower_default_add(hdr
, PROTO_IP4
);
28 proto_header_fields_add(hdr
, udp_fields
, array_size(udp_fields
));
31 static void udp_field_changed(struct proto_field
*field
)
33 field
->hdr
->is_csum_valid
= false;
36 static void udp_csum_update(struct proto_hdr
*hdr
)
38 struct proto_hdr
*lower
;
42 if (hdr
->is_csum_valid
)
44 if (proto_hdr_field_is_set(hdr
, UDP_CSUM
))
46 lower
= proto_lower_header(hdr
);
50 total_len
= packet_get(hdr
->pkt_id
)->len
- hdr
->pkt_offset
;
52 proto_hdr_field_set_default_be16(hdr
, UDP_CSUM
, 0);
54 switch (lower
->ops
->id
) {
56 csum
= p4_csum((void *) proto_header_ptr(lower
), proto_header_ptr(hdr
),
57 total_len
, IPPROTO_UDP
);
60 csum
= p6_csum((void *) proto_header_ptr(lower
), proto_header_ptr(hdr
),
61 total_len
, IPPROTO_UDP
);
68 proto_hdr_field_set_default_be16(hdr
, UDP_CSUM
, bswap_16(csum
));
69 hdr
->is_csum_valid
= true;
72 static void udp_packet_finish(struct proto_hdr
*hdr
)
74 struct packet
*pkt
= proto_hdr_packet(hdr
);
77 total_len
= pkt
->len
- hdr
->pkt_offset
;
78 proto_hdr_field_set_default_be16(hdr
, UDP_LEN
, total_len
);
83 static void udp_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
95 proto_hdr_field_set_default_be16(hdr
, UDP_DPORT
, dport
);
98 static const struct proto_ops udp_proto_ops
= {
101 .header_init
= udp_header_init
,
102 .packet_update
= udp_csum_update
,
103 .packet_finish
= udp_packet_finish
,
104 .field_changed
= udp_field_changed
,
105 .set_next_proto
= udp_set_next_proto
,
108 static struct proto_field tcp_fields
[] = {
109 { .id
= TCP_SPORT
, .len
= 2, .offset
= 0 },
110 { .id
= TCP_DPORT
, .len
= 2, .offset
= 2 },
111 { .id
= TCP_SEQ
, .len
= 4, .offset
= 4 },
112 { .id
= TCP_ACK_SEQ
, .len
= 4, .offset
= 8 },
113 { .id
= TCP_DOFF
, .len
= 2, .offset
= 12, .shift
= 12, .mask
= 0xf000 },
114 /* reserved (4 bits) */
115 { .id
= TCP_CWR
, .len
= 2, .offset
= 12, .shift
= 7, .mask
= 0x0080 },
116 { .id
= TCP_ECE
, .len
= 2, .offset
= 12, .shift
= 6, .mask
= 0x0040 },
117 { .id
= TCP_URG
, .len
= 2, .offset
= 12, .shift
= 5, .mask
= 0x0020 },
118 { .id
= TCP_ACK
, .len
= 2, .offset
= 12, .shift
= 4, .mask
= 0x0010 },
119 { .id
= TCP_PSH
, .len
= 2, .offset
= 12, .shift
= 3, .mask
= 0x0008 },
120 { .id
= TCP_RST
, .len
= 2, .offset
= 12, .shift
= 2, .mask
= 0x0004 },
121 { .id
= TCP_SYN
, .len
= 2, .offset
= 12, .shift
= 1, .mask
= 0x0002 },
122 { .id
= TCP_FIN
, .len
= 2, .offset
= 12, .shift
= 0, .mask
= 0x0001 },
123 { .id
= TCP_WINDOW
, .len
= 2, .offset
= 14 },
124 { .id
= TCP_CSUM
, .len
= 2, .offset
= 16 },
125 { .id
= TCP_URG_PTR
, .len
= 2, .offset
= 18 },
128 static void tcp_header_init(struct proto_hdr
*hdr
)
130 proto_lower_default_add(hdr
, PROTO_IP4
);
132 proto_header_fields_add(hdr
, tcp_fields
, array_size(tcp_fields
));
134 proto_hdr_field_set_default_be16(hdr
, TCP_DOFF
, 5);
137 static void tcp_field_changed(struct proto_field
*field
)
139 field
->hdr
->is_csum_valid
= false;
142 static void tcp_csum_update(struct proto_hdr
*hdr
)
144 struct proto_hdr
*lower
= proto_lower_header(hdr
);
145 struct packet
*pkt
= proto_hdr_packet(hdr
);
149 if (hdr
->is_csum_valid
)
151 if (proto_hdr_field_is_set(hdr
, TCP_CSUM
))
157 total_len
= pkt
->len
- hdr
->pkt_offset
;
159 proto_hdr_field_set_default_be16(hdr
, TCP_CSUM
, 0);
161 switch (lower
->ops
->id
) {
163 csum
= p4_csum((void *) proto_header_ptr(lower
), proto_header_ptr(hdr
),
164 total_len
, IPPROTO_TCP
);
167 csum
= p6_csum((void *) proto_header_ptr(lower
), proto_header_ptr(hdr
),
168 total_len
, IPPROTO_TCP
);
175 proto_hdr_field_set_default_be16(hdr
, TCP_CSUM
, bswap_16(csum
));
176 hdr
->is_csum_valid
= true;
179 static void tcp_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
191 proto_hdr_field_set_default_be16(hdr
, TCP_DPORT
, dport
);
194 static const struct proto_ops tcp_proto_ops
= {
197 .header_init
= tcp_header_init
,
198 .packet_update
= tcp_csum_update
,
199 .packet_finish
= tcp_csum_update
,
200 .field_changed
= tcp_field_changed
,
201 .set_next_proto
= tcp_set_next_proto
,
204 static struct proto_field icmpv4_fields
[] = {
205 { .id
= ICMPV4_TYPE
, .len
= 1, .offset
= 0 },
206 { .id
= ICMPV4_CODE
, .len
= 1, .offset
= 1 },
207 { .id
= ICMPV4_CSUM
, .len
= 2, .offset
= 2 },
208 /* Echo/Ping fields */
209 { .id
= ICMPV4_ID
, .len
= 2, .offset
= 4 },
210 { .id
= ICMPV4_SEQ
, .len
= 2, .offset
= 6 },
212 { .id
= ICMPV4_REDIR_ADDR
, .len
= 4, .offset
= 4 },
214 { .id
= ICMPV4_MTU
, .len
= 2, .offset
= 6 },
217 static void icmpv4_header_init(struct proto_hdr
*hdr
)
219 proto_lower_default_add(hdr
, PROTO_IP4
);
221 proto_header_fields_add(hdr
, icmpv4_fields
, array_size(icmpv4_fields
));
224 static void icmpv4_csum_update(struct proto_hdr
*hdr
)
229 if (hdr
->is_csum_valid
)
231 if (proto_hdr_field_is_set(hdr
, ICMPV4_CSUM
))
234 pkt
= packet_get(hdr
->pkt_id
);
236 proto_hdr_field_set_default_u16(hdr
, ICMPV4_CSUM
, 0);
237 csum
= htons(calc_csum(proto_header_ptr(hdr
), pkt
->len
- hdr
->pkt_offset
));
238 proto_hdr_field_set_default_u16(hdr
, ICMPV4_CSUM
, bswap_16(csum
));
240 hdr
->is_csum_valid
= true;
243 static void icmpv4_field_changed(struct proto_field
*field
)
245 field
->hdr
->is_csum_valid
= false;
248 static const struct proto_ops icmpv4_proto_ops
= {
251 .header_init
= icmpv4_header_init
,
252 .packet_update
= icmpv4_csum_update
,
253 .packet_finish
= icmpv4_csum_update
,
254 .field_changed
= icmpv4_field_changed
,
257 static struct proto_field icmpv6_fields
[] = {
258 { .id
= ICMPV6_TYPE
, .len
= 1, .offset
= 0 },
259 { .id
= ICMPV6_CODE
, .len
= 1, .offset
= 1 },
260 { .id
= ICMPV6_CSUM
, .len
= 2, .offset
= 2 }
263 static void icmpv6_header_init(struct proto_hdr
*hdr
)
265 proto_lower_default_add(hdr
, PROTO_IP6
);
267 proto_header_fields_add(hdr
, icmpv6_fields
, array_size(icmpv6_fields
));
270 static void icmpv6_csum_update(struct proto_hdr
*hdr
)
272 struct proto_hdr
*lower
= proto_lower_header(hdr
);
273 struct packet
*pkt
= packet_get(hdr
->pkt_id
);
277 if (unlikely(!lower
))
279 if (hdr
->is_csum_valid
)
281 if (proto_hdr_field_is_set(hdr
, ICMPV6_CSUM
))
284 total_len
= pkt
->len
- hdr
->pkt_offset
;
286 proto_hdr_field_set_be16(hdr
, ICMPV6_CSUM
, 0);
288 if (likely(lower
->ops
->id
== PROTO_IP6
)) {
289 csum
= p6_csum((void *) proto_header_ptr(lower
), proto_header_ptr(hdr
),
290 total_len
, IPPROTO_ICMPV6
);
292 proto_hdr_field_set_be16(hdr
, ICMPV6_CSUM
, bswap_16(csum
));
293 hdr
->is_csum_valid
= true;
297 static void icmpv6_field_changed(struct proto_field
*field
)
299 field
->hdr
->is_csum_valid
= false;
302 static struct proto_ops icmpv6_proto_ops
= {
305 .header_init
= icmpv6_header_init
,
306 .packet_finish
= icmpv6_csum_update
,
307 .packet_update
= icmpv6_csum_update
,
308 .field_changed
= icmpv6_field_changed
,
311 void protos_l4_init(void)
313 proto_ops_register(&udp_proto_ops
);
314 proto_ops_register(&tcp_proto_ops
);
315 proto_ops_register(&icmpv4_proto_ops
);
316 proto_ops_register(&icmpv6_proto_ops
);