librttest.c

/******************************************************************************
 *
 *   Copyright © International Business Machines  Corp., 2006-2008
 *
 *   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
 *
 * NAME
 *       librttest.c
 *
 * DESCRIPTION
 *      A set of commonly used convenience functions for writing
 *      threaded realtime test cases.
 *
 * USAGE:
 *       To be included in testcases.
 *
 * AUTHOR
 *	Darren Hart <[email protected]>
 *
 * HISTORY
 *      2006-Apr-26: Initial version by Darren Hart
 *      2006-May-08: Added atomic_{inc,set,get}, thread struct, debug function,
 *		      rt_init, buffered printing -- Vernon Mauery
 *      2006-May-09: improved command line argument handling
 *      2007-Jul-12: Added latency tracing functions and I/O helper functions
 *					      -- Josh triplett
 *	2008-Jan-10: Added RR thread support to tests -- Chirag Jog
 *
 *****************************************************************************/

#include <librttest.h>
#include <libstats.h>

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <time.h>
#include <string.h>
#include <pthread.h>
#include <sched.h>
#include <errno.h>
#include <unistd.h>
#include <getopt.h>
#include <sys/prctl.h>
#include <sys/stat.h>
#include <sys/syscall.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <math.h>

static LIST_HEAD ( taskset );
static atomic_t task_count = {-1};

//local only timestamp funtions (two different methods)
static __inline__ TICKS getticks(void);
static __inline__ unsigned long long rdtsc(void);

mutex _buffer_mutex;
char * _print_buffer = NULL;
int _print_buffer_offset = 0;
int _dbg_lvl = 0;
double pass_criteria;

static int _use_pi = 1;

/* function implementations */
void rt_help(void)
{
	printf("librt standard options:\n");
	printf("  -b(0,1)	1:enable buffered output, 0:disable buffered output\n");
	printf("  -p(0,1)	0:don't use pi mutexes, 1:use pi mutexes\n");
	printf("  -v[0-4]	0:no debug, 1:DBG_ERR, 2:DBG_WARN, 3:DBG_INFO, 4:DBG_DEBUG\n");
	printf("  -s		Enable saving stats data (default disabled)\n");
	printf("  -c		Set pass criteria\n");
}

/* Calibrate the busy work loop */
void calibrate_busyloop(void)
{
	volatile unsigned long long int i = CALIBRATE_LOOPS;
	volatile RTIME start, end;

	// The cicle should not be preempted!
	struct sched_param sp;
	sp.sched_priority = 99;
	sched_setscheduler ( 0, SCHED_FIFO, &sp );

	start = rt_gettime();
	while (--i > 0) {
		continue;
	}
	end = rt_gettime();
	
    printf ("end = %lld\n", end);
    printf ("start = %lld\n", start);

    if (!(end-start))
        iters_per_us = 0;
    else
        iters_per_us = (CALIBRATE_LOOPS * NS_PER_US) / (end-start);

#if DEBUG
	printf ("iters_per_us: %d", iters_per_us);
#endif
}

int rt_init_long(const char *options, const struct option *longopts, 
         int (*parse_arg)(int option, char *value), int argc,
		 char *argv[])
{
    //Kevin, fake inputs
    options = "hi:n:w:";

    argc = 0;
    argv[0] = "24";

	const struct option *cur_opt;
	int use_buffer = 1;
	char *longopt_vals;
	size_t i;
	int c;
	opterr = 0;
	char *all_options;

	if (asprintf(&all_options, ":b:mp:v:sc:%s", options) == -1) {
		fprintf(stderr, "Failed to allocate string for option string\n");
		exit(1);
	}

	/* Check for duplicate options in optstring */
	for (i=0; i<strlen(all_options); i++) {
		char opt = all_options[i];

		if (opt == ':')
			continue;

		/* Search ahead */
		if (strchr(&all_options[i+1], opt)) {
			fprintf(stderr, "Programmer error -- argument -%c already used at least twice\n", opt);
			exit(1);
		}
	}

	/* Ensure each long options has a known unique short option in val. */
	longopt_vals = "";
	cur_opt = longopts;
	while (cur_opt && cur_opt->name) {
		if (cur_opt->flag) {
			fprintf(stderr, "Programmer error -- argument --%s flag"
				" is non-null\n", cur_opt->name);
			exit(1);
		}
		if (!strchr(all_options, cur_opt->val)) {
			fprintf(stderr, "Programmer error -- argument --%s "
				"shortopt -%c wasn't listed in options (%s)\n",
				cur_opt->name, cur_opt->val, all_options);
			exit(1);
		}
		if (strchr(longopt_vals, cur_opt->val)) {
			fprintf(stderr, "Programmer error -- argument --%s "
				"shortopt -%c is used more than once\n",
				cur_opt->name, cur_opt->val);
			exit(1);
		}
		if (asprintf(&longopt_vals, "%s%c", longopt_vals, cur_opt->val) < 0) {
			perror("asprintf");
			exit(2);
		}
		cur_opt++;
	}

	while ((c = getopt_long(argc, argv, all_options, longopts, NULL)) != -1) {
		switch (c) {
		case 'c':
			pass_criteria = atof(optarg);
			break;
		case 'b':
			use_buffer = atoi(optarg);
			break;
		case 'p':
			_use_pi = atoi(optarg);
			break;
		case 'v':
			_dbg_lvl = atoi(optarg);
			break;
		case 's':
			save_stats = 1;
			break;
		case ':':
			if (optopt == '-')
				fprintf(stderr, "long option missing arg\n");
			else
				fprintf(stderr, "option -%c: missing arg\n", optopt);
			parse_arg('h', optarg); /* Just to display usage */
			exit (1); /* Just in case. (should normally be done by usage()) */
		case '?':
			if (optopt == '-')
				fprintf(stderr, "unrecognized long option\n");
			else
				fprintf(stderr, "option -%c not recognized\n", optopt);
			parse_arg('h', optarg); /* Just to display usage */
			exit (1); /* Just in case. (should normally be done by usage()) */
		default:
			if (parse_arg && parse_arg(c, optarg))
				break; /* Application option */

			fprintf(stderr, "Programmer error -- option -%c defined but not handled\n", c);
			exit(1);
		}
	}
	if (!_use_pi)
		printf("Priority Inheritance has been disabled for this run.\n");
	if (use_buffer)
		buffer_init();

	//RTAI and Xenomai require this - to be fair we impose this to all tests
	mlockall(MCL_CURRENT|MCL_FUTURE);

	// To calibrate busy_loop in PREEMPT_RT mode.
	calibrate_busyloop(); //KEvin, removed due to Floating point exceptions being thrown

	/*
	 * atexit() order matters here - buffer_print() will be called before
	 * buffer_fini().
	 */
	atexit(buffer_fini);
	atexit(buffer_print);

    //printf("librttest.c dump\n");
    dump_task_struct();

#if KERN_LITMUS
    printf("Initailizing for the Litmus^RT kernel\n");
    init_litmus();    

#elif KERN_CHRONOS
    //printf("Initailizing for the ChronOS kernel\n");
    struct sched_param param, old_param;

    sched_getparam(0, &old_param);
    param.sched_priority = MAIN_PRIO;

    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
        printf("[ChronOS] sched_setscheduler() failed, are you root?\n");
        exit(1);
    }
    
    if (set_scheduler(SET_SCHED_ALGO | SET_SCHED_FLAG, SET_SCHED_PRIO, SET_SCHED_PROC)) {
        printf("Selection of RT scheduler failed! Is the scheduler loaded?");
        exit(1);
    }
