/*
 * $Id: mh.c 1.103 06/05/07 21:52:43+03:00 anttit@tcs.hut.fi $
 *
 * This file is part of the MIPL Mobile IPv6 for Linux.
 * 
 * Authors: Antti Tuominen <anttit@tcs.hut.fi>
 *          Ville Nuorvala <vnuorval@tcs.hut.fi>
 *
 * Copyright 2003-2005 Go-Core Project
 * Copyright 2003-2006 Helsinki University of Technology
 *
 * MIPL Mobile IPv6 for Linux is free software; you can redistribute
 * it and/or modify it under the terms of the GNU General Public
 * License as published by the Free Software Foundation; version 2 of
 * the License.
 *
 * MIPL Mobile IPv6 for Linux is distributed in the hope that it will
 * be useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIPL Mobile IPv6 for Linux; if not, write to the Free
 * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
 * 02111-1307 USA.
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <pthread.h>
#include <netinet/in.h>
#include <netinet/ip6.h>
#include <errno.h>
#include <unistd.h>
#include <string.h>
#include <syslog.h>
#include <sys/socket.h>
#ifdef HAVE_LIBCRYPTO
#include <openssl/opensslv.h>
#include <openssl/hmac.h>
#else
#include "crypto.h"
#endif

#include "mipv6.h"
#include "xfrm.h"
#include "mh.h"
#include "util.h"
#include "debug.h"
#include "conf.h"
#include "bcache.h"
#include "keygen.h"
#include "prefix.h"
#include "statistics.h"

#define MH_DEBUG_LEVEL 1

#if MH_DEBUG_LEVEL >= 1
#define MDBG dbg
#else
#define MDBG(...)
#endif

struct sock {
	pthread_mutex_t send_mutex;
	int fd;
};

static struct sock mh_sock;

/* Are duplicate options allowed */
int mh_opts_dup_ok[] = {
	1, /* PAD1 */ 
	1, /* PADN */
	0, /* BRR */
	0, /* Alternate CoA */
	0, /* Nonce Index */
	0, /* Binding Auth Data */
	1, /* Mobile Network Prefix */
};

#define __MH_SENTINEL (IP6_MH_TYPE_MAX + 1)

static pthread_rwlock_t handler_lock;
static struct mh_handler *handlers[__MH_SENTINEL + 1];

static pthread_t mh_listener;

static inline int mh_type_map(uint8_t type)
{
	return type < __MH_SENTINEL ? type : __MH_SENTINEL;
}

static inline struct mh_handler *mh_handler_get(uint8_t type)
{
	return handlers[mh_type_map(type)]; 
}

void mh_handler_reg(uint8_t type, struct mh_handler *handler)
{
	int i = mh_type_map(type);

	assert(handler->next == NULL);

	pthread_rwlock_wrlock(&handler_lock);
	handler->next = handlers[i];
	handlers[i] = handler;
	pthread_rwlock_unlock(&handler_lock);
}

void mh_handler_dereg(uint8_t type, struct mh_handler *handler)
{
	struct mh_handler **h; 
	int i = mh_type_map(type);
	pthread_rwlock_wrlock(&handler_lock);
	h = &handlers[i];
	while (*h) {
		if (*h == handler) {
			*h = handler->next;
			handler->next = NULL;
			break;
		}
		h = &(*h)->next;
	}
	pthread_rwlock_unlock(&handler_lock);
}

static void *mh_listen(__attribute__ ((unused)) void *arg)
{
	uint8_t msg[MAX_PKT_LEN];
	struct in6_pktinfo pktinfo;
	struct in6_addr haoa, rta;
	struct sockaddr_in6 addr;
	struct ip6_mh *mh;
	struct in6_addr_bundle addrs;
	ssize_t len;
	struct mh_handler *h;

	pthread_dbg("thread started");

	while (1) {
		pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
		len = mh_recv(msg, sizeof(msg), &addr, &pktinfo, &haoa, &rta);
		/* check if socket has closed */
		if (len == -EBADF)
			break;

		/* common validity check */
		if (len < 0 || (size_t)len < sizeof(struct ip6_mh))
			continue;

		addrs.src = &addr.sin6_addr;
		addrs.dst = &pktinfo.ipi6_addr;
		if (!IN6_IS_ADDR_UNSPECIFIED(&haoa)) {
			addrs.remote_coa = &haoa;
		} else {
			addrs.remote_coa = NULL;
		}
		if (!IN6_IS_ADDR_UNSPECIFIED(&rta)) {
			addrs.local_coa = &rta;
		} else {
			addrs.local_coa = NULL;
		}
		addrs.bind_coa = NULL;
		mh = (struct ip6_mh *) msg;
		pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
		pthread_rwlock_rdlock(&handler_lock);
		h = mh_handler_get(mh->ip6mh_type);
		if (h)
			h->recv(mh, len, &addrs, pktinfo.ipi6_ifindex);

		pthread_rwlock_unlock(&handler_lock);
	}
	pthread_exit(NULL);
}

