/*      $NetBSD: if_l2tp.c,v 1.49 2023/11/02 09:43:46 yamaguchi Exp $   */

/*
 * Copyright (c) 2017 Internet Initiative Japan 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 THE NETBSD FOUNDATION, INC. 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 FOUNDATION 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.
 */

/*
 * L2TPv3 kernel interface
 */

#include <sys/cdefs.h>
__KERNEL_RCSID(0, "$NetBSD: if_l2tp.c,v 1.49 2023/11/02 09:43:46 yamaguchi Exp $");

#ifdef _KERNEL_OPT
#include "opt_inet.h"
#include "opt_net_mpsafe.h"
#endif

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/mbuf.h>
#include <sys/socket.h>
#include <sys/sockio.h>
#include <sys/errno.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/syslog.h>
#include <sys/proc.h>
#include <sys/conf.h>
#include <sys/kauth.h>
#include <sys/cpu.h>
#include <sys/cprng.h>
#include <sys/intr.h>
#include <sys/kmem.h>
#include <sys/mutex.h>
#include <sys/atomic.h>
#include <sys/pserialize.h>
#include <sys/device.h>
#include <sys/module.h>

#include <net/if.h>
#include <net/if_dl.h>
#include <net/if_ether.h>
#include <net/if_types.h>
#include <net/route.h>
#include <net/bpf.h>
#include <net/if_vlanvar.h>

#include <netinet/in.h>
#include <netinet/in_systm.h>
#include <netinet/ip.h>
#include <netinet/ip_encap.h>
#ifdef  INET
#include <netinet/in_var.h>
#include <netinet/in_l2tp.h>
#endif  /* INET */
#ifdef INET6
#include <netinet6/in6_l2tp.h>
#endif

#include <net/if_l2tp.h>

#include <net/if_vlanvar.h>

/* TODO: IP_TCPMSS support */
#undef IP_TCPMSS
#ifdef IP_TCPMSS
#include <netinet/ip_tcpmss.h>
#endif

/*
 * l2tp global variable definitions
 */
static struct {
        LIST_HEAD(l2tp_sclist, l2tp_softc) list;
        kmutex_t lock;
} l2tp_softcs __cacheline_aligned;


#if !defined(L2TP_ID_HASH_SIZE)
#define L2TP_ID_HASH_SIZE 64
#endif
static struct {
        kmutex_t lock;
        struct pslist_head *lists;
        u_long mask;
} l2tp_hash __cacheline_aligned = {
        .lists = NULL,
};

pserialize_t l2tp_psz __read_mostly;
struct psref_class *lv_psref_class __read_mostly;

static void     l2tp_ifq_init_pc(void *, void *, struct cpu_info *);
static void     l2tp_ifq_fini_pc(void *, void *, struct cpu_info *);

static int      l2tp_clone_create(struct if_clone *, int);
static int      l2tp_clone_destroy(struct ifnet *);

struct if_clone l2tp_cloner =
    IF_CLONE_INITIALIZER("l2tp", l2tp_clone_create, l2tp_clone_destroy);

static int      l2tp_tx_enqueue(struct l2tp_variant *, struct mbuf *);
static int      l2tp_output(struct ifnet *, struct mbuf *,
                    const struct sockaddr *, const struct rtentry *);
static void     l2tp_sendit(struct l2tp_variant *, struct mbuf *);
static void     l2tpintr(struct l2tp_variant *);
static void     l2tpintr_softint(void *);

static void     l2tp_hash_init(void);
static int      l2tp_hash_fini(void);

static void     l2tp_start(struct ifnet *);
static int      l2tp_transmit(struct ifnet *, struct mbuf *);

static int      l2tp_set_tunnel(struct ifnet *, struct sockaddr *,
                    struct sockaddr *);
static void     l2tp_delete_tunnel(struct ifnet *);

static int      id_hash_func(uint32_t, u_long);

static void     l2tp_variant_update(struct l2tp_softc *, struct l2tp_variant *);
static int      l2tp_set_session(struct l2tp_softc *, uint32_t, uint32_t);
static int      l2tp_clear_session(struct l2tp_softc *);
static int      l2tp_set_cookie(struct l2tp_softc *, uint64_t, u_int, uint64_t, u_int);
static void     l2tp_clear_cookie(struct l2tp_softc *);
static void     l2tp_set_state(struct l2tp_softc *, int);
static int      l2tp_encap_attach(struct l2tp_variant *);
static int      l2tp_encap_detach(struct l2tp_variant *);

static inline struct ifqueue *
l2tp_ifq_percpu_getref(percpu_t *pc)
{

        return *(struct ifqueue **)percpu_getref(pc);
}

static inline void
l2tp_ifq_percpu_putref(percpu_t *pc)
{

        percpu_putref(pc);
}

#ifndef MAX_L2TP_NEST
/*
 * This macro controls the upper limitation on nesting of l2tp tunnels.
 * Since, setting a large value to this macro with a careless configuration
 * may introduce system crash, we don't allow any nestings by default.
 * If you need to configure nested l2tp tunnels, you can define this macro
 * in your kernel configuration file.  However, if you do so, please be
 * careful to configure the tunnels so that it won't make a loop.
 */
/*
 * XXX
 * Currently, if in_l2tp_output recursively calls, it causes locking against
 * myself of struct l2tp_ro->lr_lock. So, nested l2tp tunnels is prohibited.
 */
#define MAX_L2TP_NEST 0
#endif

static int max_l2tp_nesting = MAX_L2TP_NEST;

/* ARGSUSED */
void
l2tpattach(int count)
{
        /*
         * Nothing to do here, initialization is handled by the
         * module initialization code in l2tpinit() below).
         */
}

static void
l2tpinit(void)
{

        mutex_init(&l2tp_softcs.lock, MUTEX_DEFAULT, IPL_NONE);
        LIST_INIT(&l2tp_softcs.list);

        mutex_init(&l2tp_hash.lock, MUTEX_DEFAULT, IPL_NONE);
        l2tp_psz = pserialize_create();
        lv_psref_class = psref_class_create("l2tpvar", IPL_SOFTNET);
        if_clone_attach(&l2tp_cloner);

        l2tp_hash_init();
}

