[PATCH] Add implementations of the inet6_opt* and inet6_rth* functions (RFC3542)

[Hasso Tepper <hasso@xxxxxxxxx>]

# HG changeset patch
# User Hasso Tepper <hasso@estpak.ee>
# Date 1179982956 -10800
# Node ID cf620ca047509c846553392c79948f1bbe94c26c
# Parent  a362d641c6d4dc87fc3bb121fe970825ae67ad4d
Add implementations of the inet6_opt* and inet6_rth* functions (RFC3542).

The patch brings in the part of the RFC3542 API we already have defined.
But implementations were missing which confuses third party apps. Note,
that this doesn't complete RFC3542 API, socket options are still missing.
I'm going to work on this probably unless someone stops me.

Obtained-from: KAME

diff -r a362d641c6d4 -r cf620ca04750 lib/libc/net/ip6opt.c
--- a/lib/libc/net/ip6opt.c	Wed May 23 21:42:13 2007 +0300
+++ b/lib/libc/net/ip6opt.c	Thu May 24 08:02:36 2007 +0300
@@ -366,3 +366,225 @@ inet6_insert_padopt(u_char *p, int len)
 		 return;
 	}
 }
+
+/*
+ * The following functions are defined in RFC3542, which is a successor
+ * of RFC2292.
+ */
+
+int
+inet6_opt_init(void *extbuf, socklen_t extlen)
+{
+	struct ip6_ext *ext = (struct ip6_ext *)extbuf;
+
+	if (extlen < 0 || (extlen % 8))
+		return (-1);
+
+	if (ext) {
+		if (extlen == 0)
+			return (-1);
+		ext->ip6e_len = (extlen >> 3) - 1;
+	}
+
+	return (2);		/* sizeof the next and the length fields */
+}
+
+int
+inet6_opt_append(void *extbuf, socklen_t extlen, int offset, u_int8_t type,
+		 socklen_t len, u_int8_t align, void **databufp)
+{
+	int currentlen = offset, padlen = 0;
+
+	/*
+	 * The option type must have a value from 2 to 255, inclusive.
+	 * (0 and 1 are reserved for the Pad1 and PadN options, respectively.)
+	 */
+#if 0 /* always false */
+	if (type < 2 || type > 255)
+#else
+	if (type < 2)
+#endif
+		return (-1);
+
+	/*
+	 * The option data length must have a value between 0 and 255,
+	 * inclusive, and is the length of the option data that follows.
+	 */
+	if (len < 0 || len > 255)
+		return (-1);
+
+	/*
+	 * The align parameter must have a value of 1, 2, 4, or 8.
+	 * The align value can not exceed the value of len.
+	 */
+	if (align != 1 && align != 2 && align != 4 && align != 8)
+		return (-1);
+	if (align > len)
+		return (-1);
+
+	/* Calculate the padding length. */
+	currentlen += 2 + len;	/* 2 means "type + len" */
+	if (currentlen % align)
+		padlen = align - (currentlen % align);
+
+	/* The option must fit in the extension header buffer. */
+	currentlen += padlen;
+	if (extlen &&		/* XXX: right? */
+	    currentlen > extlen)
+		return (-1);
+
+	if (extbuf) {
+		u_int8_t *optp = (u_int8_t *)extbuf + offset;
+
+		if (padlen == 1) {
+			/* insert a Pad1 option */
+			*optp = IP6OPT_PAD1;
+			optp++;
+		} else if (padlen > 0) {
+			/* insert a PadN option for alignment */
+			*optp++ = IP6OPT_PADN;
+			*optp++ = padlen - 2;
+			memset(optp, 0, padlen - 2);
+			optp += (padlen - 2);
+		}
+
+		*optp++ = type;
+		*optp++ = len;
+
+		*databufp = optp;
+	}
+
+	return (currentlen);
+}
+
+int
+inet6_opt_finish(void *extbuf, socklen_t extlen, int offset)
+{
+	int updatelen = offset > 0 ? (1 + ((offset - 1) | 7)) : 0;;
+
+	if (extbuf) {
+		u_int8_t *padp;
+		int padlen = updatelen - offset;
+
+		if (updatelen > extlen)
+			return (-1);
+
+		padp = (u_int8_t *)extbuf + offset;
+		if (padlen == 1)
+			*padp = IP6OPT_PAD1;
+		else if (padlen > 0) {
+			*padp++ = IP6OPT_PADN;
+			*padp++ = (padlen - 2);
+			memset(padp, 0, padlen - 2);
+		}
+	}
+
+	return (updatelen);
+}
+
+int
+inet6_opt_set_val(void *databuf, int offset, void *val, socklen_t vallen)
+{
+
+	memcpy((u_int8_t *)databuf + offset, val, vallen);
+	return (offset + vallen);
+}
+
+int
+inet6_opt_next(void *extbuf, socklen_t extlen, int offset, u_int8_t *typep,
+	       socklen_t *lenp, void **databufp)
+{
+	u_int8_t *optp, *lim;
+	int optlen;
+
+	/* Validate extlen. XXX: is the variable really necessary?? */
+	if (extlen == 0 || (extlen % 8))
+		return (-1);
+	lim = (u_int8_t *)extbuf + extlen;
+
+	/*
+	 * If this is the first time this function called for this options
+	 * header, simply return the 1st option.
+	 * Otherwise, search the option list for the next option.
+	 */
+	if (offset == 0)
+		optp = (u_int8_t *)((struct ip6_hbh *)extbuf + 1);
+	else
+		optp = (u_int8_t *)extbuf + offset;
+
+	/* Find the next option skipping any padding options. */
+	while (optp < lim) {
+		switch(*optp) {
+		case IP6OPT_PAD1:
+			optp++;
+			break;
+		case IP6OPT_PADN:
+			if ((optlen = ip6optlen(optp, lim)) == 0)
+				goto optend;
+			optp += optlen;
+			break;
+		default:	/* found */
+			if ((optlen = ip6optlen(optp, lim)) == 0)
+				goto optend;
+			*typep = *optp;
+			*lenp = optlen - 2;
+			*databufp = optp + 2;
+			return (optp + optlen - (u_int8_t *)extbuf);
+		}
+	}
+
+  optend:
+	*databufp = NULL; /* for safety */
+	return (-1);
+}
+
+int
+inet6_opt_find(void *extbuf, socklen_t extlen, int offset, u_int8_t type,
+	       socklen_t *lenp, void **databufp)
+{
+	u_int8_t *optp, *lim;
+	int optlen;
+
+	/* Validate extlen. XXX: is the variable really necessary?? */
+	if (extlen == 0 || (extlen % 8))
+		return (-1);
+	lim = (u_int8_t *)extbuf + extlen;
+
+	/*
+	 * If this is the first time this function called for this options
+	 * header, simply return the 1st option.
+	 * Otherwise, search the option list for the next option.
+	 */
+	if (offset == 0)
+		optp = (u_int8_t *)((struct ip6_hbh *)extbuf + 1);
+	else
+		optp = (u_int8_t *)extbuf + offset;
+
+	/* Find the specified option */
+	while (optp < lim) {
+		if ((optlen = ip6optlen(optp, lim)) == 0)
+			goto optend;
+
+		if (*optp == type) { /* found */
+			*lenp = optlen - 2;
+			*databufp = optp + 2;
+			return (optp + optlen - (u_int8_t *)extbuf);
+		}
+
+		optp += optlen;
+	}
+
+  optend:
+	*databufp = NULL; /* for safety */
+	return (-1);
+}
+
+int
+inet6_opt_get_val(void *databuf, int offset, void *val, socklen_t vallen)
+{
+
+	/* we can't assume alignment here */
+	memcpy(val, (u_int8_t *)databuf + offset, vallen);
+
+	return (offset + vallen);
+}
diff -r a362d641c6d4 -r cf620ca04750 lib/libc/net/rthdr.c
--- a/lib/libc/net/rthdr.c	Wed May 23 21:42:13 2007 +0300
+++ b/lib/libc/net/rthdr.c	Thu May 24 08:02:36 2007 +0300
@@ -296,3 +296,161 @@ inet6_rthdr_getflags(const struct cmsghd
 	return -1;
     }
 }
