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