/*      $NetBSD: qop.c,v 1.13 2024/12/24 08:35:28 ozaki-r Exp $ */
/*      $KAME: qop.c,v 1.11 2001/10/26 04:57:59 kjc Exp $       */
/*
 * Copyright (C) 1999-2000
 *      Sony Computer Science Laboratories, Inc.  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, 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 SONY CSL 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 SONY CSL 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.
 */

#include <sys/param.h>
#include <sys/socket.h>
#include <sys/sockio.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <sys/stat.h>
#if defined(__FreeBSD__) && (__FreeBSD_version > 300000)
#include <sys/linker.h>
#endif

#include <net/if.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stddef.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include <err.h>
#include <syslog.h>

#include <altq/altq.h>
#include <altq/altq_red.h>
#include <altq/altq_rio.h>
#include <altq/altq_cdnr.h>
#include "altq_qop.h"
#include "qop_cdnr.h"

#define ALTQ_DEVICE     "/dev/altq/altq"
#define RED_DEVICE      "/dev/altq/red"
#define RIO_DEVICE      "/dev/altq/rio"
#define CDNR_DEVICE     "/dev/altq/cdnr"

#ifndef LIST_HEAD_INITIALIZER
#define LIST_HEAD_INITIALIZER(head)     { NULL }
#endif

/*
 * token bucket regulator information
 */
struct tbrinfo {
        LIST_ENTRY(tbrinfo) link;
        char    ifname[IFNAMSIZ];       /* if name, e.g. "en0" */
        struct tb_profile tb_prof, otb_prof;
        int installed;
};

/*
 * Static globals
 */
/* a list of configured interfaces */
LIST_HEAD(qop_iflist, ifinfo)   qop_iflist = LIST_HEAD_INITIALIZER(&iflist);
/* a list of configured token bucket regulators */
LIST_HEAD(tbr_list, tbrinfo)    tbr_list = LIST_HEAD_INITIALIZER(&tbr_list);

/*
 * internal functions
 */
static int get_ifmtu(const char *);
static void tbr_install(const char *);
static void tbr_deinstall(const char *);
static int add_filter_rule(struct ifinfo *, struct fltrinfo *,
                           struct fltrinfo **);
static int remove_filter_rule(struct ifinfo *,
                              struct fltrinfo *);
static int filt_check_relation(struct flow_filter *, struct flow_filter *);
static int filt_disjoint(struct flow_filter *, struct flow_filter *);
static int filt_subset(struct flow_filter *, struct flow_filter *);

/*
 * QCMD (Queue Command) API
 */
int
qcmd_init(void)
{
        int error;

        /* read config file and execute commands */
        error = qcmd_config();
        if (error != 0)
                return (error);

        error = qcmd_enableall();
        if (error != 0)
                LOG(LOG_ERR, errno, "%s: qcmd_init failed", qoperror(error));
        return (error);
}

int
qcmd_enable(const char *ifname)
{
        struct ifinfo   *ifinfo;
        int error = 0;

        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                error = QOPERR_BADIF;

        if (error == 0)
                error = qop_enable(ifinfo);

        if (error == 0) {
                LOG(LOG_INFO, 0, "%s enabled on interface %s (mtu:%d)",
                    ifinfo->qdisc->qname, ifname, ifinfo->ifmtu);
        } else
                LOG(LOG_ERR, errno, "%s: enable failed!", qoperror(error));
        return (error);
}

int
qcmd_disable(const char *ifname)
{
        struct ifinfo   *ifinfo;
        int error = 0;

        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                error = QOPERR_BADIF;

        if (error == 0)
                error = qop_disable(ifinfo);

        if (error != 0)
                LOG(LOG_ERR, errno, "%s: disable failed!", qoperror(error));
        return (error);
}

int
qcmd_enableall()
{
        struct ifinfo   *ifinfo;
        int error;

        LIST_FOREACH(ifinfo, &qop_iflist, next) {
                if ((error = qop_enable(ifinfo)) != 0)
                        return (error);
                LOG(LOG_INFO, 0, "%s enabled on interface %s (mtu:%d)",
                    ifinfo->qdisc->qname, ifinfo->ifname, ifinfo->ifmtu);
        }
        return (0);
}

int
qcmd_disableall()
{
        struct ifinfo   *ifinfo;
        int     lerr, error = 0;
        
        LIST_FOREACH(ifinfo, &qop_iflist, next)
                if ((lerr = qop_disable(ifinfo)) != 0)
                        if (error == 0)
                                error = lerr;
        return (error);
}

int
qcmd_clear(const char *ifname)
{
        struct ifinfo   *ifinfo;
        int error = 0;

        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                error = QOPERR_BADIF;
                
        if (error == 0)
                error = qop_clear(ifinfo);
        if (error != 0)
                LOG(LOG_ERR, errno, "%s: clear failed!", qoperror(error));
        return (error);
}

int
qcmd_destroyall(void)
{
        while (!LIST_EMPTY(&qop_iflist))
                (void)qop_delete_if(LIST_FIRST(&qop_iflist));
        return (0);
}

int
qcmd_restart(void)
{
        qcmd_destroyall();
        return qcmd_init();
}

int
qcmd_delete_class(const char *ifname, const char *clname)
{
        struct ifinfo           *ifinfo;
        struct classinfo        *clinfo = NULL;
        int error = 0;

        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                error = QOPERR_BADIF;

        if (error == 0 &&
            (clinfo = clname2clinfo(ifinfo, clname)) == NULL)
                error = QOPERR_BADCLASS;

        if (error == 0)
                error = qop_delete_class(clinfo);
        if (error != 0)
                LOG(LOG_ERR, errno, "%s: delete_class failed",
                    qoperror(error));
        return (error);
}