int mh_init(void)
{
	pthread_mutexattr_t mattrs;
	int val;

	mh_sock.fd = socket(AF_INET6, SOCK_RAW, IPPROTO_MH);
	if (mh_sock.fd < 0) {
		syslog(LOG_ERR,
		       "Unable to open MH socket! "
		       "Do you have root permissions?");
		return mh_sock.fd;
	}
	val = 1;
	if (setsockopt(mh_sock.fd, IPPROTO_IPV6, IPV6_RECVPKTINFO,
		       &val, sizeof(val)) < 0)
		return -1;
	if (setsockopt(mh_sock.fd, IPPROTO_IPV6, IPV6_RECVDSTOPTS,
		       &val, sizeof(val)) < 0)
		return -1;
	if (setsockopt(mh_sock.fd, IPPROTO_IPV6, IPV6_RECVRTHDR,
		       &val, sizeof(val)) < 0)
		return -1;

	val = 4;
	if (setsockopt(mh_sock.fd, IPPROTO_RAW, IPV6_CHECKSUM,
		       &val, sizeof(val)) < 0)
		return -1;

	pthread_mutexattr_init(&mattrs);
	pthread_mutexattr_settype(&mattrs, PTHREAD_MUTEX_FAST_NP);
	if (pthread_mutex_init(&mh_sock.send_mutex, &mattrs) ||
	    pthread_rwlock_init(&handler_lock, NULL) ||
	    pthread_create(&mh_listener, NULL, mh_listen, NULL))
		return -1;
	return 0;
}

static const size_t _mh_len[] = {
	sizeof(struct ip6_mh_binding_request),
	sizeof(struct ip6_mh_home_test_init),
	sizeof(struct ip6_mh_careof_test_init),
	sizeof(struct ip6_mh_home_test),
	sizeof(struct ip6_mh_careof_test),
	sizeof(struct ip6_mh_binding_update),
	sizeof(struct ip6_mh_binding_ack),
	sizeof(struct ip6_mh_binding_error)
};

/**
 * mh_create - create a mobility header
 * @iov: vector
 * @type: mobility header type
 *
 * Initializes a new mobility header of type @type.  Stores pointer
 * and length in iovec vector @iov.  Returns pointer to newly created
 * header or NULL on error.
 **/
void *mh_create(struct iovec *iov, uint8_t type)
{
	struct ip6_mh *mh;

	if (type > IP6_MH_TYPE_MAX)
		return NULL;
	iov->iov_base = malloc(_mh_len[type]);
	if (iov->iov_base == NULL)
		return NULL;

	memset(iov->iov_base, 0, _mh_len[type]);

	mh = (struct ip6_mh *)iov->iov_base;
	mh->ip6mh_proto = IPPROTO_NONE;
	mh->ip6mh_hdrlen = 0; /* calculated after padding */
	mh->ip6mh_type = type;
	mh->ip6mh_reserved = 0;
	mh->ip6mh_cksum = 0; /* kernel does this for us */

	iov->iov_len = _mh_len[type];

	return mh;
}

/**
 * mh_create_opt_refresh_advice - create refresh advice option
 * @iov: vector
 * @interval: refresh interval
 *
 * Creates binding refresh advice option with @interval value.  Stores
 * pointer and length in iovec vector @iov.  Returns zero on success,
 * otherwise negative error value.
 **/
int mh_create_opt_refresh_advice(struct iovec *iov, uint16_t interval)
{
	struct ip6_mh_opt_refresh_advice *opt;
	size_t optlen = sizeof(struct ip6_mh_opt_refresh_advice);

	iov->iov_base = malloc(optlen);
	iov->iov_len = optlen;

	if (iov->iov_base == NULL)
		return -ENOMEM;

	opt = (struct ip6_mh_opt_refresh_advice *)iov->iov_base;

	opt->ip6mora_type = IP6_MHOPT_BREFRESH;
	opt->ip6mora_len = 2;
	opt->ip6mora_interval = interval;

	return 0;
}

/**
 * mh_create_opt_altcoa - create alternate care-of address option
 * @iov: vector
 * @addr: alternate care-of address
 *
 * Creates an alternate care-of address option.  Stores pointer and
 * length in iovec vector @iov.  Returns zero on success, otherwise
 * negative error value.
 **/
int mh_create_opt_altcoa(struct iovec *iov, struct in6_addr *addr)
{
	struct ip6_mh_opt_altcoa *opt;
	size_t optlen = sizeof(struct ip6_mh_opt_altcoa);

	iov->iov_base = malloc(optlen);
	iov->iov_len = optlen;

	if (iov->iov_base == NULL)
		return -ENOMEM;

	opt = (struct ip6_mh_opt_altcoa *)iov->iov_base;

	opt->ip6moa_type = IP6_MHOPT_ALTCOA;
	opt->ip6moa_len = 16;
	opt->ip6moa_addr = *addr;

	return 0;
}

/**
 * mh_create_opt_nonce_index - create nonce index option
 * @iov: vector
 * @home_nonce: home nonce value
 * @coa_nonce: coa nonce value
 *
 * Creates a nonce index option with @home_nonce and @coa_nonce
 * indices.  Stores pointer and length in iovec vector @iov.  Returns
 * zero on success, otherwise negative error value.
 **/
int mh_create_opt_nonce_index(struct iovec *iov, uint16_t home_nonce,
			      uint16_t coa_nonce)
{
	struct ip6_mh_opt_nonce_index *opt;
	size_t optlen = sizeof(struct ip6_mh_opt_nonce_index);

	iov->iov_base = malloc(optlen);
	iov->iov_len = optlen;

	if (iov->iov_base == NULL)
		return -ENOMEM;

	opt = (struct ip6_mh_opt_nonce_index *)iov->iov_base;

	opt->ip6moni_type = IP6_MHOPT_NONCEID;
	opt->ip6moni_len = 4;
	opt->ip6moni_home_nonce = htons(home_nonce);
	opt->ip6moni_coa_nonce = htons(coa_nonce);

	return 0;
}

/**
 * mh_create_opt_auth_data - create binding authorization data option
 * @iov: vector
 *
 * Creates a binding authorization data option with data set to zero.
 * Real data will be calculated after padding.  Stores pointer and
 * length in iovec vector @iov.  Returns zero on success, otherwise
 * negative error value.
 **/
int mh_create_opt_auth_data(struct iovec *iov)
{
	struct ip6_mh_opt_auth_data *opt;
	size_t optlen = sizeof(struct ip6_mh_opt_auth_data);

	iov->iov_base = malloc(optlen);
	iov->iov_len = optlen;

	if (iov->iov_base == NULL)
		return -ENOMEM;

	memset(iov->iov_base, 0, iov->iov_len);
	opt = (struct ip6_mh_opt_auth_data *)iov->iov_base;
	opt->ip6moad_type = IP6_MHOPT_BAUTH;
	opt->ip6moad_len = MIPV6_DIGEST_LEN;

	return 0;
}

/* We can use these safely, since they are only read and never change */
static const uint8_t _pad1[1] = { 0x00 };
static const uint8_t _pad2[2] = { 0x01, 0x00 };
static const uint8_t _pad3[3] = { 0x01, 0x01, 0x00 };
static const uint8_t _pad4[4] = { 0x01, 0x02, 0x00, 0x00 };
static const uint8_t _pad5[5] = { 0x01, 0x03, 0x00, 0x00, 0x00 };
static const uint8_t _pad6[6] = { 0x01, 0x04, 0x00, 0x00, 0x00, 0x00 };
static const uint8_t _pad7[7] = { 0x01, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00 };

static int create_opt_pad(struct iovec *iov, int pad)
{
	if (pad == 2)
		iov->iov_base = (void *)_pad2;
	else if (pad == 4)
		iov->iov_base = (void *)_pad4;
	else if (pad == 6)
		iov->iov_base = (void *)_pad6;
	/* Odd pads do not occur with current spec, so test them last */
	else if (pad == 1)
		iov->iov_base = (void *)_pad1;
	else if (pad == 3)
		iov->iov_base = (void *)_pad3;
	else if (pad == 5)
		iov->iov_base = (void *)_pad5;
	else if (pad == 7)
		iov->iov_base = (void *)_pad7;

	iov->iov_len = pad;

	return 0;
}

int mh_create_opt_mob_net_prefix(struct iovec *iov, int mnp_count,
				 struct list_head *mnps)
{
	int optlen = (mnp_count * sizeof(struct ip6_mh_opt_mob_net_prefix) +
		      (mnp_count - 1) * sizeof(_pad4));
	struct list_head *l;
	int i = 0;
	uint8_t *data;
	iov->iov_base = malloc(optlen);
	iov->iov_len = optlen;

	if (iov->iov_base == NULL)
		return -ENOMEM;

	memset(iov->iov_base, 0, iov->iov_len);
	data = (uint8_t *)iov->iov_base;

	list_for_each(l, mnps) {
		struct prefix_list_entry *p;
		struct ip6_mh_opt_mob_net_prefix *mnp;

		p = list_entry(l, struct prefix_list_entry, list);
		mnp = (struct ip6_mh_opt_mob_net_prefix *)data;

		mnp->ip6mnp_type = IP6_MHOPT_MOB_NET_PRFX;
		mnp->ip6mnp_len = 18;
		mnp->ip6mnp_prefix_len = p->ple_plen;
		mnp->ip6mnp_prefix = p->ple_prefix;

		data += sizeof(struct ip6_mh_opt_mob_net_prefix);

		/* do internal padding here, so one iovec for MNPs is enough */
		if (++i < mnp_count) {
		  memcpy(data, _pad4, sizeof(_pad4));
		  data += sizeof(_pad4);
		}
	}
	return 0;
}

static size_t mh_length(struct iovec *vec, int count)
{
	size_t len = 0;
	int i;

	for (i = 0; i < count; i++) {
		len += vec[i].iov_len;
	}
	return len;
}

static inline int optpad(int xn, int y, int offset)
{
	return ((y - offset) & (xn - 1));
}

static int mh_try_pad(const struct iovec *in, struct iovec *out, int count)
{
	size_t len = 0;
	int m, n = 1, pad = 0;
	struct ip6_mh_opt *opt;

	out[0].iov_len = in[0].iov_len;
	out[0].iov_base = in[0].iov_base;
	len += in[0].iov_len;

	for (m = 1; m < count; m++) {
		opt = (struct ip6_mh_opt *)in[m].iov_base;
		switch (opt->ip6mhopt_type) {
		case IP6_MHOPT_BREFRESH:
			pad = optpad(2, 0, len); /* 2n */
			break;
		case IP6_MHOPT_ALTCOA:
			pad = optpad(8, 6, len); /* 8n+6 */
			break;
		case IP6_MHOPT_NONCEID:
			pad = optpad(2, 0, len); /* 2n */
			break;
		case IP6_MHOPT_BAUTH:
			pad = optpad(8, 2, len); /* 8n+2 */
			break;
		case IP6_MHOPT_MOB_NET_PRFX:
			pad = optpad(8, 4, len); /* 8n+4 */
			break;
		}
		if (pad > 0) {
			create_opt_pad(&out[n++], pad);
			len += pad;
		}
		len += in[m].iov_len;
		out[n].iov_len = in[m].iov_len;
		out[n].iov_base = in[m].iov_base;
		n++;
	}
	if (count == 1) {
		pad = optpad(8, 0, len);
		create_opt_pad(&out[n++], pad);
	}

	return n;
}

