xref: /src/sys/net/if_l2tp.c

/*	$NetBSD: if_l2tp.c,v 1.45 2020/10/25 08:15:54 roy 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.45 2020/10/25 08:15:54 roy 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/netisr.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)
{
	int rv;

	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);

#ifdef MBUFTRACE
	struct ethercom *ec = &sc->l2tp_ec;
	struct ifnet *ifp = &sc->l2tp_ec.ec_if;

	strlcpy(ec->ec_tx_mowner.mo_name, ifp->if_xname,
	    sizeof(ec->ec_tx_mowner.mo_name));
	strlcpy(ec->ec_tx_mowner.mo_descr, "tx",
	    sizeof(ec->ec_tx_mowner.mo_descr));
	strlcpy(ec->ec_rx_mowner.mo_name, ifp->if_xname,
	    sizeof(ec->ec_rx_mowner.mo_name));
	strlcpy(ec->ec_rx_mowner.mo_descr, "rx",
	    sizeof(ec->ec_rx_mowner.mo_descr));
	MOWNER_ATTACH(&ec->ec_tx_mowner);
	MOWNER_ATTACH(&ec->ec_rx_mowner);
	ifp->if_mowner = &ec->ec_tx_mowner;
#endif

	/* 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.
	 */
	rv = if_attach(&sc->l2tp_ec.ec_if);
	if (rv != 0)
		return rv;
	if_alloc_sadl(&sc->l2tp_ec.ec_if);
	bpf_attach(&sc->l2tp_ec.ec_if, DLT_EN10MB, sizeof(struct ether_header));

	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);

	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);

	bpf_detach(ifp);

	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;

	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_state = state;
	l2tp_variant_update(sc, nvar);

	if (nvar->lv_state == L2TP_STATE_UP) {
		ifp->if_link_state = LINK_STATE_UP;
	} else {
		ifp->if_link_state = LINK_STATE_DOWN;
	}

	mutex_exit(&sc->l2tp_lock);

#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 */