tango-kni/common/src/kni_utils.cpp

#include "kni_utils.h"

int kni_addr_trans_v4(struct stream_tuple4_v4 *tuple4, char *output, int len){
	char saddr[INET_ADDRSTRLEN];
	char daddr[INET_ADDRSTRLEN];
	inet_ntop(AF_INET, &(tuple4->saddr), saddr, INET_ADDRSTRLEN);
	inet_ntop(AF_INET, &(tuple4->daddr), daddr, INET_ADDRSTRLEN);
	uint16_t source = ntohs(tuple4->source);
	uint16_t dest = ntohs(tuple4->dest);
	snprintf(output, len, "%s:%d -> %s:%d", saddr, source, daddr, dest);
	return 0;
}

int kni_addr_trans_v6(struct stream_tuple4_v6 *tuple4, char *output, int len){
	char saddr[INET6_ADDRSTRLEN];
	char daddr[INET6_ADDRSTRLEN];
	inet_ntop(AF_INET6, tuple4->saddr, saddr, INET6_ADDRSTRLEN);
	inet_ntop(AF_INET6, tuple4->daddr, daddr, INET6_ADDRSTRLEN);
	uint16_t source = ntohs(tuple4->source);
	uint16_t dest = ntohs(tuple4->dest);
	snprintf(output, len, "%s:%d -> %s:%d", saddr, source, daddr, dest);
	return 0;
}

uint16_t kni_ip_checksum(const void *buf, size_t hdr_len){
	unsigned long sum = 0;
	const uint16_t *ip1; 
	ip1 = (const uint16_t *)buf;
	while(hdr_len > 1){
		sum += *ip1++;
		if(sum & 0x80000000){
			sum = (sum & 0xFFFF) + (sum >> 16);
		}
		hdr_len -= 2;
	}
	while(sum >> 16){
		sum = (sum & 0xFFFF) + (sum >> 16); 
	}
	return (~sum);
}

uint16_t kni_tcp_checksum_v6(const void *_buf, size_t len, struct in6_addr src_addr, struct in6_addr dest_addr){
    const uint16_t *buf = (u_int16_t *)_buf;
	uint16_t *ip_src=(uint16_t *)&src_addr, *ip_dst=(uint16_t *)&dest_addr;
	uint32_t sum;
	size_t length=len;
	// Calculate the sum                                            
	sum = 0;
	while(len > 1){
		sum += *buf++;
		if (sum & 0x80000000){
			sum = (sum & 0xFFFF) + (sum >> 16);
		}
		len -= 2;
	}
	if(len & 1){
		// Add the padding if the packet lenght is odd          
		sum += *((uint8_t *)buf); 
	}
	// Add the pseudo-header
	for(int i = 0; i < 8; i++){
		sum += *ip_src;
		ip_src++;
	}            
	for(int i = 0; i < 8; i++){
		sum += *ip_dst;
		ip_dst++;
	}                            
	sum += htons(IPPROTO_TCP);
	sum += htons(length); 
	// Add the carries                                              
	while(sum >> 16){
		sum = (sum & 0xFFFF) + (sum >> 16);
	}
	// Return the one's complement of sum                           
	return ((uint16_t)(~sum));
}

uint16_t kni_tcp_checksum(const void *_buf, size_t len, in_addr_t src_addr, in_addr_t dest_addr){
    const uint16_t *buf = (u_int16_t *)_buf;
	uint16_t *ip_src=(uint16_t *)&src_addr, *ip_dst=(uint16_t *)&dest_addr;
	uint32_t sum;
	size_t length=len;
	// Calculate the sum                                            
	sum = 0;
	while(len > 1){
		sum += *buf++;
		if (sum & 0x80000000){
			sum = (sum & 0xFFFF) + (sum >> 16);
		}
		len -= 2;
	}
	if(len & 1){
		// Add the padding if the packet lenght is odd          
		sum += *((uint8_t *)buf); 
	}
	// Add the pseudo-header                                        
	sum += *(ip_src++);
	sum += *ip_src;
	sum += *(ip_dst++);
	sum += *ip_dst;
	sum += htons(IPPROTO_TCP);
	sum += htons(length); 
	// Add the carries                                              
	while(sum >> 16){
		sum = (sum & 0xFFFF) + (sum >> 16);
	}
	// Return the one's complement of sum                           
	return ((uint16_t)(~sum));
}