#elif KERN_PREEMPT
    //printf("Initailizing for the PREEMPT kernel\n");

#elif KERN_SCHED_DEAD
    //printf("Initailizing for the SCHED_DEADLINE kernel\n");

#elif KERN_RTAI
    //printf("Initailizing for the RTAI kernel\n");

#elif KERN_XENOMAI
    //printf("Initailizing for the XENOMAI kernel\n");

#elif KERN_IRMOS
    //printf("Initailizing for the IRMOS kernel\n");

#else 
    //printf("Initailizing for the vanilla Linux kernel\n");

#endif

	return 0;
}

int rt_init(const char *options, int (*parse_arg)(int option, char *value),
	    int argc, char *argv[])
{
	return rt_init_long(options, NULL, parse_arg, argc, argv);
}

void buffer_init(void)
{
	_print_buffer = (char *)malloc(PRINT_BUFFER_SIZE);
	if (!_print_buffer)
		fprintf(stderr, "insufficient memory for print buffer - printing directly to stderr\n");
	else
		memset(_print_buffer, 0, PRINT_BUFFER_SIZE);
}

void buffer_print(void)
{
	if (_print_buffer) {
		fprintf(stderr, "%s", _print_buffer);
		memset(_print_buffer, 0, PRINT_BUFFER_SIZE);
		_print_buffer_offset = 0;
	}
}

void buffer_fini(void)
{
	if (_print_buffer)
		free(_print_buffer);
	_print_buffer = NULL;
}

void cleanup(int i) {
	printf("Test terminated with asynchronous signal\n");
	buffer_print();
	buffer_fini();
	if (i)
		exit (i);
}

void setup()
{
	signal(SIGINT, cleanup);
	signal(SIGQUIT,cleanup);
	signal(SIGTERM, cleanup);
}

void ts_minus(struct timespec *ts_end, struct timespec *ts_start, struct timespec *ts_delta)
{
	if (ts_end == NULL || ts_start == NULL || ts_delta == NULL) {
		printf("ERROR in %s: one or more of the timespecs is NULL", __FUNCTION__);
		return;
	}

	ts_delta->tv_sec = ts_end->tv_sec - ts_start->tv_sec;
	ts_delta->tv_nsec = ts_end->tv_nsec - ts_start->tv_nsec;
	ts_normalize(ts_delta);
}

void ts_plus(struct timespec *ts_a, struct timespec *ts_b, struct timespec *ts_sum)
{
	if (ts_a == NULL || ts_b == NULL || ts_sum == NULL) {
		printf("ERROR in %s: one or more of the timespecs is NULL", __FUNCTION__);
		return;
	}

	ts_sum->tv_sec = ts_a->tv_sec + ts_b->tv_sec;
	ts_sum->tv_nsec = ts_a->tv_nsec + ts_b->tv_nsec;
	ts_normalize(ts_sum);
}

void ts_normalize(struct timespec *ts)
{
	if (ts == NULL) {
		/* FIXME: write a real error logging system */
		printf("ERROR in %s: ts is NULL\n", __FUNCTION__);
		return;
	}

	/* get the abs(nsec) < NS_PER_SEC */
	while (ts->tv_nsec > NS_PER_SEC) {
		ts->tv_sec++;
		ts->tv_nsec -= NS_PER_SEC;
	}
	while (ts->tv_nsec < -NS_PER_SEC) {
		ts->tv_sec--;
		ts->tv_nsec += NS_PER_SEC;
	}

	/* get the values to the same polarity */
	if (ts->tv_sec > 0 && ts->tv_nsec < 0) {
		ts->tv_sec--;
		ts->tv_nsec += NS_PER_SEC;
	}
	if (ts->tv_sec < 0 && ts->tv_nsec > 0) {
		ts->tv_sec++;
		ts->tv_nsec -= NS_PER_SEC;
	}
}

int ts_to_nsec(struct timespec *ts, RTIME *ns)
{
	struct timespec t;
	if (ts == NULL) {
		/* FIXME: write a real error logging system */
		printf("ERROR in %s: ts is NULL\n", __FUNCTION__);
		return -1;
	}
	t.tv_sec = ts->tv_sec;
	t.tv_nsec = ts->tv_nsec;
	ts_normalize(&t);

	if (t.tv_sec <= 0 && t.tv_nsec < 0) {
		printf("ERROR in %s: ts is negative\n", __FUNCTION__);
		return -1;
	}

	*ns = (RTIME)ts->tv_sec*NS_PER_SEC + ts->tv_nsec;
	return 0;
}

void nsec_to_ts(RTIME ns, struct timespec *ts)
{
	if (ts == NULL) {
		/* FIXME: write a real error logging system */
		printf("ERROR in %s: ts is NULL\n", __FUNCTION__);
		return;
	}
	ts->tv_sec = ns/NS_PER_SEC;
	ts->tv_nsec = ns%NS_PER_SEC;
}

/* return difference in microseconds */
RTIME rt_minus(RTIME start, RTIME end)
{
	RTIME delta;
	if (start <= end)
		delta = end - start;
	else {
		delta = ULL_MAX - (end - start) + 1;
		//printf("TSC wrapped, delta=%llu\n", delta);
	}
	return delta;
}

RTIME rt_gettime(void) {
/*
#if KERN_RTAI
	return rt_get_time_ns();
#elif KERN_XENOMAI
	return rt_timer_read();
#else
*/
    //chronos option here
    TICKS time = 0;
    //TICKS tick;

    //FOR IRMOS, this resolution is only 1ms!!!
	/*rc = clock_gettime(CLOCK_MONOTONIC, &ts);
	if (rc != 0) {
		printf("ERROR in %s: clock_gettime() returned %d\n", __FUNCTION__, rc);
		perror("clock_gettime() failed");
		return 0;
	}
    */


    //tick = getticks();
    //time = (unsigned)((tick)/1900.075);

	//ts_to_nsec(&ts, &ns);
	//return ns;
	//RETURN time;
    
    time = rdtsc(); //number of ticks
    return time; 
    
//#endif 
}

/**
 * busy wait for ns nanosecond
 */
void rt_busywork(RTIME ns)
{
#if KERN_RTAI
	rt_busy_sleep(ns);
#elif KERN_XENOMAI
	rt_timer_spin(ns);
#else
	volatile unsigned long long int i;
	RTIME start, now;
	int delta; /* time in us */

	i = (ns * iters_per_us) / NS_PER_US;

	start = rt_gettime();
	while (--i > 0) {
		continue;
	}
	now = rt_gettime();

	delta = (now - start)/NS_PER_US;
	/* uncomment to tune to your machine */
	/* printf("busy_work_us requested: %dus  actual: %dus\n", us, delta); */
#endif
}

