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
)
32 panic("eth: Not supported protocol id %u\n", pid
);
36 static void eth_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
38 proto_field_set_default_be16(hdr
, ETH_TYPE
, pid_to_eth(pid
));
41 static void eth_header_init(struct proto_hdr
*hdr
)
43 proto_header_fields_add(hdr
, eth_fields
, array_size(eth_fields
));
45 proto_field_set_default_dev_mac(hdr
, ETH_SRC_ADDR
);
48 static struct proto_hdr eth_hdr
= {
51 .header_init
= eth_header_init
,
52 .set_next_proto
= eth_set_next_proto
,
55 static struct proto_field vlan_fields
[] = {
56 /* TPID overlaps with Ethernet header and points to ether type */
57 { .id
= VLAN_TPID
, .len
= 2, .offset
= -2 },
58 { .id
= VLAN_TCI
, .len
= 2, .offset
= 0 },
59 { .id
= VLAN_PCP
, .len
= 2, .offset
= 0, .shift
= 13, .mask
= 0xe000 },
60 { .id
= VLAN_DEI
, .len
= 2, .offset
= 0, .shift
= 12, .mask
= 0x1000 },
61 { .id
= VLAN_VID
, .len
= 2, .offset
= 0, .shift
= 0, .mask
= 0xfff },
62 /* Original ether type is stored after VLAN header */
63 { .id
= VLAN_ETYPE
, .len
= 2, .offset
= 2 },
66 static void vlan_header_init(struct proto_hdr
*hdr
)
68 struct proto_hdr
*lower
;
69 uint16_t lower_etype
= 0;
71 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
73 proto_header_fields_add(hdr
, vlan_fields
, array_size(vlan_fields
));
75 if (lower
->id
== PROTO_ETH
)
76 lower_etype
= proto_field_get_u16(lower
, ETH_TYPE
);
77 else if (lower
->id
== PROTO_VLAN
)
78 lower_etype
= proto_field_get_u16(lower
, VLAN_ETYPE
);
80 proto_field_set_be16(hdr
, VLAN_ETYPE
, lower_etype
);
81 proto_field_set_default_be16(hdr
, VLAN_TPID
, pid_to_eth(hdr
->id
));
84 static void vlan_set_next_proto(struct proto_hdr
*hdr
, enum proto_id pid
)
86 if (pid
!= PROTO_VLAN
)
87 proto_field_set_be16(hdr
, VLAN_ETYPE
, pid_to_eth(pid
));
90 static struct proto_hdr vlan_hdr
= {
93 .header_init
= vlan_header_init
,
94 .set_next_proto
= vlan_set_next_proto
,
97 static struct proto_field arp_fields
[] = {
98 { .id
= ARP_HTYPE
, .len
= 2 },
99 { .id
= ARP_PTYPE
, .len
= 2, .offset
= 2 },
100 { .id
= ARP_HLEN
, .len
= 1, .offset
= 4 },
101 { .id
= ARP_PLEN
, .len
= 1, .offset
= 5 },
102 { .id
= ARP_OPER
, .len
= 2, .offset
= 6 },
103 { .id
= ARP_SHA
, .len
= 6, .offset
= 8 },
104 { .id
= ARP_SPA
, .len
= 4, .offset
= 14 },
105 { .id
= ARP_THA
, .len
= 6, .offset
= 18 },
106 { .id
= ARP_TPA
, .len
= 4, .offset
= 24 },
109 static void arp_header_init(struct proto_hdr
*hdr
)
111 struct proto_hdr
*lower
;
113 lower
= proto_lower_default_add(hdr
, PROTO_ETH
);
115 if (lower
->id
== PROTO_ETH
) {
116 uint8_t bcast
[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
118 proto_field_set_default_bytes(lower
, ETH_DST_ADDR
, bcast
);
121 proto_header_fields_add(hdr
, arp_fields
, array_size(arp_fields
));
123 /* Generate Announce request by default */
124 proto_field_set_default_be16(hdr
, ARP_HTYPE
, ARPHRD_ETHER
);
125 proto_field_set_default_be16(hdr
, ARP_PTYPE
, ETH_P_IP
);
126 proto_field_set_default_u8(hdr
, ARP_HLEN
, 6);
127 proto_field_set_default_u8(hdr
, ARP_PLEN
, 4);
128 proto_field_set_default_be16(hdr
, ARP_OPER
, ARPOP_REQUEST
);
129 proto_field_set_default_dev_mac(hdr
, ARP_SHA
);
130 proto_field_set_default_dev_ipv4(hdr
, ARP_SPA
);
131 proto_field_set_default_dev_ipv4(hdr
, ARP_TPA
);
134 static struct proto_hdr arp_hdr
= {
137 .header_init
= arp_header_init
,
140 void protos_l2_init(void)
142 proto_header_register(ð_hdr
);
143 proto_header_register(&vlan_hdr
);
144 proto_header_register(&arp_hdr
);