SLEEPQUEUE(9): Port to RTEMS

1 files changed, 279 insertions, 0 deletions

diff --git a/freebsd/sys/kern/subr_sleepqueue.c b/freebsd/sys/kern/subr_sleepqueue.c
index 3774e66d..00740ae4 100644
--- a/freebsd/sys/kern/subr_sleepqueue.c
+++ b/freebsd/sys/kern/subr_sleepqueue.c
@@ -2,6 +2,7 @@
 
 /*-
  * Copyright (c) 2004 John Baldwin <jhb@FreeBSD.org>
+ * Copyright (c) 2015 embedded brains GmbH <rtems@embedded-brains.de>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -85,6 +86,10 @@ __FBSDID("$FreeBSD$");
 #ifdef DDB
 #include <ddb/ddb.h>
 #endif
+#ifdef __rtems__
+#include <machine/rtems-bsd-thread.h>
+#include <rtems/score/threadimpl.h>
+#endif /* __rtems__ */
 
 /*
  * Constants for the hash table of sleep queue chains.  These constants are
@@ -155,9 +160,11 @@ static uma_zone_t sleepq_zone;
 /*
  * Prototypes for non-exported routines.
  */
+#ifndef __rtems__
 static int	sleepq_catch_signals(void *wchan, int pri);
 static int	sleepq_check_signals(void);
 static int	sleepq_check_timeout(void);
+#endif /* __rtems__ */
 #ifdef INVARIANTS
 static void	sleepq_dtor(void *mem, int size, void *arg);
 #endif
@@ -165,7 +172,11 @@ static int	sleepq_init(void *mem, int size, int flags);
 static int	sleepq_resume_thread(struct sleepqueue *sq, struct thread *td,
 		    int pri);
 static void	sleepq_switch(void *wchan, int pri);
+#ifndef __rtems__
 static void	sleepq_timeout(void *arg);
+#else /* __rtems__ */
+static void	sleepq_timeout(Objects_Id id, void *arg);
+#endif /* __rtems__ */
 
 SDT_PROBE_DECLARE(sched, , , sleep);
 SDT_PROBE_DECLARE(sched, , , wakeup);
@@ -206,7 +217,9 @@ init_sleepqueues(void)
 	    NULL, NULL, sleepq_init, NULL, UMA_ALIGN_CACHE, 0);
 #endif
 	
+#ifndef __rtems__
 	thread0.td_sleepqueue = sleepq_alloc();
+#endif /* __rtems__ */
 }
 
 /*
@@ -286,6 +299,11 @@ sleepq_add(void *wchan, struct lock_object *lock, const char *wmesg, int flags,
 	struct sleepqueue_chain *sc;
 	struct sleepqueue *sq;
 	struct thread *td;
+#ifdef __rtems__
+	ISR_lock_Context lock_context;
+	Thread_Control *executing;
+	struct thread *succ;
+#endif /* __rtems__ */
 
 	td = curthread;
 	sc = SC_LOOKUP(wchan);
@@ -341,17 +359,40 @@ sleepq_add(void *wchan, struct lock_object *lock, const char *wmesg, int flags,
 		LIST_INSERT_HEAD(&sq->sq_free, td->td_sleepqueue, sq_hash);
 	}
 	thread_lock(td);
+#ifndef __rtems__
 	TAILQ_INSERT_TAIL(&sq->sq_blocked[queue], td, td_slpq);
+#else /* __rtems__ */
+	/* FIXME: This is broken with clustered scheduling */
+	succ = NULL;
+	TAILQ_FOREACH(succ, &sq->sq_blocked[queue], td_slpq) {
+		if (td->td_thread->current_priority <
+		    succ->td_thread->current_priority)
+			break;
+	}
+	if (succ == NULL)
+		TAILQ_INSERT_TAIL(&sq->sq_blocked[queue], td, td_slpq);
+	else
+		TAILQ_INSERT_BEFORE(succ, td, td_slpq);
+#endif /* __rtems__ */
 	sq->sq_blockedcnt[queue]++;
