pfkey_v2.c

This a good VPN source

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_upmsg: "
		    "allocating %d bytes...\n",
		    (int)(pfkey_msg->sadb_msg_len * IPSEC_PFKEYv2_ALIGN));
	if(!(skb = alloc_skb(pfkey_msg->sadb_msg_len * IPSEC_PFKEYv2_ALIGN, GFP_ATOMIC) )) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_upmsg: "
			    "no buffers left to send up a message.\n");
		return -ENOBUFS;
	}
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_upmsg: "
		    "...allocated at 0p%p.\n",
		    skb);
	
	skb->dev = NULL;
	
	if(skb_tailroom(skb) < pfkey_msg->sadb_msg_len * IPSEC_PFKEYv2_ALIGN) {
		printk(KERN_WARNING "klips_error:pfkey_upmsg: "
		       "tried to skb_put %ld, %d available.  This should never happen, please report.\n",
		       (unsigned long int)pfkey_msg->sadb_msg_len * IPSEC_PFKEYv2_ALIGN,
		       skb_tailroom(skb));
		ipsec_kfree_skb(skb);
		return -ENOBUFS;
	}
	skb->h.raw = skb_put(skb, pfkey_msg->sadb_msg_len * IPSEC_PFKEYv2_ALIGN);
	memcpy(skb->h.raw, pfkey_msg, pfkey_msg->sadb_msg_len * IPSEC_PFKEYv2_ALIGN);

#ifndef NET_21
	skb->free = 1;
#endif /* !NET_21 */

	if((error = sock_queue_rcv_skb(sk, skb)) < 0) {
		skb->sk=NULL;
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_upmsg: "
			    "error=%d calling sock_queue_rcv_skb with skb=0p%p.\n",
			    error,
			    skb);
		ipsec_kfree_skb(skb);
		return error;
	}
	return error;
}

DEBUG_NO_STATIC int
pfkey_create(struct socket *sock, int protocol)
{
	struct sock *sk;

	if(sock == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_create: "
			    "socket NULL.\n");
		return -EINVAL;
	}

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_create: "
		    "sock=0p%p type:%d state:%d flags:%ld protocol:%d\n",
		    sock,
		    sock->type,
		    (unsigned int)(sock->state),
		    sock->flags, protocol);

	if(sock->type != SOCK_RAW) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_create: "
			    "only SOCK_RAW supported.\n");
		return -ESOCKTNOSUPPORT;
	}

	if(protocol != PF_KEY_V2) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_create: "
			    "protocol not PF_KEY_V2.\n");
		return -EPROTONOSUPPORT;
	}

	if((current->uid != 0)) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_create: "
			    "must be root to open pfkey sockets.\n");
		return -EACCES;
	}

#ifdef NET_21
	sock->state = SS_UNCONNECTED;
#endif /* NET_21 */

	KLIPS_INC_USE;

#ifdef NET_21
#ifdef NET_26
	sk=(struct sock *)sk_alloc(PF_KEY, GFP_KERNEL, 1, NULL);
#else
	sk=(struct sock *)sk_alloc(PF_KEY, GFP_KERNEL, 1);
#endif
#else /* NET_21 */
	sk=(struct sock *)sk_alloc(GFP_KERNEL);
#endif /* NET_21 */

	if(sk == NULL)
	{
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_create: "
			    "Out of memory trying to allocate.\n");
		KLIPS_DEC_USE;
		return -ENOMEM;
	}

#ifndef NET_21
	memset(sk, 0, sizeof(*sk));
#endif /* !NET_21 */

#ifdef NET_21
	sock_init_data(sock, sk);

	sk->sk_destruct = NULL;
	sk->sk_reuse = 1;
	sock->ops = &pfkey_ops;

	sk->sk_zapped=0;
	sk->sk_family = PF_KEY;
/*	sk->num = protocol; */
	sk->sk_protocol = protocol;
	key_pid(sk) = current->pid;
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_create: "
		    "sock->fasync_list=0p%p sk->sleep=0p%p.\n",
		    sock->fasync_list,
		    sk->sk_sleep);
#else /* NET_21 */
	sk->type=sock->type;
	init_timer(&sk->timer);
	skb_queue_head_init(&sk->sk_write_queue);
	skb_queue_head_init(&sk->sk_receive_queue);
	skb_queue_head_init(&sk->back_log);
	sk->sk_rcvbuf=SK_RMEM_MAX;
	sk->sk_sndbuf=SK_WMEM_MAX;
	sk->sk_allocation=GFP_KERNEL;
	sk->sk_state=TCP_CLOSE;
	sk->sk_priority=SOPRI_NORMAL;
	sk->sk_state_change=pfkey_state_change;
	sk->sk_data_ready=pfkey_data_ready;
	sk->sk_write_space=pfkey_write_space;
	sk->sk_error_report=pfkey_state_change;