static int calculate_auth_data(const struct iovec *iov, int iovlen,
			       const struct in6_addr *coa,
			       const struct in6_addr *cn,
			       const uint8_t *key, uint8_t *digest)
{
	uint8_t buf[HMAC_SHA1_HASH_LEN];
	int i;

#ifdef HAVE_LIBCRYPTO
	unsigned int len = HMAC_SHA1_HASH_LEN;
	HMAC_CTX ctx;
	const EVP_MD *evp_md = EVP_sha1();

	HMAC_CTX_init(&ctx);
	HMAC_Init_ex(&ctx, key, HMAC_SHA1_KEY_SIZE, evp_md, NULL);

	HMAC_Update(&ctx, (uint8_t *)coa, sizeof(*coa));
	HMAC_Update(&ctx, (uint8_t *)cn, sizeof(*coa));
	for (i = 0; i < iovlen; i++) {
		HMAC_Update(&ctx, (uint8_t *)iov[i].iov_base, iov[i].iov_len);
	}
	HMAC_Final(&ctx, buf, &len);
	HMAC_CTX_cleanup(&ctx);
#else
	HMAC_SHA1_CTX ctx;

	HMAC_SHA1_init(&ctx, key, HMAC_SHA1_KEY_SIZE);
	HMAC_SHA1_update(&ctx, (uint8_t *)coa, sizeof(*coa));
	HMAC_SHA1_update(&ctx, (uint8_t *)cn, sizeof(*coa));
	for (i = 0; i < iovlen; i++) {
		HMAC_SHA1_update(&ctx, (uint8_t *)iov[i].iov_base, 
				 iov[i].iov_len);
	}
	HMAC_SHA1_final(&ctx, buf);
#endif
	memcpy(digest, buf, MIPV6_DIGEST_LEN);
	return 0;
}

int mh_verify_auth_data(const void *msg, int len, const void *opt,
			const struct in6_addr *coa,
			const struct in6_addr *cn,
			const uint8_t *key)
{
	struct ip6_mh_opt_auth_data *bauth;
	struct iovec iov;
	uint8_t adata[MIPV6_DIGEST_LEN];

	bauth = (struct ip6_mh_opt_auth_data *)opt;
	iov.iov_base = (void *)msg;
	iov.iov_len = len - bauth->ip6moad_len;
	calculate_auth_data(&iov, 1, coa, cn, key, adata);
	if (memcmp(adata, bauth->ip6moad_data, bauth->ip6moad_len)) {
		/* binding auth data error */
		return -1;
	}
	return 0;
}

/**
 * mh_send - send mobility header message
 * @addrs: bundle of addresses
 * @mh_vec: scatter/gather array
 * @iovlen: array block count
 * @bind_key: key for calculating binding auth. data
 *
 * Sends a mobility header message to @dst with @src source address.
 * Mobility header is created from the @mh_vec vector array created by
 * the caller and initialized with mh_create() and mh_create_opt_*()
 * calls.  Padding is done automatically and mobility header length is
 * set.  Binding authorization data is calculated if present.  Returns
 * number of bytes sent on success, otherwise negative error value.
 **/
