/*-
 * SPDX-License-Identifier: BSD-2-Clause
 *
 * Copyright (c) 2019-2021 IKS Service GmbH
 *
 * 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, 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.
 *
 * Author: Lutz Donnerhacke <lutz@donnerhacke.de>
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/mbuf.h>
#include <sys/malloc.h>
#include <sys/ctype.h>
#include <sys/errno.h>
#include <sys/syslog.h>
#include <sys/types.h>
#include <sys/counter.h>

#include <net/ethernet.h>

#include <netgraph/ng_message.h>
#include <netgraph/ng_parse.h>
#include <netgraph/ng_vlan_rotate.h>
#include <netgraph/netgraph.h>

/*
 * This section contains the netgraph method declarations for the
 * sample node. These methods define the netgraph 'type'.
 */

static ng_constructor_t ng_vlanrotate_constructor;
static ng_rcvmsg_t ng_vlanrotate_rcvmsg;
static ng_shutdown_t ng_vlanrotate_shutdown;
static ng_newhook_t ng_vlanrotate_newhook;
static ng_rcvdata_t ng_vlanrotate_rcvdata;
static ng_disconnect_t ng_vlanrotate_disconnect;

/* Parse type for struct ng_vlanrotate_conf */
static const struct ng_parse_struct_field ng_vlanrotate_conf_fields[] = {
        {"rot", &ng_parse_int8_type},
        {"min", &ng_parse_uint8_type},
        {"max", &ng_parse_uint8_type},
        {NULL}
};
static const struct ng_parse_type ng_vlanrotate_conf_type = {
        &ng_parse_struct_type,
        &ng_vlanrotate_conf_fields
};

/* Parse type for struct ng_vlanrotate_stat */
static struct ng_parse_fixedarray_info ng_vlanrotate_stat_hist_info = {
        &ng_parse_uint64_type,
        NG_VLANROTATE_MAX_VLANS
};
static struct ng_parse_type ng_vlanrotate_stat_hist = {
        &ng_parse_fixedarray_type,
        &ng_vlanrotate_stat_hist_info
};
static const struct ng_parse_struct_field ng_vlanrotate_stat_fields[] = {
        {"drops", &ng_parse_uint64_type},
        {"excessive", &ng_parse_uint64_type},
        {"incomplete", &ng_parse_uint64_type},
        {"histogram", &ng_vlanrotate_stat_hist},
        {NULL}
};
static struct ng_parse_type ng_vlanrotate_stat_type = {
        &ng_parse_struct_type,
        &ng_vlanrotate_stat_fields
};


/* List of commands and how to convert arguments to/from ASCII */
static const struct ng_cmdlist ng_vlanrotate_cmdlist[] = {
        {
                NGM_VLANROTATE_COOKIE,
                NGM_VLANROTATE_GET_CONF,
                "getconf",
                NULL,
                &ng_vlanrotate_conf_type,
        },
        {
                NGM_VLANROTATE_COOKIE,
                NGM_VLANROTATE_SET_CONF,
                "setconf",
                &ng_vlanrotate_conf_type,
                NULL
        },
        {
                NGM_VLANROTATE_COOKIE,
                NGM_VLANROTATE_GET_STAT,
                "getstat",
                NULL,
                &ng_vlanrotate_stat_type
        },
        {
                NGM_VLANROTATE_COOKIE,
                NGM_VLANROTATE_CLR_STAT,
                "clrstat",
                NULL,
                &ng_vlanrotate_stat_type
        },
        {
                NGM_VLANROTATE_COOKIE,
                NGM_VLANROTATE_GETCLR_STAT,
                "getclrstat",
                NULL,
                &ng_vlanrotate_stat_type
        },
        {0}
};

