sys/netgraph/ng_vlan.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 2003 IPNET Internet Communication Company
   5  * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>
   6  * All rights reserved.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  27  * SUCH DAMAGE.
  28  *
  29  * Author: Ruslan Ermilov <ru@FreeBSD.org>
  30  *
  31  * $FreeBSD$
  32  */
  33
  34 #include <sys/param.h>
  35 #include <sys/errno.h>
  36 #include <sys/kernel.h>
  37 #include <sys/malloc.h>
  38 #include <sys/mbuf.h>
  39 #include <sys/queue.h>
  40 #include <sys/socket.h>
  41 #include <sys/systm.h>
  42
  43 #include <net/ethernet.h>
  44 #include <net/if.h>
  45 #include <net/if_vlan_var.h>
  46
  47 #include <netgraph/ng_message.h>
  48 #include <netgraph/ng_parse.h>
  49 #include <netgraph/ng_vlan.h>
  50 #include <netgraph/netgraph.h>
  51
  52 struct ng_vlan_private {
  53         hook_p          downstream_hook;
  54         hook_p          nomatch_hook;
  55         uint32_t        decap_enable;
  56         uint32_t        encap_enable;
  57         uint16_t        encap_proto;
  58         hook_p          vlan_hook[(EVL_VLID_MASK + 1)];
  59 };
  60 typedef struct ng_vlan_private *priv_p;
  61
  62 #define ETHER_VLAN_HDR_LEN (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN)
  63 #define VLAN_TAG_MASK   0xFFFF
  64 #define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))
  65 #define IS_HOOK_VLAN_SET(hdata) \
  66             ((((uintptr_t)hdata) & HOOK_VLAN_TAG_SET_MASK) == HOOK_VLAN_TAG_SET_MASK)
  67
  68 static ng_constructor_t ng_vlan_constructor;
  69 static ng_rcvmsg_t      ng_vlan_rcvmsg;
  70 static ng_shutdown_t    ng_vlan_shutdown;
  71 static ng_newhook_t     ng_vlan_newhook;
  72 static ng_rcvdata_t     ng_vlan_rcvdata;
  73 static ng_disconnect_t  ng_vlan_disconnect;
  74
  75 /* Parse type for struct ng_vlan_filter. */
  76 static const struct ng_parse_struct_field ng_vlan_filter_fields[] =
  77         NG_VLAN_FILTER_FIELDS;
  78 static const struct ng_parse_type ng_vlan_filter_type = {
  79         &ng_parse_struct_type,
  80         &ng_vlan_filter_fields
  81 };
  82
  83 static int
  84 ng_vlan_getTableLength(const struct ng_parse_type *type,
  85     const u_char *start, const u_char *buf)
  86 {
  87         const struct ng_vlan_table *const table =
  88             (const struct ng_vlan_table *)(buf - sizeof(u_int32_t));
  89
  90         return table->n;
  91 }
  92
  93 /* Parse type for struct ng_vlan_table. */
  94 static const struct ng_parse_array_info ng_vlan_table_array_info = {
  95         &ng_vlan_filter_type,
  96         ng_vlan_getTableLength
  97 };
  98 static const struct ng_parse_type ng_vlan_table_array_type = {
  99         &ng_parse_array_type,
 100         &ng_vlan_table_array_info
 101 };
 102 static const struct ng_parse_struct_field ng_vlan_table_fields[] =
 103         NG_VLAN_TABLE_FIELDS;
 104 static const struct ng_parse_type ng_vlan_table_type = {
 105         &ng_parse_struct_type,
 106         &ng_vlan_table_fields
 107 };
 108
 109 /* List of commands and how to convert arguments to/from ASCII. */
 110 static const struct ng_cmdlist ng_vlan_cmdlist[] = {
 111         {
 112           NGM_VLAN_COOKIE,
 113           NGM_VLAN_ADD_FILTER,
 114           "addfilter",
 115           &ng_vlan_filter_type,
 116           NULL
 117         },
 118         {
 119           NGM_VLAN_COOKIE,
 120           NGM_VLAN_DEL_FILTER,
 121           "delfilter",
 122           &ng_parse_hookbuf_type,
 123           NULL
 124         },
 125         {
 126           NGM_VLAN_COOKIE,
 127           NGM_VLAN_GET_TABLE,
 128           "gettable",
 129           NULL,
 130           &ng_vlan_table_type
 131         },
 132         {
 133           NGM_VLAN_COOKIE,
 134           NGM_VLAN_DEL_VID_FLT,
 135           "delvidflt",
 136           &ng_parse_uint16_type,
 137           NULL
 138         },
 139         {
 140           NGM_VLAN_COOKIE,
 141           NGM_VLAN_GET_DECAP,
 142           "getdecap",
 143           NULL,
 144           &ng_parse_hint32_type
 145         },
 146         {
 147           NGM_VLAN_COOKIE,
 148           NGM_VLAN_SET_DECAP,
 149           "setdecap",
 150           &ng_parse_hint32_type,
 151           NULL
 152         },
 153         {
 154           NGM_VLAN_COOKIE,
 155           NGM_VLAN_GET_ENCAP,
 156           "getencap",
 157           NULL,
 158           &ng_parse_hint32_type
 159         },
 160         {
 161           NGM_VLAN_COOKIE,
 162           NGM_VLAN_SET_ENCAP,
 163           "setencap",
 164           &ng_parse_hint32_type,
 165           NULL
 166         },
 167         {
 168           NGM_VLAN_COOKIE,
 169           NGM_VLAN_GET_ENCAP_PROTO,
 170           "getencapproto",
 171           NULL,
 172           &ng_parse_hint16_type
 173         },
 174         {
 175           NGM_VLAN_COOKIE,
 176           NGM_VLAN_SET_ENCAP_PROTO,
 177           "setencapproto",
 178           &ng_parse_hint16_type,
 179           NULL
 180         },
 181         { 0 }
 182 };
 183
 184 static struct ng_type ng_vlan_typestruct = {
 185         .version =      NG_ABI_VERSION,
 186         .name =         NG_VLAN_NODE_TYPE,
 187         .constructor =  ng_vlan_constructor,
 188         .rcvmsg =       ng_vlan_rcvmsg,
 189         .shutdown =     ng_vlan_shutdown,
 190         .newhook =      ng_vlan_newhook,
 191         .rcvdata =      ng_vlan_rcvdata,
 192         .disconnect =   ng_vlan_disconnect,
 193         .cmdlist =      ng_vlan_cmdlist,
 194 };
 195 NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
 196
 197 /*
 198  * Helper functions.
 199  */
 200
 201 static __inline int
 202 m_chk(struct mbuf **mp, int len)
 203 {
 204
 205         if ((*mp)->m_pkthdr.len < len) {
 206                 m_freem((*mp));
 207                 (*mp) = NULL;
 208                 return (EINVAL);
 209         }
 210         if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
 211                 return (ENOBUFS);
 212
 213         return (0);
 214 }
 215
 216 /*
 217  * Netgraph node functions.
 218  */
 219
 220 static int
 221 ng_vlan_constructor(node_p node)
 222 {
 223         priv_p priv;
 224
 225         priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
 226         priv->decap_enable = 0;
 227         priv->encap_enable = VLAN_ENCAP_FROM_FILTER;
 228         priv->encap_proto = htons(ETHERTYPE_VLAN);
 229         NG_NODE_SET_PRIVATE(node, priv);
 230         return (0);
 231 }
 232
 233 static int
 234 ng_vlan_newhook(node_p node, hook_p hook, const char *name)
 235 {
 236         const priv_p priv = NG_NODE_PRIVATE(node);
 237
 238         if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
 239                 priv->downstream_hook = hook;
 240         else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
 241                 priv->nomatch_hook = hook;
 242         else {
 243                 /*
 244                  * Any other hook name is valid and can
 245                  * later be associated with a filter rule.
 246                  */
 247         }
 248         NG_HOOK_SET_PRIVATE(hook, NULL);
 249         return (0);
 250 }
 251
 252 static int
 253 ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 254 {
 255         const priv_p priv = NG_NODE_PRIVATE(node);
 256         struct ng_mesg *msg, *resp = NULL;
 257         struct ng_vlan_filter *vf;
 258         hook_p hook;
 259         struct ng_vlan_table *t;
 260         uintptr_t hook_data;
 261         int i, vlan_count;
 262         uint16_t vid;
 263         int error = 0;
 264
 265         NGI_GET_MSG(item, msg);
 266         /* Deal with message according to cookie and command. */
 267         switch (msg->header.typecookie) {
 268         case NGM_VLAN_COOKIE:
 269                 switch (msg->header.cmd) {
 270                 case NGM_VLAN_ADD_FILTER:
 271                         /* Check that message is long enough. */
 272                         if (msg->header.arglen != sizeof(*vf)) {
 273                                 error = EINVAL;
 274                                 break;
 275                         }
 276                         vf = (struct ng_vlan_filter *)msg->data;
 277                         /* Sanity check the VLAN ID value. */
 278 #ifdef  NG_VLAN_USE_OLD_VLAN_NAME
 279                         if (vf->vid == 0 && vf->vid != vf->vlan) {
 280                                 vf->vid = vf->vlan;
 281                         } else if (vf->vid != 0 && vf->vlan != 0 &&
 282                             vf->vid != vf->vlan) {
 283                                 error = EINVAL;
 284                                 break;
 285                         }
 286 #endif
 287                         if (vf->vid & ~EVL_VLID_MASK ||
 288                             vf->pcp & ~7 ||
 289                             vf->cfi & ~1) {
 290                                 error = EINVAL;
 291                                 break;
 292                         }
 293                         /* Check that a referenced hook exists. */
 294                         hook = ng_findhook(node, vf->hook_name);
 295                         if (hook == NULL) {
 296                                 error = ENOENT;
 297                                 break;
 298                         }
 299                         /* And is not one of the special hooks. */
 300                         if (hook == priv->downstream_hook ||
 301                             hook == priv->nomatch_hook) {
 302                                 error = EINVAL;
 303                                 break;
 304                         }
 305                         /* And is not already in service. */
 306                         if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
 307                                 error = EEXIST;
 308                                 break;
 309                         }
 310                         /* Check we don't already trap this VLAN. */
 311                         if (priv->vlan_hook[vf->vid] != NULL) {
 312                                 error = EEXIST;
 313                                 break;
 314                         }
 315                         /* Link vlan and hook together. */
 316                         NG_HOOK_SET_PRIVATE(hook,
 317                             (void *)(HOOK_VLAN_TAG_SET_MASK |
 318                             EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
 319                         priv->vlan_hook[vf->vid] = hook;
 320                         break;
 321                 case NGM_VLAN_DEL_FILTER:
 322                         /* Check that message is long enough. */
 323                         if (msg->header.arglen != NG_HOOKSIZ) {
 324                                 error = EINVAL;
 325                                 break;
 326                         }
 327                         /* Check that hook exists and is active. */
 328                         hook = ng_findhook(node, (char *)msg->data);
 329                         if (hook == NULL) {
 330                                 error = ENOENT;
 331                                 break;
 332                         }
 333                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 334                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 335                                 error = ENOENT;
 336                                 break;
 337                         }
 338
 339                         KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
 340                             ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
 341                             __func__, (char *)msg->data));
 342
 343                         /* Purge a rule that refers to this hook. */
 344                         priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 345                         NG_HOOK_SET_PRIVATE(hook, NULL);
 346                         break;
 347                 case NGM_VLAN_DEL_VID_FLT:
 348                         /* Check that message is long enough. */
 349                         if (msg->header.arglen != sizeof(uint16_t)) {
 350                                 error = EINVAL;
 351                                 break;
 352                         }
 353                         vid = (*((uint16_t *)msg->data));
 354                         /* Sanity check the VLAN ID value. */
 355                         if (vid & ~EVL_VLID_MASK) {
 356                                 error = EINVAL;
 357                                 break;
 358                         }
 359                         /* Check that hook exists and is active. */
 360                         hook = priv->vlan_hook[vid];
 361                         if (hook == NULL) {
 362                                 error = ENOENT;
 363                                 break;
 364                         }
 365                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 366                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 367                                 error = ENOENT;
 368                                 break;
 369                         }
 370
 371                         KASSERT(EVL_VLANOFTAG(hook_data) == vid,
 372                             ("%s: NGM_VLAN_DEL_VID_FLT:"
 373                             " Invalid VID Hook = %us, must be: %us\n",
 374                             __func__, (uint16_t )EVL_VLANOFTAG(hook_data),
 375                             vid));
 376
 377                         /* Purge a rule that refers to this hook. */
 378                         priv->vlan_hook[vid] = NULL;
 379                         NG_HOOK_SET_PRIVATE(hook, NULL);
 380                         break;
 381                 case NGM_VLAN_GET_TABLE:
 382                         /* Calculate vlans. */
 383                         vlan_count = 0;
 384                         for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
 385                                 if (priv->vlan_hook[i] != NULL &&
 386                                     NG_HOOK_IS_VALID(priv->vlan_hook[i]))
 387                                         vlan_count ++;
 388                         }
 389
 390                         /* Allocate memory for response. */
 391                         NG_MKRESPONSE(resp, msg, sizeof(*t) +
 392                             vlan_count * sizeof(*t->filter), M_NOWAIT);
 393                         if (resp == NULL) {
 394                                 error = ENOMEM;
 395                                 break;
 396                         }
 397
 398                         /* Pack data to response. */
 399                         t = (struct ng_vlan_table *)resp->data;
 400                         t->n = 0;
 401                         vf = &t->filter[0];
 402                         for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
 403                                 hook = priv->vlan_hook[i];
 404                                 if (hook == NULL || NG_HOOK_NOT_VALID(hook))
 405                                         continue;
 406                                 hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 407                                 if (IS_HOOK_VLAN_SET(hook_data) == 0)
 408                                         continue;
 409
 410                                 KASSERT(EVL_VLANOFTAG(hook_data) == i,
 411                                     ("%s: NGM_VLAN_GET_TABLE:"
 412                                     " hook %s VID = %us, must be: %i\n",
 413                                     __func__, NG_HOOK_NAME(hook),
 414                                     (uint16_t)EVL_VLANOFTAG(hook_data), i));
 415
 416 #ifdef  NG_VLAN_USE_OLD_VLAN_NAME
 417                                 vf->vlan = i;
 418 #endif
 419                                 vf->vid = i;
 420                                 vf->pcp = EVL_PRIOFTAG(hook_data);
 421                                 vf->cfi = EVL_CFIOFTAG(hook_data);
 422                                 strncpy(vf->hook_name,
 423                                     NG_HOOK_NAME(hook), NG_HOOKSIZ);
 424                                 vf ++;
 425                                 t->n ++;
 426                         }
 427                         break;
 428                 case NGM_VLAN_GET_DECAP:
 429                         NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
 430                         if (resp == NULL) {
 431                                 error = ENOMEM;
 432                                 break;
 433                         }
 434                         (*((uint32_t *)resp->data)) = priv->decap_enable;
 435                         break;
 436                 case NGM_VLAN_SET_DECAP:
 437                         if (msg->header.arglen != sizeof(uint32_t)) {
 438                                 error = EINVAL;
 439                                 break;
 440                         }
 441                         priv->decap_enable = (*((uint32_t *)msg->data));
 442                         break;
 443                 case NGM_VLAN_GET_ENCAP:
 444                         NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
 445                         if (resp == NULL) {
 446                                 error = ENOMEM;
 447                                 break;
 448                         }
 449                         (*((uint32_t *)resp->data)) = priv->encap_enable;
 450                         break;
 451                 case NGM_VLAN_SET_ENCAP:
 452                         if (msg->header.arglen != sizeof(uint32_t)) {
 453                                 error = EINVAL;
 454                                 break;
 455                         }
 456                         priv->encap_enable = (*((uint32_t *)msg->data));
 457                         break;
 458                 case NGM_VLAN_GET_ENCAP_PROTO:
 459                         NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
 460                         if (resp == NULL) {
 461                                 error = ENOMEM;
 462                                 break;
 463                         }
 464                         (*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
 465                         break;
 466                 case NGM_VLAN_SET_ENCAP_PROTO:
 467                         if (msg->header.arglen != sizeof(uint16_t)) {
 468                                 error = EINVAL;
 469                                 break;
 470                         }
 471                         priv->encap_proto = htons((*((uint16_t *)msg->data)));
 472                         break;
 473                 default: /* Unknown command. */
 474                         error = EINVAL;
 475                         break;
 476                 }
 477                 break;
 478         case NGM_FLOW_COOKIE:
 479             {
 480                 struct ng_mesg *copy;
 481
 482                 /*
 483                  * Flow control messages should come only
 484                  * from downstream.
 485                  */
 486
 487                 if (lasthook == NULL)
 488                         break;
 489                 if (lasthook != priv->downstream_hook)
 490                         break;
 491                 /* Broadcast the event to all uplinks. */
 492                 for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
 493                         if (priv->vlan_hook[i] == NULL)
 494                                 continue;
 495
 496                         NG_COPYMESSAGE(copy, msg, M_NOWAIT);
 497                         if (copy == NULL)
 498                                 continue;
 499                         NG_SEND_MSG_HOOK(error, node, copy,
 500                             priv->vlan_hook[i], 0);
 501                 }
 502                 break;
 503             }
 504         default: /* Unknown type cookie. */
 505                 error = EINVAL;
 506                 break;
 507         }
 508         NG_RESPOND_MSG(error, node, item, resp);
 509         NG_FREE_MSG(msg);
 510         return (error);
 511 }
 512
 513 static int
 514 ng_vlan_rcvdata(hook_p hook, item_p item)
 515 {
 516         const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 517         struct ether_header *eh;
 518         struct ether_vlan_header *evl;
 519         int error;
 520         uintptr_t hook_data;
 521         uint16_t vid, eth_vtag;
 522         struct mbuf *m;
 523         hook_p dst_hook;
 524
 525         NGI_GET_M(item, m);
 526
 527         /* Make sure we have an entire header. */
 528         error = m_chk(&m, ETHER_HDR_LEN);
 529         if (error != 0)
 530                 goto mchk_err;
 531
 532         eh = mtod(m, struct ether_header *);
 533         if (hook == priv->downstream_hook) {
 534                 /*
 535                  * If from downstream, select between a match hook
 536                  * or the nomatch hook.
 537                  */
 538
 539                 dst_hook = priv->nomatch_hook;
 540
 541                 /* Skip packets without tag. */
 542                 if ((m->m_flags & M_VLANTAG) == 0 &&
 543                     eh->ether_type != priv->encap_proto) {
 544                         if (dst_hook == NULL)
 545                                 goto net_down;
 546                         goto send_packet;
 547                 }
 548
 549                 /* Process packets with tag. */
 550                 if (m->m_flags & M_VLANTAG) {
 551                         /*
 552                          * Packet is tagged, m contains a normal
 553                          * Ethernet frame; tag is stored out-of-band.
 554                          */
 555                         evl = NULL;
 556                         vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
 557                 } else { /* eh->ether_type == priv->encap_proto */
 558                         error = m_chk(&m, ETHER_VLAN_HDR_LEN);
 559                         if (error != 0)
 560                                 goto mchk_err;
 561                         evl = mtod(m, struct ether_vlan_header *);
 562                         vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
 563                 }
 564
 565                 if (priv->vlan_hook[vid] != NULL) {
 566                         /*
 567                          * VLAN filter: always remove vlan tags and
 568                          * decapsulate packet.
 569                          */
 570                         dst_hook = priv->vlan_hook[vid];
 571                         if (evl == NULL) { /* m->m_flags & M_VLANTAG */
 572                                 m->m_pkthdr.ether_vtag = 0;
 573                                 m->m_flags &= ~M_VLANTAG;
 574                                 goto send_packet;
 575                         }
 576                 } else { /* nomatch_hook */
 577                         if (dst_hook == NULL)
 578                                 goto net_down;
 579                         if (evl == NULL || priv->decap_enable == 0)
 580                                 goto send_packet;
 581                         /* Save tag out-of-band. */
 582                         m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
 583                         m->m_flags |= M_VLANTAG;
 584                 }
 585
 586                 /*
 587                  * Decapsulate:
 588                  * TPID = ether type encap
 589                  * Move DstMAC and SrcMAC to ETHER_TYPE.
 590                  * Before:
 591                  *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
 592                  *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
 593                  * After:
 594                  *  [free space ] [dmac] [smac] [ether_type] [payload]
 595                  *                |-----------| |--------------------|
 596                  */
 597                 bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
 598                     (ETHER_ADDR_LEN * 2));
 599                 m_adj(m, ETHER_VLAN_ENCAP_LEN);
 600         } else {
 601                 /*
 602                  * It is heading towards the downstream.
 603                  * If from nomatch, pass it unmodified.
 604                  * Otherwise, do the VLAN encapsulation.
 605                  */
 606                 dst_hook = priv->downstream_hook;
 607                 if (dst_hook == NULL)
 608                         goto net_down;
 609                 if (hook != priv->nomatch_hook) {/* Filter hook. */
 610                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 611                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 612                                 /*
 613                                  * Packet from hook not in filter
 614                                  * call addfilter for this hook to fix.
 615                                  */
 616                                 error = EOPNOTSUPP;
 617                                 goto drop;
 618                         }
 619                         eth_vtag = (hook_data & VLAN_TAG_MASK);
 620                         if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
 621                                 /* Just set packet header tag and send. */
 622                                 m->m_flags |= M_VLANTAG;
 623                                 m->m_pkthdr.ether_vtag = eth_vtag;
 624                                 goto send_packet;
 625                         }
 626                 } else { /* nomatch_hook */
 627                         if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
 628                             (m->m_flags & M_VLANTAG) == 0)
 629                                 goto send_packet;
 630                         /* Encapsulate tagged packet. */
 631                         eth_vtag = m->m_pkthdr.ether_vtag;
 632                         m->m_pkthdr.ether_vtag = 0;
 633                         m->m_flags &= ~M_VLANTAG;
 634                 }
 635
 636                 /*
 637                  * Transform the Ethernet header into an Ethernet header
 638                  * with 802.1Q encapsulation.
 639                  * Mod of: ether_vlanencap.
 640                  *
 641                  * TPID = ether type encap
 642                  * Move DstMAC and SrcMAC from ETHER_TYPE.
 643                  * Before:
 644                  *  [free space ] [dmac] [smac] [ether_type] [payload]
 645                  *  <<<<<<<<<<<<< |-----------| |--------------------|
 646                  * After:
 647                  *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
 648                  *  |-----------| |-- inserted tag --| |--------------------|
 649                  */
 650                 M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_NOWAIT);
 651                 if (m == NULL)
 652                         error = ENOMEM;
 653                 else
 654                         error = m_chk(&m, ETHER_VLAN_HDR_LEN);
 655                 if (error != 0)
 656                         goto mchk_err;
 657
 658                 evl = mtod(m, struct ether_vlan_header *);
 659                 bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
 660                     (char *)evl, (ETHER_ADDR_LEN * 2));
 661                 evl->evl_encap_proto = priv->encap_proto;
 662                 evl->evl_tag = htons(eth_vtag);
 663         }
 664
 665 send_packet:
 666         NG_FWD_NEW_DATA(error, item, dst_hook, m);
 667         return (error);
 668 net_down:
 669         error = ENETDOWN;
 670 drop:
 671         m_freem(m);
 672 mchk_err:
 673         NG_FREE_ITEM(item);
 674         return (error);
 675 }
 676
 677 static int
 678 ng_vlan_shutdown(node_p node)
 679 {
 680         const priv_p priv = NG_NODE_PRIVATE(node);
 681
 682         NG_NODE_SET_PRIVATE(node, NULL);
 683         NG_NODE_UNREF(node);
 684         free(priv, M_NETGRAPH);
 685         return (0);
 686 }
 687
 688 static int
 689 ng_vlan_disconnect(hook_p hook)
 690 {
 691         const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 692         uintptr_t hook_data;
 693
 694         if (hook == priv->downstream_hook)
 695                 priv->downstream_hook = NULL;
 696         else if (hook == priv->nomatch_hook)
 697                 priv->nomatch_hook = NULL;
 698         else {
 699                 /* Purge a rule that refers to this hook. */
 700                 hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 701                 if (IS_HOOK_VLAN_SET(hook_data))
 702                         priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 703         }
 704         NG_HOOK_SET_PRIVATE(hook, NULL);
 705         if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
 706             (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
 707                 ng_rmnode_self(NG_HOOK_NODE(hook));
 708         return (0);
 709 }