main.c

#include <fcntl.h>
#include <poll.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <sys/ipc.h>
#include <sys/mman.h>
#include <sys/shm.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>

#include "error.h"
#include "gpio-int-test.h"

#ifdef __GNUG__
	#define likely(x)       __builtin_expect((x), 1)
	#define unlikely(x)     __builtin_expect((x), 0)
#else
	#define likely(x) (x)
	#define unlikely(x) (x)
#endif

#define NTP_KEY	1314148400
#define DEFAULT_FUDGE_FACTOR 0.0

#define BILLION 1000000000

#define BOOL char
#define FALSE 0
#define TRUE 1

char debug = 0;

struct shmTime {
	int    mode; /* 0 - if valid set
		      *       use values, 
		      *       clear valid
		      * 1 - if valid set 
		      *       if count before and after read of 
		      *       values is equal,
		      *         use values 
		      *       clear valid
		      */
	int    count;
	time_t clockTimeStampSec;      /* external clock */
	int    clockTimeStampUSec;     /* external clock */
	time_t receiveTimeStampSec;    /* internal clock, when external value was received */
	int    receiveTimeStampUSec;   /* internal clock, when external value was received */
	int    leap;
	int    precision;
	int    nsamples;
	int    valid;
	int    dummy[10]; 
};

int write_pidfile(const char *const fname)
{
	FILE *fh = fopen(fname, "w");
	if (!fh)
		error_exit("write_pidfile: failed creating file %s", fname);

	fprintf(fh, "%d", getpid());

	fclose(fh);

	return 0;
}

struct shmTime * get_shm_pointer(const int unitNr)
{
	void *addr = NULL;
	struct shmTime *pst = NULL;
	int shmid = shmget(NTP_KEY + unitNr, sizeof(struct shmTime), IPC_CREAT);
	if (shmid == -1)
		error_exit("get_shm_pointer: shmget failed");

	addr = shmat(shmid, NULL, 0);
	if (addr == (void *)-1)
		error_exit("get_shm_pointer: shmat failed");

	pst = (struct shmTime *)addr;

	return pst;
}

void notify_ntp(struct shmTime *const pst, int *fudge_s, int *fudge_ns, struct timespec *const ts, long int *wrap, const int rebase)
{
	pst -> valid = 0;

	static int rebase_count = 0;

	if (rebase > rebase_count)
	{
		static long int rebase_total = 0.0;

		rebase_total += ts -> tv_nsec;

		if (++rebase_count == rebase)
		{
			rebase_total /= rebase_count;

			printf("rebasing to %ldns\n", rebase_total);

			*fudge_s = -(rebase_total / BILLION);
			*fudge_ns = -(rebase_total % BILLION);
		}
	}
	else
	{
		/* apply fudge */
		ts -> tv_sec += *fudge_s;
		ts -> tv_nsec += *fudge_ns;

		if (ts -> tv_nsec > BILLION - 1)
		{
			ts -> tv_nsec -= BILLION;
			ts -> tv_sec++;
		}

		/* if local time is more than 0.5 seconds off, assume
		 * it is the next second
		 */
		pst -> receiveTimeStampSec = ts -> tv_sec;
		pst -> receiveTimeStampUSec = ts -> tv_nsec / 1000;

		if (ts -> tv_nsec >= BILLION / 2)
		{
			ts -> tv_sec++;
			(*wrap)++;
		}

		pst -> clockTimeStampSec = ts -> tv_sec;
		pst -> clockTimeStampUSec = 0;

		pst -> leap = pst -> mode = pst -> count = /* */
		pst -> precision = 0;	/* 0 = precision of 1 sec., -1 = 0.5s */

		pst -> valid = 1;
	}
}

int get_value(const int fd)
{
	static char c[32];

	if (read(fd, c, sizeof c) == -1)
		error_exit("read failed");

	if (lseek(fd, 0, SEEK_SET) == -1)
		error_exit("select failed");

	return c[0];
}

void wait_for_state(const int fd, const int what)
{
	int value = 0;

	do
	{
		value = get_value(fd);
	}
	while(value != what);
}

void sleep_for_offset(const double idle_factor)
{
	struct timespec ts;
	if (unlikely(clock_gettime(CLOCK_REALTIME, &ts) == -1))
		error_exit("clock_gettime(CLOCK_REALTIME) failed");

	long offset = ts.tv_nsec;
	long next_int = BILLION - offset;
	usleep((long)((double)next_int * idle_factor) / 1000);
}

