From 619560c11203c714ae80ebf6508bcdf998f58c94 Mon Sep 17 00:00:00 2001
From: Ruslan Popov <ruslan.popov@gmail.com>
Date: Tue, 22 Sep 2015 20:05:02 +0300
Subject: [PATCH] I2C: initial support.

This commit squashes in some fixes found after debugging on the
topic branch. Test code by Traumflug, collected from Ruslan's
code on the topic branch.

Before, same as now without I2C:

    FLASH  : 22408 bytes
    RAM    :  1393 bytes
    EEPROM :    32 bytes

Now with I2C:

    FLASH  : 23224 bytes
    RAM    :  2057 bytes
    EEPROM :    32 bytes

This totals to some 800 bytes with a whole lot of test code, so
implementation is pretty small :-)
---
 cpu-avr.c  |  20 ++-
 i2c.c      | 394 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 i2c.h      | 110 +++++++++++++++
 i2c_test.c | 189 +++++++++++++++++++++++++
 mendel.c   |  18 +++
 5 files changed, 725 insertions(+), 6 deletions(-)
 create mode 100644 i2c.c
 create mode 100644 i2c.h
 create mode 100644 i2c_test.c

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