int
qcmd_add_filter(const char *ifname, const char *clname, const char *flname,
                 const struct flow_filter *fltr)
{
        struct ifinfo           *ifinfo;
        struct classinfo        *clinfo = NULL;
        int error = 0;

        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                error = QOPERR_BADIF;

        if (error == 0 &&
            (clinfo = clname2clinfo(ifinfo, clname)) == NULL) {
                /*
                 * there is no matching class.
                 * check if it is for a traffic conditioner
                 */
                if ((ifinfo = input_ifname2ifinfo(ifname)) == NULL ||
                    (clinfo = clname2clinfo(ifinfo, clname)) == NULL)
                        error = QOPERR_BADCLASS;
        }

        if (error == 0)
                error = qop_add_filter(NULL, clinfo, flname, fltr, NULL);

        if (error != 0)
                LOG(LOG_ERR, errno, "%s: add filter failed!",
                    qoperror(error));
        else if (IsDebug(DEBUG_ALTQ)) {
                LOG(LOG_DEBUG, 0, "%s: add a filter %s to class %s",
                    ifname, flname ? flname : "(null)",
                    clname ? clname : "(null)");
                print_filter(fltr);
        }
        return (error);
}

int
qcmd_delete_filter(const char *ifname, const char *clname, const char *flname)
{
        struct ifinfo           *ifinfo;
        struct classinfo        *clinfo = NULL;
        struct fltrinfo         *fltrinfo = NULL;
        int error = 0;
        
        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                error = QOPERR_BADIF;

        if (error == 0 &&
            (clinfo = clname2clinfo(ifinfo, clname)) == NULL) {
                /*
                 * there is no matching class.
                 * check if it is for a traffic conditioner
                 */
                if ((ifinfo = input_ifname2ifinfo(ifname)) == NULL ||
                    (clinfo = clname2clinfo(ifinfo, clname)) == NULL)
                error = QOPERR_BADCLASS;
        }

        if (error == 0 &&
            (fltrinfo = flname2flinfo(clinfo, flname)) == NULL)
                error = QOPERR_BADFILTER;

        if (error == 0)
                error = qop_delete_filter(fltrinfo);
        if (error != 0)
                LOG(LOG_ERR, errno, "%s: delete filter failed!",
                    qoperror(error));
        return (error);
}

int
qcmd_tbr_register(const char *ifname, uint64_t rate, u_int size)
{
        struct tbrinfo *info;

        if ((info = calloc(1, sizeof(struct tbrinfo))) == NULL)
                return (QOPERR_NOMEM);

        strlcpy(info->ifname, ifname, sizeof(info->ifname));
        info->tb_prof.rate = rate;
        info->tb_prof.depth = size;
        info->installed = 0;
        LIST_INSERT_HEAD(&tbr_list, info, link);
        return (0);
}

/*
 * QOP (Queue Operation) API
 */

int
qop_add_if(struct ifinfo **rp, const char *ifname, uint64_t bandwidth,
           struct qdisc_ops *qdisc_ops, void *if_private)
{
        struct ifinfo   *ifinfo;
        int error;

        if (ifname2ifinfo(ifname) != NULL) {
                LOG(LOG_ERR, 0, "qop_add_if: %s already exists!", ifname);
                return (QOPERR_BADIF);
        }

        if ((ifinfo = calloc(1, sizeof(struct ifinfo))) == NULL)
                return (QOPERR_NOMEM);
        ifinfo->ifname = strdup(ifname);
        ifinfo->bandwidth = bandwidth;
        ifinfo->enabled = 0;
        if (ifname[0] == '_')
                /* input interface */
                ifname += 1;
        ifinfo->ifindex = get_ifindex(ifname);
        ifinfo->ifmtu = get_ifmtu(ifname);
        if (qdisc_ops == NULL)
                ifinfo->qdisc = &nop_qdisc; /* replace syscalls by nops */
        else
                ifinfo->qdisc = qdisc_ops;
        ifinfo->private = if_private;
        LIST_INIT(&ifinfo->cllist);
        LIST_INIT(&ifinfo->fltr_rules);

        /* Link the interface info structure */
        LIST_INSERT_HEAD(&qop_iflist, ifinfo, next);

        /* install token bucket regulator, if necessary */
        tbr_install(ifname);

        /* attach the discipline to the interface */
        if ((error = (*ifinfo->qdisc->attach)(ifinfo)) != 0)
                goto err_ret;

        /* disable and clear the interface */
        if (ifinfo->qdisc->disable != NULL)
                if ((error = (*ifinfo->qdisc->disable)(ifinfo)) != 0)
                        goto err_ret;
        if (ifinfo->qdisc->clear != NULL)
                if ((error = (*ifinfo->qdisc->clear)(ifinfo)) != 0)
                        goto err_ret;

        if (rp != NULL)
                *rp = ifinfo;
        return (0);

err_ret:
        if (ifinfo != NULL) {
                LIST_REMOVE(ifinfo, next);
                if (ifinfo->ifname != NULL)
                        free(ifinfo->ifname);
                free(ifinfo);
        }
        return (error);
}

int
qop_delete_if(struct ifinfo *ifinfo)
{
        (void)qop_disable(ifinfo);
        (void)qop_clear(ifinfo);

        if (ifinfo->delete_hook != NULL)
                (*ifinfo->delete_hook)(ifinfo);

        /* remove this entry from qop_iflist */
        LIST_REMOVE(ifinfo, next);

        (void)(*ifinfo->qdisc->detach)(ifinfo);

        /* deinstall token bucket regulator, if necessary */
        tbr_deinstall(ifinfo->ifname);

        if (ifinfo->private != NULL)
                free(ifinfo->private);
        if (ifinfo->ifname != NULL)
                free(ifinfo->ifname);
        free(ifinfo);
        return (0);
}

int
qop_enable(struct ifinfo *ifinfo)
{
        int error;

        if (ifinfo->enable_hook != NULL)
                if ((error = (*ifinfo->enable_hook)(ifinfo)) != 0)
                        return (error);

        if (ifinfo->qdisc->enable != NULL)
                if ((error = (*ifinfo->qdisc->enable)(ifinfo)) != 0)
                        return (error);
        ifinfo->enabled = 1;
        return (0);
}

