diff --git a/src/SSD1306.cpp b/src/SSD1306.cpp
index d57c370..8f20f1a 100644
--- a/src/SSD1306.cpp
+++ b/src/SSD1306.cpp
@@ -139,7 +139,7 @@ void SSD1306::RefreshScreen()
 
 void SSD1306::RefreshRows(u32 start, u32 amountOfRows)
 {
-	int i;
+	unsigned int i;
 
 	start <<= 1;
 	amountOfRows <<= 1;
@@ -183,7 +183,8 @@ void SSD1306::SetContrast(u8 value)
 	contrast = value;
 	SendCommand(SSD1306_CMD_SET_CONTRAST_CONTROL);
 	SendCommand(value);
-	SetVCOMDeselect( value >> 5);
+	if (type == 1306)
+		SetVCOMDeselect( value >> 8);
 }
 
 void SSD1306::SetVCOMDeselect(u8 value)
diff --git a/src/main.cpp b/src/main.cpp
index d63819e..529aee5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1127,6 +1127,32 @@ void DisplayOptions(int y_pos)
 	screen.PrintText(false, 0, y_pos += 16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
 }
 
+void DisplayI2CScan(int y_pos)
+{
+	int BSCMaster = options.I2CBusMaster();
+
+	snprintf(tempBuffer, tempBufferSize, "Scanning i2c bus %d ...\r\n", BSCMaster);
+	screen.PrintText(false, 0, y_pos , tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
+
+	RPI_I2CInit(BSCMaster, 1);
+
+	int count=0;
+	int ptr = 0;
+	ptr = snprintf (tempBuffer+ptr, tempBufferSize-ptr, "Found ");
+	for (int address = 0; address<128; address++)
+	{
+		if (RPI_I2CScan(BSCMaster, address))
+		{
+			ptr += snprintf (tempBuffer+ptr, tempBufferSize-ptr, "%3d ", address);
+			count++;
+		}
+	}
+	if (count == 0)
+		ptr += snprintf (tempBuffer+ptr, tempBufferSize-ptr, "Nothing");
+
+	screen.PrintText(false, 0, y_pos+16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
+}
+
 static void CheckOptions()
 {
 	FIL fp;
@@ -1253,8 +1279,11 @@ extern "C"
 		snprintf(tempBuffer, tempBufferSize, "This is free software, and you are welcome to redistribute it.");
 		screen.PrintText(false, 0, y_pos+=16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
 
+		if (options.I2CScan())
+			DisplayI2CScan(y_pos+=32);
+
 		if (options.ShowOptions())
-			DisplayOptions(y_pos+32);
+			DisplayOptions(y_pos+=32);
 
 		if (!options.QuickBoot())
 			IEC_Bus::WaitMicroSeconds(3 * 1000000);
diff --git a/src/options.cpp b/src/options.cpp
index 4ebde18..fb4569c 100644
--- a/src/options.cpp
+++ b/src/options.cpp
@@ -142,6 +142,7 @@ Options::Options(void)
 	, screenHeight(768)
 	, i2cBusMaster(1)
 	, i2cLcdAddress(0x3C)
+	, i2cScan(0)
 	, i2cLcdFlip(0)
 	, i2cLcdOnContrast(127)
 	, i2cLcdModel(0)
@@ -216,6 +217,7 @@ void Options::Process(char* buffer)
 		ELSE_CHECK_DECIMAL_OPTION(screenHeight)
 		ELSE_CHECK_DECIMAL_OPTION(i2cBusMaster)
 		ELSE_CHECK_DECIMAL_OPTION(i2cLcdAddress)
+		ELSE_CHECK_DECIMAL_OPTION(i2cScan)
 		ELSE_CHECK_DECIMAL_OPTION(i2cLcdFlip)
 		ELSE_CHECK_DECIMAL_OPTION(i2cLcdOnContrast)
 		ELSE_CHECK_DECIMAL_OPTION(i2cLcdDimContrast)
diff --git a/src/options.h b/src/options.h
index f53d758..b5eb17b 100644
--- a/src/options.h
+++ b/src/options.h
@@ -72,11 +72,13 @@ public:
 
 	inline unsigned int I2CBusMaster() const { return i2cBusMaster; }
 	inline unsigned int I2CLcdAddress() const { return i2cLcdAddress; }
+	inline unsigned int I2CScan() const { return i2cScan; }
 	inline unsigned int I2CLcdFlip() const { return i2cLcdFlip; }
 	inline unsigned int I2CLcdOnContrast() const { return i2cLcdOnContrast; }
 	inline unsigned int I2CLcdDimContrast() const { return i2cLcdDimContrast; }
 	inline unsigned int I2CLcdDimTime() const { return i2cLcdDimTime; }
 	inline unsigned int I2CLcdModel() const { return i2cLcdModel; }
+
 	inline const char* GetLcdLogoName() const { return LcdLogoName; }
 
 	inline float ScrollHighlightRate() const { return scrollHighlightRate; }
@@ -114,6 +116,7 @@ private:
 
 	unsigned int i2cBusMaster;
 	unsigned int i2cLcdAddress;
+	unsigned int i2cScan;
 	unsigned int i2cLcdFlip;
 	unsigned int i2cLcdOnContrast;
 	unsigned int i2cLcdDimContrast;
diff --git a/src/rpi-i2c.c b/src/rpi-i2c.c
index 4072ab4..8630ee7 100644
--- a/src/rpi-i2c.c
+++ b/src/rpi-i2c.c
@@ -184,3 +184,32 @@ int RPI_I2CWrite(int BSCMaster, unsigned char slaveAddress, void* buffer, unsign
 	//DEBUG_LOG("I2C Write %d %d\r\n", count, success);
 	return success;
 }
+
+int RPI_I2CScan(int BSCMaster, unsigned char slaveAddress)
+{
+	int success = 1;
+	if (slaveAddress < 0x80)
+	{
+		unsigned baseAddress = GetBaseAddress(BSCMaster);
+
+		write32(baseAddress + I2C_BSC_A, slaveAddress);
+		write32(baseAddress + I2C_BSC_C, CONTROL_BIT_CLEAR1);
+		write32(baseAddress + I2C_BSC_S, STATUS_BIT_CLKT | STATUS_BIT_ERR | STATUS_BIT_DONE);
+		write32(baseAddress + I2C_BSC_DLEN, 1);
+		write32(baseAddress + I2C_BSC_C, CONTROL_BIT_I2CEN | CONTROL_BIT_ST | CONTROL_BIT_READ);
+
+		while (!(read32(baseAddress + I2C_BSC_S) & STATUS_BIT_DONE))
+		{
+		}
+
+		unsigned status = read32(baseAddress + I2C_BSC_S);
+		if (status & (STATUS_BIT_ERR | STATUS_BIT_CLKT) )
+		{
+			success = 0;
+		}
+
+		write32(baseAddress + I2C_BSC_S, STATUS_BIT_CLKT | STATUS_BIT_ERR | STATUS_BIT_DONE);
+	}
+	return success;
+}
+
diff --git a/src/rpi-i2c.h b/src/rpi-i2c.h
index ac7a81a..0adcc94 100644
--- a/src/rpi-i2c.h
+++ b/src/rpi-i2c.h
@@ -1,5 +1,5 @@
-#ifndef RPI_AUX_H
-#define RPI_AUX_H
+#ifndef RPI_I2C_H
+#define RPI_I2C_H
 
 #include "rpi-base.h"
 
@@ -7,5 +7,6 @@ extern void RPI_I2CInit(int BSCMaster, int fast);
 extern void RPI_I2CSetClock(int BSCMaster, int clock_freq);
 extern int RPI_I2CRead(int BSCMaster, unsigned char slaveAddress, void* buffer, unsigned count);
 extern int RPI_I2CWrite(int BSCMaster, unsigned char slaveAddress, void* buffer, unsigned count);
+extern int RPI_I2CScan(int BSCMaster, unsigned char slaveAddress);
 
 #endif