myip_sockglue.c

一个基于linux的TCP/IP协议栈的实现

struct ip_ra_chain *myip_ra_chain;
DEFINE_RWLOCK(myip_ra_lock);

int myip_ra_control(struct sock *sk, unsigned char on, void (*destructor)(struct sock *))
{
	struct ip_ra_chain *ra, *new_ra, **rap;

	if (sk->sk_type != SOCK_RAW || inet_sk(sk)->num == IPPROTO_RAW)
		return -EINVAL;

	new_ra = on ? kmalloc(sizeof(*new_ra), GFP_KERNEL) : NULL;

	write_lock_bh(&myip_ra_lock);
	for (rap = &myip_ra_chain; (ra=*rap) != NULL; rap = &ra->next) {
		if (ra->sk == sk) {
			if( on ){
				write_unlock_bh( &myip_ra_lock );
				kfree(new_ra);
				return -EADDRINUSE;
			}
			*rap = ra->next;
			write_unlock_bh( &myip_ra_lock );

			if (ra->destructor)
				ra->destructor(sk);
			sock_put(sk);
			kfree(ra);
			return 0;
		}
	}
	if (new_ra == NULL) {
		write_unlock_bh(&myip_ra_lock);
		return -ENOBUFS;
	}
	new_ra->sk = sk;
	new_ra->destructor = destructor;

	new_ra->next = ra;
	*rap = new_ra;
	sock_hold(sk);
	write_unlock_bh( &myip_ra_lock );

	return 0;
}

int myip_setsockopt(struct sock *sk, int level, int optname, 
						char __user *optval, int optlen)
{
	struct inet_sock *inet = inet_sk(sk);
	int val=0,err;

	if (level != SOL_IP)
		return -ENOPROTOOPT;

	if( ( (1<<optname) & ( (1<<IP_PKTINFO) | (1<<IP_RECVTTL) | (1<<IP_RECVOPTS)
									| (1<<IP_RECVTOS) | (1<<IP_RETOPTS) | (1<<IP_TOS) 
									| (1<<IP_TTL) | (1<<IP_HDRINCL) | (1<<IP_MTU_DISCOVER) 
									| (1<<IP_RECVERR) | (1<<IP_ROUTER_ALERT) | (1<<IP_FREEBIND))) 
					|| optname == IP_MULTICAST_TTL || optname == IP_MULTICAST_LOOP){ 
		if( optlen >= sizeof(int) ){
			if( get_user(val, (int __user *)optval) )
				return -EFAULT;
		}else if( optlen >= sizeof(char) ){
			unsigned char ucval;
			if( get_user(ucval, (unsigned char __user *) optval) )
				return -EFAULT;
			val = (int) ucval;
		}
	}
#ifdef CONFIG_IP_MROUTE
	//if (optname >= MRT_BASE && optname <= (MRT_BASE + 10))
	//	return ip_mroute_setsockopt(sk,optname,optval,optlen);
#endif
	err = 0;
	lock_sock(sk);
	switch( optname ){
	case IP_RECVERR:
		inet->recverr = !!val;
		if (!val)
			skb_queue_purge(&sk->sk_error_queue);
		break;
	case IP_ADD_MEMBERSHIP:
	case IP_DROP_MEMBERSHIP: 
	{
		struct ip_mreqn mreq;

		if (optlen < sizeof(struct ip_mreq))
			goto e_inval;
		err = -EFAULT;
		if (optlen >= sizeof(struct ip_mreqn)) {
			if( copy_from_user(&mreq,optval,sizeof(mreq)) )
				break;
		}else{
			memset(&mreq, 0, sizeof(mreq));
			if( copy_from_user(&mreq,optval,sizeof(struct ip_mreq)) )
				break; 
		}
		if (optname == IP_ADD_MEMBERSHIP)
			err = myip_mc_join_group(sk, &mreq);
		else
			err = myip_mc_leave_group(sk, &mreq);
		break;
	}
	case IP_MULTICAST_LOOP: 
		if (optlen<1)
			goto e_inval;
		inet->mc_loop = !!val;
		break;
	case IP_MULTICAST_TTL:
		if (sk->sk_type == SOCK_STREAM)
			goto e_inval;
		if( optlen<1 )
			goto e_inval;
		if( val==-1 )
			val = 1;
		if( val < 0 || val > 255 )
			goto e_inval;
		inet->mc_ttl = val;
		break;
	case IP_MULTICAST_IF:
	{
		struct ip_mreqn mreq;
		struct net_device *dev = NULL;

		if (sk->sk_type == SOCK_STREAM)
			goto e_inval;
		err = -EFAULT;
		if (optlen >= sizeof(struct ip_mreqn)) {
			if (copy_from_user(&mreq,optval,sizeof(mreq)))
				break;
		}else{
			memset(&mreq, 0, sizeof(mreq));
			if (optlen >= sizeof(struct in_addr) &&
							copy_from_user(&mreq.imr_address,optval,sizeof(struct in_addr)))
				break;
		}
		if( !mreq.imr_ifindex ){
			if (mreq.imr_address.s_addr == INADDR_ANY) {
				inet->mc_index = 0;
				inet->mc_addr  = 0;
				err = 0;
				break;
			}
			dev = myip_dev_find(mreq.imr_address.s_addr);
			if( dev ){
				mreq.imr_ifindex = dev->ifindex;
				dev_put(dev);
			}
		}else
			dev = __dev_get_by_index(mreq.imr_ifindex);

		err = -EADDRNOTAVAIL;
		if (!dev)
			break;

		err = -EINVAL;
		if (sk->sk_bound_dev_if && mreq.imr_ifindex != sk->sk_bound_dev_if)
			break;

		inet->mc_index = mreq.imr_ifindex;
		inet->mc_addr  = mreq.imr_address.s_addr;
		err = 0;
		break;
	}	
	default:
		err = -ENOPROTOOPT;
	}
	release_sock(sk);
	return err;
e_inval:
	release_sock(sk);
	return -EINVAL;
}