+
+/*
+ * RFC3542 (2292bis) API
+ */
+
+socklen_t
+inet6_rth_space(int type, int segments)
+{
+	switch (type) {
+	case IPV6_RTHDR_TYPE_0:
+		return (((segments * 2) + 1) << 3);
+	default:
+		return (0);	/* type not suppported */
+	}
+}
+
+void *
+inet6_rth_init(void *bp, socklen_t bp_len, int type, int segments)
+{
+	struct ip6_rthdr *rth = (struct ip6_rthdr *)bp;
+	struct ip6_rthdr0 *rth0;
+
+	switch (type) {
+	case IPV6_RTHDR_TYPE_0:
+		/* length validation */
+		if (bp_len < inet6_rth_space(IPV6_RTHDR_TYPE_0, segments))
+			return (NULL);
+
+		memset(bp, 0, bp_len);
+		rth0 = (struct ip6_rthdr0 *)rth;
+		rth0->ip6r0_len = segments * 2;
+		rth0->ip6r0_type = IPV6_RTHDR_TYPE_0;
+		rth0->ip6r0_segleft = 0;
+		rth0->ip6r0_reserved = 0;
+		break;
+	default:
+		return (NULL);	/* type not supported */
+	}
+
+	return (bp);
+}
+
+int
+inet6_rth_add(void *bp, const struct in6_addr *addr)
+{
+	struct ip6_rthdr *rth = (struct ip6_rthdr *)bp;
+	struct ip6_rthdr0 *rth0;
+	struct in6_addr *nextaddr;
+
+	switch (rth->ip6r_type) {
+	case IPV6_RTHDR_TYPE_0:
+		rth0 = (struct ip6_rthdr0 *)rth;
+		nextaddr = (struct in6_addr *)(rth0 + 1) + rth0->ip6r0_segleft;
+		*nextaddr = *addr;
+		rth0->ip6r0_segleft++;
+		break;
+	default:
+		return (-1);	/* type not supported */
+	}
+
+	return (0);
+}
+
+int
+inet6_rth_reverse(const void *in, void *out)
+{
+	struct ip6_rthdr *rth_in = (struct ip6_rthdr *)in;
+	struct ip6_rthdr0 *rth0_in, *rth0_out;
+	int i, segments;
+
+	switch (rth_in->ip6r_type) {
+	case IPV6_RTHDR_TYPE_0:
+		rth0_in = (struct ip6_rthdr0 *)in;
+		rth0_out = (struct ip6_rthdr0 *)out;
+
+		/* parameter validation XXX too paranoid? */
+		if (rth0_in->ip6r0_len % 2)
+			return (-1);
+		segments = rth0_in->ip6r0_len / 2;
+
+		/* we can't use memcpy here, since in and out may overlap */
+		memmove((void *)rth0_out, (void *)rth0_in,
+			((rth0_in->ip6r0_len) + 1) << 3);
+		rth0_out->ip6r0_segleft = segments;
+
+		/* reverse the addresses */
+		for (i = 0; i < segments / 2; i++) {
+			struct in6_addr addr_tmp, *addr1, *addr2;
+
+			addr1 = (struct in6_addr *)(rth0_out + 1) + i;
+			addr2 = (struct in6_addr *)(rth0_out + 1) +
+				(segments - i - 1);
+			addr_tmp = *addr1;
+			*addr1 = *addr2;
+			*addr2 = addr_tmp;
+		}
+		
+		break;
+	default:
+		return (-1);	/* type not supported */
+	}
+
+	return (0);
+}
+
+int
+inet6_rth_segments(const void *bp)
+{
+	struct ip6_rthdr *rh = (struct ip6_rthdr *)bp;
+	struct ip6_rthdr0 *rh0;
+	int addrs;
+
+	switch (rh->ip6r_type) {
+	case IPV6_RTHDR_TYPE_0:
+		rh0 = (struct ip6_rthdr0 *)bp;
+
+		/*
+		 * Validation for a type-0 routing header.
+		 * Is this too strict?
+		 */
+		if ((rh0->ip6r0_len % 2) != 0 ||
+		    (addrs = (rh0->ip6r0_len >> 1)) < rh0->ip6r0_segleft)
+			return (-1);
+
+		return (addrs);
+	default:
+		return (-1);	/* unknown type */
+	}
+}
+
+struct in6_addr *
+inet6_rth_getaddr(const void *bp, int idx)
+{
+	struct ip6_rthdr *rh = (struct ip6_rthdr *)bp;
+	struct ip6_rthdr0 *rh0;
+	int addrs;
+
+	switch (rh->ip6r_type) {
+	case IPV6_RTHDR_TYPE_0:
+		 rh0 = (struct ip6_rthdr0 *)bp;
+		 
+		/*
+		 * Validation for a type-0 routing header.
+		 * Is this too strict?
+		 */
+		if ((rh0->ip6r0_len % 2) != 0 ||
+		    (addrs = (rh0->ip6r0_len >> 1)) < rh0->ip6r0_segleft)
+			return (NULL);
+
+		if (idx < 0 || addrs <= idx)
+			return (NULL);
+
+		return (((struct in6_addr *)(rh0 + 1)) + idx);
+	default:
+		return (NULL);	/* unknown type */
+		break;
+	}
+}
diff -r a362d641c6d4 -r cf620ca04750 sys/netinet6/in6.h
--- a/sys/netinet6/in6.h	Wed May 23 21:42:13 2007 +0300
+++ b/sys/netinet6/in6.h	Thu May 24 08:02:36 2007 +0300
@@ -80,6 +80,8 @@
  */
 #define __KAME__
 #define __KAME_VERSION		"20010528/FreeBSD"
