diff --git a/workspace/TS100/Core/BSP/Miniware/I2CBB.cpp b/workspace/TS100/Core/BSP/Miniware/I2CBB.cpp
index 1c561c77..ef19db03 100644
--- a/workspace/TS100/Core/BSP/Miniware/I2CBB.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/I2CBB.cpp
@@ -6,14 +6,16 @@
  */
 
 #include <I2CBB.hpp>
-
+#include "FreeRTOS.h"
 #define SCL_HIGH() 	HAL_GPIO_WritePin(SCL2_GPIO_Port, SCL2_Pin, GPIO_PIN_SET)
 #define SCL_LOW() 	HAL_GPIO_WritePin(SCL2_GPIO_Port, SCL2_Pin, GPIO_PIN_RESET)
 #define SDA_HIGH() 	HAL_GPIO_WritePin(SDA2_GPIO_Port, SDA2_Pin, GPIO_PIN_SET)
 #define SDA_LOW() 	HAL_GPIO_WritePin(SDA2_GPIO_Port, SDA2_Pin, GPIO_PIN_RESET)
 #define SDA_READ()  (HAL_GPIO_ReadPin(SDA2_GPIO_Port,SDA2_Pin)==GPIO_PIN_SET?1:0)
 #define SCL_READ()  (HAL_GPIO_ReadPin(SCL2_GPIO_Port,SCL2_Pin)==GPIO_PIN_SET?1:0)
-#define I2C_DELAY() {for(int xx=0;xx<100;xx++){asm("nop");}}
+#define I2C_DELAY() {for(int xx=0;xx<700;xx++){asm("nop");}}
+SemaphoreHandle_t I2CBB::I2CSemaphore = NULL;
+StaticSemaphore_t I2CBB::xSemaphoreBuffer;
 void I2CBB::init() {
 	//Set GPIO's to output open drain
 	GPIO_InitTypeDef GPIO_InitStruct;
@@ -25,6 +27,9 @@ void I2CBB::init() {
 	HAL_GPIO_Init(SDA2_GPIO_Port, &GPIO_InitStruct);
 	SDA_HIGH();
 	SCL_HIGH();
+	I2CSemaphore = xSemaphoreCreateBinaryStatic(&xSemaphoreBuffer);
+	xSemaphoreGive(I2CSemaphore);
+	unlock();
 }
 
 bool I2CBB::probe(uint8_t address) {
@@ -36,15 +41,26 @@ bool I2CBB::probe(uint8_t address) {
 
 bool I2CBB::Mem_Read(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData,
 		uint16_t Size) {
+	if (!lock())
+		return false;
 	start();
-	bool ack = send(DevAddress | 1);
+	bool ack = send(DevAddress);
 	if (!ack) {
 		stop();
+		unlock();
 		return false;
 	}
 	ack = send(MemAddress);
 	if (!ack) {
 		stop();
+		unlock();
+		return false;
+	}
+	start();
+	ack = send(DevAddress | 1);
+	if (!ack) {
+		stop();
+		unlock();
 		return false;
 	}
 	while (Size) {
@@ -53,20 +69,27 @@ bool I2CBB::Mem_Read(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData,
 		Size--;
 	}
 	stop();
+	unlock();
 	return true;
 }
 
 bool I2CBB::Mem_Write(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData,
 		uint16_t Size) {
+	if (!lock())
+		return false;
 	start();
 	bool ack = send(DevAddress);
 	if (!ack) {
 		stop();
+		asm("bkpt");
+		unlock();
 		return false;
 	}
 	ack = send(MemAddress);
 	if (!ack) {
 		stop();
+		asm("bkpt");
+		unlock();
 		return false;
 	}
 	while (Size) {
@@ -74,40 +97,51 @@ bool I2CBB::Mem_Write(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData,
 		bool ack = send(pData[0]);
 		if (!ack) {
 			stop();
+			asm("bkpt");
+			unlock();
 			return false;
 		}
 		pData++;
 		Size--;
 	}
 	stop();
+	unlock();
 	return true;
 }
 
 void I2CBB::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
+	if (!lock())
+		return;
 	start();
 	bool ack = send(DevAddress);
 	if (!ack) {
 		stop();
+		unlock();
 		return;
 	}
 	while (Size) {
 		bool ack = send(pData[0]);
 		if (!ack) {
 			stop();
+			unlock();
 			return;
 		}
 		pData++;
 		Size--;
 	}
 	stop();
+	unlock();
 
 }
 
 void I2CBB::Receive(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
+	if (!lock())
+		return;
 	start();
 	bool ack = send(DevAddress | 1);
 	if (!ack) {
 		stop();
+		unlock();
 		return;
 	}
 	while (Size) {
@@ -116,23 +150,28 @@ void I2CBB::Receive(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
 		Size--;
 	}
 	stop();
+	unlock();
 }
 
 void I2CBB::TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx,
 		uint16_t Size_tx, uint8_t *pData_rx, uint16_t Size_rx) {
 	if (Size_tx == 0 && Size_rx == 0)
 		return;
+	if (lock() == false)
+		return;
 	if (Size_tx) {
 		start();
 		bool ack = send(DevAddress);
 		if (!ack) {
 			stop();
+			unlock();
 			return;
 		}
 		while (Size_tx) {
 			bool ack = send(pData_tx[0]);
 			if (!ack) {
 				stop();
+				unlock();
 				return;
 			}
 			pData_tx++;
@@ -144,6 +183,7 @@ void I2CBB::TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx,
 		bool ack = send(DevAddress | 1);
 		if (!ack) {
 			stop();
+			unlock();
 			return;
 		}
 		while (Size_rx) {
@@ -153,6 +193,7 @@ void I2CBB::TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx,
 		}
 	}
 	stop();
+	unlock();
 }
 
 void I2CBB::start() {
@@ -220,6 +261,17 @@ uint8_t I2CBB::read_bit() {
 	return b;
 }
 
+void I2CBB::unlock() {
+	xSemaphoreGive(I2CSemaphore);
+}
+
+bool I2CBB::lock() {
+	if (I2CSemaphore == NULL) {
+		asm("bkpt");
+	}
+	return xSemaphoreTake(I2CSemaphore, (TickType_t) 50) == pdTRUE;
+}
+
 void I2CBB::write_bit(uint8_t val) {
 	if (val > 0)
 		SDA_HIGH();
diff --git a/workspace/TS100/Core/BSP/Miniware/I2CBB.hpp b/workspace/TS100/Core/BSP/Miniware/I2CBB.hpp
index 5be764d8..8c67cd19 100644
--- a/workspace/TS100/Core/BSP/Miniware/I2CBB.hpp
+++ b/workspace/TS100/Core/BSP/Miniware/I2CBB.hpp
@@ -10,6 +10,8 @@
 #include "BSP.h"
 #include "Setup.h"
 #include "Pins.h"
+#include "FreeRTOS.h"
+#include "semphr.h"
 /*
  * Simple static I2C bit-bang class used on the TS80P
  * SCL = PA5
@@ -31,6 +33,10 @@ public:
 	static void TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx,
 			uint16_t Size_tx, uint8_t *pData_rx, uint16_t Size_rx);
 private:
+	static SemaphoreHandle_t I2CSemaphore;
+	static StaticSemaphore_t xSemaphoreBuffer;
+	static void unlock();
+	static bool lock();
 	static void start();
 	static void stop();
 	static bool send(uint8_t value);