diff --git a/src/tsg_icmp.cpp b/src/tsg_icmp.cpp index 70d610e..71594de 100644 --- a/src/tsg_icmp.cpp +++ b/src/tsg_icmp.cpp @@ -4,6 +4,7 @@ #include #include "tsg_entry.h" +#include #include "tsg_protocol_common.h" #define IPV4_TYPE 1 //ADDR_TYPE_IPV4 ==1 , 取的enum 0x0800 @@ -20,42 +21,48 @@ #define ICMPV6_PORT_UNREACHABLE 0X04 #define ICMPV6_ADMINISTRATIVELY_PROHIBITED 0x01 -#define MAC_LEN 6 -#define MAC_LEN_2 ((MAC_LEN)+(MAC_LEN)) -#define ETH_IP_TYPE_LEN 2 -#define ETH_LEN ((MAC_LEN_2)+(ETH_IP_TYPE_LEN)) +//icmp +#define ICMP_HEADR_BYTE_LEN 8 +#define ICMP_MAX_LEN 2048 -#define TCP_MAX_LEN 60 -#define ICMP_IPV4_TCP_MAX_LEN 44 //64-20 = 44 -#define IPV4_SHAM_FIXED_LEN 12 +//icmpv4 +#define IPV4_HEADR_BYTE_LEN 20 +#define IPV4_IPADDR_BYTE_LEN 4 +#define IPV4_TOTAL_LEN_INDEX 2 +#define IPV4_TOTAL_BYTE_LEN 2 +#define IPV4_SIP_INDEX 12 +#define IPV4_DIP_INDEX 16 +#define IPV4_PROTOCOL_INDEX 9 +#define IPV4_CHECKSUM_INDEX 10 +#define IPV4_CHECKSUM_BYTE_LEN 2 +#define ICMPV4_SRCPACKET_MAX_LEN 548 // 548 == ipv4(max_len 576)-ip_len(20)-icmp_head_len(8) -#define IPV4_LEN 20 -#define IPV4_IP_LEN 4 -#define IPV4_IP_LEN_INDEX 2 //eth_len(14)+ ip_len_index(2) -#define IPV6_LEN 40 -#define IPV6_IP_LEN 16 -#define IPV6_IP_PAYLOAD_INDEX 4 // ipv6_payload_index(4) -#define ICMP_IPV4_PROTOCOL_TYPE_LEN 24 -#define ICMP_HEAD_LEN 8 -#define ICMPV4_SOURCE_MAX_LEN 64 -#define ICMPV4_MAX_LEN ((ICMPV4_SOURCE_MAX_LEN)+(ICMP_HEAD_LEN)) -#define IPV6_PESUDO_HEAD_LEN 40 +//icmpv6 +//#define ICMPV6_ICMP_INDEX 40 +#define IPV6_HEADR_BYTE_LEN 40 +#define IPV6_IPADDR_BYTE_LEN 16 +#define IPV6_PAYLOAD_BYTE_LEN 2 +#define IPV6_SIP_INDEX 8 +#define IPV6_DIP_INDEX 24 +#define IPV6_PAYLOAD_INDEX 4 +#define IPV6_NEXT_HEADR_INDEX 6 +#define IPV6_2IP_BYTE_LEN 32 +#define IPV6_PESUDO_HEADR_BYTE_LEN 40 +#define IPV6_PESUDO_UPPER_LAYER_PACKET_AND_ZERO_LEN 7 +#define IPV6_PESUDO_HEAD_PAYLOAD_BYTE_LEN 4 +#define ICMPV6_SRCPACKET_MTU 1232 // 1232 == ipv6(max_len 1280)-ipv6_headr_len(40)-icmp_head_len(8) +#define ICMPV6_MTU 1280 // ICMPV6_SRCPACKET_MTU + IPV6_HEADR_BYTE_LEN + ICMPV6_HEARD_LEN -#define ICMP_MAX_LEN 65535 -#define ICMP_SRCPACKET_MAX_LEN 548 // 548 == ipv4(max_len 576)-ip_len(20)-icmp_head_len(8) -#define ICMPV6_SRCPACKET_MAX_LEN 1232 // 1232 == ipv6(max_len 1280)-ipv6_len(40)-icmp_head_len(8) -#define ICMPV6_MTU 1280 - -typedef struct icmpv4{ +typedef struct icmp_st_{ char type; char code; short checksum; char un_user[4]; - char srcPacket[ICMPV6_SRCPACKET_MAX_LEN]; -}icmpv4_st, icmpv6_st; + char origin_data[ICMPV6_SRCPACKET_MTU]; +}icmp_st; -short in_checksum(void *pkg, int size) +static short in_checksum(void *pkg, int size) { int nleft = size; int sum = 0; @@ -82,122 +89,196 @@ short in_checksum(void *pkg, int size) return answer; } -static void format_icmpv4(const char *raw_pkt, char *buf, int *len){ - char ipv4[IPV4_LEN] = {0}; - icmpv4_st icmpst = {0}; - short src_ipv4_total_len = 0; - short icmpv4_complete_len = 0; - short icmp_len = 0; - short icmp_srcpacket_len = 0; - short icmp_start_len = IPV4_LEN; - short ipv4_total_len = 0; - short ipv4_checksum = 0; - short sip_len = 12; //skip sip start index - short dip_len = 16; //skip dip start index - memcpy(&src_ipv4_total_len, &raw_pkt[IPV4_IP_LEN_INDEX], sizeof(short)); - src_ipv4_total_len = htons(src_ipv4_total_len); +static void four_byte_alignment(short *icmp_len, short *icmp_checksum_len){ + short temp = 0; - //src_packet_len = src_ipv4_total_len + ETH_LEN; - if(src_ipv4_total_len >= ICMP_SRCPACKET_MAX_LEN){ - icmp_srcpacket_len = ICMP_SRCPACKET_MAX_LEN; - }else{ - icmp_srcpacket_len = src_ipv4_total_len; + temp = *icmp_len % 4; + if(temp >0){ + *icmp_len = *icmp_len+(4-temp); + *icmp_checksum_len = *icmp_checksum_len+(4-temp); } - icmp_len = icmp_srcpacket_len + ICMP_HEAD_LEN; - ipv4_total_len = icmp_len + IPV4_LEN; - icmpv4_complete_len = IPV4_LEN + icmp_len; + return ; +} - //format icmp information - memset(&icmpst, 0, sizeof(icmpv4_st)); - memcpy(icmpst.srcPacket, raw_pkt, icmp_srcpacket_len); // - icmpst.type = ICMPV4_UNREACHABLE; - icmpst.code = ICMPV4_ADMINISTRATIVELY_PROHIBITED; - icmpst.checksum = in_checksum((void*)&icmpst, icmp_len); +/* + * ICMP 报文格式 + * 0 1 2 3 + * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Type | Code | Checksum | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | unused | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Internet Header + 64 bits of Original Data Datagram | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * ICMPV4 = [ipv4_headr]+[icmp] + * ICMPV4(MTU) 576 + */ - //format ipv4 - memcpy(ipv4, raw_pkt, IPV4_LEN); //copy source data - memcpy(&ipv4[12], &raw_pkt[dip_len], IPV4_IP_LEN); //get sip - memcpy(&ipv4[16], &raw_pkt[sip_len], IPV4_IP_LEN); //get dip - ipv4[9] = ICMP_PROTOCOL_TYPE; //set ipv4 protocol type (0x01 == ICMP) +static void format_icmpv4(const char *raw_pkt, char *ret_icmpv4, int *ret_len){ + icmp_st icmp = {0}; + char ipv4_headr[IPV4_HEADR_BYTE_LEN] = {0}; + short raw_ipv4_total_len = 0; + short icmpv4_len = 0; + short ipv4_total_len = 0; + short ipv4_checksum = 0; + short icmp_len = 0; + short icmp_original_data_len = 0; + short icmp_start_len = IPV4_HEADR_BYTE_LEN; + + memcpy(&raw_ipv4_total_len, &raw_pkt[IPV4_TOTAL_LEN_INDEX], IPV4_TOTAL_BYTE_LEN); + raw_ipv4_total_len = htons(raw_ipv4_total_len); + + //src_packet_len = raw_ipv4_total_len + ETH_LEN; + if(raw_ipv4_total_len >= ICMPV4_SRCPACKET_MAX_LEN){ + icmp_original_data_len = ICMPV4_SRCPACKET_MAX_LEN; + }else{ + icmp_original_data_len = raw_ipv4_total_len; + } + + icmp_len = icmp_original_data_len + ICMP_HEADR_BYTE_LEN; + ipv4_total_len = icmp_len + IPV4_HEADR_BYTE_LEN; + icmpv4_len = ipv4_total_len; + + //format ipv4 headr + memcpy(ipv4_headr, raw_pkt, IPV4_HEADR_BYTE_LEN); //copy source data + memcpy(&ipv4_headr[IPV4_SIP_INDEX], &raw_pkt[IPV4_DIP_INDEX], IPV4_IPADDR_BYTE_LEN); //format ipv4 sip + memcpy(&ipv4_headr[IPV4_DIP_INDEX], &raw_pkt[IPV4_SIP_INDEX], IPV4_IPADDR_BYTE_LEN); //format ipv4 dip + ipv4_headr[IPV4_PROTOCOL_INDEX] = ICMP_PROTOCOL_TYPE; //set ipv4 protocol type (0x01 == ICMP) ipv4_total_len = htons(ipv4_total_len); - memcpy(&ipv4[2], &ipv4_total_len, sizeof(short)); //format ipv4 len - memset(&ipv4[10], 0, sizeof(short)); //empty checksum data - ipv4_checksum = in_checksum(ipv4, IPV4_LEN); //calc ipv4 checksum - memcpy(&ipv4[10], &ipv4_checksum, sizeof(short)); //format checksum + memcpy(&ipv4_headr[IPV4_TOTAL_LEN_INDEX], &ipv4_total_len, IPV4_TOTAL_BYTE_LEN); //format ipv4 total len + memset(&ipv4_headr[IPV4_CHECKSUM_INDEX], 0, IPV4_CHECKSUM_BYTE_LEN); //empty checksum data + ipv4_checksum = in_checksum(ipv4_headr, IPV4_HEADR_BYTE_LEN); //calc ipv4 checksum + memcpy(&ipv4_headr[IPV4_CHECKSUM_INDEX], &ipv4_checksum, IPV4_CHECKSUM_BYTE_LEN); //format ipv4 checksum + + //format icmp + memset(&icmp, 0, sizeof(icmp_st)); + memcpy(icmp.origin_data, raw_pkt, icmp_original_data_len); + icmp.type = ICMPV4_UNREACHABLE; + icmp.code = ICMPV4_ADMINISTRATIVELY_PROHIBITED; + icmp.checksum = in_checksum((void*)&icmp, icmp_len); //format complete icmpv4 packet - memcpy(buf, ipv4, IPV4_LEN); - memcpy(&buf[icmp_start_len], &icmpst, icmp_len); - *len = icmpv4_complete_len; + memcpy(ret_icmpv4, ipv4_headr, IPV4_HEADR_BYTE_LEN); + memcpy(&ret_icmpv4[icmp_start_len], &icmp, icmp_len); + *ret_len = icmpv4_len; return; } -static void format_icmpv6(const char *data, char *buf, int *len){ - char checksum_str[ICMPV6_MTU] = {0}; - char ipv6[IPV6_LEN] = {0}; - icmpv6_st icmpst = {0}; - short src_ipv6_total_len = 0; - short icmpv6_complete_len = 0; +/* + * icmpv6数据包组装 + * [ICMPV6] = [ipv6_headr]+[icmp] + * + * icmpv6的最大MTU应是ipv6的最小MTU(1280) + * <本端内容来自RFC443> + * Every ICMPv6 error message (type < 128) MUST include as much of + * the IPv6 offending (invoking) packet (the packet that caused the + * error) as possible without making the error message packet exceed + * the minimum IPv6 MTU [IPv6]. + * + * [ICMPV6] = [ipv6_headr]+[icmp] + * + * 计算ICMPV6的校验和： (ICMP + 伪首部) 作为计算数据部分 + * 注意： ICMPV6数据要补充成四字节对齐 + * ipv6伪首部数据: 长度 + * { + * Soucre Address : 16 + * Destination Address: 16 + * Upper-Layer Packet Length: 4 + * zero: 3 + * Next Heard: 1 + * } + * + ** +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * + + + * ~ ~ + * ~ icmp ~ + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * + + + * | | + * + Source Address + + * | | + * + + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * + + + * | | + * + Destination Address + + * | | + * + + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Upper-Layer Packet Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | zero | Next Header | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * [ICMPV6] = [ipv6_headr]+[icmp] + * ICMPV6(MTU) 1280 + */ + +static void format_icmpv6(const char *raw_pkt, char *ret_icmpv6, int *ret_len){ + icmp_st icmp = {0}; + char icmp_checksum_buf[ICMPV6_MTU] = {0}; + char ipv6_headr[IPV6_HEADR_BYTE_LEN] = {0}; + short ipv6_payload_len = 0; + short raw_ipv6_payload_len = 0; + short raw_pkt_len = 0; + short icmpv6_len = 0; short icmp_len = 0; - short icmp_srcpacket_len = 0; - short icmp_start_len = IPV6_LEN; - short checksum_len = 0; - short sip_len = 8; //skip sip start index - short dip_len = 24; //skip dip start index, 16+8 == 24 - short ipv6_ip2 = IPV6_IP_LEN + IPV6_IP_LEN; - short payload_len = 0; - short fill_icmp_len = 0; - int checksum_payload_len = 0; + short icmp_original_data_len = 0; + short icmp_checksum_len = 0; + int upper_layer_packet_len = 0; - memcpy(&src_ipv6_total_len, &data[IPV6_IP_PAYLOAD_INDEX], sizeof(short)); //get ipv6_payload_len - src_ipv6_total_len = htons(src_ipv6_total_len) + IPV6_LEN; + memcpy(&raw_ipv6_payload_len, &raw_pkt[IPV6_PAYLOAD_INDEX], IPV6_PAYLOAD_BYTE_LEN); //get ipv6_payload_len + raw_pkt_len = ntohs(raw_ipv6_payload_len) + IPV6_HEADR_BYTE_LEN; - if(src_ipv6_total_len >= ICMPV6_SRCPACKET_MAX_LEN){ - icmp_srcpacket_len = ICMPV6_SRCPACKET_MAX_LEN; + if(raw_pkt_len >= ICMPV6_SRCPACKET_MTU){ + icmp_original_data_len = ICMPV6_SRCPACKET_MTU; }else{ - icmp_srcpacket_len = src_ipv6_total_len; + icmp_original_data_len = raw_pkt_len; } - icmp_len = icmp_srcpacket_len + ICMP_HEAD_LEN; - icmpv6_complete_len = IPV6_LEN + icmp_len; - checksum_len = icmp_len + IPV6_PESUDO_HEAD_LEN; - payload_len = htons(icmp_len); + icmp_len = icmp_original_data_len + ICMP_HEADR_BYTE_LEN; + icmpv6_len = IPV6_HEADR_BYTE_LEN + icmp_len; + icmp_checksum_len = icmp_len + IPV6_PESUDO_HEADR_BYTE_LEN; + ipv6_payload_len = htons(icmp_len); - //format ipv6 - memcpy(ipv6, data, IPV6_LEN); //copy source ipv6 data - memcpy(&ipv6[8], &data[dip_len], IPV6_IP_LEN); //get sip - memcpy(&ipv6[24],&data[sip_len], IPV6_IP_LEN); //get dip - memcpy(&ipv6[4], &payload_len, sizeof(short)); //format ipv6 payload - ipv6[6] = ICMPV6_PROTOCAL_TYPE; //format ipv6 protocol (icmpv6 == 0X3a); + //format ipv6 headr + memcpy(ipv6_headr, raw_pkt, IPV6_HEADR_BYTE_LEN); //format source ipv6 data + memcpy(&ipv6_headr[IPV6_SIP_INDEX], &raw_pkt[IPV6_DIP_INDEX], IPV6_IPADDR_BYTE_LEN); //modify format sip + memcpy(&ipv6_headr[IPV6_DIP_INDEX], &raw_pkt[IPV6_SIP_INDEX], IPV6_IPADDR_BYTE_LEN); //modify format dip + memcpy(&ipv6_headr[IPV6_PAYLOAD_INDEX], &ipv6_payload_len, IPV6_PAYLOAD_BYTE_LEN); //modify format ipv6 payload + ipv6_headr[IPV6_NEXT_HEADR_INDEX] = ICMPV6_PROTOCAL_TYPE; //modify format ipv6 protocol (icmpv6 == 0X3a); //format icmp - memset(&icmpst, 0, sizeof(icmpv6_st)); - icmpst.type = ICMPV6_UNREACHABLE; - icmpst.code = ICMPV6_ADMINISTRATIVELY_PROHIBITED; - memcpy(icmpst.srcPacket, data, icmp_srcpacket_len); + memset(&icmp, 0, sizeof(icmp_st)); + icmp.type = ICMPV6_UNREACHABLE; + icmp.code = ICMPV6_ADMINISTRATIVELY_PROHIBITED; + memcpy(icmp.origin_data, raw_pkt, icmp_original_data_len); - //补充为4字节 - checksum_payload_len = htonl((int)icmp_len); - fill_icmp_len = icmp_len % 4; - if( fill_icmp_len > 0){ - icmp_len = icmp_len + (4-fill_icmp_len); - checksum_len = checksum_len + (4-fill_icmp_len); - } + upper_layer_packet_len = htonl((int)icmp_len); + four_byte_alignment(&icmp_len, &icmp_checksum_len); //icmp_len和icmp_checksum_len 补充为4字节对齐 - //calc icmpv6 checksum - memcpy(checksum_str, &icmpst, icmp_len); - memcpy(&checksum_str[icmp_len], &ipv6[8], ipv6_ip2); - memcpy(&checksum_str[icmp_len+ipv6_ip2], &checksum_payload_len, sizeof(int)); - checksum_str[icmp_len+ipv6_ip2+7] = ICMPV6_PROTOCAL_TYPE; - icmpst.checksum = in_checksum(checksum_str, checksum_len); + //calc icmp checksum + memcpy(icmp_checksum_buf, &icmp, icmp_len); + memcpy(&icmp_checksum_buf[icmp_len], &ipv6_headr[IPV6_SIP_INDEX], IPV6_2IP_BYTE_LEN); + memcpy(&icmp_checksum_buf[icmp_len+IPV6_2IP_BYTE_LEN], &upper_layer_packet_len, IPV6_PESUDO_HEAD_PAYLOAD_BYTE_LEN); + icmp_checksum_buf[icmp_len+IPV6_2IP_BYTE_LEN+IPV6_PESUDO_UPPER_LAYER_PACKET_AND_ZERO_LEN] = ICMPV6_PROTOCAL_TYPE; + icmp.checksum = in_checksum(icmp_checksum_buf, icmp_checksum_len); //format complete icmpv6 packet - memcpy(buf, ipv6, IPV6_LEN); - memcpy(&buf[icmp_start_len], &icmpst, icmp_len); - *len = icmpv6_complete_len; + memcpy(ret_icmpv6, ipv6_headr, IPV6_HEADR_BYTE_LEN); + memcpy(&ret_icmpv6[IPV6_HEADR_BYTE_LEN], &icmp, icmp_len); + *ret_len = icmpv6_len; return; } @@ -214,6 +295,7 @@ static void format_icmp(const char *raw_pkt, char *icmp_buf, int *icmp_len, int unsigned char send_icmp_unreachable(const struct streaminfo *a_stream, const void *raw_pkt) { char icmp_buf[ICMP_MAX_LEN] = {0}; + char debug_buf[512] = {0}; int icmp_len = 0; if((a_stream==NULL)||(raw_pkt==NULL)){ @@ -221,7 +303,17 @@ unsigned char send_icmp_unreachable(const struct streaminfo *a_stream, const voi } format_icmp((char *)raw_pkt, icmp_buf, &icmp_len, a_stream->addr.addrtype); - tsg_send_inject_packet(a_stream, SIO_EXCLUDE_THIS_LAYER_HDR, icmp_buf, icmp_len, MESA_dir_reverse(a_stream->routedir)); + if (0 == tsg_send_inject_packet(a_stream, SIO_EXCLUDE_THIS_LAYER_HDR, icmp_buf, icmp_len, MESA_dir_reverse(a_stream->routedir))){ + sprintf(debug_buf, "Addr: %s, send_icmp_unreachable sucess, raw_pkt %p, icmp_buf %p, icmp_len %d", PRINTADDR(a_stream, g_tsg_para.level), raw_pkt, icmp_buf, icmp_len); + }else{ + sprintf(debug_buf, "Addr: %s, send_icmp_unreachable failed, raw_pkt %p, icmp_buf %p, icmp_len %d", PRINTADDR(a_stream, g_tsg_para.level), raw_pkt, icmp_buf, icmp_len); + } + + MESA_handle_runtime_log(g_tsg_para.logger, + RLOG_LV_DEBUG, + __FUNCTION__, + debug_buf); + return STATE_DROPPKT; }