/* Netgraph node type descriptor */
static struct ng_type typestruct = {
        .version = NG_ABI_VERSION,
        .name = NG_VLANROTATE_NODE_TYPE,
        .constructor = ng_vlanrotate_constructor,
        .rcvmsg = ng_vlanrotate_rcvmsg,
        .shutdown = ng_vlanrotate_shutdown,
        .newhook = ng_vlanrotate_newhook,
        .rcvdata = ng_vlanrotate_rcvdata,
        .disconnect = ng_vlanrotate_disconnect,
        .cmdlist = ng_vlanrotate_cmdlist,
};
NETGRAPH_INIT(vlanrotate, &typestruct);

struct ng_vlanrotate_kernel_stats {
        counter_u64_t   drops, excessive, incomplete;
        counter_u64_t   histogram[NG_VLANROTATE_MAX_VLANS];
};

/* Information we store for each node */
struct vlanrotate {
        hook_p          original_hook;
        hook_p          ordered_hook;
        hook_p          excessive_hook;
        hook_p          incomplete_hook;
        struct ng_vlanrotate_conf conf;
        struct ng_vlanrotate_kernel_stats stats;
};
typedef struct vlanrotate *vlanrotate_p;

/*
 * Set up the private data structure.
 */
static int
ng_vlanrotate_constructor(node_p node)
{
        int i;

        vlanrotate_p vrp = malloc(sizeof(*vrp), M_NETGRAPH, M_WAITOK | M_ZERO);

        vrp->conf.max = NG_VLANROTATE_MAX_VLANS;

        vrp->stats.drops = counter_u64_alloc(M_WAITOK);
        vrp->stats.excessive = counter_u64_alloc(M_WAITOK);
        vrp->stats.incomplete = counter_u64_alloc(M_WAITOK);
        for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
                vrp->stats.histogram[i] = counter_u64_alloc(M_WAITOK);

        NG_NODE_SET_PRIVATE(node, vrp);
        return (0);
}

/*
 * Give our ok for a hook to be added.
 */
static int
ng_vlanrotate_newhook(node_p node, hook_p hook, const char *name)
{
        const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
        hook_p *dst = NULL;

        if (strcmp(name, NG_VLANROTATE_HOOK_ORDERED) == 0) {
                dst = &vrp->ordered_hook;
        } else if (strcmp(name, NG_VLANROTATE_HOOK_ORIGINAL) == 0) {
                dst = &vrp->original_hook;
        } else if (strcmp(name, NG_VLANROTATE_HOOK_EXCESSIVE) == 0) {
                dst = &vrp->excessive_hook;
        } else if (strcmp(name, NG_VLANROTATE_HOOK_INCOMPLETE) == 0) {
                dst = &vrp->incomplete_hook;
        } else
                return (EINVAL);        /* not a hook we know about */

        if (*dst != NULL)
                return (EADDRINUSE);    /* don't override */

        *dst = hook;
        return (0);
}

/*
 * Get a netgraph control message.
 * A response is not required.
 */