#ifndef NET_26
	sk->mtu=4096;
	sk->socket=sock;
#endif

	sock->data=(void *)sk;
	sk->sk_sleep=sock->wait;
#endif /* NET_21 */

	pfkey_insert_socket(sk);
	pfkey_list_insert_socket(sock, &pfkey_open_sockets);

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_create: "
		    "Socket sock=0p%p sk=0p%p initialised.\n", sock, sk);
	return 0;
}

#ifndef NET_21
DEBUG_NO_STATIC int
pfkey_dup(struct socket *newsock, struct socket *oldsock)
{
	struct sock *sk;

	if(newsock==NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_dup: "
			    "No new socket attached.\n");
		return -EINVAL;
	}
		
	if(oldsock==NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_dup: "
			    "No old socket attached.\n");
		return -EINVAL;
	}
		
#ifdef NET_21
	sk=oldsock->sk;
#else /* NET_21 */
	sk=oldsock->data;
#endif /* NET_21 */
	
	/* May not have data attached */
	if(sk==NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_dup: "
			    "No sock attached to old socket.\n");
		return -EINVAL;
	}
		
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_dup: .\n");

	return pfkey_create(newsock, sk->protocol);
}
#endif /* !NET_21 */

DEBUG_NO_STATIC int
#ifdef NETDEV_23
pfkey_release(struct socket *sock)
#else /* NETDEV_23 */
pfkey_release(struct socket *sock, struct socket *peersock)
#endif /* NETDEV_23 */
{
	struct sock *sk;
	int i;

	if(sock==NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_release: "
			    "No socket attached.\n");
		return 0; /* -EINVAL; */
	}
		
#ifdef NET_21
	sk=sock->sk;
#else /* NET_21 */
	sk=sock->data;
#endif /* NET_21 */
	
	/* May not have data attached */
	if(sk==NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_release: "
			    "No sk attached to sock=0p%p.\n", sock);
		return 0; /* -EINVAL; */
	}
		
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_release: "
		    "sock=0p%p sk=0p%p\n", sock, sk);

#ifdef NET_21
	if(sock_flag(sk, SOCK_DEAD))
#endif /* NET_21 */
		if(sk->sk_state_change) {
			sk->sk_state_change(sk);
		}

#ifdef NET_21
	sock->sk = NULL;
#else /* NET_21 */
	sock->data = NULL;
#endif /* NET_21 */

	/* Try to flush out this socket. Throw out buffers at least */
	pfkey_destroy_socket(sk);
	pfkey_list_remove_socket(sock, &pfkey_open_sockets);
	for(i = SADB_SATYPE_UNSPEC; i <= SADB_SATYPE_MAX; i++) {
		pfkey_list_remove_socket(sock, &(pfkey_registered_sockets[i]));
	}

	KLIPS_DEC_USE;
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_release: "
		    "succeeded.\n");

	return 0;
}

#ifndef NET_21
DEBUG_NO_STATIC int
pfkey_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_bind: "
		    "operation not supported.\n");
	return -EINVAL;
}

DEBUG_NO_STATIC int
pfkey_connect(struct socket *sock, struct sockaddr *uaddr, int addr_len, int flags)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_connect: "
		    "operation not supported.\n");
	return -EINVAL;
}

DEBUG_NO_STATIC int
pfkey_socketpair(struct socket *a, struct socket *b)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_socketpair: "
		    "operation not supported.\n");
	return -EINVAL;
}

DEBUG_NO_STATIC int
pfkey_accept(struct socket *sock, struct socket *newsock, int flags)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_aaccept: "
		    "operation not supported.\n");
	return -EINVAL;
}

DEBUG_NO_STATIC int
pfkey_getname(struct socket *sock, struct sockaddr *uaddr, int *uaddr_len,
		int peer)
{
	struct sockaddr *ska = (struct sockaddr*)uaddr;
	
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_getname: .\n");
	ska->sa_family = PF_KEY;
	*uaddr_len = sizeof(*ska);
	return 0;
}

DEBUG_NO_STATIC int
pfkey_select(struct socket *sock, int sel_type, select_table *wait)
{
	
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_select: "
		    ".sock=0p%p sk=0p%p sel_type=%d\n",
		    sock,
		    sock->data,
		    sel_type);
	if(sock == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_select: "
			    "Null socket passed in.\n");
		return -EINVAL;
	}
	return datagram_select(sock->data, sel_type, wait);
}

DEBUG_NO_STATIC int
pfkey_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_ioctl: "
		    "not supported.\n");
	return -EINVAL;
}

DEBUG_NO_STATIC int
pfkey_listen(struct socket *sock, int backlog)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_listen: "
		    "not supported.\n");
	return -EINVAL;
}
#endif /* !NET_21 */

