Index: freebsd/sys/sys/proc.h
===================================================================
--- freebsd/sys/sys/proc.h	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ freebsd/sys/sys/proc.h	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -215,6 +215,6 @@
 #endif /* __rtems__ */
 	struct seltd	*td_sel;	/* Select queue/channel. */
+	struct sleepqueue *td_sleepqueue; /* (k) Associated sleep queue. */
 #ifndef __rtems__
-	struct sleepqueue *td_sleepqueue; /* (k) Associated sleep queue. */
 	struct turnstile *td_turnstile;	/* (k) Associated turnstile. */
 	struct umtx_q   *td_umtxq;	/* (c?) Link for when we're blocked. */
Index: freebsd/sys/sys/systm.h
===================================================================
--- freebsd/sys/sys/systm.h	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ freebsd/sys/sys/systm.h	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -340,6 +340,11 @@
 #define	msleep(chan, mtx, pri, wmesg, timo)				\
 	_sleep((chan), &(mtx)->lock_object, (pri), (wmesg), (timo))
+#ifndef __rtems__
 int	msleep_spin(void *chan, struct mtx *mtx, const char *wmesg, int timo)
 	    __nonnull(1);
+#else /* __rtems__ */
+#define	msleep_spin(chan, mtx, wmesg, timo)				\
+	msleep((chan), (mtx), 0, (wmesg), (timo))
+#endif /* __rtems__ */
 int	pause(const char *wmesg, int timo);
 #define	tsleep(chan, pri, wmesg, timo)					\
Index: rtemsbsd/include/machine/rtems-bsd-config.h.in
===================================================================
--- rtemsbsd/include/machine/rtems-bsd-config.h.in	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ rtemsbsd/include/machine/rtems-bsd-config.h.in	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -135,6 +135,4 @@
 #define M_RTEMS_HEAP 0
 
-#define BSD_MAXIMUM_SLEEP_QUEUES 32
-
 #define BSD_DEFAULT_FIB 0
 
Index: rtemsbsd/include/machine/rtems-bsd-thread.h
===================================================================
--- rtemsbsd/include/machine/rtems-bsd-thread.h	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ rtemsbsd/include/machine/rtems-bsd-thread.h	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -44,5 +44,7 @@
 #include <rtems/bsd/sys/types.h>
 #include <sys/proc.h>
+#include <sys/queue.h>
 
+#include <rtems/score/threadq.h>
 #include <rtems.h>
 
@@ -60,4 +62,11 @@
 #define BSD_MINIMUM_TASK_STACK_SIZE ((size_t) 32 * 1024)
 
+struct sleepqueue {
+	Thread_queue_Control sq_blocked;
+	LIST_ENTRY(sleepqueue) sq_hash;
+	LIST_HEAD(, sleepqueue) sq_free;
+	void *sq_wchan;
+};
+
 extern rtems_chain_control rtems_bsd_thread_chain;
 
Index: rtemsbsd/rtems/rtems-bsd-kern_synch.c
===================================================================
--- rtemsbsd/rtems/rtems-bsd-kern_synch.c	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ rtemsbsd/rtems/rtems-bsd-kern_synch.c	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -8,8 +8,19 @@
 
 /*
- * Copyright (c) 2009, 2010 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 1982, 1986, 1990, 1991, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ * (c) UNIX System Laboratories, Inc.
+ * All or some portions of this file are derived from material licensed
+ * to the University of California by American Telephone and Telegraph
+ * Co. or Unix System Laboratories, Inc. and are reproduced herein with
+ * the permission of UNIX System Laboratories, Inc.
+ *
+ * Copyright (c) 2004 John Baldwin <jhb@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Copyright (c) 2009-2013 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
- *  Obere Lagerstr. 30
+ *  Dornierstr. 4
  *  82178 Puchheim
  *  Germany
@@ -24,4 +35,7 @@
  *    notice, this list of conditions and the following disclaimer in the
  *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the author nor the names of any co-contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
@@ -36,11 +50,10 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
- */
-
-/*
- * FIXME: This seems to be a completely broken implementation.
+ *
+ *	@(#)kern_synch.c	8.9 (Berkeley) 5/19/95
  */
 
 #include <machine/rtems-bsd-config.h>
