#include <rtl.h>
#include <time.h>
#include <pthread.h>

#define NTASKS 3
static pthread_t threads[NTASKS];

static pthread_mutex_t mutex /* = PTHREAD_MUTEX_INITIALIZER */;

static void * start_routine(void *arg)
{
	int ret;
	hrtime_t t;
	hrtime_t t2;
	int sleep = 500000000;
	int nthread = (int) arg;

	struct sched_param p;
	p . sched_priority = 1;
	pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);

	rtl_printf("thread %d starts on CPU%d\n", nthread, rtl_getcpuid());
	ret = pthread_mutex_trylock(&mutex);
	rtl_printf ("thread %d: pthread_mutex_trylock returned %d\n", nthread, ret);

	if (ret != 0) {
		rtl_printf("thread %d: about to pthread_mutex_lock\n", nthread);
		t = gethrtime();
		ret = pthread_mutex_lock (&mutex);
		t2 = gethrtime();
		rtl_printf("thread %d: pthread_mutex_lock returned %d (%d ns elapsed)\n", nthread, ret, (unsigned) (t2 - t));
	}
	rtl_printf("thread %d is about to sleep for %d ns\n", nthread, sleep);
	nanosleep (hrt2ts(sleep), NULL);

	ret = pthread_mutex_unlock (&mutex);

	rtl_printf("thread %d: pthread_mutex_unlock returned %d\n", nthread, ret);

	return (void *) 35 + nthread;
}


int init_module(void)
{
	int ret;
	int i;
	pthread_attr_t attr;

	pthread_mutex_init (&mutex, 0);

	rtl_printf("RTLinux mutex test starts on CPU%d\n", rtl_getcpuid());
	pthread_attr_init (&attr);
	for (i = 0; i < NTASKS; i++) {
		/* try to run one thread on another CPU */
		if (i == 1 && rtl_cpu_exists(!rtl_getcpuid())) {
			pthread_attr_setcpu_np(&attr, !rtl_getcpuid());
		}


		ret = pthread_create (&threads[i], &attr, start_routine, (void *) i);
		if (ret) {
			rtl_printf("failed to create a thread\n");
			return ret;
		}
	}


	return 0;
}


void cleanup_module(void)
{
	void *retval;
	int i;
	for (i = 0; i < 3; i++) {
		pthread_join (threads[i], &retval);
		rtl_printf("pthread_join on thread %d returned %d\n", i, (int) retval);
	}
	pthread_mutex_destroy (&mutex);
}