pfkey_v2_build.c

This a good VPN source

/*
 * RFC2367 PF_KEYv2 Key management API message parser
 * Copyright (C) 1999, 2000, 2001  Richard Guy Briggs.
 * 
 * This program 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; either version 2 of the License, or (at your
 * option) any later version.  See <http://www.fsf.org/copyleft/gpl.txt>.
 * 
 * This program 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.
 *
 * RCSID $Id: pfkey_v2_build.c,v 1.51 2004/10/03 01:26:36 mcr Exp $
 */

/*
 *		Template from klips/net/ipsec/ipsec/ipsec_parser.c.
 */

char pfkey_v2_build_c_version[] = "$Id: pfkey_v2_build.c,v 1.51 2004/10/03 01:26:36 mcr Exp $";

/*
 * Some ugly stuff to allow consistent debugging code for use in the
 * kernel and in user space
*/

#ifdef __KERNEL__

# include <linux/kernel.h>  /* for printk */

# include "openswan/ipsec_kversion.h" /* for malloc switch */
# ifdef MALLOC_SLAB
#  include <linux/slab.h> /* kmalloc() */
# else /* MALLOC_SLAB */
#  include <linux/malloc.h> /* kmalloc() */
# endif /* MALLOC_SLAB */
# include <linux/errno.h>  /* error codes */
# include <linux/types.h>  /* size_t */
# include <linux/interrupt.h> /* mark_bh */

# include <linux/netdevice.h>   /* struct device, and other headers */
# include <linux/etherdevice.h> /* eth_type_trans */
# include <linux/ip.h>          /* struct iphdr */ 
# if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
#  include <linux/ipv6.h>        /* struct ipv6hdr */
# endif /* if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE) */

# define MALLOC(size) kmalloc(size, GFP_ATOMIC)
# define FREE(obj) kfree(obj)
# include <openswan.h>
#else /* __KERNEL__ */

# include <sys/types.h>
# include <linux/types.h>
# include <linux/errno.h>
# include <malloc.h>
# include <string.h> /* memset */

# include <openswan.h>

#endif /* __KERNEL__ */

#include <pfkeyv2.h>
#include <pfkey.h>

#ifdef __KERNEL__
#include "openswan/radij.h"  /* rd_nodes */
#include "openswan/ipsec_encap.h"  /* sockaddr_encap */
#endif /* __KERNEL__ */


#include "openswan/ipsec_sa.h"  /* IPSEC_SAREF_NULL, IPSEC_SA_REF_TABLE_IDX_WIDTH */
#include "openswan/pfkey_debug.h"


#define SENDERR(_x) do { error = -(_x); goto errlab; } while (0)

void
pfkey_extensions_init(struct sadb_ext *extensions[SADB_EXT_MAX + 1])
{
	int i;
	
	for (i = 0; i != SADB_EXT_MAX + 1; i++) {
		extensions[i] = NULL;
	}
}

void
pfkey_extensions_free(struct sadb_ext *extensions[SADB_EXT_MAX + 1])
{
	int i;
	
	if(!extensions) {
		return;
	}

	if(extensions[0]) {
		memset(extensions[0], 0, sizeof(struct sadb_msg));
		FREE(extensions[0]);
		extensions[0] = NULL;
	}
	
	for (i = 1; i != SADB_EXT_MAX + 1; i++) {
		if(extensions[i]) {
			memset(extensions[i], 0, extensions[i]->sadb_ext_len * IPSEC_PFKEYv2_ALIGN);
			FREE(extensions[i]);
			extensions[i] = NULL;
		}
	}
}

void
pfkey_msg_free(struct sadb_msg **pfkey_msg)
{
	if(*pfkey_msg) {
		memset(*pfkey_msg, 0, (*pfkey_msg)->sadb_msg_len * IPSEC_PFKEYv2_ALIGN);
		FREE(*pfkey_msg);
		*pfkey_msg = NULL;
	}
}

/* Default extension builders taken from the KLIPS code */