+#ifdef __rtems__
+	executing = td->td_thread;
+	_Objects_ISR_disable_and_acquire(&executing->Object, &lock_context);
+	td->td_sq_state = TD_SQ_TIRED;
+#endif /* __rtems__ */
 	td->td_sleepqueue = NULL;
 	td->td_sqqueue = queue;
 	td->td_wchan = wchan;
 	td->td_wmesg = wmesg;
+#ifndef __rtems__
 	if (flags & SLEEPQ_INTERRUPTIBLE) {
 		td->td_flags |= TDF_SINTR;
 		td->td_flags &= ~TDF_SLEEPABORT;
 	}
 	thread_unlock(td);
+#else /* __rtems__ */
+	_Objects_Release_and_ISR_enable(&executing->Object, &lock_context);
+#endif /* __rtems__ */
 }
 
 /*
@@ -361,6 +402,7 @@ sleepq_add(void *wchan, struct lock_object *lock, const char *wmesg, int flags,
 void
 sleepq_set_timeout(void *wchan, int timo)
 {
+#ifndef __rtems__
 	struct sleepqueue_chain *sc;
 	struct thread *td;
 
@@ -371,6 +413,17 @@ sleepq_set_timeout(void *wchan, int timo)
 	MPASS(td->td_sleepqueue == NULL);
 	MPASS(wchan != NULL);
 	callout_reset_curcpu(&td->td_slpcallout, timo, sleepq_timeout, td);
+#else /* __rtems__ */
+	Thread_Control *executing;
+
+	_Thread_Disable_dispatch();
+	executing = _Thread_Executing;
+	BSD_ASSERT(executing->Timer.state == WATCHDOG_INACTIVE);
+	_Watchdog_Initialize(&executing->Timer, sleepq_timeout,
+	    0, executing);
+	_Watchdog_Insert_ticks(&executing->Timer, (Watchdog_Interval)timo);
+	_Thread_Enable_dispatch();
+#endif /* __rtems__ */
 }
 
 /*
@@ -389,6 +442,7 @@ sleepq_sleepcnt(void *wchan, int queue)
 	return (sq->sq_blockedcnt[queue]);
 }
 
+#ifndef __rtems__
 /*
  * Marks the pending sleep of the current thread as interruptible and
  * makes an initial check for pending signals before putting a thread
@@ -483,6 +537,7 @@ out:
 	MPASS(td->td_lock != &sc->sc_lock);
 	return (ret);
 }
+#endif /* __rtems__ */
 
 /*
  * Switches to another thread if we are still asleep on a sleep queue.
@@ -491,6 +546,7 @@ out:
 static void
 sleepq_switch(void *wchan, int pri)
 {
+#ifndef __rtems__
 	struct sleepqueue_chain *sc;
 	struct sleepqueue *sq;
 	struct thread *td;
@@ -542,6 +598,106 @@ sleepq_switch(void *wchan, int pri)
 	KASSERT(TD_IS_RUNNING(td), ("running but not TDS_RUNNING"));
 	CTR3(KTR_PROC, "sleepq resume: thread %p (pid %ld, %s)",
 	    (void *)td, (long)td->td_proc->p_pid, (void *)td->td_name);
+#else /* __rtems__ */
+	Thread_Control *executing;
+	ISR_lock_Context lock_context;
+	struct thread *td;
+	bool block;
+	bool remove;
+
+	sleepq_release(wchan);
+
+	executing = _Thread_Acquire_executing(&lock_context);
+	td = rtems_bsd_get_thread(executing);
+	BSD_ASSERT(td != NULL);
+
+	block = false;
+	remove = false;
+	switch (td->td_sq_state) {
+	case TD_SQ_TIRED:
+		BSD_ASSERT(td->td_wchan == wchan);
+		td->td_sq_state = TD_SQ_SLEEPY;
+		block = true;
+		break;
+	case TD_SQ_NIGHTMARE:
+		BSD_ASSERT(td->td_wchan == wchan);
+		td->td_sq_state = TD_SQ_PANIC;
+		remove = true;
+		break;
+	default:
+		BSD_ASSERT(td->td_wchan == NULL);
+		BSD_ASSERT(td->td_sq_state == TD_SQ_WAKEUP);
+		break;
+	}
+
+	if (block) {
+		Per_CPU_Control *cpu_self;
+		bool unblock;
+
+		cpu_self = _Objects_Release_and_thread_dispatch_disable(
+		    &executing->Object, &lock_context);
+
+		_Giant_Acquire(cpu_self);
+		_Thread_Set_state(executing, STATES_WAITING_FOR_BSD_WAKEUP);
+		_Giant_Release(cpu_self);
+
+		_Objects_ISR_disable_and_acquire(&executing->Object,
+		    &lock_context);
+
+		unblock = false;
+		switch (td->td_sq_state) {
+		case TD_SQ_NIGHTMARE:
+			BSD_ASSERT(td->td_wchan == wchan);
+			td->td_sq_state = TD_SQ_PANIC;
+			unblock = true;
+			remove = true;
+			break;
+		case TD_SQ_WAKEUP:
+			BSD_ASSERT(td->td_wchan == NULL);
+			unblock = true;
+			break;
+		default:
+			BSD_ASSERT(td->td_wchan == wchan);
+			BSD_ASSERT(td->td_sq_state == TD_SQ_SLEEPY);
+			td->td_sq_state = TD_SQ_SLEEPING;
+			break;
+		}
+
+		_Objects_Release_and_ISR_enable(&executing->Object,
+		    &lock_context);
+
+		if (unblock) {
+			_Giant_Acquire(cpu_self);
+			_Watchdog_Remove(&executing->Timer);
+			_Thread_Clear_state(executing, STATES_WAITING_FOR_BSD_WAKEUP);
+			_Giant_Release(cpu_self);
+		}
+
+		_Thread_Dispatch_enable(cpu_self);
+
+		_Objects_ISR_disable_and_acquire(&executing->Object,
+		    &lock_context);
+
+		switch (td->td_sq_state) {
+		case TD_SQ_NIGHTMARE:
+			BSD_ASSERT(td->td_wchan == wchan);
+			td->td_sq_state = TD_SQ_PANIC;
+			remove = true;
+			break;
+		default:
+			BSD_ASSERT(td->td_sq_state == TD_SQ_WAKEUP ||
+			    td->td_sq_state == TD_SQ_PANIC);
+			break;
+		}
+	}
+
+	_Objects_Release_and_ISR_enable(&executing->Object,
+	    &lock_context);
+
+	if (remove) {
+		sleepq_remove(td, wchan);
+	}
+#endif /* __rtems__ */
 }
 
 /*
@@ -553,6 +709,7 @@ sleepq_check_timeout(void)
 	struct thread *td;
 
 	td = curthread;
+#ifndef __rtems__
 	THREAD_LOCK_ASSERT(td, MA_OWNED);
 
 	/*
@@ -581,8 +738,12 @@ sleepq_check_timeout(void)
 		mi_switch(SW_INVOL | SWT_SLEEPQTIMO, NULL);
 	}
 	return (0);
+#else /* __rtems__ */
+	return (td->td_sq_state);
+#endif /* __rtems__ */
 }
 