int
qop_disable(struct ifinfo *ifinfo)
{
        int error;

        if (ifinfo->qdisc->disable != NULL)
                if ((error = (*ifinfo->qdisc->disable)(ifinfo)) != 0)
                        return (error);
        ifinfo->enabled = 0;
        return (0);
}

int
qop_clear(struct ifinfo *ifinfo)
{
        struct classinfo        *clinfo;

        /* free all classes and filters */
        if (ifinfo->ifname[0] != '_') {
                /* output interface.  delete from leaf classes */
                while (!LIST_EMPTY(&ifinfo->cllist)) {
                        LIST_FOREACH(clinfo, &ifinfo->cllist, next) {
                                if (clinfo->child != NULL)
                                        continue;
                                qop_delete_class(clinfo);
                                /*
                                 * the list has been changed,
                                 *  restart from the head
                                 */
                                break;
                        }
                }
        } else {
                /* input interface. delete from parents */
                struct classinfo *root = get_rootclass(ifinfo);
                
                while (!LIST_EMPTY(&ifinfo->cllist)) {
                        LIST_FOREACH(clinfo, &ifinfo->cllist, next)
                                if (clinfo->parent == root) {
                                        qop_delete_cdnr(clinfo);
                                        break;
                                }
                        if (root->child != NULL)
                                qop_delete_class(root);
                }
        }
        
        /* clear the interface */
        if (ifinfo->qdisc->clear != NULL)
                return (*ifinfo->qdisc->clear)(ifinfo);
        return (0);
}

int
qop_add_class(struct classinfo **rp, const char *clname,
              struct ifinfo *ifinfo, struct classinfo *parent, 
              void *class_private)
{
        struct classinfo        *clinfo;
        int error;

        if ((clinfo = calloc(1, sizeof(*clinfo))) == NULL)
                return (QOPERR_NOMEM);

        if (clname != NULL)
                clinfo->clname = strdup(clname);
        else
                clinfo->clname = strdup("(null)");  /* dummy name */
        clinfo->ifinfo = ifinfo;
        clinfo->private = class_private;
        clinfo->parent = parent;
        clinfo->child = NULL;
        LIST_INIT(&clinfo->fltrlist);

        if ((error = (*ifinfo->qdisc->add_class)(clinfo)) != 0)
                goto err_ret;

        /* link classinfo in lists */
        LIST_INSERT_HEAD(&ifinfo->cllist, clinfo, next);

        if (parent != NULL) {
                clinfo->sibling = parent->child;
                clinfo->parent->child = clinfo;
        }

        if (rp != NULL)
                *rp = clinfo;
        return (0);

err_ret:
        if (clinfo != NULL) {
                if (clinfo->clname != NULL)
                        free(clinfo->clname);
                free(clinfo);
        }
        return (error);
}

int
qop_modify_class(struct classinfo *clinfo, void *arg)
{
        return (*clinfo->ifinfo->qdisc->modify_class)(clinfo, arg);
}

int
qop_delete_class(struct classinfo *clinfo)
{
        struct ifinfo           *ifinfo = clinfo->ifinfo;
        struct classinfo        *prev;
        int error;

        /* a class to be removed should not have a child */
        if (clinfo->child != NULL)
                return (QOPERR_CLASS_PERM);

        /* remove filters associated to this class */
        while (!LIST_EMPTY(&clinfo->fltrlist))
                (void)qop_delete_filter(LIST_FIRST(&clinfo->fltrlist));

        if (clinfo->delete_hook != NULL)
                (*clinfo->delete_hook)(clinfo);

        /* remove class info from the interface */
        LIST_REMOVE(clinfo, next);

        /* remove this class from the child list */
        if (clinfo->parent != NULL) {
                if (clinfo->parent->child == clinfo)
                        clinfo->parent->child = clinfo->sibling;
                else for (prev = clinfo->parent->child; prev->sibling != NULL;
                          prev = prev->sibling)
                        if (prev->sibling == clinfo) {
                                prev->sibling = clinfo->sibling;
                                break;
                        }
        }

        /* delete class from kernel */
        if ((error = (*ifinfo->qdisc->delete_class)(clinfo)) != 0)
                return (error);

        if (clinfo->private != NULL)
                free(clinfo->private);
        if (clinfo->clname != NULL)
                free(clinfo->clname);
        free(clinfo);
        return (0);
}

int
qop_add_filter(struct fltrinfo **rp, struct classinfo *clinfo,
                   const char *flname, const struct flow_filter *fltr,
                   struct fltrinfo **conflict)
{
        struct ifinfo   *ifinfo;
        struct fltrinfo *fltrinfo;
        int error;

        if ((fltrinfo = calloc(1, sizeof(*fltrinfo))) == NULL)
                return (QOPERR_NOMEM);

        fltrinfo->clinfo = clinfo;
        fltrinfo->fltr = *fltr;
#if 1
        /* fix this */
        fltrinfo->line_no = line_no;            /* XXX */
        fltrinfo->dontwarn = filter_dontwarn;   /* XXX */
#endif
        if (flname != NULL)
                fltrinfo->flname = strdup(flname);
        else
                fltrinfo->flname = strdup("(null)");  /* dummy name */

        /* check and save the filter */
        ifinfo = clinfo->ifinfo;
        if ((error = add_filter_rule(ifinfo, fltrinfo, conflict)) != 0)
                goto err_ret;

        /* install the filter to the kernel */
        if ((error = (*ifinfo->qdisc->add_filter)(fltrinfo)) != 0) {
                remove_filter_rule(ifinfo, fltrinfo);
                goto err_ret;
        }

        /* link fltrinfo onto fltrlist of the class */
        LIST_INSERT_HEAD(&clinfo->fltrlist, fltrinfo, next);

        if (rp != NULL)
                *rp = fltrinfo;
        return (0);

err_ret:
        if (fltrinfo != NULL) {
                if (fltrinfo->flname != NULL)
                        free(fltrinfo->flname);
                free(fltrinfo);
        }
        return (error);
}

