my_raw.c

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

/* my_raw.c
 * [e-mail]
 * 2006-05-20
 */

struct hlist_head myraw_v4_htable[RAWV4_HTABLE_SIZE];
DEFINE_RWLOCK( myraw_v4_lock );

static int myraw_rcv_skb(struct sock * sk, struct sk_buff * skb)
{
	if (sock_queue_rcv_skb(sk, skb) < 0) {
		kfree_skb(skb);
		return NET_RX_DROP;
	}

	return NET_RX_SUCCESS;

}

int myraw_rcv(struct sock *sk, struct sk_buff *skb)
{
	//if (!xfrm4_policy_check(sk, XFRM_POLICY_IN, skb)) {
	//	kfree_skb(skb);
	//	return NET_RX_DROP;
	//}
	nf_reset(skb);
	
	skb_push(skb, skb->data - skb->nh.raw);
	myraw_rcv_skb(sk, skb);
	return 0;
}

static __inline__ int myicmp_filter(struct sock *sk, struct sk_buff *skb)
{
	int type;
	if (!pskb_may_pull(skb, sizeof(struct icmphdr)))
		return 1;
	type = skb->h.icmph->type;
	if (type < 32) {
		__u32 data = raw_sk(sk)->filter.data;

		return ((1 << type) & data) != 0;
	}
	return 0;
}

struct sock *__myraw_v4_lookup(struct sock *sk, unsigned short num,
			     unsigned long raddr, unsigned long laddr,
			     int dif)
{
	struct hlist_node *node;

	sk_for_each_from(sk, node) {
		struct inet_sock *inet = inet_sk(sk);

		if (inet->num == num 					&&
		    !(inet->daddr && inet->daddr != raddr) 		&&
		    !(inet->rcv_saddr && inet->rcv_saddr != laddr)	&&
		    !(sk->sk_bound_dev_if && sk->sk_bound_dev_if != dif))
			goto found; /* gotcha */
	}
	sk = NULL;
found:
	return sk;
}

int myraw_v4_input(struct sk_buff *skb, struct iphdr *iph, int hash)
{
	struct sock *sk;
	struct hlist_head *head;
	int delivered = 0;

	read_lock(&myraw_v4_lock);
	head = &myraw_v4_htable[hash];
	if (hlist_empty(head))
		goto out;
	sk = __myraw_v4_lookup( __sk_head(head), iph->protocol + (MY_IPPROTO_UDP-IPPROTO_UDP),
			     iph->saddr, iph->daddr,
			     skb->dev->ifindex );

	while (sk) {
		delivered = 1;
		if( iph->protocol != IPPROTO_ICMP || !myicmp_filter(sk, skb) ){
			struct sk_buff *clone = skb_clone(skb, GFP_ATOMIC);

			/* Not releasing hash table! */
			if (clone)
				myraw_rcv(sk, clone);
		}
		sk = __myraw_v4_lookup(sk_next(sk), iph->protocol + (MY_IPPROTO_UDP-IPPROTO_UDP),
				     iph->saddr, iph->daddr,
				     skb->dev->ifindex);
	}
out:
	read_unlock(&myraw_v4_lock);
	return delivered;
}

static int myraw_send_hdrinc(struct sock *sk, void *from, size_t length,
			struct rtable *rt, 
			unsigned int flags)
{
	return 0;
}

static void myraw_probe_proto_opt(struct flowi *fl, struct msghdr *msg)
{
	struct iovec *iov;
	u8 __user *type = NULL;
	u8 __user *code = NULL;
	int probed = 0;
	unsigned int i;

	if (!msg->msg_iov)
		return;

	for (i = 0; i < msg->msg_iovlen; i++) {
		iov = &msg->msg_iov[i];
		if (!iov)
			continue;

		switch (fl->proto){
		case MY_IPPROTO_ICMP:
			if (iov->iov_base && iov->iov_len < 1)
				break;

			if( !type ){
				type = iov->iov_base;
				if (iov->iov_len > 1)
					code = type + 1;
			}else if( !code )
				code = iov->iov_base;

			if( type && code ){
				get_user( fl->fl_icmp_type, type );
				__get_user( fl->fl_icmp_code, code );
				probed = 1;
			}
			break;
		default:
			probed = 1;
			break;
		}
		if (probed)
			break;
	}
}

int myraw_seticmpfilter(struct sock *sk, char __user *optval, int optlen)
{
	if (optlen > sizeof(struct icmp_filter))
		optlen = sizeof(struct icmp_filter);
	if (copy_from_user(&raw_sk(sk)->filter, optval, optlen))
		return -EFAULT;
	return 0;
}

static void myraw_close( struct sock *sk, long timeout )
{
	myip_ra_control(sk, 0, NULL);
	sk_common_release(sk);
}

static int myraw_ioctl(struct sock *sk, int cmd, unsigned long arg)
{
	return 0;
}

static int myraw_init(struct sock *sk)
{
	struct raw_sock *rp = raw_sk(sk);

	if (inet_sk(sk)->num == IPPROTO_ICMP)
		memset(&rp->filter, 0, sizeof(rp->filter));
	return 0;
}

