icmp.c

/*
 * Copyright (C) 2024 TDT AG <development@tdt.de>
 *
 * This is free software, licensed under the GNU General Public License v2.
 * See https://www.gnu.org/licenses/gpl-2.0.txt for more information.
 *
 * copied and modified: https://github.com/br101/pingcheck/blob/master/icmp.c
 */

/* keep libc includes before linux headers for musl compatibility */
#include <netinet/in.h>

#include <err.h>
#include <linux/icmp.h>
#include <linux/if.h>
#include <linux/ip.h>
#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <unistd.h>

#include "logging.h"

static int pid = -1;

/* standard 1s complement checksum */
/* copied from https://github.com/br101/pingcheck/blob/master/icmp.c */
static unsigned short checksum(void* b, int len)
{
	unsigned short* buf = b;
	unsigned int sum = 0;
	unsigned short result;

	for (sum = 0; len > 1; len -= 2) {
		sum += *buf++;
	}
	if (len == 1) {
		sum += *(unsigned char*)buf;
	}
	sum = (sum >> 16) + (sum & 0xFFFF);
	sum += (sum >> 16);
	result = ~sum;

	return result;
}

int icmp_init(const char* ifname)
{
	struct ifreq ifr;
	int ret, fd;

	pid = getpid();

	fd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
	if (fd == -1) {
		LOGGING_WARN("%s: could not open socket", __func__);
		return -1;
	}

	if (ifname == NULL) {
		LOGGING_WARN("%s: ifname must not be empty", __func__);
		return -2;
	}

	if (strlen(ifname) >= IFNAMSIZ) {
		LOGGING_WARN("%s: ifname too long", __func__);
		return -3;
	}

	memset(&ifr, 0, sizeof(struct ifreq));
	strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
	ret = setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr));
	if (ret < 0) {
		LOGGING_WARN("%s: could not bind to '%s'", __func__, ifname);
		close(fd);
		return -4;
	}

	return fd;
}

int icmp_echo_send(int fd, int dst_ip, int cnt)
{
	struct sockaddr_in addr;
	struct icmphdr *icmp;
	char buf[500];
	int ret;

	addr.sin_family = AF_INET;
	addr.sin_port = 0;
	addr.sin_addr.s_addr = dst_ip;

	icmp = (struct icmphdr*)buf;

	icmp->type = ICMP_ECHO;
	icmp->code = 0;
	icmp->un.echo.id = htons(pid + fd);
	icmp->un.echo.sequence = htons(cnt);
	icmp->checksum = 0;
	icmp->checksum = checksum(buf, sizeof(struct icmphdr));

	ret = sendto(fd, &buf, sizeof(struct icmphdr), 0,
		     (struct sockaddr*)&addr,  sizeof(addr));

	if (ret <= 0) {
		LOGGING_WARN("%s: sendto", __func__);
		return -1;
	}

	LOGGING_INFO("%s: succesfully send icmp request fd=%d", __func__, fd);
	return 0;
}

int icmp_echo_receive(int fd)
{
	struct icmphdr *icmp;
	struct iphdr *ip;
	int received_fd;
	char buf[500];
	int csum_recv;
	int csum_calc;
	int ret;

	ret = recv(fd, buf, sizeof(buf), 0);
	if (ret < (int)(sizeof(struct icmphdr) + sizeof(struct iphdr))) {
		LOGGING_WARN("%s: received packet too short", __func__);
		return -1;
	}

	ip = (struct iphdr*)buf;
	icmp = (struct icmphdr*)(buf + ip->ihl * 4);

	/* need to zero before calculating checksum */
	csum_recv = icmp->checksum;
	icmp->checksum = 0;

	csum_calc = checksum(icmp, sizeof(struct icmphdr));
	received_fd = ntohs(icmp->un.echo.id) - pid;
	LOGGING_INFO("%s: icmp->un.echo.id: %d pid: %d",
		     __func__, icmp->un.echo.id, pid);

	/* incorrect checksum */
	if (csum_recv != csum_calc) {
		LOGGING_WARN("%s: checksum does not match", __func__);
		return -2;
	}

	/* incorrect icmp type */
	if (icmp->type != ICMP_ECHOREPLY) {
		LOGGING_WARN("%s: is not an echo reply", __func__);
		return -3;
	}

	if (received_fd < 0) {
		LOGGING_WARN("%s: not a valid file descriptor", __func__);
		return -4;
	}

	LOGGING_INFO("%s: succesfully received icmp request fd=%d received_fd=%d",
		     __func__, fd, received_fd);
	return received_fd;
}