/* rt-benchmark.c */
#include <time.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>

void timespec_print(const char * name, struct timespec t){
	printf("%s: %lu.%09lu\n", name, t.tv_sec, t.tv_nsec);
}

struct timespec timespec_scan(const char * s){
	struct timespec ret = {0, 0};
	while(*s >= '0' && *s <= '9'){
		ret.tv_sec *= 10;
		ret.tv_sec += *s - '0';
		s++;
	}
	if(*s != '.') return ret;
	s++;
	int radix = 1000000000;
	while(*s >= '0' && *s <= '9'){
		radix /= 10;
		ret.tv_nsec += (*s - '0') * radix;
		s++;
	}
	return ret;
}

inline struct timespec timespec_add(struct timespec a, struct timespec b){
	struct timespec sum;
	sum.tv_nsec = a.tv_nsec + b.tv_nsec;
	sum.tv_sec  = a.tv_sec  + b.tv_sec;
	if(sum.tv_nsec >= 1000000000){
		sum.tv_nsec -= 1000000000;
		sum.tv_sec  += 1;
	}
	return sum;
}

void selfish(){
	struct sched_param param;
	memset(&param, 0, sizeof(param));
	param.sched_priority = sched_get_priority_max(SCHED_FIFO);
	if(sched_setscheduler(0, SCHED_FIFO, &param))
		perror("Failed to set max scheduling priority with FIFO policy");
	if(mlockall(MCL_CURRENT | MCL_FUTURE))
		perror("Failed to lock pages in RAM");
}

struct realtime{
	struct timespec schedule;
	struct timespec interval;
	struct timespec deadline;
};

void menu(struct realtime * info, char *args[]){
	while(*(++args)){/* C99: argv is NULL terminated */
		char *arg = (*args) + 1;
		switch(arg[-1]){
			case 't': info->interval = timespec_scan(arg); break;
			case 'd': info->deadline = timespec_scan(arg); break;
			case '-': puts(
					"rt-benchmark: Behave like a light real-time program.\n"
					"\n\tOPTIONS:\n"
					"tNUMBER  Specify interval in seconds, decimal notation\n"
					"dNUMBER  Specify deadline in seconds, decimal notation\n"
				); exit(0);
		}
	}

	timespec_print("interval", info->interval);
	timespec_print("deadline", info->deadline);
}

void rtsleep(struct timespec * schedule){
	int e;
	while((e=clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, schedule, schedule)) != 0){
		fprintf(stderr, "Sleep interrupted: %s\n", strerror(e));
	}
}

void periodictask(struct realtime * info){
	struct timespec now;       clock_gettime(CLOCK_MONOTONIC, &now);
	struct timespec deadline = timespec_add(info->schedule, info->deadline);
	if(
		now.tv_sec > deadline.tv_sec || (
			now.tv_sec == deadline.tv_sec &&
			now.tv_nsec > deadline.tv_nsec
		)
	){
		fprintf(stderr, "Deadline %lu.%09lu missed! ", deadline.tv_sec, deadline.tv_nsec);
	}
	timespec_print("time", now);
}

int main(int argc, char* argv[]){
	struct realtime info;
	info.interval.tv_sec  = 1;
	info.interval.tv_nsec = 0;
	info.deadline.tv_sec  = 1;
	info.deadline.tv_nsec = 0;
	if(clock_gettime(CLOCK_MONOTONIC, &(info.schedule))){
		perror("gettime");
		exit(EXIT_FAILURE);
	}
	menu(&info, argv);
	selfish();

	for(;;){
		periodictask(&info);
		info.schedule = timespec_add(info.schedule, info.interval);
#ifndef CONFIG_CHILDISH
		rtsleep(&(info.schedule));
#else
		sleep(info.interval.tv_sec);
		usleep(info.interval.tv_nsec);
#endif /* CONFIG_CHILDISH */
	}
}