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 * $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 55 static ng_constructor_t ng_vlanrotate_constructor; 56 static ng_rcvmsg_t ng_vlanrotate_rcvmsg; 57 static ng_shutdown_t ng_vlanrotate_shutdown; 58 static ng_newhook_t ng_vlanrotate_newhook; 59 static ng_rcvdata_t ng_vlanrotate_rcvdata; 60 static ng_disconnect_t ng_vlanrotate_disconnect; 61 62 /* Parse type for struct ng_vlanrotate_conf */ 63 static 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 }; 69 static 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 */ 75 static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = { 76 &ng_parse_uint64_type, 77 NG_VLANROTATE_MAX_VLANS 78 }; 79 static struct ng_parse_type ng_vlanrotate_stat_hist = { 80 &ng_parse_fixedarray_type, 81 &ng_vlanrotate_stat_hist_info 82 }; 83 static 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 }; 90 static 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 */ 97 static 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 */ 137 static 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 }; 148 NETGRAPH_INIT(vlanrotate, &typestruct); 149 150 struct 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 */ 156 struct 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 }; 164 typedef struct vlanrotate *vlanrotate_p; 165 166 /* 167 * Set up the private data structure. 168 */ 169 static int 170 ng_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 */ 191 static int 192 ng_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 */ 219 static int 220 ng_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 326 struct ether_vlan_stack_entry { 327 uint16_t proto; 328 uint16_t tag; 329 } __packed; 330 331 struct 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 337 static int 338 ng_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 346 static void 347 ng_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 372 static int 373 ng_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 } 426 out: 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 449 send: 450 if (dst_hook == NULL) 451 goto fail; 452 NG_FWD_NEW_DATA(error, item, dst_hook, m); 453 return 0; 454 455 fail: 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 */ 467 static int 468 ng_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 */ 491 static int 492 ng_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