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 void eth_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
45 proto_hdr_field_set_default_be16(hdr
, ETH_TYPE
, pid_to_eth(pid
));
48 static void eth_header_init(struct proto_hdr
*hdr
)
50 proto_header_fields_add(hdr
, eth_fields
, array_size(eth_fields
));
52 proto_hdr_field_set_default_dev_mac(hdr
, ETH_SRC_ADDR
);
54 dev_io_link_type_set(proto_dev_get(), LINKTYPE_EN10MB
);
57 static const struct proto_ops eth_proto_ops
= {
60 .header_init
= eth_header_init
,
61 .set_next_proto
= eth_set_next_proto
,
64 static struct proto_field pause_fields
[] = {
65 { .id
= PAUSE_OPCODE
, .len
= 2, .offset
= 0 },
66 { .id
= PAUSE_TIME
, .len
= 2, .offset
= 2 },
69 static void pause_header_init(struct proto_hdr
*hdr
)
71 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
73 struct proto_hdr
*lower
;
75 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
76 proto_hdr_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
, 6);
78 proto_header_fields_add(hdr
, pause_fields
, array_size(pause_fields
));
79 proto_hdr_field_set_default_be16(hdr
, PAUSE_OPCODE
, 0x1);
82 static struct proto_ops pause_proto_ops
= {
85 .header_init
= pause_header_init
,
88 static struct proto_field pfc_fields
[] = {
89 { .id
= PFC_OPCODE
, .len
= 2, .offset
= 0 },
90 { .id
= PFC_PRIO
, .len
= 2, .offset
= 2 },
91 { .id
= PFC_PRIO_0
, .len
= 2, .offset
= 2, .mask
= 0x0001 },
92 { .id
= PFC_PRIO_1
, .len
= 2, .offset
= 2, .mask
= 0x0002, .shift
= 1 },
93 { .id
= PFC_PRIO_2
, .len
= 2, .offset
= 2, .mask
= 0x0004, .shift
= 2 },
94 { .id
= PFC_PRIO_3
, .len
= 2, .offset
= 2, .mask
= 0x0008, .shift
= 3 },
95 { .id
= PFC_PRIO_4
, .len
= 2, .offset
= 2, .mask
= 0x0010, .shift
= 4 },
96 { .id
= PFC_PRIO_5
, .len
= 2, .offset
= 2, .mask
= 0x0020, .shift
= 5 },
97 { .id
= PFC_PRIO_6
, .len
= 2, .offset
= 2, .mask
= 0x0040, .shift
= 6 },
98 { .id
= PFC_PRIO_7
, .len
= 2, .offset
= 2, .mask
= 0x0080, .shift
= 7 },
99 { .id
= PFC_TIME_0
, .len
= 2, .offset
= 4, },
100 { .id
= PFC_TIME_1
, .len
= 2, .offset
= 6, },
101 { .id
= PFC_TIME_2
, .len
= 2, .offset
= 8, },
102 { .id
= PFC_TIME_3
, .len
= 2, .offset
= 10, },
103 { .id
= PFC_TIME_4
, .len
= 2, .offset
= 12, },
104 { .id
= PFC_TIME_5
, .len
= 2, .offset
= 14, },
105 { .id
= PFC_TIME_6
, .len
= 2, .offset
= 16, },
106 { .id
= PFC_TIME_7
, .len
= 2, .offset
= 18, },
109 static void pfc_header_init(struct proto_hdr
*hdr
)
111 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
113 struct proto_hdr
*lower
;
115 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
116 proto_hdr_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
, 6);
118 proto_header_fields_add(hdr
, pfc_fields
, array_size(pfc_fields
));
119 proto_hdr_field_set_default_be16(hdr
, PFC_OPCODE
, 0x0101);
122 static struct proto_ops pfc_proto_ops
= {
125 .header_init
= pfc_header_init
,
128 static struct proto_field vlan_fields
[] = {
129 /* TPID overlaps with Ethernet header and points to ether type */
130 { .id
= VLAN_TPID
, .len
= 2, .offset
= -2 },
131 { .id
= VLAN_TCI
, .len
= 2, .offset
= 0 },
132 { .id
= VLAN_PCP
, .len
= 2, .offset
= 0, .shift
= 13, .mask
= 0xe000 },
133 { .id
= VLAN_DEI
, .len
= 2, .offset
= 0, .shift
= 12, .mask
= 0x1000 },
134 { .id
= VLAN_VID
, .len
= 2, .offset
= 0, .shift
= 0, .mask
= 0xfff },
135 /* Original ether type is stored after VLAN header */
136 { .id
= VLAN_ETYPE
, .len
= 2, .offset
= 2 },
139 static void vlan_header_init(struct proto_hdr
*hdr
)
141 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
142 uint16_t lower_etype
= 0;
144 proto_header_fields_add(hdr
, vlan_fields
, array_size(vlan_fields
));
146 if (lower
->ops
->id
== PROTO_ETH
)
147 lower_etype
= proto_hdr_field_get_u16(lower
, ETH_TYPE
);
148 else if (lower
->ops
->id
== PROTO_VLAN
)
149 lower_etype
= proto_hdr_field_get_u16(lower
, VLAN_ETYPE
);
151 proto_hdr_field_set_be16(hdr
, VLAN_ETYPE
, lower_etype
);
152 proto_hdr_field_set_default_be16(hdr
, VLAN_TPID
, pid_to_eth(hdr
->ops
->id
));
155 static void vlan_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
157 if (pid
!= PROTO_VLAN
)
158 proto_hdr_field_set_be16(hdr
, VLAN_ETYPE
, pid_to_eth(pid
));
161 static const struct proto_ops vlan_proto_ops
= {
164 .header_init
= vlan_header_init
,
165 .set_next_proto
= vlan_set_next_proto
,
168 static struct proto_field arp_fields
[] = {
169 { .id
= ARP_HTYPE
, .len
= 2 },
170 { .id
= ARP_PTYPE
, .len
= 2, .offset
= 2 },
171 { .id
= ARP_HLEN
, .len
= 1, .offset
= 4 },
172 { .id
= ARP_PLEN
, .len
= 1, .offset
= 5 },
173 { .id
= ARP_OPER
, .len
= 2, .offset
= 6 },
174 { .id
= ARP_SHA
, .len
= 6, .offset
= 8 },
175 { .id
= ARP_SPA
, .len
= 4, .offset
= 14 },
176 { .id
= ARP_THA
, .len
= 6, .offset
= 18 },
177 { .id
= ARP_TPA
, .len
= 4, .offset
= 24 },
180 static void arp_header_init(struct proto_hdr
*hdr
)
182 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
184 if (lower
->ops
->id
== PROTO_ETH
) {
185 const uint8_t bcast
[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
187 proto_hdr_field_set_default_bytes(lower
, ETH_DST_ADDR
, bcast
, 6);
190 proto_header_fields_add(hdr
, arp_fields
, array_size(arp_fields
));
192 /* Generate Announce request by default */
193 proto_hdr_field_set_default_be16(hdr
, ARP_HTYPE
, ARPHRD_ETHER
);
194 proto_hdr_field_set_default_be16(hdr
, ARP_PTYPE
, ETH_P_IP
);
195 proto_hdr_field_set_default_u8(hdr
, ARP_HLEN
, 6);
196 proto_hdr_field_set_default_u8(hdr
, ARP_PLEN
, 4);
197 proto_hdr_field_set_default_be16(hdr
, ARP_OPER
, ARPOP_REQUEST
);
198 proto_hdr_field_set_default_dev_mac(hdr
, ARP_SHA
);
199 proto_hdr_field_set_default_dev_ipv4(hdr
, ARP_SPA
);
200 proto_hdr_field_set_default_dev_ipv4(hdr
, ARP_TPA
);
203 static const struct proto_ops arp_proto_ops
= {
206 .header_init
= arp_header_init
,
209 static struct proto_field mpls_fields
[] = {
210 { .id
= MPLS_LABEL
, .len
= 4, .shift
= 12, .mask
= 0xfffff000 },
211 { .id
= MPLS_TC
, .len
= 4, .shift
= 9, .mask
= 0xe00 },
212 { .id
= MPLS_LAST
, .len
= 4, .shift
= 8, .mask
= 0x100 },
213 { .id
= MPLS_TTL
, .len
= 4, .shift
= 0, .mask
= 0xff },
216 static void mpls_header_init(struct proto_hdr
*hdr
)
218 proto_lower_default_add(hdr
, PROTO_ETH
);
220 proto_header_fields_add(hdr
, mpls_fields
, array_size(mpls_fields
));
222 proto_hdr_field_set_default_be32(hdr
, MPLS_LAST
, 1);
225 static void mpls_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
227 if (pid
== PROTO_MPLS
)
228 proto_hdr_field_set_default_be32(hdr
, MPLS_LAST
, 0);
231 static const struct proto_ops mpls_proto_ops
= {
234 .header_init
= mpls_header_init
,
235 .set_next_proto
= mpls_set_next_proto
,
238 void protos_l2_init(void)
240 proto_ops_register(ð_proto_ops
);
241 proto_ops_register(&pause_proto_ops
);
242 proto_ops_register(&pfc_proto_ops
);
243 proto_ops_register(&vlan_proto_ops
);
244 proto_ops_register(&arp_proto_ops
);
245 proto_ops_register(&mpls_proto_ops
);