468 lines
11 KiB
C++
468 lines
11 KiB
C++
#include <stdio.h>
|
||
#include <string.h>
|
||
#include <stdlib.h>
|
||
#include <ctype.h>
|
||
#include <sys/ioctl.h>
|
||
#include <errno.h>
|
||
#include <unistd.h>
|
||
#include <net/if.h>
|
||
|
||
#include <sendpkt-inl.h>
|
||
|
||
static volatile uint64_t g_rand_seed = 0x013579ABCDEF;
|
||
|
||
#define SENDPACKET_CKSUM_CARRY(x) (x = (x >> 16) + (x & 0xffff), (~(x + (x >> 16)) & 0xffff))
|
||
|
||
unsigned int deliver_rand(void)
|
||
{
|
||
return g_rand_seed ^ (unsigned int)random();
|
||
}
|
||
|
||
unsigned int deliver_rand_range(unsigned int start, unsigned int end)
|
||
{
|
||
unsigned int rand_num = deliver_rand();
|
||
|
||
if(start > end)
|
||
{
|
||
return end + rand_num % (start - end + 1);
|
||
}
|
||
|
||
return start + rand_num % (end - start + 1);
|
||
}
|
||
|
||
|
||
|
||
/* ascii字符转16进制 */
|
||
char MESA_ascii_to_hex(char ascii)
|
||
{
|
||
char c = 0;
|
||
|
||
switch(ascii)
|
||
{
|
||
case '0':
|
||
case '1':
|
||
case '2':
|
||
case '3':
|
||
case '4':
|
||
case '5':
|
||
case '6':
|
||
case '7':
|
||
case '8':
|
||
case '9':
|
||
c = ascii - 0x30;
|
||
break;
|
||
|
||
case 'a':
|
||
case 'b':
|
||
case 'c':
|
||
case 'd':
|
||
case 'e':
|
||
case 'f':
|
||
c = 10 + ascii - 0x61;
|
||
break;
|
||
|
||
case 'A':
|
||
case 'B':
|
||
case 'C':
|
||
case 'D':
|
||
case 'E':
|
||
case 'F':
|
||
c = 10 + ascii - 0x41;
|
||
break;
|
||
}
|
||
|
||
return c;
|
||
}
|
||
|
||
|
||
|
||
/* 2012-04-10 LiJia add, 获取网卡MAC地址
|
||
参数:
|
||
device: 网卡名称
|
||
mac: 存储MAC地址的数组,结果为网络序,
|
||
如网卡MAC地址为11:22:33:44:55:66,则mac[0]为0x11,mac[5]为0x66.
|
||
返回值:
|
||
0: 正常
|
||
-1:错误
|
||
*/
|
||
int deliver_get_dev_mac(const char *device, unsigned char mac[6])
|
||
{
|
||
struct ifreq ifr;
|
||
int fd;
|
||
|
||
fd = socket(AF_INET, SOCK_DGRAM, 0);
|
||
if(fd < 0)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
memset(ifr.ifr_ifrn.ifrn_name, 0, sizeof(ifr.ifr_ifrn.ifrn_name));
|
||
strncpy(ifr.ifr_ifrn.ifrn_name, device, sizeof(ifr.ifr_ifrn.ifrn_name));
|
||
if(ioctl(fd, SIOCGIFHWADDR, &ifr) == -1)
|
||
{
|
||
printf("Cann't get hwaddr of %s:%s\n", device, strerror(errno));
|
||
goto err_exit;
|
||
}
|
||
|
||
if(ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER)
|
||
{
|
||
printf("'%s' is not ethernet interface!\n", device);
|
||
goto err_exit;
|
||
}
|
||
|
||
memcpy(mac, ifr.ifr_ifru.ifru_addr.sa_data, 6);
|
||
|
||
close(fd);
|
||
|
||
return 0;
|
||
|
||
err_exit:
|
||
close(fd);
|
||
return -1;
|
||
}
|
||
|
||
|
||
|
||
/* 2012-04-11 LiJia add,将MAC字符串形式转换为16进制MAC地址.
|
||
参数:
|
||
str: MAC地址字符串
|
||
delim: 字符串分隔符，常见为':', '-'等,如: xx:xx:xx:xx:xx:xx
|
||
如果字符串无分隔符，delim设为-1.
|
||
mac: 存储MAC地址的数组(指针),结果为网络序,
|
||
如网卡MAC地址为11:22:33:44:55:66,则mac[0]为0x11,mac[5]为0x66.
|
||
返回值:
|
||
0: 正常
|
||
-1:错误
|
||
*/
|
||
int deliver_mac_pton(const char *str, int delim, char *mac)
|
||
{
|
||
#define MAC_STR_LEN_DELIM (17) /* length of "11:22:33:44:55:66" */
|
||
#define MAC_STR_LEN_NODELIM (12) /* length of "112233445566" */
|
||
const char *s = str;
|
||
int i;
|
||
|
||
/* 检查输入合法性 */
|
||
if(delim != -1)
|
||
{
|
||
if(strlen(str) != MAC_STR_LEN_DELIM)
|
||
{
|
||
printf("MAC string length error!\n");
|
||
return -1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if(strlen(str) != MAC_STR_LEN_NODELIM)
|
||
{
|
||
printf("MAC string length error!\n");
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
/* 检查输入合法性，同时转换成16进制值 */
|
||
for(i = 0; i < 6; i++)
|
||
{
|
||
mac[i] = 0; /* 先清零，赋值语句都是或操作 */
|
||
if(isxdigit(*s)==0)
|
||
{
|
||
printf("MAC string type error!\n");
|
||
return -1;
|
||
}
|
||
mac[i] |= MESA_ascii_to_hex(*s) << 4;
|
||
s++;
|
||
|
||
if(isxdigit(*s)==0)
|
||
{
|
||
printf("MAC string type error!\n");
|
||
return -1;
|
||
}
|
||
mac[i] |= MESA_ascii_to_hex(*s);
|
||
s++;
|
||
|
||
if((delim != -1) && i<5 && (*s++ != (char)delim))
|
||
{
|
||
printf("MAC string type error!\n");
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
int sendpacket_in_cksum(unsigned short *addr, int len)
|
||
{
|
||
int sum;
|
||
int nleft;
|
||
unsigned short ans;
|
||
unsigned short *w;
|
||
|
||
sum = 0;
|
||
ans = 0;
|
||
nleft = len;
|
||
w = addr;
|
||
|
||
while (nleft > 1)
|
||
{
|
||
sum += *w++;
|
||
nleft -= 2;
|
||
}
|
||
if (nleft == 1)
|
||
{
|
||
*(char *)(&ans) = *(char *)w;
|
||
sum += ans;
|
||
}
|
||
return (sum);
|
||
}
|
||
|
||
|
||
|
||
int deliver_do_checksum(unsigned char *buf, int protocol, int len)
|
||
{
|
||
struct mesa_ip4_hdr *iph_p;
|
||
struct mesa_ip6_hdr *ip6h_p;
|
||
int ip_hl;
|
||
int sum;
|
||
int is_ipv6 = 0;
|
||
|
||
sum = 0;
|
||
iph_p = (struct mesa_ip4_hdr *)buf;
|
||
|
||
if(4 == iph_p->ip_v) /* IP版本号字段，IPv4和IPv6格式是相同的 */
|
||
{
|
||
ip_hl = iph_p->ip_hl << 2;
|
||
ip6h_p = NULL;
|
||
}
|
||
else if(6 == iph_p->ip_v)
|
||
{
|
||
ip6h_p = (struct mesa_ip6_hdr *)buf;
|
||
iph_p = NULL;
|
||
ip_hl = sizeof(struct mesa_ip6_hdr);
|
||
is_ipv6 = 1;
|
||
}
|
||
else
|
||
{
|
||
return (-1);
|
||
}
|
||
|
||
switch (protocol)
|
||
{
|
||
case IPPROTO_TCP:
|
||
{
|
||
struct mesa_tcp_hdr *tcph_p = (struct mesa_tcp_hdr *)(buf + ip_hl);
|
||
|
||
tcph_p->th_sum = 0;
|
||
if(is_ipv6)
|
||
{
|
||
sum = sendpacket_in_cksum((unsigned short*)&ip6h_p->ip6_src, 32);
|
||
}
|
||
else
|
||
{
|
||
sum = sendpacket_in_cksum((unsigned short *)&iph_p->ip_src, 8);
|
||
}
|
||
sum += ntohs(IPPROTO_TCP + len);
|
||
sum += sendpacket_in_cksum((unsigned short *)tcph_p, len);
|
||
tcph_p->th_sum = SENDPACKET_CKSUM_CARRY(sum);
|
||
break;
|
||
}
|
||
|
||
case IPPROTO_UDP:
|
||
{
|
||
struct mesa_udp_hdr *udph_p =(struct mesa_udp_hdr *)(buf + ip_hl);
|
||
|
||
udph_p->uh_sum = 0;
|
||
if(is_ipv6)
|
||
{
|
||
sum = sendpacket_in_cksum((unsigned short*)&ip6h_p->ip6_src, 32);
|
||
}
|
||
else
|
||
{
|
||
sum = sendpacket_in_cksum((unsigned short*)&iph_p->ip_src, 8);
|
||
}
|
||
sum += ntohs(IPPROTO_UDP + len);
|
||
sum += sendpacket_in_cksum((unsigned short*)udph_p, len);
|
||
udph_p->uh_sum = SENDPACKET_CKSUM_CARRY(sum);
|
||
break;
|
||
}
|
||
|
||
case IPPROTO_IP:
|
||
{
|
||
iph_p->ip_sum = 0;
|
||
sum = sendpacket_in_cksum((unsigned short*)iph_p, len);
|
||
iph_p->ip_sum = SENDPACKET_CKSUM_CARRY(sum);
|
||
break;
|
||
}
|
||
|
||
default:
|
||
{
|
||
return (-1);
|
||
}
|
||
}
|
||
return (1);
|
||
}
|
||
|
||
|
||
|
||
|
||
/*
|
||
if playload2 is not NULL, it means that there is ip spice, it must be copied first, then
|
||
playload is copied
|
||
otherwise, only payload is copied and it includes ip header
|
||
*/
|
||
int deliver_build_ethhdr(unsigned char *dst, unsigned char *src, unsigned short type,unsigned char *buf)
|
||
{
|
||
struct mesa_ethernet_hdr eth_hdr;
|
||
|
||
if (!buf)
|
||
{
|
||
return (-1);
|
||
}
|
||
|
||
memcpy(eth_hdr.ether_dhost, dst, ETHER_ADDR_LEN); /* destination address */
|
||
memcpy(eth_hdr.ether_shost, src, ETHER_ADDR_LEN); /* source address */
|
||
eth_hdr.ether_type = htons(type); /* packet type */
|
||
|
||
memcpy(buf, ð_hdr, sizeof(eth_hdr));
|
||
return (0);
|
||
}
|
||
|
||
|
||
|
||
/*
|
||
if playload2 is not NULL, it means that there is ip spice, it must be copied first, then
|
||
playload is copied
|
||
otherwise, only payload is copied and it includes ip header
|
||
*/
|
||
int deliver_build_ethernet(unsigned char *dst, unsigned char *src, unsigned short type,
|
||
const unsigned char *payload, int payload_s, unsigned char *buf)
|
||
{
|
||
struct mesa_ethernet_hdr eth_hdr;
|
||
|
||
if (!buf)
|
||
{
|
||
return (-1);
|
||
}
|
||
|
||
memcpy(eth_hdr.ether_dhost, dst, ETHER_ADDR_LEN); /* destination address */
|
||
memcpy(eth_hdr.ether_shost, src, ETHER_ADDR_LEN); /* source address */
|
||
eth_hdr.ether_type = htons(type); /* packet type */
|
||
|
||
if (payload && payload_s)
|
||
{
|
||
memcpy(buf + SENDPACKET_ETH_H, payload, payload_s);
|
||
}
|
||
memcpy(buf, ð_hdr, sizeof(eth_hdr));
|
||
return (0);
|
||
}
|
||
|
||
|
||
|
||
int deliver_build_ipv6(unsigned char traffic_class, unsigned int flow_lable,
|
||
unsigned short len, unsigned char next_header, unsigned char hop,
|
||
const struct in6_addr *src, const struct in6_addr *dst,
|
||
const unsigned char *payload, int payload_s, unsigned char *buf)
|
||
{
|
||
struct mesa_ip6_hdr *ip6_h;
|
||
|
||
if(!buf){
|
||
return -1;
|
||
}
|
||
|
||
ip6_h = (struct mesa_ip6_hdr *)buf;
|
||
|
||
memset(ip6_h, 0, sizeof(struct mesa_ip6_hdr));
|
||
|
||
ip6_h->ip6_flags[0] = 0x60 | ((traffic_class & 0xF0) >> 4);
|
||
ip6_h->ip6_flags[1] = ((traffic_class & 0x0F) << 4) | ((flow_lable & 0xF0000) >> 16);
|
||
ip6_h->ip6_flags[2] = flow_lable & 0x0FF00 >> 8;
|
||
ip6_h->ip6_flags[3] = flow_lable & 0x000FF;
|
||
ip6_h->ip6_payload_len = htons(len);
|
||
ip6_h->ip6_nxt_hdr = next_header;
|
||
ip6_h->ip6_hop = hop;
|
||
memcpy(&ip6_h->ip6_src, src, sizeof(struct in6_addr));
|
||
memcpy(&ip6_h->ip6_dst, dst, sizeof(struct in6_addr));
|
||
|
||
if(payload && payload_s)
|
||
{
|
||
memcpy(buf + sizeof(struct mesa_ip6_hdr), payload, payload_s);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
int deliver_build_ipv4(unsigned short carry_layer_len, unsigned char tos, unsigned short id,
|
||
unsigned short frag, unsigned char ttl, unsigned char prot, unsigned int src, unsigned int dst,
|
||
const unsigned char *payload,int payload_s, unsigned char *buf)
|
||
{
|
||
struct mesa_ip4_hdr *ip_hdr;
|
||
|
||
if (!buf)
|
||
{
|
||
return (-1);
|
||
}
|
||
|
||
ip_hdr = (struct mesa_ip4_hdr *)buf;
|
||
|
||
ip_hdr->ip_v = 4; /* version 4 */
|
||
ip_hdr->ip_hl = 5; /* 20 byte header */
|
||
ip_hdr->ip_tos = tos; /* IP tos */
|
||
ip_hdr->ip_len = htons(SENDPACKET_IP_H + carry_layer_len); /* total length */
|
||
ip_hdr->ip_id = htons(id); /* IP ID */
|
||
ip_hdr->ip_off = htons(frag); /* fragmentation flags */
|
||
ip_hdr->ip_ttl = ttl; /* time to live */
|
||
ip_hdr->ip_p = prot; /* transport protocol */
|
||
ip_hdr->ip_sum = 0; /* do this later */
|
||
ip_hdr->ip_src.s_addr = src; /* 为什么地址用网络序? 历史遗留原因, 改动太多,只能这么继续了 */
|
||
ip_hdr->ip_dst.s_addr = dst; /* 为什么地址用网络序? 历史遗留原因, 改动太多,只能这么继续了 */
|
||
if (payload && payload_s)
|
||
{
|
||
memcpy(buf + SENDPACKET_IP_H, payload, payload_s);
|
||
}
|
||
|
||
return (0);
|
||
}
|
||
|
||
|
||
int deliver_build_tcp(unsigned short sp, unsigned short dp, unsigned int seq, unsigned int ack,
|
||
unsigned char th_flags, unsigned short win, unsigned short urg,
|
||
const unsigned char *payload, int payload_s, unsigned char *buf)
|
||
{
|
||
struct mesa_tcp_hdr *tcp_hdr;
|
||
|
||
if (!buf)
|
||
{
|
||
return (-1);
|
||
}
|
||
|
||
tcp_hdr = (struct mesa_tcp_hdr *)buf;
|
||
|
||
tcp_hdr->th_sport = htons(sp); /* source port */
|
||
tcp_hdr->th_dport = htons(dp); /* destination port */
|
||
tcp_hdr->th_seq = htonl(seq); /* sequence number */
|
||
tcp_hdr->th_ack = htonl(ack); /* acknowledgement number */
|
||
tcp_hdr->th_flags = th_flags; /* control flags */
|
||
tcp_hdr->th_x2 = 0; /* UNUSED */
|
||
tcp_hdr->th_off = 5; /* 20 byte header */
|
||
tcp_hdr->th_win = htons(win); /* window size */
|
||
tcp_hdr->th_sum = 0; /* checksum done in userland */
|
||
tcp_hdr->th_urp = urg; /* urgent pointer */
|
||
|
||
if (payload && payload_s)
|
||
{
|
||
// memcpy(buf + SENDPACKET_TCP_H, payload, payload_s);
|
||
memcpy(buf + sizeof(struct mesa_tcp_hdr), payload, payload_s);
|
||
}
|
||
|
||
return (0);
|
||
|
||
}
|
||
|
||
|
||
|
||
|