int mh_send(const struct in6_addr_bundle *addrs, const struct iovec *mh_vec,
	    int iovlen, const uint8_t *bind_key, int oif)
{
	struct ip6_mh_opt_auth_data lbad;
	struct sockaddr_in6 daddr;
	
	struct iovec iov[2*(IP6_MHOPT_MAX+1)];
	struct msghdr msg;
	struct cmsghdr *cmsg;
	int cmsglen;
	struct in6_pktinfo pinfo;
	int ret = 0, on = 1;
	struct ip6_mh *mh;
	int iov_count;
	socklen_t rthlen = 0;

	memset(iov, 0, (2*(IP6_MHOPT_MAX+1))*sizeof(struct iovec));
	iov_count = mh_try_pad(mh_vec, iov, iovlen);
	mh = (struct ip6_mh *)iov[0].iov_base;
	mh->ip6mh_hdrlen = (mh_length(iov, iov_count) >> 3) - 1;

	/*
	 * We use MH out policy for all address. Then we should update it
	 * to refresh its bundle in kernel to be used with correct
	 * route, IPsec SA and neighbor cache entry for the destination.
	 * IKE daemon does the same thing for rekeying process.
	 */
        if (xfrm_cn_policy_mh_out_touch(1) < 0) {
                MDBG("MH out policy touch failed: BA for "
                     "%x:%x:%x:%x:%x:%x:%x:%x\n", NIP6ADDR(addrs->dst));
        }

	MDBG("sending MH type %d\n"
	     "from %x:%x:%x:%x:%x:%x:%x:%x\n"
	     "to %x:%x:%x:%x:%x:%x:%x:%x\n",
	     mh->ip6mh_type, NIP6ADDR(addrs->src), NIP6ADDR(addrs->dst));
	    
	if (addrs->local_coa)
		MDBG("local CoA %x:%x:%x:%x:%x:%x:%x:%x\n",
		     NIP6ADDR(addrs->local_coa));

	if (addrs->remote_coa)
		MDBG("remote CoA %x:%x:%x:%x:%x:%x:%x:%x\n",
		     NIP6ADDR(addrs->remote_coa));

	if (bind_key) {
		assert(iov_count > 1); 
		struct ip6_mh_opt_auth_data *bauth;
		struct iovec *biov;
		struct in6_addr *cn = NULL;
		MDBG("Adding bind auth data\n");
		if (mh->ip6mh_type == IP6_MH_TYPE_BU)
			cn = addrs->dst;
		else 
			cn = addrs->src;
		assert(addrs->bind_coa != NULL && cn != NULL); 
		biov = &iov[iov_count - 1];
		bauth = (struct ip6_mh_opt_auth_data *)biov->iov_base;
		
		if (bauth->ip6moad_type == IP6_MHOPT_BAUTH) {
			size_t orig_len = biov->iov_len;
			
			MDBG("Adding auth_data\n");
			memcpy(&lbad, bauth, sizeof(lbad));
			
			/* temporarily set iov_len to option header
			 * length for auth data calculation */
			biov->iov_len -= MIPV6_DIGEST_LEN;
			biov->iov_base = &lbad;
			calculate_auth_data(iov, iov_count, addrs->bind_coa,
					    cn, bind_key, lbad.ip6moad_data);
			biov->iov_len = orig_len;
		}
	}

	memset(&daddr, 0, sizeof(struct sockaddr_in6));
	daddr.sin6_family = AF_INET6;
	daddr.sin6_addr = *addrs->dst;
	daddr.sin6_port = htons(IPPROTO_MH);

	memset(&pinfo, 0, sizeof(pinfo));
	pinfo.ipi6_addr = *addrs->src;
	pinfo.ipi6_ifindex = oif;

	cmsglen = CMSG_SPACE(sizeof(pinfo));
	if (addrs->remote_coa != NULL) {
		rthlen = inet6_rth_space(IPV6_RTHDR_TYPE_2, 1);
		if (!rthlen) {
			MDBG("inet6_rth_space error\n");
			return -1;
		}
		cmsglen += CMSG_SPACE(rthlen);
	}
	cmsg = malloc(cmsglen);
	if (cmsg == NULL) {
		MDBG("malloc failed\n");
		return -ENOMEM;
	}
	memset(cmsg, 0, cmsglen);
	memset(&msg, 0, sizeof(msg));

	msg.msg_control = cmsg;
	msg.msg_controllen = cmsglen;
	msg.msg_iov = iov;
	msg.msg_iovlen = iov_count;
	msg.msg_name = (void *)&daddr;
	msg.msg_namelen = sizeof(daddr);
	
	cmsg = CMSG_FIRSTHDR(&msg);
	cmsg->cmsg_len = CMSG_LEN(sizeof(pinfo));
	cmsg->cmsg_level = IPPROTO_IPV6;
	cmsg->cmsg_type = IPV6_PKTINFO;
	memcpy(CMSG_DATA(cmsg), &pinfo, sizeof(pinfo));

	if (addrs->remote_coa != NULL) {
		void *rthp;

		cmsg = CMSG_NXTHDR(&msg, cmsg);
		if (cmsg == NULL) {
			free(msg.msg_control);
			MDBG("internal error\n");
			return -2;
		}
		cmsg->cmsg_len = CMSG_LEN(rthlen);
		cmsg->cmsg_level = IPPROTO_IPV6;
		cmsg->cmsg_type = IPV6_RTHDR;
		rthp = CMSG_DATA(cmsg);
		rthp = inet6_rth_init(rthp, rthlen, IPV6_RTHDR_TYPE_2, 1);
		if (rthp == NULL) {
			free(msg.msg_control);
			MDBG("inet6_rth_init error\n");
			return -3;
		}
		inet6_rth_add(rthp, addrs->remote_coa);
		rthp = NULL;
	}

	pthread_mutex_lock(&mh_sock.send_mutex);
	setsockopt(mh_sock.fd, IPPROTO_IPV6, IPV6_PKTINFO,
		   &on, sizeof(int));
	ret = sendmsg(mh_sock.fd, &msg, 0);
	if (ret < 0)
		dbg("sendmsg: %s\n", strerror(errno));

	pthread_mutex_unlock(&mh_sock.send_mutex);

	free(msg.msg_control);

	statistics_inc(&mipl_stat, MIPL_STATISTICS_OUT_MH);
	return ret;
}

