1 /*
2 * Copyright 1998 Massachusetts Institute of Technology
3 *
4 * Permission to use, copy, modify, and distribute this software and
5 * its documentation for any purpose and without fee is hereby
6 * granted, provided that both the above copyright notice and this
7 * permission notice appear in all copies, that both the above
8 * copyright notice and this permission notice appear in all
9 * supporting documentation, and that the name of M.I.T. not be used
10 * in advertising or publicity pertaining to distribution of the
11 * software without specific, written prior permission. M.I.T. makes
12 * no representations about the suitability of this software for any
13 * purpose. It is provided "as is" without express or implied
14 * warranty.
15 *
16 * THIS SOFTWARE IS PROVIDED BY M.I.T. ``AS IS''. M.I.T. DISCLAIMS
17 * ALL EXPRESS OR IMPLIED WARRANTIES WITH REGARD TO THIS SOFTWARE,
18 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
19 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT
20 * SHALL M.I.T. BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
23 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
25 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
26 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27 * SUCH DAMAGE.
28 *
29 * $FreeBSD: src/sys/net/if_vlan.c,v 1.15.2.13 2003/02/14 22:25:58 fenner Exp $
30 */
31
32 #include <sys/param.h>
33 #include <sys/malloc.h> /* for M_NOWAIT */
34 #include <sys/mbuf.h>
35 #include <sys/serialize.h>
36
37 #include <net/bpf.h>
38 #include <net/ethernet.h>
39 #include <net/if.h>
40 #include <net/if_arp.h>
41 #include <net/ifq_var.h>
42 #include <net/netmsg.h>
43
44 #include <net/vlan/if_vlan_var.h>
45 #include <net/vlan/if_vlan_ether.h>
46
47 void
vlan_start_dispatch(netmsg_t msg)48 vlan_start_dispatch(netmsg_t msg)
49 {
50 struct netmsg_packet *nmp = &msg->packet;
51 struct mbuf *m;
52 struct ifnet *ifp;
53 struct altq_pktattr pktattr;
54 #ifdef DEBUG
55 char hexstr[HEX_NCPYLEN(sizeof(struct ether_vlan_header))];
56 #endif
57 m = nmp->nm_packet;
58 ifp = msg->lmsg.u.ms_resultp;
59
60 M_ASSERTPKTHDR(m);
61 KASSERT(m->m_flags & M_VLANTAG, ("mbuf has not been vlan tagged!"));
62
63 /*
64 * If ALTQ is enabled on the parent interface, do
65 * classification; the queueing discipline might
66 * not require classification, but might require
67 * the address family/header pointer in the pktattr.
68 */
69 if (ifq_is_enabled(&ifp->if_snd))
70 altq_etherclassify(&ifp->if_snd, m, &pktattr);
71
72 /*
73 * If underlying interface can do VLAN tag insertion itself,
74 * just pass the packet along.
75 */
76 if ((ifp->if_capenable & IFCAP_VLAN_HWTAGGING) == 0) {
77 uint16_t vlantag = m->m_pkthdr.ether_vlantag;
78 struct ether_vlan_header *evl;
79
80 M_PREPEND(m, EVL_ENCAPLEN, M_NOWAIT);
81 if (m == NULL) {
82 if_printf(ifp, "vlan%u M_PREPEND failed", vlantag);
83 return;
84 }
85 /* M_PREPEND takes care of m_len, m_pkthdr.len for us */
86
87 m = m_pullup(m, ETHER_HDR_LEN + EVL_ENCAPLEN);
88 if (m == NULL) {
89 if_printf(ifp, "vlan%u m_pullup failed", vlantag);
90 return;
91 }
92 m->m_pkthdr.csum_lhlen = sizeof(struct ether_vlan_header);
93
94 /*
95 * Transform the Ethernet header into an Ethernet header
96 * with 802.1Q encapsulation.
97 */
98 bcopy(mtod(m, char *) + EVL_ENCAPLEN, mtod(m, char *),
99 sizeof(struct ether_header));
100 evl = mtod(m, struct ether_vlan_header *);
101 evl->evl_proto = evl->evl_encap_proto;
102 evl->evl_encap_proto = htons(ETHERTYPE_VLAN);
103 evl->evl_tag = htons(vlantag);
104 #ifdef DEBUG
105 kprintf("vlan_start: %s\n", hexncpy((u_char *)evl, sizeof(*evl),
106 hexstr, HEX_NCPYLEN(sizeof(*evl)), ":"));
107 #endif
108 /* Hardware does not need to setup vlan tagging */
109 m->m_flags &= ~M_VLANTAG;
110 }
111 ifq_dispatch(ifp, m, &pktattr);
112 }
113
114 void
vlan_ether_ptap(struct bpf_if * bp,struct mbuf * m,uint16_t vlantag)115 vlan_ether_ptap(struct bpf_if *bp, struct mbuf *m, uint16_t vlantag)
116 {
117 const struct ether_header *eh;
118 struct ether_vlan_header evh;
119
120 KASSERT(m->m_len >= ETHER_HDR_LEN,
121 ("ether header is not contiguous!"));
122
123 eh = mtod(m, const struct ether_header *);
124 m_adj(m, ETHER_HDR_LEN);
125
126 bcopy(eh, &evh, 2 * ETHER_ADDR_LEN);
127 evh.evl_encap_proto = htons(ETHERTYPE_VLAN);
128 evh.evl_tag = htons(vlantag);
129 evh.evl_proto = eh->ether_type;
130 bpf_ptap(bp, m, &evh, ETHER_HDR_LEN + EVL_ENCAPLEN);
131
132 /* XXX assumes data was left intact */
133 M_PREPEND(m, ETHER_HDR_LEN, M_WAITOK);
134 }
135
136 void
vlan_ether_decap(struct mbuf ** m0)137 vlan_ether_decap(struct mbuf **m0)
138 {
139 struct mbuf *m = *m0;
140 struct ether_vlan_header *evh;
141
142 KKASSERT((m->m_flags & M_VLANTAG) == 0);
143
144 if (m->m_len < sizeof(*evh)) {
145 /* Error in the caller */
146 m_freem(m);
147 *m0 = NULL;
148 return;
149 }
150 evh = mtod(m, struct ether_vlan_header *);
151
152 m->m_pkthdr.ether_vlantag = ntohs(evh->evl_tag);
153 m->m_flags |= M_VLANTAG;
154
155 bcopy((uint8_t *)evh, (uint8_t *)evh + EVL_ENCAPLEN,
156 2 * ETHER_ADDR_LEN);
157 m_adj(m, EVL_ENCAPLEN);
158 *m0 = m;
159 }
160