/*
 *  lattest  by Davide Libenzi ( linux kernel scheduler latency tester )
 *  Version 0.17 - Copyright (C) 2001, 2003  Davide Libenzi
 *
 *  This program 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 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program 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 this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  Davide Libenzi <davidel@xmailserver.org>
 *
 *
 *  The purpose of this tool is to measure the real time tasks latency.
 *  Build:
 *
 *  gcc -o lattest lattest.c -lrt
 *
 *  Use:
 *
 *  lattest [--test-stime s] [--sleep-mstime ms] [--pause-mstime ms] [--priority p]
 *	    [--sched-fifo] [--sched-rr] [-- cmdpath [arg] ...]
 *
 *  --test-stime   = Set the test time in seconds
 *  --sleep-mstime = Set the sleep time in milliseconds
 *  --pause-mstime = Set the pause time in milliseconds
 *  --priority	   = Set the real time task priority ( 0 for OTHER, 1..99 for RT )
 *  --sched-other  = Set the real time task policy to OTHER
 *  --sched-fifo   = Set the real time task policy to FIFO
 *  --sched-rr	   = Set the real time task policy to RR
 *  --sched-softrr = Set the real time task policy to SOFTRR
 *  --		   = Separate the optional command to be executed during the test time
 *  cmdpath	   = Command to be executed
 *  arg		   = Command argouments
 *
 */


#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <signal.h>
#include <sched.h>
#include <sys/ioctl.h>
#include <sys/mman.h>


#define STD_SLEEP_TIME	4
#define PAUSE_SLEEP_TIME	200
#define STD_TEST_TIME	8

#if !defined(SCHED_SOFTRR)
#define SCHED_SOFTRR 3
#endif

 
#define rdtscll(val) \
     __asm__ __volatile__("rdtsc" : "=A" (val))


typedef unsigned long long cycles_t;


static volatile int stop_test = 0;


static inline cycles_t get_cycles (void)
{
	unsigned long long ret;
 
	rdtscll(ret);
	return ret;
}


void sig_int(int sig)
{
	++stop_test;
	signal(sig, sig_int);
}


int main(int argc, char *argv[]) {
	int i, icmd, pausetime = PAUSE_SLEEP_TIME, testtime = STD_TEST_TIME,
		policy = SCHED_RR, priority = 1, sleeptime = STD_SLEEP_TIME, numsamples;
	pid_t expid = -1;
	cycles_t cys, cye, totlat = 0, cylat = 0, mscycles;
	cycles_t *samples;
	struct sched_param sp;
	struct timespec ts1, ts2;

	for (i = 1; i < argc; i++) {
		if (strcmp(argv[i], "--test-stime") == 0) {
			if (++i < argc)
				testtime = atoi(argv[i]);
			continue;
		}
		if (strcmp(argv[i], "--sleep-mstime") == 0) {
			if (++i < argc)
				sleeptime = atoi(argv[i]);
			continue;
		}
		if (strcmp(argv[i], "--pause-mstime") == 0) {
			if (++i < argc)
				pausetime = atoi(argv[i]);
			continue;
		}
		if (strcmp(argv[i], "--priority") == 0) {
			if (++i < argc)
				priority = atoi(argv[i]);
			continue;
		}
		if (strcmp(argv[i], "--sched-other") == 0) {
			policy = SCHED_OTHER;
			priority = 0;
			continue;
		}
		if (strcmp(argv[i], "--sched-fifo") == 0) {
			policy = SCHED_FIFO;
			continue;
		}
		if (strcmp(argv[i], "--sched-rr") == 0) {
			policy = SCHED_RR;
			continue;
		}
		if (strcmp(argv[i], "--sched-softrr") == 0) {
			policy = SCHED_SOFTRR;
			continue;
		}
		if (strcmp(argv[i], "--") == 0) {
			icmd = ++i;
			break;
		}
	}

	numsamples = (testtime * 1000) / pausetime + 1;
	if (!(samples = (cycles_t *) malloc(numsamples * sizeof(cycles_t)))) {
		perror("malloc");
		return 1;
	}

	if (icmd > 0 && icmd < argc) {
		expid = fork();
		if (expid == -1) {
			perror("fork");
			return 5;
		} else if (expid == 0) {
			setpgid(0, getpid());
			execv(argv[icmd], &argv[icmd]);
			exit(0);
		}
	}

	memset(&sp, 0, sizeof(sp));
	sp.sched_priority = priority;

	if (sched_setscheduler(0, policy, &sp)) {
		perror("sched_setscheduler");
		if (expid > 0 && kill(-expid, SIGKILL))
			perror("SIGKILL");
		return 4;
	}

	signal(SIGINT, sig_int);

	clock_getres(CLOCK_REALTIME, &ts1);
	fprintf(stderr, "timeres=%ld\n", ts1.tv_nsec / 1000);

	clock_gettime(CLOCK_REALTIME, &ts1);
	cys = get_cycles();
	sleep(1);
	clock_gettime(CLOCK_REALTIME, &ts2);
	cye = get_cycles();
	mscycles = (cye - cys) / ((ts2.tv_sec - ts1.tv_sec) * 1000 + (ts2.tv_nsec - ts1.tv_nsec) / 1000000);
	fprintf(stderr, "mscycles=%llu\n", mscycles);

	for (i = 0; i < numsamples && !stop_test; i++) {
		ts1.tv_sec = 0;
		ts1.tv_nsec = sleeptime * 1000000;

		cys = get_cycles();
		clock_nanosleep(CLOCK_REALTIME, 0, &ts1, &ts2);
		cye = get_cycles();

		samples[i] = (cye - cys) / mscycles;
		totlat += samples[i];
		if (samples[i] > cylat)
			cylat = samples[i];

		usleep(pausetime * 1000);
	}

	numsamples = i;

	memset(&sp, 0, sizeof(sp));
	sp.sched_priority = 0;

	if (sched_setscheduler(0, SCHED_OTHER, &sp)) {
		perror("sched_setscheduler");
		if (expid > 0 && kill(-expid, SIGKILL))
			perror("SIGKILL");
		return 6;
	}

	if (expid > 0 && kill(-expid, SIGKILL))
		perror("SIGKILL");

	for (i = 0; i < numsamples; i++) {

	}

	fprintf(stdout, "maxlat=%llu\n", cylat);
	if (numsamples)
		fprintf(stdout, "avglat=%llu\n", totlat / numsamples);

	return 0;
}