     ssd1306_lcd.set_mode = vga_set_mode;
 }
 
+/////////////////////////// UART /////////////////////////////////
+
+#define BAUD 115200
+#include <util/setbaud.h>
+#define UART_BUFFER_RX  32  // :( Still need large buffer to process USART RX bytes
+static uint8_t uart_get_ptr = 0;
+volatile static uint8_t uart_put_ptr = 0;
+volatile static uint8_t uart_buf[UART_BUFFER_RX];
+
+ISR(USART_RX_vect, ISR_BLOCK)
+{
+//    if (bit_is_clear(UCSR0A, FE0)) // Do not perform error checks for now
+//    {
+        uart_buf[uart_put_ptr] = UDR0;
+        uart_put_ptr = (uart_put_ptr+1) & (UART_BUFFER_RX - 1);
+//    }
+//    else
+//    {
+//        volatile unsigned char data __attribute__((unused)) = UDR0;
+//    }
+}
+
+void uart_init()
+{
+    UBRR0H = UBRRH_VALUE;
+    UBRR0L = UBRRL_VALUE;
+
+#if USE_2X
+    UCSR0A |= _BV(U2X0);
+#else
+    UCSR0A &= ~(_BV(U2X0));
+#endif
+
+    UCSR0C = _BV(UCSZ01) | _BV(UCSZ00); /* 8-bit data */
+    UCSR0B = _BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0);    
+}
+
+void uart_send_byte(uint8_t c)
+{
+    loop_until_bit_is_set(UCSR0A, UDRE0); /* Wait until data register empty. */
+    UDR0 = c;
+}
+
+uint8_t uart_byte_available(void)
+{
+    return uart_put_ptr != uart_get_ptr;
+}
+
+static inline uint8_t uart_byte_available_direct(void)
+{
+    return bit_is_set(UCSR0A, RXC0);
+}
+
+uint8_t uart_read_byte(void)
+{
+    uint8_t data = uart_buf[uart_get_ptr];
+    uart_get_ptr = (uart_get_ptr + 1) & (UART_BUFFER_RX - 1);
+    return data;
+}
+
+static inline uint8_t uart_read_byte_direct(void)
+{
+    return UDR0;
+}
+
 #ifdef DEBUG
 void debug_print_vga_buffer()
 {

        }
        return;
    }
    // command mode
}

void vga_interface_send_bytes(const uint8_t *buffer, uint16_t len)
{
    while (len--)
    {
        vga_interface_send_byte(*buffer);
        buffer++;
    }
}

    s_vga_buffer[addr] |= color;
}

void vga_interface_send_byte4(uint8_t data)
{
    if (s_vga_command == 0xFF)
    {
