diff --git a/README.md b/README.md
index ac5d5bf..8889079 100644
--- a/README.md
+++ b/README.md
@@ -81,7 +81,7 @@ Initial value: 0x0
     the frame is considered timed out and all received data can be dropped.
 - Incoming messages are stored in a fifo until the application retrieves them.
 - The receive fifo has a size of at least 2 messages.
-- If the fifo is full new messages will override the older ones.
+- If the fifo is full all new messages will be dropped until messages are retrieved from the fifo.
     It is up to the application to retrieve message in time.
 - Even if sending was successful, there is still a chance that the receiver could not
     receive the frame due to missing buffer space or not enough processing time to react.
diff --git a/software/AVR8/Makefile b/software/AVR8/Makefile
index 6c182ac..4db3589 100644
--- a/software/AVR8/Makefile
+++ b/software/AVR8/Makefile
@@ -28,8 +28,8 @@ all: start $(OBJDIR)/$(AVRMCU)/$(TARGET).hex size
 	@echo ":: Done !"
 
 start:
-	@echo "AS5043 demo version $(VERSION)"
-	@echo "=============================="
+	@echo "SSS7 AVR port $(VERSION)"
+	@echo "========================"
 	@echo ":: Building for $(AVRMCU)"
 	@echo ":: MCU operating frequency is $(F_CPU)Hz"
 