/**
* sleep for ns nanosecond
*/
void rt_nanosleep(RTIME ns)
{
#if KERN_RTAI
	rt_sleep(nano2count(ns));
#elif KERN_XENOMAI
	rt_task_sleep((RTIME)ns);
#else
	struct timespec ts_sleep, ts_rem;
	int rc;
	nsec_to_ts(ns, &ts_sleep);
	rc = clock_nanosleep(CLOCK_MONOTONIC, 0, &ts_sleep, &ts_rem);
	/* FIXME: when should we display the remainder ? */
	if (rc != 0) {
		printf("WARNING: rt_nanosleep() returned early by %d s %d ns\n",
		       (int)ts_rem.tv_sec, (int)ts_rem.tv_nsec);
	}
#endif
}

/**
* sleep for ns nanosecond, then wake
*/
void rt_nanosleep_until(RTIME ns)
{
#if KERN_RTAI
	rt_sleep_until(nano2count(ns));
#elif KERN_XENOMAI
	rt_task_sleep_until(ns);
#else
	struct timespec ts_sleep, ts_rem;
	int rc;
	nsec_to_ts(ns, &ts_sleep);
	rc = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts_sleep, &ts_rem);
	/* FIXME: when should we display the remainder ? */
	if (rc != 0) {
		printf("WARNING: rt_nanosleep() returned early by %d s %d ns\n",
		       (int)ts_rem.tv_sec, (int)ts_rem.tv_nsec);
	}
#endif
}

/* Write the entirety of data.  Complain if unable to do so. */
static void write_or_complain(int fd, const void *data, size_t len)
{
	const char *remaining = data;

	while (len > 0) {
		ssize_t ret = write(fd, remaining, len);
		if (ret <= 0) {
			if (errno != EAGAIN && errno != EINTR) {
				perror("write");
				return;
			}
		} else {
			remaining += ret;
			len -= ret;
		}
	}
}

/* Write the given data to the existing file specified by pathname.  Complain
 * if unable to do so. */
static void write_file(const char *pathname, const void *data, size_t len)
{
	int fd = open(pathname, O_WRONLY);
	if (fd < 0) {
		printf("Failed to open file \"%s\": %d (%s)\n",
		       pathname, errno, strerror(errno));
		return;
	}

	write_or_complain(fd, data, len);

	if (close(fd) < 0) {
		printf("Failed to close file \"%s\": %d (%s)\n",
		       pathname, errno, strerror(errno));
	}
}

/* Write the given '\0'-terminated string to the existing file specified by
 * pathname.  Complain if unable to do so. */
static void write_string_to_file(const char *pathname, const char *string)
{
	write_file(pathname, string, strlen(string));
}

static void read_and_print(const char *pathname, int output_fd)
{
	char data[4096];
	int fd = open(pathname, O_RDONLY);
	if (fd < 0) {
		printf("Failed to open file \"%s\": %d (%s)\n",
		       pathname, errno, strerror(errno));
		return;
	}

	while (1) {
		ssize_t ret = read(fd, data, sizeof(data));
		if (ret < 0) {
			if (errno != EAGAIN && errno != EINTR) {
				printf("Failed to read from file \"%s\": %d (%s)\n",
					pathname, errno, strerror(errno));
				break;
			}
		} else if (ret == 0)
			break;
		else
			write_or_complain(output_fd, data, ret);
	}

	if (close(fd) < 0) {
		printf("Failed to close file \"%s\": %d (%s)\n",
			pathname, errno, strerror(errno));
	}
}

void latency_trace_enable(void)
{
	printf("Enabling latency tracer.\n");
	write_string_to_file("/proc/sys/kernel/trace_use_raw_cycles", "1");
	write_string_to_file("/proc/sys/kernel/trace_all_cpus", "1");
	write_string_to_file("/proc/sys/kernel/trace_enabled", "1");
	write_string_to_file("/proc/sys/kernel/trace_freerunning", "1");
	write_string_to_file("/proc/sys/kernel/trace_print_on_crash", "0");
	write_string_to_file("/proc/sys/kernel/trace_user_triggered", "1");
	write_string_to_file("/proc/sys/kernel/trace_user_trigger_irq", "-1");
	write_string_to_file("/proc/sys/kernel/trace_verbose", "0");
	write_string_to_file("/proc/sys/kernel/preempt_thresh", "0");
	write_string_to_file("/proc/sys/kernel/wakeup_timing", "0");
	write_string_to_file("/proc/sys/kernel/mcount_enabled", "1");
	write_string_to_file("/proc/sys/kernel/preempt_max_latency", "0");
}

#ifndef PR_SET_TRACING
#define PR_SET_TRACING 0
#endif

void latency_trace_start(void)
{
	if (prctl(PR_SET_TRACING, 1) < 0)
		perror("Failed to start tracing");
}

void latency_trace_stop(void)
{
	if (prctl(PR_SET_TRACING, 0) < 0)
		perror("Failed to stop tracing");
}

void latency_trace_print(void)
{
	read_and_print("/proc/latency_trace", STDOUT_FILENO);
}

/*--- Task management functions ---*/

task taskset_init ( int num )
{
	task info; //task is task_t *
	int i;

	info = calloc ( num, sizeof( struct task_t ) );
	if ( !info ) {
		printf ( "Insufficient memory initializing task set\n" );
		exit ( 1 );
	}
	for ( i=0; i<num; i++ ) {
		info[i].prio=1;
		info[i].policy=SCHED_FIFO;
#if KERN_RTAI
		info[i].cpus_allowed = 0xFF;
#else
        CPU_SET(0xFF,&info[i].cpus_allowed);
#endif
	}
	return info;
}

