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