+#ifndef __rtems__
 /*
  * Check to see if we were awoken by a signal.
  */
@@ -605,6 +766,7 @@ sleepq_check_signals(void)
 
 	return (0);
 }
+#endif /* __rtems__ */
 
 /*
  * Block the current thread until it is awakened from its sleep queue.
@@ -612,15 +774,20 @@ sleepq_check_signals(void)
 void
 sleepq_wait(void *wchan, int pri)
 {
+#ifndef __rtems__
 	struct thread *td;
 
 	td = curthread;
 	MPASS(!(td->td_flags & TDF_SINTR));
 	thread_lock(td);
+#endif /* __rtems__ */
 	sleepq_switch(wchan, pri);
+#ifndef __rtems__
 	thread_unlock(td);
+#endif /* __rtems__ */
 }
 
+#ifndef __rtems__
 /*
  * Block the current thread until it is awakened from its sleep queue
  * or it is interrupted by a signal.
@@ -638,6 +805,7 @@ sleepq_wait_sig(void *wchan, int pri)
 		return (rcatch);
 	return (rval);
 }
+#endif /* __rtems__ */
 
 /*
  * Block the current thread until it is awakened from its sleep queue
@@ -646,19 +814,26 @@ sleepq_wait_sig(void *wchan, int pri)
 int
 sleepq_timedwait(void *wchan, int pri)
 {
+#ifndef __rtems__
 	struct thread *td;
+#endif /* __rtems__ */
 	int rval;
 
