Index: sys/kern/kern_time.c
===================================================================
--- sys/kern/kern_time.c	(revision 239329)
+++ sys/kern/kern_time.c	(working copy)
@@ -53,6 +53,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/time.h>
 #include <sys/timers.h>
 #include <sys/timetc.h>
+#include <sys/types.h>
 #include <sys/vnode.h>
 
 #include <vm/vm.h>
@@ -60,6 +61,21 @@ __FBSDID("$FreeBSD$");
 
 #define MAX_CLOCKS 	(CLOCK_MONOTONIC+1)
 
+#define FREQ2BT(freq, bt)                                               \
+{                                                                       \
+        (bt)->sec = 0;                                                  \
+        (bt)->frac = ((uint64_t)0x8000000000000000  / (freq)) << 1;     \
+}
+
+#define TIMESEL(x, bt)							\
+   (((x) < (nanosleep_precision)) ? binuptime(&bt) :			\
+   getbinuptime(&bt))
+
+static struct bintime bt_hz;
+static int nanosleep_precision;
+SYSCTL_INT(_kern, OID_AUTO, nanosleep_precision, CTLFLAG_RW, 
+    &nanosleep_precision, 0, "Precision threshold for nanosleep function"); 
+
 static struct kclock	posix_clocks[MAX_CLOCKS];
 static uma_zone_t	itimer_zone = NULL;
 
@@ -105,7 +121,16 @@ int		itimespecfix(struct timespec *ts);
 
 SYSINIT(posix_timer, SI_SUB_P1003_1B, SI_ORDER_FIRST+4, itimer_start, NULL);
 
+static void
+init_precision(void)
+{
+	
+	nanosleep_precision = 20 * imin(1000000000 / hz, 1000000); 
+	FREQ2BT(hz, &bt_hz);
+}
 
+SYSINIT(init_precision, SI_SUB_SOFTINTR, SI_ORDER_ANY, init_precision, NULL);
+
 static int
 settime(struct thread *td, struct timeval *tv)
 {
@@ -361,29 +386,28 @@ kern_nanosleep(struct thread *td, struct timespec
 		return (EINVAL);
 	if (rqt->tv_sec < 0 || (rqt->tv_sec == 0 && rqt->tv_nsec == 0))
 		return (0);
-	binuptime(&bt);
+	TIMESEL(rqt->tv_nsec, bt);
 	timespec2bintime(rqt, &tmp);
-	bintime_add(&bt,&tmp);
-	for (;;) {
-		error = tsleep_bt(&nanowait, PWAIT | PCATCH, "nanslp", &bt, 
-		    C_DIRECT_EXEC);
-		binuptime(&bt2);
-		if (error != EWOULDBLOCK) {
-			if (error == ERESTART)
-				error = EINTR;
-			if (rmt != NULL) {
-				tmp = bt;
-				bintime_sub(&tmp, &bt2);	
-				bintime2timespec(&tmp, &ts);
-				if (ts.tv_sec < 0)
-					timespecclear(&ts);
-				*rmt = ts;
-			}
-			return (error);
+	bintime_add(&bt, &tmp);
+	if (rqt->tv_nsec > nanosleep_precision)
+		bintime_add(&bt, &bt_hz);
+	error = tsleep_bt(&nanowait, PWAIT | PCATCH, "nanslp", &bt, 
+	    C_DIRECT_EXEC);
+	TIMESEL(rqt->tv_nsec, bt2);
+	if (error != EWOULDBLOCK) {
+		if (error == ERESTART)
+			error = EINTR;
+		if (rmt != NULL) {
+			tmp = bt;
+			bintime_sub(&tmp, &bt2);	
+			bintime2timespec(&tmp, &ts);
+			if (ts.tv_sec < 0)
+				timespecclear(&ts);
+			*rmt = ts;
 		}
-		if (bintime_cmp(&bt2, &bt, >=))
-			return (0);
+		return (error);
 	}
+	return (0);
 }
 
 #ifndef _SYS_SYSPROTO_H_