/*Have a wait block*/
#define SRWLOCK_WAIT 1
/*Users are readers*/
#define SRWLOCK_SHARED 2
/*Bit-lock for editing the wait block*/
#define SRWLOCK_LOCK 4
#define SRWLOCK_LOCK_BIT 2
/*Mask for the above bits*/
#define SRWLOCK_MASK 7
/*Number of current users * 8*/
#define SRWLOCK_USERS 8
typedef struct srwlock srwlock;
struct srwlock
{
uintptr_t p;
};
typedef struct srw_sw srw_sw;
struct srw_sw
{
uintptr_t spin;
srw_sw *next;
};
typedef struct srw_wb srw_wb;
struct srw_wb
{
/*s_count is the number of shared acquirers * SRWLOCK_USERS.*/
uintptr_t s_count;
/*Last points to the last wait block in the chain. The value
is only valid when read from the first wait block.*/
srw_wb *last;
/*Next points to the next wait block in the chain.*/
srw_wb *next;
/*The wake chain is only valid for shared wait blocks*/
srw_sw *wake;
srw_sw *last_shared;
int ex;
};
/*Wait for control of wait block*/
static srw_wb *lock_wb(srwlock *l)
{
uintptr_t p;
/*Spin on the wait block bit lock*/
while (atomic_bitsetandtest(&l->p, SRWLOCK_LOCK_BIT)) cpu_relax();
p = l->p;
barrier();
if (!(p & SRWLOCK_WAIT))
{
/*Oops, looks like the wait block was removed.*/
atomic_clear_bit(&l->p, SRWLOCK_LOCK_BIT);
return NULL;
}
return (srw_wb *)(p & ~SRWLOCK_MASK);
}
static void srwlock_init(srwlock *l)
{
l->p = 0;
}
static void srwlock_rdlock(srwlock *l)
{
srw_wb swblock;
srw_sw sw;
uintptr_t p;
srw_wb *wb, *shared;
while (1)
{
barrier();
p = l->p;
cpu_relax();
if (!p)
{
/*This is a fast path, we can simply try to set the shared count to 1*/
if (!cmpxchg(&l->p, 0, SRWLOCK_USERS | SRWLOCK_SHARED)) return;
continue;
}
/*Don't interfere with locking*/
if (p & SRWLOCK_LOCK) continue;
if (p & SRWLOCK_SHARED)
{
if (!(p & SRWLOCK_WAIT))
{
/*This is a fast path, just increment the number of current shared locks*/
if (cmpxchg(&l->p, p, p + SRWLOCK_USERS) == p) return;
}
else
{
/*There's other waiters already, lock the wait blocks and increment the shared count*/
wb = lock_wb(l);
if (wb) break;
}
continue;
}
/*Initialize wait block*/
swblock.ex = FALSE;
swblock.next = NULL;
swblock.last = &swblock;
swblock.wake = &sw;
sw.next = NULL;
sw.spin = 0;
if (!(p & SRWLOCK_WAIT))
{
/** We need to setup the first wait block.
* Currently an exclusive lock is held, change the lock to contended mode.*/
swblock.s_count = SRWLOCK_USERS;
swblock.last_shared = &sw;
if (cmpxchg(&l->p, p, (uintptr_t)&swblock | SRWLOCK_WAIT) == p)
{
while (!sw.spin) cpu_relax();
return;
}
continue;
}
/*Handle the contended but not shared case*/
/** There's other waiters already, lock the wait blocks and increment the shared count.
* If the last block in the chain is an exclusive lock, add another block.*/
swblock.s_count = 0;
wb = lock_wb(l);
if (!wb) continue;
shared = wb->last;
if (shared->ex)
{
shared->next = &swblock;
wb->last = &swblock;
shared = &swblock;
}
else
{
shared->last_shared->next = &sw;
}
shared->s_count += SRWLOCK_USERS;
shared->last_shared = &sw;
/*Unlock*/
barrier();
l->p &= ~SRWLOCK_LOCK;
/*Wait to be woken*/
while (!sw.spin) cpu_relax();
return;
}
/*The contended and shared case*/
sw.next = NULL;
sw.spin = 0;
if (wb->ex)
{
/** We need to setup a new wait block.
* Although we're currently in a shared lock and we're acquiring
* a shared lock, there are exclusive locks queued in between.
* We need to wait until those are released.*/
shared = wb->last;
if (shared->ex)
{
swblock.ex = FALSE;
swblock.s_count = SRWLOCK_USERS;
swblock.next = NULL;
swblock.last = &swblock;
swblock.wake = &sw;
swblock.last_shared = &sw;
shared->next = &swblock;
wb->last = &swblock;
}
else
{
shared->last_shared->next = &sw;
shared->s_count += SRWLOCK_USERS;
shared->last_shared = &sw;
}
}
else
{
wb->last_shared->next = &sw;
wb->s_count += SRWLOCK_USERS;
wb->last_shared = &sw;
}
/*Unlock*/
barrier();
l->p &= ~SRWLOCK_LOCK;
/*Wait to be woken*/
while (!sw.spin) cpu_relax();
}
static void srwlock_rdunlock(srwlock *l)
{
uintptr_t p, np;
srw_wb *wb;
srw_wb *next;
while (1)
{
barrier();
p = l->p;
cpu_relax();
if (p & SRWLOCK_WAIT)
{
/** There's a wait block, we need to wake a pending exclusive acquirer,
* if this is the last shared release.*/
wb = lock_wb(l);
if (wb) break;
continue;
}
/*Don't interfere with locking*/
if (p & SRWLOCK_LOCK) continue;
/** This is a fast path, we can simply decrement the shared
* count and store the pointer*/
np = p - SRWLOCK_USERS;
/*If we are the last reader, then the lock is unused*/
if (np == SRWLOCK_SHARED) np = 0;
/*Try to release the lock*/
if (cmpxchg(&l->p, p, np) == p) return;
}
wb->s_count -= SRWLOCK_USERS;
if (wb->s_count)
{
/*Unlock*/
barrier();
l->p &= ~SRWLOCK_LOCK;
return;
}
next = wb->next;
if (next)
{
/** There's more blocks chained, we need to update the pointers
* in the next wait block and update the wait block pointer.*/
np = (uintptr_t)next | SRWLOCK_WAIT;
next->last = wb->last;
}
else
{
/*Convert the lock to a simple exclusive lock.*/
np = SRWLOCK_USERS;
}
barrier();
/*This also unlocks wb lock bit*/
l->p = np;
barrier();
wb->wake = (void *) 1;
barrier();
/*We released the lock*/
}
static int srwlock_rdtrylock(srwlock *s)
{
uintptr_t p = s->p;
barrier();
/*This is a fast path, we can simply try to set the shared count to 1*/
if (!p && (cmpxchg(&s->p, 0, SRWLOCK_USERS | SRWLOCK_SHARED) == 0)) return 0;
if ((p & (SRWLOCK_SHARED | SRWLOCK_WAIT)) == SRWLOCK_SHARED)
{
/*This is a fast path, just increment the number of current shared locks*/
if (cmpxchg(&s->p, p, p + SRWLOCK_USERS) == p) return 0;
}
return EBUSY;
}
static void srwlock_wrlock(srwlock *l)
{
srw_wb swblock;
uintptr_t p, np;
/*Fastpath - no other readers or writers*/
if (!l->p && (!cmpxchg(&l->p, 0, SRWLOCK_USERS))) return;
/*Initialize wait block*/
swblock.ex = TRUE;
swblock.next = NULL;
swblock.last = &swblock;
swblock.wake = NULL;
while (1)
{
barrier();
p = l->p;
cpu_relax();
if (p & SRWLOCK_WAIT)
{
srw_wb *wb = lock_wb(l);
if (!wb) continue;
/*Complete Initialization of block*/
swblock.s_count = 0;
wb->last->next = &swblock;
wb->last = &swblock;
/*Unlock*/
barrier();
l->p &= ~SRWLOCK_LOCK;
/*Has our wait block became the first one in the chain?*/
while (!swblock.wake) cpu_relax();
return;
}
/*Fastpath - no other readers or writers*/
if (!p)
{
if (!cmpxchg(&l->p, 0, SRWLOCK_USERS)) return;
continue;
}
/*Don't interfere with locking*/
if (p & SRWLOCK_LOCK) continue;
/*There are no wait blocks so far, we need to add ourselves as the first wait block.*/
if (p & SRWLOCK_SHARED)
{
swblock.s_count = p & ~SRWLOCK_MASK;
np = (uintptr_t)&swblock | SRWLOCK_SHARED | SRWLOCK_WAIT;
}
else
{
swblock.s_count = 0;
np = (uintptr_t)&swblock | SRWLOCK_WAIT;
}
/*Try to make change*/
if (cmpxchg(&l->p, p, np) == p) break;
}
/*Has our wait block became the first one in the chain?*/
while (!swblock.wake) cpu_relax();
}
static void srwlock_wrunlock(srwlock *l)
{
uintptr_t p, np;
srw_wb *wb;
srw_wb *next;
srw_sw *wake, *wake_next;
while (1)
{
barrier();
p = l->p;
cpu_relax();
if (p == SRWLOCK_USERS)
{
/** This is the fast path, we can simply clear the SRWLOCK_USERS bit.
* All other bits should be 0 now because this is a simple exclusive lock,
* and no one else is waiting.*/
if (cmpxchg(&l->p, SRWLOCK_USERS, 0) == SRWLOCK_USERS) return;
continue;
}
/*There's a wait block, we need to wake the next pending acquirer*/
wb = lock_wb(l);
if (wb) break;
}
next = wb->next;
if (next)
{
/** There's more blocks chained, we need to update the pointers
* in the next wait block and update the wait block pointer.*/
np = (uintptr_t)next | SRWLOCK_WAIT;
if (!wb->ex)
{
/*Save the shared count*/
next->s_count = wb->s_count;
np |= SRWLOCK_SHARED;
}
next->last = wb->last;
}
else
{
/*Convert the lock to a simple lock.*/
if (wb->ex)
{
np = SRWLOCK_USERS;
}
else
{
np = wb->s_count | SRWLOCK_SHARED;
}
}
barrier();
/*Also unlocks lock bit*/
l->p = np;
barrier();
if (wb->ex)
{
barrier();
/*Notify the next waiter*/
wb->wake = (void *) 1;
barrier();
return;
}
/*We now need to wake all others required.*/
for (wake = wb->wake; wake; wake = wake_next)
{
barrier();
wake_next = wake->next;
barrier();
wake->spin = 1;
barrier();
}
}
static int srwlock_wrtrylock(srwlock *s)
{
/*No other readers or writers?*/
if (!s->p && (cmpxchg(&s->p, 0, SRWLOCK_USERS) == 0)) return 0;
return EBUSY;
}