my_udp.c

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

/*
	if( up->encap_type ){
		int ret;
		ret = udp_encap_rcv(sk, skb);
		if (ret == 0) {
			kfree_skb(skb);
			return 0;
		}
		if (ret < 0) {
			ret = xfrm4_rcv_encap(skb, up->encap_type);
			UDP_INC_STATS_BH(UDP_MIB_INDATAGRAMS);
			return -ret;
		}
	}
*/
	if (sk->sk_filter && skb->ip_summed != CHECKSUM_UNNECESSARY) {
		if (__myudp_checksum_complete(skb)) {
			MYUDP_INC_STATS_BH(UDP_MIB_INERRORS);
			kfree_skb(skb);
			return -1;
		}
		skb->ip_summed = CHECKSUM_UNNECESSARY;
	}

	if( sock_queue_rcv_skb(sk,skb) < 0 ){
		MYUDP_INC_STATS_BH(UDP_MIB_INERRORS);
		kfree_skb(skb);
		return -1;
	}
	MYUDP_INC_STATS_BH(UDP_MIB_INDATAGRAMS);
	return 0;
}

static void myudp_v4_hash(struct sock *sk)
{
	BUG();
}

static void myudp_v4_unhash(struct sock *sk)
{
	write_lock_bh( &myudp_hash_lock );
	if (sk_del_node_init(sk)) {
		inet_sk(sk)->num = 0;
		sock_prot_dec_use( sk->sk_prot);
	}
	write_unlock_bh( &myudp_hash_lock );

}

static struct sock *myudp_v4_lookup_longway(u32 saddr, u16 sport,
				u32 daddr, u16 dport, int dif)
{
	struct sock *sk, *result = NULL;
	struct hlist_node *node;
	unsigned short hnum = ntohs(dport);
	int badness = -1;

	sk_for_each(sk, node, &myudp_hash[hnum & (UDP_HTABLE_SIZE - 1)]) {
		struct inet_sock *inet = inet_sk(sk);

		if (inet->num == hnum && !ipv6_only_sock(sk)) {
			int score = (sk->sk_family == MY_PF_INET ? 1 : 0);
			if (inet->rcv_saddr) {
				if (inet->rcv_saddr != daddr)
					continue;
				score+=2;
			}
			if (inet->daddr) {
				if (inet->daddr != saddr)
					continue;
				score+=2;
			}
			if (inet->dport) {
				if (inet->dport != sport)
					continue;
				score+=2;
			}
			if (sk->sk_bound_dev_if) {
				if (sk->sk_bound_dev_if != dif)
					continue;
				score+=2;
			}
			if(score == 9) {
				result = sk;
				break;
			} else if(score > badness) {
				result = sk;
				badness = score;
			}
		}
	}
	return result;
}

static __inline__ struct sock *myudp_v4_lookup(u32 saddr, u16 sport,
				u32 daddr, u16 dport, int dif)
{
	struct sock *sk;

	read_lock( &myudp_hash_lock );
	sk = myudp_v4_lookup_longway(saddr, sport, daddr, dport, dif);
	if (sk)
		sock_hold(sk);
	read_unlock( &myudp_hash_lock );
	return sk;
}

