diff --git a/extras/stdin_uart_interrupt/stdin_uart_interrupt.c b/extras/stdin_uart_interrupt/stdin_uart_interrupt.c
index 71d2efc..ba0ce4f 100644
--- a/extras/stdin_uart_interrupt/stdin_uart_interrupt.c
+++ b/extras/stdin_uart_interrupt/stdin_uart_interrupt.c
@@ -40,32 +40,55 @@
 
 #define UART0_RX_SIZE  (128) // ESP8266 UART HW FIFO size
 
-static SemaphoreHandle_t uart0_sem = NULL;
+static QueueHandle_t uart0_queue;
 static bool inited = false;
-static void uart0_rx_init(void);
+static bool uart0_rx_init(void);
 static int uart0_nonblock;
-static TickType_t uart0_vtime;
+static TickType_t uart0_vtime = portMAX_DELAY;
+
+uint32_t uart0_parity_errors;
+uint32_t uart0_framing_errors;
+uint32_t uart0_breaks_detected;
 
 IRAM void uart0_rx_handler(void *arg)
 {
     // TODO: Handle UART1, see reg 0x3ff20020, bit2, bit0 represents uart1 and uart0 respectively
-    if (!UART(UART0).INT_STATUS & UART_INT_STATUS_RXFIFO_FULL) {
-        return;
-    }
-//    printf(" [%08x (%d)]\n", READ_PERI_REG(UART_INT_ST(UART0)), READ_PERI_REG(UART_STATUS(UART0)) & (UART_RXFIFO_CNT << UART_RXFIFO_CNT_S));
-    if (UART(UART0).INT_STATUS & UART_INT_STATUS_RXFIFO_FULL) {
-        UART(UART0).INT_CLEAR = UART_INT_CLEAR_RXFIFO_FULL;
-        if (UART(UART0).STATUS & (UART_STATUS_RXFIFO_COUNT_M << UART_STATUS_RXFIFO_COUNT_S)) {
-            long int xHigherPriorityTaskWoken;
-            _xt_isr_mask(1 << INUM_UART);
-            _xt_clear_ints(1<<INUM_UART);
-            xSemaphoreGiveFromISR(uart0_sem, &xHigherPriorityTaskWoken);
-            if(xHigherPriorityTaskWoken) {
-                portYIELD();
+
+    // printf(" [%08x (%d)]\n", READ_PERI_REG(UART_INT_ST(UART0)), READ_PERI_REG(UART_STATUS(UART0)) & (UART_RXFIFO_CNT << UART_RXFIFO_CNT_S));
+
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+    do {
+        // If new data arrives and the status changes after checking here, them
+        // the interrupt will be re-triggered.
+        uint32_t int_status = UART(UART0).INT_STATUS;
+
+        if (int_status & UART_INT_STATUS_RXFIFO_FULL) {
+            size_t count = UART(UART0).STATUS & (UART_STATUS_RXFIFO_COUNT_M << UART_STATUS_RXFIFO_COUNT_S);
+            for (size_t i = 0; i < count; i++) {
+                char ch = UART(UART0).FIFO & (UART_FIFO_DATA_M << UART_FIFO_DATA_S);
+                xQueueSendToBackFromISR(uart0_queue, &ch, &xHigherPriorityTaskWoken);
             }
+            UART(UART0).INT_CLEAR = UART_INT_CLEAR_RXFIFO_FULL;
+            // If new data has arrived then the interrupt status will remain set.
+        } else if (int_status & UART_INT_STATUS_PARITY_ERR) {
+            uart0_parity_errors++;
+            UART(UART0).INT_CLEAR = UART_INT_CLEAR_PARITY_ERR;
+        } else if (int_status & UART_INT_STATUS_FRAMING_ERR) {
+            uart0_framing_errors++;
+            UART(UART0).INT_CLEAR = UART_INT_CLEAR_FRAMING_ERR;
+        } else if (int_status & UART_INT_STATUS_BREAK_DETECTED) {
+            uart0_breaks_detected++;
+            UART(UART0).INT_CLEAR = UART_INT_CLEAR_BREAK_DETECTED;
+        } else if (int_status & 0xff) {
+            printf("Error: unexpected uart irq, INT_STATUS 0x%02x\n", int_status);
+        } else {
+            break;
         }
-    } else {
-        printf("Error: unexpected uart irq, INT_STATUS 0x%02x\n", UART(UART0).INT_STATUS);
+    } while (1);
+
+    if(xHigherPriorityTaskWoken) {
+        portYIELD();
     }
 }
 
@@ -73,7 +96,7 @@ uint32_t uart0_num_char(void)
 {
     uint32_t count;
     if (!inited) uart0_rx_init();
-    count = UART(UART0).STATUS & (UART_STATUS_RXFIFO_COUNT_M << UART_STATUS_RXFIFO_COUNT_S);
+    count = uxQueueMessagesWaiting(uart0_queue);
     return count;
 }
 
@@ -95,37 +118,39 @@ TickType_t uart0_set_vtime(TickType_t ticks)
 // of this function
 long _read_stdin_r(struct _reent *r, int fd, char *ptr, int len)
 {
+    TickType_t vtime = uart0_vtime;
+    int nonblock = uart0_nonblock;
+
+    if (nonblock) {
+        vtime = 0;
+    }
+
     if (!inited) uart0_rx_init();
-    for(int i = 0; i < len; i++) {
-        if (!(UART(UART0).STATUS & (UART_STATUS_RXFIFO_COUNT_M << UART_STATUS_RXFIFO_COUNT_S))) {
-            _xt_isr_unmask(1 << INUM_UART);
-            if (uart0_nonblock) {
-                if (i > 0) {
-                    return i;
-                }
+
+    for(size_t i = 0; i < len; i++, ptr++) {
+        if (xQueueReceive(uart0_queue, (void*)ptr, vtime) == pdFALSE) {
+            if (i > 0) {
+                return i;
+            }
+            if (nonblock) {
                 r->_errno = EAGAIN;
                 return -1;
             }
-            if (uart0_vtime) {
-                if (!xSemaphoreTake(uart0_sem, uart0_vtime)) {
-                    if (i > 0) {
-                        return i;
-                    }
-                    return 0;
-                }
-            } else if (!xSemaphoreTake(uart0_sem, portMAX_DELAY)) {
-                printf("\nFailed to get sem\n");
-            }
+            return 0;
         }
-        ptr[i] = UART(UART0).FIFO & (UART_FIFO_DATA_M << UART_FIFO_DATA_S);
     }
     return len;
 }
 
-static void uart0_rx_init(void)
+static bool uart0_rx_init(void)
 {
+    uart0_queue = xQueueCreate(64, sizeof(char));
+
+    if (!uart0_queue) {
+        return false;
+    }
+
     int trig_lvl = 1;
-    uart0_sem = xSemaphoreCreateCounting(UART0_RX_SIZE, 0);
 
     _xt_isr_attach(INUM_UART, uart0_rx_handler, NULL);
     _xt_isr_unmask(1 << INUM_UART);
@@ -142,7 +167,10 @@ static void uart0_rx_init(void)
     UART(UART0).INT_CLEAR = 0x1ff;
 
     // enable rx_interrupt
-    UART(UART0).INT_ENABLE = UART_INT_ENABLE_RXFIFO_FULL;
+    UART(UART0).INT_ENABLE = UART_INT_ENABLE_RXFIFO_FULL | UART_INT_ENABLE_PARITY_ERR |
+        UART_INT_ENABLE_FRAMING_ERR | UART_INT_ENABLE_BREAK_DETECTED;
 
     inited = true;
+
+    return true;
 }