diff --git a/firmware/libboard/octsimtest/source/mcp23017.c b/firmware/libboard/octsimtest/source/mcp23017.c
new file mode 100644
index 0000000..63390b4
--- /dev/null
+++ b/firmware/libboard/octsimtest/source/mcp23017.c
@@ -0,0 +1,106 @@
+#include "board.h"
+#include <stdbool.h>
+#include "i2c.h"
+#include "mcp23017.h"
+
+
+//defines from https://github.com/adafruit/Adafruit-MCP23017-Arduino-Library/blob/master/Adafruit_MCP23017.h under BSD license
+
+// registers
+#define MCP23017_IODIRA 0x00
+#define MCP23017_IPOLA 0x02
+#define MCP23017_GPINTENA 0x04
+#define MCP23017_DEFVALA 0x06
+#define MCP23017_INTCONA 0x08
+#define MCP23017_IOCONA 0x0A
+#define MCP23017_GPPUA 0x0C
+#define MCP23017_INTFA 0x0E
+#define MCP23017_INTCAPA 0x10
+#define MCP23017_GPIOA 0x12
+#define MCP23017_OLATA 0x14
+
+
+#define MCP23017_IODIRB 0x01
+#define MCP23017_IPOLB 0x03
+#define MCP23017_GPINTENB 0x05
+#define MCP23017_DEFVALB 0x07
+#define MCP23017_INTCONB 0x09
+#define MCP23017_IOCONB 0x0B
+#define MCP23017_GPPUB 0x0D
+#define MCP23017_INTFB 0x0F
+#define MCP23017_INTCAPB 0x11
+#define MCP23017_GPIOB 0x13
+#define MCP23017_OLATB 0x15
+
+#define MCP23017_INT_ERR 255
+
+
+//bool i2c_write_byte(bool send_start, bool send_stop, uint8_t byte)
+//uint8_t i2c_read_byte(bool nack, bool send_stop)
+//static void i2c_stop_cond(void)
+
+int mcp23017_write_byte(uint8_t slave, uint8_t addr, uint8_t byte)
+{
+	bool nack;
+
+	WDT_Restart(WDT);
+
+//	 Write slave address 
+	nack = i2c_write_byte(true, false, slave << 1);
+	if (nack)
+		goto out_stop;
+	nack = i2c_write_byte(false, false, addr);
+	if (nack)
+		goto out_stop;
+	nack = i2c_write_byte(false, true, byte);
+	if (nack)
+		goto out_stop;
+
+out_stop:
+	i2c_stop_cond();
+	if (nack)
+		return -1;
+	else
+		return 0;
+}
+
+int mcp23017_read_byte(uint8_t slave, uint8_t addr)
+{
+	bool nack;
+
+	WDT_Restart(WDT);
+
+	// dummy write cycle
+	nack = i2c_write_byte(true, false, slave << 1);
+	if (nack)
+		goto out_stop;
+	nack = i2c_write_byte(false, false, addr);
+	if (nack)
+		goto out_stop;
+	// Re-start with read
+	nack = i2c_write_byte(true, false, (slave << 1) | 1);
+	if (nack)
+		goto out_stop;
+
+	return i2c_read_byte(true, true);
+
+out_stop:
+	i2c_stop_cond();
+	if (nack)
+		return -1;
+	else
+		return 0;
+}
+
+int mcp23017_init(uint8_t slave)
+{
+	printf("mcp23017_init\n");
+	// all gpio input
+	if (mcp23017_write_byte(slave, MCP23017_IODIRA, 0xff))
+		return false;
+	if (mcp23017_write_byte(slave, MCP23017_IODIRB, 0xff))
+		return false;
+	printf("mcp23017 found\n");
+	return true;
+}
+