static int
ng_vlanrotate_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
        const vlanrotate_p vrp = NG_NODE_PRIVATE(node);
        struct ng_mesg *resp = NULL;
        struct ng_mesg *msg;
        struct ng_vlanrotate_conf *pcf;
        int error = 0;

        NGI_GET_MSG(item, msg);
        /* Deal with message according to cookie and command */
        switch (msg->header.typecookie) {
        case NGM_VLANROTATE_COOKIE:
                switch (msg->header.cmd) {
                case NGM_VLANROTATE_GET_CONF:
                        NG_MKRESPONSE(resp, msg, sizeof(vrp->conf), M_NOWAIT);
                        if (!resp) {
                                error = ENOMEM;
                                break;
                        }
                        *((struct ng_vlanrotate_conf *)resp->data) = vrp->conf;
                        break;
                case NGM_VLANROTATE_SET_CONF:
                        if (msg->header.arglen != sizeof(*pcf)) {
                                error = EINVAL;
                                break;
                        }

                        pcf = (struct ng_vlanrotate_conf *)msg->data;

                        if (pcf->max == 0)      /* keep current value */
                                pcf->max = vrp->conf.max;

                        if ((pcf->max > NG_VLANROTATE_MAX_VLANS) ||
                            (pcf->min > pcf->max) ||
                            (abs(pcf->rot) >= pcf->max)) {
                                error = EINVAL;
                                break;
                        }

                        vrp->conf = *pcf;
                        break;
                case NGM_VLANROTATE_GET_STAT:
                case NGM_VLANROTATE_GETCLR_STAT:
                {
                        struct ng_vlanrotate_stat *p;
                        int i;

                        NG_MKRESPONSE(resp, msg, sizeof(*p), M_NOWAIT);
                        if (!resp) {
                                error = ENOMEM;
                                break;
                        }
                        p = (struct ng_vlanrotate_stat *)resp->data;
                        p->drops = counter_u64_fetch(vrp->stats.drops);
                        p->excessive = counter_u64_fetch(vrp->stats.excessive);
                        p->incomplete = counter_u64_fetch(vrp->stats.incomplete);
                        for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
                                p->histogram[i] = counter_u64_fetch(vrp->stats.histogram[i]);
                        if (msg->header.cmd != NGM_VLANROTATE_GETCLR_STAT)
                                break;
                }
                case NGM_VLANROTATE_CLR_STAT:
                {
                        int i;

                        counter_u64_zero(vrp->stats.drops);
                        counter_u64_zero(vrp->stats.excessive);
                        counter_u64_zero(vrp->stats.incomplete);
                        for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
                                counter_u64_zero(vrp->stats.histogram[i]);
                        break;
                }
                default:
                        error = EINVAL; /* unknown command */
                        break;
                }
                break;
        default:
                error = EINVAL; /* unknown cookie type */
                break;
        }

        /* Take care of synchronous response, if any */
        NG_RESPOND_MSG(error, node, item, resp);
        /* Free the message and return */
        NG_FREE_MSG(msg);
        return (error);
}

/*
 * Receive data, and do rotate the vlans as desired.
 *
 * Rotating is quite complicated if the rotation offset and the number
 * of vlans are not relativly prime. In this case multiple slices need
 * to be rotated separately.
 *
 * Rotation can be additive or subtractive. Some examples:
 *  01234   5 vlans given
 *  -----
 *  34012  +2 rotate
 *  12340  +4 rotate
 *  12340  -1 rotate
 *
 * First some helper functions ...
 */

struct ether_vlan_stack_entry {
        uint16_t        proto;
        uint16_t        tag;
}               __packed;

struct ether_vlan_stack_header {
        uint8_t         dst[ETHER_ADDR_LEN];
        uint8_t         src[ETHER_ADDR_LEN];
        struct ether_vlan_stack_entry vlan_stack[1];
}               __packed;

static int
ng_vlanrotate_gcd(int a, int b)
{
        if (b == 0)
                return a;
        else
                return ng_vlanrotate_gcd(b, a % b);
}

static void
ng_vlanrotate_rotate(struct ether_vlan_stack_entry arr[], int d, int n)
{
        int             i, j, k;
        struct ether_vlan_stack_entry temp;

        /* for each commensurable slice */
        for (i = ng_vlanrotate_gcd(d, n); i-- > 0;) {
                /* rotate left aka downwards */
                temp = arr[i];
                j = i;

                while (1) {
                        k = j + d;
                        if (k >= n)
                                k = k - n;
                        if (k == i)
                                break;
                        arr[j] = arr[k];
                        j = k;
                }

                arr[j] = temp;
        }
}

