switchtest.c

/*
 * Tests context switching times based on a semaphore in the RTAI kernel.
 * Author: Rob Lyerly
 */

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <sched.h>
#include <sys/sysinfo.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <librttest.h>
#include "barrier.h"
#include <librttest.h>

//#include <rtai_sem.h>

#define THREAD_PRIORITY 99

#define NANO 1000000000

/* Integer to affine a task to all CPUs */
#define ALL_CPUS 0x00ffffff

/* Memory buffer used to record statistics */
static long** statistics;

/* Variables used to synchronize/time/perform the context switch */
//static SEM** sem;
static mutex* sem; //Kevin: made single pointer rather than double
static RTIME* before;

static barrier_t barrier;
//static pthread_barrier_t barrier;

//static mutex lock = PTHREAD_MUTEX_INITIALIZER;
//static pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;

/* Thread parameters used to setup the thread */
typedef struct _threadparam {
	int numLoops;
	int useAffinity;
	int threadnum;
	int schedulingPolicy;
	int numThreads;
	cpu_set_t cpu;
} thread_param;

#define DEBUG 1

void debug_msg(char * msg) {
    #if DEBUG==1
        printf("%s\n",msg);
    #endif

}

static void* thread_func(void* param) {
    

	thread_param * my_param = (thread_param *)param;
    
	int i = 0, tnum = my_param->threadnum;
    printf("Kevin: inside thread_%d\n",my_param->threadnum);
    
    //int error = 0;

	//Check to see if we want to use thread affinities to pin threads to CPUs
    /*
	if(my_param->useAffinity) {
		err = pthread_setaffinity_np(myself, sizeof(my_param->cpu), &(my_param->cpu));
		if(err) {
			fprintf(stderr, "[%d] Could not set thread affinity: ", my_param->threadnum);
			perror("");
			fprintf(stderr, "\n");
		}
	}
    */
    /* Kevin: should already be real time */
	
    //Declare ourselves as an RTAI real-time task
	//RT_TASK* rt_self;
	//task rt_self = taskset_init(1);     //taken from lachesis' latency_sched
    //setup();
	
    //Kevin: what is the point?
   // mutex_lock(&lock);

    //return rt_task_init_schmod(name, priority, 0, max_msg_size, policy, cpus_allowed);
    //rt_task_init_schmod,(int name, int priority, int stack_size, int max_msg_size, int policy, int cpus_allowed))
    //rt_self.name = nam3num(taskname);
    //rt_self.stack_size = 0;
    //rt_self.max_msg_size = 0;
    //rt_self.policy = SCHED_FIFO;

    //rt_self->func = ;
    //rt_self->priority = ;

    
	/*if(error == -1) {
		fprintf(stderr, "[%d] Error %p - could not convert to a real-time task!", my_param->threadnum, rt_self);
		munlockall();
		return NULL;
	}
    */


	//mutex_unlock(&lock);


	//Variable declarations
	RTIME after;

	//Work loop
	/*
    rt_set_periodic_mode();
	start_rt_timer(PERIOD);
	rt_task_make_periodic_relative_ns(rt_self, 20000, PERIOD);
    
	printf("Done!\n");*/
    debug_msg("tf: before thread_func's real time seg");
	//rt_make_hard_real_time(); //Kevin will remove
    //TODO: implement rt_task_begin (rtai rt_make_hard_realtime)
    
	for(i = 0; i < my_param->numLoops; i++) {

        //TODO: implement rt_job_begin (rtai is empty)
		//Wait at the barrier
        debug_msg("tf: barrier wait 1");
		barrier_wait(&barrier);

		//Wait on the semaphore
		//rt_sem_wait(sem[tnum]);
        debug_msg("tf: lock mutexes");
        mutex_lock(&sem[tnum]);

		//Get the time
		//after = rt_get_time_ns_cpuid(tnum); //implement this in librttest
        after = rt_gettime();
		statistics[tnum][i] = after - before[tnum];

        debug_msg("tf: barrier wait 2");
		barrier_wait(&barrier);

		//Release the semaphore
		//rt_sem_signal(sem[tnum]);
        debug_msg("tf: unlock mutexes");
        mutex_unlock(&sem[tnum]);

		//Wait until the next period
		//rt_task_wait_period();    //implement in librttest
	}
	//rt_make_soft_real_time();
	//stop_rt_timer();
    debug_msg("tf: done with thread, time to leave");

	return NULL;
}

