path: root/worker/udpstack.c

//////////////////////////////////////////////////////////////////////////////////////
///  UDP Userspace Stack Implementation
///  Author :	Lu Qiuwen <[email protected]>
///  Date	:	2015-12-09
///  Last	:	2015-12-09
///	 Note	:
//////////////////////////////////////////////////////////////////////////////////////

#include <rte_udp.h>
#include <rte_ip.h>
#include <rte_ether.h>
#include <rte_arp.h>
#include <rte_icmp.h>
#include <rte_hash.h>
#include <rte_hash_crc.h>
#include <rte_lpm.h>
#include <rte_lpm6.h>
#include <rte_atomic.h>
#include <rte_memcpy.h>
#include <rte_mempool.h>
#include <rte_malloc.h>
#include <rte_errno.h>
#include <rte_ring.h>
#include <rte_config.h>
#include <rte_spinlock.h>
#include <rte_rwlock.h>
#include <rte_byteorder.h>
#include <rte_cycles.h>
#include <rte_ethdev.h>
#include <rte_ip_frag.h>
#include <string.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <assert.h>
#include <MESA_prof_load.h>

#include "udpstack.h"
#include "main.h"
#include "sw_config.h"
#include "nstat.h"


#ifdef _MSC_VER
#define rte_cpu_to_be_16(x)		htons(x)
#define rte_be_to_cpu_32(x)		ntohl(x)
#define likely(x)				x
#define unlikely(x)				x
#endif

/* 功能选项 */

// 异步ARP待决
#ifndef USE_UDPSTACK_ARP_HOLD
#define USE_UDPSTACK_ARP_HOLD		0
#endif

/* 运行时间统计 */
#ifndef UDPSTACK_STAT_RUNTIME
#define UDPSTACK_STAT_RUNTIME		1
#endif

/* 最大的FD数量 */
#ifndef UDPSTACK_MAX_FD		
#define UDPSTACK_MAX_FD			65536
#endif

#ifndef UDPSTACK_MAX_SYM
#define UDPSTACK_MAX_SYM		256
#endif

#ifndef UDPSTACK_MAX_LINE
#define UDPSTACK_MAX_LINE		1024
#endif

#ifndef UDPSTACK_CONFIG_SECTION_NAME
#define	UDPSTACK_CONFIG_SECTION_NAME		"stack"
#endif

#ifndef UDPSTACK_BULK_MAX
#define UDPSTACK_BULK_MAX		1024
#endif

#ifndef UDPSTACK_FORCE_INLINE
#define UDPSTACK_FORCE_INLINE		1
#endif

/* 使用Malloc()函数分配BUFF空间 */
#ifndef UDPSTACK_ALLOC_USE_MALLOC
#define UDPSTACK_ALLOC_USE_MALLOC		0
#endif

#if UDPSTACK_FORCE_INLINE
#define INLINE inline __attribute__((always_inline))
#else
#define INLINE inline
#endif


INLINE static int rte_pktmbuf_chain(struct rte_mbuf *head, struct rte_mbuf *tail)
{
	struct rte_mbuf *cur_tail;

	/* Check for number-of-segments-overflow */
	if (head->nb_segs + tail->nb_segs >= 1 << (sizeof(head->nb_segs) * 8))
		return -EOVERFLOW;

	/* Chain 'tail' onto the old tail */
	cur_tail = rte_pktmbuf_lastseg(head);
	cur_tail->next = tail;

	/* accumulate number of segments and total length. */
	head->nb_segs = (uint8_t)(head->nb_segs + tail->nb_segs);
	head->pkt_len += tail->pkt_len;

	/* pkt_len is only set in the head */
	tail->pkt_len = tail->data_len;

	return 0;
}

INLINE static int __mbuf_check(struct rte_mbuf * mbuf)
{
	unsigned pkt_len = 0, nb_segs = 0;
	struct rte_mbuf * curser = mbuf;
	while(curser != NULL)
	{
		pkt_len += curser->data_len;
		nb_segs++;
		curser = curser->next;
	}

	if(unlikely(mbuf->nb_segs != nb_segs))
	{
		RTE_LOG(INFO, STACK, "Warning in rte_mbuf %p, mbuf->nb_segs = %u, nb_segs = %u\n",
			mbuf, mbuf->nb_segs, nb_segs);
		rte_pktmbuf_dump(stderr, mbuf, 64);
	}

	if(unlikely(mbuf->pkt_len != pkt_len))
	{
		RTE_LOG(INFO, STACK, "Warning in rte_mbuf %p, mbuf->pkt_len = %u, pkt_len = %u\n",
			mbuf, mbuf->pkt_len, pkt_len);
		rte_pktmbuf_dump(stderr, mbuf, 64);
	}

	return 0;
}

struct udpstack_fd
{
	/* FD Information */
	int fd;
	int family;
	int proto;
	int type;

	/* Source Address Info */
	union
	{
		struct sockaddr_in srcaddr;
		struct sockaddr_in6 srcaddr6;
	};
	/* Dest Address Info */
	union
	{
		struct sockaddr_in dstaddr;
		struct sockaddr_in6 dstaddr6;
	};
};

/* 全局FD序号 */
static volatile uint32_t fd_seq;

#ifndef UDPSTACK_ROUTE_TABLE_MAX
#define UDPSTACK_ROUTE_TABLE_MAX		256
#endif

#ifndef UDPSTACK_ROUTE6_TABLE_MAX
#define UDPSTACK_ROUTE6_TABLE_MAX		256
#endif

#define ROUTE_FLAGS_USER				(1 << 0)
#define ROUTE_FLAGS_GATEWAY				(1 << 1)


// 前置声明
int udpstack_route_send_burst(struct rte_mbuf * mbufs[], uint32_t dstip[],
                              uint32_t srcip[], uint8_t l4_proto[], int nb_mbufs, unsigned flags);

int udpstack_ether_send_burst(struct rte_mbuf * mbuf[], uint32_t target_ip[],
                              uint8_t if_out[], int nb_mbufs, unsigned flags);

int udpstack_ether_send(struct rte_mbuf * mbuf, uint32_t target_ip, uint8_t if_out, unsigned flags);

int udpstack_arp_request_send(uint8_t port_id, uint32_t target_ip);
int udpstack_arp_query_hwaddr(uint32_t target_ip, uint8_t port_id, struct ether_addr * hwaddr);


uint64_t TEMP_packet_drop_count = 0;

// 公用丢包函数
static void __packet_drop(struct rte_mbuf * mbuf, int flags __rte_unused)
{
	TEMP_packet_drop_count++;
	rte_pktmbuf_free(mbuf);
}

/* 路由表 */
struct udpstack_route_table_rule
{
	uint8_t entry_id;
	uint32_t ip;
	uint8_t depth;
	uint32_t srcip;
	uint32_t gateway;
	uint8_t if_out;
	uint8_t flags;
};

struct udpstack_route_table
{
	struct rte_lpm * lpm;
	struct udpstack_route_table_rule rules[UDPSTACK_ROUTE_TABLE_MAX];
	unsigned int nb_rules;
};

struct udpstack_route6_table_rule
{
	// TODO: IPv6路由表定义 
	uint8_t zero;
};

struct udpstack_route6_table
{
	struct rte_lpm6 * lpm6;
	struct udpstack_route6_table_rule rules[UDPSTACK_ROUTE6_TABLE_MAX];
	unsigned int nb_rules;
};

/* LPM最大规则数量 */
#ifndef UDPSTACK_LPM_MAX_RULES
#define UDPSTACK_LPM_MAX_RULES			1024
#endif

/* LPM6最大规则数量 */
#ifndef UDPSTACK_LPM6_MAX_RULES
#define UDPSTACK_LPM6_MAX_RULES			1024
#endif

#ifndef UDPSTACK_LPM6_NUM_TBL8S
#define UDPSTACK_LPM6_NUM_TBL8S		(1 << 16)
#endif

static int route_table_init(struct udpstack_route_table * object, unsigned int socket_id)
{
	char symbol[UDPSTACK_MAX_SYM];
	snprintf(symbol, sizeof(symbol), "UDPSTACK_LPM_%d", socket_id);
	object->lpm = rte_lpm_create(symbol, socket_id, UDPSTACK_LPM_MAX_RULES, 0);
	if (object->lpm == NULL)
	{
		RTE_LOG(ERR, CLIENT, "Unable to create the LPM table on socket %d", socket_id);
		return -ENOBUFS;
	}

	memset(&object->rules, 0, RTE_DIM(object->rules));
	object->nb_rules = 0;
	return 0;
}

static int route_table_add(struct udpstack_route_table * object, uint32_t ip, uint8_t depth,
	uint32_t srcip, uint32_t gateway, uint8_t if_out)
{
	struct udpstack_route_table_rule * rule;
	rule = &object->rules[object->nb_rules];

	int ret = rte_lpm_add(object->lpm, rte_be_to_cpu_32(ip), depth, object->nb_rules);
	if (ret < 0) return ret;

	rule->entry_id = object->nb_rules;
	rule->ip = ip;
	rule->depth = depth;
	rule->gateway = gateway;
	rule->srcip = srcip;
	rule->if_out = if_out;
	object->nb_rules++;
	return 0;
}

// 输出路由表项
static int route_table_dump(struct udpstack_route_table * object)
{
	for (int i = 0; i < object->nb_rules; i++)
	{
		char str_ip[UDPSTACK_MAX_LINE] = { 0 };
		char str_gateway[UDPSTACK_MAX_LINE] = { 0 };

		struct udpstack_route_table_rule * rule = &object->rules[i];
		struct sockaddr_in in_addr_ip, in_addr_gateway;
		in_addr_ip.sin_addr.s_addr = rule->ip;
		in_addr_gateway.sin_addr.s_addr = rule->gateway;

		inet_ntop(AF_INET, &in_addr_ip.sin_addr, str_ip, sizeof(in_addr_ip));
		inet_ntop(AF_INET, &in_addr_gateway.sin_addr, str_gateway, sizeof(in_addr_gateway));
		RTE_LOG(INFO, STACK, "RouteTable Entry %d: Dest=%s/%d, Gateway=%s, Interface=%d\n",
			rule->entry_id, str_ip, rule->depth, str_gateway, rule->if_out);
	}

	return 0;
}

// 路由表查询
static inline struct udpstack_route_table_rule * route_table_query(
struct udpstack_route_table * object, uint32_t ip)
{
	return NULL;
}

struct fasttb_entry_value
{
	void * rt_rule;
	void * arp_rule;
};

#ifndef FASTTB_ENTIRES_MAX
#define FASTTB_ENTIRES_MAX		8
#endif

// 快速查询表项
struct fasttb_object
{
	unsigned int fasttb_curser;
	uint32_t key_entires[FASTTB_ENTIRES_MAX];	
	struct fasttb_entry_value value_entires[FASTTB_ENTIRES_MAX];
};

struct fasttb_entry_value * fasttb_query(struct fasttb_object * object, uint32_t target_ip)
{
	for(int i = 0; i < RTE_DIM(object->key_entires); i++)
	{
		if(object->key_entires[i] == target_ip) return &object->value_entires[i];
	}
	
	return NULL;	
}

void fasttb_set_rtrule(struct fasttb_object * object, uint32_t target_ip, void * rt_rule)
{
	struct fasttb_entry_value * value = fasttb_query(object, target_ip);
	if(unlikely(value == NULL)) goto add;
	value->rt_rule = rt_rule;
	return;
	
add:
	object->fasttb_curser = (object->fasttb_curser + 1) & (FASTTB_ENTIRES_MAX - 1);
	object->key_entires[object->fasttb_curser] = target_ip;
	object->value_entires[object->fasttb_curser].rt_rule = rt_rule;
	object->value_entires[object->fasttb_curser].arp_rule = NULL;
	return;	
}

