sys/netgraph/ng_vlan.c

   1 /*-
   2  * Copyright (c) 2003 IPNET Internet Communication Company
   3  * Copyright (c) 2011 - 2012 Rozhuk Ivan <rozhuk.im@gmail.com>
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  *
  27  * Author: Ruslan Ermilov <ru@FreeBSD.org>
  28  *
  29  * $FreeBSD$
  30  */
  31
  32 #include <sys/param.h>
  33 #include <sys/errno.h>
  34 #include <sys/kernel.h>
  35 #include <sys/malloc.h>
  36 #include <sys/mbuf.h>
  37 #include <sys/queue.h>
  38 #include <sys/socket.h>
  39 #include <sys/systm.h>
  40
  41 #include <net/ethernet.h>
  42 #include <net/if.h>
  43 #include <net/if_vlan_var.h>
  44
  45 #include <netgraph/ng_message.h>
  46 #include <netgraph/ng_parse.h>
  47 #include <netgraph/ng_vlan.h>
  48 #include <netgraph/netgraph.h>
  49
  50 struct ng_vlan_private {
  51         hook_p          downstream_hook;
  52         hook_p          nomatch_hook;
  53         uint32_t        decap_enable;
  54         uint32_t        encap_enable;
  55         uint16_t        encap_proto;
  56         hook_p          vlan_hook[(EVL_VLID_MASK + 1)];
  57 };
  58 typedef struct ng_vlan_private *priv_p;
  59
  60 #define ETHER_VLAN_HDR_LEN (ETHER_HDR_LEN + ETHER_VLAN_ENCAP_LEN)
  61 #define VLAN_TAG_MASK   0xFFFF
  62 #define HOOK_VLAN_TAG_SET_MASK ((uintptr_t)((~0) & ~(VLAN_TAG_MASK)))
  63 #define IS_HOOK_VLAN_SET(hdata) \
  64             ((((uintptr_t)hdata) & HOOK_VLAN_TAG_SET_MASK) == HOOK_VLAN_TAG_SET_MASK)
  65
  66 static ng_constructor_t ng_vlan_constructor;
  67 static ng_rcvmsg_t      ng_vlan_rcvmsg;
  68 static ng_shutdown_t    ng_vlan_shutdown;
  69 static ng_newhook_t     ng_vlan_newhook;
  70 static ng_rcvdata_t     ng_vlan_rcvdata;
  71 static ng_disconnect_t  ng_vlan_disconnect;
  72
  73 /* Parse type for struct ng_vlan_filter. */
  74 static const struct ng_parse_struct_field ng_vlan_filter_fields[] =
  75         NG_VLAN_FILTER_FIELDS;
  76 static const struct ng_parse_type ng_vlan_filter_type = {
  77         &ng_parse_struct_type,
  78         &ng_vlan_filter_fields
  79 };
  80
  81 static int
  82 ng_vlan_getTableLength(const struct ng_parse_type *type,
  83     const u_char *start, const u_char *buf)
  84 {
  85         const struct ng_vlan_table *const table =
  86             (const struct ng_vlan_table *)(buf - sizeof(u_int32_t));
  87
  88         return table->n;
  89 }
  90
  91 /* Parse type for struct ng_vlan_table. */
  92 static const struct ng_parse_array_info ng_vlan_table_array_info = {
  93         &ng_vlan_filter_type,
  94         ng_vlan_getTableLength
  95 };
  96 static const struct ng_parse_type ng_vlan_table_array_type = {
  97         &ng_parse_array_type,
  98         &ng_vlan_table_array_info
  99 };
 100 static const struct ng_parse_struct_field ng_vlan_table_fields[] =
 101         NG_VLAN_TABLE_FIELDS;
 102 static const struct ng_parse_type ng_vlan_table_type = {
 103         &ng_parse_struct_type,
 104         &ng_vlan_table_fields
 105 };
 106
 107 /* List of commands and how to convert arguments to/from ASCII. */
 108 static const struct ng_cmdlist ng_vlan_cmdlist[] = {
 109         {
 110           NGM_VLAN_COOKIE,
 111           NGM_VLAN_ADD_FILTER,
 112           "addfilter",
 113           &ng_vlan_filter_type,
 114           NULL
 115         },
 116         {
 117           NGM_VLAN_COOKIE,
 118           NGM_VLAN_DEL_FILTER,
 119           "delfilter",
 120           &ng_parse_hookbuf_type,
 121           NULL
 122         },
 123         {
 124           NGM_VLAN_COOKIE,
 125           NGM_VLAN_GET_TABLE,
 126           "gettable",
 127           NULL,
 128           &ng_vlan_table_type
 129         },
 130         {
 131           NGM_VLAN_COOKIE,
 132           NGM_VLAN_DEL_VID_FLT,
 133           "delvidflt",
 134           &ng_parse_uint16_type,
 135           NULL
 136         },
 137         {
 138           NGM_VLAN_COOKIE,
 139           NGM_VLAN_GET_DECAP,
 140           "getdecap",
 141           NULL,
 142           &ng_parse_hint32_type
 143         },
 144         {
 145           NGM_VLAN_COOKIE,
 146           NGM_VLAN_SET_DECAP,
 147           "setdecap",
 148           &ng_parse_hint32_type,
 149           NULL
 150         },
 151         {
 152           NGM_VLAN_COOKIE,
 153           NGM_VLAN_GET_ENCAP,
 154           "getencap",
 155           NULL,
 156           &ng_parse_hint32_type
 157         },
 158         {
 159           NGM_VLAN_COOKIE,
 160           NGM_VLAN_SET_ENCAP,
 161           "setencap",
 162           &ng_parse_hint32_type,
 163           NULL
 164         },
 165         {
 166           NGM_VLAN_COOKIE,
 167           NGM_VLAN_GET_ENCAP_PROTO,
 168           "getencapproto",
 169           NULL,
 170           &ng_parse_hint16_type
 171         },
 172         {
 173           NGM_VLAN_COOKIE,
 174           NGM_VLAN_SET_ENCAP_PROTO,
 175           "setencapproto",
 176           &ng_parse_hint16_type,
 177           NULL
 178         },
 179         { 0 }
 180 };
 181
 182 static struct ng_type ng_vlan_typestruct = {
 183         .version =      NG_ABI_VERSION,
 184         .name =         NG_VLAN_NODE_TYPE,
 185         .constructor =  ng_vlan_constructor,
 186         .rcvmsg =       ng_vlan_rcvmsg,
 187         .shutdown =     ng_vlan_shutdown,
 188         .newhook =      ng_vlan_newhook,
 189         .rcvdata =      ng_vlan_rcvdata,
 190         .disconnect =   ng_vlan_disconnect,
 191         .cmdlist =      ng_vlan_cmdlist,
 192 };
 193 NETGRAPH_INIT(vlan, &ng_vlan_typestruct);
 194
 195
 196 /*
 197  * Helper functions.
 198  */
 199
 200 static __inline int
 201 m_chk(struct mbuf **mp, int len)
 202 {
 203
 204         if ((*mp)->m_pkthdr.len < len) {
 205                 m_freem((*mp));
 206                 (*mp) = NULL;
 207                 return (EINVAL);
 208         }
 209         if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
 210                 return (ENOBUFS);
 211
 212         return (0);
 213 }
 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 responce. */
 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 responce. */
 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
 526         NGI_GET_M(item, m);
 527
 528         /* Make sure we have an entire header. */
 529         error = m_chk(&m, ETHER_HDR_LEN);
 530         if (error != 0)
 531                 goto mchk_err;
 532
 533         eh = mtod(m, struct ether_header *);
 534         if (hook == priv->downstream_hook) {
 535                 /*
 536                  * If from downstream, select between a match hook
 537                  * or the nomatch hook.
 538                  */
 539
 540                 dst_hook = priv->nomatch_hook;
 541
 542                 /* Skip packets without tag. */
 543                 if ((m->m_flags & M_VLANTAG) == 0 &&
 544                     eh->ether_type != priv->encap_proto) {
 545                         if (dst_hook == NULL)
 546                                 goto net_down;
 547                         goto send_packet;
 548                 }
 549
 550                 /* Process packets with tag. */
 551                 if (m->m_flags & M_VLANTAG) {
 552                         /*
 553                          * Packet is tagged, m contains a normal
 554                          * Ethernet frame; tag is stored out-of-band.
 555                          */
 556                         evl = NULL;
 557                         vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
 558                 } else { /* eh->ether_type == priv->encap_proto */
 559                         error = m_chk(&m, ETHER_VLAN_HDR_LEN);
 560                         if (error != 0)
 561                                 goto mchk_err;
 562                         evl = mtod(m, struct ether_vlan_header *);
 563                         vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
 564                 }
 565
 566                 if (priv->vlan_hook[vid] != NULL) {
 567                         /*
 568                          * VLAN filter: allways remove vlan tags and
 569                          * decapsulate packet.
 570                          */
 571                         dst_hook = priv->vlan_hook[vid];
 572                         if (evl == NULL) { /* m->m_flags & M_VLANTAG */
 573                                 m->m_pkthdr.ether_vtag = 0;
 574                                 m->m_flags &= ~M_VLANTAG;
 575                                 goto send_packet;
 576                         }
 577                 } else { /* nomatch_hook */
 578                         if (dst_hook == NULL)
 579                                 goto net_down;
 580                         if (evl == NULL || priv->decap_enable == 0)
 581                                 goto send_packet;
 582                         /* Save tag out-of-band. */
 583                         m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
 584                         m->m_flags |= M_VLANTAG;
 585                 }
 586
 587                 /*
 588                  * Decapsulate:
 589                  * TPID = ether type encap
 590                  * Move DstMAC and SrcMAC to ETHER_TYPE.
 591                  * Before:
 592                  *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
 593                  *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
 594                  * After:
 595                  *  [free space ] [dmac] [smac] [ether_type] [payload]
 596                  *                |-----------| |--------------------|
 597                  */
 598                 bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
 599                     (ETHER_ADDR_LEN * 2));
 600                 m_adj(m, ETHER_VLAN_ENCAP_LEN);
 601         } else {
 602                 /*
 603                  * It is heading towards the downstream.
 604                  * If from nomatch, pass it unmodified.
 605                  * Otherwise, do the VLAN encapsulation.
 606                  */
 607                 dst_hook = priv->downstream_hook;
 608                 if (dst_hook == NULL)
 609                         goto net_down;
 610                 if (hook != priv->nomatch_hook) {/* Filter hook. */
 611                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 612                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 613                                 /*
 614                                  * Packet from hook not in filter
 615                                  * call addfilter for this hook to fix.
 616                                  */
 617                                 error = EOPNOTSUPP;
 618                                 goto drop;
 619                         }
 620                         eth_vtag = (hook_data & VLAN_TAG_MASK);
 621                         if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
 622                                 /* Just set packet header tag and send. */
 623                                 m->m_flags |= M_VLANTAG;
 624                                 m->m_pkthdr.ether_vtag = eth_vtag;
 625                                 goto send_packet;
 626                         }
 627                 } else { /* nomatch_hook */
 628                         if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
 629                             (m->m_flags & M_VLANTAG) == 0)
 630                                 goto send_packet;
 631                         /* Encapsulate tagged packet. */
 632                         eth_vtag = m->m_pkthdr.ether_vtag;
 633                         m->m_pkthdr.ether_vtag = 0;
 634                         m->m_flags &= ~M_VLANTAG;
 635                 }
 636
 637                 /*
 638                  * Transform the Ethernet header into an Ethernet header
 639                  * with 802.1Q encapsulation.
 640                  * Mod of: ether_vlanencap.
 641                  *
 642                  * TPID = ether type encap
 643                  * Move DstMAC and SrcMAC from ETHER_TYPE.
 644                  * Before:
 645                  *  [free space ] [dmac] [smac] [ether_type] [payload]
 646                  *  <<<<<<<<<<<<< |-----------| |--------------------|
 647                  * After:
 648                  *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
 649                  *  |-----------| |-- inserted tag --| |--------------------|
 650                  */
 651                 M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_NOWAIT);
 652                 if (m == NULL)
 653                         error = ENOMEM;
 654                 else
 655                         error = m_chk(&m, ETHER_VLAN_HDR_LEN);
 656                 if (error != 0)
 657                         goto mchk_err;
 658
 659                 evl = mtod(m, struct ether_vlan_header *);
 660                 bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
 661                     (char *)evl, (ETHER_ADDR_LEN * 2));
 662                 evl->evl_encap_proto = priv->encap_proto;
 663                 evl->evl_tag = htons(eth_vtag);
 664         }
 665
 666 send_packet:
 667         NG_FWD_NEW_DATA(error, item, dst_hook, m);
 668         return (error);
 669 net_down:
 670         error = ENETDOWN;
 671 drop:
 672         m_freem(m);
 673 mchk_err:
 674         NG_FREE_ITEM(item);
 675         return (error);
 676 }
 677
 678 static int
 679 ng_vlan_shutdown(node_p node)
 680 {
 681         const priv_p priv = NG_NODE_PRIVATE(node);
 682
 683         NG_NODE_SET_PRIVATE(node, NULL);
 684         NG_NODE_UNREF(node);
 685         free(priv, M_NETGRAPH);
 686         return (0);
 687 }
 688
 689 static int
 690 ng_vlan_disconnect(hook_p hook)
 691 {
 692         const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 693         uintptr_t hook_data;
 694
 695         if (hook == priv->downstream_hook)
 696                 priv->downstream_hook = NULL;
 697         else if (hook == priv->nomatch_hook)
 698                 priv->nomatch_hook = NULL;
 699         else {
 700                 /* Purge a rule that refers to this hook. */
 701                 hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 702                 if (IS_HOOK_VLAN_SET(hook_data))
 703                         priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 704         }
 705         NG_HOOK_SET_PRIVATE(hook, NULL);
 706         if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
 707             (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
 708                 ng_rmnode_self(NG_HOOK_NODE(hook));
 709         return (0);
 710 }