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 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 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: always 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 } 711