diff --git a/cores/esp32/HardwareSerial.cpp b/cores/esp32/HardwareSerial.cpp
index 545c9b05562..3e6ecfff3b6 100644
--- a/cores/esp32/HardwareSerial.cpp
+++ b/cores/esp32/HardwareSerial.cpp
@@ -117,18 +117,20 @@ int HardwareSerial::availableForWrite(void)
 
 int HardwareSerial::peek(void)
 {
-    if (available()) {
-        return uartPeek(_uart);
-    }
-    return -1;
+	uint8_t data;
+	if (!uartPeek(_uart, &data, 0))
+		return -1;
+
+	return data;
 }
 
 int HardwareSerial::read(void)
 {
-    if(available()) {
-        return uartRead(_uart);
-    }
-    return -1;
+	uint8_t data;
+	if (!uartRead(_uart, &data, 0))
+		return -1;
+
+	return data;
 }
 
 void HardwareSerial::flush()
@@ -136,6 +138,22 @@ void HardwareSerial::flush()
     uartFlush(_uart);
 }
 
+int HardwareSerial::timedPeek() {
+	uint8_t data;
+	if (!uartPeek(_uart, &data, _timeout))
+		return -1;
+
+	return data;
+}
+
+int HardwareSerial::timedRead() {
+	uint8_t data;
+	if (!uartRead(_uart, &data, _timeout))
+		return -1;
+
+	return data;
+}
+
 size_t HardwareSerial::write(uint8_t c)
 {
     uartWrite(_uart, c);
diff --git a/cores/esp32/HardwareSerial.h b/cores/esp32/HardwareSerial.h
index 89eacf85d0e..6cb13917875 100644
--- a/cores/esp32/HardwareSerial.h
+++ b/cores/esp32/HardwareSerial.h
@@ -62,6 +62,8 @@ class HardwareSerial: public Stream
     int availableForWrite(void);
     int peek(void);
     int read(void);
+    virtual int timedRead();    // private method to read uart with timeout
+	virtual int timedPeek();    // private method to peek uart with timeout
     void flush(void);
     size_t write(uint8_t);
     size_t write(const uint8_t *buffer, size_t size);
diff --git a/cores/esp32/Stream.h b/cores/esp32/Stream.h
index 4ce31955acd..4293ab1d992 100644
--- a/cores/esp32/Stream.h
+++ b/cores/esp32/Stream.h
@@ -40,8 +40,8 @@ class Stream: public Print
 protected:
     unsigned long _timeout;      // number of milliseconds to wait for the next char before aborting timed read
     unsigned long _startMillis;  // used for timeout measurement
-    int timedRead();    // private method to read stream with timeout
-    int timedPeek();    // private method to peek stream with timeout
+    virtual int timedRead();    // private method to read stream with timeout
+    virtual int timedPeek();    // private method to peek stream with timeout
     int peekNextDigit(); // returns the next numeric digit in the stream or -1 if timeout
 
 public:
diff --git a/cores/esp32/esp32-hal-uart.c b/cores/esp32/esp32-hal-uart.c
index 2dee63dc325..92eecebc37c 100644
--- a/cores/esp32/esp32-hal-uart.c
+++ b/cores/esp32/esp32-hal-uart.c
@@ -29,6 +29,7 @@
 #include "soc/dport_reg.h"
 #include "soc/rtc.h"
 #include "esp_intr_alloc.h"
+#include "sdkconfig.h"
 
 #define UART_REG_BASE(u)    ((u==0)?DR_REG_UART_BASE:(      (u==1)?DR_REG_UART1_BASE:(    (u==2)?DR_REG_UART2_BASE:0)))
 #define UART_RXD_IDX(u)     ((u==0)?U0RXD_IN_IDX:(          (u==1)?U1RXD_IN_IDX:(         (u==2)?U2RXD_IN_IDX:0)))
@@ -277,26 +278,24 @@ uint32_t uartAvailableForWrite(uart_t* uart)
     return 0x7f - uart->dev->status.txfifo_cnt;
 }
 
-uint8_t uartRead(uart_t* uart)
+uint8_t uartRead(uart_t* uart, uint8_t *data, size_t to)
 {
     if(uart == NULL || uart->queue == NULL) {
         return 0;
     }
-    uint8_t c;
-    if(xQueueReceive(uart->queue, &c, 0)) {
-        return c;
+    if(xQueueReceive(uart->queue, data, to)) {
+        return 1;
     }
     return 0;
 }
 
-uint8_t uartPeek(uart_t* uart)
+uint8_t uartPeek(uart_t* uart, uint8_t *data, size_t to)
 {
     if(uart == NULL || uart->queue == NULL) {
         return 0;
     }
-    uint8_t c;
-    if(xQueuePeek(uart->queue, &c, 0)) {
-        return c;
+    if(xQueuePeek(uart->queue, data, to)) {
+        return 1;
     }
     return 0;
 }
@@ -379,6 +378,10 @@ static void uart_on_apb_change(void * arg, apb_change_ev_t ev_type, uint32_t old
         }
         // wait TX empty
         while(uart->dev->status.txfifo_cnt || uart->dev->status.st_utx_out);
+
+        if (xHigherPriorityTaskWoken) {
+			portYIELD_FROM_ISR();
+		}
     } else {
         //todo:
         // set baudrate
diff --git a/cores/esp32/esp32-hal-uart.h b/cores/esp32/esp32-hal-uart.h
index 821ca9c6ce0..cbabd682b83 100644
--- a/cores/esp32/esp32-hal-uart.h
+++ b/cores/esp32/esp32-hal-uart.h
@@ -56,8 +56,8 @@ void uartEnd(uart_t* uart);
 
 uint32_t uartAvailable(uart_t* uart);
 uint32_t uartAvailableForWrite(uart_t* uart);
-uint8_t uartRead(uart_t* uart);
-uint8_t uartPeek(uart_t* uart);
+uint8_t uartRead(uart_t* uart, uint8_t *data, size_t to);
+uint8_t uartPeek(uart_t* uart, uint8_t *data, size_t to);
 
 void uartWrite(uart_t* uart, uint8_t c);
 void uartWriteBuf(uart_t* uart, const uint8_t * data, size_t len);