void fasttb_set_arprule(struct fasttb_object * object, uint32_t target_ip, void * arp_rule)
{
	struct fasttb_entry_value * value = fasttb_query(object, target_ip);
	if (unlikely(value == NULL)) goto add;
	value->arp_rule = arp_rule;
	return;

add:
	object->fasttb_curser = (object->fasttb_curser + 1) & (FASTTB_ENTIRES_MAX - 1);
	object->key_entires[object->fasttb_curser] = target_ip;
	object->value_entires[object->fasttb_curser].rt_rule = NULL;
	object->value_entires[object->fasttb_curser].arp_rule = arp_rule;
	return;
}

// @brief 批量查询接口
// @note: result的长度与ips的长度必须一致
INLINE static int route_table_query_bulk(struct udpstack_route_table * object,
	uint32_t ips[], unsigned int nb_ip, struct udpstack_route_table_rule * result[])
{
	assert(nb_ip < UDPSTACK_BULK_MAX && "nb_ip is lager than UDPSTACK_BULK_MAX");
	struct rte_lpm * lpm = object->lpm;
	uint8_t next_hop[UDPSTACK_BULK_MAX];
	uint32_t cpu_ips[UDPSTACK_BULK_MAX];

	for (int i = 0; i < nb_ip; i++)
	{
		int lookup_ret;
		cpu_ips[i] = rte_be_to_cpu_32(ips[i]);
		lookup_ret = rte_lpm_lookup(lpm, cpu_ips[i], &(next_hop[i]));
		result[i] = lookup_ret == 0 ? &object->rules[next_hop[i]] : NULL;
	}

#if 0
	int ret = rte_lpm_lookup_bulk(lpm, ips, next_hop, nb_ip);
	if (unlikely(ret < 0)) return ret;

	int nb_result = 0;
	for (int i = 0; i < nb_ip; i++)
	{
		if ((next_hop[i] & RTE_LPM_LOOKUP_SUCCESS) == 0)
			result[i] = NULL;
		else
			result[i] = &object->rules[next_hop[i] & 0x00FF];
	}
#endif

	return 0;
}

static int route_table_destroy(struct udpstack_route_table * object)
{
	rte_lpm_free(object->lpm);
	return 0;
}

// ARP表最大数量
#ifndef UDPSTACK_ARP_TABLE_MAX
#define UDPSTACK_ARP_TABLE_MAX		256
#endif

// ARP请求和ARP应答之间，最大缓存的数据包数量
#ifndef UDPSTACK_ARP_HOLD_MAX
#define UDPSTACK_ARP_HOLD_MAX		8
#endif

enum
{
	ARP_FLAGS_DISABLE = 0,
	ARP_FLAGS_ENABLE	= 1
};

/* ARP Table Object Rule */
struct udpstack_arp_table_rule
{
	uint32_t ip;
	uint8_t timeout;
	struct ether_addr dst_eth_addr;
	struct ether_addr src_eth_addr;
	uint8_t port_id;

	/* 待处理的数据包，二层*/
	struct rte_mbuf * hold[UDPSTACK_ARP_HOLD_MAX];
	/* 待处理的数据包数量 */
	unsigned int nb_hold;
	/* 上次发送ARP请求的时间 */
	time_t time;
	/* 标志 */
	uint8_t flags;
};

struct udpstack_arp_table
{
	rte_rwlock_t lock;
	struct rte_hash * hash;
	unsigned int nb_rules;
	unsigned int timeout;
};

static int __arp_free_hold(struct udpstack_arp_table_rule * rule)
{
	if (rule->nb_hold) return 0;
	for (int i = 0; i < RTE_DIM(rule->hold); i++)
	{
		if (rule->hold[i] != NULL) __packet_drop(rule->hold[i], 0);
		rule->hold[i] = NULL;
	}
	
	rule->nb_hold = 0;
	return 0;
}


static int arp_table_init(struct udpstack_arp_table * object, unsigned socket_id)
{
	rte_rwlock_init(&object->lock);
	object->nb_rules = 0;

	char symbol[UDPSTACK_MAX_SYM];
	sprintf(symbol, "UDPSTACK_ARP_TABLE_%d", socket_id);

	struct rte_hash_parameters hash_params =
	{
		.name = symbol,
		.entries = UDPSTACK_ARP_TABLE_MAX,
		.key_len = sizeof(uint32_t),
		.hash_func = rte_hash_crc,
		.hash_func_init_val = 0,
	};

	// 检测上一次存在的HASH表，若存在，直接复位
	struct rte_hash * exist_hash = rte_hash_find_existing(symbol);
	if (exist_hash != NULL)
		rte_hash_reset(exist_hash);
	else
		exist_hash = rte_hash_create(&hash_params);
	
	object->hash = exist_hash;
	if (unlikely(object->hash == NULL)) return -ENOSYS;
	return 0;
}

static int arp_table_destory(struct udpstack_arp_table * object)
{
	rte_rwlock_write_lock(&object->lock);
	rte_hash_free(object->hash);
	rte_rwlock_write_unlock(&object->lock);
	return 0;
}

static int arp_table_add(struct udpstack_arp_table * object,
uint32_t ip, struct ether_addr * src_ethaddr, struct ether_addr * dst_ethaddr, uint8_t if_out, uint8_t flags)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto add;

	// 更新Value
	result->ip = ip;
	result->port_id = if_out;
	result->timeout = 1;
	result->flags = flags;

	ether_addr_copy(src_ethaddr, &result->src_eth_addr);
	ether_addr_copy(dst_ethaddr, &result->dst_eth_addr);
	ret = 0;
	goto exit;

add:
	result = malloc(sizeof(struct udpstack_arp_table_rule));
	if (unlikely(result == NULL)) { ret = -ENOMEM; goto exit; }

	result->ip = ip;
	result->port_id = if_out;
	result->timeout = 1;
	result->flags = flags;
	result->nb_hold = 0;
	result->time = 0;

	ether_addr_copy(src_ethaddr, &result->src_eth_addr);
	ether_addr_copy(dst_ethaddr, &result->dst_eth_addr);

	//TODO: 哈希表满了怎么处理
	ret = rte_hash_add_key_data(object->hash, &ip, result);
	goto exit;

exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}

static int arp_table_delete(struct udpstack_arp_table * object, uint32_t ip)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;


	// 释放缓存的数据包
#if USE_UDPSTACK_ARP_HOLD
	if (result->nb_hold > 0)
	{
		for (int i = 0; i < result->nb_hold; i++)
			rte_pktmbuf_free(result->hold[i]);
	}
#endif

	free(result);
	rte_hash_del_key(object->hash, &ip);
	ret = 0;
	goto exit;

exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}

INLINE static int arp_table_query(struct udpstack_arp_table * object, uint32_t ip,
                                  struct ether_addr * src_eth_addr, struct ether_addr * dst_eth_addr, 
								  uint8_t * if_out, uint8_t* flags, void** arp_rule)
{
	rte_rwlock_read_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;

	assert(result != NULL);
	ether_addr_copy(&result->src_eth_addr, src_eth_addr);
	ether_addr_copy(&result->dst_eth_addr, dst_eth_addr);
	*if_out = result->port_id;
	*flags = result->flags;
	*arp_rule = result;
	
exit:
	rte_rwlock_read_unlock(&object->lock);
	return ret;
}

#define ARP_TABLE_DUMP_FORMAT		"%-20s    %-20s    %-10s    %-10s    %-20s\n"
static int arp_table_dump(FILE * fp, struct udpstack_arp_table * object)
{
	rte_rwlock_read_lock(&object->lock);
	uint32_t * ip = 0, next = 0;
	struct udpstack_arp_table_rule * result = NULL;

	fprintf(fp, ARP_TABLE_DUMP_FORMAT, "Address", "HWaddress", "Iface", "Flags", "Time");
	while (rte_hash_iterate(object->hash, (const void **)&ip, (void **)&result, &next) >= 0)
	{
		char str_address[UDPSTACK_MAX_SYM] = { 0 };
		char str_hwaddress[UDPSTACK_MAX_SYM] = { 0 };
		char str_iface[UDPSTACK_MAX_SYM] = { 0 };
		char str_flags[UDPSTACK_MAX_SYM] = { 0 };
		char str_time[UDPSTACK_MAX_SYM] = { 0 };
		
		const uint8_t * ptr_ip = (const uint8_t *)&(result->ip);
		snprintf(str_address, sizeof(str_address), "%d.%d.%d.%d", ptr_ip[0], ptr_ip[1], ptr_ip[2], ptr_ip[3]);
		snprintf(str_iface, sizeof(str_iface), "Port%d", result->port_id);
		snprintf(str_flags, sizeof(str_flags), "%d", result->flags);
		snprintf(str_time, sizeof(str_time), "%lu", result->time);
		ether_format_addr(str_hwaddress, sizeof(str_hwaddress), &result->dst_eth_addr);
		fprintf(fp, ARP_TABLE_DUMP_FORMAT, str_address, str_hwaddress, str_iface, str_flags, str_time);
	}

	rte_rwlock_read_unlock(&object->lock);
	return 0;
}

static int arp_table_hold_add(struct udpstack_arp_table * object, uint32_t ip,
struct rte_mbuf * mbuf)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;

	int curser = result->nb_hold % RTE_DIM(result->hold);
	if (result->hold[curser] != NULL)
		__packet_drop(result->hold[curser], 0);

	result->hold[curser] = mbuf;
	result->nb_hold++;
	goto exit;
exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}

static int arp_table_hold_get(struct udpstack_arp_table * object, uint32_t ip,
struct rte_mbuf * mbuf[], int max_mbuf)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;

	int nb_mbufs = result->nb_hold > RTE_DIM(result->hold) ?
		RTE_DIM(result->hold) : result->nb_hold;

	// 需要拷贝的数据包大于缓冲区，失败返回
	if (nb_mbufs > max_mbuf) { ret = -ENOBUFS; goto exit; }

	// 复制输出
	for (int i = 0; i < nb_mbufs; i++)
	{
		mbuf[i] = result->hold[i];
		result->hold[i] == NULL;
	}

	result->nb_hold = 0;
	ret = nb_mbufs;
	goto exit;

exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}


static int arp_table_hold_set(struct udpstack_arp_table * object, uint32_t ip,
struct rte_mbuf * mbufs, int nb_mbufs)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;

	// 已经有Hold的数据包，返回错误。
	if (unlikely(result->nb_hold != 0)) { ret = -EINVAL; goto exit; }

	//TODO: 实现
exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}

static int arp_table_time_set(struct udpstack_arp_table * object, uint32_t ip,
	time_t time)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;
	result->time = time;

exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}

static int arp_table_time_get(struct udpstack_arp_table * object, uint32_t ip, time_t * time)
{
	rte_rwlock_write_lock(&object->lock);
	struct udpstack_arp_table_rule * result = NULL;
	int ret = rte_hash_lookup_data(object->hash, &ip, (void **)&result);
	if (ret < 0) goto exit;
	*time = result->time;

exit:
	rte_rwlock_write_unlock(&object->lock);
	return ret;
}

/* 快速表查询 */
INLINE static int fasttb_arp_query(struct fasttb_object * object, uint32_t target_ip,
struct ether_addr * src_eth_addr, struct ether_addr * dst_eth_addr, uint8_t * if_out, uint8_t* flags)
{
	struct fasttb_entry_value * value = fasttb_query(object, target_ip);
	if (value == NULL) return -ENOENT;