int
pfkey_msg_hdr_build(struct sadb_ext**	pfkey_ext,
		    uint8_t		msg_type,
		    uint8_t		satype,
		    uint8_t		msg_errno,
		    uint32_t		seq,
		    uint32_t		pid)
{
	int error = 0;
	struct sadb_msg *pfkey_msg = (struct sadb_msg *)*pfkey_ext;

	DEBUGGING(PF_KEY_DEBUG_BUILD,
		"pfkey_msg_hdr_build:\n");
	DEBUGGING(PF_KEY_DEBUG_BUILD,
		"pfkey_msg_hdr_build: "
		"on_entry &pfkey_ext=0p%p pfkey_ext=0p%p *pfkey_ext=0p%p.\n",
		&pfkey_ext,
		pfkey_ext,
		*pfkey_ext);
	/* sanity checks... */
	if(pfkey_msg) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_msg_hdr_build: "
			"why is pfkey_msg already pointing to something?\n");
		SENDERR(EINVAL);
	}

	if(!msg_type) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_msg_hdr_build: "
			"msg type not set, must be non-zero..\n");
		SENDERR(EINVAL);
	}

	if(msg_type > SADB_MAX) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_msg_hdr_build: "
			"msg type too large:%d.\n",
			msg_type);
		SENDERR(EINVAL);
	}

	if(satype > SADB_SATYPE_MAX) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_msg_hdr_build: "
			"satype %d > max %d\n", 
			satype, SADB_SATYPE_MAX);
		SENDERR(EINVAL);
	}

	pfkey_msg = (struct sadb_msg*)MALLOC(sizeof(struct sadb_msg));
	*pfkey_ext = (struct sadb_ext*)pfkey_msg;
	
	if(pfkey_msg == NULL) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_msg_hdr_build: "
			"memory allocation failed\n");
		SENDERR(ENOMEM);
	}
	memset(pfkey_msg, 0, sizeof(struct sadb_msg));

	pfkey_msg->sadb_msg_len = sizeof(struct sadb_msg) / IPSEC_PFKEYv2_ALIGN;

	pfkey_msg->sadb_msg_type = msg_type;
	pfkey_msg->sadb_msg_satype = satype;

	pfkey_msg->sadb_msg_version = PF_KEY_V2;
	pfkey_msg->sadb_msg_errno = msg_errno;
	pfkey_msg->sadb_msg_reserved = 0;
	pfkey_msg->sadb_msg_seq = seq;
	pfkey_msg->sadb_msg_pid = pid;
	DEBUGGING(PF_KEY_DEBUG_BUILD,
		"pfkey_msg_hdr_build: "
		"on_exit &pfkey_ext=0p%p pfkey_ext=0p%p *pfkey_ext=0p%p.\n",
		&pfkey_ext,
		pfkey_ext,
		*pfkey_ext);
errlab:
	return error;
}	

int
pfkey_sa_ref_build(struct sadb_ext **		pfkey_ext,
		   uint16_t			exttype,
		   uint32_t			spi,
		   uint8_t			replay_window,
		   uint8_t			sa_state,
		   uint8_t			auth,
		   uint8_t			encrypt,
		   uint32_t			flags,
		   uint32_t/*IPsecSAref_t*/	ref)
{
	int error = 0;
	struct sadb_sa *pfkey_sa = (struct sadb_sa *)*pfkey_ext;

	DEBUGGING(PF_KEY_DEBUG_BUILD,
		    "pfkey_sa_build: "
		    "spi=%08x replay=%d sa_state=%d auth=%d encrypt=%d flags=%d\n",
		    ntohl(spi), /* in network order */
		    replay_window,
		    sa_state,
		    auth,
		    encrypt,
		    flags);
	/* sanity checks... */
	if(pfkey_sa) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"why is pfkey_sa already pointing to something?\n");
		SENDERR(EINVAL);
	}

	if(exttype != SADB_EXT_SA &&
	   exttype != SADB_X_EXT_SA2) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"invalid exttype=%d.\n",
			exttype);
		SENDERR(EINVAL);
	}

	if(replay_window > 64) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"replay window size: %d -- must be 0 <= size <= 64\n",
			replay_window);
		SENDERR(EINVAL);
	}

	if(auth > SADB_AALG_MAX) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"auth=%d > SADB_AALG_MAX=%d.\n",
			auth,
			SADB_AALG_MAX);
		SENDERR(EINVAL);
	}

#if SADB_EALG_MAX < 255	
	if(encrypt > SADB_EALG_MAX) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"encrypt=%d > SADB_EALG_MAX=%d.\n",
			encrypt,
			SADB_EALG_MAX);
		SENDERR(EINVAL);
	}