int task_create ( task t ) {
	int id;
	int ret = 0;

	id = atomic_inc ( &task_count );
	list_add_tail ( &t->list, &taskset );
	t->id = id;
    snprintf((char *)&t->name,16,"%dTSK%x",id,id);


#if KERN_CHRONOS
	t->prio = TASK_CREATE_PRIO;
    t->policy = SCHED_FIFO;
#endif
	
#if KERN_RTAI

    //printf("[RTAI] task_create for %d\n",t->id);
    t->desc = rt_thread_create(t->func, t->arg, 4096);
    //printf("[RTAI] %s : t->desc = %p\n",__FUNCTION__,t->desc);
    //t->desc = rt_task_init_schmod(nam2num(name),t->prio,t->stack_size,t->max_msg_size,SCHED_FIFO,t->cpus_allowed);

    //printf("[RTAI] inside of task_create\n");

    //moved to rt_task_begin
	//t->desc = rt_thread_init ( nam2num ( t->name ), t->prio, t->max_msg_size, t->policy, t->cpus_allowed );

	if ( t->desc == 0 ) {
		printf ( "[RTAI] Task creation failed" );
		list_del ( &t->list );
		//free (t);
		return -1;
	}
#elif KERN_XENOMAI
    //printf("inside task_create, desc: %p func: %p arg: %p name: %s prio: %d id: %d\n", &t->desc, t->func, t->arg, t->name, t->prio, t->id);
	if (( ret  = rt_task_create ( &t->desc, t->name, t->stack_size, t->prio, T_JOINABLE/*t->flags*/ )) != 0 ) {
		printf ( "[XENOMAI] Task creation failed: %d (%s) for the task named %s\n", ret, strerror ( -ret ),t->name ); //made negative
		list_del ( &t->list );
		//free ( t );
		return -1;
	}
    if (t->name) {
        if (!strcmp(t->name,"MASTER")) {
            return ret;
        } else {
            printf("[XENOMAI] inside %s: &t->desc: %p, t->name: %s\n", __FUNCTION__, &t->desc, t->name);
            task_start (t->id);
        }
    } else {
        task_start (t->id);
    }

/* LITMUS, SCHED_DEAD, PREEMPT, VANILLA, IRMOS */
#else
    
    //printf("inside task_create's else, desc: %p attr: %p func: %p arg: %p name: %s prio: %d id: %d\n", t->desc, t->attr, t->func, t->arg, t->name, t->prio, t->id);

    /*
    if (!strcmp(t->name,"MASTER")) {
        return 0;
    }
    */
    
    
	struct sched_param param;
	pthread_cond_init ( &t->cond, NULL );	// Accept the defaults

	if (mutex_init ( &t->lock,t->id ))
        printf("task_create's mutex init failed\n"); 
    //else
        //printf("task_create's mutex init succeeded!\n"); 

    int err = 0;
    //Shouldn't this be a general thing? Doesn't seem fair
	param.sched_priority = t->prio;

    //TODO
    //look at foswiki and add in sched_fifo policy change here?

	pthread_attr_init ( &t->attr );
    pthread_attr_setdetachstate( &t->attr, PTHREAD_CREATE_JOINABLE );
	pthread_attr_setinheritsched ( &t->attr, PTHREAD_EXPLICIT_SCHED );

    //TODO: Kevin: Look into moving rt_begin_tasks' policy settings to here
	err = pthread_attr_setschedpolicy ( &t->attr, t->policy );

    if (err != 0)  {
        printf("Kevin: setschedpolicy error:%d\n",err);
    }
	err = pthread_attr_setschedparam ( &t->attr, &param );
    if (err != 0)  {
        printf("Kevin: setschedparam error:%d\n",err);
    }

	if (( ret = pthread_create( &t->desc, &t->attr, t->func, t->arg ))) {
		printf ( "[POSIX] Task creation failed!!!!!: %d (%s)\n", ret, strerror ( ret ) ); //fails with 22, Invalid Arg
		list_del ( &t->list );
		pthread_attr_destroy ( &t->attr );
		free ( t );
		return -1;
	}
	pthread_attr_destroy ( &t->attr );

    /*
    //if ((ret = sched_setaffinity(0, sizeof(t->cpus_allowed), &t->cpus_allowed)) != 0) {
    if ((ret = pthread_setaffinity_np(t->desc, sizeof(t->cpus_allowed), &t->cpus_allowed))) {
        printf("sched_setaffinity failed with error: %d strerror: (%s)\n",ret,strerror(ret)); 
	    return ret;
    }
    else {
        CPU_COUNT(&t->cpus_allowed);
        printf("sched_setaffinity set to run on core: %d\n",sched_getcpu()); 
    }
    */

#endif
   // printf("Kevin: leaving task_create\n");
	return ret;
}

int task_start ( int id ) 
{
	int ret = 0;
#if KERN_RTAI
	//rt_thread_create ( t->func, t->arg, t->stack_size );
#elif KERN_XENOMAI
	task t = task_get ( id );
    //printf ( "[Xenomai] inside task_start\n");
	if ( ( ret = rt_task_start( &t->desc, (void (*)(void*))t->func, t->arg ) ) != 0 )
	{
		printf ( "[Xenomai] Failed to start task: %d (%s)\n", ret, strerror ( ret ) );
		return -1;
	}
#endif
	return ret;
}

int task_set_priority ( int id, int prio )
{
	// Recupera il task indicato da ID, ne setta la priorità ed esce
	task t = task_get ( id );
	int ret = 0;
#if KERN_RTAI
	if ( ( ret = rt_change_prio ( t->desc, prio ) ) != 0 ) {
		printf ( "[RTAI] Failed to set priority: %d (%s)\n", ret, strerror ( ret ) );
		return -1;
	}
#elif KERN_XENOMAI
	if ( ( ret = rt_task_set_priority( &t->desc, prio ) ) != 0 ) {
		printf ( "[Xenomai] Failed to set priority: %d (%s)\n", ret, strerror ( ret ) );
		return -1;
	}
#else
	struct sched_param sp;
	sp.sched_priority = prio;

	t->prio = prio;
	// If priority is 0, set automatically policy to SCHED_OTHER
	if ( prio==0 )
		t->policy = SCHED_OTHER;

	if ( sched_setscheduler ( t->desc, t->policy, &sp ) != 0 )
		return -1;
#endif
	t->prio = prio;
	return ret;
}

int task_set_policy ( int id, int policy )
{
	task t = task_get ( id );
	int ret = 0;
#if KERN_RTAI
	if ( policy == SCHED_FIFO )
		rt_set_sched_policy ( t->desc, RT_SCHED_FIFO, 1000 );
	else if ( policy == SCHED_RR ) {
		// TODO Add support for RR quantum directly in task structure
		// Here we have quantum = 1000 just for example		
		rt_set_sched_policy ( t->desc, RT_SCHED_RR, 1000 );
	} else {
		printf ( "[RTAI] Scheduling policy not supported" );
		return -1;
	}
#elif KERN_XENOMAI
	// TODO Add support to change to SCHED_RR specifying the time slice
	printf ( "[Xenomai] Changing scheduling policy is not supported" );
	return -1;
#else
	struct sched_param sp;
	sp.sched_priority = t->prio;
	if ( sched_setscheduler ( t->desc, policy, &sp ) != 0 )
		return -1;
#endif
	t->policy = policy;
	return ret;
}

void task_join ( int i )
{
	task p, t = NULL;
	list_for_each_entry ( p, &taskset, list )
	{
		if ( p->id == i )
		{
			t = p;
			break;
		}
	}
	if ( t ) {
#if KERN_RTAI
		// WARNING RTAI has no join primitive.
		// We're going to kill each task, hoping for the best! ;)
		//if ( t->desc )
			//rt_task_delete ( t->desc );

        //printf("[RTAI] inside %s : temp->desc: %p\n",__FUNCTION__,t->desc);
        rt_thread_join(t->desc);
        //just use all
#elif KERN_XENOMAI
        //printf("[Xenomai] inside task_join task_desc: %p\n", &t->desc);
		if ( &t->desc != NULL ) {
			rt_task_join ( &t->desc );
        } else {
            printf("[Xenomai] ERROR: &t->desc is NULL!\n");
        }
#else
        //printf("[IRMOS] joining thread %d \n",t->id);
		t->flags |= THREAD_QUIT;
		if ( t->desc )
			pthread_join ( t->desc, NULL );
        //printf("[IRMOS] joined thread %d \n",t->id);
#endif
		list_del ( &t->list );
	}
}