int myudp_port_rover;
static int myudp_v4_get_port(struct sock *sk, unsigned short snum)
{
	struct hlist_node *node;
	struct sock *sk2;
	struct inet_sock *inet = inet_sk(sk);

	write_lock_bh( &myudp_hash_lock );
	if( snum == 0 ){
		int best_size_so_far, best, result, i;

		if ( myudp_port_rover > sysctl_local_port_range[1] ||
						myudp_port_rover < sysctl_local_port_range[0])
			 myudp_port_rover = sysctl_local_port_range[0];
		best_size_so_far = 32767;
		best = result = myudp_port_rover;
		for (i = 0; i < UDP_HTABLE_SIZE; i++, result++) {
			struct hlist_head *list;
			int size;

			list = &myudp_hash[result & (UDP_HTABLE_SIZE - 1)];
			if (hlist_empty(list)) {
				if (result > sysctl_local_port_range[1])
					result = sysctl_local_port_range[0] +
						((result - sysctl_local_port_range[0]) &
						 (UDP_HTABLE_SIZE - 1));
				goto gotit;
			}
			size = 0;
			sk_for_each(sk2, node, list)
				if (++size >= best_size_so_far)
					goto next;
			best_size_so_far = size;
			best = result;
		next:;
		}
		result = best;
		for(i = 0; i < (1 << 16) / UDP_HTABLE_SIZE; i++, result += UDP_HTABLE_SIZE) {
			if (result > sysctl_local_port_range[1])
				result = sysctl_local_port_range[0]
					+ ((result - sysctl_local_port_range[0]) &
					   (UDP_HTABLE_SIZE - 1));
			if (!udp_lport_inuse(result))
				break;
		}
		if (i >= (1 << 16) / UDP_HTABLE_SIZE)
			goto fail;
gotit:
		udp_port_rover = snum = result;
	} else {
		sk_for_each(sk2, node,
			    &myudp_hash[snum & (UDP_HTABLE_SIZE - 1)]) {
			struct inet_sock *inet2 = inet_sk(sk2);

			if (inet2->num == snum &&
			    sk2 != sk &&
			    !ipv6_only_sock(sk2) &&
			    (!sk2->sk_bound_dev_if ||
			     !sk->sk_bound_dev_if ||
			     sk2->sk_bound_dev_if == sk->sk_bound_dev_if) &&
			    (!inet2->rcv_saddr ||
			     !inet->rcv_saddr ||
			     inet2->rcv_saddr == inet->rcv_saddr) &&
			    (!sk2->sk_reuse || !sk->sk_reuse))
				goto fail;
		}
	}
	inet->num = snum;
	if( sk_unhashed(sk) ){
		printk(KERN_INFO "bind the port: %u\n", snum );
		struct hlist_head *h = &myudp_hash[snum & (UDP_HTABLE_SIZE - 1)];

		sk_add_node(sk, h);
		sock_prot_inc_use(sk->sk_prot);
	}
	write_unlock_bh( &myudp_hash_lock );
	return 0;

fail:
	write_unlock_bh( &myudp_hash_lock );
	return 1;
}

static unsigned short myudp_check(struct udphdr *uh, int len, 
				unsigned long saddr, unsigned long daddr, unsigned long base)
{
	return(csum_tcpudp_magic(saddr, daddr, len, IPPROTO_UDP, base));
}

static void myudp_checksum_init(struct sk_buff *skb, struct udphdr *uh,
				unsigned short ulen, u32 saddr, u32 daddr)
{
	if( uh->check == 0 ){
		skb->ip_summed = CHECKSUM_UNNECESSARY;
	}else if( skb->ip_summed == CHECKSUM_HW ){
		if( !myudp_check(uh, ulen, saddr, daddr, skb->csum) )
			skb->ip_summed = CHECKSUM_UNNECESSARY;
	}
	if (skb->ip_summed != CHECKSUM_UNNECESSARY)
		skb->csum = csum_tcpudp_nofold(saddr, daddr, ulen, IPPROTO_UDP, 0);
}

static inline struct sock *myudp_v4_mcast_next(struct sock *sk, u16 loc_port, u32 loc_addr,
				u16 rmt_port, u32 rmt_addr, int dif)
{
	struct hlist_node *node;
	struct sock *s = sk;
	unsigned short hnum = ntohs(loc_port);
	sk_for_each_from(s, node) {
		struct inet_sock *inet = inet_sk(s);
		if( inet->num != hnum || (inet->daddr && inet->daddr != rmt_addr) ||
						(inet->dport != rmt_port && inet->dport) ||
						(inet->rcv_saddr && inet->rcv_saddr != loc_addr) ||
						ipv6_only_sock(s) || (s->sk_bound_dev_if && s->sk_bound_dev_if != dif))
			continue;
		if( !myip_mc_sf_allow(s, loc_addr, rmt_addr, dif) )
			continue;
		goto found;
  	}
	s = NULL;
found:
  	return s;
}

static int myudp_v4_mcast_deliver(struct sk_buff *skb, struct udphdr *uh,
				u32 saddr, u32 daddr)
{
	struct sock *sk;
	int dif;

	read_lock( &myudp_hash_lock );
	sk = sk_head( &myudp_hash[ntohs(uh->dest) & (UDP_HTABLE_SIZE - 1)] );
	dif = skb->dev->ifindex;
	sk = myudp_v4_mcast_next(sk, uh->dest, daddr, uh->source, saddr, dif);
	if (sk) {
		struct sock *sknext = NULL;

		do {
			struct sk_buff *skb1 = skb;
			sknext = myudp_v4_mcast_next(sk_next(sk), uh->dest, daddr,
						   uh->source, saddr, dif);
			if(sknext)
				skb1 = skb_clone(skb, GFP_ATOMIC);

			if(skb1) {
				int ret = myudp_queue_rcv_skb(sk, skb1);
				if (ret > 0)
					kfree_skb(skb1);
			}
			sk = sknext;
		} while(sknext);
	} else
		kfree_skb(skb);
	read_unlock( &myudp_hash_lock );
	return 0;
}

