path: root/src/pinger.c

/* This file is part of Ping903
   Copyright (C) 2020 Sergey Poznyakoff
  
   Ping903 is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 3, or (at your option)
   any later version.
  
   Ping903 is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
  
   You should have received a copy of the GNU General Public License
   along with Ping903.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <config.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>

#include <netinet/in_systm.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <string.h>
#include <stddef.h>
#include <stdlib.h>
#include <errno.h>
#include <pthread.h>
#include <limits.h>
#include <poll.h>
#include "ping903.h"
#include "json.h"
#include "defs.h"

unsigned long probe_interval = 60;
unsigned long ping_interval = 1;
unsigned long ping_count = 10;
unsigned long ping_tolerance = 3;

static double
nabs(double a)
{
	return (a < 0) ? -a : a;
}

static double
nsqrt(double a, double prec)
{
	double x0, x1;

	if (a < 0)
		return 0;
	if (a < prec)
		return 0;
	x1 = a / 2;
	do {
		x0 = x1;
		x1 = (x0 + a / x0) / 2;
	} while (nabs(x1 - x0) > prec);
	return x1;
}

static inline int
timeval_is_null(struct timeval const *tv)
{
	return tv->tv_sec == 0 && tv->tv_usec == 0;
}

static inline double
timeval_to_double(struct timeval const *tv)
{
	return (double)tv->tv_sec + (double)tv->tv_usec / 1000000;
}

enum { NANOSEC_IN_SEC = 1000000000 };

static inline void
timespec_add(struct timespec const *a, struct timespec const *b,
	     struct timespec *res)
{
	res->tv_sec = a->tv_sec + b->tv_sec;
	res->tv_nsec = a->tv_nsec + b->tv_nsec;
	if (res->tv_nsec >= NANOSEC_IN_SEC) {
		++res->tv_sec;
		res->tv_nsec -= NANOSEC_IN_SEC;
	}
}

static inline void
timespec_incr(struct timespec *a, struct timespec const *b)
{
	struct timespec t = *a;
	timespec_add(&t, b, a);
}

static HOSTPING *hostping_head, *hostping_tail;
static size_t hostping_count;

void
hostping_add(char const *name, struct sockaddr *addr, socklen_t addrlen)
{
	HOSTPING *hp = emalloc(sizeof(*hp));
	
	memset(hp, 0, sizeof(*hp));
	hp->name = estrdup(name);
	hp->addr = emalloc(addrlen);
	memcpy(hp->addr, addr, addrlen);
	hp->addrlen =addrlen;
	pthread_mutex_init(&hp->mutex, NULL);

	hp->next = NULL;
	hp->prev = hostping_tail;
	if (hostping_tail)
		hostping_tail->next = hp;
	else
		hostping_head = hp;
	hostping_tail = hp;
	
	hostping_count++;
}

#define FOR_EACH_HOSTPING(hp) for (hp = hostping_head; hp; hp = hp->next)

static inline void
hostping_lock(HOSTPING *host)
{
	pthread_mutex_lock(&host->mutex);
}

static inline void
hostping_unlock(HOSTPING *host)
{
	pthread_mutex_unlock(&host->mutex);
}

static void
hostping_extract_stat(HOSTPING *host, struct host_stat *st)
{
	memcpy(&st->start_tv, &host->start_tv,
	       sizeof(host->stat_last.start_tv));
	memcpy(&st->stop_tv,
	       timeval_is_null(&host->recv_tv)
	         ? &host->xmit_tv : &host->recv_tv,
	       sizeof(host->stat_last.stop_tv));
	st->xmit_count = host->xmit_count;
	st->recv_count = host->recv_count;
	st->tmin       = host->tmin;            
	st->tmax       = host->tmax;
	if (host->recv_count > 0) {
		double total = host->recv_count; //FIXME: repeat count?
		double avg = host->tsum / total;
		double vari = host->tsumsq / total - avg * avg;
	
		st->avg    = avg;
		st->stddev = nsqrt(vari, 0.0005);
	}
}

static void
hostping_commit(HOSTPING *host)
{	
	if (host->stat_last.status != HOST_STAT_VALID) {
		hostping_extract_stat(host, &host->stat_last);
		host->stat_last.status = HOST_STAT_VALID;
	}
}

/* Reset runtime statistics counters */
static void
hostping_reset(HOSTPING *host)
{
	hostping_lock(host);
	if (host->xmit_count > 0)
		hostping_commit(host);
	host->xmit_count = 0;
	host->recv_count = 0;
	host->tmin = 999999999.0;
	host->tmax = 0;
	host->tsum = 0;
	host->tsumsq = 0;
	switch (host->stat_last.status) {
	case HOST_STAT_INIT:
		break;
	case HOST_STAT_VALID:
		host->stat_last.status = HOST_STAT_PENDING;
		break;
	case HOST_STAT_PENDING:
		host->stat_last.status = HOST_STAT_INVALID;
		break;
	case HOST_STAT_INVALID:
		break;
	}
	hostping_unlock(host);
}

