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