diff --git a/drivers/UARTSerial.cpp b/drivers/UARTSerial.cpp
index 96cdc831d8..5cad4ffc82 100644
--- a/drivers/UARTSerial.cpp
+++ b/drivers/UARTSerial.cpp
@@ -19,7 +19,12 @@
 #include <errno.h>
 #include "UARTSerial.h"
 #include "platform/mbed_poll.h"
+
+#if MBED_CONF_RTOS_PRESENT
+#include "rtos/Thread.h"
+#else
 #include "platform/mbed_wait_api.h"
+#endif
 
 namespace mbed {
 
@@ -277,6 +282,17 @@ void UARTSerial::tx_irq(void)
     }
 }
 
+void UARTSerial::wait_ms(uint32_t millisec)
+{
+    /* wait_ms implementation for RTOS spins until exact microseconds - we
+     * want to just sleep until next tick.
+     */
+#if MBED_CONF_RTOS_PRESENT
+    rtos::Thread::wait(millisec);
+#else
+    ::wait_ms(millisec);
+#endif
+}
 } //namespace mbed
 
 #endif //(DEVICE_SERIAL && DEVICE_INTERRUPTIN)
diff --git a/drivers/UARTSerial.h b/drivers/UARTSerial.h
index a269dea285..c0069adcb7 100644
--- a/drivers/UARTSerial.h
+++ b/drivers/UARTSerial.h
@@ -166,6 +166,8 @@ public:
 
 private:
 
+    void wait_ms(uint32_t millisec);
+
     /** SerialBase lock override */
     virtual void lock(void);
 
diff --git a/platform/mbed_poll.cpp b/platform/mbed_poll.cpp
index 28492010d4..c2a08fa3dd 100644
--- a/platform/mbed_poll.cpp
+++ b/platform/mbed_poll.cpp
@@ -66,7 +66,7 @@ int poll(pollfh fhs[], unsigned nfhs, int timeout)
 #ifdef MBED_CONF_RTOS_PRESENT
         // TODO - proper blocking
         // wait for condition variable, wait queue whatever here
-        rtos::Thread::yield();
+        rtos::Thread::wait(1);
 #endif
     }
     return count;