#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;
}