uint16_t kni_udp_checksum(const void *_buf, size_t len, in_addr_t src_addr, in_addr_t dest_addr){
    const uint16_t *buf = (u_int16_t *)_buf;
	uint16_t *ip_src=(u_int16_t *)&src_addr, *ip_dst=(u_int16_t *)&dest_addr;
	uint32_t sum;
	size_t length=len;
	// Calculate the sum                                            
	sum = 0;
	while(len > 1){
		sum += *buf++;
		if (sum & 0x80000000){
			sum = (sum & 0xFFFF) + (sum >> 16);
		}
		len -= 2;
	}
	if(len & 1){
		// Add the padding if the packet lenght is odd          
		sum += *((uint8_t *)buf);
	}

	// Add the pseudo-header                                        
	sum += *(ip_src++);
	sum += *ip_src;
	sum += *(ip_dst++);
	sum += *ip_dst;
	sum += htons(IPPROTO_UDP);
	sum += htons(length);

	// Add the carries                                              
	while(sum >> 16){
		sum = (sum & 0xFFFF) + (sum >> 16);
	}
	// Return the one's complement of sum                           
	return ( (uint16_t)(~sum)  );
 }

void kni_get_tcpopt(struct kni_tcpopt_info *tcpopt, struct tcphdr* tcphdr,int tcphdr_len){
	tcpopt->mss = KNI_DEFAULT_MSS;
	tcpopt->wscale = KNI_DEFAULT_WINSCLE;

	const unsigned char *ptr = ((const unsigned char*)tcphdr + 20);
	int length = tcphdr_len - 20;

	while (length > 0){
		int opcode = *ptr++;
		int opsize;
		switch (opcode){
            case TCPOPT_EOL:
                return;
            case TCPOPT_NOP:	/* Ref: RFC 793 section 3.1 */
                length--;
                continue;
            default:
                opsize = *ptr++;
                if (opsize < 2) /* "silly options" */
                    return;
                if (opsize > length)
                    return;	/* don't parse partial options */
                switch (opcode){
                    case TCPOPT_MAXSEG:
                        if (opsize == TCPOLEN_MAXSEG){
                            uint16_t in_mss = *(uint16_t *)ptr;
                            if(in_mss){ 
                                tcpopt->mss = ntohs(in_mss);
                            }
                        }
                        break;
                        
                    case TCPOPT_WINDOW:
                        if (opsize == TCPOLEN_WINDOW){
                            uint8_t snd_wscale = *(uint8_t *)ptr;
                            // rfc7323 page9: Thus, the shift count MUST be limited to 14 (which allows windows of 2^30 = 1 GiB). 
                            // If a Window Scale option is received with a shift.cnt value larger than 14, 
                            // the TCP SHOULD log the error but MUST use 14 instead of the  specified value.  */
                            tcpopt->wscale = snd_wscale;
                            if(tcpopt->wscale > 14){
                                tcpopt->wscale = 14;
                            }
							tcpopt->wscale_set = 1;
                            //*wscale_perm=1;
                        }
                        break;
                    case TCPOPT_TIMESTAMP:
                        if ((opsize == TCPOLEN_TIMESTAMP)){
                            tcpopt->ts_set = 1;
							tcpopt->ts_value = *(uint32_t*)ptr;
                        }
                        break;
                    case TCPOPT_SACK_PERMITTED:
                        if (opsize == TCPOLEN_SACK_PERMITTED){
                            tcpopt->sack = 1;
                        }
                        break;
                }
                ptr += opsize-2;
                length -= opsize;
        }
	}
	return;
}


int kni_ipv4_addr_get_by_eth(const char *ifname, uint32_t *ip){
    struct ifreq ifr;
    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if(sockfd == -1) {
        goto error_out;
    }
    strcpy(ifr.ifr_name, ifname);
    if(ioctl(sockfd, SIOCGIFADDR, &ifr) < 0){
        goto error_out;
    }
    *ip = ((struct sockaddr_in*)&(ifr.ifr_addr))->sin_addr.s_addr;
    close(sockfd);
    return 0;

error_out:
    close(sockfd);
    return -1;
}

static int __wrapper_MESA_htable_set_opt(MESA_htable_handle table, enum MESA_htable_opt opt_type, unsigned value, void *logger, const char *symbol)
{
	int ret = MESA_htable_set_opt(table, opt_type, &value, (int)(sizeof(value)));
	if(unlikely(ret != 0))
    {
		KNI_LOG_ERROR(logger, "Failed at MESA_htable_set_opt, htable is %s, opt_type is %d", symbol, opt_type);
    }
	return ret;
}

