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
53static ng_constructor_t ng_vlanrotate_constructor;
54static ng_rcvmsg_t ng_vlanrotate_rcvmsg;
55static ng_shutdown_t ng_vlanrotate_shutdown;
56static ng_newhook_t ng_vlanrotate_newhook;
57static ng_rcvdata_t ng_vlanrotate_rcvdata;
58static ng_disconnect_t ng_vlanrotate_disconnect;
59
60/* Parse type for struct ng_vlanrotate_conf */
61static 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};
67static 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 */
73static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = {
74	&ng_parse_uint64_type,
75	NG_VLANROTATE_MAX_VLANS
76};
77static struct ng_parse_type ng_vlanrotate_stat_hist = {
78	&ng_parse_fixedarray_type,
79	&ng_vlanrotate_stat_hist_info
80};
81static 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};
88static 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 */
95static 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 */
135static 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};
146NETGRAPH_INIT(vlanrotate, &typestruct);
147
148struct 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 */
154struct 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};
162typedef struct vlanrotate *vlanrotate_p;
163
164/*
165 * Set up the private data structure.
166 */
167static int
168ng_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 */
189static int
190ng_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 */
217static int
218ng_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
324struct ether_vlan_stack_entry {
325	uint16_t	proto;
326	uint16_t	tag;
327}		__packed;
328
329struct 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
335static int
336ng_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
344static void
345ng_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
370static int
371ng_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	}
424out:
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
447send:
448	if (dst_hook == NULL)
449		goto fail;
450	NG_FWD_NEW_DATA(error, item, dst_hook, m);
451	return 0;
452
453fail:
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 */
465static int
466ng_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 */
489static int
490ng_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