path: root/src/common/flwd_common_stack.c

/*
	ͨ��Э��ջģ��, ������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;
}