diff --git a/cpu-avr.c b/cpu-avr.c
index d617d96..8f3dd27 100644
--- a/cpu-avr.c
+++ b/cpu-avr.c
@@ -18,16 +18,24 @@
 */
 void cpu_init() {
   #ifdef PRR
-    #if defined SPI
-      PRR = MASK(PRTWI) | MASK(PRADC);
+    #if defined I2C && defined SPI
+      PRR = MASK(PRADC);
+    #elif defined SPI
+      PRR = MASK(PRADC) | MASK(PRTWI);
+    #elif defined I2C
+      PRR = MASK(PRADC) | MASK(PRSPI);
     #else
-      PRR = MASK(PRTWI) | MASK(PRADC) | MASK(PRSPI);
+      PRR = MASK(PRADC) | MASK(PRTWI) | MASK(PRSPI);
     #endif
   #elif defined PRR0
-    #if defined SPI
-      PRR0 = MASK(PRTWI) | MASK(PRADC);
+    #if defined I2C && defined SPI
+      PRR0 = MASK(PRADC);
+    #elif defined SPI
+      PRR0 = MASK(PRADC) | MASK(PRTWI);
+    #elif defined I2C
+      PRR0 = MASK(PRADC) | MASK(PRSPI);
     #else
-      PRR0 = MASK(PRTWI) | MASK(PRADC) | MASK(PRSPI);
+      PRR0 = MASK(PRADC) | MASK(PRTWI) | MASK(PRSPI);
     #endif
     #if defined(PRUSART3)
       // Don't use USART2 or USART3. Leave USART1 for GEN3 and derivatives.
diff --git a/i2c.c b/i2c.c
new file mode 100644
index 0000000..baaa033
--- /dev/null
+++ b/i2c.c
@@ -0,0 +1,394 @@
+
+/** \file
+  \brief I2C / TWI subsystem
+
+  "TWI", "Two Wire Interface", is Atmel's name for the (patented) I2C bus
+  system. I2C is technically pretty sophisticated, it also takes the current
+  state of these two wires as part of the protocol into account. Like SPI it's
+  a master/slave system with a clock signal on the wire. Unlike SPI,
+  communication partners aren't choosen by setting a pin, but by by
+  transferring an address byte before the actual data.
+
+  Accordingly, code has to deal with states, transmissions have a start and
+  an end, and actions on the bus can result in different states, like success
+  or failure.
+
+  avr-libc comes with good sample code:
+
+    http://www.nongnu.org/avr-libc/examples/twitest/twitest.c
+
+  For technical details see section 22 of atmega328 datasheet.
+*/
+
+#include "i2c.h"
+
+#ifdef I2C
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+
+#if defined I2C_MASTER_MODE && defined I2C_SLAVE_MODE
+  #error Cant be I2C master and slave at the same time.
+#endif
+
+
+// Address of the device that is communicated with.
+uint8_t i2c_address;
+// State of TWI component of MCU.
+volatile uint8_t i2c_state;
+// Index into the send/receive buffer.
+uint8_t i2c_index;
+// Count of bytes to be sent.
+uint8_t i2c_byte_count;
+
+#ifdef I2C_EEPROM_SUPPORT
+  // For SAWSARP mode (see ENHA in i2c.h).
+  uint8_t i2c_page_address[I2C_PAGE_ADDRESS_SIZE];
+  // Index into the page address buffer.
+  uint8_t i2c_page_index;
+  // Count of bytes in page address.
+  uint8_t i2c_page_count;
+#endif /* I2C_EEPROM_SUPPORT */
+
+#ifdef I2C_SLAVE_MODE
+  uint8_t i2c_in_buffer[I2C_SLAVE_RX_BUFFER_SIZE];
+  uint8_t i2c_out_buffer[I2C_SLAVE_TX_BUFFER_SIZE];
+#else
+  uint8_t *i2c_buffer;
+#endif /* I2C_SLAVE_MODE */
+
+I2C_HANDLER i2c_master_func = &i2c_do_nothing;
+I2C_HANDLER i2c_slave_func = &i2c_do_nothing;
+I2C_HANDLER i2c_error_func = &i2c_do_nothing;
+
+
+void i2c_init(uint8_t address, I2C_HANDLER func) {
+
+  i2c_address = address;
+
+  #ifdef I2C_MASTER_MODE
+    #ifdef I2C_ENABLE_PULLUPS
+      I2C_DDR &= ~((1 << I2C_SCL_PIN) | (1 << I2C_SDA_PIN));
+      I2C_PORT |= (1 << I2C_SCL_PIN) | (1 << I2C_SDA_PIN);
+    #endif /* I2C_ENABLE_PULLUPS */
+
+    i2c_master_func = func;
+
+    /**
+      TWI Bit Rate register
+      SCL_freq = CPU_freq / (16 + 2 * TWBR)
+      See: page 235 of atmega328 datasheet.
+    */
+    TWBR = ((F_CPU / I2C_BITRATE) - 16) / 2;
+
+    /**
+      TWI Status Register
+      Lower two bits set the prescaler value.
+      See: page 236 of atmega328 datasheet.
+    */
+    TWSR = 0x00;
+  #endif /* I2C_MASTER_MODE */
+
+  #ifdef I2C_SLAVE_MODE
+    i2c_slave_func = func;
+    TWAR = i2c_address; // We listen to broadcasts if lowest bit is set.
+    TWCR = (0<<TWINT)|(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+  #endif
+}
+
+static void i2c_send_handler(void) {
+
+  i2c_state = I2C_MODE_SAWP; // Just send.
+  i2c_master_func = &i2c_send_handler;
+  i2c_error_func = &i2c_send_handler;
+
+  // Start transmission.
+  TWCR = (1<<TWINT)|(0<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+  i2c_state |= I2C_MODE_BUSY;
+}
+
+/**
+  Function sends a data block to slave device.
+*/
+void i2c_send(uint8_t address, uint8_t* block, uint8_t tx_len) {
+
+  i2c_address = address;
+  i2c_buffer = block;
+  i2c_index = 0;
+  i2c_byte_count = tx_len;
+
+  i2c_send_handler();
+}
+
+/**
+  Empty handler.
+*/
+void i2c_do_nothing(void) {
+}
+
+/**
+  This is the interrupt for all the I2C things.
+
+  A few words about TWCR bits:
+
+    Bit 7 (TWINT) is used to run TWI.
+
+    Bit 6 (TWEA) is used to send ACK (if set) in cases of
+
+      a) device's own slave address has been received;
+      b) general call has been received;
+      c) a data byte has been received.
+
+    Bit 5 (TWSTA) is 1 if app wants to be a master, don't forget to
+                  clear this bit.
+
+    Bit 4 (TWSTO) is generated STOP in master mode if set (cleared
+                  automaticly), recovered from error condition in slave
+                  mode if set.
+
+    Bit 3 (TWWC) is write collision flag. Sets on try to writeto TWDR when
+                 TWINT is low.
+
+    Bit 2 (TWEN) activates SDA/SCL pins if set. Set to 0 to disable TWI.
+
+    Bit 1 (Reserved).
+
+    Bit 0 (TWIE) enables TWI interrupt.
+*/
+ISR(TWI_vect) {
+
+  switch (TWSR & 0xF8) {  // Cut the prescaler bits out.
+    case I2C_STATE_BUS_FAIL:
+      // A hardware error was detected.
+      i2c_state |= I2C_ERROR_BUS_FAIL;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      MACRO_I2C_ERROR;
+      break;
+    case I2C_STATE_START:
+      // Start happens, send a target address.
+      if ((i2c_state & I2C_MODE_MASK) == I2C_MODE_SARP) {
+        i2c_address |= 0x01;
+      } else {
+        i2c_address &= 0xFE;
+      }
+      TWDR = i2c_address;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      break;
+    case I2C_STATE_RESTART:
+      // Start happens, send a target address.
+      if ((i2c_state & I2C_MODE_MASK) == I2C_MODE_ENHA) {
+        i2c_address |= 0x01;
+      } else {
+        i2c_address &= 0xFE;
+      }
+      TWDR = i2c_address;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      break;
+    case I2C_STATE_SLAWACK:
+      // SLA+W was sent, then ACK received.
+      if ((i2c_state & I2C_MODE_MASK) == I2C_MODE_SAWP) {
+        TWDR = i2c_buffer[i2c_index++];
+        TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      #ifdef I2C_EEPROM_SUPPORT
+        if ((i2c_state & I2C_MODE_MASK) == I2C_MODE_ENHA) {
+          TWDR = i2c_page_address[i2c_page_index++];
+          TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+        }
+      #endif
+      break;
+    case I2C_STATE_SLAWNACK:
+      // SLA+W was sent, got NACK, so slave is busy or out of bus.
+      i2c_state |= I2C_ERROR_NO_ANSWER;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      MACRO_I2C_ERROR;
+      break;
+    case I2C_STATE_BYTEACK:
+      // A byte was sent, got ACK.
+      if ((i2c_state & I2C_MODE_MASK) == I2C_MODE_SAWP) {
+        if (i2c_index == i2c_byte_count) {
+          // Last byte, send stop condition.
+          TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(1<<TWEN)|(0<<TWIE);
+        } else {
+          // Send the next byte.
+          TWDR = i2c_buffer[i2c_index++];
+          TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+        }
+      }
+      #ifdef I2C_EEPROM_SUPPORT
+        if ((i2c_state & I2C_MODE_MASK) == I2C_MODE_ENHA) {
+          // It was a page address byte.
+          if (i2c_page_index == i2c_page_count) {
+            // It was the last byte of the page address.
+            TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+          } else {
+            // Send the next page address byte.
+            TWDR = i2c_page_address[i2c_page_index++];
+            TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+          }
+        }
+      #endif
+      break;
+    case I2C_STATE_BYTENACK:
+      // Byte was sent but got NACK, there are two possible reasons:
+      //  - a slave stops transmission and it is ok, or
+      //  - a slave became crazy.
+      i2c_state |= I2C_ERROR_NACK;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      MACRO_I2C_MASTER; // process exit state
+      break;
+    case I2C_STATE_COLLISION:
+      // It looks like there is another master on the bus.
+      i2c_state |= I2C_ERROR_LOW_PRIO;
+      // Setup all again.
+      i2c_index = 0;
+      #ifdef I2C_EEPROM_SUPPORT
+        i2c_page_index = 0;
+      #endif
+      // Try to resend when the bus became free.
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      break;
+    case I2C_STATE_SLARACK:
+      // SLA+R was sent, got АСК, then received a byte.
+      if (i2c_index + 1 == i2c_byte_count) {
+        // Last byte fitting into the buffer. Request a byte, then send NACK
+        // to slave and it will release the bus.
+        TWCR = (1<<TWINT)|(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // Or just receive a byte and sent ACK.
+        TWCR = (1<<TWINT)|(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      break;
+    case I2C_STATE_SLARNACK:
+      // SLA+R was sent, got NАСК, it seems the slave is busy.
+      i2c_state |= I2C_ERROR_NO_ANSWER;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      MACRO_I2C_ERROR;
+      break;
+    case I2C_STATE_GOT_BYTE:
+      i2c_buffer[i2c_index++] = TWDR;
+      // TODO: Add BUFFER OVERFLOW check.
+      if (i2c_index + 1 == i2c_byte_count) {
+        // Last byte wait the processing.
+        TWCR = (1<<TWINT)|(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // Request the next byte.
+        TWCR = (1<<TWINT)|(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      break;
+    case I2C_STATE_GOT_BYTE_NACK:
+      // Last byte received, send NACK to make the slave to release the bus.
+      i2c_buffer[i2c_index] = TWDR;
+      TWCR = (1<<TWINT)|(I2C_MODE<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      MACRO_I2C_MASTER;
+      break;
+    case I2C_STATE_SLAW_LP:
+    case I2C_STATE_SLAW_LP_ANY:
+      // Another master on the bus sent some bytes, receive them.
+      i2c_state |= I2C_ERROR_LOW_PRIO;
+      // Restore the transfer.
+      i2c_index = 0;
+      #ifdef I2C_EEPROM_SUPPORT
+        i2c_page_index = 0;
+      #endif
+
+    #ifdef I2C_SLAVE_MODE
+
+    case I2C_STATE_SLAW:
+    case I2C_STATE_SLAW_ANY:
+      i2c_state |= I2C_MODE_BUSY; // Lock bus.
+      i2c_index = 0;
+      if (I2C_SLAVE_RX_BUFFER_SIZE == 1) {
+        // We should take alone byte and send NACK.
+        TWCR = (1<<TWINT)|(0<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // Get a byte and send ACK.
+        TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      break;
+    case I2C_STATE_RCV_BYTE:
+    case I2C_STATE_RCV_BYTE_ANY:
+      i2c_in_buffer[i2c_index++] = TWDR;
+      if (i2c_index == I2C_SLAVE_RX_BUFFER_SIZE - 1) {
+        // Room for only one byte left, send NACK.
+        TWCR = (1<<TWINT)|(0<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // We have room for more bytes, send ACK.
+        TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      break;
+    case I2C_STATE_RCV_LAST_BYTE:
+    case I2C_STATE_RCV_LAST_BYTE_ANY:
+      i2c_in_buffer[i2c_index] = TWDR;
+      if (i2c_state & I2C_INTERRUPTED) {
+        // Если у нас был прерываный сеанс от имени мастера
+        // Влепим в шину свой Start поскорей и сделаем еще одну попытку
+        TWCR = (1<<TWINT)|(1<TWEA)|(1<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // Если не было такого факта, то просто отвалимся и будем ждать
+        TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      MACRO_I2C_SLAVE;
+      break;
+    case I2C_STATE_RCV_RESTART:
+      // We got a Restart. What we will do?
+      // Here we can do additional logic but we don't need it at this time.
+      // Just ignore it now.
+      TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      break;
+    case I2C_STATE_RCV_SLAR_LP:
+      // Got own address on read from another master.
+      i2c_state |= I2C_ERROR_LOW_PRIO | I2C_INTERRUPTED;
+
+      // Reinit.
+      i2c_index = 0;
+      #ifdef I2C_EEPROM_SUPPORT
+        i2c_page_index = 0;
+      #endif
+    case I2C_STATE_RCV_SLAR:
+      // We have got own address on read.
+      i2c_index = 0;
+      TWDR = i2c_out_buffer[i2c_index];
+      if (I2C_SLAVE_TX_BUFFER_SIZE == 1) {
+        // If it is last byte, we hope to receive NACK.
+        TWCR = (1<<TWINT)|(0<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // Wait for ACK.
+        TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      break;
+    case I2C_STATE_SND_BYTE_ACK:
+      // Send byte and got ACK, then send next byte to master.
+      TWDR = i2c_out_buffer[++i2c_index];
+      if (I2C_SLAVE_TX_BUFFER_SIZE - 1 == i2c_index) {
+        // It was last byte, send it and wait for NACK.
+        TWCR = (1<<TWINT)|(0<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // Send byte and wait for ACK.
+        TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(0<<TWEN)|(1<<TWIE);
+      }
+      break;
+    case I2C_STATE_SND_LAST_BYTE_NACK:
+    case I2C_STATE_SND_LAST_BYTE_ACK:
+      // We sent the last byte and received NACK or ACK (doesn't matter here).
+      if (i2c_state & I2C_INTERRUPTED) {
+        // There was interrupted master transfer.
+        i2c_state &= I2C_NOINTERRUPTED;
+        // Generate start as the bus became free.
+        TWCR = (1<<TWINT)|(1<TWEA)|(1<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      } else {
+        // If we alone then just release the bus.
+        TWCR = (1<<TWINT)|(1<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(1<<TWEN)|(1<<TWIE);
+      }
+      MACRO_I2C_SLAVE;
+      break;
+
+    #endif /* I2C_SLAVE_MODE */
+
+    default:
+      break;
+    }
+}
+
+#endif /* I2C */
diff --git a/i2c.h b/i2c.h
new file mode 100644
index 0000000..0e0cfd8
--- /dev/null
+++ b/i2c.h
@@ -0,0 +1,110 @@
+#ifndef _I2C_H
+#define _I2C_H
+
+#include "config_wrapper.h"
+
+#ifdef I2C
+
+// Uncomment if we act as master device.
+#define I2C_MASTER_MODE
+// Uncomment if we act as slave device.
+//#define I2C_SLAVE_MODE
+// Uncomment if we use EEPROM chips.
+//#define I2C_EEPROM_SUPPORT
+
+#define I2C_BITRATE                 100000
+#define I2C_PORT                    PORTC
+#define I2C_DDR                     DDRC
+
+#define I2C_SCL_PIN                 0
+#define I2C_SDA_PIN                 1
+#define I2C_ENABLE_PULLUPS
+
+#define I2C_BUFFER_SIZE             4
+#ifdef I2C_EEPROM_SUPPORT
+  // Depends on EEPROM type, usually it is 1 or 2 bytes.
+  #define I2C_PAGE_ADDRESS_SIZE     2
+#endif /* I2C_EEPROM_SUPPORT */
+
+#ifdef I2C_SLAVE_MODE
+  #define I2C_SLAVE_RX_BUFFER_SIZE  1
+  #define I2C_SLAVE_TX_BUFFER_SIZE  1
+#endif /* I2C_SLAVE_MODE */
+
+#ifdef I2C_SLAVE_MODE
+  #define I2C_MODE 1
+#else
+  #define I2C_MODE 0
+#endif
+
+#define I2C_MODE_MASK           0b00001100
+#define I2C_MODE_SARP           0b00000000 // Start-Addr_R-Read-Stop: just read mode
+#define I2C_MODE_SAWP           0b00000100 // Start-Addr_W-Write-Stop: just write mode
+#define I2C_MODE_ENHA           0b00001000 // Start-Addr_W-WrPageAdr-rStart-Addr_R-Read-Stop
+#define I2C_MODE_BUSY           0b01000000 // Transponder is busy
+#define I2C_MODE_FREE           0b10111111 // Transponder is free
+
+// Transmission interrupted.
+#define I2C_INTERRUPTED                 0b10000000
+// Transmission not interrupted.
+#define I2C_NOINTERRUPTED               0b01111111
+
+#define I2C_ERROR_BUS_FAIL              0b00000001
+#define I2C_ERROR_NACK                  0b00000010
+#define I2C_ERROR_NO_ANSWER             0b00010000
+#define I2C_ERROR_LOW_PRIO              0b00100000
+
+#define I2C_STATE_BUS_FAIL              0x00
+#define I2C_STATE_START                 0x08
+#define I2C_STATE_RESTART               0x10
+#define I2C_STATE_SLAWACK               0x18
+#define I2C_STATE_SLAWNACK              0x20
+#define I2C_STATE_BYTEACK               0x28
+#define I2C_STATE_BYTENACK              0x30
+#define I2C_STATE_COLLISION             0x38
+#define I2C_STATE_SLARACK               0x40
+#define I2C_STATE_SLARNACK              0x48
+#define I2C_STATE_GOT_BYTE              0x50
+#define I2C_STATE_GOT_BYTE_NACK         0x58
+#define I2C_STATE_SLAW_LP               0x68
+#define I2C_STATE_SLAW_LP_ANY           0x78
+
+#ifdef I2C_SLAVE_MODE
+  #define I2C_STATE_SLAW                0x60
+  #define I2C_STATE_SLAW_ANY            0x70
+  #define I2C_STATE_RCV_BYTE            0x80
+  #define I2C_STATE_RCV_BYTE_ANY        0x90
+  #define I2C_STATE_RCV_LAST_BYTE       0x88
+  #define I2C_STATE_RCV_LAST_BYTE_ANY   0x98
+  #define I2C_STATE_RCV_RESTART         0xA0
+  #define I2C_STATE_RCV_SLAR            0xA8
+  #define I2C_STATE_RCV_SLAR_LP         0xB0
+  #define I2C_STATE_SND_BYTE_ACK        0xB8
+  #define I2C_STATE_SND_LAST_BYTE_NACK  0xC0
+  #define I2C_STATE_SND_LAST_BYTE_ACK   0xC0
+#endif /* I2C_SLAVE_MODE */
+
+#define MACRO_I2C_ERROR         (i2c_error_func)()
+#ifdef I2C_SLAVE_MODE
+  #define MACRO_I2C_SLAVE         (i2c_slave_func)()
+#else
+  #define MACRO_I2C_MASTER        (i2c_master_func)()
+#endif /* I2C_SLAVE_MODE */
+
+
+typedef void (*I2C_HANDLER)(void);
+
+extern I2C_HANDLER i2c_error_func;
+#ifdef I2C_SLAVE_MODE
+  extern I2C_HANDLER i2c_slave_func;
+#else
+  extern I2C_HANDLER i2c_master_func;
+#endif /* I2C_SLAVE_MODE */
+
+void i2c_init(uint8_t address, I2C_HANDLER func);
+void i2c_send(uint8_t address, uint8_t* block, uint8_t tx_len);
+void i2c_do_nothing(void);
+
+#endif /* I2C */
+
+#endif /* _I2C_H */
diff --git a/i2c_test.c b/i2c_test.c
new file mode 100644
index 0000000..15d7f57
--- /dev/null
+++ b/i2c_test.c
@@ -0,0 +1,189 @@
+
+/**
+  Quick test for sending some visible data to a SSD1306 display connected
+  via I2C. This means not to test the display, but the I2C implementation.
+
+  To run this test, add this line to board or printer config.h:
+
+    #define I2C
+
+  With this done, a welcome message should appear on the display. Without
+  these additions, the binary size should be as small as the commit before.
+*/
+
+#ifdef I2C_TEST
+
+#include <string.h>
+#include "config_wrapper.h"
+#include "delay.h"
+#include "i2c.h"
+
+
+#define DISPLAY_I2C_ADDRESS        (0x3C << 1)
+
+static uint8_t display_init[] = {
+  0x00,             // Command marker.
+  0xAE,             // Display off.
+  0xD5, 0x80,       // Display clock divider (reset).
+  0xA8, 0x1F,       // 1/32 duty.
+  0x40 | 0x00,      // Start line (reset).
+  0x20, 0x02,       // Page addressing mode (reset).
+  0xA0 | 0x00,      // No segment remap (reset).
+  0xC0 | 0x00,      // Normal com pins mapping (reset).
+  0xDA, 0x02,       // Sequental without remap com pins.
+  0x81, 0x7F,       // Contrast (reset).
+  0xDB, 0x20,       // Vcomh (reset).
+  0xD9, 0xF1,       // Precharge period.
+  0x8D, 0x14,       // Charge pump.
+  0xA6,             // Positive display.
+  0xA4,             // Resume display.
+  0xAF              // Display on.
+};
+
+typedef struct {
+  uint8_t columns;
+  uint8_t data[4];
+} SYMBOL;
+
+SYMBOL font_8x4[] = {
+  {2, {0x00, 0x00, 0x00, 0x00}},  /* space */
+  {3, {0x0C, 0x5E, 0x0C, 0x00}},  /* excl_mark */
+  {3, {0x03, 0x00, 0x03, 0x00}},  /* quot_mark */
+  {4, {0x74, 0x2E, 0x74, 0x2E}},  /* num_sign */
+  {4, {0x24, 0xCA, 0x53, 0x24}},  /* dlr_sign */
+  {4, {0x44, 0x30, 0x0C, 0x22}},  /* prc_sign */
+  {4, {0x33, 0x4D, 0x22, 0x00}},  /* ampersand */
+  {2, {0x02, 0x01, 0x00, 0x00}},  /* apostrophe */
+  {2, {0x3E, 0x41, 0x00, 0x00}},  /* lparent */
+  {2, {0x41, 0x3E, 0x00, 0x00}},  /* rparent */
+  {3, {0x14, 0x3E, 0x14, 0x00}},  /* asterisk */
+  {3, {0x08, 0x1C, 0x08, 0x00}},  /* plus_sign */
+  {2, {0x40, 0x20, 0x00, 0x00}},  /* comma */
+  {3, {0x08, 0x08, 0x08, 0x00}},  /* minus_sign */
+  {1, {0x20, 0x00, 0x00, 0x00}},  /* dot */
+  {3, {0x30, 0x0C, 0x03, 0x00}},  /* solidus */
+
+  {4, {0x3E, 0x41, 0x41, 0x3E}},  /* num_0 */
+  {3, {0x44, 0x7F, 0x40, 0x00}},  /* num_1 */
+  {4, {0x62, 0x51, 0x49, 0x46}},  /* num_2 */
+  {4, {0x22, 0x49, 0x49, 0x36}},  /* num_3 */
+  {4, {0x0F, 0x08, 0x08, 0x7E}},  /* num_4 */
+  {4, {0x27, 0x45, 0x45, 0x39}},  /* num_5 */
+  {4, {0x3E, 0x45, 0x45, 0x38}},  /* num_6 */
+  {4, {0x01, 0x71, 0x09, 0x07}},  /* num_7 */
+  {4, {0x36, 0x49, 0x49, 0x36}},  /* num_8 */
+  {4, {0x26, 0x49, 0x49, 0x3E}},  /* num_9 */
+  {1, {0x24, 0x00, 0x00, 0x00}},  /* colon */
+  {2, {0x40, 0x24, 0x00, 0x00}},  /* semicolon */
+  {3, {0x08, 0x14, 0x22, 0x00}},  /* less_sign */
+  {3, {0x14, 0x14, 0x14, 0x00}},  /* equal_sign */
+  {3, {0x22, 0x14, 0x08, 0x00}},  /* great_sign */
+  {3, {0x02, 0x51, 0x0E, 0x00}},  /* ques_mark */
+
+  {4, {0x7E, 0x99, 0xA5, 0x3E}},  /* at */
+  {3, {0x7E, 0x11, 0x7F, 0x00}},  /* a_cap */
+  {3, {0x7F, 0x49, 0x36, 0x00}},  /* b_cap */
+  {3, {0x3E, 0x41, 0x22, 0x00}},  /* c_cap */
+  {3, {0x7F, 0x41, 0x3E, 0x00}},  /* d_cap */
+  {3, {0x7F, 0x49, 0x41, 0x00}},  /* e_cap */
+  {3, {0x7F, 0x09, 0x01, 0x00}},  /* f_cap */
+  {3, {0x3E, 0x41, 0x71, 0x00}},  /* g_cap */
+  {3, {0x7F, 0x08, 0x7F, 0x00}},  /* h_cap */
+  {3, {0x41, 0x7F, 0x41, 0x00}},  /* i_cap */
+  {3, {0x40, 0x41, 0x3F, 0x00}},  /* j_cap */
+  {3, {0x7F, 0x0C, 0x73, 0x00}},  /* k_cap */
+  {3, {0x7F, 0x40, 0x40, 0x00}},  /* l_cap */
+  {3, {0x7F, 0x06, 0x7F, 0x00}},  /* m_cap */
+  {3, {0x7F, 0x3C, 0x7F, 0x00}},  /* n_cap */
+  {3, {0x3E, 0x41, 0x3E, 0x00}},  /* o_cap */
+
+  {3, {0x7F, 0x11, 0x0E, 0x00}},  /* p_cap */
+  {3, {0x3E, 0x41, 0xBE, 0x00}},  /* q_cap */
+  {3, {0x7F, 0x11, 0x6E, 0x00}},  /* r_cap */
+  {3, {0x46, 0x49, 0x31, 0x00}},  /* s_cap */
+  {3, {0x01, 0x7F, 0x01, 0x00}},  /* t_cap */
+  {3, {0x3F, 0x40, 0x7F, 0x00}},  /* u_cap */
+  {3, {0x3F, 0x40, 0x3F, 0x00}},  /* v_cap */
+  {3, {0x7F, 0x30, 0x7F, 0x00}},  /* w_cap */
+  {3, {0x73, 0x0C, 0x73, 0x00}},  /* x_cap */
+  {3, {0x0F, 0x70, 0x0F, 0x00}},  /* y_cap */
+  {3, {0x61, 0x5D, 0x47, 0x00}},  /* z_cap */
+  {2, {0x7F, 0x41, 0x00, 0x00}},  /* lq_bracket */
+  {3, {0x06, 0x18, 0x60, 0x00}},  /* rev_solidus */
+  {2, {0x41, 0x7F, 0x00, 0x00}},  /* rq_bracket */
+  {3, {0x02, 0x01, 0x02, 0x00}},  /* circumflex_accent */
+  {3, {0x40, 0x40, 0x40, 0x00}},  /* low_line */
+
+  {2, {0x01, 0x02, 0x00, 0x00}},  /* grave_accent */
+  {3, {0x30, 0x48, 0x78, 0x00}},  /* a_sml */
+  {3, {0x7E, 0x48, 0x30, 0x00}},  /* b_sml */
+  {3, {0x30, 0x48, 0x48, 0x00}},  /* c_sml */
+  {3, {0x30, 0x48, 0x7E, 0x00}},  /* d_sml */
+  {3, {0x30, 0x58, 0x58, 0x00}},  /* e_sml */
+  {2, {0x7C, 0x12, 0x00, 0x00}},  /* f_sml */
+  {3, {0x10, 0xA8, 0x70, 0x00}},  /* g_sml */
+  {3, {0x7E, 0x08, 0x70, 0x00}},  /* h_sml */
+  {1, {0x74, 0x00, 0x00, 0x00}},  /* i_sml */
+  {2, {0x80, 0x7A, 0x00, 0x00}},  /* j_sml */
+  {3, {0x7E, 0x10, 0x68, 0x00}},  /* k_sml */
+  {2, {0x3E, 0x40, 0x00, 0x00}},  /* l_sml */
+  {3, {0x78, 0x38, 0x70, 0x00}},  /* m_sml */
+  {3, {0x78, 0x08, 0x70, 0x00}},  /* n_sml */
+  {3, {0x30, 0x48, 0x30, 0x00}},  /* o_sml */
+
+  {3, {0xF8, 0x48, 0x30, 0x00}},  /* p_sml */
+  {3, {0x30, 0x48, 0xF8, 0x00}},  /* q_sml */
+  {2, {0x70, 0x08, 0x00, 0x00}},  /* r_sml */
+  {3, {0x50, 0x58, 0x28, 0x00}},  /* s_sml */
+  {2, {0x3E, 0x44, 0x00, 0x00}},  /* t_sml */
+  {3, {0x38, 0x40, 0x78, 0x00}},  /* u_sml */
+  {3, {0x38, 0x40, 0x38, 0x00}},  /* v_sml */
+  {3, {0x78, 0x60, 0x78, 0x00}},  /* w_sml */
+  {3, {0x68, 0x10, 0x68, 0x00}},  /* x_sml */
+  {3, {0x18, 0xA0, 0x78, 0x00}},  /* y_sml */
+  {3, {0x68, 0x68, 0x58, 0x00}},  /* z_sml */
+  {3, {0x08, 0x36, 0x41, 0x00}},  /* left_curly_bracket */
+  {1, {0x7F, 0x00, 0x00, 0x00}},  /* vertical_line */
+  {3, {0x41, 0x36, 0x08, 0x00}},  /* rigth_curly_bracket */
+  {4, {0x02, 0x01, 0x02, 0x01}},  /* tilde */
+  {4, {0x00, 0x00, 0x00, 0x00}}   /* del */
+};
+
+#define FONT_SYMBOLS_SPACE 1
+
+
+static void i2c_test(void) {
+  static char* message = "Welcome to Teacup";
+  static uint8_t block[128];
+  uint8_t* pointer = block + 1;
+
+  i2c_send(DISPLAY_I2C_ADDRESS, display_init, sizeof(display_init));
+  delay_ms(250);
+
+  /**
+    Setup cursor on display.
+
+    "Welcome to Teacup" is 64 pixel columns wide, entire display is
+    128 columns, so we offset by 32 columns to get it to the center.
+  */
+  block[0] = 0x00;
+  // Line 1.
+  block[1] = 0xB0 | 1;
+  // Column 32.
+  block[2] = 0x00 | (32 & 0x0F);
+  block[3] = 0x10 | ((32 >> 4) & 0x0F);
+  i2c_send(DISPLAY_I2C_ADDRESS, block, 4);
+  delay_ms(50);
+
+  // Render text to bitmap.
+  while (*message) {
+    SYMBOL symbol = font_8x4[(uint8_t)*message - 0x20];
+    memcpy(pointer, symbol.data, symbol.columns);
+    pointer += symbol.columns + FONT_SYMBOLS_SPACE;
+    message++;
+  }
+  block[0] = 0x40; // Data marker.
+  i2c_send(DISPLAY_I2C_ADDRESS, block, pointer - block);
+}
+
+#endif /* I2C_TEST */
diff --git a/mendel.c b/mendel.c
index c79e4eb..36774bd 100644
--- a/mendel.c
+++ b/mendel.c
@@ -24,11 +24,19 @@
 			ctrl+d \endcode
 */
 
+
 #ifdef __AVR__
 #include	<avr/interrupt.h>
 #endif
 
 #include	"config_wrapper.h"
+#ifdef I2C
+  // This is temporary, until display code is completed.
+  // It includes static i2c_test(), which is called in main():
+  #define I2C_TEST
+  #include "i2c_test.c"
+  #undef I2C_TEST
+#endif
 #include "cpu.h"
 #include	"serial.h"
 #include	"dda_queue.h"
@@ -43,6 +51,7 @@
 #include	"clock.h"
 #include	"intercom.h"
 #include "spi.h"
+#include "i2c.h"
 #include "sd.h"
 #include "simulator.h"
 
@@ -86,6 +95,10 @@ void init(void) {
     spi_init();
   #endif
 
+  #ifdef I2C
+    i2c_init(DISPLAY_I2C_ADDRESS, i2c_do_nothing);
+  #endif
+
 	// set up timers
 	timer_init();
 
@@ -134,6 +147,11 @@ int main (void)
 
 	init();
 
+  #ifdef I2C
+    // This is temporary, until display code is completed.
+    i2c_test();
+  #endif
+
 	// main loop
 	for (;;)
 	{