icmp server

icmp.c

/*

The default routing mechanism of linux will reply to a ping request using the first interface configured with an ip in the same network as the source address.

This is undesireable when multiple interfaces are configured to be within the same network as the reply might not leave on the same interface the request came on.

This tool uses raw sockets to reply to all ping requests using the same interface from which the ping request originated.

First disable linux kernel ICMP handling.

echo "1" > /proc/sys/net/ipv4/icmp_echo_ignore_all

Then compile and run this code.

gcc -Wall -Werro icmp.c -o icmp

./icmp

*/

#include <arpa/inet.h>
#include <errno.h>
#include <linux/if_packet.h>
#include <net/if.h>
#include <netinet/if_ether.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <netinet/tcp.h>
#include <netinet/udp.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>

#define BUF_SIZE 65536

static void hex_dump(const char *text, const void *data, int size) {
    printf("%s", text);
    int i;
    for (i = 0; i < size; i++) {
        if (i % 16 == 15)
            printf("\n     ");
        printf("%02X ", ((const uint8_t *)data)[i]);
    }
    printf("\n");
}

void print_sockaddr_ll(const struct sockaddr_ll *sll) {
    // Print basic fields
    printf("sll_family  = %d\n", sll->sll_family);
    printf("sll_protocol= 0x%04x\n", ntohs(sll->sll_protocol));
    printf("sll_ifindex = %d\n", sll->sll_ifindex);
    printf("sll_hatype   = %d\n", sll->sll_hatype);
    printf("sll_pkttype = %d\n", sll->sll_pkttype);
    printf("sll_halen   = %d\n", sll->sll_halen);

    // Print the MAC address
    if (sll->sll_halen > 0) {
        char mac[3 * 8] = {0}; // up to 8 bytes: "xx:xx:...:xx\0"
        for (int i = 0; i < sll->sll_halen && i < 8; i++) {
            sprintf(mac + i * 3, "%02x:", sll->sll_addr[i]);
        }
        // remove trailing colon
        mac[(sll->sll_halen * 3) - 1] = '\0';
        printf("sll_addr    = %s\n", mac);
    } else {
        printf("sll_addr    = <none>\n");
    }
}

static uint16_t calc_sum(uint16_t sum, const void *data, uint16_t len) {
    uint16_t t;
    const uint8_t *dataptr = (uint8_t *)data;
    const uint8_t *last_byte = dataptr + len - 1;

    while (dataptr < last_byte) {
        // At least two more bytes
        t = ((uint16_t)dataptr[0] << 8) + dataptr[1];
        sum += t;
        if (sum < t) {
            ++sum; // Carry
        }
        dataptr += 2;
    }

    if (dataptr == last_byte) {
        t = (dataptr[0] << 8) + 0;
        sum += t;
        if (sum < t) {
            ++sum; // carry
        }
    }
    return htons(~sum);
}

static void swap(void *a, void *b, int len) {
    for (;len--;) {
        ((uint8_t *)a)[len] ^= ((uint8_t *)b)[len];
        ((uint8_t *)b)[len] ^= ((uint8_t *)a)[len];
        ((uint8_t *)a)[len] ^= ((uint8_t *)b)[len];
    }
}

static void process_packet(const int sock, const struct sockaddr_ll *const addr, const socklen_t addrlen, uint8_t *const buf, const int data_size) {
    uint8_t *const end = buf + data_size;
    if (data_size >= sizeof(struct ethhdr) + sizeof(struct iphdr) + sizeof(struct icmphdr)) {
        struct ethhdr *const eth = (struct ethhdr *)(buf);
        struct iphdr *const ip = (struct iphdr *)((uint8_t *)eth + sizeof(*eth));
        if (ip->protocol == IPPROTO_ICMP) {
            struct icmphdr *const icmp = (struct icmphdr *)((uint8_t *)ip + sizeof(*ip));
            if (icmp->type == ICMP_ECHO && icmp->code == 0) {
                printf("\n------------------------------------------------\n\n");
                hex_dump("ADDR ", addr, addrlen);
                print_sockaddr_ll((struct sockaddr_ll *)addr);
                hex_dump("ETH  ", eth, sizeof(*eth));
                hex_dump("IP   ", ip, sizeof(*ip));
                const int icmplen = end - (uint8_t *)icmp;
                hex_dump("ICMP ", icmp, icmplen);

                // flip ETH addrs
                swap(&eth->h_source, &eth->h_dest, sizeof(eth->h_source));

                // flip IP addrs
                swap(&ip->saddr, &ip->daddr, sizeof(ip->saddr));

                // create ECHO REPLY in place 
                icmp->type = ICMP_ECHOREPLY;
                icmp->checksum = 0;
                icmp->checksum = calc_sum(0, icmp, icmplen);

                // reply to sender
                const int ret = sendto(sock, eth, data_size, 0, (struct sockaddr *)addr, addrlen);
                if (ret < 0) {
                    perror("sendto");
                }
            }
        }
    }
}

int main(int argc, char **argv) {
    int raw_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_IP));
    if (raw_sock < 0) {
        perror("socket");
        exit(EXIT_FAILURE);
    }

    unsigned char *buffer = malloc(BUF_SIZE);
    if (!buffer) {
        perror("malloc");
        close(raw_sock);
        exit(EXIT_FAILURE);
    }

    while (1) {
        struct sockaddr_ll addr;
        socklen_t addrlen = sizeof(addr);
        const int data_size = recvfrom(raw_sock, buffer, BUF_SIZE, 0, (struct sockaddr *)&addr, &addrlen);
        if (data_size < 0) {
            perror("recvfrom");
            break;
        }
        process_packet(raw_sock, &addr, addrlen, buffer, data_size);
    }

    free(buffer);
    close(raw_sock);
    return 0;
}