diff --git a/linux/pcap/udp.c b/linux/pcap/udp.c new file mode 100644 index 0000000..560dce3 --- /dev/null +++ b/linux/pcap/udp.c @@ -0,0 +1,352 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +}