sys/netgraph/ng_vlan.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause
   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
  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  * Helper functions.
 197  */
 198
 199 static __inline int
 200 m_chk(struct mbuf **mp, int len)
 201 {
 202
 203         if ((*mp)->m_pkthdr.len < len) {
 204                 m_freem((*mp));
 205                 (*mp) = NULL;
 206                 return (EINVAL);
 207         }
 208         if ((*mp)->m_len < len && ((*mp) = m_pullup((*mp), len)) == NULL)
 209                 return (ENOBUFS);
 210
 211         return (0);
 212 }
 213
 214 /*
 215  * Netgraph node functions.
 216  */
 217
 218 static int
 219 ng_vlan_constructor(node_p node)
 220 {
 221         priv_p priv;
 222
 223         priv = malloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_ZERO);
 224         priv->decap_enable = 0;
 225         priv->encap_enable = VLAN_ENCAP_FROM_FILTER;
 226         priv->encap_proto = htons(ETHERTYPE_VLAN);
 227         NG_NODE_SET_PRIVATE(node, priv);
 228         return (0);
 229 }
 230
 231 static int
 232 ng_vlan_newhook(node_p node, hook_p hook, const char *name)
 233 {
 234         const priv_p priv = NG_NODE_PRIVATE(node);
 235
 236         if (strcmp(name, NG_VLAN_HOOK_DOWNSTREAM) == 0)
 237                 priv->downstream_hook = hook;
 238         else if (strcmp(name, NG_VLAN_HOOK_NOMATCH) == 0)
 239                 priv->nomatch_hook = hook;
 240         else {
 241                 /*
 242                  * Any other hook name is valid and can
 243                  * later be associated with a filter rule.
 244                  */
 245         }
 246         NG_HOOK_SET_PRIVATE(hook, NULL);
 247         return (0);
 248 }
 249
 250 static int
 251 ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
 252 {
 253         const priv_p priv = NG_NODE_PRIVATE(node);
 254         struct ng_mesg *msg, *resp = NULL;
 255         struct ng_vlan_filter *vf;
 256         hook_p hook;
 257         struct ng_vlan_table *t;
 258         uintptr_t hook_data;
 259         int i, vlan_count;
 260         uint16_t vid;
 261         int error = 0;
 262
 263         NGI_GET_MSG(item, msg);
 264         /* Deal with message according to cookie and command. */
 265         switch (msg->header.typecookie) {
 266         case NGM_VLAN_COOKIE:
 267                 switch (msg->header.cmd) {
 268                 case NGM_VLAN_ADD_FILTER:
 269                         /* Check that message is long enough. */
 270                         if (msg->header.arglen != sizeof(*vf)) {
 271                                 error = EINVAL;
 272                                 break;
 273                         }
 274                         vf = (struct ng_vlan_filter *)msg->data;
 275                         /* Sanity check the VLAN ID value. */
 276 #ifdef  NG_VLAN_USE_OLD_VLAN_NAME
 277                         if (vf->vid == 0 && vf->vid != vf->vlan) {
 278                                 vf->vid = vf->vlan;
 279                         } else if (vf->vid != 0 && vf->vlan != 0 &&
 280                             vf->vid != vf->vlan) {
 281                                 error = EINVAL;
 282                                 break;
 283                         }
 284 #endif
 285                         if (vf->vid & ~EVL_VLID_MASK ||
 286                             vf->pcp & ~7 ||
 287                             vf->cfi & ~1) {
 288                                 error = EINVAL;
 289                                 break;
 290                         }
 291                         /* Check that a referenced hook exists. */
 292                         hook = ng_findhook(node, vf->hook_name);
 293                         if (hook == NULL) {
 294                                 error = ENOENT;
 295                                 break;
 296                         }
 297                         /* And is not one of the special hooks. */
 298                         if (hook == priv->downstream_hook ||
 299                             hook == priv->nomatch_hook) {
 300                                 error = EINVAL;
 301                                 break;
 302                         }
 303                         /* And is not already in service. */
 304                         if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
 305                                 error = EEXIST;
 306                                 break;
 307                         }
 308                         /* Check we don't already trap this VLAN. */
 309                         if (priv->vlan_hook[vf->vid] != NULL) {
 310                                 error = EEXIST;
 311                                 break;
 312                         }
 313                         /* Link vlan and hook together. */
 314                         NG_HOOK_SET_PRIVATE(hook,
 315                             (void *)(HOOK_VLAN_TAG_SET_MASK |
 316                             EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
 317                         priv->vlan_hook[vf->vid] = hook;
 318                         break;
 319                 case NGM_VLAN_DEL_FILTER:
 320                         /* Check that message is long enough. */
 321                         if (msg->header.arglen != NG_HOOKSIZ) {
 322                                 error = EINVAL;
 323                                 break;
 324                         }
 325                         /* Check that hook exists and is active. */
 326                         hook = ng_findhook(node, (char *)msg->data);
 327                         if (hook == NULL) {
 328                                 error = ENOENT;
 329                                 break;
 330                         }
 331                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 332                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 333                                 error = ENOENT;
 334                                 break;
 335                         }
 336
 337                         KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
 338                             ("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
 339                             __func__, (char *)msg->data));
 340
 341                         /* Purge a rule that refers to this hook. */
 342                         priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 343                         NG_HOOK_SET_PRIVATE(hook, NULL);
 344                         break;
 345                 case NGM_VLAN_DEL_VID_FLT:
 346                         /* Check that message is long enough. */
 347                         if (msg->header.arglen != sizeof(uint16_t)) {
 348                                 error = EINVAL;
 349                                 break;
 350                         }
 351                         vid = (*((uint16_t *)msg->data));
 352                         /* Sanity check the VLAN ID value. */
 353                         if (vid & ~EVL_VLID_MASK) {
 354                                 error = EINVAL;
 355                                 break;
 356                         }
 357                         /* Check that hook exists and is active. */
 358                         hook = priv->vlan_hook[vid];
 359                         if (hook == NULL) {
 360                                 error = ENOENT;
 361                                 break;
 362                         }
 363                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 364                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 365                                 error = ENOENT;
 366                                 break;
 367                         }
 368
 369                         KASSERT(EVL_VLANOFTAG(hook_data) == vid,
 370                             ("%s: NGM_VLAN_DEL_VID_FLT:"
 371                             " Invalid VID Hook = %us, must be: %us\n",
 372                             __func__, (uint16_t )EVL_VLANOFTAG(hook_data),
 373                             vid));
 374
 375                         /* Purge a rule that refers to this hook. */
 376                         priv->vlan_hook[vid] = NULL;
 377                         NG_HOOK_SET_PRIVATE(hook, NULL);
 378                         break;
 379                 case NGM_VLAN_GET_TABLE:
 380                         /* Calculate vlans. */
 381                         vlan_count = 0;
 382                         for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
 383                                 if (priv->vlan_hook[i] != NULL &&
 384                                     NG_HOOK_IS_VALID(priv->vlan_hook[i]))
 385                                         vlan_count ++;
 386                         }
 387
 388                         /* Allocate memory for response. */
 389                         NG_MKRESPONSE(resp, msg, sizeof(*t) +
 390                             vlan_count * sizeof(*t->filter), M_NOWAIT);
 391                         if (resp == NULL) {
 392                                 error = ENOMEM;
 393                                 break;
 394                         }
 395
 396                         /* Pack data to response. */
 397                         t = (struct ng_vlan_table *)resp->data;
 398                         t->n = 0;
 399                         vf = &t->filter[0];
 400                         for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
 401                                 hook = priv->vlan_hook[i];
 402                                 if (hook == NULL || NG_HOOK_NOT_VALID(hook))
 403                                         continue;
 404                                 hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 405                                 if (IS_HOOK_VLAN_SET(hook_data) == 0)
 406                                         continue;
 407
 408                                 KASSERT(EVL_VLANOFTAG(hook_data) == i,
 409                                     ("%s: NGM_VLAN_GET_TABLE:"
 410                                     " hook %s VID = %us, must be: %i\n",
 411                                     __func__, NG_HOOK_NAME(hook),
 412                                     (uint16_t)EVL_VLANOFTAG(hook_data), i));
 413
 414 #ifdef  NG_VLAN_USE_OLD_VLAN_NAME
 415                                 vf->vlan = i;
 416 #endif
 417                                 vf->vid = i;
 418                                 vf->pcp = EVL_PRIOFTAG(hook_data);
 419                                 vf->cfi = EVL_CFIOFTAG(hook_data);
 420                                 strncpy(vf->hook_name,
 421                                     NG_HOOK_NAME(hook), NG_HOOKSIZ);
 422                                 vf ++;
 423                                 t->n ++;
 424                         }
 425                         break;
 426                 case NGM_VLAN_GET_DECAP:
 427                         NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
 428                         if (resp == NULL) {
 429                                 error = ENOMEM;
 430                                 break;
 431                         }
 432                         (*((uint32_t *)resp->data)) = priv->decap_enable;
 433                         break;
 434                 case NGM_VLAN_SET_DECAP:
 435                         if (msg->header.arglen != sizeof(uint32_t)) {
 436                                 error = EINVAL;
 437                                 break;
 438                         }
 439                         priv->decap_enable = (*((uint32_t *)msg->data));
 440                         break;
 441                 case NGM_VLAN_GET_ENCAP:
 442                         NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
 443                         if (resp == NULL) {
 444                                 error = ENOMEM;
 445                                 break;
 446                         }
 447                         (*((uint32_t *)resp->data)) = priv->encap_enable;
 448                         break;
 449                 case NGM_VLAN_SET_ENCAP:
 450                         if (msg->header.arglen != sizeof(uint32_t)) {
 451                                 error = EINVAL;
 452                                 break;
 453                         }
 454                         priv->encap_enable = (*((uint32_t *)msg->data));
 455                         break;
 456                 case NGM_VLAN_GET_ENCAP_PROTO:
 457                         NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
 458                         if (resp == NULL) {
 459                                 error = ENOMEM;
 460                                 break;
 461                         }
 462                         (*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
 463                         break;
 464                 case NGM_VLAN_SET_ENCAP_PROTO:
 465                         if (msg->header.arglen != sizeof(uint16_t)) {
 466                                 error = EINVAL;
 467                                 break;
 468                         }
 469                         priv->encap_proto = htons((*((uint16_t *)msg->data)));
 470                         break;
 471                 default: /* Unknown command. */
 472                         error = EINVAL;
 473                         break;
 474                 }
 475                 break;
 476         case NGM_FLOW_COOKIE:
 477             {
 478                 struct ng_mesg *copy;
 479
 480                 /*
 481                  * Flow control messages should come only
 482                  * from downstream.
 483                  */
 484
 485                 if (lasthook == NULL)
 486                         break;
 487                 if (lasthook != priv->downstream_hook)
 488                         break;
 489                 /* Broadcast the event to all uplinks. */
 490                 for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
 491                         if (priv->vlan_hook[i] == NULL)
 492                                 continue;
 493
 494                         NG_COPYMESSAGE(copy, msg, M_NOWAIT);
 495                         if (copy == NULL)
 496                                 continue;
 497                         NG_SEND_MSG_HOOK(error, node, copy,
 498                             priv->vlan_hook[i], 0);
 499                 }
 500                 break;
 501             }
 502         default: /* Unknown type cookie. */
 503                 error = EINVAL;
 504                 break;
 505         }
 506         NG_RESPOND_MSG(error, node, item, resp);
 507         NG_FREE_MSG(msg);
 508         return (error);
 509 }
 510
 511 static int
 512 ng_vlan_rcvdata(hook_p hook, item_p item)
 513 {
 514         const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 515         struct ether_header *eh;
 516         struct ether_vlan_header *evl;
 517         int error;
 518         uintptr_t hook_data;
 519         uint16_t vid, eth_vtag;
 520         struct mbuf *m;
 521         hook_p dst_hook;
 522
 523         NGI_GET_M(item, m);
 524
 525         /* Make sure we have an entire header. */
 526         error = m_chk(&m, ETHER_HDR_LEN);
 527         if (error != 0)
 528                 goto mchk_err;
 529
 530         eh = mtod(m, struct ether_header *);
 531         if (hook == priv->downstream_hook) {
 532                 /*
 533                  * If from downstream, select between a match hook
 534                  * or the nomatch hook.
 535                  */
 536
 537                 dst_hook = priv->nomatch_hook;
 538
 539                 /* Skip packets without tag. */
 540                 if ((m->m_flags & M_VLANTAG) == 0 &&
 541                     eh->ether_type != priv->encap_proto) {
 542                         if (dst_hook == NULL)
 543                                 goto net_down;
 544                         goto send_packet;
 545                 }
 546
 547                 /* Process packets with tag. */
 548                 if (m->m_flags & M_VLANTAG) {
 549                         /*
 550                          * Packet is tagged, m contains a normal
 551                          * Ethernet frame; tag is stored out-of-band.
 552                          */
 553                         evl = NULL;
 554                         vid = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
 555                 } else { /* eh->ether_type == priv->encap_proto */
 556                         error = m_chk(&m, ETHER_VLAN_HDR_LEN);
 557                         if (error != 0)
 558                                 goto mchk_err;
 559                         evl = mtod(m, struct ether_vlan_header *);
 560                         vid = EVL_VLANOFTAG(ntohs(evl->evl_tag));
 561                 }
 562
 563                 if (priv->vlan_hook[vid] != NULL) {
 564                         /*
 565                          * VLAN filter: always remove vlan tags and
 566                          * decapsulate packet.
 567                          */
 568                         dst_hook = priv->vlan_hook[vid];
 569                         if (evl == NULL) { /* m->m_flags & M_VLANTAG */
 570                                 m->m_pkthdr.ether_vtag = 0;
 571                                 m->m_flags &= ~M_VLANTAG;
 572                                 goto send_packet;
 573                         }
 574                 } else { /* nomatch_hook */
 575                         if (dst_hook == NULL)
 576                                 goto net_down;
 577                         if (evl == NULL || priv->decap_enable == 0)
 578                                 goto send_packet;
 579                         /* Save tag out-of-band. */
 580                         m->m_pkthdr.ether_vtag = ntohs(evl->evl_tag);
 581                         m->m_flags |= M_VLANTAG;
 582                 }
 583
 584                 /*
 585                  * Decapsulate:
 586                  * TPID = ether type encap
 587                  * Move DstMAC and SrcMAC to ETHER_TYPE.
 588                  * Before:
 589                  *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
 590                  *  |-----------| >>>>>>>>>>>>>>>>>>>> |--------------------|
 591                  * After:
 592                  *  [free space ] [dmac] [smac] [ether_type] [payload]
 593                  *                |-----------| |--------------------|
 594                  */
 595                 bcopy((char *)evl, ((char *)evl + ETHER_VLAN_ENCAP_LEN),
 596                     (ETHER_ADDR_LEN * 2));
 597                 m_adj(m, ETHER_VLAN_ENCAP_LEN);
 598         } else {
 599                 /*
 600                  * It is heading towards the downstream.
 601                  * If from nomatch, pass it unmodified.
 602                  * Otherwise, do the VLAN encapsulation.
 603                  */
 604                 dst_hook = priv->downstream_hook;
 605                 if (dst_hook == NULL)
 606                         goto net_down;
 607                 if (hook != priv->nomatch_hook) {/* Filter hook. */
 608                         hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 609                         if (IS_HOOK_VLAN_SET(hook_data) == 0) {
 610                                 /*
 611                                  * Packet from hook not in filter
 612                                  * call addfilter for this hook to fix.
 613                                  */
 614                                 error = EOPNOTSUPP;
 615                                 goto drop;
 616                         }
 617                         eth_vtag = (hook_data & VLAN_TAG_MASK);
 618                         if ((priv->encap_enable & VLAN_ENCAP_FROM_FILTER) == 0) {
 619                                 /* Just set packet header tag and send. */
 620                                 m->m_flags |= M_VLANTAG;
 621                                 m->m_pkthdr.ether_vtag = eth_vtag;
 622                                 goto send_packet;
 623                         }
 624                 } else { /* nomatch_hook */
 625                         if ((priv->encap_enable & VLAN_ENCAP_FROM_NOMATCH) == 0 ||
 626                             (m->m_flags & M_VLANTAG) == 0)
 627                                 goto send_packet;
 628                         /* Encapsulate tagged packet. */
 629                         eth_vtag = m->m_pkthdr.ether_vtag;
 630                         m->m_pkthdr.ether_vtag = 0;
 631                         m->m_flags &= ~M_VLANTAG;
 632                 }
 633
 634                 /*
 635                  * Transform the Ethernet header into an Ethernet header
 636                  * with 802.1Q encapsulation.
 637                  * Mod of: ether_vlanencap.
 638                  *
 639                  * TPID = ether type encap
 640                  * Move DstMAC and SrcMAC from ETHER_TYPE.
 641                  * Before:
 642                  *  [free space ] [dmac] [smac] [ether_type] [payload]
 643                  *  <<<<<<<<<<<<< |-----------| |--------------------|
 644                  * After:
 645                  *  [dmac] [smac] [TPID] [PCP/CFI/VID] [ether_type] [payload]
 646                  *  |-----------| |-- inserted tag --| |--------------------|
 647                  */
 648                 M_PREPEND(m, ETHER_VLAN_ENCAP_LEN, M_NOWAIT);
 649                 if (m == NULL)
 650                         error = ENOMEM;
 651                 else
 652                         error = m_chk(&m, ETHER_VLAN_HDR_LEN);
 653                 if (error != 0)
 654                         goto mchk_err;
 655
 656                 evl = mtod(m, struct ether_vlan_header *);
 657                 bcopy(((char *)evl + ETHER_VLAN_ENCAP_LEN),
 658                     (char *)evl, (ETHER_ADDR_LEN * 2));
 659                 evl->evl_encap_proto = priv->encap_proto;
 660                 evl->evl_tag = htons(eth_vtag);
 661         }
 662
 663 send_packet:
 664         NG_FWD_NEW_DATA(error, item, dst_hook, m);
 665         return (error);
 666 net_down:
 667         error = ENETDOWN;
 668 drop:
 669         m_freem(m);
 670 mchk_err:
 671         NG_FREE_ITEM(item);
 672         return (error);
 673 }
 674
 675 static int
 676 ng_vlan_shutdown(node_p node)
 677 {
 678         const priv_p priv = NG_NODE_PRIVATE(node);
 679
 680         NG_NODE_SET_PRIVATE(node, NULL);
 681         NG_NODE_UNREF(node);
 682         free(priv, M_NETGRAPH);
 683         return (0);
 684 }
 685
 686 static int
 687 ng_vlan_disconnect(hook_p hook)
 688 {
 689         const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 690         uintptr_t hook_data;
 691
 692         if (hook == priv->downstream_hook)
 693                 priv->downstream_hook = NULL;
 694         else if (hook == priv->nomatch_hook)
 695                 priv->nomatch_hook = NULL;
 696         else {
 697                 /* Purge a rule that refers to this hook. */
 698                 hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
 699                 if (IS_HOOK_VLAN_SET(hook_data))
 700                         priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
 701         }
 702         NG_HOOK_SET_PRIVATE(hook, NULL);
 703         if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
 704             (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
 705                 ng_rmnode_self(NG_HOOK_NODE(hook));
 706         return (0);
 707 }