int
qop_delete_filter(struct fltrinfo *fltrinfo)
{
        struct ifinfo           *ifinfo;
        struct classinfo        *clinfo;
        int error;

        /* remove filter info from the class */
        clinfo = fltrinfo->clinfo;
        ifinfo = clinfo->ifinfo;


        /* remove the entry from fltrlist of the class */
        LIST_REMOVE(fltrinfo, next);

        remove_filter_rule(ifinfo, fltrinfo);

        /* delete filter from kernel */
        if ((error = (*ifinfo->qdisc->delete_filter)(fltrinfo)) != 0)
                return (error);

        if (fltrinfo->flname)
                free(fltrinfo->flname);
        free(fltrinfo);
        return (0);
}

const char *
qoperror(int qoperrno)
{
        static char buf[64];
        
        if (qoperrno <= QOPERR_MAX)
                return (qop_errlist[qoperrno]);
        snprintf(buf, sizeof(buf), "unknown error %d", qoperrno);
        return (buf);
}

/*
 * misc functions
 */
struct ifinfo *
ifname2ifinfo(const char *ifname)
{
        struct ifinfo   *ifinfo;

        LIST_FOREACH(ifinfo, &qop_iflist, next)
                if (ifinfo->ifname != NULL &&
                    strcmp(ifinfo->ifname, ifname) == 0)
                        return (ifinfo);
        return (NULL);
}

struct ifinfo *
input_ifname2ifinfo(const char *ifname)
{
        struct ifinfo   *ifinfo;

        LIST_FOREACH(ifinfo, &qop_iflist, next)
                if (ifinfo->ifname[0] == '_' &&
                    strcmp(ifinfo->ifname+1, ifname) == 0)
                        return (ifinfo);
        return (NULL);
}

struct classinfo *
clname2clinfo(const struct ifinfo *ifinfo, const char *clname)
{
        struct classinfo        *clinfo;
        
        LIST_FOREACH(clinfo, &ifinfo->cllist, next)
                if (clinfo->clname != NULL &&
                    strcmp(clinfo->clname, clname) == 0)
                        return (clinfo);
        return (NULL);
}

struct classinfo *
clhandle2clinfo(struct ifinfo *ifinfo, u_long handle)
{
        struct classinfo *clinfo;

        LIST_FOREACH(clinfo, &ifinfo->cllist, next)
                if (clinfo->handle == handle)
                        return (clinfo);
        return (NULL);
}

struct fltrinfo *
flname2flinfo(const struct classinfo *clinfo, const char *flname)
{
        struct fltrinfo *fltrinfo;
        
        LIST_FOREACH(fltrinfo, &clinfo->fltrlist, next)
                if (fltrinfo->flname != NULL &&
                    strcmp(fltrinfo->flname, flname) == 0)
                        return (fltrinfo);
        return (NULL);
}

struct fltrinfo *
flhandle2fltrinfo(struct ifinfo *ifinfo, u_long handle)
{
        struct fltrinfo *fltrinfo;

        LIST_FOREACH(fltrinfo, &ifinfo->fltr_rules, nextrule)
                if (fltrinfo->handle == handle)
                        return (fltrinfo);
        return (NULL);
}

int
is_q_enabled(const char *ifname)
{
        struct ifinfo   *ifinfo;

        if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
                return (0);
        return (ifinfo->enabled);
}

/*
 * functions to walk through a class tree:
 *
 *   for (clinfo = get_rootclass(ifinfo);
 *      clinfo != NULL; clinfo = get_nextclass(clinfo)) {
 *        do_something;
 *   }
 */
struct classinfo *get_rootclass(struct ifinfo *ifinfo)
{
        struct classinfo *clinfo;

        /* find a class without parent */
        LIST_FOREACH(clinfo, &ifinfo->cllist, next)
                if (clinfo->parent == NULL)
                        return (clinfo);
        return (NULL);
}

/* return next class in the tree */
struct classinfo *get_nextclass(struct classinfo *clinfo)
{
        struct classinfo *next;

        if (clinfo->child != NULL)
                next = clinfo->child;
        else if (clinfo->sibling != NULL)
                next = clinfo->sibling;
        else {
                next = clinfo;
                while ((next = next->parent) != NULL)
                        if (next->sibling) {
                                next = next->sibling;
                                break;
                        }
        }
        return (next);
}

uint64_t
atobps(const char *s)
{
        double bandwidth;
        char *cp;
                        
        bandwidth = strtod(s, &cp);
        if (cp != NULL) {
                if (*cp == 'K' || *cp == 'k')
                        bandwidth *= 1000;
                else if (*cp == 'M' || *cp == 'm')
                        bandwidth *= 1000000;
                else if (*cp == 'G' || *cp == 'g')
                        bandwidth *= 1000000000;
        }
        if (bandwidth < 0)
                bandwidth = 0;
        return ((uint64_t)bandwidth);
}

u_long
atobytes(const char *s)
{
        double bytes;
        char *cp;
                        
        bytes = strtod(s, &cp);
        if (cp != NULL) {
                if (*cp == 'K' || *cp == 'k')
                        bytes *= 1024;
                else if (*cp == 'M' || *cp == 'm')
                        bytes *= 1024 * 1024;
                else if (*cp == 'G' || *cp == 'g')
                        bytes *= 1024 * 1024 * 1024;
        }
        if (bytes < 0)
                bytes = 0;
        return ((u_long)bytes);
}

static int 
get_ifmtu(const char *ifname)
{
        int s, mtu;
        struct ifreq ifr;
#ifdef __OpenBSD__
        struct if_data ifdata;
#endif

        mtu = 512; /* default MTU */

        if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
                return (mtu);
        strncpy(ifr.ifr_name, ifname, sizeof ifr.ifr_name);
#ifdef __OpenBSD__
        ifr.ifr_data = (caddr_t)&ifdata;
        if (ioctl(s, SIOCGIFDATA, (caddr_t)&ifr) == 0)
                mtu = ifdata.ifi_mtu;
#else
        if (ioctl(s, SIOCGIFMTU, (caddr_t)&ifr) == 0)
                mtu = ifr.ifr_mtu;
#endif
        close(s);
        return (mtu);
}