void task_quit_all()
{
	task p;
	list_for_each_entry ( p, &taskset, list )
	{
#if KERN_RTAI
#elif KERN_XENOMAI
#else
		p->flags |= THREAD_QUIT;
#endif
	}
    //printf("Kevin: inside task_quit_all\n");
}

void task_join_all()
{
    int err = 0;
	task_quit_all();
	task p, t;
	list_for_each_entry_safe ( p, t, &taskset, list ) {
#if KERN_RTAI
		// WARNING RTAI has no join primitive.
		// We're going to kill each task, hoping for the best! ;)
        // Kevin: debugging dmesg found on gist.github.com
        
       
        
        /*
		if ( p->desc ) {
            printf("[RTAI] Kevin: about to delete task for RTAI %p\n",p->desc);
        
            //rt_make_soft_real_time();
			err = rt_task_delete ( p->desc );
            if (err) 
                printf("[RTAI] error in rt_task_delete: %d (%s) \n",err,strerror(-err));
            else
                printf("Kevin: deleted task for RTAI\n");
        }
        */
        
        
        
        //printf("[RTAI]func in tja: %p\n", p->func);
        //printf("[RTAI] inside %s : temp->desc: %p\n",__FUNCTION__,p->desc);
        rt_thread_join(p->desc);
#elif KERN_XENOMAI
		if ( &p->desc != NULL ) {
            //printf("[Xenomai] inside task_join_all p_desc: %p\n", &p->desc);
			err = rt_task_join(&p->desc);
            if (err) {
                printf("[Xenomai] inside task_join_all err: %d (%s)\n", err, strerror(-err));
            }
        }
#else
		if ( p->desc ) {
            //printf("Kevin [%s]: thread desc:%p id:%d name:%s...",__FUNCTION__,p->desc,p->id,p->name);
			if ((err = pthread_join ( p->desc, NULL )) != 0) {
                printf("...FAILED!\n");
                printf("Kevin: pthread_join error:%d\n",err);
            } 
            //else {
                //printf("...succeeded!\n");
            //}
        }
#endif
		//list_del ( &p->list ); //why is this segfaulting?
	}
    //printf("Kevin: leaving task_join_all\n");
}

task task_get ( int id ) 
{
	task t;
	list_for_each_entry ( t, &taskset, list )
	{
		if ( t->id == id ) {
			return t;
		}
	}
	return NULL;
}

void task_inspect ( task t )
{
	printf ( "\nTask n.%d\n", t->id );
	printf ( "Scheduling policy: " );
	if ( t->policy == SCHED_FIFO )
		printf ( "SCHED_FIFO\n" );
	else if ( t->policy == SCHED_RR )
		printf ( "SCHED_RR\n" );
	else
		printf ( "SCHED_OTHER\n" );
	printf ( "Priority: %d\n", t->prio );
	printf ( "Start point at: %p\n", t->func );
	printf ( "Arguments at: %p\n", &t->arg );
	printf ( "Flags: %d\n", t->flags );
	printf ( "Previous task at: %p\n", t->list.prev );
	printf ( "Next task at: %p\n", t->list.next );
}

/*--- Mutex management functions ---*/
int mutex_init ( mutex * m, int namnum ) {
	int ret = 0;
#if KERN_RTAI
    char test[3];
    sprintf(test,"%d",namnum);
    //printf("[RTAI] BEFORE init mutex n:%s, p:%p\n",test,*m);
    //*m = rt_typed_named_sem_init(test, 1, CNT_SEM);
    *m = rt_named_sem_init(test,1);
    //printf("[RTAI] AFTER init mutex\n"); // n:%s, p:%p\n",test,*m);
    //m = rt_named_sem_init(test,1);
    ret = errno;
    if (!*m) {
        printf("[RTAI] AFTER failed to init mutex %d error: %d strerror: %s\n",nam2num("test"),ret,strerror(ret));
        return ret;
    } 
#elif KERN_XENOMAI
    //printf("[Xenomai] BEFORE init mutex p:%p\n",m);
	if (( ret = rt_mutex_create ( m, NULL )) != 0 ) {
		printf ( "[Xenomai] Failed to init mutex: %d (%s)\n", ret, strerror(-ret)); //Kevin: made negative
		return -1;
	}
    //printf("[Xenomai] mutex initialized: %p\n", m);
#elif KERN_CHRONOS
    if ( ( ret = chronos_mutex_init(m) ) != 0 )
        printf("[ChronOS] mutex failed to init with %d\n",ret);
//#elif KERN_LITMUS
    //ret = rd_openx(int fd, obj_type_t type, int obj_id, void *config); //relies on file descriptor?!
 //   return ret;
#else
//#if HAS_PRIORITY_INHERIT
	pthread_mutexattr_t attr;
	//int protocol;

    /*
	if ((ret = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT)) != 0) {
		printf("[POSIX] Failed to set mutexattr protocol: %d (%s)\n", ret, strerror(ret));
		return -1; 
    }
	if ((ret = pthread_mutexattr_getprotocol(&attr, &protocol)) != 0) {
		printf("[POSIX] Failed to get mutexattr protocol: %d (%s)\n", ret, strerror(ret));
		return -1;
	}
    */
	
    if ((ret = pthread_mutexattr_init(&attr)) != 0) {
		printf("[POSIX] Failed to init mutexattr: %d (%s)\n", ret, strerror(ret));
		return ret;
	}
    if ((ret = pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_ERRORCHECK)) != 0) {
		printf("[POSIX] Failed to settype mutexattr: %d (%s)\n", ret, strerror(ret));
		return ret;
    }
    if ((ret = pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED)) != 0) {
		printf("[POSIX] Failed to setpshared mutexattr: %d (%s)\n", ret, strerror(ret));
		return ret;
    }
	if ((ret = pthread_mutex_init(m, &attr)) != 0) {
		printf("[POSIX] Failed to init mutex: %d (%s)\n", ret, strerror(ret));
		return ret;
	}

//#endif /* HAS_PRIORITY_INHERIT */
#endif /* KERN_RTAI */
	return ret;
}

int mutex_destroy ( mutex *m )
{
        int ret = 0;
#if KERN_RTAI
	if ((ret = rt_named_sem_delete(*m)) < 0) {
        printf("[RTAI] kevin mutex %p failed to delete with error: 0x%x (%s)\n",*m,ret,strerror(ret));
        return ret;
    } 

#elif KERN_XENOMAI
	if ( ( ret = rt_mutex_delete ( m ) ) != 0 ) {
		printf ( "[Xenomai] Failed to destroy mutex: %p %d (%s)\n",m, ret, strerror ( -ret ) );
		return -1;
	}
#elif KERN_CHRONOS
    if ( ( ret = chronos_mutex_destroy(m) ) != 0 )
        printf("[ChronOS] mutex failed to destroy with %d\n",ret);
//#elif KERN_LITMUS
//    if ( ( ret = od_close(od) ) != 0 ) {
//        printf("[LitmusRT] mutex failed to destroy with %d\n",ret);
//        return ret;
//    }
#else
	if ( ( ret = pthread_mutex_destroy ( m ) ) != 0 ) {
		printf ( "[POSIX] Failed to destroy mutex: %d (%s)\n", ret, strerror ( ret ) );
		return -1;
	}
#endif
        return ret;
}

