 
 void vga_interface_stop()
 {
+    s_vga_command = 0xFF;
 }
 
 void vga_interface_close()
 {
 }
 
-void vga_put_pixel(uint8_t x, uint8_t y, uint8_t color)
+void vga_put_pixel4(uint8_t x, uint8_t y, uint8_t color)
 {
     uint8_t mask;
     if (x&1)

    ssd1306_intf.spi = 0;
    ssd1306_intf.start = vga_interface_init;
    ssd1306_intf.stop = vga_interface_stop;
    ssd1306_intf.send = vga_interface_send_byte;
    ssd1306_intf.send_buffer = vga_interface_send_bytes;
    ssd1306_intf.close = vga_interface_close;
}

void vga_set_block1(lcduint_t x, lcduint_t y, lcduint_t w)
{

    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()
{