diff --git a/software/Arduino/.clang_complete b/software/Arduino/.clang_complete
new file mode 100644
index 0000000..d4055d8
--- /dev/null
+++ b/software/Arduino/.clang_complete
@@ -0,0 +1,5 @@
+-I../sss7core
+-I/usr/avr/include
+-I./arduino-1.6.13/hardware/arduino/avr/cores/arduino/
+-D__AVR_ATmega2560__
+-DF_CPU=16000000
diff --git a/software/Arduino/.gitignore b/software/Arduino/.gitignore
new file mode 100644
index 0000000..632e324
--- /dev/null
+++ b/software/Arduino/.gitignore
@@ -0,0 +1,2 @@
+arduino-1.6.13
+bin
diff --git a/software/Arduino/Makefile b/software/Arduino/Makefile
new file mode 100644
index 0000000..bfd1165
--- /dev/null
+++ b/software/Arduino/Makefile
@@ -0,0 +1,111 @@
+AVRMCU ?= atmega2560
+F_CPU ?= 16000000
+ISPPORT ?= /dev/ttyUSB0
+
+VERSION = 0.1
+
+HEADERS = ardusss7.h ../sss7core/sss7.h
+SRC_C = ../sss7core/sss7.c
+SRC_CPP = main.cpp ardusss7.cpp
+TARGET = sss7
+OBJDIR = bin
+ARDUINO_DIR = ./arduino-1.6.13
+ARDUINO_BOARD = mega
+
+CC = avr-gcc
+CPP = avr-g++
+AR = avr-ar
+OBJCOPY = avr-objcopy
+OBJDUMP = avr-objdump
+SIZE = avr-size
+
+SRC_C_TMP = $(subst ../,,$(SRC_C))
+OBJ = $(SRC_C_TMP:%.c=$(OBJDIR)/$(AVRMCU)/%.o)
+OBJ += $(SRC_CPP:%.cpp=$(OBJDIR)/$(AVRMCU)/%.o)
+
+ARDU_HEADERS = Arduino.h binary.h Client.h HardwareSerial.h IPAddress.h \
+	new.h Printable.h Print.h Server.h \
+	Stream.h Udp.h USBAPI.h USBCore.h USBDesc.h WCharacter.h \
+	wiring_private.h WString.h
+
+ARDU_C_SRC = WInterrupts.c wiring_digital.c wiring_analog.c wiring_pulse.c \
+	wiring.c wiring_shift.c
+
+ARDU_CPP_SRC = CDC.cpp new.cpp HardwareSerial0.cpp PluggableUSB.cpp \
+	HardwareSerial1.cpp Print.cpp HardwareSerial2.cpp Stream.cpp \
+	HardwareSerial3.cpp Tone.cpp HardwareSerial.cpp USBCore.cpp \
+	IPAddress.cpp WMath.cpp main.cpp WString.cpp \
+
+ARDU_FULL_DIR = $(ARDUINO_DIR)/hardware/arduino/avr/cores/arduino
+ARDU_VARIANT_DIR = $(ARDUINO_DIR)/hardware/arduino/avr/variants/$(ARDUINO_BOARD)
+ARDU_FULL_HEADERS = $(ARDU_HEADERS:%.h=$(ARDU_FULL_DIR)/%.h)
+ARDU_FULL_HEADERS += $(ARDU_VARIANT_DIR)/pins_arduino.h
+ARDU_OBJS = $(ARDU_C_SRC:%.c=$(OBJDIR)/$(AVRMCU)/arduino/%.o)
+ARDU_OBJS += $(ARDU_CPP_SRC:%.cpp=$(OBJDIR)/$(AVRMCU)/arduino/%.o)
+
+CFLAGS = -I $(ARDU_FULL_DIR) -I $(ARDU_VARIANT_DIR) -I ../sss7core/ -Os -Wall -Wstrict-prototypes
+CFLAGS += -ffunction-sections -fdata-sections
+CFLAGS += -fshort-enums -fpack-struct -funsigned-char -funsigned-bitfields
+CFLAGS += -mmcu=$(AVRMCU) -DF_CPU=$(F_CPU)UL -DVERSION=$(VERSION)
+
+CPPFLAGS = -I $(ARDU_FULL_DIR) -I $(ARDU_VARIANT_DIR) -I ../sss7core/ -Os
+CPPFLAGS += -ffunction-sections -fdata-sections
+CPPFLAGS += -fshort-enums -fpack-struct -funsigned-char -funsigned-bitfields
+CPPFLAGS += -mmcu=$(AVRMCU) -DF_CPU=$(F_CPU)UL -DVERSION=$(VERSION)
+
+LDFLAGS = -mmcu=$(AVRMCU) -Wl,--gc-sections
+
+all: start $(OBJDIR)/$(AVRMCU)/$(TARGET).hex size
+	@echo ":: Done !"
+
+start:
+	@echo "SSS7 AVR port $(VERSION)"
+	@echo "========================"
+	@echo ":: Building for $(AVRMCU)"
+	@echo ":: MCU operating frequency is $(F_CPU)Hz"
+
+
+$(OBJDIR)/$(AVRMCU)/%.o : %.c $(HEADERS) Makefile
+	@mkdir -p $$(dirname $@)
+	$(CC) $(CFLAGS) -c $< -o $@
+
+$(OBJDIR)/$(AVRMCU)/%.o : %.cpp $(HEADERS) Makefile
+	@mkdir -p $$(dirname $@)
+	$(CPP) $(CPPFLAGS) -c $< -o $@
+
+$(OBJDIR)/$(AVRMCU)/sss7core/%.o : ../sss7core/%.c $(HEADERS) Makefile
+	@mkdir -p $$(dirname $@)
+	$(CC) $(CFLAGS) -c $< -o $@
+
+$(OBJDIR)/$(AVRMCU)/arduino/%.o : $(ARDU_FULL_DIR)/%.c $(ARDU_FULL_HEADERS) Makefile
+	@mkdir -p $$(dirname $@)
+	$(CC) $(CFLAGS) -c $< -o $@
+
+$(OBJDIR)/$(AVRMCU)/arduino/%.o : $(ARDU_FULL_DIR)/%.cpp $(ARDU_FULL_HEADERS) Makefile
+	@mkdir -p $$(dirname $@)
+	$(CPP) $(CPPFLAGS) -c $< -o $@
+
+$(OBJDIR)/$(AVRMCU)/arduino/libarduino.a: $(ARDU_OBJS)
+	${AR} crs $@ $+
+
+$(OBJDIR)/$(AVRMCU)/$(TARGET).elf : $(OBJ) $(OBJDIR)/$(AVRMCU)/arduino/libarduino.a
+	$(CC) $(LDFLAGS) $+ -o $@
+
+$(OBJDIR)/$(AVRMCU)/$(TARGET).hex : $(OBJDIR)/$(AVRMCU)/$(TARGET).elf
+	$(OBJCOPY) -O ihex $< $@
+
+size : $(OBJDIR)/$(AVRMCU)/$(TARGET).elf
+	@echo
+	@$(SIZE) --mcu=$(AVRMCU) -C $(OBJDIR)/$(AVRMCU)/$(TARGET).elf
+	@echo
+
+clean :
+	@rm -rf $(OBJDIR)
+
+flash : all
+	avrdude -c stk500v2 \
+		-p $(AVRMCU) -P $(ISPPORT) -D \
+        -U flash:w:$(OBJDIR)/$(AVRMCU)/$(TARGET).hex
+
+test : flash
+	screen $(ISPPORT) 9600
diff --git a/software/Arduino/ardusss7.cpp b/software/Arduino/ardusss7.cpp
new file mode 100644
index 0000000..2fe4b56
--- /dev/null
+++ b/software/Arduino/ardusss7.cpp
@@ -0,0 +1,88 @@
+#include "ardusss7.h"
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+SSS7Wrapper SSS7;
+
+void SSS7Wrapper::init() {
+	sss7_init();
+
+	this->setupUart();
+	this->setupTimer();
+}
+
+uint8_t SSS7Wrapper::canSend() {
+	return sss7_can_send();
+}
+
+void SSS7Wrapper::send(uint8_t msg[SSS7_PAYLOAD_SIZE]) {
+	return sss7_send(msg);
+}
+
+uint8_t SSS7Wrapper::sendFailed() {
+	return sss7_send_failed();
+}
+uint8_t SSS7Wrapper::hasReceived() {
+	return sss7_has_received();
+}
+
+void SSS7Wrapper::getReceived(uint8_t msg[SSS7_PAYLOAD_SIZE]) {
+	sss7_get_received(msg);
+}
+
+
+void SSS7Wrapper::setupUart() {
+	UBRR2H = UBRR_VAL >> 8;		// Setting baudrate
+	UBRR2L = UBRR_VAL & 0xFF;
+
+	UCSR2B = (1 << TXEN2) | (1 << RXEN2); // Enable TX and RX
+	UCSR2C = (1 << UCSZ21) | (1 << UCSZ20);  // Asynchronous 8N1
+
+	// flush UDR
+	do
+	{
+		UDR2;
+	}
+	while (UCSR2A & (1 << RXC2));
+
+	// reset tx and rx complete flags
+	UCSR2A = (1 << RXC2) | (1 << TXC2);
+
+	UCSR2B |= (1 << TXCIE2) | (1 << RXCIE2);  // enable tx and rx interrupts
+}
+
+void uart_put_byte(uint8_t byte) {
+	UDR2 = byte;
+}
+
+uint8_t uart_get_byte() {
+	return UDR2;
+}
+
+ISR(USART2_RX_vect) {
+	sss7_process_rx();
+}
+
+ISR(USART2_TX_vect) {
+	sss7_process_tx();
+}
+
+
+void SSS7Wrapper::setupTimer() {
+	TCCR4B = 0;
+	TCNT4 = 65535 - 16000;	//Preload for 16000 ticks to overflow
+
+	// Take the Timer by force ...
+	TCCR4A = 0;
+	TCCR4B = (1 << CS40);	// Prescaler 1
+	TCCR4C = 0;
+
+	TIMSK4 = (1 << TOIE4);
+}
+
+ISR(TIMER4_OVF_vect) {
+	TCNT4 = 65535 - 16000;	//Preload for 16000 ticks to overflow
+
+	sss7_process_ticks(sss7_timeout_increment);
+}
diff --git a/software/Arduino/ardusss7.h b/software/Arduino/ardusss7.h
new file mode 100644
index 0000000..98007fc
--- /dev/null
+++ b/software/Arduino/ardusss7.h
@@ -0,0 +1,33 @@
+#ifndef _ARDUSSS7_H_
+#define _ARDUSSS7_H_
+
+#include "sss7.h"
+
+class SSS7Wrapper {
+	public:
+		void init();
+		uint8_t canSend();
+		void send(uint8_t msg[SSS7_PAYLOAD_SIZE]);
+		uint8_t sendFailed();
+		uint8_t hasReceived();
+		void getReceived(uint8_t msg[SSS7_PAYLOAD_SIZE]);
+
+	private:
+		void setupUart();
+		void setupTimer();
+};
+
+extern SSS7Wrapper SSS7;
+
+#define BAUD 9600UL
+
+// Some calculations ...
+#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1)   // Rounding magic
+#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1)))     // Real baudrate
+#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD)      // Error in 0.1%
+
+#if ((BAUD_ERROR<950) || (BAUD_ERROR>1050))		// Make sure our UBRR_VAL will work
+  #error Baudrate error is bigger then 1% !
+#endif
+
+#endif
diff --git a/software/Arduino/main.cpp b/software/Arduino/main.cpp
new file mode 100644
index 0000000..c2864c8
--- /dev/null
+++ b/software/Arduino/main.cpp
@@ -0,0 +1,52 @@
+#include <Arduino.h>
+#include "ardusss7.h"
+
+void setup() {
+	Serial.begin(9600);
+
+	Serial.print("Initializing SSS7...");
+	SSS7.init();
+	Serial.println("Done !");
+
+};
+
+
+void loop() {
+	uint8_t msg[SSS7_PAYLOAD_SIZE];
+    memset(msg, 0, SSS7_PAYLOAD_SIZE);
+    msg[0] = 'H';
+    msg[1] = 'e';
+    msg[2] = 'l';
+    msg[3] = 'l';
+    msg[4] = 'o';
+    msg[5] = ' ';
+    msg[6] = 'W';
+    msg[7] = 'o';
+    msg[8] = 'r';
+    msg[9] = 'l';
+    msg[10] = 'd';
+
+
+    while(1) {
+		Serial.println("Waiting to send");
+        while(!SSS7.canSend());
+		Serial.println("Sending ...");
+        SSS7.send(msg);
+        while(!SSS7.canSend());
+		//Serial.println("Finished Sending");
+        if(SSS7.sendFailed()) {
+            Serial.println("Send failed");
+        }
+
+
+		if(SSS7.hasReceived()) {
+			uint8_t msg1[SSS7_PAYLOAD_SIZE];
+			SSS7.getReceived(msg1);
+			Serial.print("Got data:");
+			Serial.println((char*)msg1);
+		}
+
+        _delay_ms(1000);
+    }
+
+};
diff --git a/software/sss7core/sss7.c b/software/sss7core/sss7.c
index c2f971b..5906a7a 100644
--- a/software/sss7core/sss7.c
+++ b/software/sss7core/sss7.c
@@ -4,9 +4,9 @@
 
 volatile enum sss7State sss7_state;
 