static int
ng_vlanrotate_rcvdata(hook_p hook, item_p item)
{
        const vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
        struct ether_vlan_stack_header *evsh;
        struct mbuf *m = NULL;
        hook_p  dst_hook;
        int8_t  rotate;
        int8_t  vlans = 0;
        int     error = ENOSYS;

        NGI_GET_M(item, m);

        if (hook == vrp->ordered_hook) {
                rotate = +vrp->conf.rot;
                dst_hook = vrp->original_hook;
        } else if (hook == vrp->original_hook) {
                rotate = -vrp->conf.rot;
                dst_hook = vrp->ordered_hook;
        } else {
                dst_hook = vrp->original_hook;
                goto send;      /* everything else goes out unmodified */
        }

        if (dst_hook == NULL) {
                error = ENETDOWN;
                goto fail;
        }

        /* count the vlans */
        for (vlans = 0; vlans <= NG_VLANROTATE_MAX_VLANS; vlans++) {
                size_t expected_len = sizeof(struct ether_vlan_stack_header)
                    + vlans * sizeof(struct ether_vlan_stack_entry);

                if (m->m_len < expected_len) {
                        m = m_pullup(m, expected_len);
                        if (m == NULL) {
                                error = EINVAL;
                                goto fail;
                        }
                }

                evsh = mtod(m, struct ether_vlan_stack_header *);
                switch (ntohs(evsh->vlan_stack[vlans].proto)) {
                case ETHERTYPE_VLAN:
                case ETHERTYPE_QINQ:
                case ETHERTYPE_8021Q9100:
                case ETHERTYPE_8021Q9200:
                case ETHERTYPE_8021Q9300:
                        break;
                default:
                        goto out;
                }
        }
out:
        if ((vlans > vrp->conf.max) || (vlans >= NG_VLANROTATE_MAX_VLANS)) {
                counter_u64_add(vrp->stats.excessive, 1);
                dst_hook = vrp->excessive_hook;
                goto send;
        }

        if ((vlans < vrp->conf.min) || (vlans <= abs(rotate))) {
                counter_u64_add(vrp->stats.incomplete, 1);
                dst_hook = vrp->incomplete_hook;
                goto send;
        }
        counter_u64_add(vrp->stats.histogram[vlans], 1);

        /* rotating upwards always (using modular arithmetics) */
        if (rotate == 0) {
                /* nothing to do */
        } else if (rotate > 0) {
                ng_vlanrotate_rotate(evsh->vlan_stack, rotate, vlans);
        } else {
                ng_vlanrotate_rotate(evsh->vlan_stack, vlans + rotate, vlans);
        }

send:
        if (dst_hook == NULL)
                goto fail;
        NG_FWD_NEW_DATA(error, item, dst_hook, m);
        return 0;

fail:
        counter_u64_add(vrp->stats.drops, 1);
        if (m != NULL)
                m_freem(m);
        NG_FREE_ITEM(item);
        return (error);
}

/*
 * Do local shutdown processing..
 * All our links and the name have already been removed.
 */
static int
ng_vlanrotate_shutdown(node_p node)
{
        const           vlanrotate_p vrp = NG_NODE_PRIVATE(node);
        int i;

        NG_NODE_SET_PRIVATE(node, NULL);

        counter_u64_free(vrp->stats.drops);
        counter_u64_free(vrp->stats.excessive);
        counter_u64_free(vrp->stats.incomplete);
        for (i = 0; i < NG_VLANROTATE_MAX_VLANS; i++)
                counter_u64_free(vrp->stats.histogram[i]);

        free(vrp, M_NETGRAPH);

        NG_NODE_UNREF(node);
        return (0);
}

/*
 * Hook disconnection
 * For this type, removal of the last link destroys the node
 */
static int
ng_vlanrotate_disconnect(hook_p hook)
{
        const           vlanrotate_p vrp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));

        if (vrp->original_hook == hook)
                vrp->original_hook = NULL;
        if (vrp->ordered_hook == hook)
                vrp->ordered_hook = NULL;
        if (vrp->excessive_hook == hook)
                vrp->excessive_hook = NULL;
        if (vrp->incomplete_hook == hook)
                vrp->incomplete_hook = NULL;

        /* during shutdown the node is invalid, don't shutdown twice */
        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);
}