static int
get_hostping_stat(HOSTPING *host, struct json_value **retval)
{
	struct json_value *obj, *v;
	struct host_stat const *st;
	struct host_stat stbuf;
	static char const *host_stat_status[] = {
		"init",
		"valid",
		"pending",
		"invalid"
	};
	int validity;
	double ts;

	hostping_lock(host);
	if (!(obj = json_new_object()))
		goto err;

	if (!(v = json_new_string(host->name))
	    || json_object_set(obj, "name", v))
		goto err;

	if (host_stat_is_valid(&host->stat_last)) {
		st = &host->stat_last;
		validity = 1;
	} else if (host->stat_last.status == HOST_STAT_INIT
		   && host->xmit_count > ping_tolerance) {
		hostping_extract_stat(host, &stbuf);
		stbuf.status = HOST_STAT_PENDING;
		st = &stbuf;
		validity = 1;
	} else {
		st = NULL;
		validity = 0;
	}

	if (!(v = json_new_bool(validity))
	    || json_object_set(obj, "validity", v))
		goto err;

	if (!(v = json_new_string(host_stat_status[host->stat_last.status]))
	    || json_object_set(obj, "status", v))
		goto err;	
	ts = timeval_to_double(&host->xmit_tv);
	if (!(v = json_new_number(ts))
	    || json_object_set(obj, "xmit-timestamp", v))
			goto err;

	if (st) {
		int is_alive = st->xmit_count - st->recv_count
			        < ping_tolerance;
	
		if (!(v = json_new_string(host_stat_status[st->status]))
		    || json_object_set(obj, "status", v))
			goto err;	

		ts = timeval_to_double(&st->start_tv);
		if (!(v = json_new_number(ts))
		    || json_object_set(obj, "start-timestamp", v))
			goto err;
		ts = timeval_to_double(&st->stop_tv);
		if (!(v = json_new_number(ts))
		    || json_object_set(obj, "stop-timestamp", v))
			goto err;
		if (!(v = json_new_bool(is_alive))
		    || json_object_set(obj, "alive", v))
			goto err;
		if (!(v = json_new_number(st->xmit_count))
		    || json_object_set(obj, "xmit", v))
			goto err;		
		if (!(v = json_new_number(st->recv_count))
		    || json_object_set(obj, "recv", v))
			goto err;
		if (!(v = json_new_number((double) 100
					  * (st->xmit_count - st->recv_count)
					  / st->xmit_count))
		    || json_object_set(obj, "loss", v))
			goto err;
		
		if (st->recv_count > 0) {
			if (!(v = json_new_number(st->tmin))
			    || json_object_set(obj, "tmin", v))
				goto err;
			if (!(v = json_new_number(st->tmax))
			    || json_object_set(obj, "tmax", v))
				goto err;
			if (!(v = json_new_number(st->avg))
			    || json_object_set(obj, "avg", v))
				goto err;
			if (!(v = json_new_number(st->stddev))
			    || json_object_set(obj, "stddev", v))
				goto err;
		}
	} else {
		if (!(v = json_new_string(host_stat_status[host->stat_last.status]))
		    || json_object_set(obj, "status", v))
		goto err;	
	}

	hostping_unlock(host);

	*retval = obj;
	return 0;
	
  err:
	hostping_unlock(host);
	error("out of memory when formatting statistics for %s", host->name);
	json_value_free(v);
	json_value_free(obj);

	return -1;
}