static int myraw_setsockopt(struct sock *sk, int level, int optname, 
								char __user *optval, int optlen)
{
	if (level != SOL_RAW)
		return myip_setsockopt(sk, level, optname, optval, optlen);

	if (optname == ICMP_FILTER) {
		if (inet_sk(sk)->num != MY_IPPROTO_ICMP)
			return -EOPNOTSUPP;
		else
			return myraw_seticmpfilter(sk, optval, optlen);
	}
	return -ENOPROTOOPT;
}

static int myraw_geticmpfilter(struct sock *sk, char __user *optval, int __user *optlen)
{
	int len, ret = -EFAULT;

	if( get_user(len, optlen) )
		goto out;
	ret = -EINVAL;
	if (len < 0)
		goto out;

	if (len > sizeof(struct icmp_filter))
		len = sizeof(struct icmp_filter);
	ret = -EFAULT;
	if( put_user(len, optlen) || copy_to_user(optval, &raw_sk(sk)->filter, len) )
		goto out;
	ret = 0;
out:	
	return ret;
}

static int myraw_getsockopt(struct sock *sk, int level, int optname, 
							  char __user *optval, int __user *optlen)
{
	if (level != SOL_RAW)
		return -ENOPROTOOPT;
		//return ip_getsockopt(sk, level, optname, optval, optlen);

	if (optname == ICMP_FILTER) {
		if (inet_sk(sk)->num != MY_IPPROTO_ICMP)
			return -EOPNOTSUPP;
		else
			return myraw_geticmpfilter(sk, optval, optlen);
	}
	return -ENOPROTOOPT;
}

static int myraw_sendmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
							size_t len)
{
	struct inet_sock *inet = inet_sk(sk);
	struct ipcm_cookie ipc;
	struct rtable *rt = NULL;
	int free = 0;
	u32 daddr;
	u32 saddr;
	u8  tos;
	int err;

	err = -EMSGSIZE;
	if (len > 0xFFFF)
		goto out;

	err = -EOPNOTSUPP;
	if( msg->msg_flags & MSG_OOB )
		goto out;

	if (msg->msg_namelen) {
		unsigned short port;
		struct sockaddr_in *usin = (struct sockaddr_in*)msg->msg_name;
		err = -EINVAL;
		if (msg->msg_namelen < sizeof(*usin))
			goto out;
		if (usin->sin_family != MY_AF_INET) {
			static int complained;
			if (!complained++)
				printk(KERN_INFO "%s forgot to set MY_AF_INET in "
						 "raw sendmsg. Fix it!\n",
						 current->comm);
			err = -EAFNOSUPPORT;
			if (usin->sin_family)
				goto out;
		}
		daddr = usin->sin_addr.s_addr;
	}else{
		err = -EDESTADDRREQ;
		if (sk->sk_state != TCP_ESTABLISHED) 
			goto out;
		daddr = inet->daddr;
	}

	ipc.addr = inet->saddr;
	ipc.opt = NULL;
	ipc.oif = sk->sk_bound_dev_if;
	
	if (msg->msg_controllen) {
		err = myip_cmsg_send(msg, &ipc);
		if (err)
			goto out;
		if (ipc.opt)
			free = 1;
	}

	saddr = ipc.addr;
	ipc.addr = daddr;
	if (!ipc.opt)
		ipc.opt = inet->opt;

	if( ipc.opt ){
		err = -EINVAL;
		if (inet->hdrincl)
			goto done;
		if (ipc.opt->srr) {
			if (!daddr)
				goto done;
			daddr = ipc.opt->faddr;
		}
	}

	tos = RT_CONN_FLAGS(sk);
	if (msg->msg_flags & MSG_DONTROUTE)
		tos |= RTO_ONLINK;

	if (MULTICAST(daddr)) {
		if (!ipc.oif)
			ipc.oif = inet->mc_index;
		if (!saddr)
			saddr = inet->mc_addr;
	}

	{
		struct flowi fl = { .oif = ipc.oif,
				.nl_u = { .ip4_u =
						{ .daddr = daddr,
								.saddr = saddr,
								.tos = tos } },
				.proto = inet->hdrincl ? IPPROTO_RAW :
						sk->sk_protocol,
		};
		if (!inet->hdrincl)
			myraw_probe_proto_opt(&fl, msg);

		err = myip_route_output_flow(&rt, &fl, sk, !(msg->msg_flags&MSG_DONTWAIT));
	}

	if (err)
		goto done;

	err = -EACCES;
	if (rt->rt_flags & RTCF_BROADCAST && !sock_flag(sk, SOCK_BROADCAST))
		goto done;

	if (msg->msg_flags & MSG_CONFIRM)
		goto do_confirm;

back_from_confirm:
	if (inet->hdrincl)
		err = myraw_send_hdrinc(sk, msg->msg_iov, len, rt, msg->msg_flags);
	else{
		if (!ipc.addr)
			ipc.addr = rt->rt_dst;

		lock_sock(sk);
		err = myip_append_data(sk, myip_generic_getfrag, msg->msg_iov, len, 0,
					&ipc, rt, msg->msg_flags);
		if( err )
			myip_flush_pending_frames(sk);
		else if (!(msg->msg_flags & MSG_MORE))
			err = myip_push_pending_frames(sk);
		release_sock(sk);
	}

done:
	if( free )
		kfree(ipc.opt);
	ip_rt_put(rt);

out:
	if (err < 0)
		return err;
	return len;

do_confirm:
	dst_confirm(&rt->u.dst);
	if (!(msg->msg_flags & MSG_PROBE) || len)
		goto back_from_confirm;
	err = 0;
	goto done;
}

