/*
 * Copyright (c) 2007 The DragonFly Project.  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.
 * 3. Neither the name of The DragonFly Project nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific, prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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
 * COPYRIGHT HOLDERS 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/malloc.h>	/* for M_NOWAIT */
#include <sys/mbuf.h>
#include <sys/systm.h>

#include <net/if_var.h>

#include <netinet/ip.h>

#include <netproto/mpls/mpls.h>
#include <netproto/mpls/mpls_var.h>

static int mpls_push(struct mbuf **, mpls_label_t,
		     mpls_s_t, mpls_exp_t, mpls_ttl_t);
static int mpls_swap(struct mbuf *, mpls_label_t);
static int mpls_pop(struct mbuf *, mpls_s_t *);

int
mpls_output(struct mbuf *m, struct rtentry *rt)
{
	struct sockaddr_mpls *smpls = NULL;
	int error = 0, i;
	mpls_s_t stackempty;
	mpls_ttl_t ttl = 255;
	struct ip *ip;

	M_ASSERTPKTHDR(m);

	/*
	 * Check if we are coming from an MPLS routing table lookup.
	 * The rt_key of this rtentry will have a family AF_MPLS if so.
	 */
	stackempty = rt_key(rt)->sa_family != AF_MPLS ? 1 : 0;
	if (stackempty) {
		switch (rt_key(rt)->sa_family) {
		case AF_INET:
			ip = mtod(m, struct ip *);
			ttl = ip->ip_ttl;
			break;
		}
	}

	for (i=0; i < MPLS_MAXLOPS && rt->rt_shim[i] != NULL; ++i) {
		smpls = (struct sockaddr_mpls *)rt->rt_shim[i];
		switch (smpls->smpls_op) {
		case MPLSLOP_PUSH:
			error = mpls_push(&m,
				  ntohl(smpls->smpls_label),
				  /*
				   * If we are the first label push, then
				   * set the bottom-of-stack bit.
				   */
				  (stackempty && i == 0) ? 1 : 0,
				  0,
				  ttl);
			if (error)
				return (error);
			stackempty = 0;
			m->m_flags |= M_MPLSLABELED;
			break;
		case MPLSLOP_SWAP:
			/*
			 * Operation is only permmited if label stack
			 * is not empty.
			 */
			if (stackempty)
				return (ENOTSUP);
			KKASSERT(m->m_flags & M_MPLSLABELED);
			error = mpls_swap(m, ntohl(smpls->smpls_label));
			if (error)
				return (error);
			break;
		case MPLSLOP_POP:
			/*
			 * Operation is only permmited if label stack
			 * is not empty.
			 */
			if (stackempty)
				return (ENOTSUP);
			KKASSERT(m->m_flags & M_MPLSLABELED);
			error = mpls_pop(m, &stackempty);
			if (error)
				return (error);
			/*
			 * If we are popping out the last label then
			 * mark the mbuf as ~M_MPLSLABELED.
			 */
			if (stackempty)
				m->m_flags &= ~M_MPLSLABELED;
			break;
		default:
			/* Unknown label operation */
			return (ENOTSUP);
		}
	}

	return (error);
}

/*
 * Returns FALSE if no further output processing required.
 */
boolean_t
mpls_output_process(struct mbuf *m, struct rtentry *rt)
{
	int error;

	/* Does this route have MPLS label operations? */
	if (!(rt->rt_flags & RTF_MPLSOPS))
		return TRUE;

	error = mpls_output(m, rt);
	if (error) {
		m_freem(m);
		return FALSE;
	}

	return TRUE;
}

static int
mpls_push(struct mbuf **m, mpls_label_t label, mpls_s_t s, mpls_exp_t exp, mpls_ttl_t ttl) {
	struct mpls *mpls;
	u_int32_t buf = 0;	/* Silence warning */

	M_PREPEND(*m, sizeof(struct mpls), M_NOWAIT);
	if (*m == NULL)
		return (ENOBUFS);

	MPLS_SET_LABEL(buf, label);
	MPLS_SET_STACK(buf, s);
	MPLS_SET_EXP(buf, exp);
	MPLS_SET_TTL(buf, ttl);
	mpls = mtod(*m, struct mpls *);
	mpls->mpls_shim = htonl(buf);

	return (0);
}

static int
mpls_swap(struct mbuf *m, mpls_label_t label) {
	struct mpls *mpls;
	u_int32_t buf;
	mpls_ttl_t ttl;

	if (m->m_len < sizeof(struct mpls) &&
	   (m = m_pullup(m, sizeof(struct mpls))) == NULL)
		return (ENOBUFS);

	mpls = mtod(m, struct mpls *);
	buf = ntohl(mpls->mpls_shim);
	ttl = MPLS_TTL(buf);
	if (--ttl <= 0) {
		/* XXX: should send icmp ttl expired. */
		mplsstat.mplss_ttlexpired++;
		return (ETIMEDOUT);
	}
	MPLS_SET_LABEL(buf, label);
	MPLS_SET_TTL(buf, ttl); /* XXX tunnel mode: uniform, pipe, short pipe */
	mpls->mpls_shim = htonl(buf);

	return (0);
}

static int
mpls_pop(struct mbuf *m, mpls_s_t *sbit) {
	struct mpls *mpls;
	u_int32_t buf;

	if (m->m_len < sizeof(struct mpls)) {
		m = m_pullup(m, sizeof(struct mpls));
		if (m == NULL)
			return (ENOBUFS);
	}
	mpls = mtod(m, struct mpls *);
	buf = ntohl(mpls->mpls_shim);
	*sbit = MPLS_STACK(buf);

	m_adj(m, sizeof(struct mpls));

	return (0);
}