static void
tbr_install(const char *ifname)
{
        struct tbrinfo *info;
        struct tbrreq req;
        int fd;

        LIST_FOREACH(info, &tbr_list, link)
            if (strcmp(info->ifname, ifname) == 0)
                    break;
        if (info == NULL)
                return;
        if (info->tb_prof.rate == 0 || info->installed)
                return;

        /* get the current token bucket regulator */
        if ((fd = open(ALTQ_DEVICE, O_RDWR)) < 0)
                err(1, "can't open altq device");
        strncpy(req.ifname, ifname, IFNAMSIZ-1);
        if (ioctl(fd, ALTQTBRGET, &req) < 0)
                err(1, "ALTQTBRGET for interface %s", req.ifname);

        /* save the current values */
        info->otb_prof.rate = req.tb_prof.rate;
        info->otb_prof.depth = req.tb_prof.depth;
        
        /*
         * if tbr is not specified in the config file and tbr is already
         * configured, do not change.
         */
        if (req.tb_prof.rate != 0) {
                LOG(LOG_INFO, 0,
                    "tbr is already installed on %s,\n"
                    "  using the current setting (rate:%.2fM  size:%.2fK).",
                    info->ifname,
                    (double)req.tb_prof.rate/1000000.0,
                    (double)req.tb_prof.depth/1024.0);
                close (fd);
                return;
        }

        /* if the new size is not specified, use heuristics */
        if (info->tb_prof.depth == 0) {
                u_int rate, size;
                
                rate = info->tb_prof.rate;
                if (rate <= 1*1000*1000)
                        size = 1;
                else if (rate <= 10*1000*1000)
                        size = 4;
                else if (rate <= 200*1000*1000)
                        size = 8;
                else
                        size = 24;
                size = size * 1500;  /* assume the default mtu is 1500 */
                info->tb_prof.depth = size;
        }

        /* install the new tbr */
        strncpy(req.ifname, ifname, IFNAMSIZ-1);
        req.tb_prof.rate = info->tb_prof.rate;
        req.tb_prof.depth = info->tb_prof.depth;
        if (ioctl(fd, ALTQTBRSET, &req) < 0)
                err(1, "ALTQTBRSET for interface %s", req.ifname);
        LOG(LOG_INFO, 0,
            "tbr installed on %s (rate:%.2fM  size:%.2fK)",
            info->ifname,
            (double)info->tb_prof.rate/1000000.0,
            (double)info->tb_prof.depth/1024.0);
        close(fd);
        info->installed = 1;
}

static void
tbr_deinstall(const char *ifname)
{
        struct tbrinfo *info;
        struct tbrreq req;
        int fd;

        LIST_FOREACH(info, &tbr_list, link)
            if (strcmp(info->ifname, ifname) == 0)
                    break;
        if (info == NULL)
                return;

        /* if we installed tbr, restore the old values */
        if (info->installed != 0) {
                strncpy(req.ifname, ifname, IFNAMSIZ-1);
                req.tb_prof.rate = info->otb_prof.rate;
                req.tb_prof.depth = info->otb_prof.depth;
                if ((fd = open(ALTQ_DEVICE, O_RDWR)) < 0)
                        err(1, "can't open altq device");
                if (ioctl(fd, ALTQTBRSET, &req) < 0)
                        err(1, "ALTQTBRSET for interface %s", req.ifname);
                close(fd);
        }
        LIST_REMOVE(info, link);
        free(info);
}

void
print_filter(const struct flow_filter *filt)
{
        if (filt->ff_flow.fi_family == AF_INET) {
                struct in_addr in_addr;

                in_addr.s_addr = filt->ff_flow.fi_dst.s_addr;
                LOG(LOG_DEBUG, 0,
                    " Filter Dest Addr: %s (mask %#x) Port: %d",
                    inet_ntoa(in_addr), ntoh32(filt->ff_mask.mask_dst.s_addr),
                    ntoh16(filt->ff_flow.fi_dport));
                in_addr.s_addr = filt->ff_flow.fi_src.s_addr;
                LOG(LOG_DEBUG, 0,
                    "        Src Addr: %s (mask %#x) Port: %d",
                    inet_ntoa(in_addr), ntoh32(filt->ff_mask.mask_src.s_addr),
                    ntoh16(filt->ff_flow.fi_sport));
                LOG(LOG_DEBUG, 0, "        Protocol: %d TOS %#x (mask %#x)",
                    filt->ff_flow.fi_proto, filt->ff_flow.fi_tos,
                    filt->ff_mask.mask_tos);
        }
#ifdef INET6
        else if (filt->ff_flow.fi_family == AF_INET6) {
                char str1[INET6_ADDRSTRLEN], str2[INET6_ADDRSTRLEN];
                const struct flow_filter6 *sfilt6;

                sfilt6 = (const struct flow_filter6 *)filt; 
                LOG(LOG_DEBUG, 0, "Filter6 Dest Addr: %s (mask %s) Port: %d",
                    inet_ntop(AF_INET6, &sfilt6->ff_flow6.fi6_dst,
                              str1, sizeof(str1)),
                    inet_ntop(AF_INET6, &sfilt6->ff_mask6.mask6_dst,
                              str2, sizeof(str2)),
                    ntoh16(sfilt6->ff_flow6.fi6_dport));
                LOG(LOG_DEBUG, 0, "        Src Addr: %s (mask %s) Port: %d",
                    inet_ntop(AF_INET6, &sfilt6->ff_flow6.fi6_src,
                              str1, sizeof(str1)),
                    inet_ntop(AF_INET6, &sfilt6->ff_mask6.mask6_src,
                              str2, sizeof(str2)),
                    ntoh16(sfilt6->ff_flow6.fi6_sport));
                LOG(LOG_DEBUG, 0, "        Protocol: %d TCLASS %#x (mask %#x)",
                    sfilt6->ff_flow6.fi6_proto, sfilt6->ff_flow6.fi6_tclass,
                    sfilt6->ff_mask6.mask6_tclass);
        }
#endif /* INET6 */
}