+#ifndef __rtems__
 	td = curthread;
 	MPASS(!(td->td_flags & TDF_SINTR));
 	thread_lock(td);
+#endif /* __rtems__ */
 	sleepq_switch(wchan, pri);
 	rval = sleepq_check_timeout();
+#ifndef __rtems__
 	thread_unlock(td);
+#endif /* __rtems__ */
 
 	return (rval);
 }
 
+#ifndef __rtems__
 /*
  * Block the current thread until it is awakened from its sleep queue,
  * it is interrupted by a signal, or it times out waiting to be awakened.
@@ -678,6 +853,7 @@ sleepq_timedwait_sig(void *wchan, int pri)
 		return (rvals);
 	return (rvalt);
 }
+#endif /* __rtems__ */
 
 /*
  * Returns the type of sleepqueue given a waitchannel.
@@ -709,6 +885,13 @@ static int
 sleepq_resume_thread(struct sleepqueue *sq, struct thread *td, int pri)
 {
 	struct sleepqueue_chain *sc;
+#ifdef __rtems__
+	Thread_Control *thread;
+	ISR_lock_Context lock_context;
+	bool unblock;
+
+	BSD_ASSERT(sq != NULL);
+#endif /* __rtems__ */
 
 	MPASS(td != NULL);
 	MPASS(sq->sq_wchan != NULL);
@@ -740,9 +923,15 @@ sleepq_resume_thread(struct sleepqueue *sq, struct thread *td, int pri)
 	} else
 		td->td_sleepqueue = LIST_FIRST(&sq->sq_free);
 	LIST_REMOVE(td->td_sleepqueue, sq_hash);
+#ifdef __rtems__
+	(void)sc;
+	thread = td->td_thread;
+	_Objects_ISR_disable_and_acquire(&thread->Object, &lock_context);
+#endif /* __rtems__ */
 
 	td->td_wmesg = NULL;
 	td->td_wchan = NULL;
+#ifndef __rtems__
 	td->td_flags &= ~TDF_SINTR;
 
 	CTR3(KTR_PROC, "sleepq_wakeup: thread %p (pid %ld, %s)",
@@ -764,6 +953,39 @@ sleepq_resume_thread(struct sleepqueue *sq, struct thread *td, int pri)
 		TD_CLR_SLEEPING(td);
 		return (setrunnable(td));
 	}
+#else /* __rtems__ */
+	unblock = _Watchdog_Is_active(&thread->Timer);
+	switch (td->td_sq_state) {
+	case TD_SQ_SLEEPING:
+		unblock = true;
+		/* FALLTHROUGH */
+	case TD_SQ_TIRED:
+	case TD_SQ_SLEEPY:
+	case TD_SQ_NIGHTMARE:
+		td->td_sq_state = TD_SQ_WAKEUP;
+		break;
+	default:
+		BSD_ASSERT(td->td_sq_state == TD_SQ_PANIC);
+		break;
+	}
+
+	if (unblock) {
+		Per_CPU_Control *cpu_self;
+
+		cpu_self = _Objects_Release_and_thread_dispatch_disable(
+		    &thread->Object, &lock_context);
+		_Giant_Acquire(cpu_self);
+
+		_Watchdog_Remove(&thread->Timer);
+		_Thread_Clear_state(thread, STATES_WAITING_FOR_BSD_WAKEUP);
+
+		_Giant_Release(cpu_self);
+		_Thread_Dispatch_enable(cpu_self);
+	} else {
+		_Objects_Release_and_ISR_enable(&thread->Object,
+		    &lock_context);
+	}
+#endif /* __rtems__ */
 	return (0);
 }
 