void debug_log(const struct timespec *const ts, const long int wrap_count)
{
	static long int total_count = 0, min_count = 0, max_count = 0;
	static double min_avg = 0, avg_avg = 0, max_avg = 0;

	if (debug)
	{
		double offset = (double)((ts -> tv_nsec >= BILLION / 2) ? -(BILLION - ts -> tv_nsec) : ts -> tv_nsec) / (double)BILLION; 

		if (total_count)
		{
			double cur_avg = avg_avg / (double)total_count;

			if (offset < cur_avg)
			{
				min_avg += offset;
				min_count++;
			}
			else
			{
				max_avg += offset;
				max_count++;
			}
		}

		avg_avg += offset;
		total_count++;

		printf("%ld.%09ld] interrupt #%ld, %ld wraps, offset %fs %f/%f/%f\n", ts -> tv_sec, ts -> tv_nsec, total_count, wrap_count, offset, min_avg/(double)min_count, avg_avg/(double)total_count, max_avg/(double)max_count);
	}
}

void pulse_pin(const int pin, int *const memory)
{
	if (pin != -1)
	{
		gpio_set_value(pin, *memory);

		*memory = !*memory;
	}
}

void polling_driven(struct shmTime *const pst, int fudge_s, int fudge_ns, const int gpio_pps_in_fd, const int gpio_pps_out_pin, const double idle_factor, int rebase)
{
	long int wrap_count = 0;
	struct timespec ts = { 0, 0 };
	char first = 1;
	int pulse_out_value = 0;

	for(;;)
	{
		// wait for high
		wait_for_state(gpio_pps_in_fd, '1');

		if (unlikely(clock_gettime(CLOCK_REALTIME, &ts) == -1))
			error_exit("clock_gettime(CLOCK_REALTIME) failed");

		pulse_pin(gpio_pps_out_pin, &pulse_out_value);

		// register offset at ntp
		if (unlikely(first))
			first = 0;
		else
		{
			notify_ntp(pst, &fudge_s, &fudge_ns, &ts, &wrap_count, rebase);
		}

		debug_log(&ts, wrap_count);

		sleep_for_offset(idle_factor);
	}
}

void interrupt_driven(struct shmTime *const pst, int fudge_s, int fudge_ns, const char edge_both, const int gpio_pps_in_fd, const int gpio_pps_out_pin, const int rebase)
{
	struct timespec ts = { 0, 0 };
	char dummy = 0;
	int value = 0, gpio_pps_out_pin_value = 0;
	struct pollfd fdset[1];
	char buffer[64];
	long int wrap_count = 0;

	for(;;)
	{
		lseek(gpio_pps_in_fd, 0, SEEK_SET);

		/* clean-up interrupt flag */
		/* FIXME do using nonblocking fd to prevent blocks when bug(?) in gpio implementation */
		read(gpio_pps_in_fd, &dummy, 1);

		fdset[0].fd = gpio_pps_in_fd;
		fdset[0].events = POLLPRI;
		fdset[0].revents = 0;

		if (poll(fdset, 1, -1) <= 0)
			error_exit("poll() failed");

		if (likely(fdset[0].revents & POLLPRI))
		{
			/* see what time the local system thinks it is, ASAP */
			if (unlikely(clock_gettime(CLOCK_REALTIME, &ts) == -1))
				error_exit("clock_gettime(CLOCK_REALTIME) failed");

			if (edge_both)
			{
				value = get_value(gpio_pps_in_fd);

				if (value == '0')
					continue;
			}

			notify_ntp(pst, &fudge_s, &fudge_ns, &ts, &wrap_count, rebase);

			pulse_pin(gpio_pps_out_pin, &gpio_pps_out_pin_value);

			(void)read(fdset[0].fd, buffer, sizeof buffer);

			debug_log(&ts, wrap_count);
		}
	}
}

void lock_in_memory(void)
{
	if (mlockall(MCL_CURRENT) == -1)
		error_exit("mlockall(MCL_CURRENT)");
}

void help(void)
{
	fprintf(stderr, "-N x    x must be 0...3, it is the NTP shared memory unit number\n");
	fprintf(stderr, "-g x    gpio pin to listen on\n");
	fprintf(stderr, "-G x    explicit path to the gpio-pin-path, for special cases like the cubieboard1 (/sys.../gpio1_pg9 instead of /sys.../gpio1). Note: you need to \"export\" and configure the pin in this use-case by hand.\n");
	fprintf(stderr, "-d      debug mode\n");
	fprintf(stderr, "-F x    fudge factor (in microseconds)\n");
	fprintf(stderr, "-p x    when enabled, toggle GPIO pin x so that you can measure delays using a scope\n");
	fprintf(stderr, "-f      do not fork\n");
	fprintf(stderr, "-b      handle both on rising/falling but ignore falling\n");
	fprintf(stderr, "-P      use polling - for when the device does not support interrupts on gpio state changes\n");
	fprintf(stderr, "-i x    polling: how long shall we sleep (part of a second) and not poll for interrupts. e.g. 0.95\n");
	fprintf(stderr, "-R x    re-base: measure 'x' times the offset, then take the average and then use that as an offset. this can be useful when using e.g. a tcxo or an other non-synced pulse-source\n");
}