static int
l2tpdetach(void)
{
        int error;

        mutex_enter(&l2tp_softcs.lock);
        if (!LIST_EMPTY(&l2tp_softcs.list)) {
                mutex_exit(&l2tp_softcs.lock);
                return EBUSY;
        }
        mutex_exit(&l2tp_softcs.lock);

        error = l2tp_hash_fini();
        if (error)
                return error;

        if_clone_detach(&l2tp_cloner);
        psref_class_destroy(lv_psref_class);
        pserialize_destroy(l2tp_psz);
        mutex_destroy(&l2tp_hash.lock);

        mutex_destroy(&l2tp_softcs.lock);

        return error;
}

static int
l2tp_clone_create(struct if_clone *ifc, int unit)
{
        struct l2tp_softc *sc;
        struct l2tp_variant *var;
        int rv;
        u_int si_flags = SOFTINT_NET;
#ifdef NET_MPSAFE
        si_flags |= SOFTINT_MPSAFE;
#endif
        sc = kmem_zalloc(sizeof(struct l2tp_softc), KM_SLEEP);
        if_initname(&sc->l2tp_ec.ec_if, ifc->ifc_name, unit);
        rv = l2tpattach0(sc);
        if (rv != 0) {
                kmem_free(sc, sizeof(struct l2tp_softc));
                return rv;
        }

        var = kmem_zalloc(sizeof(struct l2tp_variant), KM_SLEEP);
        var->lv_softc = sc;
        var->lv_state = L2TP_STATE_DOWN;
        var->lv_use_cookie = L2TP_COOKIE_OFF;
        psref_target_init(&var->lv_psref, lv_psref_class);

        sc->l2tp_var = var;
        mutex_init(&sc->l2tp_lock, MUTEX_DEFAULT, IPL_NONE);
        sc->l2tp_psz = pserialize_create();
        PSLIST_ENTRY_INIT(sc, l2tp_hash);

        sc->l2tp_ro_percpu = if_tunnel_alloc_ro_percpu();

        sc->l2tp_ifq_percpu = percpu_create(sizeof(struct ifqueue *),
            l2tp_ifq_init_pc, l2tp_ifq_fini_pc, NULL);
        sc->l2tp_si = softint_establish(si_flags, l2tpintr_softint, sc);

        mutex_enter(&l2tp_softcs.lock);
        LIST_INSERT_HEAD(&l2tp_softcs.list, sc, l2tp_list);
        mutex_exit(&l2tp_softcs.lock);

        return (0);
}

int
l2tpattach0(struct l2tp_softc *sc)
{

        sc->l2tp_ec.ec_capabilities = 0;
        sc->l2tp_ec.ec_capabilities |= ETHERCAP_VLAN_MTU;
        sc->l2tp_ec.ec_capabilities |= ETHERCAP_JUMBO_MTU;

        ether_ifattach(&sc->l2tp_ec.ec_if, NULL);

        sc->l2tp_ec.ec_if.if_addrlen = 0;
        sc->l2tp_ec.ec_if.if_mtu    = L2TP_MTU;
        sc->l2tp_ec.ec_if.if_flags  = IFF_POINTOPOINT|IFF_MULTICAST|IFF_SIMPLEX;
#ifdef NET_MPSAFE
        sc->l2tp_ec.ec_if.if_extflags = IFEF_MPSAFE;
#endif
        sc->l2tp_ec.ec_if.if_ioctl  = l2tp_ioctl;
        sc->l2tp_ec.ec_if.if_output = l2tp_output;
        sc->l2tp_ec.ec_if.if_type   = IFT_L2TP;
        sc->l2tp_ec.ec_if.if_dlt    = DLT_NULL;
        sc->l2tp_ec.ec_if.if_start  = l2tp_start;
        sc->l2tp_ec.ec_if.if_transmit = l2tp_transmit;
        sc->l2tp_ec.ec_if._if_input = ether_input;
        IFQ_SET_READY(&sc->l2tp_ec.ec_if.if_snd);

        /* XXX
         * It may improve performance to use if_initialize()/if_register()
         * so that l2tp_input() calls if_input() instead of
         * if_percpuq_enqueue(). However, that causes recursive softnet_lock
         * when NET_MPSAFE is not set.
         */
        if_attach(&sc->l2tp_ec.ec_if);
        if_link_state_change(&sc->l2tp_ec.ec_if, LINK_STATE_DOWN);
        if_alloc_sadl(&sc->l2tp_ec.ec_if);

        return 0;
}

void
l2tp_ifq_init_pc(void *p, void *arg __unused, struct cpu_info *ci __unused)
{
        struct ifqueue **ifqp = p;

        *ifqp = kmem_zalloc(sizeof(**ifqp), KM_SLEEP);
        (*ifqp)->ifq_maxlen = IFQ_MAXLEN;
}

void
l2tp_ifq_fini_pc(void *p, void *arg __unused, struct cpu_info *ci __unused)
{
        struct ifqueue **ifqp = p;

        kmem_free(*ifqp, sizeof(**ifqp));
}

static int
l2tp_clone_destroy(struct ifnet *ifp)
{
        struct l2tp_variant *var;
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);

        ether_ifdetach(ifp);

        l2tp_clear_session(sc);
        l2tp_delete_tunnel(&sc->l2tp_ec.ec_if);
        /*
         * To avoid for l2tp_transmit() and l2tpintr_softint() to access
         * sc->l2tp_var after free it.
         */
        mutex_enter(&sc->l2tp_lock);
        var = sc->l2tp_var;
        l2tp_variant_update(sc, NULL);
        mutex_exit(&sc->l2tp_lock);

        softint_disestablish(sc->l2tp_si);
        percpu_free(sc->l2tp_ifq_percpu, sizeof(struct ifqueue *));

        mutex_enter(&l2tp_softcs.lock);
        LIST_REMOVE(sc, l2tp_list);
        mutex_exit(&l2tp_softcs.lock);

        if_detach(ifp);

        if_tunnel_free_ro_percpu(sc->l2tp_ro_percpu);

        kmem_free(var, sizeof(struct l2tp_variant));
        pserialize_destroy(sc->l2tp_psz);
        mutex_destroy(&sc->l2tp_lock);
        kmem_free(sc, sizeof(struct l2tp_softc));

        return 0;
}