static int myraw_recvmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
							size_t len, int noblock, int flags, int *addr_len)
{
	struct inet_sock *inet = inet_sk(sk);
	size_t copied = 0;
	int err = -EOPNOTSUPP;
	struct sockaddr_in *sin = (struct sockaddr_in *)msg->msg_name;
	struct sk_buff *skb;

	if (flags & MSG_OOB)
		goto out;
	if (addr_len)
		*addr_len = sizeof(*sin);
	if (flags & MSG_ERRQUEUE) {
		err = myip_recv_error(sk, msg, len);
		goto out;
	}

	skb = skb_recv_datagram(sk, flags, noblock, &err);
	if (!skb){
		printk(KERN_INFO "the errno: %d\n", err );
		goto out;
	}

	copied = skb->len;
	if (len < copied) {
		msg->msg_flags |= MSG_TRUNC;
		copied = len;
	}

	err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
	if (err)
		goto done;

	sock_recv_timestamp(msg, sk, skb);

	if (sin) {
		sin->sin_family = MY_AF_INET;
		sin->sin_addr.s_addr = skb->nh.iph->saddr;
		memset(&sin->sin_zero, 0, sizeof(sin->sin_zero));
	}
	if( inet->cmsg_flags )
		myip_cmsg_recv( msg, skb );
	if( flags & MSG_TRUNC )
		copied = skb->len;
done:
	skb_free_datagram(sk, skb);
out:
	if (err)
		return err;
	return copied;
}

static int myraw_bind(struct sock *sk, struct sockaddr *uaddr, int addr_len)
{
	return 0;
}

static void myraw_v4_hash(struct sock *sk)
{
	struct hlist_head *head = &myraw_v4_htable[inet_sk(sk)->num &
			(RAWV4_HTABLE_SIZE - 1)];

	write_lock_bh(&myraw_v4_lock);
	sk_add_node(sk, head);
	sock_prot_inc_use(sk->sk_prot);
	write_unlock_bh(&myraw_v4_lock);
}

static void myraw_v4_unhash(struct sock *sk)
{
	write_lock_bh(&myraw_v4_lock);
	if (sk_del_node_init(sk))
		sock_prot_dec_use(sk->sk_prot);
	write_unlock_bh(&myraw_v4_lock);
}

void myraw_err (struct sock *sk, struct sk_buff *skb, u32 info)
{
/*
	struct inet_sock *inet = inet_sk(sk);
	int type = skb->h.icmph->type;
	int code = skb->h.icmph->code;
	int err = 0;
	int harderr = 0;

	if (!inet->recverr && sk->sk_state != TCP_ESTABLISHED)
		return;

	switch (type) {
	default:
	case ICMP_TIME_EXCEEDED:
		err = EHOSTUNREACH;
		break;
	case ICMP_SOURCE_QUENCH:
		return;
	case ICMP_PARAMETERPROB:
		err = EPROTO;
		harderr = 1;
		break;
	case ICMP_DEST_UNREACH:
		err = EHOSTUNREACH;
		if (code > NR_ICMP_UNREACH)
			break;
		err = icmp_err_convert[code].errno;
		harderr = icmp_err_convert[code].fatal;
		if (code == ICMP_FRAG_NEEDED) {
			harderr = inet->pmtudisc != IP_PMTUDISC_DONT;
			err = EMSGSIZE;
		}
	}

	if (inet->recverr) {
		struct iphdr *iph = (struct iphdr*)skb->data;
		u8 *payload = skb->data + (iph->ihl << 2);

		if (inet->hdrincl)
			payload = skb->data;
		ip_icmp_error(sk, skb, err, 0, info, payload);
	}

	if (inet->recverr || harderr) {
		sk->sk_err = err;
		sk->sk_error_report(sk);
	}
*/
}

struct proto myraw_prot = {
	.name = 	"MY_RAW",
	.owner =	THIS_MODULE,
	.close =	myraw_close,
	.connect =	myip4_datagram_connect,
	.disconnect =	myudp_disconnect,
	.ioctl =		myraw_ioctl,
	.init =			myraw_init,
	.setsockopt =	myraw_setsockopt,
	.getsockopt =	myraw_getsockopt,
	.sendmsg =		myraw_sendmsg,
	.recvmsg =		myraw_recvmsg,
	.bind =			myraw_bind,
	.backlog_rcv =	myraw_rcv_skb,
	.hash = 		myraw_v4_hash,
	.unhash =		myraw_v4_unhash,
	.obj_size = 	sizeof(struct raw_sock),
};