diff --git a/source/Core/BSP/Pine64/I2C_Wrapper.cpp b/source/Core/BSP/Pine64/I2C_Wrapper.cpp
index f13bc3c1..2f061b98 100644
--- a/source/Core/BSP/Pine64/I2C_Wrapper.cpp
+++ b/source/Core/BSP/Pine64/I2C_Wrapper.cpp
@@ -47,11 +47,181 @@ struct i2c_state {
 	i2c_step currentStep;
 	bool isMemoryWrite;
 	bool wakePart;
+	uint8_t deviceAddress;
+	uint8_t memoryAddress;
+	uint8_t * buffer;
+	uint16_t numberOfBytes;
 	dma_parameter_struct dma_init_struct;
 
 };
 volatile i2c_state currentState;
 
+void perform_i2c_step() {
+	//Performs next step of the i2c state machine
+	if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
+		i2c_flag_clear(I2C0, I2C_FLAG_AERR);
+		//Arb error - we lost the bus / nacked
+		currentState.currentStep = Error_occured;
+	} else if (i2c_flag_get(I2C0, I2C_FLAG_BERR)) {
+		i2c_flag_clear(I2C0, I2C_FLAG_BERR);
+		// Bus Error
+		currentState.currentStep = Error_occured;
+	} else if (i2c_flag_get(I2C0, I2C_FLAG_LOSTARB)) {
+		i2c_flag_clear(I2C0, I2C_FLAG_LOSTARB);
+		// Bus Error
+		currentState.currentStep = Error_occured;
+	} else if (i2c_flag_get(I2C0, I2C_FLAG_PECERR)) {
+		i2c_flag_clear(I2C0, I2C_FLAG_PECERR);
+		// Bus Error
+		currentState.currentStep = Error_occured;
+	}
+	switch (currentState.currentStep) {
+	case Error_occured:
+		i2c_stop_on_bus(I2C0);
+		break;
+	case Write_start:
+
+		/* enable acknowledge */
+		i2c_ack_config(I2C0, I2C_ACK_ENABLE);
+		/* i2c master sends start signal only when the bus is idle */
+		if (!i2c_flag_get(I2C0, I2C_FLAG_I2CBSY)) {
+			/* send the start signal */
+			i2c_start_on_bus(I2C0);
+			currentState.currentStep = Write_device_address;
+		}
+		break;
+
+	case Write_device_address:
+		/* i2c master sends START signal successfully */
+		if (i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) {
+			i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+			i2c_master_addressing(I2C0, currentState.deviceAddress, I2C_TRANSMITTER);
+			currentState.currentStep = Write_device_memory_address;
+		}
+		break;
+	case Write_device_memory_address:
+		//Send the device memory location
+
+		if (i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) { //addr sent
+			i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+
+			if (i2c_flag_get(I2C0, I2C_FLAG_BERR)) {
+				i2c_flag_clear(I2C0, I2C_FLAG_BERR);
+				// Bus Error
+				currentState.currentStep = Error_occured;
+			} else if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
+				i2c_flag_clear(I2C0, I2C_FLAG_AERR);
+				//Arb error - we lost the bus / nacked
+				currentState.currentStep = Error_occured;
+			} else if (currentState.wakePart) {
+				//We are stopping here
+				currentState.currentStep = Send_stop;
+			} else if (i2c_flag_get(I2C0, I2C_FLAG_TBE)) {
+				// Write out the 8 byte address
+				i2c_data_transmit(I2C0, currentState.memoryAddress);
+
+				if (currentState.isMemoryWrite) {
+					currentState.currentStep = Write_device_data_start;
+				} else {
+					currentState.currentStep = Read_start;
+				}
+			}
+		}
+
+		break;
+	case Write_device_data_start:
+
+		/* wait until BTC bit is set */
+		if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
+			/* enable I2C0 DMA */
+			i2c_dma_enable(I2C0, I2C_DMA_ON);
+			/* enable DMA0 channel5 */
+			dma_channel_enable(DMA0, DMA_CH5);
+			currentState.currentStep = Write_device_data_finish;
+		}
+		break;
+
+	case Write_device_data_finish:					//Wait for complete then goto stop
+		/* wait until BTC bit is set */
+		if (dma_flag_get(DMA0, DMA_CH5, DMA_FLAG_FTF)) {
+			/* wait until BTC bit is set */
+			if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
+				currentState.currentStep = Send_stop;
+			}
+		}
+		break;
+	case Read_start:
+		/* wait until BTC bit is set */
+		if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
+			i2c_start_on_bus(I2C0);
+			currentState.currentStep = Read_device_address;
+		}
+		break;
+	case Read_device_address:
+		if (i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) {
+			i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+			i2c_master_addressing(I2C0, currentState.deviceAddress, I2C_RECEIVER);
+			currentState.currentStep = Read_device_data_start;
+		}
+		break;
+	case Read_device_data_start:
+		if (i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) { //addr sent
+			i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+			if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
+				//Arb error - we lost the bus / nacked
+				currentState.currentStep = Error_occured;
+			}
+			/* one byte master reception procedure (polling) */
+			if (currentState.numberOfBytes == 0) {
+				currentState.currentStep = Send_stop;
+			} else if (currentState.numberOfBytes == 1) {
+				/* disable acknowledge */
+				i2c_ack_config(I2C0, I2C_ACK_DISABLE);
+				/* clear ADDSEND register by reading I2C_STAT0 then I2C_STAT1 register
+				 * (I2C_STAT0 has already been read) */
+				i2c_flag_get(I2C0, I2C_FLAG_ADDSEND);					//sat0
+				i2c_flag_get(I2C0, I2C_FLAG_I2CBSY);					//sat1
+				/* send a stop condition to I2C bus*/
+				i2c_stop_on_bus(I2C0);
+				/* wait for the byte to be received */
+				while (!i2c_flag_get(I2C0, I2C_FLAG_RBNE))
+					;
+				/* read the byte received from the EEPROM */
+				*currentState.buffer = i2c_data_receive(I2C0);
+				currentState.currentStep = Wait_stop;
+			} else { /* more than one byte master reception procedure (DMA) */
+				/* enable I2C0 DMA */
+				i2c_dma_enable(I2C0, I2C_DMA_ON);
+				/* enable DMA0 channel5 */
+				dma_channel_enable(DMA0, DMA_CH6);
+				currentState.currentStep = Read_device_data_finish;
+			}
+		}
+		break;
+	case Read_device_data_finish:					//Wait for complete then goto stop
+		/* wait until BTC bit is set */
+		if (dma_flag_get(DMA0, DMA_CH6, DMA_FLAG_FTF)) {
+			currentState.currentStep = Send_stop;
+		}
+
+		break;
+	case Send_stop:
+		/* send a stop condition to I2C bus*/
+		i2c_stop_on_bus(I2C0);
+		currentState.currentStep = Wait_stop;
+		break;
+	case Wait_stop:
+		/* i2c master sends STOP signal successfully */
+		if ((I2C_CTL0(I2C0) & 0x0200) != 0x0200) {
+			currentState.currentStep = Done;
+		}
+		break;
+	default:
+		//If we get here something is amiss
+		return;
+	}
+}
+
 bool perform_i2c_transaction(uint16_t DevAddress, uint16_t memory_address, uint8_t *p_buffer, uint16_t number_of_byte, bool isWrite, bool isWakeOnly) {
 	{
 		//TODO is this required
@@ -66,6 +236,10 @@ bool perform_i2c_transaction(uint16_t DevAddress, uint16_t memory_address, uint8
 
 	currentState.isMemoryWrite = isWrite;
 	currentState.wakePart = isWakeOnly;
+	currentState.deviceAddress = DevAddress;
+	currentState.memoryAddress = memory_address;
+	currentState.numberOfBytes = number_of_byte;
+	currentState.buffer = p_buffer;
 	if (!isWakeOnly) {
 		//Setup DMA
 		currentState.dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
@@ -102,170 +276,7 @@ bool perform_i2c_transaction(uint16_t DevAddress, uint16_t memory_address, uint8
 			i2c_stop_on_bus(I2C0);
 			return false;
 		}
-//		if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
-//			i2c_flag_clear(I2C0, I2C_FLAG_AERR);
-//			//Arb error - we lost the bus / nacked
-//			currentState.currentStep = Error_occured;
-//		} else if (i2c_flag_get(I2C0, I2C_FLAG_BERR)) {
-//			i2c_flag_clear(I2C0, I2C_FLAG_BERR);
-//			// Bus Error
-//			currentState.currentStep = Error_occured;
-//		} else if (i2c_flag_get(I2C0, I2C_FLAG_LOSTARB)) {
-//			i2c_flag_clear(I2C0, I2C_FLAG_LOSTARB);
-//			// Bus Error
-//			currentState.currentStep = Error_occured;
-//		} else if (i2c_flag_get(I2C0, I2C_FLAG_PECERR)) {
-//			i2c_flag_clear(I2C0, I2C_FLAG_PECERR);
-//			// Bus Error
-//			currentState.currentStep = Error_occured;
-//		}
-		switch (currentState.currentStep) {
-		case Error_occured:
-
-			i2c_stop_on_bus(I2C0);
-			return false;
-			break;
-		case Write_start:
-
-			/* enable acknowledge */
-			i2c_ack_config(I2C0, I2C_ACK_ENABLE);
-			/* i2c master sends start signal only when the bus is idle */
-			if (!i2c_flag_get(I2C0, I2C_FLAG_I2CBSY)) {
-				/* send the start signal */
-				i2c_start_on_bus(I2C0);
-				currentState.currentStep = Write_device_address;
-			}
-			break;
-
-		case Write_device_address:
-			/* i2c master sends START signal successfully */
-			if (i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) {
-				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-				i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
-				currentState.currentStep = Write_device_memory_address;
-			}
-			break;
-		case Write_device_memory_address:
-			//Send the device memory location
-
-			if (i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) { //addr sent
-				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-
-				if (i2c_flag_get(I2C0, I2C_FLAG_BERR)) {
-					i2c_flag_clear(I2C0, I2C_FLAG_BERR);
-					// Bus Error
-					currentState.currentStep = Error_occured;
-				} else if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
-					i2c_flag_clear(I2C0, I2C_FLAG_AERR);
-					//Arb error - we lost the bus / nacked
-					currentState.currentStep = Error_occured;
-				} else if (currentState.wakePart) {
-					//We are stopping here
-					currentState.currentStep = Send_stop;
-				} else if (i2c_flag_get(I2C0, I2C_FLAG_TBE)) {
-					// Write out the 8 byte address
-					i2c_data_transmit(I2C0, memory_address);
-
-					if (currentState.isMemoryWrite) {
-						currentState.currentStep = Write_device_data_start;
-					} else {
-						currentState.currentStep = Read_start;
-					}
-				}
-			}
-
-			break;
-		case Write_device_data_start:
-
-			/* wait until BTC bit is set */
-			if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
-				/* enable I2C0 DMA */
-				i2c_dma_enable(I2C0, I2C_DMA_ON);
-				/* enable DMA0 channel5 */
-				dma_channel_enable(DMA0, DMA_CH5);
-				currentState.currentStep = Write_device_data_finish;
-			}
-			break;
-
-		case Write_device_data_finish:					//Wait for complete then goto stop
-			/* wait until BTC bit is set */
-			if (dma_flag_get(DMA0, DMA_CH5, DMA_FLAG_FTF)) {
-				/* wait until BTC bit is set */
-				if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
-					currentState.currentStep = Send_stop;
-				}
-			}
-			break;
-		case Read_start:
-			/* wait until BTC bit is set */
-			if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
-				i2c_start_on_bus(I2C0);
-				currentState.currentStep = Read_device_address;
-			}
-			break;
-		case Read_device_address:
-			if (i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) {
-				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-				i2c_master_addressing(I2C0, DevAddress, I2C_RECEIVER);
-				currentState.currentStep = Read_device_data_start;
-			}
-			break;
-		case Read_device_data_start:
-			if (i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) { //addr sent
-				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-				if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
-					//Arb error - we lost the bus / nacked
-					currentState.currentStep = Error_occured;
-				}
-				/* one byte master reception procedure (polling) */
-				if (number_of_byte == 0) {
-					currentState.currentStep = Send_stop;
-				} else if (number_of_byte == 1) {
-					/* disable acknowledge */
-					i2c_ack_config(I2C0, I2C_ACK_DISABLE);
-					/* clear ADDSEND register by reading I2C_STAT0 then I2C_STAT1 register
-					 * (I2C_STAT0 has already been read) */
-					i2c_flag_get(I2C0, I2C_FLAG_ADDSEND);					//sat0
-					i2c_flag_get(I2C0, I2C_FLAG_I2CBSY);					//sat1
-					/* send a stop condition to I2C bus*/
-					i2c_stop_on_bus(I2C0);
-					/* wait for the byte to be received */
-					while (!i2c_flag_get(I2C0, I2C_FLAG_RBNE))
-						;
-					/* read the byte received from the EEPROM */
-					*p_buffer = i2c_data_receive(I2C0);
-					currentState.currentStep = Wait_stop;
-				} else { /* more than one byte master reception procedure (DMA) */
-					/* enable I2C0 DMA */
-					i2c_dma_enable(I2C0, I2C_DMA_ON);
-					/* enable DMA0 channel5 */
-					dma_channel_enable(DMA0, DMA_CH6);
-					currentState.currentStep = Read_device_data_finish;
-				}
-			}
-			break;
-		case Read_device_data_finish:					//Wait for complete then goto stop
-			/* wait until BTC bit is set */
-			if (dma_flag_get(DMA0, DMA_CH6, DMA_FLAG_FTF)) {
-				currentState.currentStep = Send_stop;
-			}
-
-			break;
-		case Send_stop:
-			/* send a stop condition to I2C bus*/
-			i2c_stop_on_bus(I2C0);
-			currentState.currentStep = Wait_stop;
-			break;
-		case Wait_stop:
-			/* i2c master sends STOP signal successfully */
-			if ((I2C_CTL0(I2C0) & 0x0200) != 0x0200) {
-				currentState.currentStep = Done;
-			}
-			break;
-		default:
-			//If we get here something is amiss
-			return false;
-		}
+		perform_i2c_step();
 	}
 	return true;
 }