static int
l2tp_tx_enqueue(struct l2tp_variant *var, struct mbuf *m)
{
        struct l2tp_softc *sc;
        struct ifnet *ifp;
        struct ifqueue *ifq;
        int s;

        KASSERT(psref_held(&var->lv_psref, lv_psref_class));

        sc = var->lv_softc;
        ifp = &sc->l2tp_ec.ec_if;

        s = splsoftnet();
        ifq = l2tp_ifq_percpu_getref(sc->l2tp_ifq_percpu);
        if (IF_QFULL(ifq)) {
                if_statinc(ifp, if_oerrors);
                l2tp_ifq_percpu_putref(sc->l2tp_ifq_percpu);
                splx(s);
                m_freem(m);
                return ENOBUFS;
        }

        IF_ENQUEUE(ifq, m);
        percpu_putref(sc->l2tp_ifq_percpu);
        softint_schedule(sc->l2tp_si);
        /* counter is incremented in l2tpintr() */
        splx(s);
        return 0;
}

static int
l2tp_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,
    const struct rtentry *rt)
{
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);
        struct l2tp_variant *var;
        struct psref psref;
        int error = 0;

        var = l2tp_getref_variant(sc, &psref);
        if (var == NULL) {
                m_freem(m);
                return ENETDOWN;
        }

        IFQ_CLASSIFY(&ifp->if_snd, m, dst->sa_family);

        m->m_flags &= ~(M_BCAST|M_MCAST);

        if ((ifp->if_flags & IFF_UP) == 0) {
                m_freem(m);
                error = ENETDOWN;
                goto end;
        }

        if (var->lv_psrc == NULL || var->lv_pdst == NULL) {
                m_freem(m);
                error = ENETDOWN;
                goto end;
        }

        /* XXX should we check if our outer source is legal? */

        /* use DLT_NULL encapsulation here to pass inner af type */
        M_PREPEND(m, sizeof(int), M_DONTWAIT);
        if (!m) {
                error = ENOBUFS;
                goto end;
        }
        *mtod(m, int *) = dst->sa_family;

        error = l2tp_tx_enqueue(var, m);
end:
        l2tp_putref_variant(var, &psref);
        if (error)
                if_statinc(ifp, if_oerrors);

        return error;
}

static void
l2tp_sendit(struct l2tp_variant *var, struct mbuf *m)
{
        int len;
        int error;
        struct l2tp_softc *sc;
        struct ifnet *ifp;

        KASSERT(psref_held(&var->lv_psref, lv_psref_class));

        sc = var->lv_softc;
        ifp = &sc->l2tp_ec.ec_if;

        len = m->m_pkthdr.len;
        m->m_flags &= ~(M_BCAST|M_MCAST);
        bpf_mtap(ifp, m, BPF_D_OUT);

        switch (var->lv_psrc->sa_family) {
#ifdef INET
        case AF_INET:
                error = in_l2tp_output(var, m);
                break;
#endif
#ifdef INET6
        case AF_INET6:
                error = in6_l2tp_output(var, m);
                break;
#endif
        default:
                m_freem(m);
                error = ENETDOWN;
                break;
        }
        if (error) {
                if_statinc(ifp, if_oerrors);
        } else {
                if_statadd2(ifp, if_opackets, 1, if_obytes, len);
        }
}

static void
l2tpintr(struct l2tp_variant *var)
{
        struct l2tp_softc *sc;
        struct ifnet *ifp;
        struct mbuf *m;
        struct ifqueue *ifq;
        u_int cpuid = cpu_index(curcpu());

        KASSERT(psref_held(&var->lv_psref, lv_psref_class));

        sc = var->lv_softc;
        ifp = &sc->l2tp_ec.ec_if;

        /* output processing */
        if (var->lv_my_sess_id == 0 || var->lv_peer_sess_id == 0) {
                ifq = l2tp_ifq_percpu_getref(sc->l2tp_ifq_percpu);
                IF_PURGE(ifq);
                l2tp_ifq_percpu_putref(sc->l2tp_ifq_percpu);
                if (cpuid == 0)
                        IFQ_PURGE(&ifp->if_snd);
                return;
        }

        /* Currently, l2tpintr() is always called in softint context. */
        ifq = l2tp_ifq_percpu_getref(sc->l2tp_ifq_percpu);
        for (;;) {
                IF_DEQUEUE(ifq, m);
                if (m != NULL)
                        l2tp_sendit(var, m);
                else
                        break;
        }
        l2tp_ifq_percpu_putref(sc->l2tp_ifq_percpu);

        if (cpuid == 0) {
                for (;;) {
                        IFQ_DEQUEUE(&ifp->if_snd, m);
                        if (m != NULL)
                                l2tp_sendit(var, m);
                        else
                                break;
                }
        }
}

static void
l2tpintr_softint(void *arg)
{
        struct l2tp_variant *var;
        struct psref psref;
        struct l2tp_softc *sc = arg;

        var = l2tp_getref_variant(sc, &psref);
        if (var == NULL)
                return;

        l2tpintr(var);
        l2tp_putref_variant(var, &psref);
}