@@ -811,7 +1033,11 @@ int
 sleepq_signal(void *wchan, int flags, int pri, int queue)
 {
 	struct sleepqueue *sq;
+#ifndef __rtems__
 	struct thread *td, *besttd;
+#else /* __rtems__ */
+	struct thread *besttd;
+#endif /* __rtems__ */
 	int wakeup_swapper;
 
 	CTR2(KTR_PROC, "sleepq_signal(%p, %d)", wchan, flags);
@@ -823,6 +1049,7 @@ sleepq_signal(void *wchan, int flags, int pri, int queue)
 	KASSERT(sq->sq_type == (flags & SLEEPQ_TYPE),
 	    ("%s: mismatch between sleep/wakeup and cv_*", __func__));
 
+#ifndef __rtems__
 	/*
 	 * Find the highest priority thread on the queue.  If there is a
 	 * tie, use the thread that first appears in the queue as it has
@@ -834,6 +1061,9 @@ sleepq_signal(void *wchan, int flags, int pri, int queue)
 		if (besttd == NULL || td->td_priority < besttd->td_priority)
 			besttd = td;
 	}
+#else /* __rtems__ */
+	besttd = TAILQ_FIRST(&sq->sq_blocked[queue]);
+#endif /* __rtems__ */
 	MPASS(besttd != NULL);
 	thread_lock(besttd);
 	wakeup_swapper = sleepq_resume_thread(sq, besttd, pri);
@@ -871,6 +1101,7 @@ sleepq_broadcast(void *wchan, int flags, int pri, int queue)
 	return (wakeup_swapper);
 }
 
+#ifndef __rtems__
 /*
  * Time sleeping threads out.  When the timeout expires, the thread is
  * removed from the sleep queue and made runnable if it is still asleep.
@@ -940,6 +1171,52 @@ sleepq_timeout(void *arg)
 	if (wakeup_swapper)
 		kick_proc0();
 }
+#else /* __rtems__ */
+static void
+sleepq_timeout(Objects_Id id, void *arg)
+{
+	Thread_Control *thread;
+	struct thread *td;
+	ISR_lock_Context lock_context;
+	bool unblock;
+
+	thread = arg;
+	td = rtems_bsd_get_thread(thread);
+	BSD_ASSERT(td != NULL);
+
+	_Objects_ISR_disable_and_acquire(&thread->Object, &lock_context);
+
+	unblock = false;
+	switch (td->td_sq_state) {
+	case TD_SQ_SLEEPING:
+		unblock = true;
+		/* Fall through */
+	case TD_SQ_TIRED:
+	case TD_SQ_SLEEPY:
+		td->td_sq_state = TD_SQ_NIGHTMARE;
+		break;
+	default:
+		BSD_ASSERT(td->td_sq_state == TD_SQ_WAKEUP);
+		break;
+	}
+
+	if (unblock) {
+		Per_CPU_Control *cpu_self;
+
+		cpu_self = _Objects_Release_and_thread_dispatch_disable(
+		    &thread->Object, &lock_context);
+		_Giant_Acquire(cpu_self);
+
+		_Thread_Clear_state(thread, STATES_WAITING_FOR_BSD_WAKEUP);
+
+		_Giant_Release(cpu_self);
+		_Thread_Dispatch_enable(cpu_self);
+	} else {
+		_Objects_Release_and_ISR_enable(&thread->Object,
+		    &lock_context);
+	}
+}
+#endif /* __rtems__ */
 
 /*
  * Resumes a specific thread from the sleep queue associated with a specific
@@ -980,6 +1257,7 @@ sleepq_remove(struct thread *td, void *wchan)
 		kick_proc0();
 }
 
+#ifndef __rtems__
 /*
  * Abort a thread as if an interrupt had occurred.  Only abort
  * interruptible waits (unfortunately it isn't safe to abort others).
@@ -1021,6 +1299,7 @@ sleepq_abort(struct thread *td, int intrval)
 	/* Thread is asleep on sleep queue sq, so wake it up. */
 	return (sleepq_resume_thread(sq, td, 0));
 }
+#endif /* __rtems__ */
 
 #ifdef SLEEPQUEUE_PROFILING
 #define	SLEEPQ_PROF_LOCATIONS	1024