int
get_hostname_stat(char const *name, struct json_value **retval)
{
	HOSTPING *hp;

	FOR_EACH_HOSTPING(hp) {
		if (strcmp(hp->name, name) == 0)
			return get_hostping_stat(hp, retval);
	}
	*retval = NULL;
	return 0;
}

int
get_ipaddr_stat(struct sockaddr *sa, int salen, struct json_value **retval)
{
	HOSTPING *hp;

	FOR_EACH_HOSTPING(hp) {
		if (hp->addrlen == salen
		    && memcmp(hp->addr, sa, salen) == 0)
			return get_hostping_stat(hp, retval);
	}
	*retval = NULL;
	return 0;
}

int
get_all_host_stat(struct json_value **retval)
{
	struct json_value *ar;
	HOSTPING *hp;
	
	if (!(ar = json_new_array()))
		goto err;
	FOR_EACH_HOSTPING(hp) {
		struct json_value *v;
		if (get_hostping_stat(hp, &v))
			goto err;
		if (json_array_append(ar, v)) {
			json_value_free(v);
			goto err;
		}
	}
	*retval = ar;
	return 0;
  err:
	json_value_free(ar);
	return -1;
}

int 
get_host_matches(struct addrinfo **aip, struct json_value **retval)
{
	struct json_value *ar;
	HOSTPING *hp;
	struct addrinfo *ai_head = NULL, *ai_tail = NULL;
	struct addrinfo *ai = *aip;
	int ret = -1;
	
	if (!(ar = json_new_array()))
		goto err;
	FOR_EACH_HOSTPING(hp) {
		struct addrinfo *prev = NULL, *p;
		if (!ai)
			break;
		p = ai;
		while (p) {
			struct addrinfo *next = p->ai_next;
			if (p->ai_addrlen == hp->addrlen
			    && memcmp(hp->addr, p->ai_addr,
				      hp->addrlen) == 0) {
				struct json_value *jv;

				jv = json_new_string(hp->name);
				if (!jv)
					goto err;
				if (json_array_append(ar, jv)) {
					json_value_free(jv);
					goto err;
				}

				if (prev)
					prev->ai_next = next;
				else
					ai = next;

				p->ai_next = NULL;
				if (ai_tail)
					ai_tail->ai_next = p;
				else
					ai_head = p;
				ai_tail = p;
				
				break;
			} else {
				prev = p;
				p = next;
			}
		}
	}

	ret = 0;
	*retval = ar;
	ar = NULL;
err:	
	if (ai_tail) {
		ai_tail->ai_next = ai;
		*aip = ai_head;
	}
	json_value_free(ar);
	return ret;
}

static int ping_fd;

static pthread_mutex_t sendq_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t sendq_cond = PTHREAD_COND_INITIALIZER;
static int send_p;

static unsigned xmit_total;
static unsigned recv_total;

#define ICMP_HEADER_LEN (offsetof(struct icmp, icmp_data))
#define PING_DATALEN (64 - ICMP_HEADER_LEN)

size_t data_length = PING_DATALEN;
static unsigned char *data_buffer;

static int ping_ident;

#define MAX_PING_TIMEOUT 10
enum {
	MAX_SEQNO = USHRT_MAX,
	MOD_SEQNO = MAX_SEQNO + 1
};