void
l2tp_input(struct mbuf *m, struct ifnet *ifp)
{
        vaddr_t addr;

        KASSERT(ifp != NULL);

        /*
         * Currently, l2tp(4) supports only ethernet as inner protocol.
         */
        if (m->m_pkthdr.len < sizeof(struct ether_header)) {
                m_freem(m);
                return;
        }

        /*
         * If the head of the payload is not aligned, align it.
         */
        addr = mtod(m, vaddr_t);
        if ((addr & 0x03) != 0x2) {
                /* copy and align head of payload */
                struct mbuf *m_head;
                int copy_length;
                u_int pad = roundup(sizeof(struct ether_header), 4)
                        - sizeof(struct ether_header);

#define L2TP_COPY_LENGTH                60

                if (m->m_pkthdr.len < L2TP_COPY_LENGTH) {
                        copy_length = m->m_pkthdr.len;
                } else {
                        copy_length = L2TP_COPY_LENGTH;
                }

                if (m->m_len < copy_length) {
                        m = m_pullup(m, copy_length);
                        if (m == NULL)
                                return;
                }

                MGETHDR(m_head, M_DONTWAIT, MT_HEADER);
                if (m_head == NULL) {
                        m_freem(m);
                        return;
                }
                m_move_pkthdr(m_head, m);

                /*
                 * m_head should be:
                 *                             L2TP_COPY_LENGTH
                 *                          <-  + roundup(pad, 4) - pad ->
                 *   +-------+--------+-----+--------------+-------------+
                 *   | m_hdr | pkthdr | ... | ether header |   payload   |
                 *   +-------+--------+-----+--------------+-------------+
                 *                          ^              ^
                 *                          m_data         4 byte aligned
                 */
                m_align(m_head, L2TP_COPY_LENGTH + roundup(pad, 4));
                m_head->m_data += pad;

                memcpy(mtod(m_head, void *), mtod(m, void *), copy_length);
                m_head->m_len = copy_length;
                m->m_data += copy_length;
                m->m_len -= copy_length;

                /* construct chain */
                if (m->m_len == 0) {
                        m_head->m_next = m_free(m);
                } else {
                        m_head->m_next = m;
                }

                /* override m */
                m = m_head;
        }

        m_set_rcvif(m, ifp);

        /*
         * bpf_mtap() and ifp->if_ipackets++ is done in if_input()
         *
         * obytes is incremented at ether_output() or bridge_enqueue().
         */
        if_percpuq_enqueue(ifp->if_percpuq, m);
}

void
l2tp_start(struct ifnet *ifp)
{
        struct psref psref;
        struct l2tp_variant *var;
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);

        var = l2tp_getref_variant(sc, &psref);
        if (var == NULL)
                return;

        if (var->lv_psrc == NULL || var->lv_pdst == NULL)
                return;

        kpreempt_disable();
        softint_schedule(sc->l2tp_si);
        kpreempt_enable();
        l2tp_putref_variant(var, &psref);
}

int
l2tp_transmit(struct ifnet *ifp, struct mbuf *m)
{
        int error;
        struct psref psref;
        struct l2tp_variant *var;
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);

        var = l2tp_getref_variant(sc, &psref);
        if (var == NULL) {
                m_freem(m);
                return ENETDOWN;
        }

        if (var->lv_psrc == NULL || var->lv_pdst == NULL) {
                m_freem(m);
                error = ENETDOWN;
                goto out;
        }

        m->m_flags &= ~(M_BCAST|M_MCAST);

        error = l2tp_tx_enqueue(var, m);
out:
        l2tp_putref_variant(var, &psref);
        return error;
}