+#include <machine/rtems-bsd-thread.h>
 
 #include <rtems/score/statesimpl.h>
@@ -53,204 +66,150 @@
 #include <sys/systm.h>
 #include <sys/kernel.h>
-#include <sys/ktr.h>
 #include <rtems/bsd/sys/lock.h>
 #include <sys/mutex.h>
-#include <sys/proc.h>
-#include <machine/pcpu.h>
-
-#define STATES_WAITING_FOR_SLEEP              0x40000
 
 static int pause_wchan;
 
-typedef struct
-{
-  Chain_Node node;
-  void *ident;
-  Thread_queue_Control queue;
-}sleep_queue_control_t;
-
-sleep_queue_control_t sleep_queue[BSD_MAXIMUM_SLEEP_QUEUES]; //this memory allocation could use _Workspace_Allocate once inside RTEMS tree
-Chain_Control sleep_queue_inactive_nodes;  //chain of inactive nodes
-Chain_Control sleep_queue_active_nodes;    //chain of active nodes
-
-void
-sleepinit(void)
-{
-  int ii;
-
-  /* initialize the sleep queue */
-  for( ii = 0; ii < BSD_MAXIMUM_SLEEP_QUEUES; ii++ )
-  {
-    sleep_queue[ii].ident = NULL;
-    /*
-     *  Initialize the queue we use to block for signals
-     */
-    _Thread_queue_Initialize(
-      &sleep_queue[ii].queue,
-      THREAD_QUEUE_DISCIPLINE_PRIORITY,
-      STATES_WAITING_FOR_SLEEP | STATES_INTERRUPTIBLE_BY_SIGNAL,
-      EAGAIN
-    );
-  }
-  //initialize active chain
-  _Chain_Initialize_empty( &sleep_queue_active_nodes );
-  //initialize inactive chain
-  _Chain_Initialize( &sleep_queue_inactive_nodes, sleep_queue, BSD_MAXIMUM_SLEEP_QUEUES, sizeof( sleep_queue_control_t ));
-}
-
-sleep_queue_control_t*
-sleep_queue_lookup(void *ident)
-{
-  int ii;
-
-  /* initialize the sleep queue */
-  for( ii = 0; ii < BSD_MAXIMUM_SLEEP_QUEUES; ii++ )
-  {
-    if( sleep_queue[ii].ident == ident )
-    {
-      return &sleep_queue[ii];
-    }
-  }
-  return NULL;
-}
-
-sleep_queue_control_t*
-sleep_queue_get(void *ident)
-{
-  sleep_queue_control_t *sq;
-
-  sq = sleep_queue_lookup( ident );
-  if (sq == NULL)
-  {
-    KASSERT(!_Chain_Is_empty( &inactive_nodes ), ("sleep_queue_get"));
-    //get a control from the inactive chain
-    sq = ( sleep_queue_control_t * )_Chain_Get( &sleep_queue_inactive_nodes );
-    sq->ident = ident;
-    _Chain_Append( &sleep_queue_active_nodes, &sq->node );
-  }
-  return sq;
-}
-
-/*
- * Block the current thread until it is awakened from its sleep queue
- * or it times out while waiting.
- */
+/*
+ * Constants for the hash table of sleep queue chains.  These constants are
+ * the same ones that 4BSD (and possibly earlier versions of BSD) used.
+ * Basically, we ignore the lower 8 bits of the address since most wait
+ * channel pointers are aligned and only look at the next 7 bits for the
+ * hash.  SC_TABLESIZE must be a power of two for SC_MASK to work properly.
+ */
+#define	SC_TABLESIZE	128			/* Must be power of 2. */
+#define	SC_MASK		(SC_TABLESIZE - 1)
+#define	SC_SHIFT	8
+#define	SC_HASH(wc)	(((uintptr_t)(wc) >> SC_SHIFT) & SC_MASK)
+#define	SC_LOOKUP(wc)	&sleepq_chains[SC_HASH(wc)]
+
+struct sleepqueue_chain {
+	LIST_HEAD(, sleepqueue) sc_queues;	/* List of sleep queues. */
+};
+
+static struct sleepqueue_chain sleepq_chains[SC_TABLESIZE];
+
+static void
+init_sleepqueues(void)
+{
+	size_t i;
+
+	for (i = 0; i < SC_TABLESIZE; i++) {
+		LIST_INIT(&sleepq_chains[i].sc_queues);
+	}
+}
+
+SYSINIT(rtems_bsd_sleep, SI_SUB_INTRINSIC, SI_ORDER_FIRST, init_sleepqueues, NULL);
+
+/*
+ * Look up the sleep queue associated with a given wait channel in the hash
+ * table locking the associated sleep queue chain.  If no queue is found in
+ * the table, NULL is returned.
+ */
+static struct sleepqueue *
+sleepq_lookup(void *wchan)
+{
+	struct sleepqueue_chain *sc;
+	struct sleepqueue *sq;
+
+	KASSERT(wchan != NULL, ("%s: invalid NULL wait channel", __func__));
+	sc = SC_LOOKUP(wchan);
+	LIST_FOREACH(sq, &sc->sc_queues, sq_hash)
+		if (sq->sq_wchan == wchan)
+			return (sq);
+	return (NULL);
+}
+
 int