struct seqidx {
	HOSTPING *host;
	struct timeval tv;
	int replied;
};

static struct seqidx *seqidx;
static unsigned short next_seqno;

static int
seqno_alloc(HOSTPING *addr, struct timeval *tv)
{
	int n = next_seqno;
	do {
		if (tv->tv_sec - seqidx[n].tv.tv_sec > MAX_PING_TIMEOUT) {
			memcpy(&seqidx[n].tv, tv, sizeof(*tv));
			seqidx[n].host = addr;
			seqidx[n].replied = 0;
			next_seqno = (n + 1) % MOD_SEQNO;
			return n;
		}
		n = (n + 1) % MOD_SEQNO;
	} while (n != next_seqno);
	error("no free sequence numbers");
	return -1;
}

static HOSTPING *
hostping_from_seqno(int seq)
{
	HOSTPING *host = seqidx[seq].host;
	if (seqidx[seq].replied)
		info("%s:%d: DUP!", host->name, seq);
	seqidx[seq].replied++;
	hostping_lock(host);
	return host;
}

void
p903_init(void)
{
        struct protoent *proto;
	int i;
	
	if (hostping_count == 0) {
		fatal("no host IPs configured, nothing to do?");
		exit(1);
	}

	proto = getprotobyname("icmp");
	if (!proto) {
		fatal("no entry for icmp in the system protocol database");
		exit(1);
	}
        ping_fd = socket(AF_INET, SOCK_RAW, proto->p_proto);
        if (ping_fd < 0) {
                fatal("can't create ICMP socket: %s", strerror(errno));
		exit(1);
        }

	ping_ident = getpid() & 0xffff;
	
	data_buffer = emalloc(data_length);
	for (i = 0; i < data_length; i++)
		data_buffer[i] = i;

	seqidx = calloc(MAX_SEQNO + 1, sizeof(seqidx[0]));
	if (!seqidx)
		emalloc_die();	
}

static unsigned short
icmp_cksum(unsigned char * addr, int len)
{
	register int sum = 0;
	unsigned short answer = 0;
	unsigned short *wp;

	for (wp = (unsigned short *) addr; len > 1; wp++, len -= 2)
		sum += *wp;

	/* Take in an odd byte if present */
	if (len == 1) {
		*(unsigned char *) & answer = *(unsigned char *) wp;
		sum += answer;
	}

	sum = (sum >> 16) + (sum & 0xffff);	/* add high 16 to low 16 */
	sum += (sum >> 16);		/* add carry */
	answer = ~sum;		/* truncate to 16 bits */
	return answer;
}

static int
icmp_generic_encode(unsigned char * buffer, size_t bufsize, int type,
		    int ident, int seqno)
{
	struct icmp *icmp;
	
	if (bufsize < ICMP_MINLEN)
		return -1;
	icmp = (struct icmp *) buffer;
	icmp->icmp_type = type;
	icmp->icmp_code = 0;
	icmp->icmp_cksum = 0;
	icmp->icmp_seq = htons(seqno);
	icmp->icmp_id = htons(ident);
	
	icmp->icmp_cksum = icmp_cksum(buffer, bufsize);
	return 0;
}

static int
icmp_generic_decode(unsigned char * buffer, size_t bufsize,
		    struct ip **ipp, struct icmp ** icmpp)
{
	size_t hlen;
	unsigned short cksum;
	struct ip *ip;
	struct icmp *icmp;
	
	/* IP header */
	ip = (struct ip *) buffer;
	hlen = ip->ip_hl << 2;
	if (bufsize < hlen + ICMP_MINLEN)
		return -1;

	/* ICMP header */
	icmp = (struct icmp *) (buffer + hlen);

	/* Prepare return values */
	*ipp = ip;
	*icmpp = icmp;

