/*
 *  rtlats  by Davide Libenzi ( linux kernel scheduler rt latency sampler )
 *  Version 0.17 - Copyright (C) 2001  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 scheduler latency for
 *  real time tasks using the "latsched" kernel patch.
 *  Build:
 *
 *  gcc -o rtlats rtlats.c
 *
 *  Use:
 *
 *  rtlats [--samples n] [--sleep-mstime ms] [--priority p]
 *         [--sched-fifo] [--sched-rr] [-- cmdpath [arg] ...]
 *
 *  --samples      = Set the size of the sample buffer
 *  --sleep-mstime = Set the sleep time in milliseconds
 *  --priority     = Set the real time task priority ( 1..99 )
 *  --sched-fifo   = Set the real time task policy to FIFO
 *  --sched-rr     = Set the real time task policy to RR
 *  --             = 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 <sched.h>
#include <asm/page.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <linux/latsched.h>



#define TEST_NUM_SAMPLES	4000
#define SLEEP_SYNC_TIME		1500000
#define STD_SLEEP_TIME		(5 * 1000000)



int main(int argc, char *argv[]) {
	int ii, res, lsfd, cpu, ncpus, icmd, samples = TEST_NUM_SAMPLES,
		policy = SCHED_FIFO, priority = 1;
	int sleeptime = STD_SLEEP_TIME;
	pid_t expid = -1;
	unsigned long long scycl = 0, rtlat = 0;
	char const *lsfile = "/dev/latsched";
	struct sched_param sp;
	struct lsctl_getdata lsgd;

	for (ii = 1; ii < argc; ii++) {
		if (strcmp(argv[ii], "--samples") == 0) {
			if (++ii < argc)
				samples = atoi(argv[ii]);
			continue;
		}
		if (strcmp(argv[ii], "--sleep-mstime") == 0) {
			if (++ii < argc)
				sleeptime = atoi(argv[ii]) * 1000;
			continue;
		}
		if (strcmp(argv[ii], "--priority") == 0) {
			if (++ii < argc)
				priority = atoi(argv[ii]);
			continue;
		}
		if (strcmp(argv[ii], "--sched-fifo") == 0) {
			policy = SCHED_FIFO;
			continue;
		}
		if (strcmp(argv[ii], "--sched-rr") == 0) {
			policy = SCHED_RR;
			continue;
		}
		if (strcmp(argv[ii], "--") == 0) {
			icmd = ++ii;
			break;
		}
	}

	ncpus = sysconf(_SC_NPROCESSORS_CONF);

	if ((lsfd = open(lsfile, O_RDWR)) == -1) {
		perror(lsfile);
		return 1;
	}

	if ((res = ioctl(lsfd, LS_SAMPLES, samples))) {
		if ((res = ioctl(lsfd, LS_STOP, 0)) || (res = ioctl(lsfd, LS_SAMPLES, samples))) {
			perror("LS_SAMPLES");
			close(lsfd);
			return 2;
		}
	}

	if ((res = ioctl(lsfd, LS_START, 0))) {
		perror("LS_START");
		close(lsfd);
		return 3;
	}

	if (icmd > 0 && icmd < argc) {
		expid = fork();
		if (expid == -1) {
			perror("fork");
			close(lsfd);
			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");
		close(lsfd);
		return 4;
	}

	usleep(sleeptime);

	if ((res = ioctl(lsfd, LS_STOP, 0))) {
		perror("LS_STOP");
		if (expid > 0 && kill(-expid, SIGKILL))
			perror("SIGKILL");
		close(lsfd);
		return 5;
	}

	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");
		close(lsfd);
		return 6;
	}

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

	usleep(SLEEP_SYNC_TIME);

	memset(&lsgd, 0, sizeof(lsgd));
	lsgd.size = samples;
	lsgd.data = (struct latsched_sample *) malloc(samples * sizeof(struct latsched_sample));

	for (cpu = 0; cpu < ncpus; cpu++) {
		lsgd.cpu = cpu;
		lsgd.size = samples;

		if ((res = ioctl(lsfd, LS_FETCH, &lsgd))) {
			perror("LS_FETCH");
			close(lsfd);
			return 7;
		}

		for (ii = 0; ii < lsgd.rsize; ii++) {
			if (lsgd.data[ii].lss_pid == getpid() && scycl < lsgd.data[ii].lss_out) {
				scycl = lsgd.data[ii].lss_out;
				rtlat = lsgd.data[ii].lss_out - lsgd.data[ii].lss_in;
			}
		}
	}
	fprintf(stdout, "%llu\n", rtlat);
	close(lsfd);
	return 0;

}