-sleep_queue_timedwait(void *wchan, int pri, int timeout, int catch)
-{
-  sleep_queue_control_t *sq;
-  Thread_Control *executing;
-  ISR_Level       level;
-
-  _Thread_Disable_dispatch();
-
-  sq = sleep_queue_get( wchan );
-
-  executing = _Thread_Executing;
-  if( timeout )
-  {
-    executing->Wait.return_code = EWOULDBLOCK;
-  }
-  else
-  {
-    executing->Wait.return_code = 0;
-  }
-  _ISR_Disable( level );
-  _Thread_queue_Enter_critical_section( &sq->queue );
-  if( catch )
-  {
-    sq->queue.state |= STATES_INTERRUPTIBLE_BY_SIGNAL;
-  }
-  else
-  {
-    sq->queue.state &= ~STATES_INTERRUPTIBLE_BY_SIGNAL;
-  }
-  executing->Wait.queue = &sq->queue;
-  _ISR_Enable( level );
-
-  _Thread_queue_Enqueue( &sq->queue, executing, timeout );
-  _Thread_Enable_dispatch();
-  return _Thread_Executing->Wait.return_code;
-}
-
-/*
- * General sleep call.  Suspends the current thread until a wakeup is
- * performed on the specified identifier.  The thread will then be made
- * runnable with the specified priority.  Sleeps at most timo/hz seconds
- * (0 means no timeout).  If pri includes PCATCH flag, signals are checked
- * before and after sleeping, else signals are not checked.  Returns 0 if
- * awakened, EWOULDBLOCK if the timeout expires.  If PCATCH is set and a
- * signal needs to be delivered, ERESTART is returned if the current system
- * call should be restarted if possible, and EINTR is returned if the system
- * call should be interrupted by the signal (return EINTR).
- *
- * The lock argument is unlocked before the caller is suspended, and
- * re-locked before _sleep() returns.  If priority includes the PDROP
- * flag the lock is not re-locked before returning.
- */
-int
-_sleep(void *ident, struct lock_object *lock, int priority, const char *wmesg, int timo)
-{
-  struct thread *td;
-  struct lock_class *class;
-  int catch, flags, lock_state, pri, rval;
-
-  td = curthread;
+_sleep(void *wchan, struct lock_object *lock, int priority, const char *wmesg, int timo)
+{
+	Thread_Control *executing;
+	struct thread *td;
+	struct lock_class *class;
+	int lock_state;
+	int rval;
+	struct sleepqueue *sq;
+	struct sleepqueue_chain *sc;
+
 #ifdef KTRACE
-  if (KTRPOINT(td, KTR_CSW))
-    ktrcsw(1, 0);
+	if (KTRPOINT(td, KTR_CSW))
+		ktrcsw(1, 0);
 #endif
-  KASSERT(timo != 0 || mtx_owned(&Giant) || lock != NULL,
-      ("sleeping without a lock"));
-  KASSERT(ident != NULL && TD_IS_RUNNING(td), ("msleep"));
-  if (priority & PDROP)
-    KASSERT(lock != NULL && lock != &Giant.lock_object,
-        ("PDROP requires a non-Giant lock"));
-  if (lock != NULL)
-    class = LOCK_CLASS(lock);
-  else
-    class = NULL;
-
-  if (cold) {
-    /*
-     * During autoconfiguration, just return;
-     * don't run any other threads or panic below,
-     * in case this is the idle thread and already asleep.
-     * XXX: this used to do "s = splhigh(); splx(safepri);
-     * splx(s);" to give interrupts a chance, but there is
-     * no way to give interrupts a chance now.
-     */
-    if (lock != NULL && priority & PDROP)
-      class->lc_unlock(lock);
-    return (0);
-  }
-  catch = priority & PCATCH;
-  pri = priority & PRIMASK;
-
-  if (lock == &Giant.lock_object)
-    mtx_assert(&Giant, MA_OWNED);
-  DROP_GIANT();
-  if (lock != NULL && lock != &Giant.lock_object &&
-      !(class->lc_flags & LC_SLEEPABLE)) {
-    lock_state = class->lc_unlock(lock);
-  } else
-    /* GCC needs to follow the Yellow Brick Road */
-    lock_state = -1;
-
-  if (lock != NULL && class->lc_flags & LC_SLEEPABLE) {
-    lock_state = class->lc_unlock(lock);
-  }
-
-  rval = sleep_queue_timedwait(ident, pri, timo, catch);
-
-#ifdef KTRACE
-  if (KTRPOINT(td, KTR_CSW))
-    ktrcsw(0, 0);
-#endif
-  PICKUP_GIANT();
-  if (lock != NULL && lock != &Giant.lock_object && !(priority & PDROP)) {
-    class->lc_lock(lock, lock_state);
-  }
-  return (rval);
+	WITNESS_WARN(WARN_GIANTOK | WARN_SLEEPOK, lock,
+	    "Sleeping on \"%s\"", wmesg);
+	KASSERT(timo != 0 || mtx_owned(&Giant) || lock != NULL,
+	    ("sleeping without a lock"));
+	KASSERT(p != NULL, ("msleep1"));
+	KASSERT(wchan != NULL && TD_IS_RUNNING(td), ("msleep"));
+	if (priority & PDROP)
+		KASSERT(lock != NULL && lock != &Giant.lock_object,
+		    ("PDROP requires a non-Giant lock"));
+	if (lock != NULL)
+		class = LOCK_CLASS(lock);
+	else
+		class = NULL;
+
+	if (lock == &Giant.lock_object)
+		mtx_assert(&Giant, MA_OWNED);
+	DROP_GIANT();
+
+	td = curthread;
+
+	_Thread_Disable_dispatch();
+
+	if (lock != NULL) {
+		lock_state = class->lc_unlock(lock);
+	}
+
+	sc = SC_LOOKUP(wchan);
+
+	/* Look up the sleep queue associated with the wait channel 'wchan'. */
+	sq = sleepq_lookup(wchan);
+
+	/*
+	 * If the wait channel does not already have a sleep queue, use
+	 * this thread's sleep queue.  Otherwise, insert the current thread
+	 * into the sleep queue already in use by this wait channel.
+	 */
+	if (sq == NULL) {
+		sq = td->td_sleepqueue;
+		LIST_INSERT_HEAD(&sc->sc_queues, sq, sq_hash);
+		sq->sq_wchan = wchan;
+	} else {
+		LIST_INSERT_HEAD(&sq->sq_free, td->td_sleepqueue, sq_hash);
+	}
+	td->td_sleepqueue = NULL;
+
+	_Thread_queue_Enter_critical_section(&sq->sq_blocked);
+	executing = _Thread_Executing;
+	executing->Wait.queue = &sq->sq_blocked;
+	_Thread_queue_Enqueue(&sq->sq_blocked, executing, (Watchdog_Interval) timo);
+
+	_Thread_Enable_dispatch();
+
+	rval = (int) executing->Wait.return_code;
+
+	_Thread_Disable_dispatch();
+
+	/*
+	 * Get a sleep queue for this thread.  If this is the last waiter,
+	 * use the queue itself and take it out of the chain, otherwise,
+	 * remove a queue from the free list.
+	 */
+	if (LIST_EMPTY(&sq->sq_free)) {
+		td->td_sleepqueue = sq;
+	} else
+		td->td_sleepqueue = LIST_FIRST(&sq->sq_free);
+	LIST_REMOVE(td->td_sleepqueue, sq_hash);
+
+	_Thread_Enable_dispatch();
+
+	PICKUP_GIANT();
+	if (lock != NULL && lock != &Giant.lock_object && !(priority & PDROP)) {
+		class->lc_lock(lock, lock_state);
+		WITNESS_RESTORE(lock, lock_witness);
+	}
+
+	return (rval);
 }
 
@@ -265,48 +224,50 @@
 {
 
-  KASSERT(timo != 0, ("pause: timeout required"));
-  return (tsleep(&pause_wchan, 0, wmesg, timo));
-}
-
-/*
- * Make all threads sleeping on the specified identifier runnable.
- */
+	KASSERT(timo != 0, ("pause: timeout required"));
+	return (tsleep(&pause_wchan, 0, wmesg, timo));
+}
+
+static void
+rtems_bsd_sleepq_wakeup(struct sleepqueue *sq, Thread_Control *thread)
+{
+	thread->Wait.return_code = 0;
+}
+
 void
