#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <machine/atomic.h>
#include <pthread.h>
#include <sys/timespec.h>
#include <string.h>
#include <sys/queue.h>
#include <limits.h>

#define NUM_THREADS 6
#define RW_LOCK_MASK 0x00000003
#define RW_LOCK_SHIFT 2


unsigned long long threads_count[NUM_THREADS];
unsigned long long thread_ptr_seen[NUM_THREADS];
unsigned long long thread_passes[NUM_THREADS];
unsigned long long thread_writer_passes = 0;
unsigned long long thread_writer_even_passes = 0;

struct mainlist {
	struct mainlist *next;
	uint64_t spin_count;
	uint64_t addvalue;
};

struct mainlist *head = NULL, *oldo = NULL, *olde = NULL;


struct mylock_t {
	u_long count;
	u_long reader_wait;
	u_long readers_cnt;
} lock;


int
rdunlock(struct mylock_t *rw)
{
	u_long riq, num, mask, nirq, old_val;
	u_long flag;

retry:
	riq = rw->count;
	mask = riq & RW_LOCK_MASK;
	num = riq >> RW_LOCK_SHIFT;
	if (num) {
		num--;
		nirq = num << RW_LOCK_SHIFT | mask | flag;
		if (atomic_cmpset_long(&rw->count, riq, nirq) == 0) {
			goto retry;
		}
	}
}

int
rdlock(struct mylock_t *rw)
{
	u_long riq, num, mask, nirq, old_val;
	int i;

try_again:
	riq = rw->count;
	mask = riq & RW_LOCK_MASK;
	num = riq >> RW_LOCK_SHIFT;
	if (mask == 0) {
		num++;
		nirq = (num << RW_LOCK_SHIFT) | mask;
		if (atomic_cmpset_long(&rw->count, riq, nirq) == 0) {
			goto try_again;
		}
	} else {
	    /* We should only hit here once - before we release the dragons :-) */
		atomic_fetchadd_long(&rw->readers_cnt, 1);
		old_val = atomic_fetchadd_long(&rw->reader_wait, 1);
		old_val++;
		_thr_umtx_wait((u_long *)&rw->reader_wait, old_val, NULL);
		atomic_fetchadd_long(&rw->readers_cnt, -1);
		goto try_again;
	}
	return (0);
}

void
release_the_dragons(struct mylock_t *rw)
{
	u_long riq, num, mask, nirq, old_val;
	lwpid_t me;

	if (rw->count!= 1) {
	  printf("Gak gak.. count is %lx\n", rw->count);
	  return;
	}
 again:
	
	if (atomic_cmpset_long(&rw->count, 1, 0) == 0) {
	  rw->count = 1;
	  printf("huh?\n");
	  goto again;
	}
	
	if (rw->readers_cnt) {
		/* Wake latent readers first so they all get a chance */
		atomic_fetchadd_long(&rw->reader_wait, 1);
		_thr_umtx_wake((u_long *)&rw->reader_wait, INT_MAX);
	}
	return;
}

void *
thread_reader(void *num)
{
	struct mainlist *ptr;
	uint64_t i;
	int myid;
	long myindex;
	uint32_t count;

	myindex = (long)num;
	myid = myindex + 1;
	
	for (count = 0; count < 0xfffff; count++) {
		rdlock(&lock);
		ptr = head;
		while (ptr != NULL) {
			/*
			 * for(i=0;i<ptr->spin_count; i++) { ; }
			 */
			threads_count[myindex] += ptr->addvalue;
			thread_ptr_seen[myindex]++;
			ptr = ptr->next;
		}
		thread_passes[myindex]++;
		rdunlock(&lock);
	}
	pthread_exit(NULL);
	return (NULL);
}


void
print_report()
{
	int i;
	for (i = 0; i < NUM_THREADS; i++) {
		printf("writer passes odd:%lld even:%lld\n", thread_writer_passes,
		    thread_writer_even_passes);
		printf("Thread:%d pass:%lld nodes:%lld count:%lld\n",
		    i, thread_passes[i], thread_ptr_seen[i],
		    threads_count[i]);
	}
	printf("************\n");
}

void *
thread_rpt(void *foo)
{
    struct timespec ts;
    ts.tv_sec = 3;
    ts.tv_nsec = 0;

	while (1) {
	    nanosleep(&ts, NULL);  
		print_report();
	}
	return (NULL);
}

void
do_setup_for_all()
{
	struct timespec ts;
	struct mainlist *tmp;
	long rand1, rand2, rand3, i;

	/* Gather the count of nodes. */
	rand1 = random() % 4096;
	rand2 = random() % 4096;
	rand3 = random() % 4096;
	/* Create our lists */
	for (i = 0; i < rand1; i++) {
		tmp = malloc(sizeof(struct mainlist));
		memset(tmp, 0, sizeof(struct mainlist));
		tmp->next = head;
		tmp->spin_count = random() % 65536;
		tmp->addvalue = random() % 32555;
		head = tmp;
	}
	for (i = 0; i < rand2; i++) {
		tmp = malloc(sizeof(struct mainlist));
		memset(tmp, 0, sizeof(struct mainlist));
		tmp->next = oldo;
		tmp->spin_count = random() % 65536;
		tmp->addvalue = random() % 32555;
		oldo = tmp;
	}
	for (i = 0; i < rand3; i++) {
		tmp = malloc(sizeof(struct mainlist));
		memset(tmp, 0, sizeof(struct mainlist));
		tmp->next = olde;
		tmp->spin_count = random() % 65536;
		tmp->addvalue = random() % 32555;
		olde = tmp;
	}
}


int
main(int argc, char **argv)
{
	long i;
	unsigned long long total;
	pthread_t thread_list[NUM_THREADS];
	pthread_t writer;
	pthread_t writer2;
	pthread_t reader;

	memset(threads_count, 0, sizeof(threads_count));
	memset(thread_passes, 0, sizeof(thread_passes));
	memset(thread_ptr_seen, 0, sizeof(thread_ptr_seen));
	memset(&lock, 0, sizeof(lock));
	
	/* Setup so that we lock out all threads */
	lock.count = 1;
	
	srandom(100);
	for (i = 0; i < NUM_THREADS; i++) {
		threads_count[i] = 0;
	}

	do_setup_for_all();
	
	if (pthread_create(&reader, NULL, thread_rpt, NULL)) {
		printf("Can't start reporter thread\n");
		exit(0);
	}
	for (i = 0; i < NUM_THREADS; i++) {
		if (pthread_create(&thread_list[i], NULL, thread_reader, (void *)i)) {
			printf("Can't start thread %d\n", i);
			exit(0);
		}
	}
	release_the_dragons(&lock);
	/* now all must complete */
	for (i = 0; i < NUM_THREADS; i++) {
		pthread_join(thread_list[i], NULL);
	}
	print_report();
	return (0);
}