-uint8_t sss7_rx_buffer[SSS7_RX_BUFFER_COUNT][SSS7_PAYLOAD_SIZE];
-uint8_t sss7_rx_active_buffer;
-uint8_t sss7_rx_oldest_buffer;
+uint8_t sss7_rx_buffer[SSS7_RX_BUFFER_SIZE][SSS7_PAYLOAD_SIZE];
+uint8_t sss7_rx_buffer_write;
+uint8_t sss7_rx_buffer_read;
 uint8_t sss7_rx_pos;
 
 uint8_t sss7_tx_buffer[SSS7_PAYLOAD_SIZE];
@@ -23,8 +23,8 @@ void sss7_init(void) {
 	sss7_state = SSS7_IDLE;
 
 	sss7_rx_pos = 0;
-	sss7_rx_oldest_buffer = 0;
-	sss7_rx_active_buffer = 0;
+	sss7_rx_buffer_read = 0;
+	sss7_rx_buffer_write = 0;
 
 	sss7_tx_pos = 0;
 	sss7_tx_crc = 0;
@@ -82,6 +82,7 @@ void sss7_send(uint8_t msg[SSS7_PAYLOAD_SIZE]) {
 void sss7_process_rx(void) {
 	uint8_t byte = uart_get_byte();
 	uint8_t crc = 0;
+	uint8_t next_buffer_write = 0;
 	sss7_timeout_counter = 0;
 
 	switch(sss7_state) {
@@ -105,7 +106,7 @@ void sss7_process_rx(void) {
 		break;
 
 		case SSS7_RX_PAYLOAD:
-			sss7_rx_buffer[sss7_rx_active_buffer][sss7_rx_pos] = byte;
+			sss7_rx_buffer[sss7_rx_buffer_write][sss7_rx_pos] = byte;
 			sss7_rx_pos++;
 			if(sss7_rx_pos >= SSS7_PAYLOAD_SIZE) {
 				sss7_state = SSS7_RX_CRC;
@@ -113,9 +114,12 @@ void sss7_process_rx(void) {
 		break;
 
 		case SSS7_RX_CRC:
-			crc = sss7_payload_crc(sss7_rx_buffer[sss7_rx_active_buffer]);
-			if(byte == crc) {
-				sss7_rx_active_buffer = (sss7_rx_active_buffer + 1) % SSS7_RX_BUFFER_COUNT;
+			crc = sss7_payload_crc(sss7_rx_buffer[sss7_rx_buffer_write]);
+
+			next_buffer_write = (sss7_rx_buffer_write + 1) % SSS7_RX_BUFFER_SIZE;
+			// Ensure CRC is okay and we don't overwrite other frames
+			if(byte == crc && next_buffer_write != sss7_rx_buffer_read) {
+				sss7_rx_buffer_write = next_buffer_write;
 			}
 			sss7_state = SSS7_IDLE;
 		break;
@@ -189,7 +193,7 @@ void sss7_process_ticks(uint16_t ticks) {
 
 void sss7_get_received(uint8_t msg[SSS7_PAYLOAD_SIZE]) {
 	if(sss7_has_received()) {
-		memcpy(msg, sss7_rx_buffer[sss7_rx_oldest_buffer], SSS7_PAYLOAD_SIZE);
-		sss7_rx_oldest_buffer = (sss7_rx_oldest_buffer + 1) % SSS7_RX_BUFFER_COUNT;
+		memcpy(msg, sss7_rx_buffer[sss7_rx_buffer_read], SSS7_PAYLOAD_SIZE);
+		sss7_rx_buffer_read = (sss7_rx_buffer_read + 1) % SSS7_RX_BUFFER_SIZE;
 	};
 }
diff --git a/software/sss7core/sss7.h b/software/sss7core/sss7.h
index 7e82f8e..e8e0d20 100644
--- a/software/sss7core/sss7.h
+++ b/software/sss7core/sss7.h
@@ -1,7 +1,12 @@
 #ifndef _SSS7_H_
 #define _SSS7_H_
 
-#include "stdint.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#include <stdint.h>
 
 enum sss7State {
 	SSS7_IDLE,
@@ -19,13 +24,13 @@ const static uint16_t sss7_timeout = 50;
 const static uint16_t sss7_timeout_increment = 1;
 
 #define SSS7_PAYLOAD_SIZE 16
-#define SSS7_RX_BUFFER_COUNT 2
+#define SSS7_RX_BUFFER_SIZE 2
 
 
 extern volatile enum sss7State sss7_state;
 extern volatile uint8_t sss7_tx_failed;
-extern uint8_t sss7_rx_active_buffer;
-extern uint8_t sss7_rx_oldest_buffer;
+extern uint8_t sss7_rx_buffer_write;
+extern uint8_t sss7_rx_buffer_read;
 
 void sss7_process_rx(void);
 void sss7_process_tx(void);
@@ -48,9 +53,13 @@ static inline uint8_t sss7_send_failed(void) {
 }
 
 static inline uint8_t sss7_has_received(void) {
-	return sss7_rx_oldest_buffer < sss7_rx_active_buffer;
+	return sss7_rx_buffer_read != sss7_rx_buffer_write;
 }
 
 void sss7_get_received(uint8_t msg[SSS7_PAYLOAD_SIZE]);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/software/testutils/sniff.py b/software/testutils/sniff.py
index 3fe2b82..7539a44 100644
--- a/software/testutils/sniff.py
+++ b/software/testutils/sniff.py
@@ -28,6 +28,7 @@ def main():
 
         if len(data) != 19:
             print "No Frame:\t" + hexdump(data)
+            continue
 
 
         header_ok = data.startswith(chr(0xAA) + chr(0xFE))