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
)
40 static void eth_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
42 proto_field_set_default_be16(hdr
, ETH_TYPE
, pid_to_eth(pid
));
45 static void eth_header_init(struct proto_hdr
*hdr
)
47 proto_header_fields_add(hdr
, eth_fields
, array_size(eth_fields
));
49 proto_field_set_default_dev_mac(hdr
, ETH_SRC_ADDR
);
52 static const struct proto_ops eth_proto_ops
= {
55 .header_init
= eth_header_init
,
56 .set_next_proto
= eth_set_next_proto
,
59 static struct proto_field pause_fields
[] = {
60 { .id
= PAUSE_OPCODE
, .len
= 2, .offset
= 0 },
61 { .id
= PAUSE_TIME
, .len
= 2, .offset
= 2 },
64 static void pause_header_init(struct proto_hdr
*hdr
)
66 uint8_t eth_dst
[6] = { 0x01, 0x80, 0xC2, 0x00, 0x00, 0x01 };
68 struct proto_hdr
*lower
;
70 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
71 proto_field_set_default_bytes(lower
, ETH_DST_ADDR
, eth_dst
);
73 proto_header_fields_add(hdr
, pause_fields
, array_size(pause_fields
));
74 proto_field_set_default_be16(hdr
, PAUSE_OPCODE
, 0x1);
77 static struct proto_ops pause_proto_ops
= {
80 .header_init
= pause_header_init
,
83 static struct proto_field vlan_fields
[] = {
84 /* TPID overlaps with Ethernet header and points to ether type */
85 { .id
= VLAN_TPID
, .len
= 2, .offset
= -2 },
86 { .id
= VLAN_TCI
, .len
= 2, .offset
= 0 },
87 { .id
= VLAN_PCP
, .len
= 2, .offset
= 0, .shift
= 13, .mask
= 0xe000 },
88 { .id
= VLAN_DEI
, .len
= 2, .offset
= 0, .shift
= 12, .mask
= 0x1000 },
89 { .id
= VLAN_VID
, .len
= 2, .offset
= 0, .shift
= 0, .mask
= 0xfff },
90 /* Original ether type is stored after VLAN header */
91 { .id
= VLAN_ETYPE
, .len
= 2, .offset
= 2 },
94 static void vlan_header_init(struct proto_hdr
*hdr
)
96 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
97 uint16_t lower_etype
= 0;
99 proto_header_fields_add(hdr
, vlan_fields
, array_size(vlan_fields
));
101 if (lower
->ops
->id
== PROTO_ETH
)
102 lower_etype
= proto_field_get_u16(lower
, ETH_TYPE
);
103 else if (lower
->ops
->id
== PROTO_VLAN
)
104 lower_etype
= proto_field_get_u16(lower
, VLAN_ETYPE
);
106 proto_field_set_be16(hdr
, VLAN_ETYPE
, lower_etype
);
107 proto_field_set_default_be16(hdr
, VLAN_TPID
, pid_to_eth(hdr
->ops
->id
));
110 static void vlan_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
112 if (pid
!= PROTO_VLAN
)
113 proto_field_set_be16(hdr
, VLAN_ETYPE
, pid_to_eth(pid
));
116 static const struct proto_ops vlan_proto_ops
= {
119 .header_init
= vlan_header_init
,
120 .set_next_proto
= vlan_set_next_proto
,
123 static struct proto_field arp_fields
[] = {
124 { .id
= ARP_HTYPE
, .len
= 2 },
125 { .id
= ARP_PTYPE
, .len
= 2, .offset
= 2 },
126 { .id
= ARP_HLEN
, .len
= 1, .offset
= 4 },
127 { .id
= ARP_PLEN
, .len
= 1, .offset
= 5 },
128 { .id
= ARP_OPER
, .len
= 2, .offset
= 6 },
129 { .id
= ARP_SHA
, .len
= 6, .offset
= 8 },
130 { .id
= ARP_SPA
, .len
= 4, .offset
= 14 },
131 { .id
= ARP_THA
, .len
= 6, .offset
= 18 },
132 { .id
= ARP_TPA
, .len
= 4, .offset
= 24 },
135 static void arp_header_init(struct proto_hdr
*hdr
)
137 struct proto_hdr
*lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
139 if (lower
->ops
->id
== PROTO_ETH
) {
140 const uint8_t bcast
[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
142 proto_field_set_default_bytes(lower
, ETH_DST_ADDR
, bcast
);
145 proto_header_fields_add(hdr
, arp_fields
, array_size(arp_fields
));
147 /* Generate Announce request by default */
148 proto_field_set_default_be16(hdr
, ARP_HTYPE
, ARPHRD_ETHER
);
149 proto_field_set_default_be16(hdr
, ARP_PTYPE
, ETH_P_IP
);
150 proto_field_set_default_u8(hdr
, ARP_HLEN
, 6);
151 proto_field_set_default_u8(hdr
, ARP_PLEN
, 4);
152 proto_field_set_default_be16(hdr
, ARP_OPER
, ARPOP_REQUEST
);
153 proto_field_set_default_dev_mac(hdr
, ARP_SHA
);
154 proto_field_set_default_dev_ipv4(hdr
, ARP_SPA
);
155 proto_field_set_default_dev_ipv4(hdr
, ARP_TPA
);
158 static const struct proto_ops arp_proto_ops
= {
161 .header_init
= arp_header_init
,
164 static struct proto_field mpls_fields
[] = {
165 { .id
= MPLS_LABEL
, .len
= 4, .shift
= 12, .mask
= 0xfffff000 },
166 { .id
= MPLS_TC
, .len
= 4, .shift
= 9, .mask
= 0xe00 },
167 { .id
= MPLS_LAST
, .len
= 4, .shift
= 8, .mask
= 0x100 },
168 { .id
= MPLS_TTL
, .len
= 4, .shift
= 0, .mask
= 0xff },
171 static void mpls_header_init(struct proto_hdr
*hdr
)
173 proto_lower_default_add(hdr
, PROTO_ETH
);
175 proto_header_fields_add(hdr
, mpls_fields
, array_size(mpls_fields
));
177 proto_field_set_default_be32(hdr
, MPLS_LAST
, 1);
180 static void mpls_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
182 if (pid
== PROTO_MPLS
)
183 proto_field_set_default_be32(hdr
, MPLS_LAST
, 0);
186 static const struct proto_ops mpls_proto_ops
= {
189 .header_init
= mpls_header_init
,
190 .set_next_proto
= mpls_set_next_proto
,
193 void protos_l2_init(void)
195 proto_ops_register(ð_proto_ops
);
196 proto_ops_register(&pause_proto_ops
);
197 proto_ops_register(&vlan_proto_ops
);
198 proto_ops_register(&arp_proto_ops
);
199 proto_ops_register(&mpls_proto_ops
);