2 * netsniff-ng - the packet sniffing beast
3 * Subject to the GPL, version 2.
6 #include <net/if_arp.h>
7 #include <linux/if_ether.h>
12 #include "trafgen_l2.h"
13 #include "trafgen_dev.h"
14 #include "trafgen_proto.h"
16 static struct proto_field eth_fields
[] = {
17 { .id
= ETH_DST_ADDR
, .len
= 6, },
18 { .id
= ETH_SRC_ADDR
, .len
= 6, .offset
= 6 },
19 { .id
= ETH_TYPE
, .len
= 2, .offset
= 12 },
22 static uint16_t pid_to_eth(enum proto_id pid
)
43 static uint16_t eth_to_pid(uint16_t etype
)
64 static void eth_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
66 proto_hdr_field_set_default_be16(hdr
, ETH_TYPE
, pid_to_eth(pid
));
69 static enum proto_id
eth_get_next_proto(struct proto_hdr
*hdr
)
71 return eth_to_pid(proto_hdr_field_get_u16(hdr
, ETH_TYPE
));
74 static void eth_header_init(struct proto_hdr
*hdr
)
76 proto_header_fields_add(hdr
, eth_fields
, array_size(eth_fields
));
78 proto_hdr_field_set_default_dev_mac(hdr
, ETH_SRC_ADDR
);
80 dev_io_link_type_set(proto_dev_get(), LINKTYPE_EN10MB
);
83 static const struct proto_ops eth_proto_ops
= {
86 .header_init
= eth_header_init
,
87 .set_next_proto
= eth_set_next_proto
,
88 .get_next_proto
= eth_get_next_proto
,
91 static struct proto_field pause_fields
[] = {
92 { .id
= PAUSE_OPCODE
, .len
= 2, .offset
= 0 },
93 { .id
= PAUSE_TIME
, .len
= 2, .offset
= 2 },
96 static void pause_header_init(struct proto_hdr
*hdr
)
98 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
100 struct proto_hdr
*lower
;
102 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
103 proto_hdr_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
, 6);
105 proto_header_fields_add(hdr
, pause_fields
, array_size(pause_fields
));
106 proto_hdr_field_set_default_be16(hdr
, PAUSE_OPCODE
, 0x1);
109 static struct proto_ops pause_proto_ops
= {
112 .header_init
= pause_header_init
,
115 static struct proto_field pfc_fields
[] = {
116 { .id
= PFC_OPCODE
, .len
= 2, .offset
= 0 },
117 { .id
= PFC_PRIO
, .len
= 2, .offset
= 2 },
118 { .id
= PFC_PRIO_0
, .len
= 2, .offset
= 2, .mask
= 0x0001 },
119 { .id
= PFC_PRIO_1
, .len
= 2, .offset
= 2, .mask
= 0x0002, .shift
= 1 },
120 { .id
= PFC_PRIO_2
, .len
= 2, .offset
= 2, .mask
= 0x0004, .shift
= 2 },
121 { .id
= PFC_PRIO_3
, .len
= 2, .offset
= 2, .mask
= 0x0008, .shift
= 3 },
122 { .id
= PFC_PRIO_4
, .len
= 2, .offset
= 2, .mask
= 0x0010, .shift
= 4 },
123 { .id
= PFC_PRIO_5
, .len
= 2, .offset
= 2, .mask
= 0x0020, .shift
= 5 },
124 { .id
= PFC_PRIO_6
, .len
= 2, .offset
= 2, .mask
= 0x0040, .shift
= 6 },
125 { .id
= PFC_PRIO_7
, .len
= 2, .offset
= 2, .mask
= 0x0080, .shift
= 7 },
126 { .id
= PFC_TIME_0
, .len
= 2, .offset
= 4, },
127 { .id
= PFC_TIME_1
, .len
= 2, .offset
= 6, },
128 { .id
= PFC_TIME_2
, .len
= 2, .offset
= 8, },
129 { .id
= PFC_TIME_3
, .len
= 2, .offset
= 10, },
130 { .id
= PFC_TIME_4
, .len
= 2, .offset
= 12, },
131 { .id
= PFC_TIME_5
, .len
= 2, .offset
= 14, },
132 { .id
= PFC_TIME_6
, .len
= 2, .offset
= 16, },
133 { .id
= PFC_TIME_7
, .len
= 2, .offset
= 18, },
136 static void pfc_header_init(struct proto_hdr
*hdr
)
138 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
140 struct proto_hdr
*lower
;
142 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
143 proto_hdr_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
, 6);
145 proto_header_fields_add(hdr
, pfc_fields
, array_size(pfc_fields
));
146 proto_hdr_field_set_default_be16(hdr
, PFC_OPCODE
, 0x0101);
149 static struct proto_ops pfc_proto_ops
= {
152 .header_init
= pfc_header_init
,
155 static struct proto_field vlan_fields
[] = {
156 /* TPID overlaps with Ethernet header and points to ether type */
157 { .id
= VLAN_TPID
, .len
= 2, .offset
= -2 },
158 { .id
= VLAN_TCI
, .len
= 2, .offset
= 0 },
159 { .id
= VLAN_PCP
, .len
= 2, .offset
= 0, .shift
= 13, .mask
= 0xe000 },
160 { .id
= VLAN_DEI
, .len
= 2, .offset
= 0, .shift
= 12, .mask
= 0x1000 },
161 { .id
= VLAN_VID
, .len
= 2, .offset
= 0, .shift
= 0, .mask
= 0xfff },
162 /* Original ether type is stored after VLAN header */
163 { .id
= VLAN_ETYPE
, .len
= 2, .offset
= 2 },
166 static void vlan_header_init(struct proto_hdr
*hdr
)
168 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
169 uint16_t lower_etype
= 0;
171 proto_header_fields_add(hdr
, vlan_fields
, array_size(vlan_fields
));
173 if (lower
->ops
->id
== PROTO_ETH
)
174 lower_etype
= proto_hdr_field_get_u16(lower
, ETH_TYPE
);
175 else if (lower
->ops
->id
== PROTO_VLAN
)
176 lower_etype
= proto_hdr_field_get_u16(lower
, VLAN_ETYPE
);
178 proto_hdr_field_set_be16(hdr
, VLAN_ETYPE
, lower_etype
);
179 proto_hdr_field_set_default_be16(hdr
, VLAN_TPID
, pid_to_eth(hdr
->ops
->id
));
182 static void vlan_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
184 if (pid
!= PROTO_VLAN
)
185 proto_hdr_field_set_be16(hdr
, VLAN_ETYPE
, pid_to_eth(pid
));
188 static enum proto_id
vlan_get_next_proto(struct proto_hdr
*hdr
)
190 return eth_to_pid(proto_hdr_field_get_u16(hdr
, VLAN_ETYPE
));
193 static const struct proto_ops vlan_proto_ops
= {
196 .header_init
= vlan_header_init
,
197 .set_next_proto
= vlan_set_next_proto
,
198 .get_next_proto
= vlan_get_next_proto
,
201 static struct proto_field arp_fields
[] = {
202 { .id
= ARP_HTYPE
, .len
= 2 },
203 { .id
= ARP_PTYPE
, .len
= 2, .offset
= 2 },
204 { .id
= ARP_HLEN
, .len
= 1, .offset
= 4 },
205 { .id
= ARP_PLEN
, .len
= 1, .offset
= 5 },
206 { .id
= ARP_OPER
, .len
= 2, .offset
= 6 },
207 { .id
= ARP_SHA
, .len
= 6, .offset
= 8 },
208 { .id
= ARP_SPA
, .len
= 4, .offset
= 14 },
209 { .id
= ARP_THA
, .len
= 6, .offset
= 18 },
210 { .id
= ARP_TPA
, .len
= 4, .offset
= 24 },
213 static void arp_header_init(struct proto_hdr
*hdr
)
215 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
217 if (lower
->ops
->id
== PROTO_ETH
) {
218 const uint8_t bcast
[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
220 proto_hdr_field_set_default_bytes(lower
, ETH_DST_ADDR
, bcast
, 6);
223 proto_header_fields_add(hdr
, arp_fields
, array_size(arp_fields
));
225 /* Generate Announce request by default */
226 proto_hdr_field_set_default_be16(hdr
, ARP_HTYPE
, ARPHRD_ETHER
);
227 proto_hdr_field_set_default_be16(hdr
, ARP_PTYPE
, ETH_P_IP
);
228 proto_hdr_field_set_default_u8(hdr
, ARP_HLEN
, 6);
229 proto_hdr_field_set_default_u8(hdr
, ARP_PLEN
, 4);
230 proto_hdr_field_set_default_be16(hdr
, ARP_OPER
, ARPOP_REQUEST
);
231 proto_hdr_field_set_default_dev_mac(hdr
, ARP_SHA
);
232 proto_hdr_field_set_default_dev_ipv4(hdr
, ARP_SPA
);
233 proto_hdr_field_set_default_dev_ipv4(hdr
, ARP_TPA
);
236 static const struct proto_ops arp_proto_ops
= {
239 .header_init
= arp_header_init
,
242 static struct proto_field mpls_fields
[] = {
243 { .id
= MPLS_LABEL
, .len
= 4, .shift
= 12, .mask
= 0xfffff000 },
244 { .id
= MPLS_TC
, .len
= 4, .shift
= 9, .mask
= 0xe00 },
245 { .id
= MPLS_LAST
, .len
= 4, .shift
= 8, .mask
= 0x100 },
246 { .id
= MPLS_TTL
, .len
= 4, .shift
= 0, .mask
= 0xff },
249 static void mpls_header_init(struct proto_hdr
*hdr
)
251 proto_lower_default_add(hdr
, PROTO_ETH
);
253 proto_header_fields_add(hdr
, mpls_fields
, array_size(mpls_fields
));
255 proto_hdr_field_set_default_be32(hdr
, MPLS_LAST
, 1);
258 static void mpls_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
260 if (pid
== PROTO_MPLS
)
261 proto_hdr_field_set_default_be32(hdr
, MPLS_LAST
, 0);
264 static const struct proto_ops mpls_proto_ops
= {
267 .header_init
= mpls_header_init
,
268 .set_next_proto
= mpls_set_next_proto
,
271 void protos_l2_init(void)
273 proto_ops_register(ð_proto_ops
);
274 proto_ops_register(&pause_proto_ops
);
275 proto_ops_register(&pfc_proto_ops
);
276 proto_ops_register(&vlan_proto_ops
);
277 proto_ops_register(&arp_proto_ops
);
278 proto_ops_register(&mpls_proto_ops
);