myicmp.c

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

DEFINE_SNMP_STAT(struct icmp_mib, myicmp_statistics) __read_mostly;
#define MYICMP_INC_STATS(field)		SNMP_INC_STATS(myicmp_statistics, field)
#define MYICMP_INC_STATS_BH(field)	SNMP_INC_STATS_BH(myicmp_statistics, field)
#define MYICMP_INC_STATS_USER(field) 	SNMP_INC_STATS_USER(myicmp_statistics, field)

static DEFINE_PER_CPU(struct socket *, __icmp_socket) = NULL;
#define myicmp_socket	__get_cpu_var(__icmp_socket)

struct icmp_bxm {
	struct sk_buff *skb;
	int offset;
	int data_len;

	struct {
		struct icmphdr icmph;
		__u32	       times[3];
	} data;
	int head_len;
	struct ip_options replyopts;
	unsigned char  optbuf[40];
};

struct myicmp_control {
	int output_entry;
	int input_entry;
	void (*handler)(struct sk_buff *skb);
	short   error;
};
static const struct myicmp_control myicmp_pointers[NR_ICMP_TYPES+1];

static __inline__ int myicmp_xmit_lock(void)
{
	local_bh_disable();

	if( unlikely(!spin_trylock(&myicmp_socket->sk->sk_lock.slock)) ){
		local_bh_enable();
		return 1;
	}
	return 0;
}

static void myicmp_xmit_unlock(void)
{
	spin_unlock_bh( &myicmp_socket->sk->sk_lock.slock );
}

static void myicmp_out_count(int type)
{
	if (type <= NR_ICMP_TYPES) {
		MYICMP_INC_STATS(myicmp_pointers[type].output_entry);
		MYICMP_INC_STATS(ICMP_MIB_OUTMSGS);
	}
}

static void myicmp_discard(struct sk_buff *skb)
{
}

static void myicmp_unreach(struct sk_buff *skb)
{
	struct iphdr *iph;
	struct icmphdr *icmph;
	int hash, protocol;
	struct net_protocol *ipprot;
	struct sock *raw_sk;
	u32 info = 0;

	if (!pskb_may_pull(skb, sizeof(struct iphdr)))
		goto out_err;

	icmph = skb->h.icmph;
	iph   = (struct iphdr *)skb->data;

	if (iph->ihl < 5)
		goto out_err;

	if( icmph->type == ICMP_DEST_UNREACH ){
		switch (icmph->code & 15) {
		case ICMP_NET_UNREACH:
		case ICMP_HOST_UNREACH:
		case ICMP_PROT_UNREACH:
		case ICMP_PORT_UNREACH:
			break;
		case ICMP_FRAG_NEEDED:
			if( ipv4_config.no_pmtu_disc ){
				LIMIT_NETDEBUG(KERN_INFO "ICMP: %u.%u.%u.%u: "
								"fragmentation needed "
								"and DF set.\n",
								NIPQUAD(iph->daddr));
			}else{
				//info = ip_rt_frag_needed(iph, ntohs(icmph->un.frag.mtu));
				//if (!info)
				//	goto out;
			}
			break;
		case ICMP_SR_FAILED:
			LIMIT_NETDEBUG(KERN_INFO "ICMP: %u.%u.%u.%u: Source "
							"Route Failed.\n",
							NIPQUAD(iph->daddr));
			break;
		default:
			break;
		}
		if (icmph->code > NR_ICMP_UNREACH)
			goto out;
	} else if (icmph->type == ICMP_PARAMETERPROB)
		info = ntohl(icmph->un.gateway) >> 24;
/*		
	if (!sysctl_icmp_ignore_bogus_error_responses &&
					inet_addr_type(iph->daddr) == RTN_BROADCAST) {
		if (net_ratelimit())
			printk(KERN_WARNING "%u.%u.%u.%u sent an invalid ICMP "
							"type %u, code %u "
							"error to a broadcast: %u.%u.%u.%u on %s\n",
							NIPQUAD(skb->nh.iph->saddr),
							icmph->type, icmph->code,
							NIPQUAD(iph->daddr),
							skb->dev->name);
		goto out;
	}
*/
	if (!pskb_may_pull(skb, iph->ihl * 4 + 8))
		goto out;

	iph = (struct iphdr *)skb->data;
	protocol = iph->protocol + (MY_IPPROTO_UDP-IPPROTO_UDP);
	hash = protocol & (MAX_INET_PROTOS - 1);
	read_lock(&myraw_v4_lock);
	if ((raw_sk = sk_head(&myraw_v4_htable[hash])) != NULL) {
		while ((raw_sk = __myraw_v4_lookup(raw_sk, protocol, iph->daddr,
						 iph->saddr, skb->dev->ifindex)) != NULL) {
			myraw_err(raw_sk, skb, info);
			raw_sk = sk_next(raw_sk);
			iph = (struct iphdr *)skb->data;
		}
	}
	read_unlock( &myraw_v4_lock );
	
	rcu_read_lock();
	ipprot = rcu_dereference( myinet_protos[hash] );
	if (ipprot && ipprot->err_handler)
		ipprot->err_handler(skb, info);
	rcu_read_unlock();

out:
	return;
out_err:
	MYICMP_INC_STATS_BH(ICMP_MIB_INERRORS);
	goto out;
}