#endif

	if(sa_state > SADB_SASTATE_MAX) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"sa_state=%d exceeds MAX=%d.\n",
			sa_state,
			SADB_SASTATE_MAX);
		SENDERR(EINVAL);
	}

	if(sa_state == SADB_SASTATE_DEAD) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"sa_state=%d is DEAD=%d is not allowed.\n",
			sa_state,
			SADB_SASTATE_DEAD);
		SENDERR(EINVAL);
	}
	
	if((IPSEC_SAREF_NULL != ref) && (ref >= (1 << IPSEC_SA_REF_TABLE_IDX_WIDTH))) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			  "pfkey_sa_build: "
			  "SAref=%d must be (SAref == IPSEC_SAREF_NULL(%d) || SAref < IPSEC_SA_REF_TABLE_NUM_ENTRIES(%d)).\n",
			  ref,
			  IPSEC_SAREF_NULL,
			  IPSEC_SA_REF_TABLE_NUM_ENTRIES);
		SENDERR(EINVAL);
	}
	
	pfkey_sa = (struct sadb_sa*)MALLOC(sizeof(struct sadb_sa));
	*pfkey_ext = (struct sadb_ext*)pfkey_sa;

	if(pfkey_sa == NULL) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_sa_build: "
			"memory allocation failed\n");
		SENDERR(ENOMEM);
	}
	memset(pfkey_sa, 0, sizeof(struct sadb_sa));
	
	pfkey_sa->sadb_sa_len = sizeof(*pfkey_sa) / IPSEC_PFKEYv2_ALIGN;
	pfkey_sa->sadb_sa_exttype = exttype;
	pfkey_sa->sadb_sa_spi = spi;
	pfkey_sa->sadb_sa_replay = replay_window;
	pfkey_sa->sadb_sa_state = sa_state;
	pfkey_sa->sadb_sa_auth = auth;
	pfkey_sa->sadb_sa_encrypt = encrypt;
	pfkey_sa->sadb_sa_flags = flags;
	pfkey_sa->sadb_x_sa_ref = ref;  

errlab:
	return error;
}	

int
pfkey_sa_build(struct sadb_ext **	pfkey_ext,
	       uint16_t			exttype,
	       uint32_t			spi,
	       uint8_t			replay_window,
	       uint8_t			sa_state,
	       uint8_t			auth,
	       uint8_t			encrypt,
	       uint32_t			flags)
{
	return pfkey_sa_ref_build(pfkey_ext,
			   exttype,
			   spi,
			   replay_window,
			   sa_state,
			   auth,
			   encrypt,
			   flags,
			   IPSEC_SAREF_NULL);
}

int
pfkey_lifetime_build(struct sadb_ext **	pfkey_ext,
		     uint16_t		exttype,
		     uint32_t		allocations,
		     uint64_t		bytes,
		     uint64_t		addtime,
		     uint64_t		usetime,
		     uint32_t		packets)
{
	int error = 0;
	struct sadb_lifetime *pfkey_lifetime = (struct sadb_lifetime *)*pfkey_ext;

	DEBUGGING(PF_KEY_DEBUG_BUILD,
		"pfkey_lifetime_build:\n");
	/* sanity checks... */
	if(pfkey_lifetime) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_lifetime_build: "
			"why is pfkey_lifetime already pointing to something?\n");
		SENDERR(EINVAL);
	}

	if(exttype != SADB_EXT_LIFETIME_CURRENT &&
	   exttype != SADB_EXT_LIFETIME_HARD &&
	   exttype != SADB_EXT_LIFETIME_SOFT) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_lifetime_build: "
			"invalid exttype=%d.\n",
			exttype);
		SENDERR(EINVAL);
	}

	pfkey_lifetime = (struct sadb_lifetime*)MALLOC(sizeof(struct sadb_lifetime));
	*pfkey_ext = (struct sadb_ext*) pfkey_lifetime;

	if(pfkey_lifetime == NULL) {
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_lifetime_build: "
			"memory allocation failed\n");
		SENDERR(ENOMEM);
	}
	memset(pfkey_lifetime, 0, sizeof(struct sadb_lifetime));

	pfkey_lifetime->sadb_lifetime_len = sizeof(struct sadb_lifetime) / IPSEC_PFKEYv2_ALIGN;
	pfkey_lifetime->sadb_lifetime_exttype = exttype;
	pfkey_lifetime->sadb_lifetime_allocations = allocations;
	pfkey_lifetime->sadb_lifetime_bytes = bytes;
	pfkey_lifetime->sadb_lifetime_addtime = addtime;
	pfkey_lifetime->sadb_lifetime_usetime = usetime;
	pfkey_lifetime->sadb_x_lifetime_packets = packets;

errlab:
	return error;
}

int
pfkey_address_build(struct sadb_ext**	pfkey_ext,
		    uint16_t		exttype,
		    uint8_t		proto,
		    uint8_t		prefixlen,
		    struct sockaddr*	address)
{
	int error = 0;
	int saddr_len = 0;
	char ipaddr_txt[ADDRTOT_BUF + 6/*extra for port number*/];
	struct sadb_address *pfkey_address = (struct sadb_address *)*pfkey_ext;
	
