Astral/kernel-src/lib/semaphore.c

#include <semaphore.h>
#include <kernel/scheduler.h>
#include <errno.h>
#include <logging.h>

static void insert(semaphore_t *sem, thread_t *thread) {
	thread->sleepnext = sem->head;
	thread->sleepprev = NULL;
	sem->head = thread;

	if (sem->tail == NULL)
		sem->tail = thread;

	if (thread->sleepnext)
		thread->sleepnext->sleepprev = thread;
}

static thread_t *get(semaphore_t *sem) {
	thread_t *thread = sem->tail;
	if (thread == NULL)
		return NULL;

	sem->tail = thread->sleepprev;

	if (sem->tail == NULL)
		sem->head = NULL;
	else
		sem->tail->sleepnext = NULL;

	thread->sleepprev = NULL;
	thread->sleepnext = NULL;

	return thread;
}

static bool removeself(semaphore_t *sem) {
	thread_t *thread = current_thread();

	// first see if we have been removed already
	if ((sem->head == NULL && sem->tail == NULL) || // no threads in sleep list
	   !((thread->sleepnext != NULL || thread->sleepprev != NULL) || // 2 or more threads in sleep list and we are one of them
	    (sem->head == thread && sem->tail == thread))) // 1 thread in the sleep list and it is us
		return false;

	if (sem->head == thread) {
		sem->head = thread->sleepnext;
	} else {
		thread->sleepprev->sleepnext = thread->sleepnext;
	}

	if (sem->tail == thread) {
		sem->tail = thread->sleepprev;
	} else {
		thread->sleepnext->sleepprev = thread->sleepprev;
	}

	thread->sleepprev = NULL;
	thread->sleepnext = NULL;
	return true;
}

int semaphore_wait(semaphore_t *sem, bool interruptible) {
	if (current_thread() == NULL) {
		while (semaphore_test(sem) == false) CPU_PAUSE();
		return 0;
	}

	bool intstate = interrupt_set(false);
	int ret = 0;
	spinlock_acquire(&sem->lock);

	if (--sem->i < 0) {
		insert(sem, current_thread());
		sched_prepare_sleep(interruptible);
		spinlock_release(&sem->lock);
		ret = sched_yield();

		// Wait for the signaller to finish using this semaphore before
		// returning as it may belong to the caller's stack.
		spinlock_acquire(&sem->lock);
		if (ret) {
			if (removeself(sem))
				++sem->i;
			else
				ret = 0; // The semaphore signal won the race.
		}
		spinlock_release(&sem->lock);
		goto leave;
	}

	spinlock_release(&sem->lock);
	leave:
	interrupt_set(intstate);
	return ret;
}

int semaphore_timedwait(semaphore_t *sem, time_t timeoutusec, bool interruptible) {
	time_t sleepus;

	do {
		if (semaphore_test(sem))
			return true;

		sleepus = min(timeoutusec, 10);
		timeoutusec -= sleepus;

		if (sleepus)
			sched_sleep_us(sleepus);
	} while (timeoutusec);

	return false;
}

void semaphore_signal(semaphore_t *sem) {
	bool intstate = interrupt_set(false);
	spinlock_acquire(&sem->lock);

	thread_t *thread = NULL;
	if (++sem->i <= 0) {
		thread = get(sem);
		// a thread will always be returned if sem->i <= 0
		__assert(thread);
	}

	// Complete the wake before an interrupted waiter can observe its
	// removal and return (otherwise this wake could reach its next wait).
	if (thread)
		sched_wakeup(thread, 0);

	spinlock_release(&sem->lock);
	interrupt_set(intstate);
}

bool semaphore_signal_limit(semaphore_t *sem, int limit) {
	bool intstate = interrupt_set(false);
	spinlock_acquire(&sem->lock);

	bool successful = false;
	thread_t *thread = NULL;

	if (sem->i >= limit)
		goto leave;

	if (++sem->i <= 0) {
		thread = get(sem);
		// a thread will always be returned if sem->i <= 0
		__assert(thread);
	}

	successful = true;

leave:
	// Complete the wake before an interrupted waiter can observe its
	// removal and return (otherwise this wake could reach its next wait).
	if (thread)
		sched_wakeup(thread, 0);
	spinlock_release(&sem->lock);
	interrupt_set(intstate);
	return successful;
}

void semaphore_reset(semaphore_t *sem) {
	bool intstate = spinlock_acquire_irq_clear(&sem->lock);
	__assert(sem->head == NULL);
	sem->i = 0;
	spinlock_release_irq_restore(&sem->lock, intstate);
}

bool semaphore_haswaiters(semaphore_t *sem) {
	bool intstate = interrupt_set(false);
	spinlock_acquire(&sem->lock);

	bool v = sem->head != NULL;

	spinlock_release(&sem->lock);
	interrupt_set(intstate);
	return v;
}

bool semaphore_test(semaphore_t *sem) {
	bool intstate = interrupt_set(false);
	bool ret = false;
	spinlock_acquire(&sem->lock);

	if (sem->i > 0) {
		--sem->i;
		ret = true;
	}

	spinlock_release(&sem->lock);
	interrupt_set(intstate);
	return ret;
}