/* XXX how should we handle IPv6 scope on SIOC[GS]IFPHYADDR? */
int
l2tp_ioctl(struct ifnet *ifp, u_long cmd, void *data)
{
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);
        struct l2tp_variant *var, *var_tmp;
        struct ifreq     *ifr = data;
        int error = 0, size;
        struct sockaddr *dst, *src;
        struct l2tp_req l2tpr;
        u_long mtu;
        int bound;
        struct psref psref;

        switch (cmd) {
        case SIOCSIFADDR:
                ifp->if_flags |= IFF_UP;
                break;

        case SIOCSIFDSTADDR:
                break;

        case SIOCADDMULTI:
        case SIOCDELMULTI:
                switch (ifr->ifr_addr.sa_family) {
#ifdef INET
                case AF_INET:   /* IP supports Multicast */
                        break;
#endif /* INET */
#ifdef INET6
                case AF_INET6:  /* IP6 supports Multicast */
                        break;
#endif /* INET6 */
                default:  /* Other protocols doesn't support Multicast */
                        error = EAFNOSUPPORT;
                        break;
                }
                break;

        case SIOCSIFMTU:
                mtu = ifr->ifr_mtu;
                if (mtu < L2TP_MTU_MIN || mtu > L2TP_MTU_MAX)
                        return (EINVAL);
                ifp->if_mtu = mtu;
                break;

#ifdef INET
        case SIOCSIFPHYADDR:
                src = (struct sockaddr *)
                        &(((struct in_aliasreq *)data)->ifra_addr);
                dst = (struct sockaddr *)
                        &(((struct in_aliasreq *)data)->ifra_dstaddr);
                if (src->sa_family != AF_INET || dst->sa_family != AF_INET)
                        return EAFNOSUPPORT;
                else if (src->sa_len != sizeof(struct sockaddr_in)
                    || dst->sa_len != sizeof(struct sockaddr_in))
                        return EINVAL;

                error = l2tp_set_tunnel(&sc->l2tp_ec.ec_if, src, dst);
                break;

#endif /* INET */
#ifdef INET6
        case SIOCSIFPHYADDR_IN6:
                src = (struct sockaddr *)
                        &(((struct in6_aliasreq *)data)->ifra_addr);
                dst = (struct sockaddr *)
                        &(((struct in6_aliasreq *)data)->ifra_dstaddr);
                if (src->sa_family != AF_INET6 || dst->sa_family != AF_INET6)
                        return EAFNOSUPPORT;
                else if (src->sa_len != sizeof(struct sockaddr_in6)
                    || dst->sa_len != sizeof(struct sockaddr_in6))
                        return EINVAL;

                error = l2tp_set_tunnel(&sc->l2tp_ec.ec_if, src, dst);
                break;

#endif /* INET6 */
        case SIOCSLIFPHYADDR:
                src = (struct sockaddr *)
                        &(((struct if_laddrreq *)data)->addr);
                dst = (struct sockaddr *)
                        &(((struct if_laddrreq *)data)->dstaddr);
                if (src->sa_family != dst->sa_family)
                        return EINVAL;
                else if (src->sa_family == AF_INET
                    && src->sa_len != sizeof(struct sockaddr_in))
                        return EINVAL;
                else if (src->sa_family == AF_INET6
                    && src->sa_len != sizeof(struct sockaddr_in6))
                        return EINVAL;
                else if (dst->sa_family == AF_INET
                    && dst->sa_len != sizeof(struct sockaddr_in))
                        return EINVAL;
                else if (dst->sa_family == AF_INET6
                    && dst->sa_len != sizeof(struct sockaddr_in6))
                        return EINVAL;

                error = l2tp_set_tunnel(&sc->l2tp_ec.ec_if, src, dst);
                break;

        case SIOCDIFPHYADDR:
                l2tp_delete_tunnel(&sc->l2tp_ec.ec_if);
                break;

        case SIOCGIFPSRCADDR:
#ifdef INET6
        case SIOCGIFPSRCADDR_IN6:
#endif /* INET6 */
                bound = curlwp_bind();
                var = l2tp_getref_variant(sc, &psref);
                if (var == NULL) {
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                if (var->lv_psrc == NULL) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                src = var->lv_psrc;
                switch (cmd) {
#ifdef INET
                case SIOCGIFPSRCADDR:
                        dst = &ifr->ifr_addr;
                        size = sizeof(ifr->ifr_addr);
                        break;
#endif /* INET */
#ifdef INET6
                case SIOCGIFPSRCADDR_IN6:
                        dst = (struct sockaddr *)
                                &(((struct in6_ifreq *)data)->ifr_addr);
                        size = sizeof(((struct in6_ifreq *)data)->ifr_addr);
                        break;
#endif /* INET6 */
                default:
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                if (src->sa_len > size) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        return EINVAL;
                }
                sockaddr_copy(dst, src->sa_len, src);
                l2tp_putref_variant(var, &psref);
                curlwp_bindx(bound);
                break;

        case SIOCGIFPDSTADDR:
#ifdef INET6
        case SIOCGIFPDSTADDR_IN6:
#endif /* INET6 */
                bound = curlwp_bind();
                var = l2tp_getref_variant(sc, &psref);
                if (var == NULL) {
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                if (var->lv_pdst == NULL) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                src = var->lv_pdst;
                switch (cmd) {
#ifdef INET
                case SIOCGIFPDSTADDR:
                        dst = &ifr->ifr_addr;
                        size = sizeof(ifr->ifr_addr);
                        break;
#endif /* INET */
#ifdef INET6
                case SIOCGIFPDSTADDR_IN6:
                        dst = (struct sockaddr *)
                                &(((struct in6_ifreq *)data)->ifr_addr);
                        size = sizeof(((struct in6_ifreq *)data)->ifr_addr);
                        break;
#endif /* INET6 */
                default:
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                if (src->sa_len > size) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        return EINVAL;
                }
                sockaddr_copy(dst, src->sa_len, src);
                l2tp_putref_variant(var, &psref);
                curlwp_bindx(bound);
                break;

        case SIOCGLIFPHYADDR:
                bound = curlwp_bind();
                var = l2tp_getref_variant(sc, &psref);
                if (var == NULL) {
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }
                if (var->lv_psrc == NULL || var->lv_pdst == NULL) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }

                /* copy src */
                src = var->lv_psrc;
                dst = (struct sockaddr *)
                        &(((struct if_laddrreq *)data)->addr);
                size = sizeof(((struct if_laddrreq *)data)->addr);
                if (src->sa_len > size) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        return EINVAL;
                }
                sockaddr_copy(dst, src->sa_len, src);

                /* copy dst */
                src = var->lv_pdst;
                dst = (struct sockaddr *)
                        &(((struct if_laddrreq *)data)->dstaddr);
                size = sizeof(((struct if_laddrreq *)data)->dstaddr);
                if (src->sa_len > size) {
                        l2tp_putref_variant(var, &psref);
                        curlwp_bindx(bound);
                        return EINVAL;
                }
                sockaddr_copy(dst, src->sa_len, src);
                l2tp_putref_variant(var, &psref);
                curlwp_bindx(bound);
                break;

        case SIOCSL2TPSESSION:
                if ((error = copyin(ifr->ifr_data, &l2tpr, sizeof(l2tpr))) != 0)
                        break;

                /* session id must not zero */
                if (l2tpr.my_sess_id == 0 || l2tpr.peer_sess_id == 0)
                        return EINVAL;

                bound = curlwp_bind();
                var_tmp = l2tp_lookup_session_ref(l2tpr.my_sess_id, &psref);
                if (var_tmp != NULL) {
                        /* duplicate session id */
                        log(LOG_WARNING, "%s: duplicate session id %" PRIu32 " of %s\n",
                                sc->l2tp_ec.ec_if.if_xname, l2tpr.my_sess_id,
                                var_tmp->lv_softc->l2tp_ec.ec_if.if_xname);
                        psref_release(&psref, &var_tmp->lv_psref,
                            lv_psref_class);
                        curlwp_bindx(bound);
                        return EINVAL;
                }
                curlwp_bindx(bound);

                error = l2tp_set_session(sc, l2tpr.my_sess_id, l2tpr.peer_sess_id);
                break;
        case SIOCDL2TPSESSION:
                l2tp_clear_session(sc);
                break;
        case SIOCSL2TPCOOKIE:
                if ((error = copyin(ifr->ifr_data, &l2tpr, sizeof(l2tpr))) != 0)
                        break;

                error = l2tp_set_cookie(sc, l2tpr.my_cookie, l2tpr.my_cookie_len,
                    l2tpr.peer_cookie, l2tpr.peer_cookie_len);
                break;
        case SIOCDL2TPCOOKIE:
                l2tp_clear_cookie(sc);
                break;
        case SIOCSL2TPSTATE:
                if ((error = copyin(ifr->ifr_data, &l2tpr, sizeof(l2tpr))) != 0)
                        break;

                l2tp_set_state(sc, l2tpr.state);
                break;
        case SIOCGL2TP:
                /* get L2TPV3 session info */
                memset(&l2tpr, 0, sizeof(l2tpr));

                bound = curlwp_bind();
                var = l2tp_getref_variant(sc, &psref);
                if (var == NULL) {
                        curlwp_bindx(bound);
                        error = EADDRNOTAVAIL;
                        goto bad;
                }

                l2tpr.state = var->lv_state;
                l2tpr.my_sess_id = var->lv_my_sess_id;
                l2tpr.peer_sess_id = var->lv_peer_sess_id;
                l2tpr.my_cookie = var->lv_my_cookie;
                l2tpr.my_cookie_len = var->lv_my_cookie_len;
                l2tpr.peer_cookie = var->lv_peer_cookie;
                l2tpr.peer_cookie_len = var->lv_peer_cookie_len;
                l2tp_putref_variant(var, &psref);
                curlwp_bindx(bound);

                error = copyout(&l2tpr, ifr->ifr_data, sizeof(l2tpr));
                break;

        default:
                error = ifioctl_common(ifp, cmd, data);
                break;
        }
 bad:
        return error;
}

