sys/netgraph/ng_vlan_rotate.c

   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
  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 }