-wakeup(void *ident)
-{
-  sleep_queue_control_t *sq;
-  Thread_Control *the_thread;
-
-  sq = sleep_queue_lookup( ident );
-  if (sq == NULL)
-  {
-    return (0);
-  }
-
-  while ( (the_thread = _Thread_queue_Dequeue(&sq->queue)) )
-  {
-  }
-  return 0;
-}
-
-/*
- * Make a thread sleeping on the specified identifier runnable.
- * May wake more than one thread if a target thread is currently
- * swapped out.
- */
+wakeup(void *wchan)
+{
+	struct sleepqueue *sq;
+
+	_Thread_Disable_dispatch();
+
+	sq = sleepq_lookup(wchan);
+	if (sq != NULL) {
+		Thread_Control *thread;
+
+		while ((thread = _Thread_queue_Dequeue(&sq->sq_blocked)) != NULL) {
+			rtems_bsd_sleepq_wakeup(sq, thread);
+		}
+	}
+
+	_Thread_Enable_dispatch();
+}
+
 void
-wakeup_one(void *ident)
-{
-  sleep_queue_control_t *sq;
-  Thread_Control *the_thread;
-
-  sq = sleep_queue_lookup( ident );
-  if (sq == NULL)
-  {
-    return (0);
-  }
-  the_thread = _Thread_queue_Dequeue(&sq->queue); 
-  return 0;
-
-}
-
+wakeup_one(void *wchan)
+{
+	struct sleepqueue *sq;
+
+	_Thread_Disable_dispatch();
+
+	sq = sleepq_lookup(wchan);
+	if (sq != NULL) {
+		Thread_Control *thread;
+
+		thread = _Thread_queue_Dequeue(&sq->sq_blocked);
+		if (thread != NULL) {
+			rtems_bsd_sleepq_wakeup(sq, thread);
+		}
+	}
+
+	_Thread_Enable_dispatch();
+}
Index: rtemsbsd/rtems/rtems-bsd-mutex.c
===================================================================
--- rtemsbsd/rtems/rtems-bsd-mutex.c	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ rtemsbsd/rtems/rtems-bsd-mutex.c	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -54,10 +54,8 @@
 static void assert_mtx(struct lock_object *lock, int what);
 static void lock_mtx(struct lock_object *lock, int how);