int myip_getsockopt(struct sock *sk, int level, int optname, 
				char __user *optval, int __user *optlen)
{
	return 0;
}

int myip_cmsg_send(struct msghdr *msg, struct ipcm_cookie *ipc)
{
	return 0;
}

void myip_icmp_error(struct sock *sk, struct sk_buff *skb, int err, 
				u16 port, u32 info, u8 *payload)
{
	struct inet_sock *inet = inet_sk(sk);
	struct sock_exterr_skb *serr;

	if (!inet->recverr)
		return;

	skb = skb_clone(skb, GFP_ATOMIC);
	if (!skb)
		return;

	serr = SKB_EXT_ERR(skb);  
	serr->ee.ee_errno = err;
	serr->ee.ee_origin = SO_EE_ORIGIN_ICMP;
	serr->ee.ee_type = skb->h.icmph->type; 
	serr->ee.ee_code = skb->h.icmph->code;
	serr->ee.ee_pad = 0;
	serr->ee.ee_info = info;
	serr->ee.ee_data = 0;
	serr->addr_offset = (u8*)&(((struct iphdr*)(skb->h.icmph+1))->daddr) - skb->nh.raw;
	serr->port = port;

	skb->h.raw = payload;
	if (!skb_pull(skb, payload - skb->data) || sock_queue_err_skb(sk, skb))
		kfree_skb(skb);
}

void myip_local_error(struct sock *sk, int err, u32 daddr, u16 port, u32 info)
{
	struct inet_sock *inet = inet_sk(sk);
	struct sock_exterr_skb *serr;
	struct iphdr *iph;
	struct sk_buff *skb;

	if( !inet->recverr )
		return;

	skb = alloc_skb(sizeof(struct iphdr), GFP_ATOMIC);
	if (!skb)
		return;

	iph = (struct iphdr*)skb_put(skb, sizeof(struct iphdr));
	skb->nh.iph = iph;
	iph->daddr = daddr;

	serr = SKB_EXT_ERR(skb);  
	serr->ee.ee_errno = err;
	serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL;
	serr->ee.ee_type = 0; 
	serr->ee.ee_code = 0;
	serr->ee.ee_pad = 0;
	serr->ee.ee_info = info;
	serr->ee.ee_data = 0;
	serr->addr_offset = (u8*)&iph->daddr - skb->nh.raw;
	serr->port = port;

	skb->h.raw = skb->tail;
	__skb_pull(skb, skb->tail - skb->data);

	if (sock_queue_err_skb(sk, skb))
		kfree_skb(skb);
}