/*
 * functions to check the filter-rules.
 * when a new filter is added, we check the relation to the existing filters
 * and if some inconsistency is found, produce an error or a warning message.
 *
 * filter matching is performed from the head of the list.
 * let
 *    S: a set of packets that filter s matches
 *    T: a set of packets that filter t matches
 * filter relations are:
 *   disjoint: S ^ T = empty
 *   subset:   S <= T
 *   intersect: S ^ T = not empty
 *
 * a new filter is disjoint or subset of the existing filters --> ok
 * a new filter is superset of an existing filter --> order problem
 * a new filter intersect an existing filter --> warning
 *
 * port-intersect: a special case we don't make warning
 *      - intersection is only port numbers
 *      - one specifies src port and the other specifies dst port
 * there must be no packet with well-known port numbers in
 * both src and dst ports.  so this is ok.
 */

#define FILT_DISJOINT           1
#define FILT_SUBSET             2
#define FILT_SUPERSET           3
#define FILT_INTERSECT          4
#define FILT_PORTINTERSECT      5

static int 
add_filter_rule(struct ifinfo *ifinfo, struct fltrinfo *fltrinfo,
                struct fltrinfo **conflict)
{
        struct fltrinfo *fp, *front, *back, *prev = NULL;
        int relation;

        LIST_FOREACH(fp, &ifinfo->fltr_rules, nextrule) {
                if (fp->fltr.ff_ruleno > fltrinfo->fltr.ff_ruleno) {
                        front = fp;
                        back = fltrinfo;
                        prev = fp;
                } else {
                        front = fltrinfo;
                        back = fp;
                }

                relation = filt_check_relation(&front->fltr, &back->fltr);

                switch (relation) {
                case FILT_SUBSET:
                case FILT_DISJOINT:
                        /* OK */
                        break;
                case FILT_SUPERSET:
                        if (front->dontwarn == 0 && back->dontwarn == 0)
                                LOG(LOG_ERR, 0,
                                    "filters for \"%s\" at line %d and for \"%s\" at line %d has an order problem!", 
                                    front->clinfo->clname, front->line_no,
                                    back->clinfo->clname, back->line_no);

                        if (conflict != NULL)
                                *conflict = fp;
                        return (QOPERR_FILTER_SHADOW);
                case FILT_PORTINTERSECT:
                        break;
                case FILT_INTERSECT:
                        /*
                         * if the intersecting two filters belonging to the
                         * same class, it's ok.
                         */
                        if (front->clinfo == back->clinfo)
                                break;
                        if (front->dontwarn == 0 && back->dontwarn == 0)
                                LOG(LOG_WARNING, 0,
                                    "warning: filter for \"%s\" at line %d could override filter for \"%s\" at line %d", 
                                    front->clinfo->clname, front->line_no,
                                    back->clinfo->clname, back->line_no);
                        break;
                }
        }

        if (prev == NULL)
                LIST_INSERT_HEAD(&ifinfo->fltr_rules, fltrinfo, nextrule);
        else
                LIST_INSERT_AFTER(prev, fltrinfo, nextrule);
        return (0);
}

static int 
remove_filter_rule(struct ifinfo *ifinfo, struct fltrinfo *fltrinfo)
{
        LIST_REMOVE(fltrinfo, nextrule);
        return (0);
}

static int
filt_check_relation(struct flow_filter *front, struct flow_filter *back)
{
        int rval;
        
        if (front->ff_flow.fi_family != back->ff_flow.fi_family)
                return (FILT_DISJOINT);

        if (filt_disjoint(front, back))
                return (FILT_DISJOINT);

        if ((rval = filt_subset(front, back)) == 1)
                return (FILT_SUBSET);

        if (filt_subset(back, front) == 1)
                return (FILT_SUPERSET);

        if (rval == 2)
                return (FILT_PORTINTERSECT);
        
        return (FILT_INTERSECT);
}