+
+#include <sys/socket.h>
 
 /*
  * Local port number conventions:
@@ -603,20 +605,20 @@ int inet6_option_next (const struct cmsg
 int inet6_option_next (const struct cmsghdr *, uint8_t **);
 int inet6_option_space (int);
 
-int inet6_opt_append (void *, size_t, int, uint8_t, size_t, uint8_t, void **);
-int inet6_opt_find (void *, size_t, int, uint8_t, size_t *, void **);
-int inet6_opt_finish (void *, size_t, int);
-int inet6_opt_get_val (void *, size_t, void *, int);
-int inet6_opt_init (void *, size_t);
-int inet6_opt_next (void *, size_t, int, uint8_t *, size_t *, void **);
-int inet6_opt_set_val (void *, size_t, void *, int);
+int inet6_opt_append (void *, socklen_t, int, uint8_t, size_t, uint8_t, void **);
+int inet6_opt_find (void *, socklen_t, int, uint8_t, size_t *, void **);
+int inet6_opt_finish (void *, socklen_t, int);
+int inet6_opt_get_val (void *, int, void *, socklen_t);
+int inet6_opt_init (void *, socklen_t);
+int inet6_opt_next (void *, socklen_t, int, uint8_t *, socklen_t *, void **);
+int inet6_opt_set_val (void *, int, void *, socklen_t);
 
 int		 inet6_rth_add (void *, const struct in6_addr *);
 struct in6_addr *inet6_rth_getaddr (const void *, int);
-void		*inet6_rth_init (void *, int, int, int);
+void		*inet6_rth_init (void *, socklen_t, int, int);
 int		 inet6_rth_reverse (const void *, void *);
 int		 inet6_rth_segments (const void *);
-size_t		 inet6_rth_space (int, int);
+socklen_t	 inet6_rth_space (int, int);
 
 int		 inet6_rthdr_add (struct cmsghdr *, const struct in6_addr *,
 				  unsigned int);