	/* Recompute checksum */
	cksum = icmp->icmp_cksum;
	icmp->icmp_cksum = 0;
	icmp->icmp_cksum = icmp_cksum((unsigned char *) icmp, bufsize - hlen);
	if (icmp->icmp_cksum != cksum)
		return 1;
	icmp->icmp_seq = ntohs(icmp->icmp_seq);
	icmp->icmp_id = ntohs(icmp->icmp_id);
	
	return 0;
}

static void
send_echo(HOSTPING *host, unsigned char *ping_buffer)
{
	struct icmp *icmp = (struct icmp *) ping_buffer;
	size_t buflen;
	ssize_t n;
	int seqno;

	hostping_lock(host);
	gettimeofday(&host->xmit_tv, NULL);
	hostping_unlock(host);
	
	memcpy(icmp->icmp_data, &host->xmit_tv, sizeof(host->xmit_tv));
	if (data_buffer)
		memcpy(icmp->icmp_data + sizeof(host->xmit_tv), data_buffer,
		       data_length - sizeof(host->xmit_tv));

	buflen = ICMP_HEADER_LEN + data_length;

	seqno = seqno_alloc(host, &host->xmit_tv);
	if (seqno == -1) {
		//FIXME
		return;
	}
	
	if (verbose > 2)
		info("sending %d bytes to %s, icmp_seq=%d", buflen, host->name,
		     seqno);
	icmp_generic_encode(ping_buffer, buflen, ICMP_ECHO, ping_ident, seqno);

	n = sendto(ping_fd, (char *) ping_buffer, buflen, 0,
		   host->addr, host->addrlen);
	if (n < 0) {
		error("%s: sendto: %s", host->name, strerror(errno));
	} else {
		hostping_lock(host);
		if (host->xmit_count == 0)
			host->start_tv = host->xmit_tv;
		host->xmit_count++;
		hostping_unlock(host);		
		xmit_total++;
		if (n != buflen)
			error("ping: wrote %s %d chars, ret=%d\n",
			      host->name, buflen, n);
	}
}

static void
start_probe(void)
{
	pthread_mutex_lock(&sendq_mutex);
	send_p = 1;
	pthread_cond_broadcast(&sendq_cond);
	pthread_mutex_unlock(&sendq_mutex);
}

static void hostping_commit(HOSTPING *host);

void *
p903_sender(void *p)
{
	size_t i;
	unsigned char *ping_buffer;
	struct pollfd pfd;
	int n;
	unsigned send_count = 0;
	double d;
	struct timespec delay;
	
	pfd.fd = ping_fd;
	pfd.events = POLLOUT;
	
	ping_buffer = emalloc(sizeof(struct icmp) + data_length);

	d = (double) ping_interval / hostping_count;
	delay.tv_sec = (int) d;
	delay.tv_nsec = (d - delay.tv_sec) * NANOSEC_IN_SEC;

	pthread_mutex_lock(&sendq_mutex);
	while (1) {
		struct timespec ts;
		HOSTPING *hp;
		
		if (!send_p) {
			while (!send_p)
				pthread_cond_wait(&sendq_cond, &sendq_mutex);
			send_count = 0;
		}
		clock_gettime(CLOCK_MONOTONIC, &ts);
		FOR_EACH_HOSTPING(hp) {
			struct timespec nts;
			clock_gettime(CLOCK_MONOTONIC, &nts);
			timespec_incr(&nts, &delay);
			n = poll(&pfd, 1, -1);
			if (n == 1) {
				send_echo(hp, ping_buffer);
			} else if (n == -1) {
				fatal("poll: %s", strerror(errno));
				exit(1);
			}
			if (i < hostping_count - 1)
				clock_nanosleep(CLOCK_MONOTONIC,
						TIMER_ABSTIME, &nts,
						NULL);
		}
		send_count++;
		if (send_count == ping_count)
			send_p = 0;
		else {
			ts.tv_sec += ping_interval;
			clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts,
					NULL);
		}
	}
	pthread_mutex_unlock(&sendq_mutex);
	return NULL;
}

