/*- * Spdx-License-Identifier: BSD-2-Clause-FreeBSD * * Copyright (c) 2019-2021 IKS Service GmbH * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * Author: Lutz Donnerhacke * * $FreeBSD$ */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /* * This section contains the netgraph method declarations for the * sample node. These methods define the netgraph 'type'. */ static ng_constructor_t ng_vlanrotate_constructor; static ng_rcvmsg_t ng_vlanrotate_rcvmsg; static ng_shutdown_t ng_vlanrotate_shutdown; static ng_newhook_t ng_vlanrotate_newhook; static ng_rcvdata_t ng_vlanrotate_rcvdata; static ng_disconnect_t ng_vlanrotate_disconnect; /* Parse type for struct ng_vlanrotate_conf */ static const struct ng_parse_struct_field ng_vlanrotate_conf_fields[] = { {"rot", &ng_parse_int8_type}, {"min", &ng_parse_uint8_type}, {"max", &ng_parse_uint8_type}, {NULL} }; static const struct ng_parse_type ng_vlanrotate_conf_type = { &ng_parse_struct_type, &ng_vlanrotate_conf_fields }; /* Parse type for struct ng_vlanrotate_stat */ static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = { &ng_parse_uint64_type, NG_VLANROTATE_MAX_VLANS }; static struct ng_parse_type ng_vlanrotate_stat_hist = { &ng_parse_fixedarray_type, &ng_vlanrotate_stat_hist_info }; static const struct ng_parse_struct_field ng_vlanrotate_stat_fields[] = { {"drops", &ng_parse_uint64_type}, {"excessive", &ng_parse_uint64_type}, {"incomplete", &ng_parse_uint64_type}, {"histogram", &ng_vlanrotate_stat_hist}, {NULL} }; static struct ng_parse_type ng_vlanrotate_stat_type = { &ng_parse_struct_type, &ng_vlanrotate_stat_fields }; /* List of commands and how to convert arguments to/from ASCII */ static const struct ng_cmdlist ng_vlanrotate_cmdlist[] = { { NGM_VLANROTATE_COOKIE, NGM_VLANROTATE_GET_CONF, "getconf", NULL, &ng_vlanrotate_conf_type, }, { NGM_VLANROTATE_COOKIE, NGM_VLANROTATE_SET_CONF, "setconf", &ng_vlanrotate_conf_type, NULL }, { NGM_VLANROTATE_COOKIE, NGM_VLANROTATE_GET_STAT, "getstat", NULL, &ng_vlanrotate_stat_type }, { NGM_VLANROTATE_COOKIE, NGM_VLANROTATE_CLR_STAT, "clrstat", NULL, &ng_vlanrotate_stat_type }, { NGM_VLANROTATE_COOKIE, NGM_VLANROTATE_GETCLR_STAT, "getclrstat", NULL, &ng_vlanrotate_stat_type }, {0} }; /* Netgraph node type descriptor */ static struct ng_type typestruct = { .version = NG_ABI_VERSION, .name = NG_VLANROTATE_NODE_TYPE, .constructor = ng_vlanrotate_constructor, .rcvmsg = ng_vlanrotate_rcvmsg, .shutdown = ng_vlanrotate_shutdown, .newhook = ng_vlanrotate_newhook, .rcvdata = ng_vlanrotate_rcvdata, .disconnect = ng_vlanrotate_disconnect, .cmdlist = ng_vlanrotate_cmdlist, }; NETGRAPH_INIT(vlanrotate, &typestruct); struct ng_vlanrotate_kernel_stats { counter_u64_t drops, excessive, incomplete; counter_u64_t histogram[NG_VLANROTATE_MAX_VLANS]; }; /* Information we store for each node */ struct vlanrotate { hook_p original_hook; hook_p ordered_hook; hook_p excessive_hook; hook_p incomplete_hook; struct ng_vlanrotate_conf conf; struct ng_vlanrotate_kernel_stats stats; }; typedef struct vlanrotate *vlanrotate_p; /* * Set up the private data structure. */ static int ng_vlanrotate_constructor(node_p node) { int i; vlanrotate_p vrp = malloc(sizeof(*vrp), M_NETGRAPH, M_WAITOK | M_ZERO); vrp->conf.max = NG_VLANROTATE_MAX_VLANS; vrp->stats.drops = counter_u64_alloc(M_WAITOK); vrp->stats.excessive = counter_u64_alloc(M_WAITOK); vrp->stats.incomplete = counter_u64_alloc(M_WAITOK); for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++) vrp->stats.histogram[i] = counter_u64_alloc(M_WAITOK); NG_NODE_SET_PRIVATE(node, vrp); return (0); } /* * Give our ok for a hook to be added. */ static int ng_vlanrotate_newhook(node_p node, hook_p hook, const char *name) { const vlanrotate_p vrp = NG_NODE_PRIVATE(node); hook_p *dst = NULL; if (strcmp(name, NG_VLANROTATE_HOOK_ORDERED) == 0) { dst = &vrp->ordered_hook; } else if (strcmp(name, NG_VLANROTATE_HOOK_ORIGINAL) == 0) { dst = &vrp->original_hook; } else if (strcmp(name, NG_VLANROTATE_HOOK_EXCESSIVE) == 0) { dst = &vrp->excessive_hook; } else if (strcmp(name, NG_VLANROTATE_HOOK_INCOMPLETE) == 0) { dst = &vrp->incomplete_hook; } else return (EINVAL); /* not a hook we know about */ if (*dst != NULL) return (EADDRINUSE); /* don't override */ *dst = hook; return (0); } /* * Get a netgraph control message. * A response is not required. */ static int ng_vlanrotate_rcvmsg(node_p node, item_p item, hook_p lasthook) { const vlanrotate_p vrp = NG_NODE_PRIVATE(node); struct ng_mesg *resp = NULL; struct ng_mesg *msg; struct ng_vlanrotate_conf *pcf; int error = 0; NGI_GET_MSG(item, msg); /* Deal with message according to cookie and command */ switch (msg->header.typecookie) { case NGM_VLANROTATE_COOKIE: switch (msg->header.cmd) { case NGM_VLANROTATE_GET_CONF: NG_MKRESPONSE(resp, msg, sizeof(vrp->conf), M_NOWAIT); if (!resp) { error = ENOMEM; break; } *((struct ng_vlanrotate_conf *)resp->data) = vrp->conf; break; case NGM_VLANROTATE_SET_CONF: if (msg->header.arglen != sizeof(*pcf)) { error = EINVAL; break; } pcf = (struct ng_vlanrotate_conf *)msg->data; if (pcf->max == 0) /* keep current value */ pcf->max = vrp->conf.max; if ((pcf->max > NG_VLANROTATE_MAX_VLANS) || (pcf->min > pcf->max) || (abs(pcf->rot) >= pcf->max)) { error = EINVAL; break; } vrp->conf = *pcf; break; case NGM_VLANROTATE_GET_STAT: case NGM_VLANROTATE_GETCLR_STAT: { struct ng_vlanrotate_stat *p; int i; NG_MKRESPONSE(resp, msg, sizeof(*p), M_NOWAIT); if (!resp) { error = ENOMEM; break; } p = (struct ng_vlanrotate_stat *)resp->data; p->drops = counter_u64_fetch(vrp->stats.drops); p->excessive = counter_u64_fetch(vrp->stats.excessive); p->incomplete = counter_u64_fetch(vrp->stats.incomplete); for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++) p->histogram[i] = counter_u64_fetch(vrp->stats.histogram[i]); if (msg->header.cmd != NGM_VLANROTATE_GETCLR_STAT) break; } case NGM_VLANROTATE_CLR_STAT: { int i; counter_u64_zero(vrp->stats.drops); counter_u64_zero(vrp->stats.excessive); counter_u64_zero(vrp->stats.incomplete); for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++) counter_u64_zero(vrp->stats.histogram[i]); break; } default: error = EINVAL; /* unknown command */ break; } break; default: error = EINVAL; /* unknown cookie type */ break; } /* Take care of synchronous response, if any */ NG_RESPOND_MSG(error, node, item, resp); /* Free the message and return */ NG_FREE_MSG(msg); return (error); } /* * Receive data, and do rotate the vlans as desired. * * Rotating is quite complicated if the rotation offset and the number * of vlans are not relativly prime. In this case multiple slices need * to be rotated separately. * * Rotation can be additive or subtractive. Some examples: * 01234 5 vlans given * ----- * 34012 +2 rotate * 12340 +4 rotate * 12340 -1 rotate * * First some helper functions ... */ struct ether_vlan_stack_entry { uint16_t proto; uint16_t tag; } __packed; struct ether_vlan_stack_header { uint8_t dst[ETHER_ADDR_LEN]; uint8_t src[ETHER_ADDR_LEN]; struct ether_vlan_stack_entry vlan_stack[1]; } __packed; static int ng_vlanrotate_gcd(int a, int b) { if (b == 0) return a; else return ng_vlanrotate_gcd(b, a % b); } static void ng_vlanrotate_rotate(struct ether_vlan_stack_entry arr[], int d, int n) { int i, j, k; struct ether_vlan_stack_entry temp; /* for each commensurable slice */ for (i = ng_vlanrotate_gcd(d, n); i-- > 0;) { /* rotate left aka downwards */ temp = arr[i]; j = i; while (1) { k = j + d; if (k >= n) k = k - n; if (k == i) break; arr[j] = arr[k]; j = k; } arr[j] = temp; } } static int ng_vlanrotate_rcvdata(hook_p hook, item_p item) { const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); struct ether_vlan_stack_header *evsh; struct mbuf *m = NULL; hook_p dst_hook; int8_t rotate; int8_t vlans = 0; int error = ENOSYS; NGI_GET_M(item, m); if (hook == vrp->ordered_hook) { rotate = +vrp->conf.rot; dst_hook = vrp->original_hook; } else if (hook == vrp->original_hook) { rotate = -vrp->conf.rot; dst_hook = vrp->ordered_hook; } else { dst_hook = vrp->original_hook; goto send; /* everything else goes out unmodified */ } if (dst_hook == NULL) { error = ENETDOWN; goto fail; } /* count the vlans */ for (vlans = 0; vlans <= NG_VLANROTATE_MAX_VLANS; vlans++) { size_t expected_len = sizeof(struct ether_vlan_stack_header) + vlans * sizeof(struct ether_vlan_stack_entry); if (m->m_len < expected_len) { m = m_pullup(m, expected_len); if (m == NULL) { error = EINVAL; goto fail; } } evsh = mtod(m, struct ether_vlan_stack_header *); switch (ntohs(evsh->vlan_stack[vlans].proto)) { case ETHERTYPE_VLAN: case ETHERTYPE_QINQ: case ETHERTYPE_8021Q9100: case ETHERTYPE_8021Q9200: case ETHERTYPE_8021Q9300: break; default: goto out; } } out: if ((vlans > vrp->conf.max) || (vlans >= NG_VLANROTATE_MAX_VLANS)) { counter_u64_add(vrp->stats.excessive, 1); dst_hook = vrp->excessive_hook; goto send; } if ((vlans < vrp->conf.min) || (vlans <= abs(rotate))) { counter_u64_add(vrp->stats.incomplete, 1); dst_hook = vrp->incomplete_hook; goto send; } counter_u64_add(vrp->stats.histogram[vlans], 1); /* rotating upwards always (using modular arithmetics) */ if (rotate == 0) { /* nothing to do */ } else if (rotate > 0) { ng_vlanrotate_rotate(evsh->vlan_stack, rotate, vlans); } else { ng_vlanrotate_rotate(evsh->vlan_stack, vlans + rotate, vlans); } send: if (dst_hook == NULL) goto fail; NG_FWD_NEW_DATA(error, item, dst_hook, m); return 0; fail: counter_u64_add(vrp->stats.drops, 1); if (m != NULL) m_freem(m); NG_FREE_ITEM(item); return (error); } /* * Do local shutdown processing.. * All our links and the name have already been removed. */ static int ng_vlanrotate_shutdown(node_p node) { const vlanrotate_p vrp = NG_NODE_PRIVATE(node); int i; NG_NODE_SET_PRIVATE(node, NULL); counter_u64_free(vrp->stats.drops); counter_u64_free(vrp->stats.excessive); counter_u64_free(vrp->stats.incomplete); for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++) counter_u64_free(vrp->stats.histogram[i]); free(vrp, M_NETGRAPH); NG_NODE_UNREF(node); return (0); } /* * Hook disconnection * For this type, removal of the last link destroys the node */ static int ng_vlanrotate_disconnect(hook_p hook) { const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook)); if (vrp->original_hook == hook) vrp->original_hook = NULL; if (vrp->ordered_hook == hook) vrp->ordered_hook = NULL; if (vrp->excessive_hook == hook) vrp->excessive_hook = NULL; if (vrp->incomplete_hook == hook) vrp->incomplete_hook = NULL; /* during shutdown the node is invalid, don't shutdown twice */ if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) && (NG_NODE_IS_VALID(NG_HOOK_NODE(hook)))) ng_rmnode_self(NG_HOOK_NODE(hook)); return (0); }