static int __wrapper_MESA_htable_set_opt(MESA_htable_handle table, enum MESA_htable_opt opt_type, void * val, size_t len, void *logger, const char *symbol)
{
	int ret = MESA_htable_set_opt(table, opt_type, val, (int)len);
	if(unlikely(ret != 0))
    {
		KNI_LOG_ERROR(logger, "Failed at MESA_htable_set_opt, htable is %s, opt_type is %d", symbol, opt_type);
    }
	return ret;
}

MESA_htable_handle kni_create_htable(const char *profile, const char *section, void *free_data_cb, void *expire_notify_cb, void *logger)
{
    int mho_screen_print_ctrl;
    int mho_thread_safe;
    int mho_mutex_num;
    int mho_hash_slot_size;
	int mho_hash_max_element_num;
    int mho_expire_time;
	char mho_eliminate_type[KNI_SYMBOL_MAX];
	MESA_load_profile_int_def(profile, section, "mho_screen_print_ctrl", &mho_screen_print_ctrl, 1);
    MESA_load_profile_int_def(profile, section, "mho_thread_safe", &mho_thread_safe, 0);
    MESA_load_profile_int_def(profile, section, "mho_mutex_num", &mho_mutex_num, 12);
    MESA_load_profile_int_def(profile, section, "mho_hash_slot_size", &mho_hash_slot_size, 1234);
	MESA_load_profile_int_def(profile, section, "mho_hash_max_element_num", &mho_hash_max_element_num, 12345);
    MESA_load_profile_int_def(profile, section, "mho_expire_time", &mho_expire_time, 3600);
	MESA_load_profile_string_def(profile, section, "mho_eliminate_type", mho_eliminate_type, sizeof(mho_eliminate_type), "FIFO");
	KNI_LOG_ERROR(logger, "MESA_prof_load, [%s]:\n mho_screen_print_ctrl: %d\n mho_thread_safe: %d\n mho_mutex_num: %d\n"
		"mho_hash_slot_size: %d\n mho_hash_max_element_num: %d\n mho_expire_time: %d\n mho_eliminate_type: %s\n", section,
		mho_screen_print_ctrl, mho_thread_safe, mho_mutex_num, mho_hash_slot_size, mho_hash_max_element_num, mho_expire_time, mho_eliminate_type);
	MESA_htable_handle htable = MESA_htable_born();
	if(htable == NULL)
	{
		KNI_LOG_ERROR(logger, "MESA_htable: failed at MESA_htable_born");
		return NULL;
	}
	__wrapper_MESA_htable_set_opt(htable, MHO_SCREEN_PRINT_CTRL, mho_screen_print_ctrl, logger, section);
	__wrapper_MESA_htable_set_opt(htable, MHO_THREAD_SAFE, mho_thread_safe, logger, section);
	__wrapper_MESA_htable_set_opt(htable, MHO_MUTEX_NUM, mho_mutex_num, logger, section);
	__wrapper_MESA_htable_set_opt(htable, MHO_HASH_SLOT_SIZE, mho_hash_slot_size, logger, section);
	__wrapper_MESA_htable_set_opt(htable, MHO_HASH_MAX_ELEMENT_NUM, mho_hash_max_element_num, logger, section);
	__wrapper_MESA_htable_set_opt(htable, MHO_EXPIRE_TIME, mho_expire_time, logger, section);
	if(strncmp(mho_eliminate_type, "LRU", KNI_SYMBOL_MAX) == 0)
	{
		__wrapper_MESA_htable_set_opt(htable, MHO_ELIMIMINATE_TYPE, HASH_ELIMINATE_ALGO_LRU, logger, section);
	}
	else
	{
		__wrapper_MESA_htable_set_opt(htable, MHO_ELIMIMINATE_TYPE, HASH_ELIMINATE_ALGO_FIFO, logger, section);
	}
	if(free_data_cb != NULL){
	 	__wrapper_MESA_htable_set_opt(htable, MHO_CBFUN_DATA_FREE,
	    	(void *)free_data_cb, sizeof(free_data_cb), logger, section);
	}
	if(expire_notify_cb != NULL){
		__wrapper_MESA_htable_set_opt(htable, MHO_CBFUN_DATA_EXPIRE_NOTIFY,
		(void *)expire_notify_cb, sizeof(free_data_cb), logger, section);
	}
	int ret = MESA_htable_mature(htable);
	if(unlikely(ret != 0))
    {
		KNI_LOG_ERROR(logger, "MESA_htable: failed at MESA_htable_mature, htable is %s", section);
		return NULL;
    }
	return htable;
}