static void
log_echo(struct sockaddr *addr,	socklen_t addrlen,
	 struct icmp *icmp, struct ip *ip,
	 size_t datalen, double rtt)
{
	char hbuf[NI_MAXHOST];

	datalen -= ip->ip_hl << 2;
	if (getnameinfo(addr, addrlen, hbuf, sizeof(hbuf),
			NULL, 0, NI_NUMERICHOST)) {
		strcpy(hbuf, "UNKNOWN");
	}
	
	info("%d bytes from %s: icmp_seq=%u ttl=%d time=%.3f ms", datalen,
	     hbuf, icmp->icmp_seq, ip->ip_ttl, rtt);
}

void *
p903_receiver(void *p)
{
	size_t buflen = sizeof(struct icmp) + data_length;
	unsigned char *ping_buffer;

	ping_buffer = emalloc(buflen);
	
	while (1) {
		struct sockaddr_storage addr;
		socklen_t addrlen = sizeof(addr);
		ssize_t n;
		int rc;
		struct icmp *icmp;
		struct ip *ip;
		HOSTPING *host;

		n = recvfrom(ping_fd, ping_buffer, buflen, 0,
			     (struct sockaddr *) &addr, &addrlen);
		if (n < 0) {
			error("recvfrom: %s", strerror(errno));
			continue;
		}

		rc = icmp_generic_decode(ping_buffer, n, &ip, &icmp);
		if (rc < 0) {
			char hbuf[NI_MAXHOST];
			if (getnameinfo((struct sockaddr *) &addr,
					addrlen,
					hbuf, sizeof(hbuf),
					NULL, 0, NI_NUMERICHOST)) {
				error("packet too short (%d bytes)", n);
			} else {
				error("packet too short (%d bytes), from %s",
				      n, hbuf);
			}
			continue;
		}

		if (icmp->icmp_type != ICMP_ECHOREPLY
		    || icmp->icmp_id != ping_ident)
			continue;

		recv_total++;

		if (rc) {
			char hbuf[NI_MAXHOST];
			if (getnameinfo((struct sockaddr *) &addr,
					addrlen,
					hbuf, sizeof(hbuf),
					NULL, 0, NI_NUMERICHOST)) {
				error("checksum mismatch");
			} else {
				error("checksum mismatch from %s", hbuf);
			}
			continue;
		}

		host = hostping_from_seqno(icmp->icmp_seq);
		if (host) {
			struct timeval tv_orig, tv_diff, *tp;
			double rtt;

			gettimeofday(&host->recv_tv, NULL);
			
			tp = (struct timeval *) icmp->icmp_data;

			/* Avoid unaligned data: */
			memcpy(&tv_orig, tp, sizeof (tv_orig));
			timersub(&host->recv_tv, &tv_orig, &tv_diff);

			rtt = timeval_to_double(&tv_diff) * 1000.0;
			host->tsum += rtt;
			host->tsumsq += rtt * rtt;
			if (rtt < host->tmin)
				host->tmin = rtt;
			if (rtt > host->tmax)
				host->tmax = rtt;
			
			host->recv_count++;

			if (verbose > 1)
				log_echo((struct sockaddr *)&addr, addrlen,
					 icmp, ip, n, rtt);
			if (host->recv_count == ping_count)
				hostping_commit(host);
			hostping_unlock(host);
		} else
			fatal("no host found for sequence number %d",
			      icmp->icmp_seq);
	}
}

void *
p903_scheduler(void *p)
{
	while (1) {
		HOSTPING *hp;

		/* Reset all statistics */
		FOR_EACH_HOSTPING(hp) {
			hostping_reset(hp);
		}
		start_probe();
		sleep(probe_interval);
		if (verbose)
			info("total sent=%u, received=%u",
			     xmit_total, recv_total);
		xmit_total = recv_total = 0;
	}
}