sys/netgraph/ng_macfilter.c

   1 /*
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 2002 Ericsson Research & Pekka Nikander
   5  * Copyright (c) 2020 Nick Hibma <n_hibma@FreeBSD.org>
   6  * All rights reserved.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice unmodified, this list of conditions, and the following
  13  *    disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in the
  16  *    documentation and/or other materials provided with the distribution.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  22  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  23  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  24  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  25  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  26  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  27  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  28  * SUCH DAMAGE.
  29  *
  30  * $FreeBSD$
  31  */
  32
  33 /*
  34  * MACFILTER NETGRAPH NODE TYPE
  35  *
  36  * This node type routes packets from the ether hook to either the default hook
  37  * if sender MAC address is not in the MAC table, or out over the specified
  38  * hook if it is.
  39  *
  40  * Other node types can then be used to apply specific processing to the
  41  * packets on each hook.
  42  *
  43  * If compiled with NG_MACFILTER_DEBUG the flow and resizing of the MAC table
  44  * are logged to the console.
  45  *
  46  * If compiled with NG_MACFILTER_DEBUG_RECVDATA every packet handled is logged
  47  * on the console.
  48  */
  49
  50 #include <sys/param.h>
  51 #include <sys/ctype.h>
  52 #include <sys/systm.h>
  53
  54 #include <sys/lock.h>
  55 #include <sys/mbuf.h>
  56 #include <sys/mutex.h>
  57
  58 #include <sys/kernel.h>
  59 #include <sys/malloc.h>
  60
  61 #include <sys/socket.h>
  62 #include <net/ethernet.h>
  63
  64 #include <netgraph/ng_message.h>
  65 #include <netgraph/netgraph.h>
  66 #include <netgraph/ng_parse.h>
  67
  68 #include "ng_macfilter.h"
  69
  70 #ifdef NG_SEPARATE_MALLOC
  71 MALLOC_DEFINE(M_NETGRAPH_MACFILTER, "netgraph_macfilter", "netgraph macfilter node ");
  72 #else
  73 #define M_NETGRAPH_MACFILTER M_NETGRAPH
  74 #endif
  75
  76 #define MACTABLE_BLOCKSIZE      128      /* block size for incrementing table */
  77
  78 #ifdef NG_MACFILTER_DEBUG
  79 #define MACFILTER_DEBUG(fmt, ...)         printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, __VA_ARGS__)
  80 #else
  81 #define MACFILTER_DEBUG(fmt, ...)
  82 #endif
  83 #define MAC_FMT                 "%02x:%02x:%02x:%02x:%02x:%02x"
  84 #define MAC_S_ARGS(v)           (v)[0], (v)[1], (v)[2], (v)[3], (v)[4], (v)[5]
  85
  86 /*
  87  * Parse type for struct ngm_macfilter_direct
  88  */
  89
  90 static const struct ng_parse_struct_field macfilter_direct_fields[]
  91         = NGM_MACFILTER_DIRECT_FIELDS;
  92 static const struct ng_parse_type ng_macfilter_direct_type = {
  93     &ng_parse_struct_type,
  94     &macfilter_direct_fields
  95 };
  96
  97 /*
  98  * Parse type for struct ngm_macfilter_direct_hookid.
  99  */
 100
 101 static const struct ng_parse_struct_field macfilter_direct_ndx_fields[]
 102         = NGM_MACFILTER_DIRECT_NDX_FIELDS;
 103 static const struct ng_parse_type ng_macfilter_direct_hookid_type = {
 104     &ng_parse_struct_type,
 105     &macfilter_direct_ndx_fields
 106 };
 107
 108 /*
 109  * Parse types for struct ngm_macfilter_get_macs.
 110  */
 111 static int
 112 macfilter_get_macs_count(const struct ng_parse_type *type,
 113     const u_char *start, const u_char *buf)
 114 {
 115         const struct ngm_macfilter_macs *const ngm_macs =
 116             (const struct ngm_macfilter_macs *)(buf - OFFSETOF(struct ngm_macfilter_macs, macs));
 117
 118         return ngm_macs->n;
 119 }
 120 static const struct ng_parse_struct_field ng_macfilter_mac_fields[]
 121         = NGM_MACFILTER_MAC_FIELDS;
 122 static const struct ng_parse_type ng_macfilter_mac_type = {
 123     &ng_parse_struct_type,
 124     ng_macfilter_mac_fields,
 125 };
 126 static const struct ng_parse_array_info ng_macfilter_macs_array_info = {
 127     &ng_macfilter_mac_type,
 128     macfilter_get_macs_count
 129 };
 130 static const struct ng_parse_type ng_macfilter_macs_array_type = {
 131     &ng_parse_array_type,
 132     &ng_macfilter_macs_array_info
 133 };
 134 static const struct ng_parse_struct_field ng_macfilter_macs_fields[]
 135         = NGM_MACFILTER_MACS_FIELDS;
 136 static const struct ng_parse_type ng_macfilter_macs_type = {
 137     &ng_parse_struct_type,
 138     &ng_macfilter_macs_fields
 139 };
 140
 141 /*
 142  * Parse types for struct ngm_macfilter_get_hooks.
 143  */
 144 static int
 145 macfilter_get_upper_hook_count(const struct ng_parse_type *type,
 146     const u_char *start, const u_char *buf)
 147 {
 148         const struct ngm_macfilter_hooks *const ngm_hooks =
 149             (const struct ngm_macfilter_hooks *)(buf - OFFSETOF(struct ngm_macfilter_hooks, hooks));
 150
 151         MACFILTER_DEBUG("buf %p, ngm_hooks %p, n %d", buf, ngm_hooks, ngm_hooks->n);
 152
 153         return ngm_hooks->n;
 154 }
 155
 156 static const struct ng_parse_struct_field ng_macfilter_hook_fields[]
 157         = NGM_MACFILTER_HOOK_FIELDS;
 158 static const struct ng_parse_type ng_macfilter_hook_type = {
 159     &ng_parse_struct_type,
 160     ng_macfilter_hook_fields,
 161 };
 162 static const struct ng_parse_array_info ng_macfilter_hooks_array_info = {
 163     &ng_macfilter_hook_type,
 164     macfilter_get_upper_hook_count
 165 };
 166 static const struct ng_parse_type ng_macfilter_hooks_array_type = {
 167     &ng_parse_array_type,
 168     &ng_macfilter_hooks_array_info
 169 };
 170 static const struct ng_parse_struct_field ng_macfilter_hooks_fields[]
 171         = NGM_MACFILTER_HOOKS_FIELDS;
 172 static const struct ng_parse_type ng_macfilter_hooks_type = {
 173     &ng_parse_struct_type,
 174     &ng_macfilter_hooks_fields
 175 };
 176
 177 /*
 178  * List of commands and how to convert arguments to/from ASCII
 179  */
 180 static const struct ng_cmdlist ng_macfilter_cmdlist[] = {
 181     {
 182         NGM_MACFILTER_COOKIE,
 183         NGM_MACFILTER_RESET,
 184         "reset",
 185         NULL,
 186         NULL
 187     },
 188     {
 189         NGM_MACFILTER_COOKIE,
 190         NGM_MACFILTER_DIRECT,
 191         "direct",
 192         &ng_macfilter_direct_type,
 193         NULL
 194     },
 195     {
 196         NGM_MACFILTER_COOKIE,
 197         NGM_MACFILTER_DIRECT_HOOKID,
 198         "directi",
 199         &ng_macfilter_direct_hookid_type,
 200         NULL
 201     },
 202     {
 203         NGM_MACFILTER_COOKIE,
 204         NGM_MACFILTER_GET_MACS,
 205         "getmacs",
 206         NULL,
 207         &ng_macfilter_macs_type
 208     },
 209     {
 210         NGM_MACFILTER_COOKIE,
 211         NGM_MACFILTER_GETCLR_MACS,
 212         "getclrmacs",
 213         NULL,
 214         &ng_macfilter_macs_type
 215     },
 216     {
 217         NGM_MACFILTER_COOKIE,
 218         NGM_MACFILTER_CLR_MACS,
 219         "clrmacs",
 220         NULL,
 221         NULL,
 222     },
 223     {
 224         NGM_MACFILTER_COOKIE,
 225         NGM_MACFILTER_GET_HOOKS,
 226         "gethooks",
 227         NULL,
 228         &ng_macfilter_hooks_type
 229     },
 230     { 0 }
 231 };
 232
 233 /*
 234  * Netgraph node type descriptor
 235  */
 236 static ng_constructor_t ng_macfilter_constructor;
 237 static ng_rcvmsg_t      ng_macfilter_rcvmsg;
 238 static ng_shutdown_t    ng_macfilter_shutdown;
 239 static ng_newhook_t     ng_macfilter_newhook;
 240 static ng_rcvdata_t     ng_macfilter_rcvdata;
 241 static ng_disconnect_t  ng_macfilter_disconnect;
 242
 243 static struct ng_type typestruct = {
 244     .version =     NG_ABI_VERSION,
 245     .name =        NG_MACFILTER_NODE_TYPE,
 246     .constructor = ng_macfilter_constructor,
 247     .rcvmsg =      ng_macfilter_rcvmsg,
 248     .shutdown =    ng_macfilter_shutdown,
 249     .newhook =     ng_macfilter_newhook,
 250     .rcvdata =     ng_macfilter_rcvdata,
 251     .disconnect =  ng_macfilter_disconnect,
 252     .cmdlist =     ng_macfilter_cmdlist
 253 };
 254 NETGRAPH_INIT(macfilter, &typestruct);
 255
 256 /*
 257  * Per MAC address info: the hook where to send to, the address
 258  * Note: We use the same struct as in the netgraph message, so we can bcopy the
 259  * array.
 260  */
 261 typedef struct ngm_macfilter_mac *mf_mac_p;
 262
 263 /*
 264  * Node info
 265  */
 266 typedef struct {
 267     hook_p     mf_ether_hook;   /* Ethernet hook */
 268
 269     hook_p     *mf_upper;       /* Upper hooks */
 270     u_int      mf_upper_cnt;    /* Allocated # of upper slots */
 271
 272     struct mtx mtx;             /* Mutex for MACs table */
 273     mf_mac_p   mf_macs;         /* MAC info: dynamically allocated */
 274     u_int      mf_mac_allocated;/* Allocated # of MAC slots */
 275     u_int      mf_mac_used;     /* Used # of MAC slots */
 276 } *macfilter_p;
 277
 278 /*
 279  * Resize the MAC table to accommodate at least mfp->mf_mac_used + 1 entries.
 280  *
 281  * Note: mtx already held
 282  */
 283 static int
 284 macfilter_mactable_resize(macfilter_p mfp)
 285 {
 286     int error = 0;
 287
 288     int n = mfp->mf_mac_allocated;
 289     if (mfp->mf_mac_used < 2*MACTABLE_BLOCKSIZE-1)                              /* minimum size */
 290         n = 2*MACTABLE_BLOCKSIZE-1;
 291     else if (mfp->mf_mac_used + 2*MACTABLE_BLOCKSIZE < mfp->mf_mac_allocated)   /* reduce size */
 292         n = mfp->mf_mac_allocated - MACTABLE_BLOCKSIZE;
 293     else if (mfp->mf_mac_used == mfp->mf_mac_allocated)                         /* increase size */
 294         n = mfp->mf_mac_allocated + MACTABLE_BLOCKSIZE;
 295
 296     if (n != mfp->mf_mac_allocated) {
 297         MACFILTER_DEBUG("used=%d allocated=%d->%d",
 298               mfp->mf_mac_used, mfp->mf_mac_allocated, n);
 299
 300         mf_mac_p mfp_new = realloc(mfp->mf_macs,
 301                 sizeof(mfp->mf_macs[0])*n,
 302                 M_NETGRAPH, M_NOWAIT | M_ZERO);
 303         if (mfp_new == NULL) {
 304             error = -1;
 305         } else {
 306             mfp->mf_macs = mfp_new;
 307             mfp->mf_mac_allocated = n;
 308         }
 309     }
 310
 311     return error;
 312 }
 313
 314 /*
 315  * Resets the macfilter to pass all received packets
 316  * to the default hook.
 317  *
 318  * Note: mtx already held
 319  */
 320 static void
 321 macfilter_reset(macfilter_p mfp)
 322 {
 323     mfp->mf_mac_used = 0;
 324
 325     macfilter_mactable_resize(mfp);
 326 }
 327
 328 /*
 329  * Resets the counts for each MAC address.
 330  *
 331  * Note: mtx already held
 332  */
 333 static void
 334 macfilter_reset_stats(macfilter_p mfp)
 335 {
 336     int i;
 337
 338     for (i = 0; i < mfp->mf_mac_used; i++) {
 339         mf_mac_p p = &mfp->mf_macs[i];
 340         p->packets_in = p->packets_out = 0;
 341         p->bytes_in = p->bytes_out = 0;
 342     }
 343 }
 344
 345 /*
 346  * Count the number of matching macs routed to this hook.
 347  *
 348  * Note: mtx already held
 349  */
 350 static int
 351 macfilter_mac_count(macfilter_p mfp, int hookid)
 352 {
 353     int i;
 354     int cnt = 0;
 355
 356     for (i = 0; i < mfp->mf_mac_used; i++)
 357         if (mfp->mf_macs[i].hookid == hookid)
 358             cnt++;
 359
 360     return cnt;
 361 }
 362
 363 /*
 364  * Find a MAC address in the mac table.
 365  *
 366  * Returns 0 on failure with *ri set to index before which to insert a new
 367  * element. Or returns 1 on success with *ri set to the index of the element
 368  * that matches.
 369  *
 370  * Note: mtx already held.
 371  */
 372 static u_int
 373 macfilter_find_mac(macfilter_p mfp, const u_char *ether, u_int *ri)
 374 {
 375     mf_mac_p mf_macs = mfp->mf_macs;
 376
 377     u_int base = 0;
 378     u_int range = mfp->mf_mac_used;
 379     while (range > 0) {
 380         u_int middle = base + (range >> 1);             /* middle */
 381         int d = bcmp(ether, mf_macs[middle].ether, ETHER_ADDR_LEN);
 382         if (d == 0) {                                   /* match */
 383             *ri = middle;
 384             return 1;
 385         } else if (d > 0) {                             /* move right */
 386             range -= middle - base + 1;
 387             base = middle + 1;
 388         } else {                                        /* move left */
 389             range = middle - base;
 390         }
 391     }
 392
 393     *ri = base;
 394     return 0;
 395 }
 396
 397 /*
 398  * Change the upper hook for the given MAC address. If the hook id is zero (the
 399  * default hook), the MAC address is removed from the table. Otherwise it is
 400  * inserted to the table at a proper location, and the id of the hook is
 401  * marked.
 402  *
 403  * Note: mtx already held.
 404  */
 405 static int
 406 macfilter_mactable_change(macfilter_p mfp, u_char *ether, int hookid)
 407 {
 408     u_int i;
 409     int found = macfilter_find_mac(mfp, ether, &i);
 410
 411     mf_mac_p mf_macs = mfp->mf_macs;
 412
 413     MACFILTER_DEBUG("ether=" MAC_FMT " found=%d i=%d ether=" MAC_FMT " hookid=%d->%d used=%d allocated=%d",
 414           MAC_S_ARGS(ether), found, i, MAC_S_ARGS(mf_macs[i].ether),
 415           (found? mf_macs[i].hookid:NG_MACFILTER_HOOK_DEFAULT_ID), hookid,
 416           mfp->mf_mac_used, mfp->mf_mac_allocated);
 417
 418     if (found) {
 419         if (hookid == NG_MACFILTER_HOOK_DEFAULT_ID) {   /* drop */
 420             /* Compress table */
 421             mfp->mf_mac_used--;
 422             size_t len = (mfp->mf_mac_used - i) * sizeof(mf_macs[0]);
 423             if (len > 0)
 424                 bcopy(&mf_macs[i+1], &mf_macs[i], len);
 425
 426             macfilter_mactable_resize(mfp);
 427         } else {                                        /* modify */
 428             mf_macs[i].hookid = hookid;
 429         }
 430     } else {
 431         if (hookid == NG_MACFILTER_HOOK_DEFAULT_ID) {   /* not found */
 432             /* not present and not inserted */
 433             return 0;
 434         } else {                                        /* add */
 435             if (macfilter_mactable_resize(mfp) == -1) {
 436                 return ENOMEM;
 437             } else {
 438                 mf_macs = mfp->mf_macs;                 /* reassign; might have moved during resize */
 439
 440                 /* make room for new entry, unless appending */
 441                 size_t len = (mfp->mf_mac_used - i) * sizeof(mf_macs[0]);
 442                 if (len > 0)
 443                     bcopy(&mf_macs[i], &mf_macs[i+1], len);
 444
 445                 mf_macs[i].hookid = hookid;
 446                 bcopy(ether, mf_macs[i].ether, ETHER_ADDR_LEN);
 447
 448                 mfp->mf_mac_used++;
 449             }
 450         }
 451     }
 452
 453     return 0;
 454 }
 455
 456 static int
 457 macfilter_mactable_remove_by_hookid(macfilter_p mfp, int hookid)
 458 {
 459     int i, j;
 460
 461     for (i = 0, j = 0; i < mfp->mf_mac_used; i++) {
 462         if (mfp->mf_macs[i].hookid != hookid) {
 463             if (i != j)
 464                 bcopy(&mfp->mf_macs[i], &mfp->mf_macs[j], sizeof(mfp->mf_macs[0]));
 465             j++;
 466         }
 467     }
 468
 469     int removed = i - j;
 470     mfp->mf_mac_used = j;
 471     macfilter_mactable_resize(mfp);
 472
 473     return removed;
 474 }
 475
 476 static int
 477 macfilter_find_hook(macfilter_p mfp, const char *hookname)
 478 {
 479     int hookid;
 480
 481     for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++) {
 482         if (mfp->mf_upper[hookid]) {
 483             if (strncmp(NG_HOOK_NAME(mfp->mf_upper[hookid]),
 484                     hookname, NG_HOOKSIZ) == 0) {
 485                 return hookid;
 486             }
 487         }
 488     }
 489
 490     return 0;
 491 }
 492
 493 static int
 494 macfilter_direct(macfilter_p mfp, struct ngm_macfilter_direct *md)
 495 {
 496     MACFILTER_DEBUG("ether=" MAC_FMT " hook=%s",
 497         MAC_S_ARGS(md->ether), md->hookname);
 498
 499     int hookid = macfilter_find_hook(mfp, md->hookname);
 500     if (hookid < 0)
 501         return ENOENT;
 502
 503     return macfilter_mactable_change(mfp, md->ether, hookid);
 504 }
 505
 506 static int
 507 macfilter_direct_hookid(macfilter_p mfp, struct ngm_macfilter_direct_hookid *mdi)
 508 {
 509     MACFILTER_DEBUG("ether=" MAC_FMT " hookid=%d",
 510         MAC_S_ARGS(mdi->ether), mdi->hookid);
 511
 512     if (mdi->hookid >= mfp->mf_upper_cnt)
 513         return EINVAL;
 514     else if (mfp->mf_upper[mdi->hookid] == NULL)
 515         return EINVAL;
 516
 517     return macfilter_mactable_change(mfp, mdi->ether, mdi->hookid);
 518 }
 519
 520 /*
 521  * Packet handling
 522  */
 523
 524 /*
 525  * Pass packets received from any upper hook to
 526  * a lower hook
 527  */
 528 static int
 529 macfilter_ether_output(hook_p hook, macfilter_p mfp, struct mbuf *m, hook_p *next_hook)
 530 {
 531     struct ether_header *ether_header = mtod(m, struct ether_header *);
 532     u_char *ether = ether_header->ether_dhost;
 533
 534     *next_hook = mfp->mf_ether_hook;
 535
 536     mtx_lock(&mfp->mtx);
 537
 538     u_int i;
 539     int found = macfilter_find_mac(mfp, ether, &i);
 540     if (found) {
 541         mf_mac_p mf_macs = mfp->mf_macs;
 542
 543         mf_macs[i].packets_out++;
 544         if (m->m_len > ETHER_HDR_LEN)
 545             mf_macs[i].bytes_out += m->m_len - ETHER_HDR_LEN;
 546
 547 #ifdef NG_MACFILTER_DEBUG_RECVDATA
 548         MACFILTER_DEBUG("ether=" MAC_FMT " len=%db->%lldb: bytes: %s -> %s",
 549             MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN, mf_macs[i].bytes_out,
 550             NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
 551 #endif
 552     } else {
 553 #ifdef NG_MACFILTER_DEBUG_RECVDATA
 554         MACFILTER_DEBUG("ether=" MAC_FMT " len=%db->?b: bytes: %s->%s",
 555             MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN,
 556             NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
 557 #endif
 558     }
 559
 560     mtx_unlock(&mfp->mtx);
 561
 562     return 0;
 563 }
 564
 565 /*
 566  * Search for the right upper hook, based on the source ethernet
 567  * address.  If not found, pass to the default upper hook.
 568  */
 569 static int
 570 macfilter_ether_input(hook_p hook, macfilter_p mfp, struct mbuf *m, hook_p *next_hook)
 571 {
 572     struct ether_header *ether_header = mtod(m, struct ether_header *);
 573     u_char *ether = ether_header->ether_shost;
 574     int hookid = NG_MACFILTER_HOOK_DEFAULT_ID;
 575
 576     mtx_lock(&mfp->mtx);
 577
 578     u_int i;
 579     int found = macfilter_find_mac(mfp, ether, &i);
 580     if (found) {
 581         mf_mac_p mf_macs = mfp->mf_macs;
 582
 583         mf_macs[i].packets_in++;
 584         if (m->m_len > ETHER_HDR_LEN)
 585             mf_macs[i].bytes_in += m->m_len - ETHER_HDR_LEN;
 586
 587         hookid = mf_macs[i].hookid;
 588
 589 #ifdef NG_MACFILTER_DEBUG_RECVDATA
 590         MACFILTER_DEBUG("ether=" MAC_FMT " len=%db->%lldb: bytes: %s->%s",
 591             MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN, mf_macs[i].bytes_in,
 592             NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
 593 #endif
 594     } else {
 595 #ifdef NG_MACFILTER_DEBUG_RECVDATA
 596         MACFILTER_DEBUG("ether=" MAC_FMT " len=%db->?b: bytes: %s->%s",
 597             MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN,
 598             NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
 599 #endif
 600     }
 601
 602     if (hookid >= mfp->mf_upper_cnt)
 603         *next_hook = NULL;
 604     else
 605         *next_hook = mfp->mf_upper[hookid];
 606
 607     mtx_unlock(&mfp->mtx);
 608
 609     return 0;
 610 }
 611
 612 /*
 613  * ======================================================================
 614  * Netgraph hooks
 615  * ======================================================================
 616  */
 617
 618 /*
 619  * See basic netgraph code for comments on the individual functions.
 620  */
 621
 622 static int
 623 ng_macfilter_constructor(node_p node)
 624 {
 625     macfilter_p mfp = malloc(sizeof(*mfp), M_NETGRAPH, M_NOWAIT | M_ZERO);
 626     if (mfp == NULL)
 627         return ENOMEM;
 628
 629     int error = macfilter_mactable_resize(mfp);
 630     if (error)
 631         return error;
 632
 633     NG_NODE_SET_PRIVATE(node, mfp);
 634
 635     mtx_init(&mfp->mtx, "Macfilter table", NULL, MTX_DEF);
 636
 637     return (0);
 638 }
 639
 640 static int
 641 ng_macfilter_newhook(node_p node, hook_p hook, const char *hookname)
 642 {
 643     const macfilter_p mfp = NG_NODE_PRIVATE(node);
 644
 645     MACFILTER_DEBUG("%s", hookname);
 646
 647     if (strcmp(hookname, NG_MACFILTER_HOOK_ETHER) == 0) {
 648         mfp->mf_ether_hook = hook;
 649     } else {
 650         int hookid;
 651         if (strcmp(hookname, NG_MACFILTER_HOOK_DEFAULT) == 0) {
 652             hookid = NG_MACFILTER_HOOK_DEFAULT_ID;
 653         } else {
 654             for (hookid = 1; hookid < mfp->mf_upper_cnt; hookid++)
 655                 if (mfp->mf_upper[hookid] == NULL)
 656                     break;
 657         }
 658
 659         if (hookid >= mfp->mf_upper_cnt) {
 660             MACFILTER_DEBUG("upper cnt %d -> %d", mfp->mf_upper_cnt, hookid + 1);
 661
 662             mfp->mf_upper_cnt = hookid + 1;
 663             mfp->mf_upper = realloc(mfp->mf_upper,
 664                     sizeof(mfp->mf_upper[0])*mfp->mf_upper_cnt,
 665                     M_NETGRAPH, M_NOWAIT | M_ZERO);
 666         }
 667
 668         mfp->mf_upper[hookid] = hook;
 669     }
 670
 671     return(0);
 672 }
 673
 674 static int
 675 ng_macfilter_rcvmsg(node_p node, item_p item, hook_p lasthook)
 676 {
 677     const macfilter_p mfp = NG_NODE_PRIVATE(node);
 678     struct ng_mesg *resp = NULL;
 679     struct ng_mesg *msg;
 680     int error = 0;
 681     struct ngm_macfilter_macs *ngm_macs;
 682     struct ngm_macfilter_hooks *ngm_hooks;
 683     struct ngm_macfilter_direct *md;
 684     struct ngm_macfilter_direct_hookid *mdi;
 685     int n = 0, i = 0;
 686     int hookid = 0;
 687     int resplen;
 688
 689     NGI_GET_MSG(item, msg);
 690
 691     mtx_lock(&mfp->mtx);
 692
 693     switch (msg->header.typecookie) {
 694     case NGM_MACFILTER_COOKIE:
 695         switch (msg->header.cmd) {
 696
 697         case NGM_MACFILTER_RESET:
 698             macfilter_reset(mfp);
 699             break;
 700
 701         case NGM_MACFILTER_DIRECT:
 702             if (msg->header.arglen != sizeof(struct ngm_macfilter_direct)) {
 703                 MACFILTER_DEBUG("direct: wrong type length (%d, expected %zu)",
 704                       msg->header.arglen, sizeof(struct ngm_macfilter_direct));
 705                 error = EINVAL;
 706                 break;
 707             }
 708             md = (struct ngm_macfilter_direct *)msg->data;
 709             error = macfilter_direct(mfp, md);
 710             break;
 711         case NGM_MACFILTER_DIRECT_HOOKID:
 712             if (msg->header.arglen != sizeof(struct ngm_macfilter_direct_hookid)) {
 713                 MACFILTER_DEBUG("direct hookid: wrong type length (%d, expected %zu)",
 714                       msg->header.arglen, sizeof(struct ngm_macfilter_direct));
 715                 error = EINVAL;
 716                 break;
 717             }
 718             mdi = (struct ngm_macfilter_direct_hookid *)msg->data;
 719             error = macfilter_direct_hookid(mfp, mdi);
 720             break;
 721
 722         case NGM_MACFILTER_GET_MACS:
 723         case NGM_MACFILTER_GETCLR_MACS:
 724             n = mfp->mf_mac_used;
 725             resplen = sizeof(struct ngm_macfilter_macs) + n * sizeof(struct ngm_macfilter_mac);
 726             NG_MKRESPONSE(resp, msg, resplen, M_NOWAIT);
 727             if (resp == NULL) {
 728                 error = ENOMEM;
 729                 break;
 730             }
 731             ngm_macs = (struct ngm_macfilter_macs *)resp->data;
 732             ngm_macs->n = n;
 733             bcopy(mfp->mf_macs, &ngm_macs->macs[0], n * sizeof(struct ngm_macfilter_mac));
 734
 735             if (msg->header.cmd == NGM_MACFILTER_GETCLR_MACS)
 736                 macfilter_reset_stats(mfp);
 737             break;
 738
 739         case NGM_MACFILTER_CLR_MACS:
 740             macfilter_reset_stats(mfp);
 741             break;
 742
 743         case NGM_MACFILTER_GET_HOOKS:
 744             for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++)
 745                 if (mfp->mf_upper[hookid] != NULL)
 746                     n++;
 747             resplen = sizeof(struct ngm_macfilter_hooks) + n * sizeof(struct ngm_macfilter_hook);
 748             NG_MKRESPONSE(resp, msg, resplen, M_NOWAIT | M_ZERO);
 749             if (resp == NULL) {
 750                 error = ENOMEM;
 751                 break;
 752             }
 753
 754             ngm_hooks = (struct ngm_macfilter_hooks *)resp->data;
 755             ngm_hooks->n = n;
 756             for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++) {
 757                 if (mfp->mf_upper[hookid] != NULL) {
 758                     struct ngm_macfilter_hook *ngm_hook = &ngm_hooks->hooks[i++];
 759                     strlcpy(ngm_hook->hookname,
 760                             NG_HOOK_NAME(mfp->mf_upper[hookid]),
 761                             NG_HOOKSIZ);
 762                     ngm_hook->hookid = hookid;
 763                     ngm_hook->maccnt = macfilter_mac_count(mfp, hookid);
 764                 }
 765             }
 766             break;
 767
 768         default:
 769             error = EINVAL;             /* unknown command */
 770             break;
 771         }
 772         break;
 773
 774     default:
 775         error = EINVAL;                 /* unknown cookie type */
 776         break;
 777     }
 778
 779     mtx_unlock(&mfp->mtx);
 780
 781     NG_RESPOND_MSG(error, node, item, resp);
 782     NG_FREE_MSG(msg);
 783
 784     return error;
 785 }
 786
 787 static int
 788 ng_macfilter_rcvdata(hook_p hook, item_p item)
 789 {
 790     const macfilter_p mfp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 791     int error;
 792     hook_p next_hook = NULL;
 793     struct mbuf *m;
 794
 795     m = NGI_M(item);    /* 'item' still owns it. We are peeking */
 796     MACFILTER_DEBUG("%s", NG_HOOK_NAME(hook));
 797
 798     if (hook == mfp->mf_ether_hook)
 799         error = macfilter_ether_input(hook, mfp, m, &next_hook);
 800     else if (mfp->mf_ether_hook != NULL)
 801         error = macfilter_ether_output(hook, mfp, m, &next_hook);
 802
 803     if (next_hook == NULL) {
 804         NG_FREE_ITEM(item);
 805         return (0);
 806     }
 807
 808     NG_FWD_ITEM_HOOK(error, item, next_hook);
 809
 810     return error;
 811 }
 812
 813 static int
 814 ng_macfilter_disconnect(hook_p hook)
 815 {
 816     const macfilter_p mfp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 817
 818     mtx_lock(&mfp->mtx);
 819
 820     if (mfp->mf_ether_hook == hook) {
 821         mfp->mf_ether_hook = NULL;
 822
 823         MACFILTER_DEBUG("%s", NG_HOOK_NAME(hook));
 824     } else {
 825         int hookid;
 826
 827         for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++) {
 828             if (mfp->mf_upper[hookid] == hook) {
 829                 mfp->mf_upper[hookid] = NULL;
 830
 831 #ifndef NG_MACFILTER_DEBUG
 832                 macfilter_mactable_remove_by_hookid(mfp, hookid);
 833 #else
 834                 int cnt = macfilter_mactable_remove_by_hookid(mfp, hookid);
 835
 836                 MACFILTER_DEBUG("%s: removed %d MACs", NG_HOOK_NAME(hook), cnt);
 837 #endif
 838                 break;
 839             }
 840         }
 841
 842         if (hookid == mfp->mf_upper_cnt - 1) {
 843             /* Reduce the size of the array when the last element was removed */
 844             for (--hookid; hookid >= 0 && mfp->mf_upper[hookid] == NULL; hookid--)
 845                 ;
 846
 847             MACFILTER_DEBUG("upper cnt %d -> %d", mfp->mf_upper_cnt, hookid + 1);
 848             mfp->mf_upper_cnt = hookid + 1;
 849             mfp->mf_upper = realloc(mfp->mf_upper,
 850                     sizeof(mfp->mf_upper[0])*mfp->mf_upper_cnt,
 851                     M_NETGRAPH, M_NOWAIT | M_ZERO);
 852         }
 853     }
 854
 855     mtx_unlock(&mfp->mtx);
 856
 857     if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0)
 858         && (NG_NODE_IS_VALID(NG_HOOK_NODE(hook)))) {
 859         ng_rmnode_self(NG_HOOK_NODE(hook));
 860     }
 861
 862     return (0);
 863 }
 864
 865 static int
 866 ng_macfilter_shutdown(node_p node)
 867 {
 868     const macfilter_p mfp = NG_NODE_PRIVATE(node);
 869
 870     mtx_destroy(&mfp->mtx);
 871     free(mfp->mf_upper, M_NETGRAPH);
 872     free(mfp->mf_macs, M_NETGRAPH);
 873     free(mfp, M_NETGRAPH);
 874
 875     NG_NODE_UNREF(node);
 876
 877     return (0);
 878 }