1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2019-2021 IKS Service GmbH
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: Lutz Donnerhacke <lutz@donnerhacke.de>
28 */
29
30 #include <sys/param.h>
31 #include <sys/systm.h>
32 #include <sys/kernel.h>
33 #include <sys/mbuf.h>
34 #include <sys/malloc.h>
35 #include <sys/ctype.h>
36 #include <sys/errno.h>
37 #include <sys/syslog.h>
38 #include <sys/types.h>
39 #include <sys/counter.h>
40
41 #include <net/ethernet.h>
42
43 #include <netgraph/ng_message.h>
44 #include <netgraph/ng_parse.h>
45 #include <netgraph/ng_vlan_rotate.h>
46 #include <netgraph/netgraph.h>
47
48 /*
49 * This section contains the netgraph method declarations for the
50 * sample node. These methods define the netgraph 'type'.
51 */
52
53 static ng_constructor_t ng_vlanrotate_constructor;
54 static ng_rcvmsg_t ng_vlanrotate_rcvmsg;
55 static ng_shutdown_t ng_vlanrotate_shutdown;
56 static ng_newhook_t ng_vlanrotate_newhook;
57 static ng_rcvdata_t ng_vlanrotate_rcvdata;
58 static ng_disconnect_t ng_vlanrotate_disconnect;
59
60 /* Parse type for struct ng_vlanrotate_conf */
61 static const struct ng_parse_struct_field ng_vlanrotate_conf_fields[] = {
62 {"rot", &ng_parse_int8_type},
63 {"min", &ng_parse_uint8_type},
64 {"max", &ng_parse_uint8_type},
65 {NULL}
66 };
67 static const struct ng_parse_type ng_vlanrotate_conf_type = {
68 &ng_parse_struct_type,
69 &ng_vlanrotate_conf_fields
70 };
71
72 /* Parse type for struct ng_vlanrotate_stat */
73 static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = {
74 &ng_parse_uint64_type,
75 NG_VLANROTATE_MAX_VLANS
76 };
77 static struct ng_parse_type ng_vlanrotate_stat_hist = {
78 &ng_parse_fixedarray_type,
79 &ng_vlanrotate_stat_hist_info
80 };
81 static const struct ng_parse_struct_field ng_vlanrotate_stat_fields[] = {
82 {"drops", &ng_parse_uint64_type},
83 {"excessive", &ng_parse_uint64_type},
84 {"incomplete", &ng_parse_uint64_type},
85 {"histogram", &ng_vlanrotate_stat_hist},
86 {NULL}
87 };
88 static struct ng_parse_type ng_vlanrotate_stat_type = {
89 &ng_parse_struct_type,
90 &ng_vlanrotate_stat_fields
91 };
92
93
94 /* List of commands and how to convert arguments to/from ASCII */
95 static const struct ng_cmdlist ng_vlanrotate_cmdlist[] = {
96 {
97 NGM_VLANROTATE_COOKIE,
98 NGM_VLANROTATE_GET_CONF,
99 "getconf",
100 NULL,
101 &ng_vlanrotate_conf_type,
102 },
103 {
104 NGM_VLANROTATE_COOKIE,
105 NGM_VLANROTATE_SET_CONF,
106 "setconf",
107 &ng_vlanrotate_conf_type,
108 NULL
109 },
110 {
111 NGM_VLANROTATE_COOKIE,
112 NGM_VLANROTATE_GET_STAT,
113 "getstat",
114 NULL,
115 &ng_vlanrotate_stat_type
116 },
117 {
118 NGM_VLANROTATE_COOKIE,
119 NGM_VLANROTATE_CLR_STAT,
120 "clrstat",
121 NULL,
122 &ng_vlanrotate_stat_type
123 },
124 {
125 NGM_VLANROTATE_COOKIE,
126 NGM_VLANROTATE_GETCLR_STAT,
127 "getclrstat",
128 NULL,
129 &ng_vlanrotate_stat_type
130 },
131 {0}
132 };
133
134 /* Netgraph node type descriptor */
135 static struct ng_type typestruct = {
136 .version = NG_ABI_VERSION,
137 .name = NG_VLANROTATE_NODE_TYPE,
138 .constructor = ng_vlanrotate_constructor,
139 .rcvmsg = ng_vlanrotate_rcvmsg,
140 .shutdown = ng_vlanrotate_shutdown,
141 .newhook = ng_vlanrotate_newhook,
142 .rcvdata = ng_vlanrotate_rcvdata,
143 .disconnect = ng_vlanrotate_disconnect,
144 .cmdlist = ng_vlanrotate_cmdlist,
145 };
146 NETGRAPH_INIT(vlanrotate, &typestruct);
147
148 struct ng_vlanrotate_kernel_stats {
149 counter_u64_t drops, excessive, incomplete;
150 counter_u64_t histogram[NG_VLANROTATE_MAX_VLANS];
151 };
152
153 /* Information we store for each node */
154 struct vlanrotate {
155 hook_p original_hook;
156 hook_p ordered_hook;
157 hook_p excessive_hook;
158 hook_p incomplete_hook;
159 struct ng_vlanrotate_conf conf;
160 struct ng_vlanrotate_kernel_stats stats;
161 };
162 typedef struct vlanrotate *vlanrotate_p;
163
164 /*
165 * Set up the private data structure.
166 */
167 static int
ng_vlanrotate_constructor(node_p node)168 ng_vlanrotate_constructor(node_p node)
169 {
170 int i;
171
172 vlanrotate_p vrp = malloc(sizeof(*vrp), M_NETGRAPH, M_WAITOK | M_ZERO);
173
174 vrp->conf.max = NG_VLANROTATE_MAX_VLANS;
175
176 vrp->stats.drops = counter_u64_alloc(M_WAITOK);
177 vrp->stats.excessive = counter_u64_alloc(M_WAITOK);
178 vrp->stats.incomplete = counter_u64_alloc(M_WAITOK);
179 for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
180 vrp->stats.histogram[i] = counter_u64_alloc(M_WAITOK);
181
182 NG_NODE_SET_PRIVATE(node, vrp);
183 return (0);
184 }
185
186 /*
187 * Give our ok for a hook to be added.
188 */
189 static int
ng_vlanrotate_newhook(node_p node,hook_p hook,const char * name)190 ng_vlanrotate_newhook(node_p node, hook_p hook, const char *name)
191 {
192 const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
193 hook_p *dst = NULL;
194
195 if (strcmp(name, NG_VLANROTATE_HOOK_ORDERED) == 0) {
196 dst = &vrp->ordered_hook;
197 } else if (strcmp(name, NG_VLANROTATE_HOOK_ORIGINAL) == 0) {
198 dst = &vrp->original_hook;
199 } else if (strcmp(name, NG_VLANROTATE_HOOK_EXCESSIVE) == 0) {
200 dst = &vrp->excessive_hook;
201 } else if (strcmp(name, NG_VLANROTATE_HOOK_INCOMPLETE) == 0) {
202 dst = &vrp->incomplete_hook;
203 } else
204 return (EINVAL); /* not a hook we know about */
205
206 if (*dst != NULL)
207 return (EADDRINUSE); /* don't override */
208
209 *dst = hook;
210 return (0);
211 }
212
213 /*
214 * Get a netgraph control message.
215 * A response is not required.
216 */
217 static int
ng_vlanrotate_rcvmsg(node_p node,item_p item,hook_p lasthook)218 ng_vlanrotate_rcvmsg(node_p node, item_p item, hook_p lasthook)
219 {
220 const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
221 struct ng_mesg *resp = NULL;
222 struct ng_mesg *msg;
223 struct ng_vlanrotate_conf *pcf;
224 int error = 0;
225
226 NGI_GET_MSG(item, msg);
227 /* Deal with message according to cookie and command */
228 switch (msg->header.typecookie) {
229 case NGM_VLANROTATE_COOKIE:
230 switch (msg->header.cmd) {
231 case NGM_VLANROTATE_GET_CONF:
232 NG_MKRESPONSE(resp, msg, sizeof(vrp->conf), M_NOWAIT);
233 if (!resp) {
234 error = ENOMEM;
235 break;
236 }
237 *((struct ng_vlanrotate_conf *)resp->data) = vrp->conf;
238 break;
239 case NGM_VLANROTATE_SET_CONF:
240 if (msg->header.arglen != sizeof(*pcf)) {
241 error = EINVAL;
242 break;
243 }
244
245 pcf = (struct ng_vlanrotate_conf *)msg->data;
246
247 if (pcf->max == 0) /* keep current value */
248 pcf->max = vrp->conf.max;
249
250 if ((pcf->max > NG_VLANROTATE_MAX_VLANS) ||
251 (pcf->min > pcf->max) ||
252 (abs(pcf->rot) >= pcf->max)) {
253 error = EINVAL;
254 break;
255 }
256
257 vrp->conf = *pcf;
258 break;
259 case NGM_VLANROTATE_GET_STAT:
260 case NGM_VLANROTATE_GETCLR_STAT:
261 {
262 struct ng_vlanrotate_stat *p;
263 int i;
264
265 NG_MKRESPONSE(resp, msg, sizeof(*p), M_NOWAIT);
266 if (!resp) {
267 error = ENOMEM;
268 break;
269 }
270 p = (struct ng_vlanrotate_stat *)resp->data;
271 p->drops = counter_u64_fetch(vrp->stats.drops);
272 p->excessive = counter_u64_fetch(vrp->stats.excessive);
273 p->incomplete = counter_u64_fetch(vrp->stats.incomplete);
274 for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
275 p->histogram[i] = counter_u64_fetch(vrp->stats.histogram[i]);
276 if (msg->header.cmd != NGM_VLANROTATE_GETCLR_STAT)
277 break;
278 }
279 case NGM_VLANROTATE_CLR_STAT:
280 {
281 int i;
282
283 counter_u64_zero(vrp->stats.drops);
284 counter_u64_zero(vrp->stats.excessive);
285 counter_u64_zero(vrp->stats.incomplete);
286 for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
287 counter_u64_zero(vrp->stats.histogram[i]);
288 break;
289 }
290 default:
291 error = EINVAL; /* unknown command */
292 break;
293 }
294 break;
295 default:
296 error = EINVAL; /* unknown cookie type */
297 break;
298 }
299
300 /* Take care of synchronous response, if any */
301 NG_RESPOND_MSG(error, node, item, resp);
302 /* Free the message and return */
303 NG_FREE_MSG(msg);
304 return (error);
305 }
306
307 /*
308 * Receive data, and do rotate the vlans as desired.
309 *
310 * Rotating is quite complicated if the rotation offset and the number
311 * of vlans are not relativly prime. In this case multiple slices need
312 * to be rotated separately.
313 *
314 * Rotation can be additive or subtractive. Some examples:
315 * 01234 5 vlans given
316 * -----
317 * 34012 +2 rotate
318 * 12340 +4 rotate
319 * 12340 -1 rotate
320 *
321 * First some helper functions ...
322 */
323
324 struct ether_vlan_stack_entry {
325 uint16_t proto;
326 uint16_t tag;
327 } __packed;
328
329 struct ether_vlan_stack_header {
330 uint8_t dst[ETHER_ADDR_LEN];
331 uint8_t src[ETHER_ADDR_LEN];
332 struct ether_vlan_stack_entry vlan_stack[1];
333 } __packed;
334
335 static int
ng_vlanrotate_gcd(int a,int b)336 ng_vlanrotate_gcd(int a, int b)
337 {
338 if (b == 0)
339 return a;
340 else
341 return ng_vlanrotate_gcd(b, a % b);
342 }
343
344 static void
ng_vlanrotate_rotate(struct ether_vlan_stack_entry arr[],int d,int n)345 ng_vlanrotate_rotate(struct ether_vlan_stack_entry arr[], int d, int n)
346 {
347 int i, j, k;
348 struct ether_vlan_stack_entry temp;
349
350 /* for each commensurable slice */
351 for (i = ng_vlanrotate_gcd(d, n); i-- > 0;) {
352 /* rotate left aka downwards */
353 temp = arr[i];
354 j = i;
355
356 while (1) {
357 k = j + d;
358 if (k >= n)
359 k = k - n;
360 if (k == i)
361 break;
362 arr[j] = arr[k];
363 j = k;
364 }
365
366 arr[j] = temp;
367 }
368 }
369
370 static int
ng_vlanrotate_rcvdata(hook_p hook,item_p item)371 ng_vlanrotate_rcvdata(hook_p hook, item_p item)
372 {
373 const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
374 struct ether_vlan_stack_header *evsh;
375 struct mbuf *m = NULL;
376 hook_p dst_hook;
377 int8_t rotate;
378 int8_t vlans = 0;
379 int error = ENOSYS;
380
381 NGI_GET_M(item, m);
382
383 if (hook == vrp->ordered_hook) {
384 rotate = +vrp->conf.rot;
385 dst_hook = vrp->original_hook;
386 } else if (hook == vrp->original_hook) {
387 rotate = -vrp->conf.rot;
388 dst_hook = vrp->ordered_hook;
389 } else {
390 dst_hook = vrp->original_hook;
391 goto send; /* everything else goes out unmodified */
392 }
393
394 if (dst_hook == NULL) {
395 error = ENETDOWN;
396 goto fail;
397 }
398
399 /* count the vlans */
400 for (vlans = 0; vlans <= NG_VLANROTATE_MAX_VLANS; vlans++) {
401 size_t expected_len = sizeof(struct ether_vlan_stack_header)
402 + vlans * sizeof(struct ether_vlan_stack_entry);
403
404 if (m->m_len < expected_len) {
405 m = m_pullup(m, expected_len);
406 if (m == NULL) {
407 error = EINVAL;
408 goto fail;
409 }
410 }
411
412 evsh = mtod(m, struct ether_vlan_stack_header *);
413 switch (ntohs(evsh->vlan_stack[vlans].proto)) {
414 case ETHERTYPE_VLAN:
415 case ETHERTYPE_QINQ:
416 case ETHERTYPE_8021Q9100:
417 case ETHERTYPE_8021Q9200:
418 case ETHERTYPE_8021Q9300:
419 break;
420 default:
421 goto out;
422 }
423 }
424 out:
425 if ((vlans > vrp->conf.max) || (vlans >= NG_VLANROTATE_MAX_VLANS)) {
426 counter_u64_add(vrp->stats.excessive, 1);
427 dst_hook = vrp->excessive_hook;
428 goto send;
429 }
430
431 if ((vlans < vrp->conf.min) || (vlans <= abs(rotate))) {
432 counter_u64_add(vrp->stats.incomplete, 1);
433 dst_hook = vrp->incomplete_hook;
434 goto send;
435 }
436 counter_u64_add(vrp->stats.histogram[vlans], 1);
437
438 /* rotating upwards always (using modular arithmetics) */
439 if (rotate == 0) {
440 /* nothing to do */
441 } else if (rotate > 0) {
442 ng_vlanrotate_rotate(evsh->vlan_stack, rotate, vlans);
443 } else {
444 ng_vlanrotate_rotate(evsh->vlan_stack, vlans + rotate, vlans);
445 }
446
447 send:
448 if (dst_hook == NULL)
449 goto fail;
450 NG_FWD_NEW_DATA(error, item, dst_hook, m);
451 return 0;
452
453 fail:
454 counter_u64_add(vrp->stats.drops, 1);
455 if (m != NULL)
456 m_freem(m);
457 NG_FREE_ITEM(item);
458 return (error);
459 }
460
461 /*
462 * Do local shutdown processing..
463 * All our links and the name have already been removed.
464 */
465 static int
ng_vlanrotate_shutdown(node_p node)466 ng_vlanrotate_shutdown(node_p node)
467 {
468 const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
469 int i;
470
471 NG_NODE_SET_PRIVATE(node, NULL);
472
473 counter_u64_free(vrp->stats.drops);
474 counter_u64_free(vrp->stats.excessive);
475 counter_u64_free(vrp->stats.incomplete);
476 for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
477 counter_u64_free(vrp->stats.histogram[i]);
478
479 free(vrp, M_NETGRAPH);
480
481 NG_NODE_UNREF(node);
482 return (0);
483 }
484
485 /*
486 * Hook disconnection
487 * For this type, removal of the last link destroys the node
488 */
489 static int
ng_vlanrotate_disconnect(hook_p hook)490 ng_vlanrotate_disconnect(hook_p hook)
491 {
492 const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
493
494 if (vrp->original_hook == hook)
495 vrp->original_hook = NULL;
496 if (vrp->ordered_hook == hook)
497 vrp->ordered_hook = NULL;
498 if (vrp->excessive_hook == hook)
499 vrp->excessive_hook = NULL;
500 if (vrp->incomplete_hook == hook)
501 vrp->incomplete_hook = NULL;
502
503 /* during shutdown the node is invalid, don't shutdown twice */
504 if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
505 (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
506 ng_rmnode_self(NG_HOOK_NODE(hook));
507 return (0);
508 }
509