	struct udpstack_arp_table_rule * arp_rule = (struct udpstack_arp_table_rule *)value->arp_rule;
	ether_addr_copy(&arp_rule->src_eth_addr, src_eth_addr);
	ether_addr_copy(&arp_rule->dst_eth_addr, dst_eth_addr);
	*if_out = arp_rule->port_id;
	*flags = arp_rule->flags;
	return 0;
}


/* 统计模块 */
struct udpstack_stat_record
{
	uint64_t udps_opackets;
	uint64_t udps_obytes;
	uint64_t udps_odrops;

	uint64_t ips_opackets;
	uint64_t ips_obytes;
	uint64_t ips_odrops;

	uint64_t ether_opackets;
	uint64_t ether_obytes;
	uint64_t ether_odrops;

	uint64_t arp_request_send;
	uint64_t arp_request_recv;
	uint64_t arp_reply_send;
	uint64_t arp_reply_recv;

	uint64_t runtime;
	uint64_t cycles;

} __rte_cache_aligned;


struct udpstack_lcore_handle
{
	/* UDP Packets Mempool */
	struct rte_mempool * mbuf_pool;
	/* UDP Indirect Mempool */
	struct rte_mempool * indirect_mbuf_pool;
	/* Thread Stats */
	struct udpstack_stat_record * stat;
	/* FastTB Object */
	struct fasttb_object * fasttb;
};

struct udpstack_socket_handle
{
	/* ARP Lookup Map */
	struct udpstack_arp_table * table_arp;
	/* IPv4 Route Map */
	struct udpstack_route_table * table_route;
	/* IPv6 Route Map */
	struct udpstack_route6_table * table_route6;
};

struct udpstack_port_index
{
	struct udpstack_fd * udp_port[UINT16_MAX];
};

struct udpstack_global_handle
{
	/* Marsio FD Structure */
	struct udpstack_fd * fds[UDPSTACK_MAX_FD];
	/* Source UDP Port */
	struct udpstack_port_index port_index;
	/* Global Handle Lock */
	rte_spinlock_t spinlock;
	/* IP Counter */
	rte_atomic16_t ip_counter;
	/* Global Time */
	time_t time;
};

static struct udpstack_global_handle __global_handle;
struct udpstack_lcore_handle * udpstack_lcore_handle_pool[RTE_MAX_LCORE] = { 0 };
struct udpstack_socket_handle * udpstack_socket_handle_pool[RTE_MAX_NUMA_NODES] = { 0 };

#ifdef __GNUC__
#define container_of(ptr, type, member) ({                      \
		const typeof(((type *)0)->member) *__mptr = (ptr);		\
		(type *)((char *)__mptr - offsetof(type,member));})
#else
#define container_of(ptr, type,member) \
		((type *)((char *)(ptr) - offsetof(type, member)))
#endif

#define UDPSTACK_MAGIC_CHECK		0

struct internal_udp_buff
{
#if UDPSTACK_MAGIC_CHECK
	char magic_0[2];
#endif
	struct rte_mbuf * mbuf;
#if UDPSTACK_MAGIC_CHECK
	char magic_1[2];
#endif
	struct marsio_udp_buff buff;
};

/* 获取本线程UDPSTACK对象句柄 */
INLINE static struct udpstack_lcore_handle * __get_thread_handle()
{
	return udpstack_lcore_handle_pool[rte_lcore_id()];
}

/* 获取本Socket UDPSTACK对象句柄 */
INLINE static struct udpstack_socket_handle * __get_socket_handle()
{
	return udpstack_socket_handle_pool[rte_socket_id()];
}

/* 获取全局UDPSTACK对象句柄*/
INLINE static struct udpstack_global_handle * __get_global_handle()
{
	return &__global_handle;
}

/* 申请本线程MBUF池中的MBUF结构 */
INLINE static struct rte_mempool * __get_lcore_mbufpool(unsigned lcore_id)
{
	return app.lcore_params[lcore_id].pool;
}

/* 申请本线程Indirect Mbuf池 */
INLINE static struct rte_mempool * __get_lcore_indirect_mbufpool(unsigned lcore_id)
{
	return app.lcore_params[lcore_id].indirect_pool;
}

/* BUFF结构互相转换函数 */
INLINE static struct marsio_udp_buff * __buff_from_internal_buff(struct internal_udp_buff * in_buff)
{
	return &(in_buff->buff);
}

INLINE static struct internal_udp_buff * __internal_buff_from_buff(struct marsio_udp_buff * buff)
{
	return container_of(buff, struct internal_udp_buff, buff);
}

INLINE static struct rte_mbuf * __mbuf_from_buff(struct marsio_udp_buff * buff)
{
	return __internal_buff_from_buff(buff)->mbuf;
}

/* 获取全局时钟 */
INLINE static time_t __get_time()
{
	return __get_global_handle()->time;
}

/* 释放BUFF链表 */
INLINE static void udpbuff_free_chain(struct marsio_udp_buff * buff_head)
{
#if UDPSTACK_ALLOC_USE_MALLOC
	struct internal_udp_buff * in_buff_head = __internal_buff_from_buff(buff_head);
	assert(buff_head != NULL);
	assert(in_buff_head != NULL);

	struct internal_udp_buff * in_buff_now;
	for (in_buff_now = in_buff_head; in_buff_now != NULL;
	in_buff_now = __internal_buff_from_buff(in_buff_now->buff.next))
	{
		free(in_buff_now);
	}
#endif
	return;
}

#if UDPSTACK_MAGIC_CHECK
void __magic_check(struct internal_udp_buff * in_buff)
{
	assert(in_buff->magic_0[0] == 0x4d);
	assert(in_buff->magic_0[1] == 0x5a);
	assert(in_buff->magic_1[0] == 0x4d);
	assert(in_buff->magic_1[1] == 0x5a);
	return;
}
#endif

INLINE struct marsio_udp_buff * marsio_udp_alloc()
{
	struct rte_mbuf * mbuf_buffer = rte_pktmbuf_alloc(__get_thread_handle()->mbuf_pool);
	if (unlikely(mbuf_buffer == NULL))
	{
		RTE_LOG(INFO, STACK, "%s(): cannot alloc mbuf from lcore %d\n", __FUNCTION__, rte_lcore_id());
		return NULL;
	}

#if UDPSTACK_ALLOC_USE_MALLOC
	struct internal_udp_buff * in_buff = (struct internal_udp_buff *)malloc(sizeof(struct internal_udp_buff));
	if (unlikely(in_buff == NULL)) return NULL;
#else
	assert(mbuf_buffer->data_off >= sizeof(struct internal_udp_buff));
	
	// 利用MBUF预留的私有空间
	struct internal_udp_buff * in_buff = rte_pktmbuf_mtod_offset(mbuf_buffer, 
										 struct internal_udp_buff *, 
										 -sizeof(struct internal_udp_buff));
#endif

#if UDPSTACK_MAGIC_CHECK
	in_buff->magic_0[0] = 0x4d;
	in_buff->magic_0[1] = 0x5a;
	in_buff->magic_1[0] = 0x4d;
	in_buff->magic_1[1] = 0x5a;
#endif

	in_buff->mbuf = mbuf_buffer;
	in_buff->buff.start = rte_pktmbuf_mtod(mbuf_buffer, void *);
	in_buff->buff.len = mbuf_buffer->buf_len - mbuf_buffer->data_off;
	in_buff->buff.next = NULL;
	in_buff->buff.next_pkt = NULL;
	
#if UDPSTACK_MAGIC_CHECK
	__magic_check(in_buff);
#endif

	return __buff_from_internal_buff(in_buff);
}

INLINE void marsio_udp_free(struct marsio_udp_buff * buff)
{
	assert(buff != NULL);
	assert(buff->start != NULL);

	struct rte_mbuf * mbuf = __mbuf_from_buff(buff);
	rte_pktmbuf_free(mbuf);
	udpbuff_free_chain(buff);
	return;
}

INLINE static struct udpstack_stat_record * udpstack_stat_local_handle()
{
	struct udpstack_lcore_handle * local_handle = __get_thread_handle();
	return local_handle->stat;
}

#define STAT_DUMP_FORMAT " %-20s : Packets %-10"PRIu64  "Bytes %-10"PRIu64  "Drops %-10"PRIu64"\n"

int udpstack_stat_dump(FILE * fp, struct udpstack_stat_record * handle)
{
	fprintf(fp, STAT_DUMP_FORMAT, "UDP Layer OUTPUT", handle->udps_opackets, handle->udps_obytes, handle->udps_odrops);
	fprintf(fp, STAT_DUMP_FORMAT, "IP Layer OUTPUT", handle->ips_opackets, handle->ips_obytes, handle->ips_odrops);
	fprintf(fp, STAT_DUMP_FORMAT, "ETHER Layer OUTPUT", handle->ether_opackets, handle->ether_obytes, handle->ether_odrops);
	fprintf(fp, " %-20s : %-20"PRIu64"\n", "ARP Request Send", handle->arp_request_send);
	fprintf(fp, " %-20s : %-20"PRIu64"\n", "ARP Request Recv", handle->arp_request_recv);
	fprintf(fp, " %-20s : %-20"PRIu64"\n", "ARP Reply Send", handle->arp_reply_send);
	fprintf(fp, " %-20s : %-20"PRIu64"\n", "ARP Reply Recv", handle->arp_reply_recv);

#if UDPSTACK_STAT_RUNTIME
	float runtime_per_cycle;
	runtime_per_cycle = handle->cycles == 0 ? 0 : handle->runtime / (float)handle->cycles;
	fprintf(fp, " %-20s : %-20.2f\n", "Runtime", runtime_per_cycle);
#endif
	
	return 0;
}

#define UDPSTACK_UDP_SPORT_RMAX			60000
#define UDPSTACK_UDP_SPORT_RMIN			4096

// 端口随机选择
INLINE static uint16_t select_random_sport(struct udpstack_fd * port_index[], uint16_t port_min, uint16_t port_max)
{
	uint16_t base_port, finally_port;
	base_port = rte_rand() % (port_max - port_min) + port_min;

	for (finally_port = base_port; finally_port < port_max; finally_port++)
	{
		if (port_index[finally_port] == NULL) return finally_port;
	}

	for (finally_port = port_min; finally_port < base_port; finally_port++)
	{
		if (port_index[finally_port] == NULL) return finally_port;
	}

	return 0;
}

static int marsio_bind_unsafe(int marsio_fd, const struct sockaddr * addr, socklen_t addrlen)
{
	struct udpstack_global_handle * handle = __get_global_handle();
	int ret = 0;

	struct udpstack_fd * fd_hand = handle->fds[marsio_fd];
	if (fd_hand == NULL) { ret = -EINVAL; goto exit; }

	// IPv4 UDP Socket
	if (addr->sa_family == AF_INET && fd_hand->type == SOCK_DGRAM && fd_hand->family == AF_INET)
	{
		struct sockaddr_in * in_addr = (struct sockaddr_in *)addr;
		// 检查是否已经绑定
		if (fd_hand->srcaddr.sin_addr.s_addr != 0 || fd_hand->srcaddr.sin_port != 0) goto exit;
		// 检查端口是否已经被使用
		if (handle->port_index.udp_port[in_addr->sin_port] != NULL)
		{
			ret = -EINVAL; goto exit;
		}
		fd_hand->srcaddr.sin_addr = in_addr->sin_addr;
		fd_hand->srcaddr.sin_port = in_addr->sin_port;

		// 随机绑定端口
		int retry_times = 0;
		while (fd_hand->srcaddr.sin_port == 0 && retry_times < 3)
		{
			fd_hand->srcaddr.sin_port = select_random_sport(handle->port_index.udp_port,
				UDPSTACK_UDP_SPORT_RMIN, UDPSTACK_UDP_SPORT_RMAX);
			retry_times++;
		}

		// 申请失败
		if (unlikely(fd_hand->srcaddr.sin_port == 0))
		{
			ret = -ENOBUFS; goto exit;
		}

		// 注册端口
		handle->port_index.udp_port[fd_hand->srcaddr.sin_port] = fd_hand;
		ret = 0;
		goto exit;
	}

exit:
	return ret;
}

