1 files changed, 5 insertions, 13 deletions
diff --git a/os/hal/src/serial.c b/os/hal/src/serial.c
index 5b883e16a..8b0d7c8cf 100644
--- a/os/hal/src/serial.c
+++ b/os/hal/src/serial.c
@@ -64,22 +64,14 @@ static size_t reads(void *ip, uint8_t *bp, size_t n) {
                          n, TIME_INFINITE);
 }
 
-static bool_t putwouldblock(void *ip) {
-  bool_t b;
+static msg_t put(void *ip, uint8_t b) {
 
-  chSysLock();
-  b = chOQIsFullI(&((SerialDriver *)ip)->oqueue);
-  chSysUnlock();
-  return b;
+  return chOQPutTimeout(&((SerialDriver *)ip)->oqueue, b, TIME_INFINITE);
 }
 
-static bool_t getwouldblock(void *ip) {
-  bool_t b;
+static msg_t get(void *ip) {
 
-  chSysLock();
-  b = chIQIsEmptyI(&((SerialDriver *)ip)->iqueue);
-  chSysUnlock();
-  return b;
+  return chIQGetTimeout(&((SerialDriver *)ip)->iqueue, TIME_INFINITE);
 }
 
 static msg_t putt(void *ip, uint8_t b, systime_t timeout) {
@@ -107,7 +99,7 @@ static chnflags_t getflags(void *ip) {
 }
 
 static const struct SerialDriverVMT vmt = {
-  writes, reads, putwouldblock, getwouldblock,
+  writes, reads, put, get,
   putt, gett, writet, readt,
   getflags
 };