static int
l2tp_set_tunnel(struct ifnet *ifp, struct sockaddr *src, struct sockaddr *dst)
{
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);
        struct sockaddr *osrc, *odst;
        struct sockaddr *nsrc, *ndst;
        struct l2tp_variant *ovar, *nvar;
        int error;

        nsrc = sockaddr_dup(src, M_WAITOK);
        ndst = sockaddr_dup(dst, M_WAITOK);

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        error = encap_lock_enter();
        if (error)
                goto error;

        mutex_enter(&sc->l2tp_lock);

        ovar = sc->l2tp_var;
        osrc = ovar->lv_psrc;
        odst = ovar->lv_pdst;
        *nvar = *ovar;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        nvar->lv_psrc = nsrc;
        nvar->lv_pdst = ndst;
        error = l2tp_encap_attach(nvar);
        if (error) {
                mutex_exit(&sc->l2tp_lock);
                encap_lock_exit();
                goto error;
        }
        l2tp_variant_update(sc, nvar);

        mutex_exit(&sc->l2tp_lock);

        (void)l2tp_encap_detach(ovar);
        encap_lock_exit();

        if (osrc)
                sockaddr_free(osrc);
        if (odst)
                sockaddr_free(odst);
        kmem_free(ovar, sizeof(*ovar));
        return 0;

error:
        sockaddr_free(nsrc);
        sockaddr_free(ndst);
        kmem_free(nvar, sizeof(*nvar));

        return error;
}

static void
l2tp_delete_tunnel(struct ifnet *ifp)
{
        struct l2tp_softc *sc = container_of(ifp, struct l2tp_softc,
            l2tp_ec.ec_if);
        struct sockaddr *osrc, *odst;
        struct l2tp_variant *ovar, *nvar;
        int error;

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        error = encap_lock_enter();
        if (error) {
                kmem_free(nvar, sizeof(*nvar));
                return;
        }
        mutex_enter(&sc->l2tp_lock);

        ovar = sc->l2tp_var;
        osrc = ovar->lv_psrc;
        odst = ovar->lv_pdst;
        *nvar = *ovar;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        nvar->lv_psrc = NULL;
        nvar->lv_pdst = NULL;
        l2tp_variant_update(sc, nvar);

        mutex_exit(&sc->l2tp_lock);

        (void)l2tp_encap_detach(ovar);
        encap_lock_exit();

        if (osrc)
                sockaddr_free(osrc);
        if (odst)
                sockaddr_free(odst);
        kmem_free(ovar, sizeof(*ovar));
}

static int
id_hash_func(uint32_t id, u_long mask)
{
        uint32_t hash;

        hash = (id >> 16) ^ id;
        hash = (hash >> 4) ^ hash;

        return hash & mask;
}

static void
l2tp_hash_init(void)
{

        l2tp_hash.lists = hashinit(L2TP_ID_HASH_SIZE, HASH_PSLIST, true,
            &l2tp_hash.mask);
}

static int
l2tp_hash_fini(void)
{
        int i;

        mutex_enter(&l2tp_hash.lock);

        for (i = 0; i < l2tp_hash.mask + 1; i++) {
                if (PSLIST_WRITER_FIRST(&l2tp_hash.lists[i], struct l2tp_softc,
                        l2tp_hash) != NULL) {
                        mutex_exit(&l2tp_hash.lock);
                        return EBUSY;
                }
        }
        for (i = 0; i < l2tp_hash.mask + 1; i++)
                PSLIST_DESTROY(&l2tp_hash.lists[i]);

        mutex_exit(&l2tp_hash.lock);

        hashdone(l2tp_hash.lists, HASH_PSLIST, l2tp_hash.mask);

        return 0;
}

static int
l2tp_set_session(struct l2tp_softc *sc, uint32_t my_sess_id,
    uint32_t peer_sess_id)
{
        uint32_t idx;
        struct l2tp_variant *nvar;
        struct l2tp_variant *ovar;
        struct ifnet *ifp = &sc->l2tp_ec.ec_if;

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        mutex_enter(&sc->l2tp_lock);
        ovar = sc->l2tp_var;
        *nvar = *ovar;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        nvar->lv_my_sess_id = my_sess_id;
        nvar->lv_peer_sess_id = peer_sess_id;

        mutex_enter(&l2tp_hash.lock);
        if (ovar->lv_my_sess_id > 0 && ovar->lv_peer_sess_id > 0) {
                PSLIST_WRITER_REMOVE(sc, l2tp_hash);
                pserialize_perform(l2tp_psz);
        }
        mutex_exit(&l2tp_hash.lock);
        PSLIST_ENTRY_DESTROY(sc, l2tp_hash);

        l2tp_variant_update(sc, nvar);
        mutex_exit(&sc->l2tp_lock);

        idx = id_hash_func(nvar->lv_my_sess_id, l2tp_hash.mask);
        if ((ifp->if_flags & IFF_DEBUG) != 0)
                log(LOG_DEBUG, "%s: add hash entry: sess_id=%" PRIu32 ", idx=%" PRIu32 "\n",
                    sc->l2tp_ec.ec_if.if_xname, nvar->lv_my_sess_id, idx);

        PSLIST_ENTRY_INIT(sc, l2tp_hash);
        mutex_enter(&l2tp_hash.lock);
        PSLIST_WRITER_INSERT_HEAD(&l2tp_hash.lists[idx], sc, l2tp_hash);
        mutex_exit(&l2tp_hash.lock);

        kmem_free(ovar, sizeof(*ovar));
        return 0;
}

