#define _GNU_SOURCE
#include <limits.h>
#include <stdbool.h>
#include <unistd.h>
#include <errno.h>
#include <linux/types.h>
#include <time.h>
#include <sys/resource.h>
#include <sched.h>
#include <setjmp.h>
#include <signal.h>
#include <pthread.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

//#define PRINT_LOCKS
//#define PRINT_ATTEMPTS
/* Seconds */
#define TIMED_TEST 0
/* Loops, not used if TIMED_TEST != 0 */
#define TRIES_TODO CLOCKS_PER_SEC

typedef unsigned int uint;
typedef unsigned long int ulong;
typedef struct _LOCK
{
	uint num;
	void *ud;
	struct timespec ts;
	volatile pid_t tid;
	volatile pid_t trying;
} LOCK;

volatile LOCK *_shared = NULL;

void lock_handler( int signal )
{
	/* We don't want the pointer we're working with to change midway
	 * through so we take a copy then work with that */
	volatile LOCK *shared = _shared;
	(void)signal;

	if ( !(shared->tid) )
		shared->tid = shared->trying;
#ifdef PRINT_ATTEMPTS
	flockfile( stdout );
	printf( "Thread %lu attempted lock\n", (ulong)(shared->trying) );
	funlockfile( stdout );
#endif
}

int LockSiData( LOCK *shared )
{
	int const sig = SIGCONT;
	pid_t tid = gettid(), was;
	struct sigaction this = {NULL}, prev = {NULL};

	/* Possible our signal handler will be called before _shared is not
	 * NULL so we set it prior to trying then continue on */
	_shared = shared;
	this.sa_handler = lock_handler;

	sigaction( sig, &this, &prev );

	for ( was = shared->tid; was != tid; was = shared->tid )
	{
		if ( !was )
		{
			shared->trying = tid;
			_shared = shared;
			kill( getpid(), sig );
		}
	}

	sigaction( sig, &prev, &this );

	clock_gettime( CLOCK_PROCESS_CPUTIME_ID, &(shared->ts) );
	shared->num++;

#ifdef PRINT_LOCKS
	flockfile( stdout );
	printf( "Thread %lu took lock\n", (ulong)tid );
	funlockfile( stdout );
#endif
	return 0;
}

int FreeSiData( LOCK *shared )
{
	pid_t tid = gettid();
	if ( shared->tid != tid )
		return 0;
	shared->num--;
	if ( shared->num )
		return 0;
#ifdef PRINT_LOCKS
	flockfile( stdout );
	printf( "Thread %lu released lock\n", (ulong)tid );
	funlockfile( stdout );
#endif
	shared->tid = (pid_t)0;
	return 0;
}

LOCK tlock = {0};
pthread_mutex_t mutex;

typedef int (*lock_cb)( void *ud );
typedef struct _TEST
{
	volatile uint quit;
	volatile uint data;
	void *ud;
	char *name;
	lock_cb lock;
	lock_cb free;
} TEST;

void* Abort( TEST *test, uint got, uint expected, clock_t start )
{
	ulong ticks = (ulong)(clock() - start);
	test->free( test->ud );
	flockfile( stdout );
	printf
	(
		"Thread %lu (lock%s) ended at %lu ticks, "
		"got = %u, expected %u\n",
		(ulong)gettid(), test->name, ticks, got, expected
	);
	funlockfile( stdout );
	exit(1);
	/* Prevents going further than expected */
	return test;
}

void* thread( void *ud )
{
	TEST *test = ud;
	uint got, expected;
	pid_t tid = gettid();
	clock_t start = clock(), end = start + (CLOCKS_PER_SEC * TIMED_TEST);
	struct timespec ts = {0};
	ts.tv_nsec = 1;
	(void)ud;

	flockfile( stdout );
	printf( "Thread %lu (lock%s)\n", (ulong)tid, test->name );
	funlockfile( stdout );

#if TIMED_TEST
	while ( end > clock() )
#else
	while ( test->quit < TRIES_TODO )
#endif
	{
		test->lock( test->ud );

		expected = 0;
		got = (test->data)++;
		if (got != expected)
			return Abort( test, got, expected, start );

		clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, 0);

		expected = 1;
		got = (test->data)--;
		if (got != expected)
			return Abort( test, got, expected, start );

		test->quit++;
		test->free( test->ud );
	}

	end = clock();
	flockfile( stdout );
	printf
	(
		"lock%s (%lu) took %5lu clock ticks\n",
		test->name, (ulong)tid, (ulong)(end - start)
	);
	funlockfile( stdout );
	return ud;
}

int main()
{
	pthread_t pt;
	int i;

	TEST *test;
	TEST tests[2] = {{0}};

	setbuf(stdout,NULL);

	test = tests;
	test->ud = &tlock;
	test->name = "sidata";
	test->lock = (lock_cb)LockSiData;
	test->free = (lock_cb)FreeSiData;

	test = tests + 1;
	test->ud = &mutex;
	test->name = "mutex";
	test->lock = (lock_cb)pthread_mutex_lock;
	test->free = (lock_cb)pthread_mutex_unlock;

	for (i = 0; i < 2; i++)
	{
		if ((errno = pthread_create(&pt, 0, thread, tests)) != 0 )
		{
			flockfile( stdout );
			printf("pthread_create failed: %m\n");
			funlockfile( stdout );
			return 1;
		}

		if ((errno = pthread_create(&pt, 0, thread, tests + 1)) != 0 )
		{
			flockfile( stdout );
			printf("pthread_create failed: %m\n");
			funlockfile( stdout );
			return 1;
		}
	}

	pthread_exit(0);
	pthread_mutex_destroy( &mutex );
}