#include <u.h>
#include <libc.h>
#include "mcslock.h"

extern void pause(void);

void
·mcslock(Qnode **L, Qnode *l)
{
	Qnode *prev;

	l->next = nil;
	prev = fasp(L, l);
	if(prev != nil){
		l->locked = 1;
		prev->next = l;
		while(l->locked)
			pause();
	}
}

int
·mcscanlock(Qnode **L, Qnode *l)
{
	l->next = nil;
	return casp(L, nil, l);
}

void
·mcsunlock(Qnode **L, Qnode *l)
{
	if(l->next == nil){
		do{
			if(casp(L, l, nil))
				return;
			pause();
		}while(l->next == nil);
	}
	l->next->locked = 0;
}

#include <tos.h>

enum {
	Nbucket		= 64,
	Cachelinesz	= 64,
};

typedef struct Qstore Qstore;
struct Qstore {
	union{
		struct {
			Qnode;				/* must be first */
			int	pid;
			Lock	*lk;
			Qstore	*next;
		};
		uchar	pad[Cachelinesz];
	};
};

static Qstore *qslock[Nbucket];
static Qstore *qsfree[Nbucket];

/* definition of pidhash is bogus */
#define pidhash()	(_tos->pid%Nbucket)
#define	splhi()		(0)
#define	splx(x)		USED(x)
typedef	int	Mpl;

Qstore*
qalloc(Lock *l)
{
	int h;
	Qstore *q;
	Mpl pl;

	h = pidhash();
	pl = splhi();

	/* find node */
	if((q = qsfree[h]) == nil){
		q = mallocz(sizeof *q, 1);
		q->pid = _tos->pid;
	} else {
		assert(q->pid == _tos->pid);
		qsfree[h] = q->next;
		q->next = nil;
	}

	q->lk = l;

	/* link on locked list */
	q->next = qslock[h];
	qslock[h] = q;

	splx(pl);
	return q;
}

Qstore*
qlocked(Lock *l, int h)
{
	Qstore **qq, *x;
	Mpl pl;

	pl = splhi();

	for(qq =&qslock[h]; (x = *qq) != nil; qq = &x->next)
		if(x->lk == l && x->pid == _tos->pid)
			break;
	if(x != nil)
		*qq = x->next;
	splx(pl);

	if(x == nil){
		print("mcslock: qlocked: not locked\n");
abort();
		return nil;
	}

	x->next = nil;
	return x;
}

void
qfree(Qstore *q, int h)
{
	Mpl pl;

	pl = splhi();
	q->next = qsfree[h];
	qsfree[h] = q;
	splx(pl);
}

void
mcslock(Lock *l)
{
	·mcslock(l, qalloc(l));
}

int
mcscanlock(Lock *l)
{
	int h;
	Qstore *q;
	Mpl pl;

	q = qalloc(l);
	if(·mcscanlock(l, q))
		return 1;

	/* clean up */
	h = pidhash();
	pl = splhi();
	q = qlocked(l, h);
	if(q != nil){
		assert(q->pid == _tos->pid);
		·mcsunlock(l, q);
		qfree(q, h);
	}
	splx(pl);
	return 0;
}

void
mcsunlock(Lock *l)
{
	int h;
	Qstore *q;
	Mpl pl;

	h = pidhash();

	pl = splhi();
	q = qlocked(l, h);
	if(q != nil){
		assert(q->pid == _tos->pid);
		·mcsunlock(l, q);
		qfree(q, h);
	}
	splx(pl);
}