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