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>
11 #include "trafgen_l2.h"
12 #include "trafgen_proto.h"
14 static struct proto_field eth_fields
[] = {
15 { .id
= ETH_DST_ADDR
, .len
= 6, },
16 { .id
= ETH_SRC_ADDR
, .len
= 6, .offset
= 6 },
17 { .id
= ETH_TYPE
, .len
= 2, .offset
= 12 },
20 static uint16_t pid_to_eth(enum proto_id pid
)
41 static void eth_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
43 proto_field_set_default_be16(hdr
, ETH_TYPE
, pid_to_eth(pid
));
46 static void eth_header_init(struct proto_hdr
*hdr
)
48 proto_header_fields_add(hdr
, eth_fields
, array_size(eth_fields
));
50 proto_field_set_default_dev_mac(hdr
, ETH_SRC_ADDR
);
53 static const struct proto_ops eth_proto_ops
= {
56 .header_init
= eth_header_init
,
57 .set_next_proto
= eth_set_next_proto
,
60 static struct proto_field pause_fields
[] = {
61 { .id
= PAUSE_OPCODE
, .len
= 2, .offset
= 0 },
62 { .id
= PAUSE_TIME
, .len
= 2, .offset
= 2 },
65 static void pause_header_init(struct proto_hdr
*hdr
)
67 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
69 struct proto_hdr
*lower
;
71 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
72 proto_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
);
74 proto_header_fields_add(hdr
, pause_fields
, array_size(pause_fields
));
75 proto_field_set_default_be16(hdr
, PAUSE_OPCODE
, 0x1);
78 static struct proto_ops pause_proto_ops
= {
81 .header_init
= pause_header_init
,
84 static struct proto_field pfc_fields
[] = {
85 { .id
= PFC_OPCODE
, .len
= 2, .offset
= 0 },
86 { .id
= PFC_PRIO
, .len
= 2, .offset
= 2 },
87 { .id
= PFC_PRIO_0
, .len
= 2, .offset
= 2, .mask
= 0x0001 },
88 { .id
= PFC_PRIO_1
, .len
= 2, .offset
= 2, .mask
= 0x0002, .shift
= 1 },
89 { .id
= PFC_PRIO_2
, .len
= 2, .offset
= 2, .mask
= 0x0004, .shift
= 2 },
90 { .id
= PFC_PRIO_3
, .len
= 2, .offset
= 2, .mask
= 0x0008, .shift
= 3 },
91 { .id
= PFC_PRIO_4
, .len
= 2, .offset
= 2, .mask
= 0x0010, .shift
= 4 },
92 { .id
= PFC_PRIO_5
, .len
= 2, .offset
= 2, .mask
= 0x0020, .shift
= 5 },
93 { .id
= PFC_PRIO_6
, .len
= 2, .offset
= 2, .mask
= 0x0040, .shift
= 6 },
94 { .id
= PFC_PRIO_7
, .len
= 2, .offset
= 2, .mask
= 0x0080, .shift
= 7 },
95 { .id
= PFC_TIME_0
, .len
= 2, .offset
= 4, },
96 { .id
= PFC_TIME_1
, .len
= 2, .offset
= 6, },
97 { .id
= PFC_TIME_2
, .len
= 2, .offset
= 8, },
98 { .id
= PFC_TIME_3
, .len
= 2, .offset
= 10, },
99 { .id
= PFC_TIME_4
, .len
= 2, .offset
= 12, },
100 { .id
= PFC_TIME_5
, .len
= 2, .offset
= 14, },
101 { .id
= PFC_TIME_6
, .len
= 2, .offset
= 16, },
102 { .id
= PFC_TIME_7
, .len
= 2, .offset
= 18, },
105 static void pfc_header_init(struct proto_hdr
*hdr
)
107 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
109 struct proto_hdr
*lower
;
111 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
112 proto_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
);
114 proto_header_fields_add(hdr
, pfc_fields
, array_size(pfc_fields
));
115 proto_field_set_default_be16(hdr
, PFC_OPCODE
, 0x0101);
118 static struct proto_ops pfc_proto_ops
= {
121 .header_init
= pfc_header_init
,
124 static struct proto_field vlan_fields
[] = {
125 /* TPID overlaps with Ethernet header and points to ether type */
126 { .id
= VLAN_TPID
, .len
= 2, .offset
= -2 },
127 { .id
= VLAN_TCI
, .len
= 2, .offset
= 0 },
128 { .id
= VLAN_PCP
, .len
= 2, .offset
= 0, .shift
= 13, .mask
= 0xe000 },
129 { .id
= VLAN_DEI
, .len
= 2, .offset
= 0, .shift
= 12, .mask
= 0x1000 },
130 { .id
= VLAN_VID
, .len
= 2, .offset
= 0, .shift
= 0, .mask
= 0xfff },
131 /* Original ether type is stored after VLAN header */
132 { .id
= VLAN_ETYPE
, .len
= 2, .offset
= 2 },
135 static void vlan_header_init(struct proto_hdr
*hdr
)
137 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
138 uint16_t lower_etype
= 0;
140 proto_header_fields_add(hdr
, vlan_fields
, array_size(vlan_fields
));
142 if (lower
->ops
->id
== PROTO_ETH
)
143 lower_etype
= proto_field_get_u16(lower
, ETH_TYPE
);
144 else if (lower
->ops
->id
== PROTO_VLAN
)
145 lower_etype
= proto_field_get_u16(lower
, VLAN_ETYPE
);
147 proto_field_set_be16(hdr
, VLAN_ETYPE
, lower_etype
);
148 proto_field_set_default_be16(hdr
, VLAN_TPID
, pid_to_eth(hdr
->ops
->id
));
151 static void vlan_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
153 if (pid
!= PROTO_VLAN
)
154 proto_field_set_be16(hdr
, VLAN_ETYPE
, pid_to_eth(pid
));
157 static const struct proto_ops vlan_proto_ops
= {
160 .header_init
= vlan_header_init
,
161 .set_next_proto
= vlan_set_next_proto
,
164 static struct proto_field arp_fields
[] = {
165 { .id
= ARP_HTYPE
, .len
= 2 },
166 { .id
= ARP_PTYPE
, .len
= 2, .offset
= 2 },
167 { .id
= ARP_HLEN
, .len
= 1, .offset
= 4 },
168 { .id
= ARP_PLEN
, .len
= 1, .offset
= 5 },
169 { .id
= ARP_OPER
, .len
= 2, .offset
= 6 },
170 { .id
= ARP_SHA
, .len
= 6, .offset
= 8 },
171 { .id
= ARP_SPA
, .len
= 4, .offset
= 14 },
172 { .id
= ARP_THA
, .len
= 6, .offset
= 18 },
173 { .id
= ARP_TPA
, .len
= 4, .offset
= 24 },
176 static void arp_header_init(struct proto_hdr
*hdr
)
178 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
180 if (lower
->ops
->id
== PROTO_ETH
) {
181 const uint8_t bcast
[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
183 proto_field_set_default_bytes(lower
, ETH_DST_ADDR
, bcast
);
186 proto_header_fields_add(hdr
, arp_fields
, array_size(arp_fields
));
188 /* Generate Announce request by default */
189 proto_field_set_default_be16(hdr
, ARP_HTYPE
, ARPHRD_ETHER
);
190 proto_field_set_default_be16(hdr
, ARP_PTYPE
, ETH_P_IP
);
191 proto_field_set_default_u8(hdr
, ARP_HLEN
, 6);
192 proto_field_set_default_u8(hdr
, ARP_PLEN
, 4);
193 proto_field_set_default_be16(hdr
, ARP_OPER
, ARPOP_REQUEST
);
194 proto_field_set_default_dev_mac(hdr
, ARP_SHA
);
195 proto_field_set_default_dev_ipv4(hdr
, ARP_SPA
);
196 proto_field_set_default_dev_ipv4(hdr
, ARP_TPA
);
199 static const struct proto_ops arp_proto_ops
= {
202 .header_init
= arp_header_init
,
205 static struct proto_field mpls_fields
[] = {
206 { .id
= MPLS_LABEL
, .len
= 4, .shift
= 12, .mask
= 0xfffff000 },
207 { .id
= MPLS_TC
, .len
= 4, .shift
= 9, .mask
= 0xe00 },
208 { .id
= MPLS_LAST
, .len
= 4, .shift
= 8, .mask
= 0x100 },
209 { .id
= MPLS_TTL
, .len
= 4, .shift
= 0, .mask
= 0xff },
212 static void mpls_header_init(struct proto_hdr
*hdr
)
214 proto_lower_default_add(hdr
, PROTO_ETH
);
216 proto_header_fields_add(hdr
, mpls_fields
, array_size(mpls_fields
));
218 proto_field_set_default_be32(hdr
, MPLS_LAST
, 1);
221 static void mpls_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
223 if (pid
== PROTO_MPLS
)
224 proto_field_set_default_be32(hdr
, MPLS_LAST
, 0);
227 static const struct proto_ops mpls_proto_ops
= {
230 .header_init
= mpls_header_init
,
231 .set_next_proto
= mpls_set_next_proto
,
234 void protos_l2_init(void)
236 proto_ops_register(ð_proto_ops
);
237 proto_ops_register(&pause_proto_ops
);
238 proto_ops_register(&pfc_proto_ops
);
239 proto_ops_register(&vlan_proto_ops
);
240 proto_ops_register(&arp_proto_ops
);
241 proto_ops_register(&mpls_proto_ops
);