int main(int argc, char *argv[])
{
	struct shmTime *pst = NULL;
	int fudge_s = 0, fudge_ns = 0;
	int unit = 0; /* 0...3 */
	int gpio_pps_in_pin = -1, gpio_pps_out_pin = -1;
	const char *gpio_pps_in_pin_path = NULL;
	int gpio_pps_in_fd = -1;
	char do_fork = 1, gpio_pps_out_pin_value = 1;
	int c = -1;
	char edge_both = 0, polling = 0;
	double idle_factor = 0.95;
	int rebase = -1;

	printf("rpi_gpio_ntp v" VERSION ", (C) 2013-2015 by folkert@vanheusden.com\n\n");

	while((c = getopt(argc, argv, "R:G:i:bp:fN:g:F:dPh")) != -1)
	{
		switch(c)
		{
			case 'R':
				rebase = atoi(optarg);
				break;

			case 'G':
				gpio_pps_in_pin_path = optarg;
				break;

			case 'P':
				polling = 1;
				break;

			case 'i':
				idle_factor = atof(optarg);
				break;

			case 'b':
				edge_both = 1;
				break;

			case 'p':
				gpio_pps_out_pin = atoi(optarg);
				break;

			case 'f':
				do_fork = 0;
				break;

			case 'N':
				unit = atoi(optarg);
				break;

			case 'g':
				gpio_pps_in_pin = atoi(optarg);
				break;

			case 'F':
				fudge_s = atol(optarg) / 1000000;
				fudge_ns = (atol(optarg) % 1000000) * 1000;
				break;

			case 'd':
				debug = 1;
				break;

			case 'h':
				help();
				return 0;

			default:
				error_exit("%c is an invalid switch", c);
		}
	}

	if (gpio_pps_in_pin == -1 && gpio_pps_in_pin_path == NULL)
		error_exit("You need to select a GPIO pin to \"listen\" on.");

	if (gpio_pps_out_pin == gpio_pps_in_pin && gpio_pps_in_pin_path == NULL)
		error_exit("You can't use the same pin for both in- and output.");

	if (debug)
	{
		printf("NTP unit : %d\n", unit);
		if (gpio_pps_in_pin_path)
			printf("GPIO pin : %s\n", gpio_pps_in_pin_path);
		else
			printf("GPIO pin : %d\n", gpio_pps_in_pin);
		printf("GPIO pout: %d\n", gpio_pps_out_pin);
		printf("Fudge    : %d.%09d\n", fudge_s, fudge_ns);

		if (polling)
			printf("Polling mode(!)\n");

		if (do_fork)
		{
			do_fork = 0;

			printf("\"Fork into the background\" disabled because of debug mode.\n");
		}
	}

	lock_in_memory();

	/* connect to ntp */
	pst = get_shm_pointer(unit);

	if (gpio_pps_in_pin_path == NULL)
	{
		/* setup gpio */
		gpio_export(gpio_pps_in_pin);
		gpio_set_dir(gpio_pps_in_pin, 0);

		int rc = 0;
		if (edge_both)
			rc = gpio_set_edge(gpio_pps_in_pin, "both\n");
		else
			rc = gpio_set_edge(gpio_pps_in_pin, "rising\n");

		if (rc == -1)
			fprintf(stderr, "Failed to set direction: on the gl-inet this is normally. On other platforms this may be a problem. Continuing(!)\n");
	}

	if (gpio_pps_out_pin != -1)
	{
		gpio_export(gpio_pps_out_pin);
		gpio_set_dir(gpio_pps_out_pin, 1);
	}

	if (gpio_pps_in_pin_path)
	{
		int len = strlen(gpio_pps_in_pin_path);

		if (len < 6 || strcmp(&gpio_pps_in_pin_path[len - 6], "/value") != 0)
		{
			char *buffer = NULL;
			asprintf(&buffer, "%s/value", gpio_pps_in_pin_path);
			gpio_pps_in_fd = open(buffer, O_RDWR);
			free(buffer);
		}
		else
		{
			gpio_pps_in_fd = open(gpio_pps_in_pin_path, O_RDWR);
		}

		if (gpio_pps_in_fd == -1)
			error_exit("Failed opening GPIO in-pin. Make sure you use the complete path to the \"value\"-file, e.g.: /sys/class/gpio/gpio1_pg9/value\n");
	}
	else
	{
		gpio_pps_in_fd = gpio_fd_open(gpio_pps_in_pin);
	}

	if (do_fork && daemon(0, 0) == -1)
		error_exit("daemon() failed");

	if (polling)
		polling_driven(pst, fudge_s, fudge_ns, gpio_pps_in_fd, gpio_pps_out_pin, idle_factor, rebase);
	else
		interrupt_driven(pst, fudge_s, fudge_ns, edge_both, gpio_pps_in_fd, gpio_pps_out_pin, rebase);

	return 0;
}