static int marsio_unbind_unsafe(int marsio_fd)
{
	struct udpstack_global_handle * global_handle = __get_global_handle();
	struct udpstack_fd * fd_hand = global_handle->fds[marsio_fd];
	int ret = 0;

	if (unlikely(fd_hand == NULL)) { ret = -EINVAL; goto exit; }

	// IPv4 UDP 关端口
	if (fd_hand->family == AF_INET && fd_hand->type == SOCK_DGRAM)
	{
		// 检查是否已经绑定
		if (fd_hand->srcaddr.sin_addr.s_addr == 0 && fd_hand->srcaddr.sin_port)
		{
			ret = -EINVAL; goto exit;
		}

		uint16_t source_port = fd_hand->srcaddr.sin_port;
		global_handle->port_index.udp_port[source_port] = NULL;
		fd_hand->srcaddr.sin_addr.s_addr = 0;
		fd_hand->srcaddr.sin_port = 0;
		ret = 0; goto exit;
	}

	// IPv6 UDP 关端口
	if (fd_hand->family == AF_INET6 && fd_hand->type == SOCK_DGRAM)
	{
		RTE_LOG(WARNING, CLIENT, "%s() in file %s : Not support IPv6 UDP Socket\n", __FUNCTION__, __FILE__);
		ret = -EINVAL; goto exit;
	}

exit:
	return ret;
}

// Marsio Bind (Thread-safe Wrapper)
int marsio_bind(int marsio_fd, const struct sockaddr * addr, socklen_t addrlen)
{
	struct udpstack_global_handle * handle = __get_global_handle();

	rte_spinlock_lock(&handle->spinlock);
	int ret = marsio_bind_unsafe(marsio_fd, addr, addrlen);
	rte_spinlock_unlock(&handle->spinlock);
	return ret;
}

// Marsio Unbind (Thread-safe Wrapper)
int marsio_unbind(int marsio_fd)
{
	struct udpstack_global_handle * global_handle = __get_global_handle();

	rte_spinlock_lock(&global_handle->spinlock);
	int ret = marsio_unbind_unsafe(marsio_fd);
	rte_spinlock_unlock(&global_handle->spinlock);
	return ret;
}

int marsio_socket(int family, int type, int protocol)
{
	// 现在的实现只支持UDP
	if (unlikely(family != AF_INET && family != AF_INET6))
		return -EINVAL;
	if (unlikely(type != SOCK_DGRAM))
		return -EINVAL;

	struct udpstack_global_handle * global_handle = __get_global_handle();
	struct udpstack_fd * fd_hand = NULL;
	int fd = 0, ret = 0;

	// 查找全局空间FD，加锁
	rte_spinlock_lock(&global_handle->spinlock);
	for (fd = 0; fd < UDPSTACK_MAX_FD; fd++)
	{
		if (global_handle->fds[fd] != NULL) continue;
		goto found_fd;
	}

	ret = -ENFILE;
	goto exit;

	// 找到了FD
found_fd:
	fd_hand = (struct udpstack_fd *)calloc(1, sizeof(struct udpstack_fd));
	if (unlikely(fd_hand == NULL)) { ret = -ENOMEM; goto exit; }

	fd_hand->family = family;
	fd_hand->type = type;
	fd_hand->proto = protocol;
	fd_hand->fd = fd;
	global_handle->fds[fd] = fd_hand;

	// 分配随机的端口号
	struct sockaddr_in in_addr;
	memset(&in_addr, 0, sizeof(in_addr));
	in_addr.sin_family = AF_INET;

	// 绑定端口，已经处于spinlock的保护下，不用调用线程安全的函数。
	ret = marsio_bind_unsafe(fd, (struct sockaddr *)&in_addr, sizeof(in_addr));
	if (unlikely(ret < 0)) goto exit;
	
	// 分配成功
	ret = fd;
	goto exit;

exit:
	rte_spinlock_unlock(&global_handle->spinlock);
	return ret;
}

int marsio_close(int fd)
{
	if (unlikely(fd >= UDPSTACK_MAX_FD))
		return -EINVAL;

	int ret = 0;
	struct udpstack_global_handle * global_handle = __get_global_handle();
	rte_spinlock_lock(&global_handle->spinlock);

	struct udpstack_fd * fd_hand = global_handle->fds[fd];
	if (unlikely(fd_hand == NULL)) { ret = -EINVAL; goto exit; }

	// 释放Socket结构
	free(fd_hand);
	global_handle->fds[fd] = NULL;
	ret = 0;
	goto exit;

exit:
	rte_spinlock_unlock(&global_handle->spinlock);
	return ret;
}

/* IP接口支持 */

/* 单接口最大IP地址数量 */
#ifndef UDPSTACK_IFACE_MAX_ADDR
#define UDPSTACK_IFACE_MAX_ADDR			16
#endif


struct udpstack_iface_ipaddr
{
	uint32_t ipaddr;
	uint32_t mask;
};

struct udpstack_iface_ip6addr
{
	uint8_t ip6addr[16];
	uint8_t mask[16];
};

struct udpstack_iface
{
	/* 启用标志 */
	unsigned int enable;
	/* 设备Port号 */
	unsigned int port_id;
	/* MTU */
	unsigned int mtu;
	/* 设备MAC地址 */
	struct ether_addr dev_macaddr;
	/* 用户定义MAC地址 */
	struct ether_addr user_macaddr;
	/* IPv4地址 */
	struct udpstack_iface_ipaddr ipaddr[UDPSTACK_IFACE_MAX_ADDR];
	/* IPv6地址 */
	struct udpstack_iface_ip6addr ip6addr[UDPSTACK_IFACE_MAX_ADDR];
	/* IPv4地址数量*/
	unsigned int nb_ipaddr;
	/* IPv6地址数量*/
	unsigned int nb_ip6addr;

};

static struct udpstack_iface iface_info[RTE_MAX_ETHPORTS];

// 取网卡描述符
INLINE static struct udpstack_iface * __get_iface_desc(unsigned int port_id)
{
	return &iface_info[port_id];
}

// 取网卡与传入IP同网段的IP地址和掩码
INLINE static struct udpstack_iface_ipaddr * __get_iface_localaddr(struct udpstack_iface * iface,
	uint32_t target_ip)
{
	struct udpstack_iface_ipaddr * iface_addr;
	for (int i = 0; i < iface->nb_ipaddr; i++)
	{
		iface_addr = &iface->ipaddr[i];
		if ((iface_addr->ipaddr & iface_addr->mask) ==
			(target_ip & iface_addr->mask))
			return iface_addr;
	}

	return NULL;
}

/* 向后链接RTE_MBUF结构 */
INLINE static int __link_rte_mbuf(struct rte_mbuf * mbuf_perv, struct rte_mbuf * mbuf_next)
{
	if (mbuf_perv == NULL) return 0;
	return rte_pktmbuf_chain(mbuf_perv, mbuf_next);
}

/* 链接BUFF结构 */
INLINE static void __link_udp_buff(struct marsio_udp_buff * buff_perv, struct marsio_udp_buff * buff_next)
{
	if (buff_perv == NULL || buff_next == NULL) return;
	struct rte_mbuf * mbuf_perv = __mbuf_from_buff(buff_perv);
	struct rte_mbuf * mbuf_next = __mbuf_from_buff(buff_next);
	buff_perv->next = buff_next;
	mbuf_perv->next = mbuf_next;
}

/* 根据UDP Buff结构构建串接的MBUF结构*/
INLINE static struct rte_mbuf * update_rte_mbuf(struct marsio_udp_buff * buff_head)
{
	struct marsio_udp_buff * now_buff_head = buff_head;
	struct rte_mbuf * mbuf_chain_head = __mbuf_from_buff(buff_head);
	
	// 第一个包初始化
	mbuf_chain_head->pkt_len = buff_head->len;
	mbuf_chain_head->data_len = buff_head->len;
	mbuf_chain_head->nb_segs = 1;

	// 根据BUFF结构构建MBUF链表
	while (now_buff_head != NULL)
	{

#if UDPSTACK_MAGIC_CHECK
		struct internal_udp_buff * __check_in_buff = __internal_buff_from_buff(now_buff_head);
		__magic_check(__check_in_buff);
#endif

		struct rte_mbuf * mbuf_now = __mbuf_from_buff(now_buff_head);
		if (mbuf_now != mbuf_chain_head) rte_pktmbuf_chain(mbuf_chain_head, mbuf_now);
		now_buff_head = now_buff_head->next;
	}

	return mbuf_chain_head;
}

// 申请UDP+IP+以太网头缓冲区
INLINE static struct rte_mbuf * alloc_header_mbuf_v4()
{
	struct rte_mbuf * header = rte_pktmbuf_alloc(__get_thread_handle()->mbuf_pool);
	if (unlikely(header == NULL))
	{
		RTE_LOG(INFO, STACK, "%s(): cannot alloc mbuf from lcore %d\n", __FUNCTION__, rte_lcore_id());
		return NULL;
	}

	// 预留UDP+IP+以太网头
	uint16_t reserve_len = sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr);
	header->data_off += reserve_len;
	return header;
}

ssize_t marsio_udp_sendto_chain(int marsio_fd, struct marsio_udp_buff * chain_head, int nb_buff,
	int flags, struct sockaddr * to[], socklen_t addrlen[], int nb_sockaddrs)
{
	int nb_chain_pkts = 0;
	struct marsio_udp_buff * buff_array[UDPSTACK_BULK_MAX];

	if (unlikely(nb_buff > RTE_DIM(buff_array))) return -ENOBUFS;
	for (struct marsio_udp_buff * __head = chain_head; __head != NULL; __head = __head->next_pkt)
	{
		buff_array[nb_chain_pkts++] = __head;
	}

	assert(nb_chain_pkts == nb_buff && nb_chain_pkts <= RTE_DIM(buff_array));
	return marsio_udp_sendto_burst(marsio_fd, buff_array, nb_buff, flags, to, addrlen, nb_sockaddrs);
}


// 构建UDP报头
INLINE static struct rte_mbuf * build_udp_header(int fd, struct sockaddr * to,
struct rte_mbuf * header, struct rte_mbuf * udp_data)
{
	struct udpstack_fd * fd_hand = __get_global_handle()->fds[fd];

	// IPv4和IPv6的sockaddr前两个成员变量一致，采用同一变量取得目的端口
	uint16_t dst_port = ((struct sockaddr_in *)to)->sin_port;
	uint16_t src_port = fd_hand->srcaddr.sin_port;

	if (unlikely(src_port == 0 || dst_port == 0))
		goto failure;
	
	if (unlikely(rte_pktmbuf_prepend(header, sizeof(struct udp_hdr)) == NULL))
		goto failure;

	// 构建头部
	struct udp_hdr * udp_hdr = rte_pktmbuf_mtod(header, struct udp_hdr *);
	udp_hdr->src_port = rte_cpu_to_be_16(src_port);
	udp_hdr->dst_port = rte_cpu_to_be_16(dst_port);
	udp_hdr->dgram_len = rte_cpu_to_be_16(sizeof(struct udp_hdr) + rte_pktmbuf_pkt_len(udp_data));
	udp_hdr->dgram_cksum = 0;

	//硬件标志位
	header->l4_len = sizeof(struct udp_hdr);
	header->ol_flags |= PKT_TX_UDP_CKSUM;
	return header;

failure:
	return NULL;
}

