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