static int
filt_disjoint(struct flow_filter *front, struct flow_filter *back)
{
        u_int32_t mask;
        u_int8_t tosmask;
        
        if (front->ff_flow.fi_family == AF_INET) {
                if (front->ff_flow.fi_proto != 0 && back->ff_flow.fi_proto != 0
                    && front->ff_flow.fi_proto != back->ff_flow.fi_proto)
                        return (1);
                if (front->ff_flow.fi_sport != 0 && back->ff_flow.fi_sport != 0
                    && front->ff_flow.fi_sport != back->ff_flow.fi_sport)
                        return (1);
                if (front->ff_flow.fi_dport != 0 && back->ff_flow.fi_dport != 0
                    && front->ff_flow.fi_dport != back->ff_flow.fi_dport)
                        return (1);
                if (front->ff_flow.fi_gpi != 0 && back->ff_flow.fi_gpi != 0
                    && front->ff_flow.fi_gpi != back->ff_flow.fi_gpi)
                        return (1);
                if (front->ff_flow.fi_src.s_addr != 0 &&
                    back->ff_flow.fi_src.s_addr != 0) {
                        mask = front->ff_mask.mask_src.s_addr &
                                back->ff_mask.mask_src.s_addr;
                        if ((front->ff_flow.fi_src.s_addr & mask) !=
                            (back->ff_flow.fi_src.s_addr & mask))
                                return (1);
                }
                if (front->ff_flow.fi_dst.s_addr != 0 &&
                    back->ff_flow.fi_dst.s_addr != 0) {
                        mask = front->ff_mask.mask_dst.s_addr &
                                back->ff_mask.mask_dst.s_addr;
                        if ((front->ff_flow.fi_dst.s_addr & mask) !=
                            (back->ff_flow.fi_dst.s_addr & mask))
                                return (1);
                }
                if (front->ff_flow.fi_tos != 0 && back->ff_flow.fi_tos != 0) {
                        tosmask = front->ff_mask.mask_tos &
                                back->ff_mask.mask_tos;
                        if ((front->ff_flow.fi_tos & tosmask) !=
                            (back->ff_flow.fi_tos & tosmask))
                                return (1);
                }
                return (0);
        }
#ifdef INET6
        else if (front->ff_flow.fi_family == AF_INET6) {
                struct flow_filter6 *front6, *back6;
                int i;

                front6 = (struct flow_filter6 *)front;
                back6 = (struct flow_filter6 *)back;

                if (front6->ff_flow6.fi6_proto != 0 &&
                    back6->ff_flow6.fi6_proto != 0 &&
                    front6->ff_flow6.fi6_proto != back6->ff_flow6.fi6_proto)
                        return (1);
                if (front6->ff_flow6.fi6_flowlabel != 0 &&
                    back6->ff_flow6.fi6_flowlabel != 0 &&
                    front6->ff_flow6.fi6_flowlabel !=
                    back6->ff_flow6.fi6_flowlabel)
                        return (1);
                if (front6->ff_flow6.fi6_sport != 0 &&
                    back6->ff_flow6.fi6_sport != 0 &&
                    front6->ff_flow6.fi6_sport != back6->ff_flow6.fi6_sport)
                        return (1);
                if (front6->ff_flow6.fi6_dport != 0 &&
                    back6->ff_flow6.fi6_dport != 0 &&
                    front6->ff_flow6.fi6_dport != back6->ff_flow6.fi6_dport)
                        return (1);
                if (front6->ff_flow6.fi6_gpi != 0 &&
                    back6->ff_flow6.fi6_gpi != 0 &&
                    front6->ff_flow6.fi6_gpi != back6->ff_flow6.fi6_gpi)
                        return (1);
                if (!IN6_IS_ADDR_UNSPECIFIED(&front6->ff_flow6.fi6_src) &&
                    !IN6_IS_ADDR_UNSPECIFIED(&back6->ff_flow6.fi6_src)) {
                        for (i=0; i<4; i++) {
                                mask = IN6ADDR32_GET(&front6->ff_mask6.mask6_src, i)
                                        & IN6ADDR32_GET(&back6->ff_mask6.mask6_src, i);
                                if ((IN6ADDR32_GET(&front6->ff_flow6.fi6_src, i) & mask) !=
                                    (IN6ADDR32_GET(&back6->ff_flow6.fi6_src, i) & mask))
                                        return (1);
                        }
                }
                if (!IN6_IS_ADDR_UNSPECIFIED(&front6->ff_flow6.fi6_dst) &&
                    !IN6_IS_ADDR_UNSPECIFIED(&back6->ff_flow6.fi6_dst)) {
                        for (i=0; i<4; i++) {
                                mask = IN6ADDR32_GET(&front6->ff_mask6.mask6_dst, i)
                                        & IN6ADDR32_GET(&back6->ff_mask6.mask6_dst, i);
                                if ((IN6ADDR32_GET(&front6->ff_flow6.fi6_dst, i) & mask) !=
                                    (IN6ADDR32_GET(&back6->ff_flow6.fi6_dst, i) & mask))
                                return (1);
                        }
                }
                if (front6->ff_flow6.fi6_tclass != 0 &&
                    back6->ff_flow6.fi6_tclass != 0) {
                        tosmask = front6->ff_mask6.mask6_tclass &
                                back6->ff_mask6.mask6_tclass;
                        if ((front6->ff_flow6.fi6_tclass & tosmask) !=
                            (back6->ff_flow6.fi6_tclass & tosmask))
                                return (1);
                }
                return (0);
        }
#endif /* INET6 */
        return (0);
}

/*
 * check if "front" is a subset of "back".  assumes they are not disjoint
 * return value 0: not a subset
 *              1: subset
 *              2: subset except src & dst ports
 *                 (possible port-intersect)
 */