static int mh_opt_len_chk(uint8_t type, int len)
{
	switch (type) {
	case IP6_MHOPT_BREFRESH:
		return len != sizeof(struct ip6_mh_opt_refresh_advice);
	case IP6_MHOPT_ALTCOA:
		return len != sizeof(struct ip6_mh_opt_altcoa);
	case IP6_MHOPT_NONCEID:
		return len != sizeof(struct ip6_mh_opt_nonce_index);
	case IP6_MHOPT_BAUTH:
		return len != sizeof(struct ip6_mh_opt_auth_data);
	case IP6_MHOPT_MOB_NET_PRFX:
		return len != sizeof(struct ip6_mh_opt_mob_net_prefix);
	case IP6_MHOPT_PADN:
	default:
		return 0;
	}	
}

		
/**
 * mh_opt_parse - parse mobility options
 * @mh_opts: pointer to mh_options structure
 * @opts: pointer to beginning of options
 * @optlen: total length of options in bytes
 *
 * Parses mobility options from a buffer @opts.  Allocates an array of
 * ip6_mh_opt pointers pointing to beginnings of mobility options.  Array
 * is indexed with ip6mhopt_type values.  %IP6_MHOPT_PAD1 and
 * %IP6_MHOPT_PADN are ignored.  Returns number of options parsed
 * (excluding pad) on success, otherwise negative error value.
 **/
int mh_opt_parse(const struct ip6_mh *mh, ssize_t len, ssize_t offset,
		 struct mh_options *mh_opts)
{
	const uint8_t *opts = (uint8_t *) mh;
	ssize_t left = len - offset;
	ssize_t i = offset;
	int ret = 0;
	int bauth = 0;

	memset(mh_opts, 0, sizeof(*mh_opts));
	while (left > 0) {
		struct ip6_mh_opt *op = (struct ip6_mh_opt *)&opts[i];
		/* make sure the binding authorization data is last */
		if (bauth)
			return -EINVAL;

		if (op->ip6mhopt_type == IP6_MHOPT_PAD1) {
			left--;
			i++;
			continue;
		} 
		if ((size_t)left < sizeof(struct ip6_mh_opt) ||
		    mh_opt_len_chk(op->ip6mhopt_type, op->ip6mhopt_len + 2)) {
			syslog(LOG_ERR,
			       "Kernel failed to catch malformed Mobility"
			       "Option type %d. Update kernel!",
			       op->ip6mhopt_type);
			return -EINVAL;
		}
		if (op->ip6mhopt_type == IP6_MHOPT_PADN) {
			left -= op->ip6mhopt_len + 2;
			i += op->ip6mhopt_len + 2;
			continue;
		}
		if (op->ip6mhopt_type <= IP6_MHOPT_MAX) {
			if (op->ip6mhopt_type == IP6_MHOPT_BAUTH)
				bauth = 1;

			if (!mh_opts->opts[op->ip6mhopt_type])
				mh_opts->opts[op->ip6mhopt_type] = i;
			else if (mh_opts_dup_ok[op->ip6mhopt_type])
				mh_opts->opts_end[op->ip6mhopt_type] = i;
			else
				return -EINVAL;
			ret++;
		}
		left -= op->ip6mhopt_len + 2;
		i += op->ip6mhopt_len + 2;
	}
	return ret;
}

#define CMSG_BUF_LEN 128

/**
 * mh_recv - receive mobility header signaling message
 * @msg: buffer to store message in
 * @addr: packet source address
 * @pkt_info: packet destination and interface
 * @haoaddr: address in home address option (if any)
 * @rtaddr: address in routing header type 2 (if any)
 *
 * Waits for a packet from mobility header @mh_sock.fd socket.
 * Stores information about the packet to @addr, @pkt_info, @hoa, and
 * @rtaddr.  Packet data (i.e. mobility header) is stored in @msg.
 * Returns length of packet data received, or negative error value on
 * failure.
 **/
ssize_t mh_recv(unsigned char *msg, size_t msglen,
		struct sockaddr_in6 *addr, struct in6_pktinfo *pkt_info,
		struct in6_addr *haoaddr, struct in6_addr *rtaddr)
{
	struct ip6_mh *mh;
	struct msghdr mhdr;
	struct cmsghdr *cmsg;
	struct iovec iov;
	static unsigned char chdr[CMSG_BUF_LEN];
	void *databufp = NULL;
	int sockfd = mh_sock.fd;
	socklen_t hao_len;
	ssize_t len;

	iov.iov_len = msglen;
	iov.iov_base = (unsigned char *)msg;

	mhdr.msg_name = (void *)addr;
	mhdr.msg_namelen = sizeof(struct sockaddr_in6);
	mhdr.msg_iov = &iov;
	mhdr.msg_iovlen = 1;
	mhdr.msg_control = (void *)&chdr;
	mhdr.msg_controllen = CMSG_BUF_LEN;

	if ((len = recvmsg(sockfd, &mhdr, 0)) < 0)
		return -errno;

	memset(haoaddr, 0, sizeof(*haoaddr));
	memset(rtaddr, 0, sizeof(*rtaddr));

	statistics_inc(&mipl_stat, MIPL_STATISTICS_IN_MH);