-static void lock_spin(struct lock_object *lock, int how);
 #ifdef KDTRACE_HOOKS
 static int  owner_mtx(struct lock_object *lock, struct thread **owner);
 #endif
 static int  unlock_mtx(struct lock_object *lock);
-static int  unlock_spin(struct lock_object *lock);
 
 RTEMS_CHAIN_DEFINE_EMPTY(rtems_bsd_mtx_chain);
@@ -87,6 +85,6 @@
   .lc_ddb_show = db_show_mtx,
 #endif
-  .lc_lock = lock_spin,
-  .lc_unlock = unlock_spin,
+  .lc_lock = lock_mtx,
+  .lc_unlock = unlock_mtx,
 #ifdef KDTRACE_HOOKS
   .lc_owner = owner_mtx,
@@ -107,11 +105,4 @@
 
   mtx_lock((struct mtx *)lock);
-}
-
-void
-lock_spin(struct lock_object *lock, int how)
-{
-
-  panic("spin locks can only use msleep_spin");
 }
 
@@ -127,10 +118,4 @@
 }
 
-int
-unlock_spin(struct lock_object *lock)
-{
-
-  panic("spin locks can only use msleep_spin");
-}
 
 #ifdef KDTRACE_HOOKS
Index: rtemsbsd/rtems/rtems-bsd-thread.c
===================================================================
--- rtemsbsd/rtems/rtems-bsd-thread.c	(revision 24732b6de53cb69c1a17f2451a8eda0f936bd171)
+++ rtemsbsd/rtems/rtems-bsd-thread.c	(revision f9c271459034e777d586f589af90deb9b7acd5a0)
@@ -50,6 +50,10 @@
 #include <sys/selinfo.h>
 
