diff --git a/cores/arduino/UARTClass.cpp b/cores/arduino/UARTClass.cpp
index 000985f2..57d3f99e 100644
--- a/cores/arduino/UARTClass.cpp
+++ b/cores/arduino/UARTClass.cpp
@@ -168,7 +168,7 @@ int UARTClass::read( void )
 
 void UARTClass::flush( void )
 {
-  while (_tx_buffer->_iHead != _tx_buffer->_iTail); //wait for transmit data to be sent
+  while (_tx_buffer->_iHead != *(volatile int*)&(_tx_buffer->_iTail)); //wait for transmit data to be sent
   // Wait for transmission to complete
   while(!uart_tx_complete(CONFIG_UART_CONSOLE_INDEX));
 }
@@ -179,12 +179,11 @@ size_t UARTClass::write( const uint8_t uc_data )
     return(0);
 
   // Is the hardware currently busy?
-  if (_tx_buffer->_iTail != _tx_buffer->_iHead)
+  if (_tx_buffer->_iTail != _tx_buffer->_iHead || !uart_tx_ready(CONFIG_UART_CONSOLE_INDEX))
   {
     // If busy we buffer
     int l = (_tx_buffer->_iHead + 1) % UART_BUFFER_SIZE;
-    while (_tx_buffer->_iTail == l)
-      ; // Spin locks if we're about to overwrite the buffer. This continues once the data is sent
+    while (*(volatile int*)&(_tx_buffer->_iTail) == l); // Spin locks if we're about to overwrite the buffer. This continues once the data is sent
 
     _tx_buffer->_aucBuffer[_tx_buffer->_iHead] = uc_data;
     _tx_buffer->_iHead = l;
@@ -201,21 +200,29 @@ size_t UARTClass::write( const uint8_t uc_data )
 
 void UARTClass::IrqHandler( void )
 {
-  uint8_t uc_data;
-  int ret;
-  ret = uart_poll_in(CONFIG_UART_CONSOLE_INDEX, &uc_data);
-  
-  while ( ret != -1 ) {
-    _rx_buffer->store_char(uc_data);
+  uart_irq_update(CONFIG_UART_CONSOLE_INDEX);
+  // if irq is Receiver Data Available
+  if(uart_irq_rx_ready(CONFIG_UART_CONSOLE_INDEX))
+  {
+    uint8_t uc_data;
+    int ret;
     ret = uart_poll_in(CONFIG_UART_CONSOLE_INDEX, &uc_data);
+    
+    while ( ret != -1 ) {
+      _rx_buffer->store_char(uc_data);
+      ret = uart_poll_in(CONFIG_UART_CONSOLE_INDEX, &uc_data);
+    }
   }
 
-  // Do we need to keep sending data?
-  if (!uart_irq_tx_ready(CONFIG_UART_CONSOLE_INDEX))
+  // if irq is Transmitter Holding Register
+  else if(uart_irq_tx_ready(CONFIG_UART_CONSOLE_INDEX))
   {
-    if (_tx_buffer->_iTail != _tx_buffer->_iHead) {
-      uart_poll_out(CONFIG_UART_CONSOLE_INDEX, _tx_buffer->_aucBuffer[_tx_buffer->_iTail]);
-      _tx_buffer->_iTail = (unsigned int)(_tx_buffer->_iTail + 1) % UART_BUFFER_SIZE;
+    if(_tx_buffer->_iTail != _tx_buffer->_iHead)
+    {
+      int end = (_tx_buffer->_iTail < _tx_buffer->_iHead) ? _tx_buffer->_iHead:UART_BUFFER_SIZE;
+      int l = min(end - _tx_buffer->_iTail, UART_FIFO_SIZE);
+      uart_fifo_fill(CONFIG_UART_CONSOLE_INDEX, _tx_buffer->_aucBuffer+_tx_buffer->_iTail, l);
+      _tx_buffer->_iTail = (_tx_buffer->_iTail+l)%UART_BUFFER_SIZE;
     }
     else
     {
diff --git a/system/libarc32_arduino101/drivers/ns16550.c b/system/libarc32_arduino101/drivers/ns16550.c
index 386c6b22..f32d5d32 100644
--- a/system/libarc32_arduino101/drivers/ns16550.c
+++ b/system/libarc32_arduino101/drivers/ns16550.c
@@ -340,7 +340,7 @@ unsigned char uart_poll_out(
 	)
 {
 	/* wait for transmitter to ready to accept a character */
-	while ((INBYTE(LSR(which)) & LSR_TEMT) == 0)
+	while ((INBYTE(LSR(which)) & LSR_THRE) == 0)
 		;
 
 	OUTBYTE(THR(which), outChar);
@@ -352,6 +352,8 @@ unsigned char uart_poll_out(
 *
 * uart_fifo_fill - fill FIFO with data
 *
+* It is up to the caller to make sure that FIFO capcity is not exceeded
+*
 * RETURNS: number of bytes sent
 */
 
@@ -362,8 +364,8 @@ int uart_fifo_fill(int which, /* UART on which to send */
 {
 	int i;
 
-	for (i = 0; i < size && (INBYTE(LSR(which)) &
-			LSR_BOTH_EMPTY) != 0; i++) {
+	for (i = 0; i < size ; i++) 
+	{
 		OUTBYTE(THR(which), txData[i]);
 	}
 	return i;
@@ -640,6 +642,19 @@ uint8_t uart_tx_complete(int which)
 	return INBYTE(LSR(which)) & LSR_TEMT;
 }
 
+/*******************************************************************************
+*
+* uart_tx_complete - check if tx holding register is empty
+*
+* RETURNS: zero if register is non-empty, 
+*          non-zero if register is empty (ready to receive new data)
+*/
+
+uint8_t uart_tx_ready(int which)
+{
+	return INBYTE(LSR(which)) & LSR_THRE;
+}
+
 /*******************************************************************************
 *
 * uart_loop_enable - enable loopback
diff --git a/system/libarc32_arduino101/drivers/uart.h b/system/libarc32_arduino101/drivers/uart.h
index ed737780..456c244b 100644
--- a/system/libarc32_arduino101/drivers/uart.h
+++ b/system/libarc32_arduino101/drivers/uart.h
@@ -54,6 +54,9 @@ extern "C" {
 /* options for uart init */
 #define UART_OPTION_AFCE 0x01
 
+/* Size of the FIFO in bytes */
+#define UART_FIFO_SIZE 16
+
 /* generic UART info structure */
 struct uart_init_info {
 	int baud_rate;
@@ -92,6 +95,7 @@ int uart_break_check(int port);
 void uart_break_send(int port, int delay);
 void uart_disable(int port);
 uint8_t uart_tx_complete(int which);
+uint8_t uart_tx_ready(int which);
 void uart_loop_enable(int which);
 void uart_loop_disable(int which);
 
diff --git a/variants/arduino_101/libarc32drv_arduino101.a b/variants/arduino_101/libarc32drv_arduino101.a
index e0cb6b8e..2f8ce4f0 100644
Binary files a/variants/arduino_101/libarc32drv_arduino101.a and b/variants/arduino_101/libarc32drv_arduino101.a differ