INLINE static ssize_t udpstack_sendto_bulk(int marsio_fd, struct marsio_udp_buff * buff, int flags,
struct sockaddr * to[], socklen_t addrlen[], int nb_sockaddrs)
{

#if UDPSTACK_STAT_RUNTIME
	uint64_t _tsc_start = rte_rdtsc();
#endif

	struct udpstack_lcore_handle * thread_handle = __get_thread_handle();
	int ret = 0, fail_cur = 0;
	
	// 链转换，由BUFF链转换成MBUF链
	struct rte_mbuf * data_mbuf = update_rte_mbuf(buff);
	struct rte_mbuf * clone_mbuf[UDPSTACK_BULK_MAX];
	struct rte_mbuf * header_mbuf[UDPSTACK_BULK_MAX];

	// 统计
	thread_handle->stat->udps_opackets += data_mbuf->nb_segs;
	thread_handle->stat->udps_obytes += data_mbuf->pkt_len;

	uint32_t dst_ip[UDPSTACK_BULK_MAX];
	uint32_t src_ip[UDPSTACK_BULK_MAX];
	uint8_t l4_proto[UDPSTACK_BULK_MAX];
	
	// 四层头构建
	for (int i = 0; i < nb_sockaddrs; i++)
	{
		clone_mbuf[i] = rte_pktmbuf_clone(data_mbuf, thread_handle->indirect_mbuf_pool);
		header_mbuf[i] = alloc_header_mbuf_v4();

		if (unlikely(clone_mbuf[i] == NULL)) { ret = -ENOBUFS; fail_cur = i; goto failure; }
		if (unlikely(header_mbuf[i] == NULL)) { ret = -ENOBUFS; fail_cur = i; goto failure; }

		struct sockaddr_in * in_addr = (struct sockaddr_in *)to[i];
		build_udp_header(marsio_fd, to[i], header_mbuf[i], clone_mbuf[i]);
		rte_pktmbuf_chain(header_mbuf[i], clone_mbuf[i]);

		dst_ip[i] = in_addr->sin_addr.s_addr;
		src_ip[i] = 0;
		l4_proto[i] = IPPROTO_UDP;
	}

	// 释放原始报文缓冲区
	rte_pktmbuf_free(data_mbuf);
	ret = udpstack_route_send_burst(header_mbuf, dst_ip, src_ip, l4_proto, nb_sockaddrs, 0);

#if UDPSTACK_STAT_RUNTIME
	uint64_t _tsc_end = rte_rdtsc();
	thread_handle->stat->runtime += _tsc_end - _tsc_start;
	thread_handle->stat->cycles++;
#endif

	if (unlikely(ret < 0)) { fail_cur = nb_sockaddrs; goto failure; }
	return 0;

failure:
	// Release all the rte_mbuf applied by myself.
	for (int i = 0; i < fail_cur; i++)
		rte_pktmbuf_free(header_mbuf[i]);
	return ret;
}


ssize_t marsio_udp_sendto_burst(int marsio_fd, struct marsio_udp_buff * buff[], int nb_buff, int flags,
struct sockaddr * to[], socklen_t addrlen[], int nb_sockaddrs)
{
	for (int i = 0; i < nb_buff; i++)
		udpstack_sendto_bulk(marsio_fd, buff[i], flags, to, addrlen, nb_sockaddrs);

	return 0;
}

ssize_t marsio_udp_sendto(int marsio_fd, struct marsio_udp_buff * buff, int flags,
struct sockaddr * to, socklen_t addrlen)
{
	return 0;
}


// 软件过滤到达不是本端口的包
#ifndef UDPSTACK_CHECK_LOCALFRAME
#define UDPSTACK_CHECK_LOCALFRAME		1
#endif


static int udpstack_filter_localframe(struct rte_mbuf * mbufs[], int nb_mbufs)
{
	for (int i = 0; i < nb_mbufs; i++)
	{
		struct rte_mbuf * mbuf = mbufs[i];
		struct udpstack_iface * iface = &iface_info[mbuf->port];

		struct ether_hdr * ether_hdr = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
		struct ether_addr * macaddr_line = &ether_hdr->d_addr;
		struct ether_addr * macaddr_user = &iface->dev_macaddr;

		if (is_broadcast_ether_addr(macaddr_line))
			continue;
		if (is_multicast_ether_addr(macaddr_line))
			continue;
		if (is_same_ether_addr(macaddr_line, macaddr_user))
			continue;

		rte_pktmbuf_free(mbuf);
		mbufs[i] == NULL;
	}

	return 0;
}

// 子网掩码转CIDR
INLINE static int __mask_to_depth(uint32_t mask)
{
	int count = 0;
	while (mask)
	{
		++count;
		mask = (mask - 1)&mask;
	}
	return count;
}

int udpstack_send_packet_fast(struct rte_mbuf * mbuf, uint8_t port_id)
{
	unsigned lcore_id = rte_lcore_id();
	struct app_lcore_params_worker * lp = &app.lcore_params[lcore_id].worker;

	int ret = rte_ring_enqueue(lp->fast_rings_out[port_id], mbuf);
	if (unlikely(ret < 0))
	{
		// TODO: 发包统计
#if 0
		nstat_client_count_wk_drop(lp->mbuf_out[port_id].array, bsz_wr, lcore_id);
#endif
		rte_pktmbuf_free(mbuf);
	}
	return 0;
}

// 二层发包接口
INLINE static int udpstack_send_packet(struct rte_mbuf * mbuf, uint8_t port_id)
{

	unsigned lcore_id = rte_lcore_id();
	struct app_lcore_params_worker * lp = &app.lcore_params[lcore_id].worker;
	uint32_t bsz_wr = app.burst_size_worker_write;
	uint64_t pos;

	mbuf->port = port_id;
	pos = lp->mbuf_out[port_id].n_mbufs;
	lp->mbuf_out[port_id].array[pos++] = mbuf;

	if (likely(pos < bsz_wr)) {
		lp->mbuf_out[port_id].n_mbufs = pos;
		return 0;
	}

	int ret = rte_ring_sp_enqueue_bulk(lp->rings_out[port_id], (void **)lp->mbuf_out[port_id].array, bsz_wr);

	if (unlikely(ret == -ENOBUFS)) {
		// TODO: 发包统计
#if 1
		nstat_client_count_wk_drop(lp->mbuf_out[port_id].array, bsz_wr, lcore_id);
#endif
		for (int k = 0; k < bsz_wr; k++) {
			struct rte_mbuf *pkt_to_free = lp->mbuf_out[port_id].array[k];
			rte_pktmbuf_free(pkt_to_free);
		}
	}

	lp->mbuf_out[port_id].n_mbufs = 0;
	lp->mbuf_out_flush[port_id] = 0;
	return 0;
}

/**
* 批量二层发包接口
* @param [in]	mbuf		数据包数组
* @param [in]	nb_mbuf		数据包数组长度
* @param [in]	port_id		发送网络接口
* @return		int			成功发送数据包的数量
**/
int udpstack_send_packet_burst(struct rte_mbuf* mbuf[], unsigned nb_mbuf, uint8_t port_id)
{
	for (int i = 0; i < nb_mbuf; i++)
		udpstack_send_packet(mbuf[i], port_id);
	return 0;
}

#define __IP_VERSION_IHL(version, len)	(version << 4 | len << 0)
#define UDPSTACK_IP_TTL	75
#include <rte_hexdump.h>

enum
{
	/* 带外数据，通过快速路径发包 */
	UDPSTACK_SEND_FLAG_OOB = 1 << 0,
	/* 发出数据包禁止分片 */
	UDPSTACK_SEND_NO_FRAG = 1 << 1
};

// 三层发包接口
int udpstack_route_send_burst(struct rte_mbuf * mbufs[], uint32_t dstip[],
                              uint32_t srcip[], uint8_t l4_proto[], int nb_mbufs, unsigned flags)
{
	if (unlikely(nb_mbufs > UDPSTACK_BULK_MAX)) return -ENOBUFS;
	uint16_t datalen[UDPSTACK_BULK_MAX];
	
	for (int i = 0; i < nb_mbufs; i++)
	{
		//TODO: 数据包前面的空间不够如何处理
		struct rte_mbuf * header = mbufs[i];
		rte_pktmbuf_prepend(header, sizeof(struct ipv4_hdr));
		datalen[i] = rte_pktmbuf_pkt_len(mbufs[i]);
	}

	for (int i = 0; i < nb_mbufs; i++)
	{
		struct rte_mbuf * header = mbufs[i];
		struct ipv4_hdr * ip_hdr = rte_pktmbuf_mtod(header, struct ipv4_hdr *);
		uint16_t ip_counter = rte_atomic16_add_return(&__get_global_handle()->ip_counter, 1);

		ip_hdr->version_ihl = __IP_VERSION_IHL(4, sizeof(struct ipv4_hdr) / 4);
		ip_hdr->type_of_service = 0;
		ip_hdr->total_length = rte_cpu_to_be_16(datalen[i]);
		ip_hdr->packet_id = rte_cpu_to_be_16(ip_counter);
		ip_hdr->fragment_offset = 0;
		ip_hdr->time_to_live = UDPSTACK_IP_TTL;
		ip_hdr->next_proto_id = l4_proto[i];
		ip_hdr->hdr_checksum = 0;

		ip_hdr->dst_addr = dstip[i];
		ip_hdr->src_addr = srcip[i];
	}

	struct udpstack_socket_handle * socket_handle = __get_socket_handle();
	struct udpstack_route_table_rule * route_result[UDPSTACK_BULK_MAX];
	route_table_query_bulk(socket_handle->table_route, dstip, nb_mbufs, route_result);

	struct rte_mbuf * ready_to_send[UDPSTACK_BULK_MAX];
	uint32_t ready_to_send_ip[UDPSTACK_BULK_MAX];
	unsigned int nb_ready_to_send = 0;
	uint8_t ready_to_send_if[UDPSTACK_BULK_MAX];

	for (int i = 0; i < nb_mbufs; i++)
	{
		struct udpstack_route_table_rule * route_rule = route_result[i];
		if (unlikely(route_rule == NULL)) continue; //TODO: 丢包
		struct rte_mbuf * header = mbufs[i];
		struct ipv4_hdr * ipv4_hdr = rte_pktmbuf_mtod(header, struct ipv4_hdr *);

		// 无源IP地址时，填路由出口IP地址
		if (ipv4_hdr->src_addr == 0)
			ipv4_hdr->src_addr = route_rule->srcip;

		ready_to_send[nb_ready_to_send] = header;
		ready_to_send_ip[nb_ready_to_send] = ipv4_hdr->dst_addr;
		ready_to_send_if[nb_ready_to_send] = route_rule->if_out;

		// 硬件计算校验和标志
		header->l3_len = sizeof(struct ipv4_hdr);
		header->ol_flags |= PKT_TX_IPV4 | PKT_TX_IP_CKSUM;

		// 网关路由
		if (route_rule->flags & ROUTE_FLAGS_GATEWAY)
			ready_to_send_ip[nb_ready_to_send] = route_rule->gateway;

		nb_ready_to_send++;
	}

	// 送链路层发包
	udpstack_ether_send_burst(ready_to_send, ready_to_send_ip,
	                          ready_to_send_if, nb_ready_to_send, flags);

	return 0;
}