	for (cmsg = CMSG_FIRSTHDR(&mhdr); cmsg;
	     cmsg = CMSG_NXTHDR(&mhdr, cmsg)) {
		int ret = 0;

		if (cmsg->cmsg_level != IPPROTO_IPV6)
			continue;
		switch (cmsg->cmsg_type) {
		case IPV6_PKTINFO:
			memcpy(pkt_info, CMSG_DATA(cmsg), sizeof(*pkt_info));
			break;
		case IPV6_DSTOPTS:
			ret = inet6_opt_find(CMSG_DATA(cmsg), cmsg->cmsg_len,
					     0, IP6OPT_HOME_ADDRESS,
					     &hao_len, &databufp);
			if (ret >= 0 && databufp != NULL &&
			    hao_len == sizeof(struct in6_addr)) {
				*haoaddr = *(struct in6_addr *) databufp;
			}
			break;
		case IPV6_RTHDR:
			if (inet6_rth_gettype(CMSG_DATA(cmsg)) ==
			    IPV6_RTHDR_TYPE_2) {
				struct in6_addr *seg = NULL;
				/* Kernel already processed routing
				 * header type 2 for us */
				seg = inet6_rth_getaddr(CMSG_DATA(cmsg), 0);
				if (!seg)
					MDBG("Invalid rth\n");
				else
					*rtaddr = *seg;
				statistics_inc(&mipl_stat, MIPL_STATISTICS_IN_RH2);
			}
			break;
		}
	}
	mh = (struct ip6_mh *)msg;

	if (mh->ip6mh_type > IP6_MH_TYPE_MAX) {
		struct in6_addr *src, *dst, *hoa;
		if (!IN6_IS_ADDR_UNSPECIFIED(rtaddr))
			src = rtaddr;
		else
			src = &pkt_info->ipi6_addr;

		if (!IN6_IS_ADDR_UNSPECIFIED(haoaddr)) {
			hoa = &addr->sin6_addr;
			dst = haoaddr;
		} else {
			hoa = NULL;
			dst = &addr->sin6_addr;
		}
		mh_send_be(dst, hoa, src, IP6_MH_BES_UNKNOWN_MH,
			   pkt_info->ipi6_ifindex);
		return -EINVAL;
	}

	/* No need to perform any other validity checks, since kernel
	 * does this for us. */

	statistics_inc(&mipl_stat, MIPL_STATISTICS_IN_MH);
	return len;
}

void mh_send_be(struct in6_addr *dst, struct in6_addr *hoa, 
		struct in6_addr *src, uint8_t status, int iif)
{
	struct ip6_mh_binding_error *be;
	struct iovec iov;
	struct in6_addr_bundle out;

	if (IN6_IS_ADDR_UNSPECIFIED(dst) ||
	    IN6_IS_ADDR_LOOPBACK(dst) ||
	    IN6_IS_ADDR_MULTICAST(dst)) {
		MDBG("Omit BE for non-unicast "
		     "%x:%x:%x:%x:%x:%x:%x:%x\n", NIP6ADDR(dst));
		return;
	}

	out.remote_coa = NULL;
	out.local_coa = NULL;
	be = mh_create(&iov, IP6_MH_TYPE_BERROR);
	if (!be)
		return;
	be->ip6mhbe_status = status;
	out.src = src;
	out.dst = dst;
	if (hoa)
		be->ip6mhbe_homeaddr = *hoa;

	mh_send(&out, &iov, 1, NULL, iif);
	free_iov_data(&iov, 1);
	statistics_inc(&mipl_stat, MIPL_STATISTICS_OUT_BE);
}

/* Check if binding has been used recently and send a binding refresh
 * request to get a new BU
 */
void mh_send_brr(struct in6_addr *mn_addr, struct in6_addr *local)
{
	struct iovec mh_vec;
	struct in6_addr_bundle addrs;
	struct timespec now;
	long last_used;

	clock_gettime(CLOCK_REALTIME, &now);

	last_used = xfrm_last_used(mn_addr, local, IPPROTO_ROUTING, &now);
	if (last_used < 0)
		return;
	if (last_used > CN_BRR_TIME_THRESH) {
		dbg("Binding is about to expire, no need to send a BRR\n");
		return;
	}
	/* Should we check for a BUL entry here? */
	dbg("Binding is about to expire, sending a BRR\n");
	addrs.src = local;
	addrs.dst = mn_addr;
	addrs.remote_coa = NULL;
	addrs.local_coa = NULL;
	
	if (!mh_create(&mh_vec, IP6_MH_TYPE_BRR))
		return;
	
	mh_send(&addrs, &mh_vec, 1, NULL, 0);
	free_iov_data(&mh_vec, 1);
	statistics_inc(&mipl_stat, MIPL_STATISTICS_OUT_BRR);
}

void mh_send_ba(const struct in6_addr_bundle *addrs, uint8_t status,
		uint8_t flags, uint16_t sequence, 
		const struct timespec *lifetime, const uint8_t *key, int iif)
{
	int iovlen = 1;
	struct ip6_mh_binding_ack *ba;
	struct iovec mh_vec[2];

	MDBG("status %s (%d)\n", mh_ba_status_to_str(status), status);

	ba = mh_create(mh_vec, IP6_MH_TYPE_BACK);
	if (!ba)
		return;

	ba->ip6mhba_status = status;
	ba->ip6mhba_flags = flags;
	ba->ip6mhba_seqno = htons(sequence);
	ba->ip6mhba_lifetime = htons(lifetime->tv_sec >> 2);

	if (status < IP6_MH_BAS_UNSPECIFIED && !conf.NonVolatileBindingCache) {
		struct timespec refresh;
		tsclear(refresh);
		if (conf.pmgr.use_bradv(addrs->dst, addrs->bind_coa,
					addrs->src, lifetime, &refresh) &&
		    tsbefore(*lifetime, refresh))
			mh_create_opt_refresh_advice(&mh_vec[iovlen++], 
						     refresh.tv_sec);
	}
	if (key)
		mh_create_opt_auth_data(&mh_vec[iovlen++]);
	mh_send(addrs, mh_vec, iovlen, key, iif);
	free_iov_data(mh_vec, iovlen);
	statistics_inc(&mipl_stat, MIPL_STATISTICS_OUT_BA);
}