int myip_recv_error(struct sock *sk, struct msghdr *msg, int len)
{
	struct sock_exterr_skb *serr;
	struct sk_buff *skb, *skb2;
	struct sockaddr_in *sin;
	struct {
		struct sock_extended_err ee;
		struct sockaddr_in	 offender;
	} errhdr;
	int err;
	int copied;

	err = -EAGAIN;
	skb = skb_dequeue(&sk->sk_error_queue);
	if (skb == NULL)
		goto out;

	copied = skb->len;
	if (copied > len) {
		msg->msg_flags |= MSG_TRUNC;
		copied = len;
	}
	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	if (err)
		goto out_free_skb;

	sock_recv_timestamp(msg, sk, skb);

	serr = SKB_EXT_ERR(skb);

	sin = (struct sockaddr_in *)msg->msg_name;
	if (sin) {
		sin->sin_family = MY_AF_INET;
		sin->sin_addr.s_addr = *(u32*)(skb->nh.raw + serr->addr_offset);
		sin->sin_port = serr->port;
		memset(&sin->sin_zero, 0, sizeof(sin->sin_zero));
	}
	
	memcpy(&errhdr.ee, &serr->ee, sizeof(struct sock_extended_err));
	sin = &errhdr.offender;
	sin->sin_family = AF_UNSPEC;
	if (serr->ee.ee_origin == SO_EE_ORIGIN_ICMP) {
		struct inet_sock *inet = inet_sk(sk);

		sin->sin_family = MY_AF_INET;
		sin->sin_addr.s_addr = skb->nh.iph->saddr;
		sin->sin_port = 0;
		memset(&sin->sin_zero, 0, sizeof(sin->sin_zero));
		if (inet->cmsg_flags)
			myip_cmsg_recv(msg, skb);
	}

	put_cmsg(msg, SOL_IP, IP_RECVERR, sizeof(errhdr), &errhdr);

	msg->msg_flags |= MSG_ERRQUEUE;
	err = copied;

	spin_lock_bh(&sk->sk_error_queue.lock);
	sk->sk_err = 0;
	if ((skb2 = skb_peek(&sk->sk_error_queue)) != NULL) {
		sk->sk_err = SKB_EXT_ERR(skb2)->ee.ee_errno;
		spin_unlock_bh(&sk->sk_error_queue.lock);
		sk->sk_error_report(sk);
	} else
		spin_unlock_bh(&sk->sk_error_queue.lock);

out_free_skb:	
	kfree_skb(skb);
out:
	return err;
}

static void myip_cmsg_recv_pktinfo(struct msghdr *msg, struct sk_buff *skb)
{

}

static void myip_cmsg_recv_ttl(struct msghdr *msg, struct sk_buff *skb)
{

}

static void myip_cmsg_recv_tos(struct msghdr *msg, struct sk_buff *skb)
{

}

static void myip_cmsg_recv_opts(struct msghdr *msg, struct sk_buff *skb)
{

}

static void myip_cmsg_recv_retopts(struct msghdr *msg, struct sk_buff *skb)
{

}

void myip_cmsg_recv(struct msghdr *msg, struct sk_buff *skb)
{
	struct inet_sock *inet = inet_sk(skb->sk);
	unsigned flags = inet->cmsg_flags;

	if (flags & 1)
		myip_cmsg_recv_pktinfo(msg, skb);
	if ((flags>>=1) == 0)
		return;

	if (flags & 1)
		myip_cmsg_recv_ttl(msg, skb);
	if ((flags>>=1) == 0)
		return;

	if (flags & 1)
		myip_cmsg_recv_tos(msg, skb);
	if ((flags>>=1) == 0)
		return;

	if (flags & 1)
		myip_cmsg_recv_opts(msg, skb);
	if ((flags>>=1) == 0)
		return;

	if (flags & 1)
		myip_cmsg_recv_retopts(msg, skb);
}