New Netgraph module ng_macfilter:

Macfilter to route packets through different hooks based on sender MAC address.

Based on ng_macfilter written by Pekka Nikander

Sponsered by Retina b.v.

Reviewed by:	afedorov
MFC after:	2 weeks
Differential Revision:	https://reviews.freebsd.org/D27268

5 files changed, 1021 insertions, 0 deletions

diff --git a/sys/conf/files b/sys/conf/files
index e2fc810672b9..9299653834e8 100644
--- a/sys/conf/files
+++ b/sys/conf/files
@@ -4299,6 +4299,7 @@ netgraph/ng_ipfw.c		optional netgraph_ipfw inet ipfirewall
 netgraph/ng_ksocket.c		optional netgraph_ksocket
 netgraph/ng_l2tp.c		optional netgraph_l2tp
 netgraph/ng_lmi.c		optional netgraph_lmi
+netgraph/ng_macfilter.c		optional netgraph_macfilter
 netgraph/ng_mppc.c		optional netgraph_mppc_compression | \
 					 netgraph_mppc_encryption
 netgraph/ng_nat.c		optional netgraph_nat inet libalias
diff --git a/sys/modules/netgraph/Makefile b/sys/modules/netgraph/Makefile
index 56ad7bd4a543..df97324a19f9 100644
--- a/sys/modules/netgraph/Makefile
+++ b/sys/modules/netgraph/Makefile
@@ -31,6 +31,7 @@ SUBDIR=	async \
 	ksocket \
 	l2tp \
 	lmi \
+	macfilter \
 	${_mppc} \
 	nat \
 	netflow \