int mutex_lock ( mutex *m)
{
        int ret = 0;
#if KERN_RTAI
#elif KERN_XENOMAI
	if (( ret = rt_mutex_acquire ( m, TM_INFINITE )) != 0 ) { //TM_INIFITE for infinite timeout
        printf("[Xenomai] (%s) m:%p timeout%d : ",__FUNCTION__, m, TM_INFINITE);
		printf ( "[Xenomai] Failed to lock mutex: %d (%s)\n", ret, strerror( -ret ));
		return -1;
	}
#elif KERN_CHRONOS
    if ( ( ret = chronos_mutex_lock(m) ) != 0 )
        printf("[ChronOS] mutex failed to lock with %d\n",ret);
#else
        //printf("mlock %p\n",m);
        if (( ret = pthread_mutex_lock ( m )) != 0 ) {
                printf ( "[POSIX] Failed to lock mutex: %d (%s)\n", ret, strerror ( ret ) );
                return -1;
        }
#endif
        return ret;
}

int mutex_unlock ( mutex *m)
{
	int ret=0;
#if KERN_RTAI
#elif KERN_XENOMAI
	if (( ret = rt_mutex_release( m )) != 0 ) {
		printf ( "[Xenomai] Failed to unlock mutex: %p %d (%s)\n",m, ret, strerror ( -ret ) );
		return ret;
	}	
#elif KERN_CHRONOS
    if ( ( ret = chronos_mutex_unlock(m) ) != 0 )
        printf("[ChronOS] mutex failed to unlock with %d (%s)\n",ret, strerror(ret));
#else
    //printf("munlock %p\n",m);
	if ((ret = pthread_mutex_unlock(m)) != 0) {
		printf("[POSIX] Failed to unlock mutex: ret=%d (%s)\n", ret, strerror(ret));
		return ret;
	}
#endif
	return ret;
}

/** [ABANDONED] RT COND PRIMITIVES **/
/*--- Conditional variable management functions ---*/
int cond_init (char * name, cond_t * cv) {
    int ret = 0;
    //printf("Kevin: inside cond_init\n");

#if KERN_RTAI  
    rt_cond_init(nam2num(name));//, type, arg ) )
#elif KERN_XENOMAI  
    rt_cond_create(cv,name);
#else 
#if HAS_PRIORITY_INHERIT
    /* Kevin: Straight copy from mutex_init */
    printf("Kevin: inside has_prio_inherit\n");
	pthread_condattr_t attr;
	//int protocol;

	if ((ret = pthread_condattr_init(&attr)) != 0)
	{
		printf("[POSIX] Failed to init condattr: %d (%s)\n", ret, strerror(ret));
		return -1;
	}
	if ((ret = pthread_cond_init(cv, &attr)) != 0)
	{
		printf("[POSIX] Failed to init cond: %d (%s)\n", ret, strerror(ret));
		return -1;
	}
    
#endif /* HAS_PRIORITY_INHERIT */
    printf("Kevin: outside has_prio_inherit\n");
    
#endif
    return ret;
}

int cond_wait (cond_t * cv, mutex * lk, int timeout) {
    int ret = 0;
#if KERN_RTAI  
    rt_cond_wait(cv, lk); //Kevin: not sure what to put for timeout here
#elif KERN_XENOMAI  
    rt_cond_wait(cv, lk, timeout); //Kevin: not sure what to put for timeout here
#else
    if((ret = pthread_cond_wait(cv,lk)) != 0)
        return ret;
#endif
    return ret;
}

int cond_destroy (cond_t * cv) {
    int ret = 0;

#if KERN_RTAI  
    if ((ret = rt_cond_destroy(cv)))
        return ret;
#elif KERN_XENOMAI  
    if ((ret = rt_cond_delete(cv)))
        return ret;
#else
	if ((ret = pthread_cond_destroy(cv)) != 0) 
        return ret;
#endif
    return ret;
}

int cond_broadcast (cond_t * cv) {
    int ret = 0;
#if KERN_RTAI  
    rt_cond_broadcast(cv);
#elif KERN_XENOMAI  
    rt_cond_broadcast(cv);
#else
    if ((ret = pthread_cond_broadcast(cv)) !=0 ) 
        return ret;
#endif
    return ret;
}

/** RT TASK PRIMITIVES **/
int rt_task_begin (RTIME exec_cost, RTIME period, period_param* my_period, int isPeriodic, int thread_id, unsigned long cpu_mask, task t) {

    //cpu_mask += 1;//no zeroes
    int ret = 0;
    cpu_set_t cpus;
    CPU_ZERO(&cpus);
    CPU_SET(cpu_mask,&cpus); //set in task_set? 

    //printf("[ALL] rt_task_begin top of\n");
    //printf("[ALL] CPU_COUNT=%d, current cpu to add %lu to thread %d\n",CPU_COUNT(&cpus),cpu_mask,thread_id);

    
/** Set Affinity **/
#if !KERN_RTAI && !KERN_XENOMAI
    /*
    if ((ret = sched_setaffinity(0, sizeof(cpu_mask), (cpu_set_t *)&cpu_mask)) != 0) {
    //if(sched_setaffinity(0, sizeof(t->cpu_mask), (cpu_set_t *) &t->cpu_mask))

    //Kevin: I think there is a chronos call for this
    //if ((ret = pthread_setaffinity_np(pthread_self(), sizeof(cpus), &cpus))) {
        printf("sched_setaffinity failed with error: %d strerror: (%s)\n",ret,strerror(-ret)); 
    }
    */
    /*} else {
        printf("sched_setaffinity set to run on core: %d with mask 0x%x\n",sched_getcpu(),cpu_mask); 
    }*/
#endif

/** Scheduler Initialization **/
#if KERN_SCHED_DEAD
    //Set the scheduling policy
    struct sched_param2 schedParam;
    memset(&schedParam, 0, sizeof(schedParam));
    schedParam.sched_priority = THREAD_PRIORITY;
    schedParam.sched_runtime = exec_cost;// / number_of_threads;
    schedParam.sched_deadline = period;
    schedParam.sched_period = period;
    schedParam.sched_flags = 0;
    //printf("1) ret=%d\n",ret);
    ret = sched_setscheduler2(gettid2(), SCHED_DEADLINE, &schedParam);
    //printf("2) ret=%d\n",ret);
    
#elif KERN_PREEMPT || KERN_IRMOS
    struct sched_param schedParam;
    pthread_t myself = pthread_self();
    memset(&schedParam, 0, sizeof(schedParam));
    schedParam.sched_priority = THREAD_PRIORITY;
    ret = pthread_setschedparam(myself, SCHED_FIFO, &schedParam);
    
#endif

#if KERN_CHRONOS
        //increase priority to TASK_START_PRIO
        struct sched_param param;
        param.sched_priority = TASK_START_PRIO;
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &param); //AB 10-09 (kevin): I think this may be the issue
#endif

