/*    Filename: prio-preempt.c
 *      Author: Dinakar Guniguntala <dino@us.ibm.com>
 * Description: Test priority preemption
 *
 * 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.
 *
 * Copyright (C) IBM Corporation, 2006
 *
 * 2006-Jun-01:	Initial version by Dinakar Guniguntala
 *              Changes from John Stultz and Vivek Pallantla
 */

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <time.h>
#include <pthread.h>
#include <sched.h>
#include <errno.h>
#include <sys/syscall.h>
#include "librt.h"

#define NUM_WORKERS	27
#define CHECK_LIMIT	1

volatile int busy_threads = 0;
volatile int test_over = 0;
volatile int threads_running=0;
static int rt_threads = 0;
static pthread_mutex_t  bmutex = PTHREAD_MUTEX_INITIALIZER;

static pthread_mutex_t mutex[NUM_WORKERS+1];
static pthread_cond_t cond[NUM_WORKERS+1];
static int t_after_wait[NUM_WORKERS];

#define ADD_INTERRUPTER_THREADS
#ifdef ADD_INTERRUPTER_THREADS
void *int_thread(void* arg)
{
	int j, a = 0;
	while (!test_over) {
		/* do some busy work */
		if(!(a%4))
			a = a * 3;
		else if (!(a%6))
			a = a /2;
		else
			a++;
		usleep(10);
	}
	return (void*)a;
}
#endif

void *busy_thread(void* arg)
{
	struct sched_param sched_param;
	int policy, mypri = 0, tid;
	tid = (int) (((struct thread *) arg)->arg);

	if (pthread_getschedparam(pthread_self(), &policy, &sched_param) != 0) {
		printf("ERR: Couldn't get pthread info \n");
	} else {
		mypri = sched_param.sched_priority;
	}

	pthread_mutex_lock(&bmutex);
	busy_threads++;
	printf("Busy Thread %d(%d): Running...\n", tid, mypri);
	pthread_mutex_unlock(&bmutex);

	/* TODO: Add sched set affinity here */

	/* Busy loop */
	while (!test_over);

	printf("Busy Thread %d(%d): Exiting\n", tid, mypri);
	return NULL;
}

void *worker_thread(void* arg)
{
	struct sched_param sched_param;
	int policy, rc, mypri = 0, tid, times = 0;
	tid = (int) (((struct thread *) arg)->arg);
	nsec_t pstart, pend;
	
	if (pthread_getschedparam(pthread_self(), &policy, &sched_param) != 0) {
		printf("ERR: Couldn't get pthread info \n");
	} else {
		mypri = sched_param.sched_priority;
	}
	/* check in */
	pthread_mutex_lock(&bmutex);
	threads_running++;
	pthread_mutex_unlock(&bmutex);

	/* block */
	rc = pthread_mutex_lock(&mutex[tid]);
	rc = pthread_cond_wait(&cond[tid], &mutex[tid]);
	rc = pthread_mutex_unlock(&mutex[tid]);
	
	printf("%llu: Thread %d(%d) wakes up from sleep \n", rt_gettime(), tid, mypri);
	
	/*check if we're the last thread */
	if (tid == NUM_WORKERS-1) {
		t_after_wait[tid] = 1;
		pthread_mutex_lock(&bmutex);
		threads_running--;
		pthread_mutex_unlock(&bmutex);
		return NULL;
	}

	/* Signal next thread */
	rc = pthread_mutex_lock(&mutex[tid+1]);
	rc = pthread_cond_signal(&cond[tid+1]);
	printf("%llu: Thread %d(%d): Sent signal (%d) to (%d)\n", rt_gettime(), tid, mypri,
                        rc, tid+1);

   	pstart = pend = rt_gettime();
	rc = pthread_mutex_unlock(&mutex[tid+1]);

	printf("%llu: Thread %d(%d) setting it's bit \n",rt_gettime(), tid, mypri);

	t_after_wait[tid] = 1;

	while (t_after_wait[tid+1] != 1) {
		pend = rt_gettime();
		times++;
	}

	if (times >= CHECK_LIMIT) {
		printf("Thread %d(%d): Non-Preempt limit reached. %llu ns max latency\n",
				tid, mypri, pend-pstart);
		exit(1);
	}

	/* check out */
	pthread_mutex_lock(&bmutex);
	threads_running--;
	pthread_mutex_unlock(&bmutex);

	return NULL;
}

void *master_thread(void* arg)
{
	int i, pri_boost;

#ifdef ADD_INTERRUPTER_THREADS
	/* start interrupter thread */
	pri_boost = 90;
	for (i = 0; i < rt_threads; i++) {
		create_fifo_thread(int_thread, (void*)0,
			sched_get_priority_min(SCHED_FIFO) + pri_boost);
	}
#endif

	/* start the (N-1) busy threads */
	pri_boost = 80;
	for (i = rt_threads; i > 1; i--) {
                create_fifo_thread(busy_thread, (void*)i,
			sched_get_priority_min(SCHED_FIFO) + pri_boost);
	}

	/* make sure children are started */
	while (busy_threads < (rt_threads-1))
		sleep(1);

	printf("Busy threads created!\n");

	/* start NUM_WORKERS worker threads */
	for (i = 0, pri_boost = 10; i < NUM_WORKERS ; i++, pri_boost+=2) {
		pthread_mutex_init(&mutex[i], NULL);
		pthread_cond_init(&cond[i], NULL);
                create_fifo_thread(worker_thread, (void*)i,
				sched_get_priority_min(SCHED_FIFO) + pri_boost);
	}


	printf("Worker threads created\n");
	/* Let the worker threads wait on the cond vars */
	while (threads_running < NUM_WORKERS)
		sleep(2);

	printf("Signaling first thread\n");
	pthread_mutex_lock(&mutex[0]);
	pthread_cond_signal(&cond[0]);
	pthread_mutex_unlock(&mutex[0]);

	while (threads_running)
		sleep(20);

	test_over = 1;
	return NULL;
}

int parse_args(int c, char *v)
{
	int handled = 1;
	switch (c) {
		case 'n':
			rt_threads = atoi(v);
			break;
		default:
			handled = 0;
			break;
	}
	return handled;
}

int main(int argc, char* argv[])
{
	int pri_boost;

	rt_init("n:", parse_args, argc, argv);

	if (rt_threads == 0) {
		printf("Usage: %s -n num_rt_threads\n", argv[0]);
		exit(1);
	}

	pri_boost = 81;
        create_fifo_thread(master_thread, (void*)0,
		 sched_get_priority_min(SCHED_FIFO) + pri_boost);

        /* wait for threads to complete */
        join_threads();

	return 0;
}