	DEBUGGING(PF_KEY_DEBUG_BUILD,
		"pfkey_address_build: "
		"exttype=%d proto=%d prefixlen=%d\n",
		exttype,
		proto,
		prefixlen);
	/* sanity checks... */
	if(pfkey_address) {
		ERROR("pfkey_address_build: "
		      "why is pfkey_address already pointing to something?\n");
		SENDERR(EINVAL);
	}

	if (!address)  {
			ERROR("pfkey_address_build: " "address is NULL\n");
			SENDERR(EINVAL);
	}
	
	switch(exttype) {	
	case SADB_EXT_ADDRESS_SRC:
	case SADB_EXT_ADDRESS_DST:
	case SADB_EXT_ADDRESS_PROXY:
	case SADB_X_EXT_ADDRESS_DST2:
	case SADB_X_EXT_ADDRESS_SRC_FLOW:
	case SADB_X_EXT_ADDRESS_DST_FLOW:
	case SADB_X_EXT_ADDRESS_SRC_MASK:
	case SADB_X_EXT_ADDRESS_DST_MASK:
#ifdef NAT_TRAVERSAL
	case SADB_X_EXT_NAT_T_OA:
#endif	
		break;
	default:
		ERROR("pfkey_address_build: "
			"unrecognised ext_type=%d.\n", 
			exttype); 
		SENDERR(EINVAL); 
	}

	switch(address->sa_family) {
	case AF_INET:
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_address_build: "
			"found address family AF_INET.\n");
		saddr_len = sizeof(struct sockaddr_in);
		sprintf(ipaddr_txt, "%d.%d.%d.%d:%d"
			, (((struct sockaddr_in*)address)->sin_addr.s_addr >>  0) & 0xFF
			, (((struct sockaddr_in*)address)->sin_addr.s_addr >>  8) & 0xFF
			, (((struct sockaddr_in*)address)->sin_addr.s_addr >> 16) & 0xFF
			, (((struct sockaddr_in*)address)->sin_addr.s_addr >> 24) & 0xFF
			, ntohs(((struct sockaddr_in*)address)->sin_port));
		break;
	case AF_INET6:
		DEBUGGING(PF_KEY_DEBUG_BUILD,
			"pfkey_address_build: "
			"found address family AF_INET6.\n");
		saddr_len = sizeof(struct sockaddr_in6);
		sprintf(ipaddr_txt, "%x:%x:%x:%x:%x:%x:%x:%x-%x"
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[0])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[1])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[2])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[3])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[4])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[5])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[6])
			, ntohs(((struct sockaddr_in6*)address)->sin6_addr.s6_addr16[7])
			, ntohs(((struct sockaddr_in6*)address)->sin6_port));
		break;
	default:
		ERROR("pfkey_address_build: "
		      "address->sa_family=%d not supported.\n",
		      address->sa_family);
		SENDERR(EPFNOSUPPORT);
	}

	DEBUGGING(PF_KEY_DEBUG_BUILD,
		"pfkey_address_build: "
		"found address=%s.\n",
		ipaddr_txt);
	if(prefixlen != 0) {
		ERROR("pfkey_address_build: "
			"address prefixes not supported yet.\n");
		SENDERR(EAFNOSUPPORT); /* not supported yet */
	}

	/* allocate some memory for the extension */
	pfkey_address = (struct sadb_address*)
		MALLOC(ALIGN_N(sizeof(struct sadb_address) + saddr_len, IPSEC_PFKEYv2_ALIGN));
	*pfkey_ext = (struct sadb_ext*)pfkey_address;

	if(pfkey_address == NULL ) {
		ERROR("pfkey_lifetime_build: "
			"memory allocation failed\n");
		SENDERR(ENOMEM);
	}
	memset(pfkey_address,
	       0,
	       ALIGN_N(sizeof(struct sadb_address) + saddr_len,
		     IPSEC_PFKEYv2_ALIGN));
	       
	pfkey_address->sadb_address_len = DIVUP(sizeof(struct sadb_address) + saddr_len,
						IPSEC_PFKEYv2_ALIGN);
	
	pfkey_address->sadb_address_exttype = exttype;
	pfkey_address->sadb_address_proto = proto;
	pfkey_address->sadb_address_prefixlen = prefixlen;
	pfkey_address->sadb_address_reserved = 0;

	memcpy((char*)pfkey_address + sizeof(struct sadb_address),
	       address,
	       saddr_len);

#if 0
	for(i = 0; i < sizeof(struct sockaddr_in) - offsetof(struct sockaddr_in, sin_zero); i++) {
		pfkey_address_s_ska.sin_zero[i] = 0;
	}
#endif
	DEBUGGING(PF_KEY_DEBUG_BUILD,
		  "pfkey_address_build: "
		  "successful created len: %d.\n", pfkey_address->sadb_address_len);

 errlab:
	return error;
}