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