DEBUG_NO_STATIC int
pfkey_shutdown(struct socket *sock, int mode)
{
	struct sock *sk;

	if(sock == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_shutdown: "
			    "NULL socket passed in.\n");
		return -EINVAL;
	}

#ifdef NET_21
	sk=sock->sk;
#else /* NET_21 */
	sk=sock->data;
#endif /* NET_21 */
	
	if(sk == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_shutdown: "
			    "No sock attached to socket.\n");
		return -EINVAL;
	}

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_shutdown: "
		    "mode=%x.\n", mode);
	mode++;
	
	if(mode&SEND_SHUTDOWN) {
		sk->sk_shutdown|=SEND_SHUTDOWN;
		sk->sk_state_change(sk);
	}

	if(mode&RCV_SHUTDOWN) {
		sk->sk_shutdown|=RCV_SHUTDOWN;
		sk->sk_state_change(sk);
	}
	return 0;
}

#ifndef NET_21
DEBUG_NO_STATIC int
pfkey_setsockopt(struct socket *sock, int level, int optname, char *optval, int optlen)
{
#ifndef NET_21
	struct sock *sk;

	if(sock == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_setsockopt: "
			    "Null socket passed in.\n");
		return -EINVAL;
	}
	
	sk=sock->data;
	
	if(sk == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_setsockopt: "
			    "Null sock passed in.\n");
		return -EINVAL;
	}
#endif /* !NET_21 */
	
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_setsockopt: .\n");
	if(level!=SOL_SOCKET) {
		return -EOPNOTSUPP;
	}
#ifdef NET_21
	return sock_setsockopt(sock, level, optname, optval, optlen);
#else /* NET_21 */
	return sock_setsockopt(sk, level, optname, optval, optlen);
#endif /* NET_21 */
}

DEBUG_NO_STATIC int
pfkey_getsockopt(struct socket *sock, int level, int optname, char *optval, int *optlen)
{
#ifndef NET_21
	struct sock *sk;

	if(sock == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_setsockopt: "
			    "Null socket passed in.\n");
		return -EINVAL;
	}
	
	sk=sock->data;
	
	if(sk == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_setsockopt: "
			    "Null sock passed in.\n");
		return -EINVAL;
	}
#endif /* !NET_21 */

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_getsockopt: .\n");
	if(level!=SOL_SOCKET) {
		return -EOPNOTSUPP;
	}
#ifdef NET_21
	return sock_getsockopt(sock, level, optname, optval, optlen);
#else /* NET_21 */
	return sock_getsockopt(sk, level, optname, optval, optlen);
#endif /* NET_21 */
}

DEBUG_NO_STATIC int
pfkey_fcntl(struct socket *sock, unsigned int cmd, unsigned long arg)
{
	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_fcntl: "
		    "not supported.\n");
	return -EINVAL;
}
#endif /* !NET_21 */

/*
 *	Send PF_KEY data down.
 */
		
DEBUG_NO_STATIC int
#ifdef NET_21
#ifdef NET_26
pfkey_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
#else
pfkey_sendmsg(struct socket *sock, struct msghdr *msg, int len, struct scm_cookie *scm)
#endif
#else /* NET_21 */
pfkey_sendmsg(struct socket *sock, struct msghdr *msg, int len, int nonblock, int flags)
#endif /* NET_21 */
{
	struct sock *sk;
	int error = 0;
	struct sadb_msg *pfkey_msg = NULL, *pfkey_reply = NULL;
	
	if(sock == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "
			    "Null socket passed in.\n");
		SENDERR(EINVAL);
	}
	
#ifdef NET_21
	sk = sock->sk;
#else /* NET_21 */
	sk = sock->data;
#endif /* NET_21 */

	if(sk == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "
			    "Null sock passed in.\n");
		SENDERR(EINVAL);
	}
	
	if(msg == NULL) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "
			    "Null msghdr passed in.\n");
		SENDERR(EINVAL);
	}

	KLIPS_PRINT(debug_pfkey,
		    "klips_debug:pfkey_sendmsg: .\n");
	if(sk->sk_err) {
		error = sock_error(sk);
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "
			    "sk->err is non-zero, returns %d.\n",
			    error);
		SENDERR(-error);
	}

	if((current->uid != 0)) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "
			    "must be root to send messages to pfkey sockets.\n");
		SENDERR(EACCES);
	}

#ifdef NET_21
	if(msg->msg_control)
#else /* NET_21 */
	if(flags || msg->msg_control)
#endif /* NET_21 */
	{
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "
			    "can't set flags or set msg_control.\n");
		SENDERR(EINVAL);
	}
		
	if(sk->sk_shutdown & SEND_SHUTDOWN) {
		KLIPS_PRINT(debug_pfkey,
			    "klips_debug:pfkey_sendmsg: "