sys/net/vlan/if_vlan_ether.c

   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
  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
 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
 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 }