diff --git a/sys/modules/netgraph/macfilter/Makefile b/sys/modules/netgraph/macfilter/Makefile
new file mode 100644
index 000000000000..c2b836c65157
--- /dev/null
+++ b/sys/modules/netgraph/macfilter/Makefile
@@ -0,0 +1,9 @@
+# $FreeBSD$
+
+KMOD=	ng_macfilter
+SRCS= 	ng_macfilter.c
+
+.include <bsd.kmod.mk>
+
+#CFLAGS+= -DNG_MACFILTER_DEBUG
+#CFLAGS+= -DNG_MACFILTER_DEBUG_RECVDATA
diff --git a/sys/netgraph/ng_macfilter.c b/sys/netgraph/ng_macfilter.c
new file mode 100644
index 000000000000..622575504f01
--- /dev/null
+++ b/sys/netgraph/ng_macfilter.c
@@ -0,0 +1,878 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+ *
+ * Copyright (c) 2002 Ericsson Research & Pekka Nikander
+ * Copyright (c) 2020 Nick Hibma <n_hibma@FreeBSD.org>
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+/* 
+ * MACFILTER NETGRAPH NODE TYPE
+ *
+ * This node type routes packets from the ether hook to either the default hook
+ * if sender MAC address is not in the MAC table, or out over the specified
+ * hook if it is.
+ *
+ * Other node types can then be used to apply specific processing to the
+ * packets on each hook.
+ *
+ * If compiled with NG_MACFILTER_DEBUG the flow and resizing of the MAC table
+ * are logged to the console.
+ *
+ * If compiled with NG_MACFILTER_DEBUG_RECVDATA every packet handled is logged
+ * on the console.
+ */
+
+#include <sys/param.h>
+#include <sys/ctype.h>
+#include <sys/systm.h>
+
+#include <sys/lock.h>
+#include <sys/mbuf.h>
+#include <sys/mutex.h>
+
+#include <sys/kernel.h>
+#include <sys/malloc.h>
+
+#include <sys/socket.h>
+#include <net/ethernet.h>
+
+#include <netgraph/ng_message.h>
+#include <netgraph/netgraph.h>
+#include <netgraph/ng_parse.h>
+
+#include "ng_macfilter.h"
+
+#ifdef NG_SEPARATE_MALLOC
+MALLOC_DEFINE(M_NETGRAPH_MACFILTER, "netgraph_macfilter", "netgraph macfilter node ");
+#else
+#define M_NETGRAPH_MACFILTER M_NETGRAPH
+#endif
+
+#define MACTABLE_BLOCKSIZE      128      /* block size for incrementing table */
+
+#ifdef NG_MACFILTER_DEBUG
+#define DEBUG(fmt, ...)         printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, __VA_ARGS__)
+#else
+#define DEBUG(fmt, ...)
+#endif
+#define MAC_FMT                 "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAC_S_ARGS(v)           (v)[0], (v)[1], (v)[2], (v)[3], (v)[4], (v)[5]
+
+/*
+ * Parse type for struct ngm_macfilter_direct 
+ */
+
+static const struct ng_parse_struct_field macfilter_direct_fields[]
+        = NGM_MACFILTER_DIRECT_FIELDS;
+static const struct ng_parse_type ng_macfilter_direct_type = {
+    &ng_parse_struct_type,
+    &macfilter_direct_fields
+};
+
+/*
+ * Parse type for struct ngm_macfilter_direct_hookid.
+ */
+
+static const struct ng_parse_struct_field macfilter_direct_ndx_fields[]
+        = NGM_MACFILTER_DIRECT_NDX_FIELDS;
+static const struct ng_parse_type ng_macfilter_direct_hookid_type = {
+    &ng_parse_struct_type,
+    &macfilter_direct_ndx_fields
+};
+
+/*
+ * Parse types for struct ngm_macfilter_get_macs.
+ */
+static int
+macfilter_get_macs_count(const struct ng_parse_type *type,
+    const u_char *start, const u_char *buf)
+{
+	const struct ngm_macfilter_macs *const ngm_macs =
+	    (const struct ngm_macfilter_macs *)(buf - OFFSETOF(struct ngm_macfilter_macs, macs));
+
+	return ngm_macs->n;
+}
+static const struct ng_parse_struct_field ng_macfilter_mac_fields[]
+        = NGM_MACFILTER_MAC_FIELDS;
+static const struct ng_parse_type ng_macfilter_mac_type = {
+    &ng_parse_struct_type,
+    ng_macfilter_mac_fields,
+};
+static const struct ng_parse_array_info ng_macfilter_macs_array_info = {
+    &ng_macfilter_mac_type,
+    macfilter_get_macs_count
+};
+static const struct ng_parse_type ng_macfilter_macs_array_type = {
+    &ng_parse_array_type,
+    &ng_macfilter_macs_array_info
+};
+static const struct ng_parse_struct_field ng_macfilter_macs_fields[]
+        = NGM_MACFILTER_MACS_FIELDS;
+static const struct ng_parse_type ng_macfilter_macs_type = {
+    &ng_parse_struct_type,
+    &ng_macfilter_macs_fields
+};
+
+/*
+ * Parse types for struct ngm_macfilter_get_hooks.
+ */
+static int
+macfilter_get_upper_hook_count(const struct ng_parse_type *type,
+    const u_char *start, const u_char *buf)
+{
+	const struct ngm_macfilter_hooks *const ngm_hooks =
+	    (const struct ngm_macfilter_hooks *)(buf - OFFSETOF(struct ngm_macfilter_hooks, hooks));
+
+        DEBUG("buf %p, ngm_hooks %p, n %d", buf, ngm_hooks, ngm_hooks->n);
+
+	return ngm_hooks->n;
+}
+
+static const struct ng_parse_struct_field ng_macfilter_hook_fields[]
+        = NGM_MACFILTER_HOOK_FIELDS;
+static const struct ng_parse_type ng_macfilter_hook_type = {
+    &ng_parse_struct_type,
+    ng_macfilter_hook_fields,
+};
+static const struct ng_parse_array_info ng_macfilter_hooks_array_info = {
+    &ng_macfilter_hook_type,
+    macfilter_get_upper_hook_count   
+};
+static const struct ng_parse_type ng_macfilter_hooks_array_type = {
+    &ng_parse_array_type,
+    &ng_macfilter_hooks_array_info
+};
+static const struct ng_parse_struct_field ng_macfilter_hooks_fields[]
+        = NGM_MACFILTER_HOOKS_FIELDS;
+static const struct ng_parse_type ng_macfilter_hooks_type = {
+    &ng_parse_struct_type,
+    &ng_macfilter_hooks_fields
+};
+
+/*
+ * List of commands and how to convert arguments to/from ASCII 
+ */
+static const struct ng_cmdlist ng_macfilter_cmdlist[] = {
+    {
+	NGM_MACFILTER_COOKIE,
+	NGM_MACFILTER_RESET,
+	"reset",
+	NULL,
+	NULL
+    },
+    {
+	NGM_MACFILTER_COOKIE,
+	NGM_MACFILTER_DIRECT,
+	"direct",
+	&ng_macfilter_direct_type,
+	NULL
+    },
+    {
+	NGM_MACFILTER_COOKIE,
+	NGM_MACFILTER_DIRECT_HOOKID,
+	"directi",
+	&ng_macfilter_direct_hookid_type,
+	NULL
+    },
+    {
+        NGM_MACFILTER_COOKIE,
+        NGM_MACFILTER_GET_MACS,
+        "getmacs",
+        NULL,
+        &ng_macfilter_macs_type
+    },
+    {
+        NGM_MACFILTER_COOKIE,
+        NGM_MACFILTER_GETCLR_MACS,
+        "getclrmacs",
+        NULL,
+        &ng_macfilter_macs_type
+    },
+    {
+        NGM_MACFILTER_COOKIE,
+        NGM_MACFILTER_CLR_MACS,
+        "clrmacs",
+        NULL,
+        NULL,
+    },
+    {
+        NGM_MACFILTER_COOKIE,
+        NGM_MACFILTER_GET_HOOKS,
+        "gethooks",
+        NULL,
+        &ng_macfilter_hooks_type
+    },
+    { 0 }
+};
+
+/*
+ * Netgraph node type descriptor 
+ */
+static ng_constructor_t	ng_macfilter_constructor;
+static ng_rcvmsg_t	ng_macfilter_rcvmsg;
+static ng_shutdown_t	ng_macfilter_shutdown;
+static ng_newhook_t	ng_macfilter_newhook;
+static ng_rcvdata_t	ng_macfilter_rcvdata;
+static ng_disconnect_t	ng_macfilter_disconnect;
+
+static struct ng_type typestruct = {
+    .version =     NG_ABI_VERSION,
+    .name =	   NG_MACFILTER_NODE_TYPE,
+    .constructor = ng_macfilter_constructor,
+    .rcvmsg =	   ng_macfilter_rcvmsg,
+    .shutdown =	   ng_macfilter_shutdown,
+    .newhook =	   ng_macfilter_newhook,
+    .rcvdata =	   ng_macfilter_rcvdata,
+    .disconnect =  ng_macfilter_disconnect,
+    .cmdlist =	   ng_macfilter_cmdlist
+};
+NETGRAPH_INIT(macfilter, &typestruct);
+
+/* 
+ * Per MAC address info: the hook where to send to, the address
+ * Note: We use the same struct as in the netgraph message, so we can bcopy the
+ * array.
+ */
+typedef struct ngm_macfilter_mac *mf_mac_p;
+
+/*
+ * Node info
+ */
+typedef struct {
+    hook_p     mf_ether_hook;	/* Ethernet hook */
+
+    hook_p     *mf_upper;       /* Upper hooks */
+    u_int      mf_upper_cnt;    /* Allocated # of upper slots */
+
+    struct mtx mtx;             /* Mutex for MACs table */
+    mf_mac_p   mf_macs;		/* MAC info: dynamically allocated */
+    u_int      mf_mac_allocated;/* Allocated # of MAC slots */
+    u_int      mf_mac_used;	/* Used # of MAC slots */
+} *macfilter_p;
+
+/*
+ * Resize the MAC table to accommodate at least mfp->mf_mac_used + 1 entries.
+ *
+ * Note: mtx already held
+ */
+static int
+macfilter_mactable_resize(macfilter_p mfp)
+{
+    int error = 0;
+
+    int n = mfp->mf_mac_allocated;
+    if (mfp->mf_mac_used < 2*MACTABLE_BLOCKSIZE-1)                              /* minimum size */
+        n = 2*MACTABLE_BLOCKSIZE-1;
+    else if (mfp->mf_mac_used + 2*MACTABLE_BLOCKSIZE < mfp->mf_mac_allocated)   /* reduce size */
+        n = mfp->mf_mac_allocated - MACTABLE_BLOCKSIZE;
+    else if (mfp->mf_mac_used == mfp->mf_mac_allocated)                         /* increase size */
+        n = mfp->mf_mac_allocated + MACTABLE_BLOCKSIZE;
+
+    if (n != mfp->mf_mac_allocated) {
+        DEBUG("used=%d allocated=%d->%d",
+              mfp->mf_mac_used, mfp->mf_mac_allocated, n);
+        
+        mf_mac_p mfp_new = realloc(mfp->mf_macs,
+                sizeof(mfp->mf_macs[0])*n,
+                M_NETGRAPH, M_NOWAIT | M_ZERO);
+        if (mfp_new == NULL) {
+            error = -1;
+        } else {
+            mfp->mf_macs = mfp_new;
+            mfp->mf_mac_allocated = n;
+        }
+    }
+
+    return error;
+}
+
+/*
+ * Resets the macfilter to pass all received packets
+ * to the default hook.
+ *
+ * Note: mtx already held
+ */
+static void
+macfilter_reset(macfilter_p mfp)
+{
+    mfp->mf_mac_used = 0;
+
+    macfilter_mactable_resize(mfp);
+}
+
+/*
+ * Resets the counts for each MAC address.
+ *
+ * Note: mtx already held
+ */
+static void
+macfilter_reset_stats(macfilter_p mfp)
+{
+    int i;
+
+    for (i = 0; i < mfp->mf_mac_used; i++) {
+        mf_mac_p p = &mfp->mf_macs[i];
+        p->packets_in = p->packets_out = 0;
+        p->bytes_in = p->bytes_out = 0;
+    }
+}
+
+/*
+ * Count the number of matching macs routed to this hook.
+ * 
+ * Note: mtx already held
+ */
+static int
+macfilter_mac_count(macfilter_p mfp, int hookid)
+{
+    int i;
+    int cnt = 0;
+
+    for (i = 0; i < mfp->mf_mac_used; i++)
+        if (mfp->mf_macs[i].hookid == hookid)
+            cnt++;
+
+    return cnt;
+}
+
+/*
+ * Find a MAC address in the mac table.
+ *
+ * Returns 0 on failure with *ri set to index before which to insert a new
+ * element. Or returns 1 on success with *ri set to the index of the element
+ * that matches. 
+ * 
+ * Note: mtx already held.
+ */
+static u_int
+macfilter_find_mac(macfilter_p mfp, const u_char *ether, u_int *ri)
+{
+    mf_mac_p mf_macs = mfp->mf_macs;
+
+    u_int base = 0;
+    u_int range = mfp->mf_mac_used;
+    while (range > 0) {
+        u_int middle = base + (range >> 1);             /* middle */
+	int d = bcmp(ether, mf_macs[middle].ether, ETHER_ADDR_LEN);
+	if (d == 0) {   	                        /* match */
+            *ri = middle;
+            return 1;
+	} else if (d > 0) {                             /* move right */
+            range -= middle - base + 1;
+            base = middle + 1;
+	} else {                                        /* move left */
+            range = middle - base;
+        }
+    }
+
+    *ri = base;
+    return 0;
+}
+
+/*
+ * Change the upper hook for the given MAC address. If the hook id is zero (the
+ * default hook), the MAC address is removed from the table. Otherwise it is
+ * inserted to the table at a proper location, and the id of the hook is
+ * marked.
+ * 
+ * Note: mtx already held.
+ */
+static int
+macfilter_mactable_change(macfilter_p mfp, u_char *ether, int hookid)
+{
+    u_int i;
+    int found = macfilter_find_mac(mfp, ether, &i);
+
+    mf_mac_p mf_macs = mfp->mf_macs;
+
+    DEBUG("ether=" MAC_FMT " found=%d i=%d ether=" MAC_FMT " hookid=%d->%d used=%d allocated=%d",
+          MAC_S_ARGS(ether), found, i, MAC_S_ARGS(mf_macs[i].ether),
+          (found? mf_macs[i].hookid:NG_MACFILTER_HOOK_DEFAULT_ID), hookid,
+          mfp->mf_mac_used, mfp->mf_mac_allocated);
+
+    if (found) {
+        if (hookid == NG_MACFILTER_HOOK_DEFAULT_ID) {   /* drop */
+            /* Compress table */
+            mfp->mf_mac_used--;
+            size_t len = (mfp->mf_mac_used - i) * sizeof(mf_macs[0]);
+            if (len > 0)
+                bcopy(&mf_macs[i+1], &mf_macs[i], len);
+
+            macfilter_mactable_resize(mfp);
+        } else {                                        /* modify */
+            mf_macs[i].hookid = hookid;
+        }   
+    } else {
+        if (hookid == NG_MACFILTER_HOOK_DEFAULT_ID) {   /* not found */
+            /* not present and not inserted */
+            return 0;
+        } else {                                        /* add */
+            if (macfilter_mactable_resize(mfp) == -1) {
+                return ENOMEM;
+            } else {
+                mf_macs = mfp->mf_macs;                 /* reassign; might have moved during resize */
+
+                /* make room for new entry, unless appending */
+                size_t len = (mfp->mf_mac_used - i) * sizeof(mf_macs[0]);
+                if (len > 0)
+                    bcopy(&mf_macs[i], &mf_macs[i+1], len);
+
+                mf_macs[i].hookid = hookid;
+                bcopy(ether, mf_macs[i].ether, ETHER_ADDR_LEN);
+
+                mfp->mf_mac_used++;
+            }
+        }
+    }
+
+    return 0;
+}
+
+static int
+macfilter_mactable_remove_by_hookid(macfilter_p mfp, int hookid)
+{
+    int i, j;
+
+    for (i = 0, j = 0; i < mfp->mf_mac_used; i++) {
+        if (mfp->mf_macs[i].hookid != hookid) {
+            if (i != j)
+                bcopy(&mfp->mf_macs[i], &mfp->mf_macs[j], sizeof(mfp->mf_macs[0]));
+            j++;
+        }
+    }
+
+    int removed = i - j;
+    mfp->mf_mac_used = j;
+    macfilter_mactable_resize(mfp);
+
+    return removed;
+}
+
+static int
+macfilter_find_hook(macfilter_p mfp, const char *hookname)
+{
+    int hookid;
+
+    for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++) {
+	if (mfp->mf_upper[hookid]) {
+            if (strncmp(NG_HOOK_NAME(mfp->mf_upper[hookid]), 
+		    hookname, NG_HOOKSIZ) == 0) {
+                return hookid;
+            }
+        }
+    }
+
+    return 0;
+}
+
+static int
+macfilter_direct(macfilter_p mfp, struct ngm_macfilter_direct *md)
+{
+    DEBUG("ether=" MAC_FMT " hook=%s",
+        MAC_S_ARGS(md->ether), md->hookname);
+
+    int hookid = macfilter_find_hook(mfp, md->hookname);
+    if (hookid < 0)
+        return ENOENT;
+
+    return macfilter_mactable_change(mfp, md->ether, hookid);
+}
+
+static int
+macfilter_direct_hookid(macfilter_p mfp, struct ngm_macfilter_direct_hookid *mdi)
+{
+    DEBUG("ether=" MAC_FMT " hookid=%d",
+        MAC_S_ARGS(mdi->ether), mdi->hookid);
+
+    if (mdi->hookid >= mfp->mf_upper_cnt)
+	return EINVAL;
+    else if (mfp->mf_upper[mdi->hookid] == NULL)
+	return EINVAL;
+    
+    return macfilter_mactable_change(mfp, mdi->ether, mdi->hookid);
+}
+
+/*
+ * Packet handling
+ */
+
+/*
+ * Pass packets received from any upper hook to
+ * a lower hook
+ */
+static int
+macfilter_ether_output(hook_p hook, macfilter_p mfp, struct mbuf *m, hook_p *next_hook)
+{
+    struct ether_header *ether_header = mtod(m, struct ether_header *);
+    u_char *ether = ether_header->ether_dhost;
+
+    *next_hook = mfp->mf_ether_hook;
+
+    mtx_lock(&mfp->mtx);
+
+    u_int i;
+    int found = macfilter_find_mac(mfp, ether, &i);
+    if (found) {
+        mf_mac_p mf_macs = mfp->mf_macs;
+
+        mf_macs[i].packets_out++;
+        if (m->m_len > ETHER_HDR_LEN)
+            mf_macs[i].bytes_out += m->m_len - ETHER_HDR_LEN;
+    
+#ifdef NG_MACFILTER_DEBUG_RECVDATA
+        DEBUG("ether=" MAC_FMT " len=%db->%lldb: bytes: %s -> %s",
+            MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN, mf_macs[i].bytes_out,
+            NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
+#endif
+    } else {
+#ifdef NG_MACFILTER_DEBUG_RECVDATA
+        DEBUG("ether=" MAC_FMT " len=%db->?b: bytes: %s->%s",
+            MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN,
+            NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
+#endif
+    }
+
+    mtx_unlock(&mfp->mtx);
+
+    return 0;
+}
+
+/*
+ * Search for the right upper hook, based on the source ethernet 
+ * address.  If not found, pass to the default upper hook.
+ */
+static int
+macfilter_ether_input(hook_p hook, macfilter_p mfp, struct mbuf *m, hook_p *next_hook)
+{
+    struct ether_header *ether_header = mtod(m, struct ether_header *);
+    u_char *ether = ether_header->ether_shost;
+    int hookid = NG_MACFILTER_HOOK_DEFAULT_ID;
+
+    mtx_lock(&mfp->mtx);
+
+    u_int i;
+    int found = macfilter_find_mac(mfp, ether, &i);
+    if (found) {
+        mf_mac_p mf_macs = mfp->mf_macs;
+
+        mf_macs[i].packets_in++;
+        if (m->m_len > ETHER_HDR_LEN)
+            mf_macs[i].bytes_in += m->m_len - ETHER_HDR_LEN;
+
+        hookid = mf_macs[i].hookid;
+    
+#ifdef NG_MACFILTER_DEBUG_RECVDATA
+        DEBUG("ether=" MAC_FMT " len=%db->%lldb: bytes: %s->%s",
+            MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN, mf_macs[i].bytes_in,
+            NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
+#endif
+    } else {
+#ifdef NG_MACFILTER_DEBUG_RECVDATA
+        DEBUG("ether=" MAC_FMT " len=%db->?b: bytes: %s->%s",
+            MAC_S_ARGS(ether), m->m_len - ETHER_HDR_LEN,
+            NG_HOOK_NAME(hook), NG_HOOK_NAME(*next_hook));
+#endif
+    }
+
+    if (hookid >= mfp->mf_upper_cnt)
+        *next_hook = NULL;
+    else
+        *next_hook = mfp->mf_upper[hookid];
+
+    mtx_unlock(&mfp->mtx);
+
+    return 0;
+}
+
+/*
+ * ======================================================================
+ * Netgraph hooks
+ * ======================================================================
+ */
+
+/*
+ * See basic netgraph code for comments on the individual functions.
+ */
+
+static int
+ng_macfilter_constructor(node_p node)
+{
+    macfilter_p mfp = malloc(sizeof(*mfp), M_NETGRAPH, M_NOWAIT | M_ZERO);
+    if (mfp == NULL)
+	return ENOMEM;
+
+    int error = macfilter_mactable_resize(mfp);
+    if (error)
+        return error;
+
+    NG_NODE_SET_PRIVATE(node, mfp);
+
+    mtx_init(&mfp->mtx, "Macfilter table", NULL, MTX_DEF);
+
+    return (0);
+}
+
+static int
+ng_macfilter_newhook(node_p node, hook_p hook, const char *hookname)
+{
+    const macfilter_p mfp = NG_NODE_PRIVATE(node);
+
+    DEBUG("%s", hookname);
+
+    if (strcmp(hookname, NG_MACFILTER_HOOK_ETHER) == 0) {
+	mfp->mf_ether_hook = hook;
+    } else {
+        int hookid;
+        if (strcmp(hookname, NG_MACFILTER_HOOK_DEFAULT) == 0) {
+            hookid = NG_MACFILTER_HOOK_DEFAULT_ID;
+        } else {
+            for (hookid = 1; hookid < mfp->mf_upper_cnt; hookid++)
+                if (mfp->mf_upper[hookid] == NULL)
+                    break;
+        }
+
+        if (hookid >= mfp->mf_upper_cnt) {
+            DEBUG("upper cnt %d -> %d", mfp->mf_upper_cnt, hookid + 1);
+
+            mfp->mf_upper_cnt = hookid + 1;
+            mfp->mf_upper = realloc(mfp->mf_upper,
+                    sizeof(mfp->mf_upper[0])*mfp->mf_upper_cnt,
+                    M_NETGRAPH, M_NOWAIT | M_ZERO);
+        }
+
+        mfp->mf_upper[hookid] = hook;
+    }
+
+    return(0);
+}
+
+static int
+ng_macfilter_rcvmsg(node_p node, item_p item, hook_p lasthook)
+{
+    const macfilter_p mfp = NG_NODE_PRIVATE(node);
+    struct ng_mesg *resp = NULL;
+    struct ng_mesg *msg;
+    int error = 0;
+    struct ngm_macfilter_macs *ngm_macs;
+    struct ngm_macfilter_hooks *ngm_hooks;
+    struct ngm_macfilter_direct *md;
+    struct ngm_macfilter_direct_hookid *mdi;
+    int n = 0, i = 0;
+    int hookid = 0;
+    int resplen;
+
+    NGI_GET_MSG(item, msg);
+
+    mtx_lock(&mfp->mtx);
+
+    switch (msg->header.typecookie) {
+    case NGM_MACFILTER_COOKIE: 
+	switch (msg->header.cmd) {
+
+	case NGM_MACFILTER_RESET:
+	    macfilter_reset(mfp);
+	    break;
+
+	case NGM_MACFILTER_DIRECT:
+	    if (msg->header.arglen != sizeof(struct ngm_macfilter_direct)) {
+		DEBUG("direct: wrong type length (%d, expected %d)",
+                      msg->header.arglen, sizeof(struct ngm_macfilter_direct));
+		error = EINVAL;
+		break;
+	    }
+            md = (struct ngm_macfilter_direct *)msg->data;
+	    error = macfilter_direct(mfp, md);
+	    break;
+	case NGM_MACFILTER_DIRECT_HOOKID:
+	    if (msg->header.arglen != sizeof(struct ngm_macfilter_direct_hookid)) {
+		DEBUG("direct hookid: wrong type length (%d, expected %d)",
+                      msg->header.arglen, sizeof(struct ngm_macfilter_direct));
+		error = EINVAL;
+		break;
+	    }
+            mdi = (struct ngm_macfilter_direct_hookid *)msg->data;
+	    error = macfilter_direct_hookid(mfp, mdi);
+	    break;
+
+        case NGM_MACFILTER_GET_MACS:
+        case NGM_MACFILTER_GETCLR_MACS:
+            n = mfp->mf_mac_used;
+            resplen = sizeof(struct ngm_macfilter_macs) + n * sizeof(struct ngm_macfilter_mac);
+            NG_MKRESPONSE(resp, msg, resplen, M_NOWAIT);
+            if (resp == NULL) {
+                error = ENOMEM;
+                break;
+            }
+            ngm_macs = (struct ngm_macfilter_macs *)resp->data;
+            ngm_macs->n = n;
+            bcopy(mfp->mf_macs, &ngm_macs->macs[0], n * sizeof(struct ngm_macfilter_mac));
+
+            if (msg->header.cmd == NGM_MACFILTER_GETCLR_MACS)
+                macfilter_reset_stats(mfp);
+            break;
+
+        case NGM_MACFILTER_CLR_MACS:
+            macfilter_reset_stats(mfp);
+            break;
+
+        case NGM_MACFILTER_GET_HOOKS:
+            for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++)
+                if (mfp->mf_upper[hookid] != NULL)
+                    n++;
+            resplen = sizeof(struct ngm_macfilter_hooks) + n * sizeof(struct ngm_macfilter_hook);
+            NG_MKRESPONSE(resp, msg, resplen, M_NOWAIT | M_ZERO);
+            if (resp == NULL) {
+                error = ENOMEM;
+                break;
+            }
+
+            ngm_hooks = (struct ngm_macfilter_hooks *)resp->data;
+            ngm_hooks->n = n;
+            for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++) {
+                if (mfp->mf_upper[hookid] != NULL) {
+                    struct ngm_macfilter_hook *ngm_hook = &ngm_hooks->hooks[i++];
+                    strlcpy(ngm_hook->hookname,
+                            NG_HOOK_NAME(mfp->mf_upper[hookid]),
+                            NG_HOOKSIZ);
+                    ngm_hook->hookid = hookid;
+                    ngm_hook->maccnt = macfilter_mac_count(mfp, hookid);
+                }
+            }
+            break;
+
+	default:
+	    error = EINVAL;		/* unknown command */
+	    break;
+	}
+	break;
+
+    default:
+	error = EINVAL;			/* unknown cookie type */
+	break;
+    }
+
+    mtx_unlock(&mfp->mtx);
+
+    NG_RESPOND_MSG(error, node, item, resp);
+    NG_FREE_MSG(msg);
+
+    return error;
+}
+
+static int
+ng_macfilter_rcvdata(hook_p hook, item_p item)
+{
+    const macfilter_p mfp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
+    int error;
+    hook_p next_hook = NULL;
+    struct mbuf *m;
+
+    m = NGI_M(item);	/* 'item' still owns it. We are peeking */
+    DEBUG("%s", NG_HOOK_NAME(hook));
+
+    if (hook == mfp->mf_ether_hook)
+	error = macfilter_ether_input(hook, mfp, m, &next_hook);
+    else if (mfp->mf_ether_hook != NULL)
+	error = macfilter_ether_output(hook, mfp, m, &next_hook);
+
+    if (next_hook == NULL) {
+        NG_FREE_ITEM(item);
+        return (0);
+    }
+
+    NG_FWD_ITEM_HOOK(error, item, next_hook);
+
+    return error;
+}
+
+static int
+ng_macfilter_disconnect(hook_p hook)
+{
+    const macfilter_p mfp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
+
+    mtx_lock(&mfp->mtx);
+
+    if (mfp->mf_ether_hook == hook) {
+        mfp->mf_ether_hook = NULL;
+
+        DEBUG("%s", NG_HOOK_NAME(hook));
+    } else {
+        int hookid;
+
+        for (hookid = 0; hookid < mfp->mf_upper_cnt; hookid++) {
+            if (mfp->mf_upper[hookid] == hook) {
+                mfp->mf_upper[hookid] = NULL;
+
+#ifndef NG_MACFILTER_DEBUG
+                macfilter_mactable_remove_by_hookid(mfp, hookid);
+#else
+                int cnt = macfilter_mactable_remove_by_hookid(mfp, hookid);
+
+                DEBUG("%s: removed %d MACs", NG_HOOK_NAME(hook), cnt);
+#endif
+                break;
+            }
+        }
+
+        if (hookid == mfp->mf_upper_cnt - 1) {
+            /* Reduce the size of the array when the last element was removed */
+            for (--hookid; hookid >= 0 && mfp->mf_upper[hookid] == NULL; hookid--)
+                ;
+
+            DEBUG("upper cnt %d -> %d", mfp->mf_upper_cnt, hookid + 1);
+            mfp->mf_upper_cnt = hookid + 1;
+            mfp->mf_upper = realloc(mfp->mf_upper,
+                    sizeof(mfp->mf_upper[0])*mfp->mf_upper_cnt,
+                    M_NETGRAPH, M_NOWAIT | M_ZERO);
+        }
+    }
+
+    mtx_unlock(&mfp->mtx);
+
+    if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0)
+	&& (NG_NODE_IS_VALID(NG_HOOK_NODE(hook)))) {
+	ng_rmnode_self(NG_HOOK_NODE(hook));
+    }
+
+    return (0);
+}
+
+static int
+ng_macfilter_shutdown(node_p node)
+{
+    const macfilter_p mfp = NG_NODE_PRIVATE(node);
+
+    mtx_destroy(&mfp->mtx);
+    free(mfp->mf_upper, M_NETGRAPH);
+    free(mfp->mf_macs, M_NETGRAPH);
+    free(mfp, M_NETGRAPH);
+
+    NG_NODE_UNREF(node);
+
+    return (0);
+}
diff --git a/sys/netgraph/ng_macfilter.h b/sys/netgraph/ng_macfilter.h
new file mode 100644
index 000000000000..b6a01a2f4088
--- /dev/null
+++ b/sys/netgraph/ng_macfilter.h
@@ -0,0 +1,132 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+ *
+ * Copyright (c) 2002 Ericsson Research & Pekka Nikander
+ * Copyright (c) 2020 Nick Hibma <n_hibma@FreeBSD.org>
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _NETGRAPH_MACFILTER_H_
+#define _NETGRAPH_MACFILTER_H_
+
+#define NG_MACFILTER_NODE_TYPE		"macfilter"
+#define NGM_MACFILTER_COOKIE 		1042445461
+
+/* Hook names */
+#define NG_MACFILTER_HOOK_ETHER		"ether"         /* connected to ether:lower */
+#define NG_MACFILTER_HOOK_DEFAULT 	"default"       /* connected to ether:upper; upper[0] */
+/* Other hooks may be named freely                         connected to ether:upper; upper[1..n]*/
+#define NG_MACFILTER_HOOK_DEFAULT_ID    0
+
+#define OFFSETOF(s, e) ((char *)&((s *)0)->e - (char *)((s *)0))
+
+/* Netgraph commands understood/sent by this node type */
+enum {
+    NGM_MACFILTER_RESET = 1,
+    NGM_MACFILTER_DIRECT = 2,
+    NGM_MACFILTER_DIRECT_HOOKID = 3,
+    NGM_MACFILTER_GET_MACS = 4,
+    NGM_MACFILTER_GETCLR_MACS = 5,
+    NGM_MACFILTER_CLR_MACS = 6,
+    NGM_MACFILTER_GET_HOOKS = 7
+};
+
+/* This structure is supplied with the NGM_MACFILTER_DIRECT command */
+struct ngm_macfilter_direct {
+    u_char	ether[ETHER_ADDR_LEN];  	/* MAC address */
+    u_char	hookname[NG_HOOKSIZ];   	/* Upper hook name*/
+};
+#define NGM_MACFILTER_DIRECT_FIELDS {                   \
+    { "ether",          &ng_parse_enaddr_type },        \
+    { "hookname",       &ng_parse_hookbuf_type },       \
+    { NULL }                                            \
+}
+
+/* This structure is supplied with the NGM_MACFILTER_DIRECT_HOOKID command */
+struct ngm_macfilter_direct_hookid {
+    u_char	ether[ETHER_ADDR_LEN];  	/* MAC address */
+    u_int16_t	hookid;		        	/* Upper hook hookid */
+};
+#define NGM_MACFILTER_DIRECT_NDX_FIELDS {               \
+    { "ether",          &ng_parse_enaddr_type },        \
+    { "hookid",         &ng_parse_uint16_type },        \
+    { NULL }                                            \
+}
+
+/* This structure is returned in the array by the NGM_MACFILTER_GET(CLR)_MACS commands */
+struct ngm_macfilter_mac {
+    u_char	ether[ETHER_ADDR_LEN];  	/* MAC address */
+    u_int16_t	hookid;		        	/* Upper hook hookid */
+    u_int64_t	packets_in;			/* packets in from downstream */
+    u_int64_t	bytes_in;			/* bytes in from upstream */
+    u_int64_t	packets_out;			/* packets out towards downstream */
+    u_int64_t	bytes_out;			/* bytes out towards downstream */
+};
+#define NGM_MACFILTER_MAC_FIELDS {                      \
+    { "ether",          &ng_parse_enaddr_type },        \
+    { "hookid",         &ng_parse_uint16_type },        \
+    { "packets_in",	&ng_parse_uint64_type },        \
+    { "bytes_in",  	&ng_parse_uint64_type },        \
+    { "packets_out",    &ng_parse_uint64_type },        \
+    { "bytes_out",      &ng_parse_uint64_type },        \
+    { NULL }                                            \
+}
+/* This structure is returned by the NGM_MACFILTER_GET(CLR)_MACS commands */
+struct ngm_macfilter_macs {
+    u_int32_t   n;                              /* Number of entries in macs */
+    struct ngm_macfilter_mac macs[];            /* Macs table */
+};
+#define NGM_MACFILTER_MACS_FIELDS {                     \
+    { "n",              &ng_parse_uint32_type },        \
+    { "macs",           &ng_macfilter_macs_array_type },\
+    { NULL }                                            \
+}
+
+/* This structure is returned in an array by the NGM_MACFILTER_GET_HOOKS command */
+struct ngm_macfilter_hook {
+    u_char	hookname[NG_HOOKSIZ];   	/* Upper hook name*/
+    u_int16_t	hookid;		        	/* Upper hook hookid */
+    u_int32_t   maccnt;                         /* Number of mac addresses associated with hook */
+};
+#define NGM_MACFILTER_HOOK_FIELDS {                     \
+    { "hookname",       &ng_parse_hookbuf_type },       \
+    { "hookid",         &ng_parse_uint16_type },        \
+    { "maccnt",         &ng_parse_uint32_type },        \
+    { NULL }                                            \
+}
+/* This structure is returned by the NGM_MACFILTER_GET_HOOKS command */
+struct ngm_macfilter_hooks {
+    u_int32_t   n;                              /* Number of entries in hooks */
+    struct ngm_macfilter_hook hooks[];          /* Hooks table */
+};
+#define NGM_MACFILTER_HOOKS_FIELDS {                     \
+    { "n",              &ng_parse_uint32_type },         \
+    { "hooks",          &ng_macfilter_hooks_array_type },\
+    { NULL }                                             \
+}
+
+#endif /* _NETGRAPH_MACFILTER_H_ */