int myudp_rcv(struct sk_buff *skb)
{
	struct sock *sk;
  	struct udphdr *uh;
	unsigned short ulen;
	struct rtable *rt = (struct rtable*)skb->dst;
	u32 saddr = skb->nh.iph->saddr;
	u32 daddr = skb->nh.iph->daddr;
	int len = skb->len;

	printk(KERN_INFO "pack type: %d\n", skb->pkt_type );
	if (!pskb_may_pull(skb, sizeof(struct udphdr)))
		goto no_header;	

	uh = skb->h.uh;
	ulen = ntohs(uh->len);
	if (ulen > len || ulen < sizeof(*uh))
		goto short_packet;

	if( pskb_trim_rcsum(skb, ulen) )
		goto short_packet;

	myudp_checksum_init(skb, uh, ulen, saddr, daddr);

	if(rt->rt_flags & (RTCF_BROADCAST|RTCF_MULTICAST))
		return myudp_v4_mcast_deliver(skb, uh, saddr, daddr);

	sk = myudp_v4_lookup(saddr, uh->source, daddr, uh->dest, skb->dev->ifindex);
	if( sk != NULL ) {
		int ret = myudp_queue_rcv_skb(sk, skb);
		sock_put(sk);
		if (ret > 0)
			return -ret;
		return 0;
	}
	//if (!xfrm4_policy_check(NULL, XFRM_POLICY_IN, skb))
	//	goto drop;
	nf_reset(skb);

	if( myudp_checksum_complete(skb) )
		goto csum_error;

	MYUDP_INC_STATS_BH(UDP_MIB_NOPORTS);
	myicmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);

	kfree_skb(skb);
	return 0;
short_packet:
no_header:
	MYUDP_INC_STATS_BH(UDP_MIB_INERRORS);
	kfree_skb(skb);
	return(0);
csum_error:
drop:
	MYUDP_INC_STATS_BH(UDP_MIB_INERRORS);
	kfree_skb(skb);
	return(0);
}

void myudp_err(struct sk_buff *skb, u32 info)
{
	struct inet_sock *inet;
	struct iphdr *iph = (struct iphdr*)skb->data;
	struct udphdr *uh = (struct udphdr*)(skb->data+(iph->ihl<<2));
	int type = skb->h.icmph->type;
	int code = skb->h.icmph->code;
	struct sock *sk;
	int harderr;
	int err;

	sk = myudp_v4_lookup(iph->daddr, uh->dest, iph->saddr, uh->source, skb->dev->ifindex);
	if( sk == NULL ){
		MYICMP_INC_STATS_BH(ICMP_MIB_INERRORS);
		return;
	}

	err = 0;
	harderr = 0;
	inet = inet_sk(sk);

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

	if (!inet->recverr) {
		if (!harderr || sk->sk_state != TCP_ESTABLISHED)
			goto out;
	}else{
		myip_icmp_error(sk, skb, err, uh->dest, info, (u8*)(uh+1));
	}
	sk->sk_err = err;
	sk->sk_error_report(sk);
out:
	sock_put(sk);
}

struct proto myudp_prot = {
	.name =		"MY_UDP",
	.owner =	THIS_MODULE,
	.close =	myudp_close,
	.connect =	myip4_datagram_connect,
	.disconnect =	myudp_disconnect,
	.ioctl =	myudp_ioctl,
	.destroy =	myudp_destroy_sock,
	.setsockopt =	myudp_setsockopt,
	.getsockopt =	myudp_getsockopt,
	.sendmsg =	myudp_sendmsg,
	.recvmsg =	myudp_recvmsg,
	.sendpage =	myudp_sendpage,
	.backlog_rcv =	myudp_queue_rcv_skb,
	.hash =		myudp_v4_hash,
	.unhash =	myudp_v4_unhash,
	.get_port =	myudp_v4_get_port,
	.obj_size =	sizeof(struct udp_sock),
};