2 * netsniff-ng - the packet sniffing beast
3 * Subject to the GPL, version 2.
6 #include <net/if_arp.h>
12 #include "trafgen_l2.h"
13 #include "trafgen_l3.h"
14 #include "trafgen_proto.h"
15 #include "trafgen_conf.h"
17 static struct proto_field ipv4_fields
[] = {
18 { .id
= IP4_VER
, .len
= 1, .offset
= 0, .shift
= 4, .mask
= 0xf0 },
19 { .id
= IP4_IHL
, .len
= 1, .offset
= 0, .shift
= 0, .mask
= 0x0f },
20 { .id
= IP4_DSCP
, .len
= 1, .offset
= 1, .shift
= 2, .mask
= 0xfc },
21 { .id
= IP4_ECN
, .len
= 1, .offset
= 1, .shift
= 0, .mask
= 0x03 },
22 { .id
= IP4_TOS
, .len
= 1, .offset
= 1 },
23 { .id
= IP4_LEN
, .len
= 2, .offset
= 2 },
24 { .id
= IP4_ID
, .len
= 2, .offset
= 4 },
25 { .id
= IP4_FLAGS
, .len
= 2, .offset
= 6, .shift
= 13, .mask
= 0xe000 },
26 { .id
= IP4_MF
, .len
= 2, .offset
= 6, .shift
= 13, .mask
= 0x2000 },
27 { .id
= IP4_DF
, .len
= 2, .offset
= 6, .shift
= 14, .mask
= 0x4000 },
28 { .id
= IP4_FRAG_OFFS
, .len
= 2, .offset
= 6, .shift
= 0, .mask
= 0x1fff },
29 { .id
= IP4_TTL
, .len
= 1, .offset
= 8 },
30 { .id
= IP4_PROTO
, .len
= 1, .offset
= 9 },
31 { .id
= IP4_CSUM
, .len
= 2, .offset
= 10 },
32 { .id
= IP4_SADDR
, .len
= 4, .offset
= 12 },
33 { .id
= IP4_DADDR
, .len
= 4, .offset
= 16 },
36 static void ipv4_header_init(struct proto_hdr
*hdr
)
38 struct dev_io
*dev
= proto_dev_get();
40 /* In case of tun interface we do not need to create Ethernet header */
41 if (dev_io_is_pcap(dev
) || device_type(dev_io_name_get(dev
)) != ARPHRD_NONE
)
42 proto_lower_default_add(hdr
, PROTO_ETH
);
44 proto_header_fields_add(hdr
, ipv4_fields
, array_size(ipv4_fields
));
46 proto_hdr_field_set_default_u8(hdr
, IP4_VER
, 4);
47 proto_hdr_field_set_default_u8(hdr
, IP4_IHL
, 5);
48 proto_hdr_field_set_default_dev_ipv4(hdr
, IP4_SADDR
);
51 static void ipv4_field_changed(struct proto_field
*field
)
53 field
->hdr
->is_csum_valid
= false;
55 if (field
->id
== IP4_SADDR
|| field
->id
== IP4_DADDR
) {
56 struct proto_hdr
*upper
= proto_upper_header(field
->hdr
);
58 if (upper
&& (upper
->ops
->id
== PROTO_UDP
|| upper
->ops
->id
== PROTO_TCP
))
59 upper
->is_csum_valid
= false;
63 static void ipv4_csum_update(struct proto_hdr
*hdr
)
68 if (hdr
->is_csum_valid
)
70 if (proto_hdr_field_is_set(hdr
, IP4_CSUM
))
73 pkt
= packet_get(hdr
->pkt_id
);
75 proto_hdr_field_set_default_u16(hdr
, IP4_CSUM
, 0);
76 csum
= htons(calc_csum(&pkt
->payload
[hdr
->pkt_offset
], hdr
->len
));
77 proto_hdr_field_set_default_u16(hdr
, IP4_CSUM
, bswap_16(csum
));
79 hdr
->is_csum_valid
= true;
82 static void ipv4_packet_finish(struct proto_hdr
*hdr
)
84 struct packet
*pkt
= current_packet();
87 total_len
= pkt
->len
- hdr
->pkt_offset
;
88 proto_hdr_field_set_default_be16(hdr
, IP4_LEN
, total_len
);
90 ipv4_csum_update(hdr
);
93 static void ipv4_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
99 ip_proto
= IPPROTO_IPIP
;
102 ip_proto
= IPPROTO_IPV6
;
105 ip_proto
= IPPROTO_ICMP
;
108 ip_proto
= IPPROTO_UDP
;
111 ip_proto
= IPPROTO_TCP
;
117 proto_hdr_field_set_default_u8(hdr
, IP4_PROTO
, ip_proto
);
120 static const struct proto_ops ipv4_proto_ops
= {
123 .header_init
= ipv4_header_init
,
124 .packet_update
= ipv4_csum_update
,
125 .field_changed
= ipv4_field_changed
,
126 .packet_finish
= ipv4_packet_finish
,
127 .set_next_proto
= ipv4_set_next_proto
,
130 static struct proto_field ipv6_fields
[] = {
131 { .id
= IP6_VER
, .len
= 4, .offset
= 0, .shift
= 28, .mask
= 0xf0000000 },
132 { .id
= IP6_CLASS
, .len
= 4, .offset
= 0, .shift
= 20, .mask
= 0x0ff00000 },
133 { .id
= IP6_FLOW_LBL
, .len
= 4, .offset
= 0, .shift
= 0, .mask
= 0x000fffff },
134 { .id
= IP6_LEN
, .len
= 2, .offset
= 4 },
135 { .id
= IP6_NEXT_HDR
, .len
= 1, .offset
= 6 },
136 { .id
= IP6_HOP_LIMIT
, .len
= 1, .offset
= 7 },
137 { .id
= IP6_SADDR
, .len
= 16, .offset
= 8 },
138 { .id
= IP6_DADDR
, .len
= 16, .offset
= 24 },
141 static void ipv6_header_init(struct proto_hdr
*hdr
)
143 struct dev_io
*dev
= proto_dev_get();
145 /* In case of tun interface we do not need to create Ethernet header */
146 if (dev_io_is_netdev(dev
) && device_type(dev_io_name_get(dev
)) != ARPHRD_NONE
)
147 proto_lower_default_add(hdr
, PROTO_ETH
);
149 proto_header_fields_add(hdr
, ipv6_fields
, array_size(ipv6_fields
));
151 proto_hdr_field_set_default_be32(hdr
, IP6_VER
, 6);
152 proto_hdr_field_set_default_dev_ipv6(hdr
, IP6_SADDR
);
155 static void ipv6_field_changed(struct proto_field
*field
)
157 if (field
->id
== IP6_SADDR
|| field
->id
== IP6_DADDR
) {
158 struct proto_hdr
*upper
= proto_upper_header(field
->hdr
);
160 if (upper
&& (upper
->ops
->id
== PROTO_UDP
|| upper
->ops
->id
== PROTO_TCP
))
161 upper
->is_csum_valid
= false;
165 #define IPV6_HDR_LEN 40
167 static void ipv6_packet_finish(struct proto_hdr
*hdr
)
169 struct packet
*pkt
= current_packet();
170 uint16_t total_len
= pkt
->len
- hdr
->pkt_offset
- IPV6_HDR_LEN
;
172 proto_hdr_field_set_default_be16(hdr
, IP6_LEN
, total_len
);
175 static void ipv6_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
181 ip_proto
= IPPROTO_ICMPV6
;
184 ip_proto
= IPPROTO_UDP
;
187 ip_proto
= IPPROTO_TCP
;
193 proto_hdr_field_set_default_u8(hdr
, IP6_NEXT_HDR
, ip_proto
);
196 static const struct proto_ops ipv6_proto_ops
= {
199 .header_init
= ipv6_header_init
,
200 .field_changed
= ipv6_field_changed
,
201 .packet_finish
= ipv6_packet_finish
,
202 .set_next_proto
= ipv6_set_next_proto
,
205 void protos_l3_init(void)
207 proto_ops_register(&ipv4_proto_ops
);
208 proto_ops_register(&ipv6_proto_ops
);