diff --git a/demos/ARM7-LPC214x-GCC-minimal/chcore.c b/demos/ARM7-LPC214x-GCC-minimal/chcore.c
index f844a6aa5..f4127cc8e 100644
--- a/demos/ARM7-LPC214x-GCC-minimal/chcore.c
+++ b/demos/ARM7-LPC214x-GCC-minimal/chcore.c
@@ -184,6 +184,6 @@ void NonVectoredIrq(void) {
 void Timer0Irq(void) {
 
   T0IR = 1;             /* Clear interrupt on match MR0. */
-  chSchTimerHandlerI();
+  chSysTimerHandlerI();
   VICVectAddr = 0;
 }
diff --git a/demos/ARM7-LPC214x-GCC/chcore.c b/demos/ARM7-LPC214x-GCC/chcore.c
index 640622e04..613841437 100644
--- a/demos/ARM7-LPC214x-GCC/chcore.c
+++ b/demos/ARM7-LPC214x-GCC/chcore.c
@@ -185,6 +185,6 @@ void NonVectoredIrq(void) {
 void Timer0Irq(void) {
 
   T0IR = 1;             /* Clear interrupt on match MR0. */
-  chSchTimerHandlerI();
+  chSysTimerHandlerI();
   VICVectAddr = 0;
 }
diff --git a/demos/AVR-AT90CANx-GCC/chcore2.S b/demos/AVR-AT90CANx-GCC/chcore2.S
index 3564de521..96c17f011 100644
--- a/demos/AVR-AT90CANx-GCC/chcore2.S
+++ b/demos/AVR-AT90CANx-GCC/chcore2.S
@@ -97,7 +97,7 @@ __vector_17:
         in      r0, _SFR_IO_ADDR(SREG)
         push    r0
         clr     r1
-        call    chSchTimerHandlerI
+        call    chSysTimerHandlerI
 intcommon:
         call    chSchRescRequiredI
         tst     r24
diff --git a/demos/Win32-MSVS/chcore.c b/demos/Win32-MSVS/chcore.c
index 3915d6271..99876b8d6 100644
--- a/demos/Win32-MSVS/chcore.c
+++ b/demos/Win32-MSVS/chcore.c
@@ -62,7 +62,7 @@ void ChkIntSources(void) {
   QueryPerformanceCounter(&n);
   if (n.QuadPart > nextcnt.QuadPart) {
     nextcnt.QuadPart += slice.QuadPart;
-    chSchTimerHandlerI();
+    chSysTimerHandlerI();
     if (chSchRescRequiredI())
       chSchDoRescheduleI();
   }
diff --git a/demos/Win32-MinGW/chcore.c b/demos/Win32-MinGW/chcore.c
index eab41ab27..866ac791e 100644
--- a/demos/Win32-MinGW/chcore.c
+++ b/demos/Win32-MinGW/chcore.c
@@ -85,7 +85,7 @@ void ChkIntSources(void) {
   QueryPerformanceCounter(&n);
   if (n.QuadPart > nextcnt.QuadPart) {
     nextcnt.QuadPart += slice.QuadPart;
-    chSchTimerHandlerI();
+    chSysTimerHandlerI();
     if (chSchRescRequiredI())
       chSchDoRescheduleI();
   }
diff --git a/readme.txt b/readme.txt
index 8ca71bfb7..5b24f0041 100644
--- a/readme.txt
+++ b/readme.txt
@@ -46,6 +46,11 @@ AVR-AT90CANx-GCC       - Port on AVR AT90CAN128, not complete yet.
 - Now the threads working area is filled with a 0x55 when in debug mode, this
   will make easier to track stack usage using a JTAG probe.
 - Added an I/O Queues benchmark to the test suite.
+- Fixed a bug in chIQGetTimeout(), interrupts were not re-enabled when exiting
+  the function because a timeout. The problem affected that API only.
+- Removed the chSchTimerHandlerI() routine from chschd.c and moved it into
+  chinit.c renaming it chSysTimerHandlerI() because it is not part of the
+  scheduler.
 
 *** 0.4.3 ***
 - Size optimization in the events code, now the chEvtWait() reuses the
diff --git a/src/chinit.c b/src/chinit.c
index 9fa079e2c..c7c0c7735 100644
--- a/src/chinit.c
+++ b/src/chinit.c
@@ -58,4 +58,22 @@ void chSysInit(void) {
   chThdCreate(IDLEPRIO, 0, waIdleThread, sizeof(waIdleThread), (t_tfunc)_IdleThread, NULL);
 }
 
+/**
+ * Preemption routine, this function must be called into an interrupt
+ * handler invoked by a system timer.
+ * The frequency of the timer determines the system tick granularity and,
+ * together with the \p CH_TIME_QUANTUM macro, the round robin interval.
+ */
+void chSysTimerHandlerI(void) {
+
+  rlist.r_preempt--;
+#ifdef CH_USE_SYSTEMTIME
+  rlist.r_stime++;
+#endif
+
+#ifdef CH_USE_VIRTUAL_TIMERS
+  chVTDoTickI();
+#endif
+}
+
 /** @} */
diff --git a/src/chqueues.c b/src/chqueues.c
index d0412598b..ddc2cfdff 100644
--- a/src/chqueues.c
+++ b/src/chqueues.c
@@ -125,8 +125,11 @@ t_msg chIQGetTimeout(Queue *qp, t_time time) {
 
   chSysLock();
 
-  if ((msg = chSemWaitTimeoutS(&qp->q_sem, time)) < RDY_OK)
+  if ((msg = chSemWaitTimeoutS(&qp->q_sem, time)) < RDY_OK) {
+
+    chSysUnlock();
     return msg;
+  }
   b = *qp->q_rdptr++;
   if (qp->q_rdptr >= qp->q_top)
     qp->q_rdptr = qp->q_buffer;
diff --git a/src/chschd.c b/src/chschd.c
index c4b5df5a9..e091bb0a0 100644
--- a/src/chschd.c
+++ b/src/chschd.c
@@ -166,22 +166,4 @@ BOOL chSchRescRequiredI(void) {
   return TRUE;
 }
 
-/**
- * Preemption routine, this function must be called into an interrupt
- * handler invoked by a system timer.
- * The frequency of the timer determines the system tick granularity and,
- * together with the \p CH_TIME_QUANTUM macro, the round robin interval.
- */
-void chSchTimerHandlerI(void) {
-
-  rlist.r_preempt--;
-#ifdef CH_USE_SYSTEMTIME
-  rlist.r_stime++;
-#endif
-
-#ifdef CH_USE_VIRTUAL_TIMERS
-  chVTDoTickI();
-#endif
-}
-
 /** @} */
diff --git a/src/chsleep.c b/src/chsleep.c
index 3abf22cd2..b6c31e1a4 100644
--- a/src/chsleep.c
+++ b/src/chsleep.c
@@ -27,10 +27,7 @@
 #ifdef CH_USE_SLEEP
 static void wakeup(void *p) {
 
-#ifdef CH_USE_DEBUG
-  if (((Thread *)p)->p_state != PRSLEEP)
-    chDbgPanic("chsleep.c, wakeup()");
-#endif
+  chDbgAssert(((Thread *)p)->p_state == PRSLEEP, "chsleep.c, wakeup()");
   chSchReadyI(p, RDY_OK);
 }
 
@@ -72,4 +69,3 @@ void chThdSleepUntil(t_time time) {
 #endif /* CH_USE_SLEEP */
 
 /** @} */
-
diff --git a/src/include/ch.h b/src/include/ch.h
index 0ef78fbf4..be251415f 100644
--- a/src/include/ch.h
+++ b/src/include/ch.h
@@ -106,6 +106,7 @@
 extern "C" {
 #endif
   void chSysInit(void);
+  void chSysTimerHandlerI(void);
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/include/scheduler.h b/src/include/scheduler.h
index 4aed76e28..183e3af79 100644
--- a/src/include/scheduler.h
+++ b/src/include/scheduler.h
@@ -64,7 +64,6 @@ extern "C" {
   void chSchRescheduleS(void);
   void chSchDoRescheduleI(void);
   BOOL chSchRescRequiredI(void);
-  void chSchTimerHandlerI(void);
 #ifdef __cplusplus
 }
 #endif