The diff inlined in this mail fixes a regression introduced by a commit that removed the VLAN tagging from vlan_start.
Since ether_addheader is only called once now (by the first and the inner most VLAN) we need to add tags there. Doing all the tagging in ether_addheader avoids all the ether_header popping and pushing that we used to do in vlan_start so we also gain a small performance improvement. I've added a new function called vlan_addheader that will do all VLAN tagging plus it will prepend the ethernet header: it will look all parent interfaces pointed by the VLAN softc (ifv_p) to decide how many VLANs will be prepended, then it will alocate the necessary memory and do all the VLAN tagging. (if there is too many VLANs in the stack, M_PREPEND will return NULL and ether_addheader will fail) To decide whether the parent interface is a VLAN or not I've used the if_output function to determine the interface type. Index: net/if_ethersubr.c =================================================================== RCS file: /cvs/src/sys/net/if_ethersubr.c,v retrieving revision 1.177 diff -u -p -r1.177 if_ethersubr.c --- net/if_ethersubr.c 6 Nov 2014 14:28:47 -0000 1.177 +++ net/if_ethersubr.c 18 Nov 2014 13:11:30 -0000 @@ -206,18 +206,10 @@ ether_addheader(struct mbuf **m, struct ((*m)->m_pkthdr.pf.prio << EVL_PRIO_BITS); /* don't return, need to add regular ethernet header */ } else { - struct ether_vlan_header *evh; - - M_PREPEND(*m, sizeof(*evh), M_DONTWAIT); - if (*m == NULL) + *m = vlan_addheader(*m, ifp, etype, esrc, edst); + if (m == NULL) return (-1); - evh = mtod(*m, struct ether_vlan_header *); - memcpy(evh->evl_dhost, edst, sizeof(evh->evl_dhost)); - memcpy(evh->evl_shost, esrc, sizeof(evh->evl_shost)); - evh->evl_proto = etype; - evh->evl_encap_proto = htons(ifv->ifv_type); - evh->evl_tag = htons(ifv->ifv_tag + - ((*m)->m_pkthdr.pf.prio << EVL_PRIO_BITS)); + (*m)->m_flags &= ~M_VLANTAG; return (0); } Index: net/if_vlan.c =================================================================== RCS file: /cvs/src/sys/net/if_vlan.c,v retrieving revision 1.109 diff -u -p -r1.109 if_vlan.c --- net/if_vlan.c 7 Oct 2014 11:16:23 -0000 1.109 +++ net/if_vlan.c 18 Nov 2014 13:11:30 -0000 @@ -788,3 +788,69 @@ vlan_ether_resetmulti(struct ifvlan *ifv (void)(*p->if_ioctl)(p, SIOCADDMULTI, (caddr_t)ifr); } } + +/* + * This procedures alocates the space required for ethernet header + + * all VLANs, it also fill the information of all inner VLANs and the + * packet ethernet header. + * + * When successful this function returns the new mbuf else NULL. + */ +struct mbuf * +vlan_addheader(struct mbuf *m, const struct ifnet *ifp0, uint16_t etype, + u_char *esrc, u_char *edst) +{ + struct ifvlan *ifv = ifp0->if_softc; + struct ether_vlan_header *evh; + struct ifnet *ifp; + int nvlan = 0, off; + struct vlan_shim { + uint16_t vs_proto; + uint16_t vs_tag; + } vs; + +#define ISVLAN(ifp) \ + ((ifp)->if_output == vlan_output) + + /* Find out how many VLANs we are going to prepend */ + while ((ifp = ifv->ifv_p) != NULL && ISVLAN(ifp)) { + ifv = ifp->if_softc; + nvlan++; + } +#undef ISVLAN + + /* Prepend VLANs + ethernet */ + M_PREPEND(m, (nvlan * sizeof(vs)) + sizeof(*evh), M_DONTWAIT); + if (m == NULL) + return (NULL); + + /* + * Calculate the offset of packet ethertype and copy: + * First VLAN + (VLAN header size * VLAN count) - ethertype size + */ + off = sizeof(*evh) + (nvlan * sizeof(vs)) - sizeof(etype); + m_copyback(m, off, sizeof(etype), &etype, M_NOWAIT); + + /* Fill inner VLAN information if any */ + ifv = ifp0->if_softc; + while (nvlan-- > 0) { + vs.vs_tag = htons((ifv->ifv_prio << EVL_PRIO_BITS) + + ifv->ifv_tag); + vs.vs_proto = htons(ifv->ifv_type); + + off -= sizeof(vs); + m_copyback(m, off, sizeof(vs), &vs, M_NOWAIT); + + ifp = ifv->ifv_p; + ifv = ifp->if_softc; + } + + evh = mtod(m, struct ether_vlan_header *); + memcpy(evh->evl_dhost, edst, sizeof(evh->evl_dhost)); + memcpy(evh->evl_shost, esrc, sizeof(evh->evl_shost)); + evh->evl_encap_proto = htons(ifv->ifv_type); + evh->evl_tag = htons(ifv->ifv_tag + + (m->m_pkthdr.pf.prio << EVL_PRIO_BITS)); + + return (m); +} Index: net/if_vlan_var.h =================================================================== RCS file: /cvs/src/sys/net/if_vlan_var.h,v retrieving revision 1.24 diff -u -p -r1.24 if_vlan_var.h --- net/if_vlan_var.h 24 Oct 2013 11:14:33 -0000 1.24 +++ net/if_vlan_var.h 18 Nov 2014 13:11:30 -0000 @@ -96,6 +96,8 @@ struct ifvlan { #define IFVF_PROMISC 0x01 extern int vlan_input(struct ether_header *eh, struct mbuf *m); +struct mbuf *vlan_addheader(struct mbuf *, const struct ifnet *, uint16_t, + u_char *, u_char *); #endif /* _KERNEL */ #endif /* _NET_IF_VLAN_VAR_H_ */