static int
l2tp_clear_session(struct l2tp_softc *sc)
{
        struct l2tp_variant *nvar;
        struct l2tp_variant *ovar;

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        mutex_enter(&sc->l2tp_lock);
        ovar = sc->l2tp_var;
        *nvar = *ovar;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        nvar->lv_my_sess_id = 0;
        nvar->lv_peer_sess_id = 0;

        mutex_enter(&l2tp_hash.lock);
        if (ovar->lv_my_sess_id > 0 && ovar->lv_peer_sess_id > 0) {
                PSLIST_WRITER_REMOVE(sc, l2tp_hash);
                pserialize_perform(l2tp_psz);
        }
        mutex_exit(&l2tp_hash.lock);

        l2tp_variant_update(sc, nvar);
        mutex_exit(&sc->l2tp_lock);
        kmem_free(ovar, sizeof(*ovar));
        return 0;
}

struct l2tp_variant *
l2tp_lookup_session_ref(uint32_t id, struct psref *psref)
{
        int idx;
        int s;
        struct l2tp_softc *sc;

        idx = id_hash_func(id, l2tp_hash.mask);

        s = pserialize_read_enter();
        PSLIST_READER_FOREACH(sc, &l2tp_hash.lists[idx], struct l2tp_softc,
            l2tp_hash) {
                struct l2tp_variant *var = atomic_load_consume(&sc->l2tp_var);
                if (var == NULL)
                        continue;
                if (var->lv_my_sess_id != id)
                        continue;
                psref_acquire(psref, &var->lv_psref, lv_psref_class);
                pserialize_read_exit(s);
                return var;
        }
        pserialize_read_exit(s);
        return NULL;
}

/*
 * l2tp_variant update API.
 *
 * Assumption:
 * reader side dereferences sc->l2tp_var in reader critical section only,
 * that is, all of reader sides do not reader the sc->l2tp_var after
 * pserialize_perform().
 */
static void
l2tp_variant_update(struct l2tp_softc *sc, struct l2tp_variant *nvar)
{
        struct ifnet *ifp = &sc->l2tp_ec.ec_if;
        struct l2tp_variant *ovar = sc->l2tp_var;

        KASSERT(mutex_owned(&sc->l2tp_lock));

        atomic_store_release(&sc->l2tp_var, nvar);
        pserialize_perform(sc->l2tp_psz);
        psref_target_destroy(&ovar->lv_psref, lv_psref_class);

        if (nvar != NULL) {
                if (nvar->lv_psrc != NULL && nvar->lv_pdst != NULL)
                        ifp->if_flags |= IFF_RUNNING;
                else
                        ifp->if_flags &= ~IFF_RUNNING;
        }
}

static int
l2tp_set_cookie(struct l2tp_softc *sc, uint64_t my_cookie, u_int my_cookie_len,
    uint64_t peer_cookie, u_int peer_cookie_len)
{
        struct l2tp_variant *nvar;

        if (my_cookie == 0 || peer_cookie == 0)
                return EINVAL;

        if (my_cookie_len != 4 && my_cookie_len != 8
            && peer_cookie_len != 4 && peer_cookie_len != 8)
                return EINVAL;

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        mutex_enter(&sc->l2tp_lock);

        *nvar = *sc->l2tp_var;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        nvar->lv_my_cookie = my_cookie;
        nvar->lv_my_cookie_len = my_cookie_len;
        nvar->lv_peer_cookie = peer_cookie;
        nvar->lv_peer_cookie_len = peer_cookie_len;
        nvar->lv_use_cookie = L2TP_COOKIE_ON;
        l2tp_variant_update(sc, nvar);

        mutex_exit(&sc->l2tp_lock);

        struct ifnet *ifp = &sc->l2tp_ec.ec_if;
        if ((ifp->if_flags & IFF_DEBUG) != 0) {
                log(LOG_DEBUG,
                    "%s: set cookie: "
                    "local cookie_len=%u local cookie=%" PRIu64 ", "
                    "remote cookie_len=%u remote cookie=%" PRIu64 "\n",
                    ifp->if_xname, my_cookie_len, my_cookie,
                    peer_cookie_len, peer_cookie);
        }

        return 0;
}

static void
l2tp_clear_cookie(struct l2tp_softc *sc)
{
        struct l2tp_variant *nvar;

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        mutex_enter(&sc->l2tp_lock);

        *nvar = *sc->l2tp_var;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        nvar->lv_my_cookie = 0;
        nvar->lv_my_cookie_len = 0;
        nvar->lv_peer_cookie = 0;
        nvar->lv_peer_cookie_len = 0;
        nvar->lv_use_cookie = L2TP_COOKIE_OFF;
        l2tp_variant_update(sc, nvar);

        mutex_exit(&sc->l2tp_lock);
}

static void
l2tp_set_state(struct l2tp_softc *sc, int state)
{
        struct ifnet *ifp = &sc->l2tp_ec.ec_if;
        struct l2tp_variant *nvar;
        int ostate;

        nvar = kmem_alloc(sizeof(*nvar), KM_SLEEP);

        mutex_enter(&sc->l2tp_lock);

        *nvar = *sc->l2tp_var;
        psref_target_init(&nvar->lv_psref, lv_psref_class);
        ostate = nvar->lv_state;
        nvar->lv_state = state;
        l2tp_variant_update(sc, nvar);
        mutex_exit(&sc->l2tp_lock);

        if (ostate != state) {
                int lstate;

                if (state == L2TP_STATE_UP)
                        lstate = LINK_STATE_UP;
                else
                        lstate = LINK_STATE_DOWN;

                if_link_state_change(ifp, lstate);
        }

#ifdef NOTYET
        vlan_linkstate_notify(ifp, ifp->if_link_state);
#endif
}

