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 * $DragonFly: src/sys/net/vlan/if_vlan_ether.c,v 1.3 2008/05/16 13:19:12 sephe Exp $ 31 */ 32 33 #include <sys/param.h> 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 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 55 m = nmp->nm_packet; 56 ifp = msg->lmsg.u.ms_resultp; 57 58 M_ASSERTPKTHDR(m); 59 KASSERT(m->m_flags & M_VLANTAG, ("mbuf has not been vlan tagged!\n")); 60 61 ifnet_serialize_tx(ifp); 62 63 /* 64 * Make sure that the interface is still UP and RUNNING, 65 * since interface state may have been changed when the 66 * mbuf is pending on the msgport. 67 */ 68 if ((ifp->if_flags & (IFF_UP | IFF_RUNNING)) != 69 (IFF_UP | IFF_RUNNING)) { 70 m_freem(m); 71 goto back; 72 } 73 74 /* 75 * If ALTQ is enabled on the parent interface, do 76 * classification; the queueing discipline might 77 * not require classification, but might require 78 * the address family/header pointer in the pktattr. 79 */ 80 if (ifq_is_enabled(&ifp->if_snd)) 81 altq_etherclassify(&ifp->if_snd, m, &pktattr); 82 83 /* 84 * If underlying interface can do VLAN tag insertion itself, 85 * just pass the packet along. 86 */ 87 if ((ifp->if_capenable & IFCAP_VLAN_HWTAGGING) == 0) { 88 uint16_t vlantag = m->m_pkthdr.ether_vlantag; 89 struct ether_vlan_header *evl; 90 91 M_PREPEND(m, EVL_ENCAPLEN, MB_DONTWAIT); 92 if (m == NULL) { 93 if_printf(ifp, "vlan%u M_PREPEND failed", vlantag); 94 goto back; 95 } 96 /* M_PREPEND takes care of m_len, m_pkthdr.len for us */ 97 98 m = m_pullup(m, ETHER_HDR_LEN + EVL_ENCAPLEN); 99 if (m == NULL) { 100 if_printf(ifp, "vlan%u m_pullup failed", vlantag); 101 goto back; 102 } 103 104 /* 105 * Transform the Ethernet header into an Ethernet header 106 * with 802.1Q encapsulation. 107 */ 108 bcopy(mtod(m, char *) + EVL_ENCAPLEN, mtod(m, char *), 109 sizeof(struct ether_header)); 110 evl = mtod(m, struct ether_vlan_header *); 111 evl->evl_proto = evl->evl_encap_proto; 112 evl->evl_encap_proto = htons(ETHERTYPE_VLAN); 113 evl->evl_tag = htons(vlantag); 114 #ifdef DEBUG 115 kprintf("vlan_start: %*D\n", sizeof *evl, 116 (unsigned char *)evl, ":"); 117 #endif 118 /* Hardware does not need to setup vlan tagging */ 119 m->m_flags &= ~M_VLANTAG; 120 } 121 ifq_handoff(ifp, m, &pktattr); 122 back: 123 ifnet_deserialize_tx(ifp); 124 } 125 126 void 127 vlan_ether_ptap(struct bpf_if *bp, struct mbuf *m, uint16_t vlantag) 128 { 129 const struct ether_header *eh; 130 struct ether_vlan_header evh; 131 132 KASSERT(m->m_len >= ETHER_HDR_LEN, 133 ("ether header is not contiguous!\n")); 134 135 eh = mtod(m, const struct ether_header *); 136 m_adj(m, ETHER_HDR_LEN); 137 138 bcopy(eh, &evh, 2 * ETHER_ADDR_LEN); 139 evh.evl_encap_proto = htons(ETHERTYPE_VLAN); 140 evh.evl_tag = htons(vlantag); 141 evh.evl_proto = eh->ether_type; 142 bpf_ptap(bp, m, &evh, ETHER_HDR_LEN + EVL_ENCAPLEN); 143 144 /* XXX assumes data was left intact */ 145 M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT); 146 } 147 148 void 149 vlan_ether_decap(struct mbuf **m0) 150 { 151 struct mbuf *m = *m0; 152 struct ether_vlan_header *evh; 153 154 KKASSERT((m->m_flags & M_VLANTAG) == 0); 155 156 if (m->m_len < sizeof(*evh)) { 157 /* Error in the caller */ 158 m_freem(m); 159 *m0 = NULL; 160 return; 161 } 162 evh = mtod(m, struct ether_vlan_header *); 163 164 m->m_pkthdr.ether_vlantag = ntohs(evh->evl_tag); 165 m->m_flags |= M_VLANTAG; 166 167 bcopy((uint8_t *)evh, (uint8_t *)evh + EVL_ENCAPLEN, 168 2 * ETHER_ADDR_LEN); 169 m_adj(m, EVL_ENCAPLEN); 170 *m0 = m; 171 } 172