static void myicmp_redirect(struct sk_buff *skb)
{

}

static void myicmp_echo(struct sk_buff *skb)
{

}

static void myicmp_timestamp(struct sk_buff *skb)
{

}

static void myicmp_address(struct sk_buff *skb)
{

}

static void myicmp_address_reply(struct sk_buff *skb)
{

}

static const struct myicmp_control myicmp_pointers[NR_ICMP_TYPES + 1] = {
	[ICMP_ECHOREPLY] = {
		.output_entry = ICMP_MIB_OUTECHOREPS,
		.input_entry = ICMP_MIB_INECHOREPS,
		.handler = myicmp_discard,
	},
	[1] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_INERRORS,
		.handler = myicmp_discard,
		.error = 1,
	},
	[2] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_INERRORS,
		.handler = myicmp_discard,
		.error = 1,
	},
	[ICMP_DEST_UNREACH] = {
		.output_entry = ICMP_MIB_OUTDESTUNREACHS,
		.input_entry = ICMP_MIB_INDESTUNREACHS,
		.handler = myicmp_unreach,
		.error = 1,
	},
	[ICMP_SOURCE_QUENCH] = {
		.output_entry = ICMP_MIB_OUTSRCQUENCHS,
		.input_entry = ICMP_MIB_INSRCQUENCHS,
		.handler = myicmp_unreach,
		.error = 1,
	},
	[ICMP_REDIRECT] = {
		.output_entry = ICMP_MIB_OUTREDIRECTS,
		.input_entry = ICMP_MIB_INREDIRECTS,
		.handler = myicmp_redirect,
		.error = 1,
	},
	[6] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_INERRORS,
		.handler = myicmp_discard,
		.error = 1,
	},
	[7] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_INERRORS,
		.handler = myicmp_discard,
		.error = 1,
	},
	[ICMP_ECHO] = {
		.output_entry = ICMP_MIB_OUTECHOS,
		.input_entry = ICMP_MIB_INECHOS,
		.handler = myicmp_echo,
	},
	[9] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_INERRORS,
		.handler = myicmp_discard,
		.error = 1,
	},
	[10] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_INERRORS,
		.handler = myicmp_discard,
		.error = 1,
	},
	[ICMP_TIME_EXCEEDED] = {
		.output_entry = ICMP_MIB_OUTTIMEEXCDS,
		.input_entry = ICMP_MIB_INTIMEEXCDS,
		.handler = myicmp_unreach,
		.error = 1,
	},
	[ICMP_PARAMETERPROB] = {
		.output_entry = ICMP_MIB_OUTPARMPROBS,
		.input_entry = ICMP_MIB_INPARMPROBS,
		.handler = myicmp_unreach,
		.error = 1,
	},
	[ICMP_TIMESTAMP] = {
		.output_entry = ICMP_MIB_OUTTIMESTAMPS,
		.input_entry = ICMP_MIB_INTIMESTAMPS,
		.handler = myicmp_timestamp,
	},
	[ICMP_TIMESTAMPREPLY] = {
		.output_entry = ICMP_MIB_OUTTIMESTAMPREPS,
		.input_entry = ICMP_MIB_INTIMESTAMPREPS,
		.handler = myicmp_discard,
	},
	[ICMP_INFO_REQUEST] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_DUMMY,
		.handler = myicmp_discard,
	},
 	[ICMP_INFO_REPLY] = {
		.output_entry = ICMP_MIB_DUMMY,
		.input_entry = ICMP_MIB_DUMMY,
		.handler = myicmp_discard,
	},
	[ICMP_ADDRESS] = {
		.output_entry = ICMP_MIB_OUTADDRMASKS,
		.input_entry = ICMP_MIB_INADDRMASKS,
		.handler = myicmp_address,
	},
	[ICMP_ADDRESSREPLY] = {
		.output_entry = ICMP_MIB_OUTADDRMASKREPS,
		.input_entry = ICMP_MIB_INADDRMASKREPS,
		.handler = myicmp_address_reply,
	},
};