static int
filt_subset(struct flow_filter *front, struct flow_filter *back)
{
        u_int16_t srcport, dstport;
        
        if (front->ff_flow.fi_family == AF_INET) {
                if (front->ff_flow.fi_proto == 0 &&
                    back->ff_flow.fi_proto != 0)
                        return (0);
                if (front->ff_flow.fi_gpi == 0 && back->ff_flow.fi_gpi != 0)
                        return (0);
                if (front->ff_flow.fi_src.s_addr == 0) {
                        if (back->ff_flow.fi_src.s_addr != 0)
                                return (0);
                } else if (back->ff_flow.fi_src.s_addr != 0 &&
                         (~front->ff_mask.mask_src.s_addr &
                          back->ff_mask.mask_src.s_addr))
                        return (0);
                if (front->ff_flow.fi_dst.s_addr == 0) {
                        if (back->ff_flow.fi_dst.s_addr != 0)
                                return (0);
                } else if (back->ff_flow.fi_dst.s_addr != 0 &&
                         (~front->ff_mask.mask_dst.s_addr &
                          back->ff_mask.mask_dst.s_addr))
                        return (0);
                if (~front->ff_mask.mask_tos & back->ff_mask.mask_tos)
                        return (0);

                if (front->ff_flow.fi_sport == 0 &&
                    back->ff_flow.fi_sport != 0) {
                        srcport = ntohs(back->ff_flow.fi_sport);
                        dstport = ntohs(front->ff_flow.fi_dport);
                        if (dstport > 0 /* && dstport < 1024 */ &&
                            srcport > 0 /* && srcport < 1024 */)
                                return (2);
                        return (0);
                }
                if (front->ff_flow.fi_dport == 0 &&
                    back->ff_flow.fi_dport != 0) {
                        dstport = ntohs(back->ff_flow.fi_dport);
                        srcport = ntohs(front->ff_flow.fi_sport);
                        if (srcport > 0 /* && srcport < 1024 */ &&
                            dstport > 0 /* && dstport < 1024 */)
                                return (2);
                        return (0);
                }
                        
                return (1);
        }
#ifdef INET6
        else if (front->ff_flow.fi_family == AF_INET6) {
                struct flow_filter6 *front6, *back6;
                int i;
                
                front6 = (struct flow_filter6 *)front;
                back6 = (struct flow_filter6 *)back;

                if (front6->ff_flow6.fi6_proto == 0 &&
                    back6->ff_flow6.fi6_proto != 0)
                        return (0);
                if (front6->ff_flow6.fi6_flowlabel == 0 &&
                    back6->ff_flow6.fi6_flowlabel != 0)
                        return (0);
                if (front6->ff_flow6.fi6_gpi == 0 &&
                    back6->ff_flow6.fi6_gpi != 0)
                        return (0);
                
                if (IN6_IS_ADDR_UNSPECIFIED(&front6->ff_flow6.fi6_src)) {
                        if (!IN6_IS_ADDR_UNSPECIFIED(&back6->ff_flow6.fi6_src))
                                return (0);
                } else if (!IN6_IS_ADDR_UNSPECIFIED(&back6->ff_flow6.fi6_src))
                        for (i=0; i<4; i++)
                                if (~IN6ADDR32_GET(&front6->ff_mask6.mask6_src, i) &
                                    IN6ADDR32_GET(&back6->ff_mask6.mask6_src, i))
                                        return (0);
                if (IN6_IS_ADDR_UNSPECIFIED(&front6->ff_flow6.fi6_dst)) {
                        if (!IN6_IS_ADDR_UNSPECIFIED(&back6->ff_flow6.fi6_dst))
                                return (0);
                } else if (!IN6_IS_ADDR_UNSPECIFIED(&back6->ff_flow6.fi6_dst))
                        for (i=0; i<4; i++)
                                if (~IN6ADDR32_GET(&front6->ff_mask6.mask6_dst, i) &
                                    IN6ADDR32_GET(&back6->ff_mask6.mask6_dst, i))
                                        return (0);

                if (~front6->ff_mask6.mask6_tclass &
                    back6->ff_mask6.mask6_tclass)
                        return (0);

                if (front6->ff_flow6.fi6_sport == 0 &&
                    back6->ff_flow6.fi6_sport != 0) {
                        srcport = ntohs(back6->ff_flow6.fi6_sport);
                        dstport = ntohs(front6->ff_flow6.fi6_dport);
                        if (dstport > 0 /* && dstport < 1024 */ &&
                            srcport > 0 /* && srcport < 1024 */)
                                return (2);
                        return (0);
                }
                if (front6->ff_flow6.fi6_dport == 0 &&
                    back6->ff_flow6.fi6_dport != 0) {
                        dstport = ntohs(back6->ff_flow6.fi6_dport);
                        srcport = ntohs(front6->ff_flow6.fi6_sport);
                        if (srcport > 0 /* && srcport < 1024 */ &&
                            dstport > 0 /* && dstport < 1024 */)
                                return (2);
                        return (0);
                }
        }
#endif /* INET6 */
        return (1);
}


/*
 * setting RED or RIO default parameters
 */
int
qop_red_set_defaults(int th_min, int th_max, int inv_pmax)
{
        struct redparams params;
        int fd;

        if ((fd = open(RED_DEVICE, O_RDWR)) < 0) {
                LOG(LOG_ERR, errno, "RED open");
                return (QOPERR_SYSCALL);
        }

        params.th_min = th_min;
        params.th_max = th_max;
        params.inv_pmax = inv_pmax;

        if (ioctl(fd, RED_SETDEFAULTS, &params) < 0) {
                LOG(LOG_ERR, errno, "RED_SETDEFAULTS");
                (void)close(fd);
                return (QOPERR_SYSCALL);
        }

        (void)close(fd);
        return (0);
}

int
qop_rio_set_defaults(struct redparams *params)
{
        int i, fd;

        /* sanity check */
        for (i = 1; i < RIO_NDROPPREC; i++) {
                if (params[i].th_max > params[i-1].th_min)
                        LOG(LOG_WARNING, 0,
                            "warning: overlap found in RIO thresholds");
        }

        if ((fd = open(RIO_DEVICE, O_RDWR)) < 0) {
                LOG(LOG_ERR, errno, "RIO open");
                return (QOPERR_SYSCALL);
        }

        if (ioctl(fd, RIO_SETDEFAULTS, params) < 0) {
                LOG(LOG_ERR, errno, "RIO_SETDEFAULTS");
                (void)close(fd);
                return (QOPERR_SYSCALL);
        }

        (void)close(fd);
        return (0);
}

/*
 * try to load and open KLD module
 * (also check the altq device file)
 */
int
open_module(const char *dvname, int flags)
{
#if defined(__FreeBSD__) && (__FreeBSD_version > 300000)
        char modname[64], filename[MAXPATHLEN], *cp;
        int fd;
#endif
        struct stat sbuf;

        /* check if the altq device exists */
        if (stat(dvname, &sbuf) < 0) {
                LOG(LOG_ERR, errno, "can't access %s!", dvname);
                return (-1);
        }

#if defined(__FreeBSD__) && (__FreeBSD_version > 300000)
        /* turn discipline name into module name */
        strlcpy(modname, "altq_", sizeof(modname));
        if ((cp = strrchr(devname, '/')) == NULL)
                return (-1);
        strlcat(modname, cp + 1, sizeof(modname));

        /* check if the kld module exists */
        snprintf(filename, sizeof(filename), "/modules/%s.ko", modname);
        if (stat(filename, &sbuf) < 0) {
                /* module file doesn't exist */
                return (-1);
        }

        if (kldload(modname) < 0) {
                LOG(LOG_ERR, errno, "kldload %s failed!", modname);
                return (-1);
        }

        /* successfully loaded, open the device */
        LOG(LOG_INFO, 0, "kld module %s loaded", modname);
        fd = open(devname, flags);
        return (fd);
#else
        return (-1);
#endif
}