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
ng_vlan_getTableLength(const struct ng_parse_type * type,const u_char * start,const u_char * buf)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
m_chk(struct mbuf ** mp,int len)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
ng_vlan_constructor(node_p node)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
ng_vlan_newhook(node_p node,hook_p hook,const char * name)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
ng_vlan_rcvmsg(node_p node,item_p item,hook_p lasthook)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
ng_vlan_rcvdata(hook_p hook,item_p item)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
ng_vlan_shutdown(node_p node)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
ng_vlan_disconnect(hook_p hook)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