sys/netgraph/ng_vlan_rotate.c

   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 }