int myicmp_rcv(struct sk_buff *skb)
{
	struct icmphdr *icmph;
	struct rtable *rt = (struct rtable *)skb->dst;

	MYICMP_INC_STATS_BH(ICMP_MIB_INMSGS);

	switch (skb->ip_summed) {
	case CHECKSUM_HW:
		if (!(u16)csum_fold(skb->csum))
			break;
		/* fall through */
	case CHECKSUM_NONE:
		skb->csum = 0;
		if (__skb_checksum_complete(skb))
			goto error;
	}
	if (!pskb_pull(skb, sizeof(struct icmphdr)))
		goto error;

	icmph = skb->h.icmph;
	if (icmph->type > NR_ICMP_TYPES)
		goto error;
	if (rt->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST)) {
		if ((icmph->type == ICMP_ECHO ||
		     icmph->type == ICMP_TIMESTAMP) /*&&
		    sysctl_icmp_echo_ignore_broadcasts*/) {
			goto error;
		}
		if (icmph->type != ICMP_ECHO &&
		    icmph->type != ICMP_TIMESTAMP &&
		    icmph->type != ICMP_ADDRESS &&
		    icmph->type != ICMP_ADDRESSREPLY) {
			goto error;
  		}
	}
	printk(KERN_INFO "icmp type: %d\n", icmph->type);
	MYICMP_INC_STATS_BH( myicmp_pointers[icmph->type].input_entry );
	myicmp_pointers[icmph->type].handler(skb);

drop:
	kfree_skb(skb);
	return 0;
error:
	MYICMP_INC_STATS_BH(ICMP_MIB_INERRORS);
	goto drop;
}

static int myicmp_glue_bits(void *from, char *to, int offset, int len, int odd,
				struct sk_buff *skb)
{
	struct icmp_bxm *icmp_param = (struct icmp_bxm *)from;
	unsigned int csum;

	csum = skb_copy_and_csum_bits(icmp_param->skb,
				      icmp_param->offset + offset,
				      to, len, 0);

	skb->csum = csum_block_add(skb->csum, csum, odd);
	if( myicmp_pointers[icmp_param->data.icmph.type].error)
		nf_ct_attach(skb, icmp_param->skb);
	return 0;
}

static void myicmp_push_reply(struct icmp_bxm *icmp_param,
			    struct ipcm_cookie *ipc, struct rtable *rt)
{
	struct sk_buff *skb;

	if( myip_append_data( myicmp_socket->sk, myicmp_glue_bits, icmp_param,
							icmp_param->data_len+icmp_param->head_len,
							icmp_param->head_len,
							ipc, rt, MSG_DONTWAIT) < 0)
		myip_flush_pending_frames( myicmp_socket->sk );
	else if( (skb = skb_peek( &myicmp_socket->sk->sk_write_queue)) != NULL ){
		struct icmphdr *icmph = skb->h.icmph;
		unsigned int csum = 0;
		struct sk_buff *skb1;

		skb_queue_walk( &myicmp_socket->sk->sk_write_queue, skb1 ){
			csum = csum_add(csum, skb1->csum);
		}
		csum = csum_partial_copy_nocheck((void *)&icmp_param->data,
						 (char *)icmph,
						 icmp_param->head_len, csum);
		icmph->checksum = csum_fold(csum);
		skb->ip_summed = CHECKSUM_NONE;
		myip_push_pending_frames( myicmp_socket->sk );
	}
}