+#include <rtems/score/objectimpl.h>
+#include <rtems/score/statesimpl.h>
+#include <rtems/score/threaddispatch.h>
+#include <rtems/score/thread.h>
 #include <rtems/score/threadimpl.h>
-#include <rtems/score/objectimpl.h>
+#include <rtems/score/threadqimpl.h>
 
 RTEMS_CHAIN_DEFINE_EMPTY(rtems_bsd_thread_chain);
@@ -92,7 +96,22 @@
 {
 	struct thread *td = malloc(sizeof(*td), M_TEMP, M_ZERO | wait);
-
-	if (td != NULL) {
+	struct sleepqueue *sq = malloc(sizeof(*sq), M_TEMP, wait);
+
+	if (td != NULL && sq != NULL) {
 		td->td_thread = thread;
+		td->td_sleepqueue = sq;
+
+		LIST_INIT(&sq->sq_free);
+
+		_Thread_queue_Initialize(
+			&sq->sq_blocked,
+			THREAD_QUEUE_DISCIPLINE_PRIORITY,
+			STATES_WAITING_FOR_BSD_WAKEUP,
+			EWOULDBLOCK
+		);
+	} else {
+		free(td, M_TEMP);
+		free(sq, M_TEMP);
+		td = NULL;
 	}
 
@@ -168,4 +187,5 @@
 		}
 
+		free(td->td_sleepqueue, M_TEMP);
 		free(td, M_TEMP);
 	}
