Index: c/src/lib/libbsp/arm/atsam/Makefile.am
===================================================================
--- c/src/lib/libbsp/arm/atsam/Makefile.am	(revision 0fddefe65b605e4d09d88c7036a323c133f87038)
+++ c/src/lib/libbsp/arm/atsam/Makefile.am	(revision 0c1413c28476173022c70c451127d4bce361aac1)
@@ -52,4 +52,6 @@
 include_bsp_HEADERS += include/irq.h
 include_bsp_HEADERS += include/pin-config.h
+include_bsp_HEADERS += include/atsam-i2c.h
+include_bsp_HEADERS += include/i2c.h
 
 include_libchipdir = $(includedir)/libchip
@@ -427,4 +429,8 @@
 endif
 
+# i2c
+libbsp_a_SOURCES += i2c/atsam_i2c_bus.c
+libbsp_a_SOURCES += i2c/atsam_i2c_init.c
+
 # Includes
 libbsp_a_CPPFLAGS += -I$(srcdir)/../shared/CMSIS/Include
Index: c/src/lib/libbsp/arm/atsam/i2c/atsam_i2c_bus.c
===================================================================
--- c/src/lib/libbsp/arm/atsam/i2c/atsam_i2c_bus.c	(revision 0c1413c28476173022c70c451127d4bce361aac1)
+++ c/src/lib/libbsp/arm/atsam/i2c/atsam_i2c_bus.c	(revision 0c1413c28476173022c70c451127d4bce361aac1)
@@ -0,0 +1,397 @@
+/*
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <bsp/atsam-i2c.h>
+
+#include <rtems/irq-extension.h>
+
+#define ATSAMV_I2C_IRQ_ERROR \
+	(TWIHS_IDR_ARBLST \
+	| TWIHS_IDR_TOUT \
+	| TWIHS_IDR_OVRE \
+	| TWIHS_IDR_UNRE \
+	| TWIHS_IDR_NACK)
+
+#define TEN_BIT_MASK 0xFC00
+#define SEVEN_BIT_MASK 0xFF80
+#define TEN_BIT_START_ADDR_MASK 0x78
+#define LAST_TWO_BITS_MASK 0x03
+#define LAST_BYTE_MASK 0x00FF
+
+static void
+atsam_i2c_disable_interrupts(Twihs *regs)
+{
+	regs->TWIHS_IDR = 0xFFFFFFFF;
+}
+
+static void
+atsam_i2c_set_transfer_status(transfer_desc *trans_desc, transfer_state state)
+{
+	trans_desc->trans_state = state;
+}
+
+static void
+atsam_i2c_continue_read(Twihs *regs, transfer_desc *trans_desc)
+{
+	trans_desc->data[trans_desc->already_transferred] = TWI_ReadByte(regs);
+	trans_desc->already_transferred++;
+
+	/* check for transfer finish */
+	if (trans_desc->already_transferred == trans_desc->data_size) {
+		if (trans_desc->stop_request){
+			TWI_DisableIt(regs, TWIHS_IDR_RXRDY);
+			TWI_EnableIt(regs, TWIHS_IER_TXCOMP);
+			atsam_i2c_set_transfer_status(trans_desc, TX_RX_STOP_SENT);
+		} else {
+			atsam_i2c_set_transfer_status(trans_desc, RX_CONT_MESSAGE_NEEDED);
+		}
+	}
+	/* Last byte? */
+	else if ((trans_desc->already_transferred == (trans_desc->data_size - 1))
+	    && (trans_desc->stop_request)){
+		TWI_Stop(regs);
+	}
+}
+
+static bool
+atsam_i2c_is_state(transfer_desc *trans_desc, transfer_state state)
+{
+	return (trans_desc->trans_state == state);
+}
+
+static void
+atsam_i2c_continue_write(Twihs *regs, transfer_desc *trans_desc)
+{
+	/* Transfer finished ? */
+	if (trans_desc->already_transferred == trans_desc->data_size) {
+		TWI_DisableIt(regs, TWIHS_IDR_TXRDY);
+		if (trans_desc->stop_request){
+			TWI_EnableIt(regs, TWIHS_IER_TXCOMP);
+			TWI_SendSTOPCondition(regs);
+			atsam_i2c_set_transfer_status(trans_desc, TX_RX_STOP_SENT);
+		} else {
+			atsam_i2c_set_transfer_status(trans_desc, TX_CONT_MESSAGE_NEEDED);
+		}
+	}
+	/* Bytes remaining */
+	else {
+		TWI_WriteByte(regs,
+		    trans_desc->data[trans_desc->already_transferred]);
+		trans_desc->already_transferred++;
+	}
+}
+
+static void
+atsam_i2c_finish_write_transfer(Twihs *regs, transfer_desc *trans_desc)
+{
+		TWI_ReadByte(regs);
+		TWI_DisableIt(regs, TWIHS_IDR_TXCOMP);
+		trans_desc->status = 0;
+}
+
+static void
+atsam_i2c_next_packet(atsam_i2c_bus *bus)
+{
+	i2c_msg *msg;
+
+	++bus->msgs;
+	--bus->msg_todo;
+
+	msg = &bus->msgs[0];
+
+	bus->current_msg_todo = msg->len;
+	bus->current_msg_byte = msg->buf;
+}
+
+static void
+atsam_i2c_set_td(atsam_i2c_bus *bus, uint32_t already_transferred,
+    bool stop_needed)
+{
+	transfer_desc *trans_desc = &bus->trans_desc;
+
+	trans_desc->status = ASYNC_STATUS_PENDING;
+	trans_desc->data = bus->current_msg_byte;
+	trans_desc->data_size = bus->current_msg_todo;
+	trans_desc->already_transferred = already_transferred;
+	trans_desc->stop_request = stop_needed;
+}
+
+static bool
+atsam_i2c_set_address_size(const i2c_msg *msg)
+{
+	bool rv = ((msg->flags & I2C_M_TEN) == 0) ? false : true;
+	return rv;
+}
+
+static void
+atsam_i2c_set_address_regs(Twihs *regs, uint16_t address, bool ten_bit_addr,
+    bool read_transfer)
+{
+	/* No internal addresses used */
+	uint32_t mmr_temp = 0;
+	uint32_t iadr_temp = 0;
+
+	assert(regs != NULL);
+	if (ten_bit_addr){
+		uint8_t addr_temp = TEN_BIT_START_ADDR_MASK;
+		assert(address & TEN_BIT_MASK);
+		mmr_temp = (1u << 8) | ((addr_temp & LAST_TWO_BITS_MASK) << 16);
+		iadr_temp = (addr_temp & LAST_BYTE_MASK);
+	} else {
+		assert((address & SEVEN_BIT_MASK) == 0);
+		mmr_temp = (address << 16);
+	}
+
+	if (read_transfer){
+		mmr_temp |= TWIHS_MMR_MREAD;
+	}
+
+	/* Set slave and number of internal address bytes */
+	regs->TWIHS_MMR = 0;
+	regs->TWIHS_MMR = mmr_temp;
+
+	/* Set internal address bytes */
+	regs->TWIHS_IADR = 0;
+	regs->TWIHS_IADR = iadr_temp;
+}
+
+static void
+atsam_i2c_setup_read_transfer(Twihs *regs, bool ctrl, uint16_t slave_addr,
+    bool send_stop)
+{
+	TWI_EnableIt(regs, TWIHS_IER_RXRDY);
+	atsam_i2c_set_address_regs(regs, slave_addr, ctrl, true);
+	if (send_stop){
+		regs->TWIHS_CR = TWIHS_CR_START | TWIHS_CR_STOP;
+	} else {
+		regs->TWIHS_CR = TWIHS_CR_START;
+	}
+}
+
+static void
+atsam_i2c_setup_write_transfer(atsam_i2c_bus *bus, Twihs *regs, bool ctrl,
+    uint16_t slave_addr)
+{
+	atsam_i2c_set_address_regs(regs, slave_addr, ctrl, false);
+	TWI_WriteByte(regs, *bus->current_msg_byte);
+	TWI_EnableIt(regs, TWIHS_IER_TXRDY);
+}
+
+static void
+atsam_i2c_setup_transfer(atsam_i2c_bus *bus, Twihs *regs)
+{
+	const i2c_msg *msgs = bus->msgs;
+	bool send_stop = false;
+	uint32_t msg_todo = bus->msg_todo;
+	uint16_t slave_addr;
+	bool ten_bit_addr = false;
+	uint32_t already_transferred;
+	bool stop_needed = true;
+
+	ten_bit_addr = atsam_i2c_set_address_size(msgs);
+
+	if ((msg_todo > 1) && ((msgs[1].flags & I2C_M_NOSTART) != 0)){
+		stop_needed = false;
+	}
+
+	bus->read = (msgs->flags & I2C_M_RD) != 0;
+	slave_addr = msgs->addr;
+	already_transferred = (bus->read == true) ? 0 : 1;
+	atsam_i2c_set_td(bus, already_transferred, stop_needed);
+
+	transfer_desc *trans_desc = &bus->trans_desc;
+
+	if (bus->read){
+		if (bus->current_msg_todo == 1){
+			send_stop = true;
+		}
+		atsam_i2c_set_transfer_status(trans_desc, RX_SEND_DATA);
+		atsam_i2c_setup_read_transfer(regs, ten_bit_addr,
+		    slave_addr, send_stop);
+	} else {
+		atsam_i2c_set_transfer_status(trans_desc, TX_SEND_DATA);
+		atsam_i2c_setup_write_transfer(bus, regs, ten_bit_addr,
+		    slave_addr);
+	}
+}
+
+static void
+atsam_i2c_interrupt(void *arg)
+{
+	atsam_i2c_bus *bus = arg;
+	uint32_t irqstatus;
+	bool done = false;
+	transfer_desc *trans_desc;
+
+	Twihs *regs = bus->regs;
+
+	/* read interrupts */
+	irqstatus = regs->TWIHS_SR;
+
+	if((irqstatus & (TWIHS_SR_ARBLST | TWIHS_SR_NACK)) != 0) {
+		done = true;
+	}
+
+	trans_desc = &bus->trans_desc;
+
+	if (((irqstatus & TWIHS_SR_RXRDY) != 0) &&
+	    (atsam_i2c_is_state(trans_desc, RX_SEND_DATA))){
+		/* carry on read transfer */
+		atsam_i2c_continue_read(regs, trans_desc);
+	} else if (((irqstatus & TWIHS_SR_TXCOMP) != 0) &&
+	    (atsam_i2c_is_state(trans_desc, TX_RX_STOP_SENT))){
+		atsam_i2c_finish_write_transfer(regs, trans_desc);
+		done = true;
+	} else if (((irqstatus & TWIHS_SR_TXRDY) != 0) &&
+	    (atsam_i2c_is_state(trans_desc, TX_SEND_DATA))){
+		atsam_i2c_continue_write(regs, trans_desc);
+		if (trans_desc->trans_state == TX_CONT_MESSAGE_NEEDED){
+			done = true;
+		}
+	}
+
+	if(done){
+		uint32_t err = irqstatus & ATSAMV_I2C_IRQ_ERROR;
+
+		atsam_i2c_next_packet(bus);
+		if (bus->msg_todo == 0 || err != 0) {
+			rtems_status_code sc;
+
+			atsam_i2c_disable_interrupts(regs);
+			sc = rtems_event_transient_send(bus->task_id);
+			assert(sc == RTEMS_SUCCESSFUL);
+		} else {
+			atsam_i2c_setup_transfer(bus, regs);
+		}
+	}
+}
+
+static int
+atsam_i2c_transfer(i2c_bus *base, i2c_msg *msgs, uint32_t msg_count)
+{
+	rtems_status_code sc;
+	atsam_i2c_bus *bus = (atsam_i2c_bus *)base;
+	Twihs *regs;
+	uint32_t i;
+
+	rtems_task_wake_after(1);
+
+	if (msg_count < 1){
+		return 1;
+	}
+
+	for (i = 0; i < msg_count; ++i) {
+		if ((msgs[i].flags & I2C_M_RECV_LEN) != 0) {
+			return -EINVAL;
+		}
+	}
+
+	bus->msgs = &msgs[0];
+	bus->msg_todo = msg_count;
+	bus->current_msg_todo = msgs[0].len;
+	bus->current_msg_byte = msgs[0].buf;
+	bus->task_id = rtems_task_self();
+
+	regs = bus->regs;
+
+	atsam_i2c_setup_transfer(bus, regs);
+
+	regs->TWIHS_IER = ATSAMV_I2C_IRQ_ERROR;
+
+	sc = rtems_event_transient_receive(RTEMS_WAIT, bus->base.timeout);
+	if (sc != RTEMS_SUCCESSFUL){
+		rtems_event_transient_clear();
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int
+atsam_i2c_set_clock(i2c_bus *base, unsigned long clock)
+{
+	atsam_i2c_bus *bus = (atsam_i2c_bus *)base;
+	Twihs *regs = bus->regs;
+	TWI_ConfigureMaster(regs, clock, BOARD_MCK);
+	return 0;
+}
+
+static void
+atsam_i2c_destroy(i2c_bus *base)
+{
+	atsam_i2c_bus *bus = (atsam_i2c_bus *)base;
+	rtems_status_code sc;
+
+	sc = rtems_interrupt_handler_remove(bus->irq, atsam_i2c_interrupt, bus);
+	assert(sc == RTEMS_SUCCESSFUL);
+
+	i2c_bus_destroy_and_free(&bus->base);
+}
+
+static void
+atsam_i2c_init(atsam_i2c_bus *bus, uint32_t input_clock, Twihs *board_base,
+    uint32_t board_id, const Pin *pins)
+{
+
+	/* Initialize the TWI */
+	PIO_Configure(pins, TWI_AMOUNT_PINS);
+
+	/* Enable the TWI clock */
+	ENABLE_PERIPHERAL(board_id);
+
+	TWI_ConfigureMaster(board_base, input_clock, BOARD_MCK);
+}
+
+int
+i2c_bus_register_atsam(
+    const char *bus_path,
+    Twihs *register_base,
+    rtems_vector_number irq,
+    const Pin pins[TWI_AMOUNT_PINS]
+)
+{
+	atsam_i2c_bus *bus;
+	rtems_status_code sc;
+	uint32_t board_id = (uint32_t) irq;
+
+	bus = (atsam_i2c_bus *) i2c_bus_alloc_and_init(sizeof(*bus));
+	if (bus == NULL){
+		return -1;
+	}
+
+	bus->regs = register_base;
+	bus->irq = irq;
+
+	atsam_i2c_init(bus, I2C_BUS_CLOCK_DEFAULT, bus->regs,
+	    board_id, pins);
+
+	sc = rtems_interrupt_handler_install(
+	    irq,
+	    "Atsamv_I2C",
+	    RTEMS_INTERRUPT_UNIQUE,
+	    atsam_i2c_interrupt,
+	    bus
+	);
+	if(sc != RTEMS_SUCCESSFUL){
+		(*bus->base.destroy)(&bus->base);
+
+		rtems_set_errno_and_return_minus_one(EIO);
+	}
+
+	bus->base.transfer = atsam_i2c_transfer;
+	bus->base.set_clock = atsam_i2c_set_clock;
+	bus->base.destroy = atsam_i2c_destroy;
+
+	return i2c_bus_register(&bus->base, bus_path);
+}
Index: c/src/lib/libbsp/arm/atsam/i2c/atsam_i2c_init.c
===================================================================
--- c/src/lib/libbsp/arm/atsam/i2c/atsam_i2c_init.c	(revision 0c1413c28476173022c70c451127d4bce361aac1)
+++ c/src/lib/libbsp/arm/atsam/i2c/atsam_i2c_init.c	(revision 0c1413c28476173022c70c451127d4bce361aac1)
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <bsp/atsam-i2c.h>
+#include <bsp/i2c.h>
+
+/** TWI0 data pin */
+#define PIN_TWI_TWD0 {PIO_PA3A_TWD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+/** TWI0 clock pin */
+#define PIN_TWI_TWCK0 {PIO_PA4A_TWCK0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+
+/** TWI1 data pin */
+#define PIN_TWI_TWD1 {PIO_PB4A_TWD1, PIOB, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT}
+/** TWI1 clock pin */
+#define PIN_TWI_TWCK1 {PIO_PB5A_TWCK1, PIOB, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT}
+
+/** TWI2 data pin */
+#define PIN_TWI_TWD2 {PIO_PD27C_TWD2, PIOD, ID_PIOD, PIO_PERIPH_C, PIO_DEFAULT}
+/** TWI2 clock pin */
+#define PIN_TWI_TWCK2 {PIO_PD28C_TWCK2, PIOD, ID_PIOD, PIO_PERIPH_C, PIO_DEFAULT}
+
+int atsam_register_i2c_0(void)
+{
+	static const Pin pins[] = {PIN_TWI_TWD0, PIN_TWI_TWCK0};
+
+	return i2c_bus_register_atsam(
+	    ATSAM_I2C_0_BUS_PATH,
+	    TWIHS0,
+	    ID_TWIHS0,
+	    pins);
+}
+
+int atsam_register_i2c_1(void)
+{
+	static const Pin pins[] = {PIN_TWI_TWD1, PIN_TWI_TWCK1};
+
+	return i2c_bus_register_atsam(
+	    ATSAM_I2C_1_BUS_PATH,
+	    TWIHS1,
+	    ID_TWIHS1,
+	    pins);
+}
+
+int atsam_register_i2c_2(void)
+{
+	static const Pin pins[] = {PIN_TWI_TWD2, PIN_TWI_TWCK2};
+
+	return i2c_bus_register_atsam(
+	    ATSAM_I2C_2_BUS_PATH,
+	    TWIHS2,
+	    ID_TWIHS2,
+	    pins);
+}
Index: c/src/lib/libbsp/arm/atsam/include/atsam-i2c.h
===================================================================
--- c/src/lib/libbsp/arm/atsam/include/atsam-i2c.h	(revision 0c1413c28476173022c70c451127d4bce361aac1)
+++ c/src/lib/libbsp/arm/atsam/include/atsam-i2c.h	(revision 0c1413c28476173022c70c451127d4bce361aac1)
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#ifndef LIBBSP_ARM_ATSAM_ATSAM_I2C_H
+#define LIBBSP_ARM_ATSAM_ATSAM_I2C_H
+
+#include <libchip/chip.h>
+#include <libchip/include/pio.h>
+
+#include <bsp.h>
+#include <dev/i2c/i2c.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+#define TWI_AMOUNT_PINS 2
+
+typedef enum {
+	TX_SEND_DATA,
+	TX_SEND_STOP,
+	TX_CONT_MESSAGE_NEEDED,
+	RX_SEND_DATA,
+	RX_SEND_STOP,
+	RX_CONT_MESSAGE_NEEDED,
+	TX_RX_STOP_SENT
+}transfer_state;
+
+typedef struct {
+	uint8_t status;
+	uint8_t *data;
+	bool stop_request;
+	uint32_t data_size;
+	uint32_t already_transferred;
+	transfer_state trans_state;
+} transfer_desc;
+
+typedef struct {
+	i2c_bus base;
+	i2c_msg *msgs;
+	Twihs *regs;
+	transfer_desc trans_desc;
+	uint32_t msg_todo;
+	uint32_t current_msg_todo;
+	uint8_t *current_msg_byte;
+	uint32_t input_clock;
+	bool read;
+	rtems_id task_id;
+	rtems_vector_number irq;
+} atsam_i2c_bus;
+
+int i2c_bus_register_atsam(
+    const char *bus_path,
+    Twihs *register_base,
+    rtems_vector_number irq,
+    const Pin pins[TWI_AMOUNT_PINS]
+);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_ARM_ATSAM_ATSAM_I2C_H */
Index: c/src/lib/libbsp/arm/atsam/include/i2c.h
===================================================================
--- c/src/lib/libbsp/arm/atsam/include/i2c.h	(revision 0c1413c28476173022c70c451127d4bce361aac1)
+++ c/src/lib/libbsp/arm/atsam/include/i2c.h	(revision 0c1413c28476173022c70c451127d4bce361aac1)
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#ifndef LIBBSP_ARM_ATSAM_I2C_H
+#define LIBBSP_ARM_ATSAM_I2C_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+#define ATSAM_I2C_0_BUS_PATH "/dev/i2c-0"
+#define ATSAM_I2C_1_BUS_PATH "/dev/i2c-1"
+#define ATSAM_I2C_2_BUS_PATH "/dev/i2c-2"
+
+int atsam_register_i2c_0(void);
+
+int atsam_register_i2c_1(void);
+
+int atsam_register_i2c_2(void);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_ARM_ATSAM_I2C_H */
Index: c/src/lib/libbsp/arm/atsam/preinstall.am
===================================================================
--- c/src/lib/libbsp/arm/atsam/preinstall.am	(revision 0fddefe65b605e4d09d88c7036a323c133f87038)
+++ c/src/lib/libbsp/arm/atsam/preinstall.am	(revision 0c1413c28476173022c70c451127d4bce361aac1)
@@ -134,4 +134,12 @@
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/pin-config.h
 
+$(PROJECT_INCLUDE)/bsp/atsam-i2c.h: include/atsam-i2c.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/atsam-i2c.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/atsam-i2c.h
+
+$(PROJECT_INCLUDE)/bsp/i2c.h: include/i2c.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/i2c.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/i2c.h
+
 $(PROJECT_INCLUDE)/libchip/$(dirstamp):
 	@$(MKDIR_P) $(PROJECT_INCLUDE)/libchip
