sys/netgraph/ng_macfilter.c

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