353 lines
8.6 KiB
C
353 lines
8.6 KiB
C
#include <pcap.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
|
|
#define STR_MAC_SRC "1C:75:08:6A:4E:6E"
|
|
#define STR_MAC_DST "1C:1B:0D:2E:A9:99"
|
|
#define STR_IP_SRC "192.168.2.32"
|
|
#define STR_IP_DST "192.168.2.24"
|
|
#define STR_PORT_SRC "40103"
|
|
#define STR_PORT_DST "40103"
|
|
#define UDP_CHECKSUM_ENABLE 1
|
|
|
|
struct udp_cfg_s {
|
|
char *mac_src;
|
|
char *mac_dst;
|
|
char *ip_src;
|
|
char *ip_dst;
|
|
char *port_src;
|
|
char *port_dst;
|
|
};
|
|
|
|
struct udp_cfg_s udp_cfg = {
|
|
.mac_src = STR_MAC_SRC,
|
|
.mac_dst = STR_MAC_DST,
|
|
.ip_src = STR_IP_SRC,
|
|
.ip_dst = STR_IP_DST,
|
|
.port_src = STR_PORT_SRC,
|
|
.port_dst = STR_PORT_DST,
|
|
};
|
|
|
|
struct udp_s {
|
|
/* MAC */
|
|
uint8_t mac_dst[6];
|
|
uint8_t mac_src[6];
|
|
uint8_t ip_type[2]; // 08 00 for IPv4 type
|
|
/* IP */
|
|
uint8_t ver_head_length; // 0x45 for IPv4 and head length is 5*4=20 bytes
|
|
uint8_t tos; // type of service, normal 0
|
|
uint8_t ip_total_length[2]; // 28 + user data length, e.g. 00 20 for 32 bytes
|
|
uint8_t id[2];
|
|
uint8_t offset[2];
|
|
uint8_t ttl; // time to live, normal 0x80
|
|
uint8_t prototal; // 0x11 for UDP prototal
|
|
uint8_t ip_checksum[2]; // normal 00 00
|
|
uint8_t ip_src[4]; // e.g. C0 A8 02 18 is for 192.168.2.24
|
|
uint8_t ip_dst[4]; // e.g. C0 A8 02 20 is for 192.168.2.32
|
|
/* UDP */
|
|
uint8_t port_src[2]; // e.g. 26 E5 is for 9957
|
|
uint8_t port_dst[2]; // e.g. 0E D3 is for 3795
|
|
uint8_t udp_total_length[2]; // 8 + user data length, e.g. 00 0C for 12 bytes
|
|
uint8_t udp_checksum[2];
|
|
/* data */
|
|
uint8_t data[64];
|
|
};
|
|
|
|
static int char2idx(char c, uint8_t *idx)
|
|
{
|
|
if (c >= '0' && c <= '9') {
|
|
*idx = c - '0';
|
|
} else if (c >= 'A' && c <= 'F') {
|
|
*idx = c - 'A' + 10;
|
|
} else if (c >= 'a' && c <= 'f') {
|
|
*idx = c - 'a' + 10;
|
|
} else {
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int str2mac(char *str, uint8_t *mac)
|
|
{
|
|
int i, ret;
|
|
uint8_t high, low;
|
|
|
|
for (i = 0; i < 6; i++) {
|
|
if (i >= 5) {
|
|
if (str[2] != '\0') {
|
|
return -2;
|
|
}
|
|
} else {
|
|
if (str[2] != '-' && str[2] != ':' && str[2] != ' ' && str[2] != '_') {
|
|
return -3;
|
|
}
|
|
}
|
|
ret = char2idx(str[0], &high);
|
|
ret |= char2idx(str[1], &low);
|
|
if (ret) {
|
|
return -1;
|
|
}
|
|
mac[i] = (high << 4) | low;
|
|
str += 3;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static int str2ip(char *str, uint8_t *ip)
|
|
{
|
|
char c;
|
|
int idx, len;
|
|
uint8_t num;
|
|
|
|
idx = 0;
|
|
len = 0;
|
|
num = 0;
|
|
while (1) {
|
|
c = *str;
|
|
if (c >= '0' && c <= '9') {
|
|
c = c - '0';
|
|
num = num * 10 + c;
|
|
len++;
|
|
if (len > 3) {
|
|
return -3;
|
|
}
|
|
} else if (c == '.') {
|
|
ip[idx++] = num;
|
|
num = 0;
|
|
len = 0;
|
|
if (idx >= 4) {
|
|
break;
|
|
}
|
|
} else if (c == '\0') {
|
|
ip[idx++] = num;
|
|
break;
|
|
} else {
|
|
return -1;
|
|
}
|
|
str++;
|
|
}
|
|
if (idx == 4) {
|
|
return 0;
|
|
} else {
|
|
return -2;
|
|
}
|
|
}
|
|
|
|
static int str2port(char *str, uint8_t *port)
|
|
{
|
|
char c;
|
|
int idx, len;
|
|
uint16_t num;
|
|
|
|
len = 0;
|
|
num = 0;
|
|
while (1) {
|
|
c = *str;
|
|
if (c >= '0' && c <= '9') {
|
|
c = c - '0';
|
|
num = num * 10 + c;
|
|
len++;
|
|
if (len > 5) {
|
|
return -2;
|
|
}
|
|
} else if (c == '\0') {
|
|
break;
|
|
} else {
|
|
return -1;
|
|
}
|
|
str++;
|
|
}
|
|
port[0] = (num >> 8) & 0xFF;
|
|
port[1] = num & 0xFF;
|
|
|
|
return 0;
|
|
}
|
|
|
|
void padding_length(struct udp_s *udp, uint16_t length)
|
|
{
|
|
uint16_t num;
|
|
|
|
num = length + 28;
|
|
udp->ip_total_length[0] = (num >> 8) & 0xFF;
|
|
udp->ip_total_length[1] = num & 0xFF;
|
|
num = length + 8;
|
|
udp->udp_total_length[0] = (num >> 8) & 0xFF;
|
|
udp->udp_total_length[1] = num & 0xFF;
|
|
}
|
|
|
|
static uint16_t checksum(uint32_t init, uint16_t *addr, uint16_t len)
|
|
{
|
|
uint32_t sum = init;
|
|
|
|
while (len > 1) {
|
|
sum += *addr++;
|
|
len -= 2;
|
|
}
|
|
if (len == 1) {
|
|
sum += *(uint8_t *)addr;
|
|
}
|
|
while (sum >> 16) {
|
|
sum = (sum >> 16) + (sum & 0xFFFF);
|
|
sum += (sum >> 16);
|
|
}
|
|
return (uint16_t)~sum;
|
|
}
|
|
|
|
void checksum_ip(struct udp_s *packet)
|
|
{
|
|
uint32_t sum;
|
|
uint16_t *p;
|
|
|
|
packet->ip_checksum[0] = 0;
|
|
packet->ip_checksum[1] = 0;
|
|
p = (uint16_t *)(&(packet->ver_head_length));
|
|
sum = checksum(0, p, 20);
|
|
packet->ip_checksum[0] = sum & 0xFF;
|
|
packet->ip_checksum[1] = (sum >> 8) & 0xFF;
|
|
}
|
|
|
|
void checksum_udp(struct udp_s *packet)
|
|
{
|
|
#if UDP_CHECKSUM_ENABLE
|
|
uint32_t sum = 0;
|
|
uint16_t *p;
|
|
uint16_t length;
|
|
|
|
/* pseudo_header */
|
|
p = (uint16_t *)(&(packet->ip_src));
|
|
sum = sum + p[0] + p[1];
|
|
p = (uint16_t *)(&(packet->ip_dst));
|
|
sum = sum + p[0] + p[1];
|
|
sum = sum + (packet->prototal << 8);
|
|
p = (uint16_t *)(&(packet->udp_total_length));
|
|
sum = sum + p[0];
|
|
/* UDP head and data */
|
|
packet->udp_checksum[0] = 0;
|
|
packet->udp_checksum[1] = 0;
|
|
p = (uint16_t *)(&(packet->port_src));
|
|
length = (packet->udp_total_length[0] << 8) + packet->udp_total_length[1];
|
|
sum = checksum(sum, p, length);
|
|
packet->udp_checksum[0] = sum & 0xFF;
|
|
packet->udp_checksum[1] = (sum >> 8) & 0xFF;
|
|
#else
|
|
packet->udp_checksum[0] = 0;
|
|
packet->udp_checksum[1] = 0;
|
|
#endif
|
|
}
|
|
|
|
int udp_create_default(struct udp_cfg_s *cfg, struct udp_s *udp)
|
|
{
|
|
int ret;
|
|
|
|
/* MAC */
|
|
ret = str2mac(cfg->mac_dst, udp->mac_dst);
|
|
if (ret) {
|
|
return -1;
|
|
}
|
|
ret = str2mac(cfg->mac_src, udp->mac_src);
|
|
if (ret) {
|
|
return -2;
|
|
}
|
|
udp->ip_type[0] = 0x08; // 08 00 for IPv4 type
|
|
udp->ip_type[1] = 0x00;
|
|
/* IP */
|
|
udp->ver_head_length = 0x45; // 0x45 for IPv4 and head length is 5*4=20 bytes
|
|
udp->tos = 0; // type of service, normal 0
|
|
udp->ip_total_length[0] = 0; // 28 + user data length, e.g. 00 20 for 32 bytes
|
|
udp->ip_total_length[1] = 0;
|
|
udp->id[0] = 0;
|
|
udp->id[1] = 0;
|
|
udp->offset[0] = 0;
|
|
udp->offset[1] = 0;
|
|
udp->ttl = 0x80; // time to live, normal 0x80
|
|
udp->prototal = 0x11; // 0x11 for UDP prototal
|
|
udp->ip_checksum[0] = 0;
|
|
udp->ip_checksum[1] = 0;
|
|
ret = str2ip(cfg->ip_src, udp->ip_src); // e.g. C0 A8 02 18 is for 192.168.2.24
|
|
if (ret) {
|
|
return -3;
|
|
}
|
|
ret = str2ip(cfg->ip_dst, udp->ip_dst); // e.g. C0 A8 02 20 is for 192.168.2.32
|
|
if (ret) {
|
|
return -4;
|
|
}
|
|
/* UDP */
|
|
ret = str2port(cfg->port_src, udp->port_src); // e.g. 26 E5 is for 9957
|
|
if (ret) {
|
|
return -5;
|
|
}
|
|
ret = str2port(cfg->port_dst, udp->port_dst); // e.g. 0E D3 is for 3795
|
|
if (ret) {
|
|
return -6;
|
|
}
|
|
udp->udp_total_length[0] = 0; // 8 + user data length, e.g. 00 0C for 12 bytes
|
|
udp->udp_total_length[1] = 0;
|
|
udp->udp_checksum[0] = 0;
|
|
udp->udp_checksum[1] = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
struct udp_s udp;
|
|
|
|
void data_calc(void)
|
|
{
|
|
int ret, i, j;
|
|
float val;
|
|
|
|
ret = udp_create_default(&udp_cfg, &udp);
|
|
if (ret) {
|
|
printf("%s, err=%d\r\n", __FUNCTION__, ret);
|
|
}
|
|
for (i = 0; i < 8; i += 2) {
|
|
val = i * i;
|
|
for (j = 0; j < 4; j++) {
|
|
udp.data[i * 4 + j] = ((uint8_t *)(&val))[j];
|
|
}
|
|
udp.data[i * 4 + 4] = 0x00;
|
|
udp.data[i * 4 + 5] = 0x00;
|
|
udp.data[i * 4 + 6] = 0x80;
|
|
udp.data[i * 4 + 7] = 0x7F;
|
|
}
|
|
padding_length(&udp, 32);
|
|
checksum_ip(&udp);
|
|
checksum_udp(&udp);
|
|
}
|
|
|
|
int main() {
|
|
char errbuf[PCAP_ERRBUF_SIZE];
|
|
pcap_t *handle;
|
|
int i;
|
|
|
|
// 打开网络设备
|
|
handle = pcap_open_live("eth0", BUFSIZ, 0, 1000, errbuf);
|
|
if (handle == NULL) {
|
|
fprintf(stderr, "Couldn't open device: %s\n", errbuf);
|
|
return 1;
|
|
}
|
|
|
|
data_calc();
|
|
|
|
for (i = 0; i < 100; i++) {
|
|
int ret = pcap_inject(handle, &udp, sizeof(udp));
|
|
if (ret == -1) {
|
|
fprintf(stderr, "Couldn't send packet: %s\n", pcap_geterr(handle));
|
|
pcap_close(handle);
|
|
return 1;
|
|
} else {
|
|
printf("Packet sent successfully (%d bytes)\n", ret);
|
|
}
|
|
usleep(1000);
|
|
}
|
|
|
|
// 关闭网络设备
|
|
pcap_close(handle);
|
|
|
|
return 0;
|
|
}
|