/** Periodic Initialization **/
#if KERN_LITMUS
    struct rt_task param;
    memset(&param, 0, sizeof(param));
    
    param.exec_cost = exec_cost; 
    param.period = period;
    param.relative_deadline = period;
    param.cls = RT_CLASS_HARD;
    param.budget_policy = NO_ENFORCEMENT;
    param.priority = LITMUS_HIGHEST_PRIORITY;                                                                                                           
    CALL( init_rt_thread() );


    int cpu_dec = mask2dec(cpu_mask);
    
    param.cpu = cpu_dec;
    be_migrate_to_cpu(cpu_dec);
    //printf("to be migrated to: %d, mask 0x%lx\n",cpu_dec,cpu_mask);
    
    CALL( set_rt_task_param(gettid(), &param) );
    CALL( task_mode(LITMUS_RT_TASK) );

#elif KERN_RTAI
    //printf("[RTAI] KEVIN inside rt_task_begin for task %d",thread_id);
    task temp;
    temp = task_get( thread_id );

    if (temp == NULL) {
        printf("[RTAI] ERROR: could not retrieve current task\n");
        exit (1);
    }
    int hard_timer_running = 0; 
    RTIME period_cnt = 0;

    if (isPeriodic) {
       if (!(hard_timer_running = rt_is_hard_timer_running())) {
          printf("[RTAI] setting rt_periodic mode\n");
          rt_set_periodic_mode();
          period_cnt = start_rt_timer(nano2count(period));
          //printf("1 period_cnt:%ld\n",period_cnt);
       }
       else {
          printf("[RTAI] hard timer is NOT running\n");
          period_cnt = nano2count(period);
          //printf("2 period_cnt:%ld\n",period_cnt);
       }
    }

    //printf("[RTAI] KEVIN inside rt_task_begin for desc %p\n",temp->desc);
	//RT_TASK* temp_rt_task = temp->desc;


    //printf("[RTAI] cpus_allowed: %d\n",temp->cpus_allowed);
    /*printf("[RTAI] rt_task_init_schmod((%s)%ld,%d,%d,%d,%d,%ld)\n",
            temp->name, nam2num ( temp->name ), temp->prio, temp->stack_size, 
            temp->max_msg_size, temp->policy, cpu_mask );
            */

    //if (!(temp->desc = rt_task_init_schmod(nam2num(temp->name), temp->prio, temp->stack_size,temp->max_msg_size, temp->policy, cpu_mask))) {
    temp->rtai_rt_desc = rt_task_init_schmod(nam2num(temp->name), temp->prio, temp->stack_size,temp->max_msg_size, temp->policy, cpu_mask); //(int)cpu_mask);
    /*
    printf("[RTAI] inside %s : rtai_rt_desc: %p pid: %ld tid: %p name: %s nam2num: %d\n",
            __FUNCTION__,
            temp->rtai_rt_desc,
            getpid(),
            pthread_self(),
            temp->name,nam2num(temp->name)); rt_make_hard_real_time(); //printf("[RTAI] inside hard_real_time\n");
            */
    
    
    unsigned long expected = rt_get_time() + 200*period_cnt;

    //printf("[RTAI] KEVIN t->thread_uid desc func: %p %p %p\n",temp_rt_task, t->desc, t->func);

    if (isPeriodic) {
        //printf("t->thread_uid desc func: %p %p %p\n",temp_rt_task, t->desc, t->func);
        if ((ret = rt_task_make_periodic(temp->desc, expected, period_cnt)) != 0) {                  
            printf("[RTAI] could not make periodic with : %d (%s)\n",ret,strerror(-ret));
            return ret;
        }
    }

#elif KERN_XENOMAI
    //printf("here 7\n");
    
    if (isPeriodic) {
       rt_task_set_periodic(NULL, TM_NOW, period);    
    }

#else
    //printf("here 8\n");
    //Variable declarations //this should be moved from else
    struct timespec period_ts;
    ts_zero(&period_ts);
    period_ts.tv_sec = (period) / NS_PER_SEC;
    period_ts.tv_nsec = (period) % NS_PER_SEC;

                                  
    if (isPeriodic) {
        if(start_periodic(period_ts, my_period) == -1) {
            fprintf(stderr, "Could not make task periodic!\n");
            return -1;
        }
    }
#endif
    //printf("here 9\n");
    return ret;
}

int rt_task_end (period_param* my_period,int isPeriodic,int thread_id) {
    int ret = 0;
    //printf("[ALL] inside rt_task_end\n");
#if KERN_LITMUS
    CALL( task_mode(BACKGROUND_TASK) ); /* BACKGROUND_TASK */
#elif KERN_RTAI
    //rt_make_soft_real_time();
    //printf("[RTAI] inside rt_task_end\n");
    //vvvvTHIS CAUSES A SEGFAULTvvvv
    task temp = task_get( thread_id );
    rt_thread_delete(temp->rtai_rt_desc); //<-- kills everything
    //printf("[RTAI] after rt_task_end\n");
    //rt_task_delete(tsk);
#elif KERN_CHRONOS 
#elif KERN_XENOMAI
#else

    if (isPeriodic) {
        end_periodic(my_period);
    }
#endif
    return ret;
}

/** RT JOB PRIMITIVES **/
long rt_job_begin (int prio, int max_util, struct timespec* deadline, struct timespec* period, unsigned long exec_time, period_param* periodic_timer, int isPeriodic) {

    int ret = 0;
#if KERN_CHRONOS

    //Inside or outside rt_seg?
    if (isPeriodic) {
        wait_period(periodic_timer);
    }
    //RTIME before = rt_gettime();
    ret = begin_rtseg_self(TASK_RUN_PRIO, max_util, deadline, period, exec_time);
    //printf("[CHRONOS] rt_job_begin\n");
    //RTIME after = rt_gettime();
    //printf("chronos_begin_rtseg_self takes: %ldns\n",(after-before)); 

    return ret;
#elif KERN_LITMUS
    if (isPeriodic) {
        sleep_next_period();
    }
    return ret;

#elif KERN_RTAI 
    if (isPeriodic) {
        RTIME before,after;
        before = rt_get_time();
        ret = rt_task_wait_period();
        after = rt_get_time();
        //printf("[RTAI] rt_task_wait_period waited %lld cycles or %f ns\n",after-before,((float)(after-before)/1.900273f));

        if (ret)
            printf("rt_task_wait_period failed with err: %d (%s)\n",ret,strerror(ret));
    }
    return ret;

#elif KERN_XENOMAI 
    if (isPeriodic) {
        rt_task_wait_period(NULL);
    }
    return ret;

#else
    //printf("Inside rt_job_begin periodic_timer: %p\n",periodic_timer);
    if (isPeriodic) {
        wait_period(periodic_timer);
    }
    return ret; 
#endif
    return ret;
}

long rt_job_end (int prio) {
    long ret = 0;
#if KERN_CHRONOS

    //RTIME before = rt_gettime();
    ret = end_rtseg_self(TASK_CLEANUP_PRIO);
    //printf("[CHRONOS] rt_job_ended\n");
    //RTIME after = rt_gettime();
    //printf("chronos_end_rtseg_self takes: %ldns\n",(after-before)); 
    return ret;
#else
    return ret; 
#endif
}