int udpstack_route_rulegen(unsigned int socket_id)
{
	struct udpstack_route_table * rtable_obj = udpstack_socket_handle_pool[socket_id]->table_route;

	for (unsigned port_id = 0; port_id < RTE_DIM(iface_info); port_id++)
	{
		struct udpstack_iface * iface_handle = &(iface_info[port_id]);
		if (iface_handle->enable == 0) continue;

		for (unsigned i = 0; i < iface_handle->nb_ipaddr; i++)
		{
			uint32_t ip = iface_handle->ipaddr[i].ipaddr;
			uint32_t mask = iface_handle->ipaddr[i].mask;
			uint8_t depth = __mask_to_depth(mask);

			int ret = route_table_add(rtable_obj, ip, depth, ip, 0, port_id);
			if (ret < 0) return ret;
		}
	}

	// 调试使用，输出路由表：
	route_table_dump(rtable_obj);
	return 0;
}

// 路由系统初始化
int udpstack_route_init()
{
	for (unsigned socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
	{
		if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
		struct udpstack_socket_handle * socket_handle = udpstack_socket_handle_pool[socket_id];

		char symbol[UDPSTACK_MAX_SYM];
		sprintf(symbol, "UDPSTACK_TABLE_ROUTE_%d", socket_id);
		socket_handle->table_route = rte_zmalloc_socket(symbol, sizeof(struct udpstack_route_table), 0, socket_id);
		if (socket_handle->table_route == NULL) return -ENOMEM;

		// 初始化路由表
		route_table_init(socket_handle->table_route, socket_id);
		// 向路由表增加路由
		udpstack_route_rulegen(socket_id);
	}

	return 0;
}

int udpstack_ether_send_burst(struct rte_mbuf * mbuf[], uint32_t target_ip[],
                              uint8_t if_out[], int nb_mbufs, unsigned flags)
{
	int burst_ret[UDPSTACK_BULK_MAX];
	for (int i = 0; i < nb_mbufs; i++)
		burst_ret[i] = udpstack_ether_send(mbuf[i], target_ip[i], if_out[i], flags);

	return 0;
}


#ifndef UDPSTACK_FRAG_MAX
#define UDPSTACK_FRAG_MAX		128
#endif

#ifndef UDPSTACK_USE_FRAG
#define UDPSTACK_USE_FRAG		0
#endif

static int udpstack_ether_send_frag(struct rte_mbuf * mbuf, uint32_t target_ip, uint8_t if_out, unsigned flags)
{
	struct udpstack_socket_handle * socket_handle = __get_socket_handle();
	struct udpstack_iface * iface = __get_iface_desc(if_out);
	if (unlikely(iface->enable == 0))  return -EINVAL;

	struct rte_mbuf * frag_mbufs[UDPSTACK_FRAG_MAX] = { 0 };
	uint16_t mtu = iface->mtu;

	if (mbuf->pkt_len <= mtu)
	{
		RTE_LOG(INFO, STACK, "%s(): No frag frame called frag function", __FUNCTION__);
		return 0;
	}

	int nb_frag_mbufs = rte_ipv4_fragment_packet(mbuf, frag_mbufs, UDPSTACK_FRAG_MAX, mtu,
		__get_thread_handle()->mbuf_pool, __get_thread_handle()->indirect_mbuf_pool);
	
	if (nb_frag_mbufs < 0) return nb_frag_mbufs;
	assert(nb_frag_mbufs <= UDPSTACK_FRAG_MAX);

	for (int i = 0; i < nb_frag_mbufs; i++)
		udpstack_ether_send(frag_mbufs[i], target_ip, if_out, flags | UDPSTACK_SEND_NO_FRAG);

	return 0;
}

struct ether_addr zero_addr = { 0 };
int udpstack_ether_send(struct rte_mbuf * mbuf, uint32_t target_ip, uint8_t if_out, unsigned flags)
{
	int ret = 0;
	struct udpstack_socket_handle * socket_handle = __get_socket_handle();
	struct udpstack_iface * iface = __get_iface_desc(if_out);
	if (unlikely(iface->enable == 0))  return -EINVAL;

#if UDPSTACK_USE_FRAG
	__mbuf_check(mbuf);
	if(unlikely((mbuf->pkt_len > iface->mtu) && ((flags & UDPSTACK_SEND_NO_FRAG) == 0)))
	{
		udpstack_ether_send_frag(mbuf, target_ip, if_out, flags);
		rte_pktmbuf_free(mbuf);
		return 0;
	}
#endif

	rte_pktmbuf_prepend(mbuf, sizeof(struct ether_hdr));
	//TODO: 前面的空间不够，再申请一块空间

	// 查ARP表
	struct ether_addr src_hwaddr;
	struct ether_addr dst_hwaddr;
	
	// 填发包网卡的MAC地址
	ether_addr_copy(&iface->user_macaddr, &dst_hwaddr);
	ret = udpstack_arp_query_hwaddr(target_ip, if_out, &dst_hwaddr);

	// 没有记录，释放包
	if (ret < 0) goto drop;	

	// 构造以太网头
	struct ether_hdr * ether_hdr = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
	ether_addr_copy(&src_hwaddr, &ether_hdr->s_addr);
	ether_addr_copy(&dst_hwaddr, &ether_hdr->d_addr);
	ether_hdr->ether_type = rte_cpu_to_be_16(ETHER_TYPE_IPv4);

	// 硬件校验和标志
	mbuf->l2_len = sizeof(struct ether_hdr);

	// 带外数据
	if (unlikely(flags & UDPSTACK_SEND_FLAG_OOB))
	{
		udpstack_send_packet_fast(mbuf, iface->port_id);
		return 0;
	}
		
	udpstack_send_packet(mbuf, iface->port_id);
	return 0;

drop:
	__packet_drop(mbuf, 0);
	return ret;
}

// ARP子系统初始化
int udpstack_arp_init()
{
	for (unsigned socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
	{
		if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
		struct udpstack_socket_handle * socket_handle = udpstack_socket_handle_pool[socket_id];

		char symbol[UDPSTACK_MAX_SYM];
		sprintf(symbol, "UDPSTACK_TABLE_ARP_%d", socket_id);
		socket_handle->table_arp = rte_zmalloc_socket(symbol, sizeof(struct udpstack_arp_table), 0, socket_id);
		if (socket_handle->table_arp == NULL)
		{
			RTE_LOG(INFO, STACK, "Cannot alloc memory for %s\n", symbol);
			return -ENOMEM;
		}

		// 初始化ARP表
		int ret = arp_table_init(socket_handle->table_arp, socket_id);
		if(ret < 0)
		{
			RTE_LOG(INFO, STACK, "Cannot init arp table for socket %d\n", socket_id);
			return ret;
		}
	}

	return 0;
}

/**
 * 通过ARP查询IP对应的MAC地址
 * @param[in]	ip			  	待查IP地址
 * @param[out]	hwaddr			查询结果
 * @return		0				查询成功
 * 				-1				查询失败
**/
int udpstack_arp_query_hwaddr(uint32_t target_ip, uint8_t port_id, struct ether_addr * hwaddr)
{
	struct udpstack_socket_handle * socket_handle = __get_socket_handle();
	struct udpstack_lcore_handle * lcore_handle = __get_thread_handle();
	
	struct ether_addr src_hwaddr;
	struct ether_addr dst_hwaddr;
	time_t timenow = 0, timelast = 0;
		
	uint8_t arp_if_out;
	uint8_t arp_flags;

	int ret = 0;
	struct udpstack_arp_table_rule * arp_rule_cache;

	// 查线程缓冲快表
	ret = fasttb_arp_query(lcore_handle->fasttb, target_ip, &src_hwaddr,
		&dst_hwaddr, &arp_if_out, &arp_flags);
	
	if (unlikely(ret < 0)) goto arp_query;
	if (unlikely(arp_flags == ARP_FLAGS_DISABLE)) goto send_request;
	ether_addr_copy(&dst_hwaddr, hwaddr);
	return 0;

	// 查找失败，从全局ARP表查询
arp_query:
	ret = arp_table_query(socket_handle->table_arp, target_ip, &src_hwaddr,
			&dst_hwaddr, &arp_if_out, &arp_flags, (void **)&arp_rule_cache);
	
	if (unlikely(ret < 0)) goto rule_add;
	if (unlikely(arp_flags == ARP_FLAGS_DISABLE)) goto send_request;
	ether_addr_copy(&dst_hwaddr, hwaddr);
	return 0;
	
rule_add:
	// 增加ARP空表项
	for (unsigned int socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
	{
		if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
		struct udpstack_socket_handle * handle = udpstack_socket_handle_pool[socket_id];
		arp_table_add(handle->table_arp, target_ip, &zero_addr, &zero_addr, 0, ARP_FLAGS_DISABLE);
	}

send_request:
	// 判断是否可以发ARP，避免ARP泛洪
	timenow = __get_time();
	arp_table_time_get(socket_handle->table_arp, target_ip, &timelast);
	if(timenow != timelast)
	{
		arp_table_time_set(socket_handle->table_arp, target_ip, timenow);
		udpstack_arp_request_send(port_id, target_ip);
	}
	
	// 查询失败
	return -1;
}


/** 发ARP请求报文
 *  @param[in]	port_id		出口网络接口编号
 *  @param[in]	target_ip	目标IP地址
 */
int udpstack_arp_request_send(uint8_t port_id, uint32_t target_ip)
{
	struct udpstack_iface * iface = __get_iface_desc(port_id);
	if (iface->enable == 0) return -EINVAL;

	struct ether_addr * src_hwaddr = &iface->user_macaddr;
	struct ether_addr dst_hwaddr = { 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF };

	struct udpstack_iface_ipaddr * ipaddr = __get_iface_localaddr(iface, target_ip);
	if (unlikely(ipaddr == NULL)) return -EINVAL;

	struct rte_mbuf * req_mbuf = rte_pktmbuf_alloc(__get_thread_handle()->mbuf_pool);
	if (unlikely(req_mbuf == NULL)) return -ENOBUFS;

	// 构造以太网头
	struct ether_hdr * ether_hdr = (struct ether_hdr *)rte_pktmbuf_append(req_mbuf, sizeof(struct ether_hdr));
	ether_addr_copy(src_hwaddr, &ether_hdr->s_addr);
	ether_addr_copy(&dst_hwaddr, &ether_hdr->d_addr);
	ether_hdr->ether_type = rte_cpu_to_be_16(ETHER_TYPE_ARP);

	// 构造ARP请求报文
	struct arp_hdr * arp_hdr = (struct arp_hdr *)rte_pktmbuf_append(req_mbuf, sizeof(struct arp_hdr));
	arp_hdr->arp_hrd = rte_cpu_to_be_16(ARP_HRD_ETHER);
	arp_hdr->arp_pro = rte_cpu_to_be_16(ETHER_TYPE_IPv4);
	arp_hdr->arp_hln = 6;
	arp_hdr->arp_pln = 4;
	arp_hdr->arp_op = rte_cpu_to_be_16(ARP_OP_REQUEST);
	arp_hdr->arp_data.arp_sip = ipaddr->ipaddr;
	arp_hdr->arp_data.arp_tip = target_ip;
	ether_addr_copy(src_hwaddr, &arp_hdr->arp_data.arp_sha);
	memset(&arp_hdr->arp_data.arp_tha, 0, sizeof(arp_hdr->arp_data.arp_tha));

	// 统计
	__get_thread_handle()->stat->arp_request_send++;

	// 发包
	udpstack_send_packet_fast(req_mbuf, port_id);
	return 0;
}

int udpstack_arp_req_entry(struct arp_hdr * arp_header, uint8_t port_id)
{
	struct udpstack_iface * iface = __get_iface_desc(port_id);
	if (iface->enable == 0) return 0;

	// 过滤非本网段的ARP请求报文
	if (__get_iface_localaddr(iface, arp_header->arp_data.arp_sip) == NULL)
		return 0;

	// 统计：ARP请求到达
	__get_thread_handle()->stat->arp_request_recv++;
	
	// 根据到达信息，更新ARP表
	for (unsigned int socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
	{
		if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
		struct udpstack_socket_handle * socket_handle = udpstack_socket_handle_pool[socket_id];

		uint32_t ip = arp_header->arp_data.arp_sip;
		struct ether_addr * dst_ethaddr_p = &arp_header->arp_data.arp_sha;
		struct ether_addr * src_ethaddr_p = &iface->user_macaddr;
		arp_table_add(socket_handle->table_arp, ip, src_ethaddr_p, dst_ethaddr_p, port_id, ARP_FLAGS_ENABLE);
	}

	uint32_t reply_ip = 0;
	uint32_t target_ip = 0;

	for (int i = 0; i < iface->nb_ipaddr; i++)
	{
		if (iface->ipaddr[i].ipaddr != arp_header->arp_data.arp_tip) continue;
		reply_ip = iface->ipaddr[i].ipaddr; break;
	}

	// 不是本机的ARP请求，忽略
	if (reply_ip == 0) return RET_CHAIN_DROP;
	target_ip = arp_header->arp_data.arp_sip;


	struct rte_mbuf * reply_mbuf = rte_pktmbuf_alloc(__get_thread_handle()->mbuf_pool);
	if (unlikely(reply_mbuf == NULL)) return -ENOBUFS;

	// 构造以太网头
	struct ether_hdr * ether_hdr = (struct ether_hdr *)rte_pktmbuf_append(reply_mbuf, sizeof(struct ether_hdr));
	ether_addr_copy(&iface->user_macaddr, &ether_hdr->s_addr);
	ether_addr_copy(&arp_header->arp_data.arp_sha, &ether_hdr->d_addr);
	ether_hdr->ether_type = rte_cpu_to_be_16(ETHER_TYPE_ARP);

	// 构造ARP应答
	struct arp_hdr * reply_arp_hdr = (struct arp_hdr *)rte_pktmbuf_append(reply_mbuf, sizeof(struct arp_hdr));
	rte_memcpy(reply_arp_hdr, arp_header, sizeof(struct arp_hdr));
	reply_arp_hdr->arp_op = rte_cpu_to_be_16(ARP_OP_REPLY);
	ether_addr_copy(&ether_hdr->s_addr, &reply_arp_hdr->arp_data.arp_sha);
	ether_addr_copy(&ether_hdr->d_addr, &reply_arp_hdr->arp_data.arp_tha);
	reply_arp_hdr->arp_data.arp_sip = reply_ip;
	reply_arp_hdr->arp_data.arp_tip = target_ip;

	// 统计：ARP应答
	__get_thread_handle()->stat->arp_reply_send++;

	// 发包
	udpstack_send_packet_fast(reply_mbuf, port_id);
	return 0;
}

/**
* 将ARP请求和应答间缓冲下的数据包填写完整，送入线路发送。.
* @param [in]	arp_rule	ARP条目对象.
* @return		An int.
**/
int udpstack_arp_flush_hold(struct udpstack_iface * iface, uint32_t target_ip, struct ether_addr * target_mac)
{
	struct rte_mbuf * out_mbuf_buffer[UDPSTACK_BULK_MAX];
	struct rte_mbuf ** out_curser = out_mbuf_buffer;
	unsigned int nb_out_mbuf_buffer = 0;
	unsigned int left_out_mbuf_buffer = UDPSTACK_BULK_MAX;

	// 取已经Hold的数据包
	for (unsigned int socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
	{
		if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
		struct udpstack_socket_handle * socket_handle = udpstack_socket_handle_pool[socket_id];

		int nb_hold_mbufs;
		nb_hold_mbufs = arp_table_hold_get(socket_handle->table_arp, target_ip, out_curser, left_out_mbuf_buffer);

		//TODO: 错误处理，不严谨，需要进一步细化
		assert(nb_hold_mbufs >= 0);
		nb_out_mbuf_buffer += nb_hold_mbufs;
		out_curser += nb_hold_mbufs;
		left_out_mbuf_buffer -= nb_hold_mbufs;
		assert(left_out_mbuf_buffer >= 0);
	}

	// 构造数据包
	for (unsigned int i = 0; i < nb_out_mbuf_buffer; i++)
	{
		struct ether_hdr * ether_hdr = rte_pktmbuf_mtod(out_mbuf_buffer[i], struct ether_hdr *);
		ether_addr_copy(&iface->user_macaddr, &ether_hdr->s_addr);
		ether_addr_copy(target_mac, &ether_hdr->d_addr);
	}

	udpstack_send_packet_burst(out_mbuf_buffer, nb_out_mbuf_buffer, iface->port_id);
	return 0;
}

static inline struct udpstack_iface_ipaddr * __search_iface_localip(struct udpstack_iface * iface,
	uint32_t ip)
{
	for (int i = 0; i < iface->nb_ipaddr; i++)
	{
		if (iface->ipaddr[i].ipaddr == ip) return &iface->ipaddr[i];
	}
	return NULL;
}

// ARP应答入站解析
// @param[in]	arp_header	入站ARP报文头
// @param[in]	port_id		入站ARP报文网络接口编号
// @return		0			执行成功
int udpstack_arp_reply_entry(struct arp_hdr * arp_header, uint8_t port_id)
{
	struct udpstack_iface * iface = __get_iface_desc(port_id);
	if (iface->enable == 0) return -EINVAL;

	struct udpstack_iface_ipaddr * ipaddr = __search_iface_localip(iface, arp_header->arp_data.arp_tip);
	if (unlikely(ipaddr == NULL)) return 0;

	uint32_t * ip = &arp_header->arp_data.arp_sip;
	struct ether_addr * hwaddr = &arp_header->arp_data.arp_sha;

	for (unsigned int socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
	{
		if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
		struct udpstack_socket_handle * socket_handle = udpstack_socket_handle_pool[socket_id];
		arp_table_add(socket_handle->table_arp, *ip, &iface->user_macaddr, hwaddr, iface->port_id, ARP_FLAGS_ENABLE);
	}

	// 统计：ARP回复到达
	__get_thread_handle()->stat->arp_reply_recv++;

#if 0
	udpstack_arp_flush_hold(iface, *ip, hwaddr);
#endif
	return 0;
}

// ARP入站
int udpstack_packet_arp_entry(struct rte_mbuf * mbufs[], int nb_mbufs)
{
#if UDPSTACK_CHECK_LOCALFRAME
	udpstack_filter_localframe(mbufs, nb_mbufs);
#endif

	for (int i = 0; i < nb_mbufs; i++)
	{
		struct rte_mbuf * mbuf = mbufs[i];
		if (mbuf == NULL) continue;

		struct arp_hdr * arp_hdr = rte_pktmbuf_mtod_offset(mbuf, struct arp_hdr *,
			sizeof(struct ether_hdr));

		if (arp_hdr->arp_op == rte_cpu_to_be_16(ARP_OP_REQUEST))
			udpstack_arp_req_entry(arp_hdr, mbuf->port);
		if (arp_hdr->arp_op == rte_cpu_to_be_16(ARP_OP_REPLY))
			udpstack_arp_reply_entry(arp_hdr, mbuf->port);
	}

	return RET_CHAIN_DROP;
}

int udpstack_icmp_echo_request(struct rte_mbuf * mbuf, uint32_t srcip, uint32_t dstip)
{
	struct icmp_hdr * icmp_hdr = rte_pktmbuf_mtod(mbuf, struct icmp_hdr *);

	uint32_t cksum;
	uint8_t l4_proto = IPPROTO_ICMP;
	
	icmp_hdr->icmp_type = IP_ICMP_ECHO_REPLY;
	cksum = ~icmp_hdr->icmp_cksum & 0xffff;
	cksum += ~htons(IP_ICMP_ECHO_REQUEST << 8) & 0xffff;
	cksum += htons(IP_ICMP_ECHO_REPLY << 8);
	cksum = (cksum & 0xffff) + (cksum >> 16);
	cksum = (cksum & 0xffff) + (cksum >> 16);
	icmp_hdr->icmp_cksum = cksum;

	// 发应答，交换源IP和目的IP，带外数据传输
	udpstack_route_send_burst(&mbuf, &srcip, &dstip, &l4_proto, 1, UDPSTACK_SEND_FLAG_OOB);
	return 0;
}

int udpstack_icmp_entry(struct rte_mbuf * mbuf, uint32_t srcip, uint32_t dstip)
{
	struct icmp_hdr * icmp_hdr = rte_pktmbuf_mtod(mbuf, struct icmp_hdr *);
	
	// ICMP回显请求
	if (icmp_hdr->icmp_type == IP_ICMP_ECHO_REQUEST)
	{
		udpstack_icmp_echo_request(mbuf, srcip, dstip);
		goto out;
	}

	// TODO: ICMP回显应答
	if(icmp_hdr->icmp_type == IP_ICMP_ECHO_REPLY)
	{
		goto out;
	}

out:
	return 0;
}


// ICMP入站
int udpstack_packet_icmp_entry(struct rte_mbuf * mbufs[], int nb_mbufs)
{
#if UDPSTACK_CHECK_LOCALFRAME
	udpstack_filter_localframe(mbufs, nb_mbufs);
#endif

	for (int i = 0; i < nb_mbufs; i++)
	{
		struct rte_mbuf * mbuf = mbufs[i];
		if (mbuf == NULL) continue;

		struct ipv4_hdr * ip_hdr = rte_pktmbuf_mtod_offset(mbuf, struct ipv4_hdr *,
			sizeof(struct ether_hdr));
		
		if (unlikely(ip_hdr->next_proto_id != IPPROTO_ICMP))
			continue;
		
		uint32_t srcip = ip_hdr->src_addr;
		uint32_t dstip = ip_hdr->dst_addr;

		rte_pktmbuf_adj(mbuf, sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr));
		udpstack_icmp_entry(mbuf, srcip, dstip);
	}

	return RET_CHAIN_DROP;
}



#include <sys/select.h>
#include <sys/time.h>

// 临时脚手架
static void * udpstack_thread_stat(void * arg)
{
	while (1)
	{
		struct timeval timeval_local;
		timeval_local.tv_sec = 1;
		timeval_local.tv_usec = 0;
		int ret = select(0, NULL, NULL, NULL, &timeval_local);

		FILE * fp = fopen("marsio.log", "w");
		if (fp == NULL) continue;

#if 0
		for (unsigned int lcore_id = 0; lcore_id < RTE_MAX_LCORE; lcore_id++)
		{
			if (rte_lcore_is_enabled(lcore_id) == 0) continue;
			printf("LCore %d Mempool Free Count: %d \n", lcore_id,
				rte_mempool_free_count(__get_lcore_mbufpool(lcore_id)));
		}
#endif
		fprintf(fp, "[A] Arp Table Info \n");
		for (unsigned int socket_id = 0; socket_id < RTE_MAX_NUMA_NODES; socket_id++)
		{
			if (udpstack_socket_handle_pool[socket_id] == NULL) continue;
			fprintf(fp, " Socket %d\n", socket_id);
			struct udpstack_socket_handle * socket_handle = udpstack_socket_handle_pool[socket_id];
			arp_table_dump(fp, socket_handle->table_arp);
		}

		fprintf(fp, "[B] Packet Info\n");
		for (unsigned int lcore_id = 0; lcore_id < RTE_MAX_LCORE; lcore_id++)
		{
			if (udpstack_lcore_handle_pool[lcore_id] == NULL) continue;
			struct udpstack_lcore_handle * handle = udpstack_lcore_handle_pool[lcore_id];
			fprintf(fp, " lcore %d\n", lcore_id);
			udpstack_stat_dump(fp, handle->stat);
		}

		fclose(fp);
	}

	return NULL;
}


#include <sys/select.h>
#include <sys/time.h>

static void udpstack_sleep(int second)
{
	struct timeval timeval_local;
	timeval_local.tv_sec = second;
	timeval_local.tv_usec = 0;
	select(0, NULL, NULL, NULL, &timeval_local);
	return;
}

void * udpstack_thread_time(void * arg __rte_unused) 
{
	struct udpstack_global_handle * handle = __get_global_handle();
	while(1)
	{
		handle->time = time(NULL);
		udpstack_sleep(1);
	}

	return (void *)NULL;
}


// 模块初始化
int udpstack_init()
{
	// Socket句柄初始化
	for (unsigned int lcore_id = 0; lcore_id < RTE_MAX_LCORE; lcore_id++)
	{
		if (rte_lcore_is_enabled(lcore_id) == 0) continue;
		unsigned int socket_id = rte_lcore_to_socket_id(lcore_id);
		if (udpstack_socket_handle_pool[socket_id] != NULL) continue;
		udpstack_socket_handle_pool[socket_id] = rte_zmalloc_socket("UDPSTACK_SOCKET_HANDLE",
			sizeof(struct udpstack_socket_handle), 0, socket_id);
	}

	// LCORE句柄初始化
	for (unsigned int lcore_id = 0; lcore_id < RTE_MAX_LCORE; lcore_id++)
	{
		if (rte_lcore_is_enabled(lcore_id) == 0) continue;
		udpstack_lcore_handle_pool[lcore_id] = rte_zmalloc_socket("UDPSTACK_LCORE_HANDLE",
			sizeof(struct udpstack_lcore_handle), 0, rte_lcore_to_socket_id(lcore_id));

		udpstack_lcore_handle_pool[lcore_id]->mbuf_pool = __get_lcore_mbufpool(lcore_id);
		udpstack_lcore_handle_pool[lcore_id]->indirect_mbuf_pool = __get_lcore_indirect_mbufpool(lcore_id);
		udpstack_lcore_handle_pool[lcore_id]->stat = rte_zmalloc_socket(NULL, sizeof(struct udpstack_stat_record),
			0, rte_lcore_to_socket_id(lcore_id));
		udpstack_lcore_handle_pool[lcore_id]->fasttb = rte_zmalloc_socket(NULL, sizeof(struct fasttb_object),
			0, rte_lcore_to_socket_id(lcore_id));

		assert(udpstack_lcore_handle_pool[lcore_id]->mbuf_pool != NULL);
		assert(udpstack_lcore_handle_pool[lcore_id]->indirect_mbuf_pool != NULL);
		assert(udpstack_lcore_handle_pool[lcore_id]->stat != NULL);
		assert(udpstack_lcore_handle_pool[lcore_id]->fasttb != NULL);
	}

	udpstack_route_init();
	udpstack_arp_init();

	// 临时 
	pthread_t temp_thread;
	pthread_create(&temp_thread, NULL, udpstack_thread_stat, NULL);
	pthread_detach(temp_thread);

	// 时钟更新
	pthread_t time_thread;
	pthread_create(&time_thread, NULL, udpstack_thread_time, NULL);
	pthread_detach(time_thread);


	return 0;
}

struct udpstack_arp_default_rule
{
	uint32_t ip;
};

// 默认ARP规则最大条目数量
#ifndef UDPSTACK_ARP_DEFAULT_RULE_MAX
#define UDPSTACK_ARP_DEFAULT_RULE_MAX	1024
#endif

#ifndef UDPSTACK_ARP_SECTION_NAME
#define UDPSTACK_ARP_SECTION_NAME		"stack-arp"
#endif


struct udpstack_arp_config
{
	/* 允许ARP表动态更新 */
	int en_dynamic_arp;
	/* 载入预配置ARP表 */
	int en_default_arp;
	/* 默认ARP表规则 */
	struct udpstack_arp_table_rule default_rule[UDPSTACK_ARP_DEFAULT_RULE_MAX];
	/* 默认ARP表规则数量 */
	int nb_default_rule;
};

static struct udpstack_arp_config arp_config_info;

// 载入默认的ARP规则
// TODO: 还没有完成，默认规则的部分字段无法填充。
static int udpstack_config_arp_defualt_rule(const char * cfgfile)
{
	struct udpstack_arp_config * cfghandle = &arp_config_info;
	
	for (int i = 0; i < UDPSTACK_ARP_DEFAULT_RULE_MAX; i++)
	{
		char str_key[UDPSTACK_MAX_SYM];
		char str_value[UDPSTACK_MAX_LINE];
		snprintf(str_key, sizeof(str_key), "rule%d", i);
		MESA_load_profile_string_def(cfgfile, UDPSTACK_ARP_SECTION_NAME, str_key, str_value,
			sizeof(str_value), "");

		if (strncmp(str_value, "", sizeof(str_value)) == 0)
			continue;

		char str_ip[UDPSTACK_MAX_SYM];
		char str_hwaddr[UDPSTACK_MAX_SYM];
		int ret = sscanf(str_value, "%[^,],%[^,]", str_ip, str_hwaddr);
		if (ret != 2) continue;
		
		struct sockaddr_in in_addr;
		ret = inet_pton(AF_INET, str_hwaddr, &in_addr.sin_addr);
		if (ret != 1) continue;

		struct ether_addr hwaddr;
		ret = sscanf(str_hwaddr, "%u:%u:%u:%u:%u:%u", &hwaddr.addr_bytes[0], &hwaddr.addr_bytes[1],
			&hwaddr.addr_bytes[2], &hwaddr.addr_bytes[3], &hwaddr.addr_bytes[4], &hwaddr.addr_bytes[5]);
		
		if (ret != 6) continue;
		
		cfghandle->default_rule[cfghandle->nb_default_rule].ip = in_addr.sin_addr.s_addr;
		ether_addr_copy(&hwaddr, &cfghandle->default_rule[cfghandle->nb_default_rule].dst_eth_addr);
	}

	return 0;
}

// ARP配置选项
static int udpstack_config_arp(const char * cfgfile)
{
	MESA_load_profile_int_def(cfgfile, UDPSTACK_ARP_SECTION_NAME, "en_dynamic_arp",
		&arp_config_info.en_dynamic_arp, 1);
	MESA_load_profile_int_def(cfgfile, UDPSTACK_ARP_SECTION_NAME, "en_default_arp",
		&arp_config_info.en_default_arp, 1);
	
	// 读默认ARP配置表
	if (arp_config_info.en_default_arp)
		return 0;

	return 0;
}

static int udpstack_config_mtu(unsigned int port_id, const char * cfgfile)
{
	char key[UDPSTACK_MAX_SYM] = { 0 };
	int value = 0;

	snprintf(key, sizeof(key), "Port%d.MTU", port_id);
	MESA_load_profile_int_def(cfgfile, UDPSTACK_CONFIG_SECTION_NAME, key, &value, 0);
	iface_info[port_id].mtu = value;
	return 0;
}

static int udpstack_config_ipv4(unsigned int port_id, const char * cfgfile)
{
	char key[UDPSTACK_MAX_SYM] = { 0 };
	char value[UDPSTACK_MAX_LINE] = { 0 };
	snprintf(key, sizeof(key), "Port%d.IPv4", port_id);
	MESA_load_profile_string_def(cfgfile, UDPSTACK_CONFIG_SECTION_NAME, key, value, sizeof(value), "");

	// 判断端口是否存在
	if (strncmp(value, "", UDPSTACK_MAX_LINE) == 0)
		return 0;

	// 分割读取IP地址
	for (char * str_ip = strtok(value, ";,"); str_ip != NULL;
	str_ip = strtok(NULL, ";,"))
	{
		char str_ip_local[UDPSTACK_MAX_LINE];
		char str_mask_local[UDPSTACK_MAX_LINE];
		int ret = 0;

		ret = sscanf(str_ip, "%[^/]/%[^/]", str_ip_local, str_mask_local);
		if (ret != 2) continue;

		struct sockaddr_in s_addr;
		struct sockaddr_in s_mask;
		if (inet_pton(AF_INET, str_ip_local, &s_addr.sin_addr) != 1)
			goto parse_err;
		if (inet_pton(AF_INET, str_mask_local, &s_mask.sin_addr) != 1)
			goto parse_err;

		struct udpstack_iface * info = &(iface_info[port_id]);
		rte_memcpy(&info->ipaddr[info->nb_ipaddr].ipaddr, &s_addr.sin_addr, sizeof(struct in_addr));
		rte_memcpy(&info->ipaddr[info->nb_ipaddr].mask, &s_mask.sin_addr, sizeof(struct in_addr));
		info->nb_ipaddr++;

		RTE_LOG(INFO, STACK, "Load IPv4 Address %s/%s for Port%d successfully\n", str_ip_local,
			str_mask_local, port_id);
		continue;

		if (info->nb_ipaddr > UDPSTACK_IFACE_MAX_ADDR)
		{
			RTE_LOG(INFO, STACK, "Max IP Adddress asigned to Port%d\n", port_id);
			break;
		}

	parse_err:
		RTE_LOG(WARNING, STACK, "Wrong Format in reading configure file"
			"%s, section %s, item %s = %s\n", cfgfile, UDPSTACK_CONFIG_SECTION_NAME, key, value);
	}


	// 填MTU，硬件MAC地址
	struct udpstack_iface * info = &(iface_info[port_id]);
	struct ether_addr ether_addr;
	unsigned short mtu;
	rte_eth_macaddr_get(port_id, &ether_addr);
	rte_eth_dev_get_mtu(port_id, &mtu);

	info->port_id = port_id;
	info->mtu = info->mtu == 0 ? mtu : info->mtu;
	ether_addr_copy(&ether_addr, &info->dev_macaddr);
	ether_addr_copy(&ether_addr, &info->user_macaddr);

	char str_macaddr[UDPSTACK_MAX_SYM];
	ether_format_addr(str_macaddr, sizeof(str_macaddr), &ether_addr);

	// 配置MTU
	int ret = rte_eth_dev_set_mtu(port_id, info->mtu);

	if(unlikely(ret < 0))
	{
		RTE_LOG(WARNING, STACK, "Error in setting MTU for port %d: %s", port_id, strerror(ret));
	}

	RTE_LOG(INFO, STACK, "Port%d: MTU=%d, MAC: %s\n", port_id, info->mtu, str_macaddr);
	return 0;
}

static int udpstack_config_ipv6(unsigned int port_id, const char * cfgfile)
{
	char key[UDPSTACK_MAX_SYM];
	char value[UDPSTACK_MAX_LINE];
	snprintf(key, sizeof(key), "Port%d.IPv6", port_id);
	MESA_load_profile_string_def(cfgfile, UDPSTACK_CONFIG_SECTION_NAME, key, value, sizeof(value), "");

	if (strncmp(value, "", UDPSTACK_MAX_LINE) == 0)
		return 0;

	RTE_LOG(WARNING, CLIENT, "Port%d.IPv6 is not supported\n", port_id);
	return 0;
}

// 读配置文件
int udpstack_config(const char * cfgfile)
{
	// 读各网卡IP地址和子网掩码配置
	for (unsigned int port_id = 0; port_id < RTE_MAX_ETHPORTS; port_id++)
	{
		struct udpstack_iface * iface = &(iface_info[port_id]);
		udpstack_config_mtu(port_id, cfgfile);
		udpstack_config_ipv4(port_id, cfgfile);
		udpstack_config_ipv6(port_id, cfgfile);

		// 成功读取到IP地址，就启用这个端口
		if (iface->nb_ipaddr != 0 || iface->nb_ip6addr != 0)
			iface->enable = 1;
	}

	return 0;
}

int udpstack_destory()
{
	return 0;
}