/* Main BU parser, used by both HA (H flag set) and CN (H flag not set).
 * Additional specific checks are performed for HA and CN via two
 * specific helpers: ha_bu_check() (ha.c) and cn_bu_check() (cn.c). */
int mh_bu_parse(struct ip6_mh_binding_update *bu, ssize_t len,
		const struct in6_addr_bundle *in_addrs,
		struct in6_addr_bundle *out_addrs,
		struct mh_options *mh_opts,
		struct timespec *lifetime)
{
	struct in6_addr *our_addr, *peer_addr, *remote_coa;
	struct ip6_mh_opt_altcoa *alt_coa;

	MDBG("Binding Update Received\n");
	if (len < 0 || (size_t)len < sizeof(struct ip6_mh_binding_update) ||
	    mh_opt_parse(&bu->ip6mhbu_hdr, len,
			 sizeof(struct ip6_mh_binding_update), mh_opts) < 0)
		return -1;
			 
	peer_addr = in_addrs->src;
	if (!in6_is_addr_routable_unicast(peer_addr))
		return -1;

	remote_coa = in_addrs->remote_coa;
	if (remote_coa && !IN6_ARE_ADDR_EQUAL(remote_coa, peer_addr))
		out_addrs->remote_coa = remote_coa;
	else
		out_addrs->remote_coa = NULL;

	alt_coa = mh_opt(&bu->ip6mhbu_hdr, mh_opts, IP6_MHOPT_ALTCOA);
	if (alt_coa)
		out_addrs->bind_coa = &alt_coa->ip6moa_addr;
	else
		out_addrs->bind_coa = in_addrs->remote_coa;

	our_addr = in_addrs->dst;
	tsclear(*lifetime);
	if (out_addrs->bind_coa) {
		if (!in6_is_addr_routable_unicast(out_addrs->bind_coa))
			return -1;
		if (!IN6_ARE_ADDR_EQUAL(out_addrs->bind_coa, peer_addr)) {
			/* check that there is no circular reference */
			if (bce_exists(our_addr, out_addrs->bind_coa))
				return -1;
			tssetsec(*lifetime, ntohs(bu->ip6mhbu_lifetime) << 2);
		}
	}
	/* Use Home address of MN for calculating BU and BA auth data
	 * for deregs. */
	if (!out_addrs->bind_coa)
		out_addrs->bind_coa = in_addrs->src;

	out_addrs->src = in_addrs->dst;
	out_addrs->dst = in_addrs->src;
	out_addrs->local_coa = NULL;

	return 0;
}

void mh_cleanup(void)
{
	close(mh_sock.fd);
	pthread_cancel(mh_listener);
	pthread_join(mh_listener, NULL);
}

/* based on http://www.iana.org/assignments/mobility-parameters (last
 * updated 2008-10-10 version), but only for protocols we support, i.e.
 * MIPv6 and NEMO. */
static const char *mh_ba_status_str[256] = {
	[IP6_MH_BAS_ACCEPTED]            = "Binding Update accepted",
	[IP6_MH_BAS_PRFX_DISCOV]         = "Accepted but prefix discovery necessary",
	[IP6_MH_BAS_UNSPECIFIED]         = "Reason unspecified",
	[IP6_MH_BAS_PROHIBIT]            = "Administratively prohibited",
	[IP6_MH_BAS_INSUFFICIENT]        = "Insufficient resources",
	[IP6_MH_BAS_HA_NOT_SUPPORTED]    = "Home registration not supported",
	[IP6_MH_BAS_NOT_HOME_SUBNET]     = "Not home subnet",
	[IP6_MH_BAS_NOT_HA]              = "Not home agent for this mobile node",
	[IP6_MH_BAS_DAD_FAILED]          = "Duplicate Address Detection failed",
	[IP6_MH_BAS_SEQNO_BAD]           = "Sequence number out of window",
	[IP6_MH_BAS_HOME_NI_EXPIRED]     = "Expired home nonce index",
	[IP6_MH_BAS_COA_NI_EXPIRED]      = "Expired care-of nonce index",
	[IP6_MH_BAS_NI_EXPIRED]          = "Expired nonces",
	[IP6_MH_BAS_REG_NOT_ALLOWED]     = "Registration type change disallowed",
	[IP6_MH_BAS_MR_OP_NOT_PERMITTED] = "Mobile Router Operation not permitted",
	[IP6_MH_BAS_INVAL_PRFX]          = "Invalid Prefix",
	[IP6_MH_BAS_NOT_AUTH_FOR_PRFX]   = "Not Authorized for Prefix",
	[IP6_MH_BAS_FWDING_FAILED]       = "Forwarding Setup failed",
};

const char *mh_ba_status_to_str(int status)
{
	static char unknown[] = "unknown by UMIP";
	const char *s;

	s = mh_ba_status_str[status];
	if (s == NULL)
		s = unknown;

	return s;
}
