3 files changed, 28 insertions, 0 deletions
diff --git a/libmaple/usart.c b/libmaple/usart.c
index 717a9d8..282fc5d 100644
--- a/libmaple/usart.c
+++ b/libmaple/usart.c
@@ -209,6 +209,9 @@ void usart_disable(uint8 usart_num) {
 
     /* Disable UE */
     port->CR1 = 0;
+
+    /* Clean up buffer */
+    usart_clear_buffer(usart_num);
 }
 
 
@@ -308,6 +311,26 @@ uint32 usart_data_available(uint8 usart_num) {
     return rb->tail - rb->head;
 }
 
+void usart_clear_buffer(uint8 usart_num) {
+    usart_ring_buf *rb;
+
+    switch (usart_num) {
+    case 1:
+        rb = &ring_buf1;
+        break;
+    case 2:
+        rb = &ring_buf2;
+        break;
+    case 3:
+        rb = &ring_buf3;
+        break;
+    default:
+        ASSERT(0);
+    }
+
+    rb->tail = rb->head;
+}
+
 
 
 /**
diff --git a/libmaple/usart.h b/libmaple/usart.h
index e2c2328..02fb6e0 100644
--- a/libmaple/usart.h
+++ b/libmaple/usart.h
@@ -48,6 +48,7 @@ void usart_putc(uint8 usart_num, uint8 ch);
 
 uint32 usart_data_available(uint8 usart_num);
 uint8 usart_getc(uint8 usart_num);
+void usart_clear_buffer(uint8 usart_num);
 
 #ifdef __cplusplus
 } // extern "C"
diff --git a/wirish/comm/HardwareSerial.cpp b/wirish/comm/HardwareSerial.cpp
index 93cf909..fc0d01e 100644
--- a/wirish/comm/HardwareSerial.cpp
+++ b/wirish/comm/HardwareSerial.cpp
@@ -101,6 +101,10 @@ void HardwareSerial::end(void) {
     usart_disable(usartNum);
 }
 
+void HardwareSerial::flush(void) {
+    usart_clear_buffer(usartNum);
+}
+
 HardwareSerial Serial1(1);
 HardwareSerial Serial2(2);
 HardwareSerial Serial3(3);