/*
   sched_task - scheduler test task -> for power evaluation

   Copyright (C) 2013 Lukasz Majewski <l.majewski@samsung.com>

   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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301
   USA
*/

/* This is based on the pi_stress.c rt-test program by Clark Williams */

#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <sched.h>
#include <sys/mman.h>
#include <string.h>
#include <getopt.h>

#define NSEC_PER_SEC    1000000000
#define USEC_TO_NSEC    1000

#define TIME_MIN 100000 /* 100 us minimal time */

#define TEST_TAB_SIZE 512
/*
 * TODO: add support for chosing schedule policy (other, idle, rt)
 */
int sched_policy = SCHED_FIFO;
int priority = 10;

int task_period = 0;
int task_count = 0;
int work_time = 0;
int work_count = 0;
int work_offset = 0;

int lockall_flag = 0;
int debug_flag = 0;

#define DBG(fmt, args...) do {	  \
	if (debug_flag) \
		printf(fmt, args); \
	} while (0);


/* command line options */
struct option options[] = {
	{"tperiod", required_argument, NULL, 't'},
	{"work", required_argument, NULL, 'w'},
	{"count", required_argument, NULL, 'c'},
	{"offset", required_argument, NULL, 'o'},
	{"prio", no_argument, NULL, 'p'},
	{"debug", no_argument, &debug_flag, 'd'},
	{"mlockall", no_argument, &lockall_flag, 'm'},
	{"help", no_argument, NULL, 'h'},
	{"sched_other", no_argument, NULL, 's'},
	{NULL, 0, NULL, 0},
};

void busy_test(void)
{
	volatile static unsigned char test[TEST_TAB_SIZE];
	int i, j;

	for(i = 0xFF; i; i--)
		for(j = 0; j < sizeof(test); j++)
			test[j] = i;
}

int adjust_wakeup_time(struct timespec *t, int time)
{
	t->tv_nsec += time;

	while (t->tv_nsec >= NSEC_PER_SEC) {
		t->tv_nsec -= NSEC_PER_SEC;
		t->tv_sec++;
	}

	return 0;
}

int time_passed(struct timespec *x, struct timespec *y, struct timespec *result)
{
	result->tv_sec = x->tv_sec - y->tv_sec;
	result->tv_nsec = x->tv_nsec - y->tv_nsec;

	if (result->tv_nsec < 0) {
		--result->tv_sec;
		result->tv_nsec += NSEC_PER_SEC;
	}

	/* Return 1 if result is negative. */
	return result->tv_sec < 0;
}

void usage(void)
{
	printf("usage: sched_task <options>\n");
	printf("    options:\n");
	printf("\t--prio\t\t- specify priority [real time] of the task\n");
	printf("\t--work\t\t- specify time the task is executed [us]\n");
	printf("\t--count\t\t- specify how many times the task is executed\n");
	printf("\t\t\t- if --count is specified but --work is not then\n");
	printf("\t\t\t  the task will be executed (count/10) times before\n");
	printf("\t\t\t  each sleep time\n");
	printf("\t\t\t- if both --count and --work are specified then\n");
	printf("\t\t\t  the task will be executed for --work time before\n");
	printf("\t\t\t  going to sleep\n");
	printf("\t--offset\t- specify time after which execution starts [us]\n");
	printf("\t--tperiod\t- specify period of the task [us]\n");
	printf("\t--mlockall\t- lock current and future memory\n");
	printf("\t--sched_other\t- use standard SCHED_OTHER scheduling policy for normal\n");
	printf("\t\t\t  task; priority will be ignored\n");
	printf("\t--debug\t\t- turn on debug prints\n");
	printf("\t--help\t\t- print this message\n");
}


void process_command_line(int argc, char **argv)
{
	int opt;
	while ((opt = getopt_long(argc, argv, "+", options, NULL)) != -1) {
		switch (opt) {
		case '?':
		case 'h':
			usage();
			exit(0);
		case 'p':
			priority = strtol(optarg, NULL, 10);
			break;
		case 's':
			sched_policy = SCHED_OTHER;
			break;
		case 't':
			task_period = strtoul(optarg, NULL, 10) * USEC_TO_NSEC;
			break;
		case 'w':
			work_time = strtoul(optarg, NULL, 10) * USEC_TO_NSEC;
			break;
		case 'c':
			task_count = strtoul(optarg, NULL, 10);
			break;
		case 'o':
			work_offset = strtoul(optarg, NULL, 10) * USEC_TO_NSEC;
			break;
		}
	}
	if (!work_time)
		work_count = task_count / 10;
}

int main(int argc, char* argv[])
{
	struct timespec t, tt, ttt, result;
	struct sched_param param;
	int count = 0;

	process_command_line(argc, argv);

	if(work_time > task_period) {
		perror("Wrong value of \'work\' time");
		exit(-1);
	}

	if(task_period < TIME_MIN) {
		error(0, 0, "Task period time (%d) must be grater than %d [us]\n",
		      task_period/1000, TIME_MIN/1000);
		exit(-1);
	}

	if(work_time && (work_time < TIME_MIN)) {
		error(0, 0, "Work time (%d) must be grater than %d [us]\n",
		      work_time/1000, TIME_MIN/1000);
		exit(-1);
	}
	if (task_count < 0) {
		perror("Wrong value for --count\n");
		exit(-1);
	}
	if (!work_time && !task_count) {
		perror("At least one of (--work,--count) must be provided\n");
		exit(-1);
	}

	if ((sched_policy != SCHED_FIFO) && (sched_policy != SCHED_RR))
		priority = 0;
	param.sched_priority = priority;
	if(sched_setscheduler(0, sched_policy, &param) == -1) {
		perror("sched_setscheduler for policy %d failed\n");
	}

	if (lockall_flag)
		if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
			perror("mlockall failed");
			exit(-2);
		}

	clock_gettime(CLOCK_MONOTONIC ,&t);
	adjust_wakeup_time(&t, work_offset);

	while(!task_count || (count < task_count)) {
		/* wait until next shot */
		clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);

		clock_gettime(CLOCK_MONOTONIC ,&tt);
		clock_gettime(CLOCK_MONOTONIC ,&ttt);

		DBG("t:%d %d tt:%d %d ", t.tv_sec, t.tv_nsec,
		       tt.tv_sec, tt.tv_nsec);
		adjust_wakeup_time(&tt, work_time);

		if (work_time) {
			do {
				busy_test();
				clock_gettime(CLOCK_MONOTONIC ,&ttt);
				count++;
			} while (!time_passed(&tt, &ttt, &result));
		} else {
			int i = 0;
			do {
				busy_test();
				clock_gettime(CLOCK_MONOTONIC ,&ttt);
				count++;
			} while (++i < work_count);
		}

		DBG("tt:%d %d ttt:%d %d\n", tt.tv_sec, tt.tv_nsec,
		       ttt.tv_sec, ttt.tv_nsec);
		/* calculate next shot */
		adjust_wakeup_time(&t, task_period);
	}
	printf("Work executions: %d\n", count);
}