void myicmp_send(struct sk_buff *skb_in, int type, int code, u32 info)
{
	struct iphdr *iph;
	int room;
	struct icmp_bxm icmp_param;
	struct rtable *rt = (struct rtable *)skb_in->dst;
	struct ipcm_cookie ipc;
	u32 saddr;
	u8  tos;

	if (!rt)
		goto out;

	iph = skb_in->nh.iph;
	if ((u8 *)iph < skb_in->head || (u8 *)(iph + 1) > skb_in->tail)
		goto out;

	if (skb_in->pkt_type != PACKET_HOST)
		goto out;
	if (rt->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST))
		goto out;
	if (iph->frag_off & htons(IP_OFFSET))
		goto out;
	
	if( myicmp_pointers[type].error ){
		if (iph->protocol == IPPROTO_ICMP){
			u8 _inner_type, *itp;
			itp = skb_header_pointer(skb_in,
							skb_in->nh.raw + (iph->ihl << 2) + offsetof(struct icmphdr,type) -
							skb_in->data,
							sizeof(_inner_type),
							&_inner_type);
			if (itp == NULL)
				goto out;

			if( *itp > NR_ICMP_TYPES || myicmp_pointers[*itp].error )
				goto out;
		}
	}

	if( myicmp_xmit_lock() )
		return;

	saddr = iph->daddr;
	if (!(rt->rt_flags & RTCF_LOCAL)) {
		//if( sysctl_icmp_errors_use_inbound_ifaddr )
		//	saddr = inet_select_addr(skb_in->dev, 0, RT_SCOPE_LINK);
		//else
			saddr = 0;
	}

	tos = myicmp_pointers[type].error ? ((iph->tos & IPTOS_TOS_MASK) |
					IPTOS_PREC_INTERNETCONTROL) : iph->tos;

	if( myip_options_echo(&icmp_param.replyopts, skb_in) )
		goto out_unlock;

	icmp_param.data.icmph.type	 = type;
	icmp_param.data.icmph.code	 = code;
	icmp_param.data.icmph.un.gateway = info;
	icmp_param.data.icmph.checksum	 = 0;
	icmp_param.skb	  = skb_in;
	icmp_param.offset = skb_in->nh.raw - skb_in->data;
	myicmp_out_count(icmp_param.data.icmph.type);
	inet_sk(myicmp_socket->sk)->tos = tos;
	ipc.addr = iph->saddr;
	ipc.opt = &icmp_param.replyopts;
	
	{
		struct flowi fl = {
				.nl_u = {
						.ip4_u = {
								.daddr = icmp_param.replyopts.srr ?
										icmp_param.replyopts.faddr :
										iph->saddr,
								.saddr = saddr,
								.tos = RT_TOS(tos)
						}
				},
				.proto = MY_IPPROTO_ICMP,
				.uli_u = {
						.icmpt = {
								.type = type,
								.code = code
						}
				}
		};
		if( myip_route_output_key(&rt, &fl) )
			goto out_unlock;
	}
	//if (!icmpv4_xrlim_allow(rt, type, code))
	//	goto ende;

	room = dst_mtu(&rt->u.dst);
	if (room > 576) room = 576;
	room -= sizeof(struct iphdr) + icmp_param.replyopts.optlen;
	room -= sizeof(struct icmphdr);

	icmp_param.data_len = skb_in->len - icmp_param.offset;
	if (icmp_param.data_len > room)
		icmp_param.data_len = room;
	icmp_param.head_len = sizeof(struct icmphdr);

	myicmp_push_reply(&icmp_param, &ipc, rt);
ende:
	ip_rt_put(rt);
out_unlock:
	myicmp_xmit_unlock();
out:;
}

void __init myicmp_init(struct net_proto_family *ops)
{
	struct inet_sock *inet;
	int i;

	for_each_cpu(i){
		int err = sock_create_kern(MY_PF_INET, SOCK_RAW, MY_IPPROTO_ICMP,
						&per_cpu(__icmp_socket, i));
		if (err < 0)
			panic("Failed to create the ICMP control socket.\n");
		module_put( per_cpu(__icmp_socket, i)->ops->owner );
		module_put( per_cpu(__icmp_socket, i)->ops->owner );
		per_cpu(__icmp_socket, i)->sk->sk_allocation = GFP_ATOMIC;
		per_cpu(__icmp_socket, i)->sk->sk_sndbuf =
				(2 * ((64 * 1024) + sizeof(struct sk_buff)));

		inet = inet_sk(per_cpu(__icmp_socket, i)->sk);
		inet->uc_ttl = -1;
		inet->pmtudisc = IP_PMTUDISC_DONT;

		per_cpu(__icmp_socket, i)->sk->sk_prot->unhash(per_cpu(__icmp_socket, i)->sk);
	}
}

void __exit myicmp_exit(void)
{
	int i;
	for_each_cpu(i){
    	try_module_get( per_cpu(__icmp_socket, i)->ops->owner );
    	try_module_get( per_cpu(__icmp_socket, i)->ops->owner );
    	sock_release( per_cpu(__icmp_socket, i) );	
	}
}