char* kni_ipv4_errmsg_get(enum kni_ipv4hdr_parse_error _errno){
	switch(_errno){
		case KNI_IPV4HDR_PARSE_ERROR_NULL_PACKET:
			return (char*)"null packet";
		default:
			return (char*)"unknown error";
	}
}

char* kni_ipv6_errmsg_get(enum kni_ipv6hdr_parse_error _errno){
	switch(_errno){
		case KNI_IPV6HDR_PARSE_ERROR_NULL_PACKET:
			return (char*)"null packet";
		case KNI_IPV6HDR_PARSE_ERROR_NO_TCPHDR:
			return (char*)"no tcp header";
		case KNI_IPV6HDR_PARSE_ERROR_INVALID_TYPE:
			return (char*)"invalid header type";
		default:
			return (char*)"unknown error";
	}
}

int kni_ipv4_header_parse(const void *a_packet, struct pkt_info *pktinfo){
	if(a_packet == NULL){
		return KNI_IPV4HDR_PARSE_ERROR_NULL_PACKET;
	}
	pktinfo->addr_type = ADDR_TYPE_IPV4;
	pktinfo->iphdr.v4 = (struct iphdr*)a_packet;
	pktinfo->iphdr_len = pktinfo->iphdr.v4->ihl * 4;
	pktinfo->ip_totlen = ntohs(pktinfo->iphdr.v4->tot_len);
	pktinfo->tcphdr = (struct tcphdr*)((char*)pktinfo->iphdr.v4 + pktinfo->iphdr_len);
	pktinfo->tcphdr_len = pktinfo->tcphdr->doff * 4;
	pktinfo->data = (char*)pktinfo->tcphdr + pktinfo->tcphdr_len;
	pktinfo->data_len = pktinfo->ip_totlen - pktinfo->iphdr_len - pktinfo->tcphdr_len;
	return 0;
}

int kni_ipv6_header_parse(const void *a_packet, struct pkt_info *pktinfo){
	if(a_packet == NULL){
		return KNI_IPV6HDR_PARSE_ERROR_NULL_PACKET;
	}
	pktinfo->addr_type = ADDR_TYPE_IPV6;
	pktinfo->iphdr.v6 = (struct ip6_hdr*)a_packet;
	pktinfo->ip_totlen = ntohs(pktinfo->iphdr.v6->ip6_ctlun.ip6_un1.ip6_un1_plen) + sizeof(struct ip6_hdr);
	uint8_t next_hdr_type = pktinfo->iphdr.v6->ip6_ctlun.ip6_un1.ip6_un1_nxt;
	char *next_hdr_ptr = (char*)pktinfo->iphdr.v6 + sizeof(struct ip6_hdr);
	int skip_len = 0;
	int ret = 0;
	while(true){
		switch(next_hdr_type)
		{
			case IPPROTO_TCP:
				//parse tcphdr
				pktinfo->iphdr_len = next_hdr_ptr - (char*)a_packet;
				pktinfo->tcphdr = (struct tcphdr*)next_hdr_ptr;
				pktinfo->tcphdr_len = pktinfo->tcphdr->doff * 4;
				pktinfo->data = (char*)pktinfo->tcphdr + pktinfo->tcphdr_len;
				pktinfo->data_len = pktinfo->ip_totlen - pktinfo->iphdr_len - pktinfo->tcphdr_len;
				return 0;
			case IPPROTO_HOPOPTS:
			case IPPROTO_ROUTING:
			case IPPROTO_AH:
			case IPPROTO_DSTOPTS:
				skip_len = (*(next_hdr_ptr + 1)) * 8 + 8;
				next_hdr_type = *next_hdr_ptr;
				next_hdr_ptr += skip_len;
				break;
			case IPPROTO_NONE:
				ret = KNI_IPV6HDR_PARSE_ERROR_NO_TCPHDR;
				goto error_out;
			default:
				ret = KNI_IPV6HDR_PARSE_ERROR_INVALID_TYPE;
				goto error_out;
		}
	}

error_out:
	return ret;
}