int switchtest(int threads, int loops, int affinity, int policy, char* fn) {

	//pthread_t* thread;
	thread_param* param;
	int i = 0, j = 0;
	char sem_name[40];
    int ret=0;

	//Need a thread for initiating the context switch by releasing the semaphore
	if(threads < 2) {
		printf("Must have at least two threads for the switch test!\n");
		return -1;
	}

	mlockall(MCL_CURRENT | MCL_FUTURE);

	//RT_TASK* mainbuddy = rt_task_init_schmod(nam2num("MASTER"), 0, 0, 0, SCHED_FIFO, ALL_CPUS);
	/*thr_t* mainbuddy = rt_task_init_schmod(nam2num("MASTER"), 0, 0, 0, SCHED_FIFO, ALL_CPUS);
	if(!mainbuddy) {
		printf("Could not make the main task real-time!\n");
		return -1;
	}
    */
    
    //Kevin create RT threads
    
	//Kevin  1) Make the main task real-time
#if KERN_TRAI || KERN_XENOMAI
    task t = taskset_init(threads);
    t->name = "MASTER";
    t->prio = 0;
    t->max_msg_size = 0;
    t->policy = SCHED_FIFO;
    t->cpus_allowed = ALL_CPUS;
    task_create(t);
    debug_msg("1) Main is RT");
#else
    task t = taskset_init(threads-1); //exclude main
    debug_msg("1) Main is not RT!");
#endif
   

    //Kevin TODO Find librttest equivalent
	//start_rt_timer(0); //use gettime instead
    //RTIME start = rt_gettime();

	//Allocate the statistics buffer
	statistics = (long**)calloc(threads-1, sizeof(long));
	for(i = 0; i < threads-1; i++)
		statistics[i] = (long*)calloc(loops, sizeof(long));
    
	//Start the requested number of threads
	/*thread = (pthread_t*)calloc(threads - 1, sizeof(pthread_t));
	sem = (SEM**)calloc(threads - 1, sizeof(SEM*));
	before = (RTIME*)calloc(threads - 1, sizeof(RTIME));
	pthread_barrier_init(&barrier, NULL, threads);
    */
    //int barrier_init( barrier_t *bp, int count, int type, void *arg ) {
	param = (thread_param*)calloc(threads - 1, sizeof(thread_param));

	sem = (mutex**)calloc(threads - 1, sizeof(mutex*));
	before = (RTIME*)calloc(threads - 1, sizeof(RTIME));

    //Kevin: create and integrate the barrier class into librttest    
    debug_msg("before barrier init");
    printf("threads=%d\n",threads);
    barrier_init(&barrier, threads, 0, NULL);
    debug_msg("barrier initialized");
    //pthread_barrier_init(&barrier, NULL, threads);

    //Kevin TODO 2) Create semaphors and lock them 
	for(i = 0; i < threads-1; i++) {
		param[i].numLoops = loops;
		param[i].useAffinity = affinity;
		param[i].threadnum = i;
		param[i].schedulingPolicy = policy;
    
        /*
		sprintf(sem_name, "sem_%d", i);
		sem[i] = rt_sem_init(nam2num(sem_name), 1);
		rt_sem_wait(sem[i]);
		CPU_ZERO(&(param[i].cpu));
		CPU_SET(i, &(param[i].cpu));
		thread[i] = rt_thread_create(thread_func, &(param[i]), 0);
		if(!thread[i])
			perror("Could not create thread");
        */
	}
    debug_msg("thread parameters set");
	for(i = 0; i < threads - 1; i++) {
		sprintf(sem_name, "sem_%d", i);

		mutex_init(&sem[i]); //error thrown from this
		mutex_lock(&sem[i]); //mutex is uninitialized here
	}
    debug_msg("2) mutexes are created and locked");
	
    char taskname[7];
    
    //Kevin 3) Spawn RT threads
	for (i = 0; i < threads-1; i++, t++) {
	    sprintf(taskname, "%d",i);
		t->func = thread_func;
		t->arg = (void *)&(param[i]);
		//t->arg = (void*)i;
		t->prio = THREAD_PRIORITY;
        t->name = taskname;
        t->stack_size = 0;
        t->max_msg_size = 0;
        t->policy = SCHED_FIFO;
	 
        //if ((ret = pthread_create(&t->desc, &t->attr, t->func, t->arg)));

        
        if(param[i].useAffinity) {
            t->cpus_allowed = 1 << i;
        }
        else {
        
            t->cpus_allowed = ALL_CPUS;
        }
        
        //rt_task_init_schmod,(int name, int priority, int stack_size, int max_msg_size, int policy, int cpus_allowed))
        //rt_self.name = nam3num(taskname);
        //rt_self.stack_size = 0;
        //rt_self.max_msg_size = 0;
        //rt_self.policy = SCHED_FIFO;
        
		if ((ret = task_create(t)) < 0) {
            printf("task%d failed", i);
			exit(ret);
        } else {
            debug_msg("creating task");
        }
        //debug_msg("starting tasks");
        //task_start(i);
    }
    debug_msg("3) threads are spawned");
	
//	rt_make_hard_real_time();   //remove for now as this isn't implemented in librt, however, look into adding this when adding chronos' begin_rt and end_rt calls
	for(i = 0; i < loops; i++) {
		//Wait for all threads to be ready
        //chronos' realtime segment will go here
        
        debug_msg("before first barrier_wait");
        barrier_wait(&barrier);
        debug_msg("after first barrier_wait");
		//pthread_barrier_wait(&barrier);

		//Set the before time and signal the semaphore
		for(j = 0; j < threads - 1; j++) {
			before[j] = rt_gettime();   //rt_get_time_ns_cpuid(j); //gets local timestamp per core
			mutex_unlock(&sem[j]);       //rt_sem_signal(sem[j]);
		}

        //Kevin: created a barrier implementation
        barrier_wait(&barrier);
		//pthread_barrier_wait(&barrier);

		//Take back all the semaphores
		/*
         for(j = 0; j < threads - 1; j++) {
			mutex_wait(sem[j]);       //rt_sem_waie(sem[j]);
			//rt_sem_wait(sem[j]);
		}
        */
	}
//	rt_make_soft_real_time();//integrate into librttest
    debug_msg("out of main's rt portion");
    //stop_rt_timer();    //look into equivalent

	//Join the threads
	munlockall();
    task_join_all();
    debug_msg("threads joined");
    /*
	for(i = 0; i < threads - 1; i++) {
		rt_sem_delete(sem[i]);
		rt_thread_join(thread[i]);
	}
	rt_task_delete(mainbuddy);
    */

//    task_join_all();
//    debug_msg("threads joined");

	//Write statistics to file
	char filename[40];
	char thread_string[5];
	sprintf(thread_string, "%d", (threads-1));
	strcpy(filename, "/data/");
	strcat(filename, fn);
	strcat(filename, thread_string);
	strcat(filename, ".dat");
	FILE* fp = fopen(filename, "a");
	for(i = 0; i < threads - 1; i++) {
		for(j = 0; j < loops; j++) {
			fprintf(fp, "%ld\n", statistics[i][j]);
		}
	}
	fclose(fp);

	for(i = 0; i < threads - 1; i++)
		free(statistics[i]);
	free(statistics);
	//free(thread);
	free(param);
	free(sem);
	free(before);
	//pthread_barrier_destroy(&barrier);
	barrier_destroy(&barrier);
    debug_msg("switchtest ended");

	return 0;
}