1 files changed, 449 insertions, 0 deletions

diff --git a/src/common/flwd_common_stack.c b/src/common/flwd_common_stack.c
new file mode 100644
index 0000000..9dfc09b
--- /dev/null
+++ b/src/common/flwd_common_stack.c
@@ -0,0 +1,449 @@
+/*
+	ͨ��Э��ջģ��, ������ARP, ICMPЭ��Ĵ���.
+*/
+
+#include "flowood.h"
+#include "flowood_fun.h"
+#include "flwd_net.h"
+#include "MESA_handle_logger.h"
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <string.h>
+#include <assert.h>
+#include <arpa/inet.h>
+
+const unsigned char G_FLWD_BROADCAST_ADDR[ETH_ALEN] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
+/*
+	TODO:
+	�����ӿ�����IP�����Ĭ��·��, ʵ��������IPЭ��ջ,
+	��Ŀ���������ܲ���һ������(��·������VLAN),
+	��Ҫ��������arp����һ������.
+*/
+
+
+static inline int sendpacket_in_cksum(u_int16_t *addr, int len)
+{
+    int sum;
+    int nleft;
+    u_int16_t ans;
+    u_int16_t *w;
+
+    sum = 0;
+    ans = 0;
+    nleft = len;
+    w = addr;
+
+    while (nleft > 1)
+    {
+        sum += *w++;
+        nleft -= 2;
+    }
+    if (nleft == 1)
+    {
+        *(char *)(&ans) = *(char *)w;
+        sum += ans;
+    }
+    return (sum);
+}
+
+/*
+ *  Checksum stuff
+ */
+#define SENDPACKET_CKSUM_CARRY(x) \
+    (x = (x >> 16) + (x & 0xffff), (~(x + (x >> 16)) & 0xffff))
+    
+int flwd_sendpacket_do_checksum(char *buf, int protocol, int len)
+{
+    flwd_ipv4_hdr_t *iph_p;
+    flwd_ipv6_hdr_t *ip6h_p;
+    int ip_hl;
+    int sum;
+    int is_ipv6 = 0;
+
+    sum = 0;
+    iph_p = (flwd_ipv4_hdr_t *)buf;
+
+    if(4 == iph_p->ip_v)  /* IP�汾���ֶΣ�IPv4��IPv6��ʽ����ͬ�� */
+    {
+        ip_hl = iph_p->ip_hl << 2;
+        ip6h_p = NULL;
+    }
+    else if(6 == iph_p->ip_v)
+    {
+        ip6h_p = (flwd_ipv6_hdr_t *)buf;
+        iph_p = NULL;
+        ip_hl = sizeof(flwd_ipv6_hdr_t);
+        is_ipv6 = 1;
+    }
+    else
+    {
+        return (-1);
+    }
+
+    /*
+     *  Dug Song came up with this very cool checksuming implementation
+     *  eliminating the need for explicit psuedoheader use.  Check it out.
+     */
+    switch (protocol)
+    {
+        /*
+         *  Style note: normally I don't advocate declaring variables inside
+         *  blocks of control, but it makes good sense here. -- MDS
+         */
+        case IPPROTO_TCP:
+        {
+            flwd_tcp_hdr_t *tcph_p =
+                (flwd_tcp_hdr_t *)(buf + ip_hl);
+
+#if (STUPID_SOLARIS_CHECKSUM_BUG)
+            tcph_p->th_sum = tcph_p->th_off << 2;
+            return (1);
+#endif /* STUPID_SOLARIS_CHECKSUM_BUG */
+
+            tcph_p->th_sum = 0;
+            /* 2012-03-19 LiJia add, for IPv6 */
+            if(is_ipv6)
+            {
+                sum = sendpacket_in_cksum((u_int16_t *)&ip6h_p->ip6_src, 32);
+            }
+            else
+            {
+                sum = sendpacket_in_cksum((u_int16_t *)&iph_p->ip_src, 8);
+            }    
+            sum += ntohs(IPPROTO_TCP + len);
+            sum += sendpacket_in_cksum((u_int16_t *)tcph_p, len);
+            tcph_p->th_sum = SENDPACKET_CKSUM_CARRY(sum);
+            break;
+        }
+        
+        case IPPROTO_UDP:
+        {
+            flwd_udp_hdr_t *udph_p =
+                (flwd_udp_hdr_t *)(buf + ip_hl);
+
+            udph_p->uh_sum = 0;
+            /* 2012-03-19 LiJia add, for IPv6 */
+            if(is_ipv6)
+            {
+                sum = sendpacket_in_cksum((u_int16_t *)&ip6h_p->ip6_src, 32);
+            }
+            else
+            {
+                sum = sendpacket_in_cksum((u_int16_t *)&iph_p->ip_src, 8);
+            }   
+            sum += ntohs(IPPROTO_UDP + len);
+            sum += sendpacket_in_cksum((u_int16_t *)udph_p, len);
+            udph_p->uh_sum = SENDPACKET_CKSUM_CARRY(sum);
+            break;
+        }
+        
+        case IPPROTO_IP: /* Dummy protocol for TCP.  */
+        {
+            iph_p->ip_sum = 0;
+            sum = sendpacket_in_cksum((u_int16_t *)iph_p, len);
+            iph_p->ip_sum = SENDPACKET_CKSUM_CARRY(sum);
+            break;
+        }
+
+        case IPPROTO_ICMP:
+        {
+            flwd_icmp_hdr_t *icmph_p =
+                (flwd_icmp_hdr_t *)(buf + ip_hl);
+
+            icmph_p->icmp_sum = 0;
+            sum = sendpacket_in_cksum((u_short *)icmph_p, len);
+            icmph_p->icmp_sum = SENDPACKET_CKSUM_CARRY(sum);
+            break;
+        }
+        
+        default:
+        {
+            return (-1);
+        }
+    }
+    
+    return (1);
+}
+
+
+
+/*
+	�����������߰���İ�(�㲥���鲥, �DZ���IP,MAC�İ�)ʶ�𲢶���.
+
+	args:
+		check_dip_expect_cmp_res: ���Ŀ��IP�ͱ�������IP�Ĺ�ϵ,
+			user->acc_gateway���ܼ��, ��Ϊdip�϶���������IP;
+			acc_gateway->fwd_gateway, fwd_gateway->acc_gateway������, ��Ϊ�Ǿ�������ͨ��;
+			gdev->fwd_gateway������, ���ʹ��marsio����, ��������ʵ��.
+*/
+int flwd_rubbish_pkt_identify(flwd_device_handle_t *device_handle, 
+    flwd_raw_pkt_t *raw_pkt, int check_dip_expect_cmp_res)
+{
+	const flwd_eth_hdr_t *eth_hdr = (const flwd_eth_hdr_t *)raw_pkt->outer_pkt_data;
+	flwd_ipv4_hdr_t *ip4hdr;
+	flwd_ipv6_hdr_t *ip6hdr;
+	unsigned eth_pro_type = ntohs(eth_hdr->h_proto);
+
+	if(CAP_MODEL_SOCKET == device_handle->io_para.cap_mode){
+		flwd_eth_hdr_t *inner_eth_hdr;
+		if((unsigned int)raw_pkt->inner_pkt_len < sizeof(flwd_eth_hdr_t) + sizeof(flwd_ipv4_hdr_t) + sizeof(flwd_vxlan_hdr_t)){
+			return 1;
+		}
+
+		inner_eth_hdr = (flwd_eth_hdr_t *)raw_pkt->inner_pkt_data;
+		if(ETH_P_IP == ntohs(inner_eth_hdr->h_proto)){
+			ip4hdr = (flwd_ipv4_hdr_t *)(raw_pkt->inner_pkt_data + sizeof(flwd_eth_hdr_t));
+			if(ip4hdr->ip_v != 4){
+				return 1;
+			}
+			if(ip4hdr->ip_hl < 5){
+				return 1;
+			}
+			if(ntohs(ip4hdr->ip_len) != (raw_pkt->inner_pkt_len - sizeof(flwd_eth_hdr_t))){
+				return 1;
+			}
+		}else if(ETH_P_IPV6 != ntohs(inner_eth_hdr->h_proto)){
+			ip6hdr = (flwd_ipv6_hdr_t *)(flwd_ipv6_hdr_t *)(raw_pkt->inner_pkt_data + sizeof(flwd_eth_hdr_t));
+			if((ip6hdr->ip6_flags[0] & 0xF0) != 6){
+				return 1;
+			}
+			if(ntohs(ip6hdr->ip6_payload_len) != (raw_pkt->inner_pkt_len - sizeof(flwd_eth_hdr_t) - sizeof(flwd_ipv6_hdr_t))){
+				return 1;
+			}
+		}else{
+			return 1;
+		}
+	}
+
+	if(device_handle->io_para.cap_mode != CAP_MODEL_SOCKET){ /* �ڻ��Ӳ���ģʽ�¼�� */
+		if(memcmp(G_FLWD_BROADCAST_ADDR, eth_hdr->h_dest, ETH_ALEN) == 0){
+			if(ETH_P_ARP != eth_pro_type){ /* ����ǹ㲥������ARP, ������϶���Ӧ�ó�NAT����, ֱ�Ӷ��� */
+				return 1;
+			}
+		}else if(memcmp(device_handle->io_para.local_mac_addr, eth_hdr->h_dest, ETH_ALEN) != 0){
+			/* ���ǹ㲥, Ŀ��MACҲ���DZ���, ���� */
+			return 1;
+		}
+
+#if 0  /* IPv6���ھӷ��ְ�ʹ���鲥��ַʵ��, ����IPv4��ARP, ��Ҫ����, ���ܶ��� */
+	else if((eth_hdr->h_dest[0] & 0x01) == 0x01){		
+		/* �鲥MAC��ַ, ͨ��Ϊ�������ڿ��������ݰ�, ��LLMNR, SPT��Э��, һ�����账�� */	
+		return 1;
+	}
+#endif
+
+		if(ETH_P_IP == eth_pro_type){
+			ip4hdr = (flwd_ipv4_hdr_t *)((char *)eth_hdr + sizeof(flwd_eth_hdr_t));
+
+			if(FLWD_IPV4_MULTICAST_ADDR(ntohl(ip4hdr->ip_dst.s_addr)) != 0){
+				return 1;  /* �鲥IP��ַ, ������ */
+			}
+
+			if(check_dip_expect_cmp_res != (ip4hdr->ip_dst.s_addr == device_handle->io_para.device_ip_net_order)){ 
+				return 1;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int flwd_protocol_stack_icmp_layer_process(
+    flwd_device_handle_t *io_handle, int tid, flwd_raw_pkt_t *raw_pkt, 
+    flwd_ipv4_hdr_t *raw_ip_hdr, const flwd_simple_icmp_hdr_t *raw_icmp_hdr)
+{
+	int ret;
+	void *io_mbuff;
+	char *send_user_buf;
+	int raw_ip_tot_len = ntohs(raw_ip_hdr->ip_len);
+	int icmp_payload_len = raw_ip_tot_len - raw_ip_hdr->ip_hl * 4 - sizeof(flwd_simple_icmp_hdr_t);
+	
+	///flwd_eth_hdr_t *snd_eth_hdr;
+	const flwd_eth_hdr_t *raw_eth_hdr = (const flwd_eth_hdr_t *)raw_pkt->outer_pkt_data;
+
+	/* Ϊ��Ӧ��ͬ�ײ�����, �������ڴ�, ����ICMP_REPLY���ٷ���, ������ֱ���޸�ԭʼ���� */
+	io_mbuff = io_handle->low_level_mbuff_malloc(io_handle, tid, raw_pkt->outer_pkt_len);
+	assert(io_mbuff != NULL);
+
+	send_user_buf = (char *)io_handle->low_level_mbuff_mtod(io_mbuff);
+
+	flwd_sendpacket_build_icmpv4_echo(ICMP_ECHOREPLY, 0, 0, 
+									raw_icmp_hdr->icd_id,
+									raw_icmp_hdr->icd_seq,
+									(char *)raw_icmp_hdr + sizeof(flwd_simple_icmp_hdr_t),
+									icmp_payload_len,
+									send_user_buf + sizeof(flwd_eth_hdr_t) + sizeof(flwd_ipv4_hdr_t));
+									
+	flwd_sendpacket_build_ipv4(raw_ip_tot_len - sizeof(flwd_ipv4_hdr_t), 
+							0,
+							0x1234,
+							0,
+							64,
+							IPPROTO_ICMP,
+							raw_ip_hdr->ip_dst.s_addr, /* ��ַȡ�� */
+							raw_ip_hdr->ip_src.s_addr, /* ��ַȡ�� */
+							NULL,
+							0,
+							send_user_buf + sizeof(flwd_eth_hdr_t));
+
+	/* ������� */
+	flwd_sendpacket_do_checksum(send_user_buf + sizeof(flwd_eth_hdr_t),
+						   IPPROTO_IP, 
+						   sizeof(flwd_ipv4_hdr_t));
+						   
+	flwd_sendpacket_do_checksum(send_user_buf + sizeof(flwd_eth_hdr_t),
+						   IPPROTO_ICMP, 
+						   icmp_payload_len + sizeof(flwd_simple_icmp_hdr_t));
+    					   
+	flwd_sendpacket_build_ethernet(ETH_P_IP, raw_eth_hdr->h_dest, raw_eth_hdr->h_source, send_user_buf);
+
+	io_handle->low_level_mbuff_set_pkt_len(io_mbuff, raw_pkt->outer_pkt_len);
+
+	ret = io_handle->low_level_send(io_handle, tid, io_mbuff);
+	if(ret < 0){
+		flwd_log(RLOG_LV_FATAL, "send icmp reply error!");
+	}else{
+		char icmp_dip_str[16];
+		inet_ntop(AF_INET, &raw_ip_hdr->ip_dst.s_addr, icmp_dip_str, 16);
+		flwd_log(RLOG_LV_DEBUG, "ICMP: recv icmp request to %s, send icmp reply!\n", icmp_dip_str); 
+	}
+
+	io_handle->low_level_mbuff_free_after_send(io_handle, tid, io_mbuff);
+	
+	return ret;
+}
+
+
+static int flwd_protocol_stack_ipv4_layer_process(
+    flwd_device_handle_t *io_handle, int tid, flwd_raw_pkt_t *raw_pkt, flwd_ipv4_hdr_t *iphdr)
+{
+	const flwd_simple_icmp_hdr_t *flwd_simple_icmp_hdr;
+	
+	if(io_handle->io_para.device_ip_net_order != iphdr->ip_dst.s_addr){
+		return 0;
+	}
+
+	if(iphdr->ip_p != IPPROTO_ICMP){
+		return 0;
+	}
+
+	flwd_simple_icmp_hdr = (flwd_simple_icmp_hdr_t *)((char *)iphdr + iphdr->ip_hl * 4);
+	if(flwd_simple_icmp_hdr->icmp_type != ICMP_ECHO){
+		return 0;
+	}
+
+	flwd_protocol_stack_icmp_layer_process(io_handle, tid, raw_pkt, iphdr, flwd_simple_icmp_hdr);
+
+	return 1;
+}
+
+
+static int flwd_protocol_stack_ipv6_layer_process(
+    flwd_device_handle_t *io_handle, int tid, flwd_raw_pkt_t *raw_pkt, flwd_ipv6_hdr_t *ip6hdr)
+{
+	if(IPPROTO_ICMPV6 == ip6hdr->ip6_nxt_hdr){
+		/* TODO, 
+			��ICMPv6���п������Ƿ���Netighbor ����Э��,
+			ȡ��IP��ַ, �鿴�Ƿ��DZ���, 
+			Ȼ��ظ�Ӧ���.
+		*/
+	}
+	
+	return 0;
+}
+
+static int flwd_protocol_stack_arp_layer_process(
+    flwd_device_handle_t *io_handle, int tid, flwd_raw_pkt_t *raw_pkt, flwd_arp_hdr_t *arp_hdr)
+{
+	int ret;
+	void *io_mbuff;
+	char *send_user_buf;
+	const flwd_eth_hdr_t *raw_eth_hdr;
+	
+	if(memcmp(arp_hdr->ar_tpa,  &io_handle->io_para.device_ip_net_order, sizeof(int)) != 0){
+		return 1; /* �DZ���ARP, ����Ҳ����1, ���ⲿ�����߲��ٴ��������ݰ�, ��������Ҳ���ظ�ARPӦ�� */
+	}
+
+	if(arp_hdr->ar_op == htons(ARPOP_REPLY)){
+		flwd_arp_response_update(arp_hdr);
+		return 1;
+	}
+
+	if(arp_hdr->ar_op != htons(ARPOP_REQUEST)){
+		/* ֻ����REQUEST��REPLY, �������Ͳ����� */
+		return 1;
+	}
+	
+	/* Ϊ��Ӧ��ͬ�ײ�����, �������ڴ�, ����ICMP_REPLY���ٷ���, ������ֱ���޸�ԭʼ���� */
+	io_mbuff = io_handle->low_level_mbuff_malloc(io_handle, tid, raw_pkt->outer_pkt_len);
+	assert(io_mbuff != NULL);
+
+	send_user_buf = (char *)io_handle->low_level_mbuff_mtod(io_mbuff);
+	
+	flwd_sendpacket_build_arp(ARPHRD_ETHER, ETH_P_IP, arp_hdr->ar_hln, arp_hdr->ar_pln,
+							ARPOP_REPLY, 
+							io_handle->io_para.local_mac_addr, 
+							arp_hdr->ar_tpa, 
+							arp_hdr->ar_sha, 
+							arp_hdr->ar_spa, 
+							send_user_buf + sizeof(flwd_eth_hdr_t));
+
+	raw_eth_hdr = (const flwd_eth_hdr_t *)raw_pkt->outer_pkt_data;
+	flwd_sendpacket_build_ethernet(ETH_P_ARP, io_handle->io_para.local_mac_addr, 
+									raw_eth_hdr->h_source, send_user_buf);
+
+	io_handle->low_level_mbuff_set_pkt_len(io_mbuff, raw_pkt->outer_pkt_len);
+
+	ret = io_handle->low_level_send(io_handle, tid, io_mbuff);
+	if(ret < 0){
+		flwd_log(RLOG_LV_FATAL, "send arp reply error!");
+	}else{
+		char arp_dip_str[16];
+		inet_ntop(AF_INET, arp_hdr->ar_tpa, arp_dip_str, 16);
+		flwd_log(RLOG_LV_DEBUG, "ARP: recv arp request to %s, send arp reply!\n", arp_dip_str); 
+	}
+
+	io_handle->low_level_mbuff_free_after_send(io_handle, tid, io_mbuff);
+	
+	return 1; /* ARPЭ��϶�����flowoodģ��Ҫ������, �����Dz��DZ�����, ���̶�����1 */
+}
+
+/* 
+	IP��Э��ջ�����������, ����arp����, icmp�����.
+	return value:
+		1: ��Э��ջ����, ����flowoodģ���������;
+		0: ����Э��ջ����, ��IP��������.
+*/
+int flwd_protocol_stack_process(flwd_device_handle_t *io_handle, 
+    int tid, flwd_raw_pkt_t *raw_pkt)
+{
+	int is_stack = 0;
+	
+	const flwd_eth_hdr_t *flwd_ethhdr = (const flwd_eth_hdr_t *)raw_pkt->outer_pkt_data;
+
+	switch(ntohs(flwd_ethhdr->h_proto)){
+		case ETH_P_IP:
+			is_stack = flwd_protocol_stack_ipv4_layer_process(io_handle, tid, raw_pkt, (flwd_ipv4_hdr_t *)((char *)flwd_ethhdr + sizeof(flwd_eth_hdr_t)));
+			flwd_thread_val[tid].pkt_stat.ip_pkt_num++;
+			flwd_thread_val[tid].pkt_stat.ip_pkt_byte += raw_pkt->outer_pkt_len;
+		break;
+
+		case ETH_P_IPV6:
+			is_stack = flwd_protocol_stack_ipv6_layer_process(io_handle, tid, raw_pkt, (flwd_ipv6_hdr_t *)((char *)flwd_ethhdr + sizeof(flwd_eth_hdr_t)));
+		break;
+
+		case ETH_P_ARP:
+			is_stack = flwd_protocol_stack_arp_layer_process(io_handle, tid, raw_pkt, (flwd_arp_hdr_t *)((char *)flwd_ethhdr + sizeof(flwd_eth_hdr_t)));
+		break;
+
+		default:
+		break;
+	}
+	
+	return is_stack;
+}
+