static int
l2tp_encap_attach(struct l2tp_variant *var)
{
        int error;

        if (var == NULL || var->lv_psrc == NULL)
                return EINVAL;

        switch (var->lv_psrc->sa_family) {
#ifdef INET
        case AF_INET:
                error = in_l2tp_attach(var);
                break;
#endif
#ifdef INET6
        case AF_INET6:
                error = in6_l2tp_attach(var);
                break;
#endif
        default:
                error = EINVAL;
                break;
        }

        return error;
}

static int
l2tp_encap_detach(struct l2tp_variant *var)
{
        int error;

        if (var == NULL || var->lv_psrc == NULL)
                return EINVAL;

        switch (var->lv_psrc->sa_family) {
#ifdef INET
        case AF_INET:
                error = in_l2tp_detach(var);
                break;
#endif
#ifdef INET6
        case AF_INET6:
                error = in6_l2tp_detach(var);
                break;
#endif
        default:
                error = EINVAL;
                break;
        }

        return error;
}

int
l2tp_check_nesting(struct ifnet *ifp, struct mbuf *m)
{

        return if_tunnel_check_nesting(ifp, m, max_l2tp_nesting);
}

/*
 * Module infrastructure
 */
#include "if_module.h"

IF_MODULE(MODULE_CLASS_DRIVER, l2tp, NULL)


/* TODO: IP_TCPMSS support */
#ifdef IP_TCPMSS
static int l2tp_need_tcpmss_clamp(struct ifnet *);
#ifdef INET
static struct mbuf *l2tp_tcpmss4_clamp(struct ifnet *, struct mbuf *);
#endif
#ifdef INET6
static struct mbuf *l2tp_tcpmss6_clamp(struct ifnet *, struct mbuf *);
#endif

struct mbuf *
l2tp_tcpmss_clamp(struct ifnet *ifp, struct mbuf *m)
{
        struct ether_header *eh;
        struct ether_vlan_header evh;

        if (!l2tp_need_tcpmss_clamp(ifp)) {
                return m;
        }

        if (m->m_pkthdr.len < sizeof(evh)) {
                m_freem(m);
                return NULL;
        }

        /* save ether header */
        m_copydata(m, 0, sizeof(evh), (void *)&evh);
        eh = (struct ether_header *)&evh;

        switch (ntohs(eh->ether_type)) {
        case ETHERTYPE_VLAN: /* Ether + VLAN */
                if (m->m_pkthdr.len <= sizeof(struct ether_vlan_header))
                        break;
                m_adj(m, sizeof(struct ether_vlan_header));
                switch (ntohs(evh.evl_proto)) {
#ifdef INET
                case ETHERTYPE_IP: /* Ether + VLAN + IPv4 */
                        m = l2tp_tcpmss4_clamp(ifp, m);
                        if (m == NULL)
                                return NULL;
                        break;
#endif /* INET */
#ifdef INET6
                case ETHERTYPE_IPV6: /* Ether + VLAN + IPv6 */
                        m = l2tp_tcpmss6_clamp(ifp, m);
                        if (m == NULL)
                                return NULL;
                        break;
#endif /* INET6 */
                default:
                        break;
                }

                /* restore ether header */
                M_PREPEND(m, sizeof(struct ether_vlan_header),
                    M_DONTWAIT);
                if (m == NULL)
                        return NULL;
                *mtod(m, struct ether_vlan_header *) = evh;
                break;

#ifdef INET
        case ETHERTYPE_IP: /* Ether + IPv4 */
                if (m->m_pkthdr.len <= sizeof(struct ether_header))
                        break;
                m_adj(m, sizeof(struct ether_header));
                m = l2tp_tcpmss4_clamp(ifp, m);
                if (m == NULL)
                        return NULL;
                /* restore ether header */
                M_PREPEND(m, sizeof(struct ether_header), M_DONTWAIT);
                if (m == NULL)
                        return NULL;
                *mtod(m, struct ether_header *) = *eh;
                break;
#endif /* INET */

#ifdef INET6
        case ETHERTYPE_IPV6: /* Ether + IPv6 */
                if (m->m_pkthdr.len <= sizeof(struct ether_header))
                        break;
                m_adj(m, sizeof(struct ether_header));
                m = l2tp_tcpmss6_clamp(ifp, m);
                if (m == NULL)
                        return NULL;
                /* restore ether header */
                M_PREPEND(m, sizeof(struct ether_header), M_DONTWAIT);
                if (m == NULL)
                        return NULL;
                *mtod(m, struct ether_header *) = *eh;
                break;
#endif /* INET6 */

        default:
                break;
        }

        return m;
}

static int
l2tp_need_tcpmss_clamp(struct ifnet *ifp)
{
        int ret = 0;

#ifdef INET
        if (ifp->if_tcpmss != 0)
                ret = 1;
#endif

#ifdef INET6
        if (ifp->if_tcpmss6 != 0)
                ret = 1;
#endif

        return ret;
}

#ifdef INET
static struct mbuf *
l2tp_tcpmss4_clamp(struct ifnet *ifp, struct mbuf *m)
{

        if (ifp->if_tcpmss != 0) {
                return ip_tcpmss(m, (ifp->if_tcpmss < 0) ?
                        ifp->if_mtu - IP_TCPMSS_EXTLEN :
                        ifp->if_tcpmss);
        }
        return m;
}
#endif /* INET */

#ifdef INET6
static struct mbuf *
l2tp_tcpmss6_clamp(struct ifnet *ifp, struct mbuf *m)
{
        int ip6hdrlen;

        if (ifp->if_tcpmss6 != 0 &&
            ip6_tcpmss_applicable(m, &ip6hdrlen)) {
                return ip6_tcpmss(m, ip6hdrlen,
                        (ifp->if_tcpmss6 < 0) ?
                        ifp->if_mtu - IP6_TCPMSS_EXTLEN :
                        ifp->if_tcpmss6);
        }
        return m;
}
#endif /* INET6 */

#endif /* IP_TCPMSS */