/** TIMER_UTIL PRIMITIVES **/
inline void ts_zero(struct timespec* val) {
	val->tv_sec = 0;
	val->tv_nsec = 0;
}

/** PERIODIC PRIMITIVES **/
/*
 * Created (stolen from Rob) for kernels that do not support periodic task primitives (like Litmus^RT)
 */
/*
 * Method used to create a timer and start it.  Creates a timer with a period
 * specified by "period", for the timer handle "periodic_timer".
 */
int start_periodic(struct timespec period, period_param* periodic_timer) {

	//Initialize the timer
	int timer_fd = timerfd_create(CLOCK_REALTIME, 0);
	if(timer_fd == -1)
		return -1;

	//We succeeded in creating the timer, update the struct with the info
	periodic_timer->timer = timer_fd;
	periodic_timer->missed_wakeups = 0;

	//Now make the timer periodic
	struct itimerspec timer_value;
	timer_value.it_interval.tv_sec = period.tv_sec;
	timer_value.it_interval.tv_nsec = period.tv_nsec; //Specify the period
	timer_value.it_value.tv_sec = period.tv_sec;
	timer_value.it_value.tv_nsec = period.tv_nsec; //Specify the initial interval
	return timerfd_settime(timer_fd, 0, &timer_value, NULL);	
}

/*
 * Method used to sleep until the next period starts.
 */
void wait_period(period_param* periodic_timer) {

	//Wait for the timer to expire
	uint64_t num_missed;
	if(read(periodic_timer->timer, &num_missed, sizeof(uint64_t)) == -1) {
		perror("Could not read timer");
		return;
    }
	
	//Updated the number of missed intervals
	periodic_timer->missed_wakeups += (num_missed - 1);
    //printf("End of wait_period\n");

}

/*
 * Closes the timer.
 */
int end_periodic(period_param* periodic_timer) {
	if(close(periodic_timer->timer) == -1) {
		perror("Could not close the timer");
		return -1;
	}
    else
        return 0;
}

#if KERN_SCHED_DEAD
int sched_setscheduler2(pid_t pid, int policy, const struct sched_param2 *param) {
	return syscall(__NR_sched_setscheduler2, pid, policy, param);
}

int sched_setparam2(pid_t pid, const struct sched_param2 *param) {
	return syscall(__NR_sched_setparam2, pid, param);
}

int sched_getparam2(pid_t pid, struct sched_param2 *param) {
	return syscall(__NR_sched_getparam2, pid, param);
}

pid_t gettid2() {
	return syscall(SYS_gettid);
}

#endif

/*--- Semaphore Management Functions ---*/
/**
 * \brief Create a semaphore
 */
    //not tested
int librt_sem_init ( sem_rt *s ) {

    int ret = 0;

#if KERN_RTAI
    s = rt_sem_init(1, 1);
        if (!s) {
            printf("[RTAI] sem failed to init\n");
            ret = -1;
            return ret;
        }
    
#elif KERN_XENOMAI

    if (( ret = rt_sem_create(s, "sem", 1, S_FIFO)) != 0) {
        printf("[XENO] failed to create sem! with error: %d strerror: (%s)\n", ret, strerror(-ret));
        return ret;
    }
#else
    if ((ret = sem_init(s,0,1)) != 0){
        printf("[POSIX] sem_init failed with error: %d\n",ret);
        return ret;
    }
#endif
    return ret;
}

/**
 * \brief Destroy a sem
 */
int librt_sem_destroy ( sem_rt *s ) {

    int ret = 0;

#if KERN_RTAI
    if ((ret = rt_sem_delete(s)) != 0) {
        printf("[RTAI] failed to destroy sem!\n");
        return ret;
    }

#elif KERN_XENOMAI
    if ((ret = rt_sem_delete(s)) != 0) {
        printf("[XENO] failed to destroy sem!\n");
        return ret;
    }

#else
    if ((ret = sem_destroy(s)) != 0){
        printf("[POSIX] sem_destroy failed with error: %d\n",ret);
        return ret;
    }

#endif
    return ret;


}

/**
 * \brief tryLock a sem Kevin: depreciated... unfinished
 */
int librt_sem_trywait ( sem_rt *s ) {

    int ret = 0;

#if KERN_RTAI
#elif KERN_XENOMAI
    printf("[XENO] YOU STILL HAVE TO IMPLEMENT ME!");

#else
    if ((ret = sem_wait(s)) != 0){
        printf("[POSIX] sem_trywait failed with error: %d\n",ret);
        return ret;
    }

#endif
    return ret;
}

/**
 * \brief Lock a sem
 */
int librt_sem_wait ( sem_rt *s ) {
    int ret = 0;
#if KERN_RTAI
    if ((ret = rt_sem_wait(s)) != 0) {
        printf("[RTAI] failed to wait on sem!\n");
        return ret;
    }
#elif KERN_XENOMAI
    if (!s)
        printf("[XENO] semaphore is NULL!\n");

    if ((ret = rt_sem_p(s,TM_INFINITE)) != 0) {
        printf("[XENO] failed to pend on sem! with err: %d stderr: (%s)\n",ret,strerror(-ret));
        return ret;
    }

#else
    if ((ret = sem_wait(s)) != 0) {
        printf("[POSIX] sem_wait failed with error: %d\n",ret);
        return ret;
    }

#endif
    return ret;
}

/**
 * \brief Unlock a sem
 */
int librt_sem_post ( sem_rt * s ) {
    int ret = 0;
#if KERN_RTAI
    if ((ret = rt_sem_signal(s)) != 0) {
        printf("[RTAI] failed to signal sem!\n");
        return ret;
    }

#elif KERN_XENOMAI
    if ((ret = rt_sem_v(s)) != 0) {
        printf("[XENO] failed to signal sem!\n");
        return ret;
    }

#else
    if ((ret = sem_post(s)) != 0){
        printf("[POSIX] sem_post failed with error: %d\n",ret);
        return ret;
    }

#endif
    return ret;
}

/*
 * RDTSC gettime - used in rt_gettime
 */
//32bit option
/*
static __inline__ unsigned long long rdtsc(void)
{
        unsigned long long int x;
        __asm__ volatile (".byte 0x0f, 0x31" : "=A" (x));
            return x;
}
*/

int mask2dec (unsigned long mask) 
{
    int i = 0,dec = 0;
    unsigned long mask_before = mask;
    for (i=0;i<(sizeof(mask)*8);i++) {
        if ( mask & 0x1 )
            return dec;
        else
            dec++;

        mask >>= 1;
    }
    return dec;

    //printf("i=%d, dec=%d, mask=0x%lx, mask_before=0x%lx\n",i,dec,mask,mask_before);

}

//64bit option
static __inline__ unsigned long long rdtsc(void)
{
    unsigned hi, lo;
    __asm__ __volatile__ ("rdtsc" : "=a"(lo), "=d"(hi));
    return ( (unsigned long long)lo)|( ((unsigned long long)hi)<<32 );
}

static __inline__ TICKS getticks(void)
{
    unsigned a, d;
    asm("cpuid");
    asm volatile("rdtsc" : "=a" (a), "=d" (d));

    return (((TICKS)a) | (((TICKS)d) << 32));
}