ping.c

 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <net/ethernet.h>
#include <net/if_arp.h>
#include <net/if.h>
#include <netinet/if_ether.h>
#include <netinet/ip_icmp.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netpacket/packet.h>
#include <arpa/inet.h>
 
#ifndef arp_hrd /*android not define struct ether_arp*/
struct ether_arp {
    struct    arphdr ea_hdr;        /* fixed-size header */
    u_int8_t arp_sha[ETH_ALEN];    /* sender hardware address */
    u_int8_t arp_spa[4];        /* sender protocol address */
    u_int8_t arp_tha[ETH_ALEN];    /* target hardware address */
    u_int8_t arp_tpa[4];        /* target protocol address */
};
#define    arp_hrd    ea_hdr.ar_hrd
#define    arp_pro    ea_hdr.ar_pro
#define    arp_hln    ea_hdr.ar_hln
#define    arp_pln    ea_hdr.ar_pln
#define    arp_op    ea_hdr.ar_op
#endif
 
#define xprint_log(fmt, ...) \
    printf("[%04d]%s() " fmt, __LINE__, __FUNCTION__, ####__VA_ARGS__)
#define xprint_err(fmt, ...) \
    printf("[%04d]%s() err: " fmt, __LINE__, __FUNCTION__, ####__VA_ARGS__)
 
#define xdebug 0
#define xunused __attribute__((unused))
 
#define HDR_LEN_ETH  sizeof(struct ether_header)
#define HDR_LEN_ARP  sizeof(struct ether_arp)
#define HDR_LEN_IP   sizeof(struct ip)
#define HDR_LEN_ICMP sizeof(struct icmp)
 
 
static unsigned char  s_frame_data[ETH_FRAME_LEN];
static unsigned int   s_frame_size = 0;
static int            s_interface_index = -1;
static unsigned char  s_interface_mac[ETH_ALEN];
static struct in_addr s_interface_ip; 
static unsigned char  s_src_mac[ETH_ALEN] = {0x00,0x11,0x22,0x33,0x44,0x55};
 
 
static int 
xsend_frame_ether(uint8_t *frame, int size, int ifindex, int skfd);
static int
xrecv_frame_ether(uint8_t *frame, int size, int ifindex, int skfd);
 
static int 
xsend_reply_arp(in_addr_t ipaddr, int skfd);
static int
xsend_reply_icmp(in_addr_t ipaddr, int skfd);
 
static uint16_t 
xutil_check_sum(uint16_t* data, int size);
static void
xutil_swap_int(uint32_t *a, uint32_t *b);
 
static int xunused
xdump_frame_byte(uint8_t *data, int size);
static int xunused
xdump_frame_ether(struct ether_header *eth);
static int xunused
xdump_frame_arp  (struct ether_arp *arp);
static int xunused
xdump_frame_ip   (struct ip *iph);
static int xunused
xdump_frame_icmp (struct icmp *icmph);
 
 
#define __DEFINITION__
 
 
static uint16_t 
xutil_check_sum(uint16_t* data, int size)
{
    unsigned int cksm = 0;
    
    while (size > 1) {
        cksm += *data++;
        size -= sizeof(uint16_t);
    }
    
    if (size) {
        cksm += *(uint8_t*)data;
    }
    
    cksm  = (cksm>>16) + (cksm&0xffff); 
    cksm += (cksm>>16); 
    
    return (uint16_t)(~cksm);
}
 
static void
xutil_swap_int(uint32_t *a, uint32_t *b)
{
    *a = *a ^ *b;
    *b = *a ^ *b;
    *a = *a ^ *b;
    return ;
}
 
static int
xdump_frame_byte(uint8_t *data, int size)
{
    int i;
 
    for(i=0; i<size; i++) {
        if((i%16) == 0) {
            printf( "[%02x] ", i/16 );
        }
        printf( "%02x ", data[i] );
        if(((i+1)%16) == 0) {
            printf( "\n" );
        }
    }
 
    printf( "\n" );
    return 0;
}
 
static int
xdump_frame_ether(struct ether_header *eth)
{
    if (NULL == eth) {
        return -1;
    }
 
    printf("========frame ether========\n");
    printf("type :0x%04x\n", htons(eth->ether_type));
    printf("d-mac:%02x-%02x-%02x-%02x-%02x-%02x\n",\
        eth->ether_dhost[0], eth->ether_dhost[1], eth->ether_dhost[2], \
        eth->ether_dhost[3], eth->ether_dhost[4], eth->ether_dhost[5]);
 
    printf("s-mac:%02x-%02x-%02x-%02x-%02x-%02x\n",\
        eth->ether_shost[0], eth->ether_shost[1], eth->ether_shost[2], \
        eth->ether_shost[3], eth->ether_shost[4], eth->ether_shost[5]);
    return 0;
}
 
static int
xdump_frame_arp  (struct ether_arp *arp)
{
    if (NULL == arp) {
        return -1;
    }
 
    printf("========frame arp  ========\n");
    printf("arp_hrd=%d    \n", htons(arp->arp_hrd));
    printf("arp_pro=0x%04x\n", htons(arp->arp_pro));
    printf("arp_op =%d    \n", htons(arp->arp_op));
    printf("arp_sdr=%02x-%02x-%02x-%02x-%02x-%02x %d.%d.%d.%d\n", \
        arp->arp_sha[0], arp->arp_sha[1], arp->arp_sha[2], \
        arp->arp_sha[3], arp->arp_sha[4], arp->arp_sha[5], \
        arp->arp_spa[0], arp->arp_spa[1], arp->arp_spa[2], \
        arp->arp_spa[3]);
    printf("arp_tgr=%02x-%02x-%02x-%02x-%02x-%02x %d.%d.%d.%d\n", \
        arp->arp_tha[0], arp->arp_tha[1], arp->arp_tha[2], \
        arp->arp_tha[3], arp->arp_tha[4], arp->arp_tha[5], \
        arp->arp_tpa[0], arp->arp_tpa[1], arp->arp_tpa[2], \
        arp->arp_tpa[3]);
    return 0;
}
 
static int
xdump_frame_ip(struct ip *iph)
{
    if (NULL == iph) {
        return -1;
    }
    
    printf("========frame ip   ========\n");
    printf("ip_v  =0x%x\n", iph->ip_v             ); /* 4位版本号           */
    printf("ip_hl =0x%x\n", iph->ip_hl            ); /* 4位IP头部长度 32bit */
    printf("ip_tos=0x%x\n", iph->ip_tos           ); /* 8位服务类型         */
    printf("ip_len=0x%x\n", htons(iph->ip_len)    ); /*16位数据包总长度     */
    printf("ip_id =0x%x\n", htons(iph->ip_id)     ); /*16位标志符           */
    printf("ip_off=0x%x\n", htons(iph->ip_off)    ); /* 3位标记+13位片偏移  */
    printf("ip_ttl=0x%x\n", iph->ip_ttl           ); /* 8位生存时间         */
    printf("ip_p  =0x%x\n", iph->ip_p             ); /* 8位协议号           */
    printf("ip_sum=0x%x\n", htons(iph->ip_sum)    ); /*16位首部校验和       */
    printf("ip_src=%s  \n", inet_ntoa(iph->ip_src)); /*32位源地址           */
    printf("ip_dst=%s  \n", inet_ntoa(iph->ip_dst)); /*32位目的地址         */
    return 0;
}
 
static int 
xdump_frame_icmp (struct icmp *icmph)
{
    if (NULL == icmph) {
        return -1;
    }
    
    printf("========frame icmp ========\n");
    printf("icmp_type =0x%x\n", icmph->icmp_type ); /*  8位类型          */
    printf("icmp_code =0x%x\n", icmph->icmp_code ); /*  8位代码          */
    printf("icmp_cksum=0x%x\n", icmph->icmp_cksum); /* 16位校验和        */
    printf("icmp_id   =0x%x\n", icmph->icmp_id   ); /* 16位识别号 进程id */
    printf("icmp_seq  =0x%x\n", icmph->icmp_seq  ); /* 16位序列号        */
    return 0;
}
 
static int 
xsend_frame_ether(uint8_t *frame, int size, int ifindex, int skfd)
{
    struct sockaddr_ll sll;
    socklen_t          sln = 0;
    
    struct sockaddr_ll *psll = NULL;
 
    if (-1 !=  ifindex) {
        bzero(&sll, sizeof(sll));
        sll.sll_ifindex  = ifindex;
        sll.sll_family   = PF_PACKET;
        sll.sll_protocol = htons(ETH_P_ALL);
 
        psll = &sll;
        sln  = sizeof(struct sockaddr_ll);
    }
    
    size = sendto(skfd, frame, size, 0, (struct sockaddr*)psll, sln);
    if (size < 0) {
        xprint_err("ioctl() SIOCGIFINDEX failed! errno=%d (%s)\n", \
            errno, strerror(errno));
    }
        
    return size;
}
 
static int
xrecv_frame_ether(uint8_t *frame, int size, int ifindex, int skfd)
{
    struct sockaddr_ll sll;
    socklen_t          sln = sizeof(struct sockaddr_ll);
 
    struct sockaddr_ll *psll = NULL;
    socklen_t          *psln = NULL;
 
    if (NULL==frame || size<=0) {
        xprint_err("param failed! frame=%p size=%d\n", frame, size);
        return -1;
    }
 
    if (-1 !=  ifindex) {
        bzero(&sll, sizeof(sll));
        sll.sll_ifindex  = ifindex;
        sll.sll_family   = PF_PACKET;
        sll.sll_protocol = htons(ETH_P_ALL);
 
        psll = &sll;
        psln = &sln;
    }
 
    memset(frame, 0, size*sizeof(uint8_t));
    size = recvfrom(skfd, frame, size, 0, (struct sockaddr*)psll, psln);
 
    if (size < 0) {
        xprint_err("recvfrom() failed! errno=%d (%s)\n", \
            errno, strerror(errno));
    }
    return size;
}
 
static int 
xsend_reply_arp(in_addr_t ipaddr, int skfd)
{
    struct ether_header *eth = NULL;
    struct ether_arp    *arp = NULL;
    
    eth = (struct ether_header*)s_frame_data;
    arp = (struct ether_arp*)(s_frame_data + HDR_LEN_ETH);
 
    if (*(unsigned int*)arp->arp_tpa != ipaddr) {
        return -1;
    }  
   
    /*ether*/
    memcpy(eth->ether_dhost, eth->ether_shost, ETH_ALEN);
    memcpy(eth->ether_shost, s_src_mac       , ETH_ALEN);
   
    /*arp*/
    arp->arp_op = htons(ARPOP_REPLY);
    memcpy(arp->arp_tha, arp->arp_sha, ETH_ALEN);
    memcpy(arp->arp_tpa, arp->arp_spa, 4);
 
    memcpy(arp->arp_sha, s_src_mac, ETH_ALEN);
    memcpy(arp->arp_spa, &ipaddr, 4);
 
#if xdebug
    printf("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
    printf("========frame size:%d\n", s_frame_size);
    xdump_frame_ether(eth);
    xdump_frame_arp  (arp);
    printf("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
    printf("\n");
#endif
 
    if (s_frame_size != xsend_frame_ether(\
        s_frame_data, s_frame_size, s_interface_index, skfd)) {
        return -1;
    }
 
    xprint_log("ok. size=%d\n", s_frame_size);
    return 0;
}
 
static int 
xsend_reply_icmp(in_addr_t ipaddr, int skfd)
{
    struct ip *iph           = NULL;
    struct ether_header *eth = NULL;
    struct icmp *icmph       = NULL;
    
    eth   = (struct ether_header*)s_frame_data;
    iph   = (struct ip*)(s_frame_data + HDR_LEN_ETH);
    icmph = (struct icmp*)(s_frame_data + HDR_LEN_ETH + HDR_LEN_IP);
    
    if ((iph->ip_p!=IPPROTO_ICMP) || iph->ip_dst.s_addr!=ipaddr) {
        return 1;
    }
 
    /*ether*/
    memcpy(eth->ether_dhost, eth->ether_shost, ETH_ALEN);
    memcpy(eth->ether_shost, s_src_mac, ETH_ALEN);
    
    /*ip*/
    xutil_swap_int(&(iph->ip_src.s_addr), &(iph->ip_dst.s_addr));
    iph->ip_off = 0;
    iph->ip_sum = 0;
    iph->ip_sum = xutil_check_sum((uint16_t*)iph, HDR_LEN_IP);
 
    /*icmp*/
    icmph->icmp_type  = ICMP_ECHOREPLY;
    icmph->icmp_cksum = 0;
    icmph->icmp_cksum = \
        xutil_check_sum((uint16_t*)icmph, s_frame_size-HDR_LEN_ETH-HDR_LEN_IP);
 
#if xdebug
    printf("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
    printf("========frame size:%d\n", s_frame_size);
    xdump_frame_ether(eth  );
    xdump_frame_ip   (iph  );
    xdump_frame_icmp (icmph);
    printf("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
    printf("\n");
#endif
 
    if (s_frame_size != xsend_frame_ether( \
        s_frame_data, s_frame_size, s_interface_index, skfd)) {
        return -1;
    }
 
    xprint_log("ok. size=%d\n", s_frame_size);
    return 0;
}
 
int main(int argc, char **argv)
{
    int       skfd       = -1;
    in_addr_t xping_addr = 0;
   
    if (argc <= 2) {
        printf("usage: %s interface ipaddr\n",argv[0]);
        printf("   ex: %s eth0 192.168.88.1\n", argv[0]);
        return -1;
    }
 
    skfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
    if (skfd < 0) {
        xprint_err("socket() failed! errno=%d (%s)\n", errno, strerror(errno));
        return -1;
    } 
 
    struct ifreq ifr;
    bzero(&ifr,sizeof(ifr));
    strcpy(ifr.ifr_name, argv[1]);
    if (-1 == ioctl(skfd, SIOCGIFINDEX, &ifr)) {
        xprint_err("ioctl() SIOCGIFINDEX failed! errno=%d (%s)\n", \
            errno, strerror(errno));
        return -1;
    }
    s_interface_index = ifr.ifr_ifindex;
    
    if (-1 == ioctl(skfd, SIOCGIFHWADDR, &ifr)) {
        xprint_err("ioctl() SIOCGIFHWADDR failed! errno=%d (%s)\n", \
            errno, strerror(errno));
        return -1;
    }
    memcpy(s_interface_mac, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
 
    if (-1 == ioctl(skfd, SIOCGIFADDR, &ifr)) {
        xprint_err("ioctl() SIOCGIFADDR failed! errno=%d (%s)\n", \
            errno, strerror(errno));
        return -1;
    }
    s_interface_ip.s_addr = \
        ((struct sockaddr_in*)&(ifr.ifr_addr))->sin_addr.s_addr;
        
    if (-1 == ioctl(skfd, SIOCGIFFLAGS, &ifr)) {
        xprint_err("ioctl() IFF_PROMISC failed! errno=%d (%s)\n", \
            errno, strerror(errno));
        return -1;
    }
 
    if ((ifr.ifr_flags&IFF_PROMISC) != IFF_PROMISC) {
        ifr.ifr_flags |= IFF_PROMISC;
        if(-1 == ioctl(skfd, SIOCSIFFLAGS, &ifr)) {
            xprint_err("ioctl() IFF_PROMISC failed! errno=%d (%s)\n", \
                errno, strerror(errno));
            return -1;
        }
    }
 
    printf("========host info  ========\n");
    printf("ifr_ifindex=%d %s\n", s_interface_index, argv[1]);
    printf("ifr_hwaddr =%02x-%02x-%02x-%02x-%02x-%02x\n", \
        s_interface_mac[0], s_interface_mac[1], s_interface_mac[2], 
        s_interface_mac[3], s_interface_mac[4], s_interface_mac[5]);
    printf("ifr_addr   =%s\n", inet_ntoa(s_interface_ip));
    printf("ifr_flags  =IFF_PROMISC\n");
    printf("pid        =0x%x\n", getpid());
    printf("header_eth =%d\n", HDR_LEN_ETH);
    printf("header_arp =%d\n", HDR_LEN_ARP);
    printf("header_ip  =%d\n", HDR_LEN_IP);
    printf("header_icmp=%d\n", HDR_LEN_ICMP);
    printf("\n");
 
    printf("press any key continue!\n");
    getchar();
    printf("waiting for someone ping %s ...\n", argv[2]);
    
#if 0
    int on = 1;
    if (0 != setsockopt(skfd, IPPROTO_IP, IP_HDRINCL, &on, sizeof(on))) {
        xprint_err("setsockopt() IP_HDRINCL failed! errno=%d (%s)\n", \
            errno, strerror(errno));
        return -1;
    }
#endif    
    
    xping_addr = inet_addr(argv[2]);
    
    while(1) {
        uint16_t ether_type = 0;
        struct ether_header* eth = NULL;
        
        memset(s_frame_data, 0x00, sizeof(unsigned char)*ETH_FRAME_LEN);
        s_frame_size = xrecv_frame_ether(s_frame_data, ETH_FRAME_LEN, \
            s_interface_index, skfd);
            
        eth = (struct ether_header*)s_frame_data;
        ether_type = htons(eth->ether_type);
 
        switch(ether_type) {
            case ETHERTYPE_ARP: {
                xsend_reply_arp(xping_addr, skfd);
                break;
            }
            case ETHERTYPE_IP: {     
                xsend_reply_icmp(xping_addr, skfd);
                break;
            }
            default: {
                break;
            }
        }
    }
    
    close(skfd);
    return 0;
}
https://blog.csdn.net/maxzero/article/details/52790853