diff --git a/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp b/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp
index 0181a019..db011a89 100755
--- a/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp
+++ b/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp
@@ -12,521 +12,530 @@ SemaphoreHandle_t FRToSI2C::I2CSemaphore = nullptr;
 StaticSemaphore_t FRToSI2C::xSemaphoreBuffer;
 #define I2C_TIME_OUT (uint16_t)(12000)
 void FRToSI2C::CpltCallback() {
-  // TODO
+	// TODO
 }
 
 bool FRToSI2C::I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data) {
-  return Mem_Write(address, reg, &data, 1);
+	return Mem_Write(address, reg, &data, 1);
 }
 
 uint8_t FRToSI2C::I2C_RegisterRead(uint8_t add, uint8_t reg) {
-  uint8_t temp = 0;
-  Mem_Read(add, reg, &temp, 1);
-  return temp;
+	uint8_t temp = 0;
+	Mem_Read(add, reg, &temp, 1);
+	return temp;
 }
 
 bool FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t read_address,
-                        uint8_t *p_buffer, uint16_t number_of_byte) {
-  if (!lock())
-    return false;
-  i2c_interrupt_disable(I2C0, I2C_INT_ERR);
-  i2c_interrupt_disable(I2C0, I2C_INT_BUF);
-  i2c_interrupt_disable(I2C0, I2C_INT_EV);
-  dma_parameter_struct dma_init_struct;
+		uint8_t *p_buffer, uint16_t number_of_byte) {
+	if (!lock())
+		return false;
+	i2c_interrupt_disable(I2C0, I2C_INT_ERR);
+	i2c_interrupt_disable(I2C0, I2C_INT_BUF);
+	i2c_interrupt_disable(I2C0, I2C_INT_EV);
+	dma_parameter_struct dma_init_struct;
 
-  uint8_t state = I2C_START;
-  uint8_t in_rx_cycle = 0;
-  uint16_t timeout = 0;
-  uint8_t tries = 0;
-  uint8_t i2c_timeout_flag = 0;
-  while (!(i2c_timeout_flag)) {
-    switch (state) {
-    case I2C_START:
-      tries++;
-      if (tries > 64) {
-        i2c_stop_on_bus(I2C0);
-        /* i2c master sends STOP signal successfully */
-        while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        unlock();
-        return false;
-      }
-      if (0 == in_rx_cycle) {
-        /* disable I2C0 */
-        i2c_disable(I2C0);
-        /* enable I2C0 */
-        i2c_enable(I2C0);
+	uint8_t state = I2C_START;
+	uint8_t in_rx_cycle = 0;
+	uint16_t timeout = 0;
+	uint8_t tries = 0;
+	uint8_t i2c_timeout_flag = 0;
+	while (!(i2c_timeout_flag)) {
+		switch (state) {
+		case I2C_START:
+			tries++;
+			if (tries > 64) {
+				i2c_stop_on_bus(I2C0);
+				/* i2c master sends STOP signal successfully */
+				while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				unlock();
+				return false;
+			}
+			if (0 == in_rx_cycle) {
+				/* disable I2C0 */
+				i2c_disable(I2C0);
+				/* enable I2C0 */
+				i2c_enable(I2C0);
 
-        /* enable acknowledge */
-        i2c_ack_config(I2C0, I2C_ACK_ENABLE);
-        /* i2c master sends start signal only when the bus is idle */
-        while (i2c_flag_get(I2C0, I2C_FLAG_I2CBSY) &&
-               (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        if (timeout < I2C_TIME_OUT) {
-          /* send the start signal */
-          i2c_start_on_bus(I2C0);
-          timeout = 0;
-          state = I2C_SEND_ADDRESS;
-        } else {
-          I2C_Unstick();
-          timeout = 0;
-          state = I2C_START;
-        }
-      } else {
-        i2c_start_on_bus(I2C0);
-        timeout = 0;
-        state = I2C_SEND_ADDRESS;
-      }
-      break;
-    case I2C_SEND_ADDRESS:
-      /* i2c master sends START signal successfully */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) &&
-             (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        if (RESET == in_rx_cycle) {
-          i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
-          state = I2C_CLEAR_ADDRESS_FLAG;
-        } else {
-          i2c_master_addressing(I2C0, DevAddress, I2C_RECEIVER);
-          state = I2C_CLEAR_ADDRESS_FLAG;
-        }
-        timeout = 0;
-      } else {
-        timeout = 0;
-        state = I2C_START;
-        in_rx_cycle = 0;
-      }
-      break;
-    case I2C_CLEAR_ADDRESS_FLAG:
-      /* address flag set means i2c slave sends ACK */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) &&
-             (timeout < I2C_TIME_OUT)) {
-        timeout++;
-        if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
-          i2c_flag_clear(I2C0, I2C_FLAG_AERR);
-          i2c_stop_on_bus(I2C0);
-          /* i2c master sends STOP signal successfully */
-          while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-            timeout++;
-          }
-          // Address NACK'd
-          unlock();
-          return false;
-        }
-      }
-      if (timeout < I2C_TIME_OUT) {
-        i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-        timeout = 0;
-        state = I2C_TRANSMIT_DATA;
-      } else {
-        i2c_stop_on_bus(I2C0);
-        /* i2c master sends STOP signal successfully */
-        while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        // Address NACK'd
-        unlock();
-        return false;
-      }
-      break;
-    case I2C_TRANSMIT_DATA:
-      if (0 == in_rx_cycle) {
-        /* wait until the transmit data buffer is empty */
-        while ((!i2c_flag_get(I2C0, I2C_FLAG_TBE)) &&
-               (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        if (timeout < I2C_TIME_OUT) {
-          // Write out the 8 byte address
-          i2c_data_transmit(I2C0, read_address);
-          timeout = 0;
-        } else {
-          timeout = 0;
-          state = I2C_START;
-          in_rx_cycle = 0;
-        }
-        /* wait until BTC bit is set */
-        while ((!i2c_flag_get(I2C0, I2C_FLAG_BTC)) &&
-               (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        if (timeout < I2C_TIME_OUT) {
-          timeout = 0;
-          state = I2C_START;
-          in_rx_cycle = 1;
-        } else {
-          timeout = 0;
-          state = I2C_START;
-          in_rx_cycle = 0;
-        }
-      } else {
-        /* one byte master reception procedure (polling) */
-        if (number_of_byte < 2) {
-          /* 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);
-          /* 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);
-          /* decrement the read bytes counter */
-          number_of_byte--;
-          timeout = 0;
-        } else { /* more than one byte master reception procedure (DMA) */
-          dma_deinit(DMA0, DMA_CH6);
-          dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
-          dma_init_struct.memory_addr = (uint32_t)p_buffer;
-          dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
-          dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
-          dma_init_struct.number = number_of_byte;
-          dma_init_struct.periph_addr = (uint32_t)&I2C_DATA(I2C0);
-          dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
-          dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
-          dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH;
-          dma_init(DMA0, DMA_CH6, &dma_init_struct);
+				/* enable acknowledge */
+				i2c_ack_config(I2C0, I2C_ACK_ENABLE);
+				/* i2c master sends start signal only when the bus is idle */
+				while (i2c_flag_get(I2C0, I2C_FLAG_I2CBSY)
+						&& (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				if (timeout < I2C_TIME_OUT) {
+					/* send the start signal */
+					i2c_start_on_bus(I2C0);
+					timeout = 0;
+					state = I2C_SEND_ADDRESS;
+				} else {
+					I2C_Unstick();
+					timeout = 0;
+					state = I2C_START;
+				}
+			} else {
+				i2c_start_on_bus(I2C0);
+				timeout = 0;
+				state = I2C_SEND_ADDRESS;
+			}
+			break;
+		case I2C_SEND_ADDRESS:
+			/* i2c master sends START signal successfully */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_SBSEND))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				if (RESET == in_rx_cycle) {
+					i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
+					state = I2C_CLEAR_ADDRESS_FLAG;
+				} else {
+					i2c_master_addressing(I2C0, DevAddress, I2C_RECEIVER);
+					state = I2C_CLEAR_ADDRESS_FLAG;
+				}
+				timeout = 0;
+			} else {
+				timeout = 0;
+				state = I2C_START;
+				in_rx_cycle = 0;
+			}
+			break;
+		case I2C_CLEAR_ADDRESS_FLAG:
+			/* address flag set means i2c slave sends ACK */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+				if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
+					i2c_flag_clear(I2C0, I2C_FLAG_AERR);
+					i2c_stop_on_bus(I2C0);
+					/* i2c master sends STOP signal successfully */
+					while ((I2C_CTL0(I2C0) & 0x0200)
+							&& (timeout < I2C_TIME_OUT )) {
+						timeout++;
+					}
+					// Address NACK'd
+					unlock();
+					return false;
+				}
+			}
+			if (timeout < I2C_TIME_OUT) {
+				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+				timeout = 0;
+				state = I2C_TRANSMIT_DATA;
+			} else {
+				i2c_stop_on_bus(I2C0);
+				/* i2c master sends STOP signal successfully */
+				while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				// Address NACK'd
+				unlock();
+				return false;
+			}
+			break;
+		case I2C_TRANSMIT_DATA:
+			if (0 == in_rx_cycle) {
+				/* wait until the transmit data buffer is empty */
+				while ((!i2c_flag_get(I2C0, I2C_FLAG_TBE))
+						&& (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				if (timeout < I2C_TIME_OUT) {
+					// Write out the 8 byte address
+					i2c_data_transmit(I2C0, read_address);
+					timeout = 0;
+				} else {
+					timeout = 0;
+					state = I2C_START;
+					in_rx_cycle = 0;
+				}
+				/* wait until BTC bit is set */
+				while ((!i2c_flag_get(I2C0, I2C_FLAG_BTC))
+						&& (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				if (timeout < I2C_TIME_OUT) {
+					timeout = 0;
+					state = I2C_START;
+					in_rx_cycle = 1;
+				} else {
+					timeout = 0;
+					state = I2C_START;
+					in_rx_cycle = 0;
+				}
+			} else {
+				/* one byte master reception procedure (polling) */
+				if (number_of_byte < 2) {
+					/* 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);
+					/* 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);
+					/* decrement the read bytes counter */
+					number_of_byte--;
+					timeout = 0;
+				} else { /* more than one byte master reception procedure (DMA) */
+					dma_deinit(DMA0, DMA_CH6);
+					dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
+					dma_init_struct.memory_addr = (uint32_t) p_buffer;
+					dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+					dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
+					dma_init_struct.number = number_of_byte;
+					dma_init_struct.periph_addr = (uint32_t) &I2C_DATA(I2C0);
+					dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+					dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
+					dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH;
+					dma_init(DMA0, DMA_CH6, &dma_init_struct);
 
-          i2c_dma_last_transfer_config(I2C0, I2C_DMALST_ON);
-          /* enable I2C0 DMA */
-          i2c_dma_enable(I2C0, I2C_DMA_ON);
-          /* enable DMA0 channel5 */
-          dma_channel_enable(DMA0, DMA_CH6);
-          /* wait until BTC bit is set */
-          while (!dma_flag_get(DMA0, DMA_CH6, DMA_FLAG_FTF)) {
-            osDelay(1);
-          }
-          /* send a stop condition to I2C bus*/
-          i2c_stop_on_bus(I2C0);
-        }
-        timeout = 0;
-        state = I2C_STOP;
-      }
-      break;
-    case I2C_STOP:
-      /* i2c master sends STOP signal successfully */
-      while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        timeout = 0;
-        state = I2C_END;
-        i2c_timeout_flag = I2C_OK;
-      } else {
-        timeout = 0;
-        state = I2C_START;
-        in_rx_cycle = 0;
-      }
-      break;
-    default:
-      state = I2C_START;
-      in_rx_cycle = 0;
-      i2c_timeout_flag = I2C_OK;
-      timeout = 0;
-      break;
-    }
-  }
-  unlock();
-  return true;
+					i2c_dma_last_transfer_config(I2C0, I2C_DMALST_ON);
+					/* enable I2C0 DMA */
+					i2c_dma_enable(I2C0, I2C_DMA_ON);
+					/* enable DMA0 channel5 */
+					dma_channel_enable(DMA0, DMA_CH6);
+					/* wait until BTC bit is set */
+					while (!dma_flag_get(DMA0, DMA_CH6, DMA_FLAG_FTF)) {
+
+					}
+					/* send a stop condition to I2C bus*/
+					i2c_stop_on_bus(I2C0);
+				}
+				timeout = 0;
+				state = I2C_STOP;
+			}
+			break;
+		case I2C_STOP:
+			/* i2c master sends STOP signal successfully */
+			while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				timeout = 0;
+				state = I2C_END;
+				i2c_timeout_flag = I2C_OK;
+			} else {
+				timeout = 0;
+				state = I2C_START;
+				in_rx_cycle = 0;
+			}
+			break;
+		default:
+			state = I2C_START;
+			in_rx_cycle = 0;
+			i2c_timeout_flag = I2C_OK;
+			timeout = 0;
+			break;
+		}
+	}
+	unlock();
+	return true;
 }
 
 bool FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
-                         uint8_t *p_buffer, uint16_t number_of_byte) {
-  if (!lock())
-    return false;
+		uint8_t *p_buffer, uint16_t number_of_byte) {
+	if (!lock())
+		return false;
 
-  i2c_interrupt_disable(I2C0, I2C_INT_ERR);
-  i2c_interrupt_disable(I2C0, I2C_INT_EV);
-  i2c_interrupt_disable(I2C0, I2C_INT_BUF);
-  dma_parameter_struct dma_init_struct;
+	i2c_interrupt_disable(I2C0, I2C_INT_ERR);
+	i2c_interrupt_disable(I2C0, I2C_INT_EV);
+	i2c_interrupt_disable(I2C0, I2C_INT_BUF);
+	dma_parameter_struct dma_init_struct;
 
-  uint8_t state = I2C_START;
-  uint16_t timeout = 0;
-  bool done = false;
-  bool timedout = false;
-  while (!(done || timedout)) {
-    switch (state) {
-    case I2C_START:
-      /* i2c master sends start signal only when the bus is idle */
-      while (i2c_flag_get(I2C0, I2C_FLAG_I2CBSY) && (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        i2c_start_on_bus(I2C0);
-        timeout = 0;
-        state = I2C_SEND_ADDRESS;
-      } else {
-        I2C_Unstick();
-        timeout = 0;
-        state = I2C_START;
-      }
-      break;
-    case I2C_SEND_ADDRESS:
-      /* i2c master sends START signal successfully */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) &&
-             (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
-        timeout = 0;
-        state = I2C_CLEAR_ADDRESS_FLAG;
-      } else {
-        timedout = true;
-        done = true;
-        timeout = 0;
-        state = I2C_START;
-      }
-      break;
-    case I2C_CLEAR_ADDRESS_FLAG:
-      /* address flag set means i2c slave sends ACK */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) &&
-             (timeout < I2C_TIME_OUT)) {
-        timeout++;
-        if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
-          i2c_flag_clear(I2C0, I2C_FLAG_AERR);
-          i2c_stop_on_bus(I2C0);
-          /* i2c master sends STOP signal successfully */
-          while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-            timeout++;
-          }
-          // Address NACK'd
-          unlock();
-          return false;
-        }
-      }
-      timeout = 0;
-      if (timeout < I2C_TIME_OUT) {
-        i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-        state = I2C_TRANSMIT_DATA;
-      } else {
-        // Dont retry as this means a NAK
-        i2c_stop_on_bus(I2C0);
-        /* i2c master sends STOP signal successfully */
-        while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        unlock();
-        return false;
-      }
-      break;
-    case I2C_TRANSMIT_DATA:
-      /* wait until the transmit data buffer is empty */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_TBE)) && (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        /* send the EEPROM's internal address to write to : only one byte
-         * address */
-        i2c_data_transmit(I2C0, MemAddress);
-        timeout = 0;
-      } else {
-        timedout = true;
-        timeout = 0;
-        state = I2C_START;
-      }
-      /* wait until BTC bit is set */
-      while (!i2c_flag_get(I2C0, I2C_FLAG_BTC))
-        ;
-      dma_deinit(DMA0, DMA_CH5);
-      dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
-      dma_init_struct.memory_addr = (uint32_t)p_buffer;
-      dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
-      dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
-      dma_init_struct.number = number_of_byte;
-      dma_init_struct.periph_addr = (uint32_t)&I2C_DATA(I2C0);
-      dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
-      dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
-      dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH;
-      dma_init(DMA0, DMA_CH5, &dma_init_struct);
-      /* enable I2C0 DMA */
-      i2c_dma_enable(I2C0, I2C_DMA_ON);
-      /* enable DMA0 channel5 */
-      dma_channel_enable(DMA0, DMA_CH5);
-      /* wait until BTC bit is set */
-      while (!dma_flag_get(DMA0, DMA_CH5, DMA_FLAG_FTF)) {
-        osDelay(2);
-      }
-      /* wait until BTC bit is set */
-      while (!i2c_flag_get(I2C0, I2C_FLAG_BTC))
-        ;
-      state = I2C_STOP;
-      break;
-    case I2C_STOP:
-      /* send a stop condition to I2C bus */
-      i2c_stop_on_bus(I2C0);
-      /* i2c master sends STOP signal successfully */
-      while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        timeout = 0;
-        state = I2C_END;
-        done = true;
-      } else {
-        timedout = true;
-        done = true;
-        timeout = 0;
-        state = I2C_START;
-      }
-      break;
-    default:
-      state = I2C_START;
-      timeout = 0;
-      break;
-    }
-  }
-  unlock();
-  return timedout == false;
+	uint8_t state = I2C_START;
+	uint16_t timeout = 0;
+	bool done = false;
+	bool timedout = false;
+	while (!(done || timedout)) {
+		switch (state) {
+		case I2C_START:
+			/* i2c master sends start signal only when the bus is idle */
+			while (i2c_flag_get(I2C0, I2C_FLAG_I2CBSY)
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				i2c_start_on_bus(I2C0);
+				timeout = 0;
+				state = I2C_SEND_ADDRESS;
+			} else {
+				I2C_Unstick();
+				timeout = 0;
+				state = I2C_START;
+			}
+			break;
+		case I2C_SEND_ADDRESS:
+			/* i2c master sends START signal successfully */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_SBSEND))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
+				timeout = 0;
+				state = I2C_CLEAR_ADDRESS_FLAG;
+			} else {
+				timedout = true;
+				done = true;
+				timeout = 0;
+				state = I2C_START;
+			}
+			break;
+		case I2C_CLEAR_ADDRESS_FLAG:
+			/* address flag set means i2c slave sends ACK */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+				if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
+					i2c_flag_clear(I2C0, I2C_FLAG_AERR);
+					i2c_stop_on_bus(I2C0);
+					/* i2c master sends STOP signal successfully */
+					while ((I2C_CTL0(I2C0) & 0x0200)
+							&& (timeout < I2C_TIME_OUT )) {
+						timeout++;
+					}
+					// Address NACK'd
+					unlock();
+					return false;
+				}
+			}
+			timeout = 0;
+			if (timeout < I2C_TIME_OUT) {
+				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+				state = I2C_TRANSMIT_DATA;
+			} else {
+				// Dont retry as this means a NAK
+				i2c_stop_on_bus(I2C0);
+				/* i2c master sends STOP signal successfully */
+				while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				unlock();
+				return false;
+			}
+			break;
+		case I2C_TRANSMIT_DATA:
+			/* wait until the transmit data buffer is empty */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_TBE))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				/* send the EEPROM's internal address to write to : only one byte
+				 * address */
+				i2c_data_transmit(I2C0, MemAddress);
+				timeout = 0;
+			} else {
+				timedout = true;
+				timeout = 0;
+				state = I2C_START;
+			}
+			/* wait until BTC bit is set */
+			while (!i2c_flag_get(I2C0, I2C_FLAG_BTC))
+				;
+			dma_deinit(DMA0, DMA_CH5);
+			dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
+			dma_init_struct.memory_addr = (uint32_t) p_buffer;
+			dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
+			dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
+			dma_init_struct.number = number_of_byte;
+			dma_init_struct.periph_addr = (uint32_t) &I2C_DATA(I2C0);
+			dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
+			dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
+			dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH;
+			dma_init(DMA0, DMA_CH5, &dma_init_struct);
+			/* enable I2C0 DMA */
+			i2c_dma_enable(I2C0, I2C_DMA_ON);
+			/* enable DMA0 channel5 */
+			dma_channel_enable(DMA0, DMA_CH5);
+			/* wait until BTC bit is set */
+			while (!dma_flag_get(DMA0, DMA_CH5, DMA_FLAG_FTF)) {
+
+			}
+			/* wait until BTC bit is set */
+			while (!i2c_flag_get(I2C0, I2C_FLAG_BTC)) {
+			}
+			state = I2C_STOP;
+			break;
+		case I2C_STOP:
+			/* send a stop condition to I2C bus */
+			i2c_stop_on_bus(I2C0);
+			/* i2c master sends STOP signal successfully */
+			while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				timeout = 0;
+				state = I2C_END;
+				done = true;
+			} else {
+				timedout = true;
+				done = true;
+				timeout = 0;
+				state = I2C_START;
+			}
+			break;
+		default:
+			state = I2C_START;
+			timeout = 0;
+			break;
+		}
+	}
+	unlock();
+	return timedout == false;
 }
 
 bool FRToSI2C::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
-  return Mem_Write(DevAddress, pData[0], pData + 1, Size - 1);
+	return Mem_Write(DevAddress, pData[0], pData + 1, Size - 1);
 }
 
 bool FRToSI2C::probe(uint16_t DevAddress) {
-  uint8_t temp[1];
-  return Mem_Read(DevAddress, 0x00, temp, sizeof(temp));
+	uint8_t temp[1];
+	return Mem_Read(DevAddress, 0x00, temp, sizeof(temp));
 }
 
-void FRToSI2C::I2C_Unstick() { unstick_I2C(); }
+void FRToSI2C::I2C_Unstick() {
+	unstick_I2C();
+}
 
 bool FRToSI2C::lock() {
-  if (I2CSemaphore == nullptr) {
-    return false;
-  }
-  return xSemaphoreTake(I2CSemaphore, 1000) == pdTRUE;
+	if (I2CSemaphore == nullptr) {
+		return false;
+	}
+	return xSemaphoreTake(I2CSemaphore, TICKS_SECOND) == pdTRUE;
 }
 
-void FRToSI2C::unlock() { xSemaphoreGive(I2CSemaphore); }
+void FRToSI2C::unlock() {
+	xSemaphoreGive(I2CSemaphore);
+}
 
 bool FRToSI2C::writeRegistersBulk(const uint8_t address,
-                                  const I2C_REG *registers,
-                                  const uint8_t registersLength) {
-  for (int index = 0; index < registersLength; index++) {
-    if (!I2C_RegisterWrite(address, registers[index].reg,
-                           registers[index].val)) {
-      return false;
-    }
-    if (registers[index].pause_ms) {
-      delay_ms(registers[index].pause_ms);
-    }
-  }
-  return true;
+		const I2C_REG *registers, const uint8_t registersLength) {
+	for (int index = 0; index < registersLength; index++) {
+		if (!I2C_RegisterWrite(address, registers[index].reg,
+				registers[index].val)) {
+			return false;
+		}
+		if (registers[index].pause_ms) {
+			delay_ms(registers[index].pause_ms);
+		}
+	}
+	return true;
 }
 
 bool FRToSI2C::wakePart(uint16_t DevAddress) {
-  // wakepart is a special case  where only the device address is sent
-  if (!lock())
-    return false;
+	// wakepart is a special case  where only the device address is sent
+	if (!lock())
+		return false;
 
-  i2c_interrupt_disable(I2C0, I2C_INT_ERR);
-  i2c_interrupt_disable(I2C0, I2C_INT_EV);
-  i2c_interrupt_disable(I2C0, I2C_INT_BUF);
+	i2c_interrupt_disable(I2C0, I2C_INT_ERR);
+	i2c_interrupt_disable(I2C0, I2C_INT_EV);
+	i2c_interrupt_disable(I2C0, I2C_INT_BUF);
 
-  uint8_t state = I2C_START;
-  uint16_t timeout = 0;
-  bool done = false;
-  bool timedout = false;
-  while (!(done || timedout)) {
-    switch (state) {
-    case I2C_START:
-      /* i2c master sends start signal only when the bus is idle */
-      while (i2c_flag_get(I2C0, I2C_FLAG_I2CBSY) && (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        i2c_start_on_bus(I2C0);
-        timeout = 0;
-        state = I2C_SEND_ADDRESS;
-      } else {
-        I2C_Unstick();
-        timeout = 0;
-        state = I2C_START;
-      }
-      break;
-    case I2C_SEND_ADDRESS:
-      /* i2c master sends START signal successfully */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) &&
-             (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
-        timeout = 0;
-        state = I2C_CLEAR_ADDRESS_FLAG;
-      } else {
-        timedout = true;
-        done = true;
-        timeout = 0;
-        state = I2C_START;
-      }
-      break;
-    case I2C_CLEAR_ADDRESS_FLAG:
-      /* address flag set means i2c slave sends ACK */
-      while ((!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) &&
-             (timeout < I2C_TIME_OUT)) {
-        timeout++;
-        if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
-          i2c_flag_clear(I2C0, I2C_FLAG_AERR);
-          i2c_stop_on_bus(I2C0);
-          /* i2c master sends STOP signal successfully */
-          while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-            timeout++;
-          }
-          // Address NACK'd
-          unlock();
-          return false;
-        }
-      }
-      if (timeout < I2C_TIME_OUT) {
-        i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
-        timeout = 0;
-        state = I2C_STOP;
-      } else {
-        // Dont retry as this means a NAK
-        i2c_stop_on_bus(I2C0);
-        /* i2c master sends STOP signal successfully */
-        while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-          timeout++;
-        }
-        unlock();
-        return false;
-      }
-      break;
+	uint8_t state = I2C_START;
+	uint16_t timeout = 0;
+	bool done = false;
+	bool timedout = false;
+	while (!(done || timedout)) {
+		switch (state) {
+		case I2C_START:
+			/* i2c master sends start signal only when the bus is idle */
+			while (i2c_flag_get(I2C0, I2C_FLAG_I2CBSY)
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				i2c_start_on_bus(I2C0);
+				timeout = 0;
+				state = I2C_SEND_ADDRESS;
+			} else {
+				I2C_Unstick();
+				timeout = 0;
+				state = I2C_START;
+			}
+			break;
+		case I2C_SEND_ADDRESS:
+			/* i2c master sends START signal successfully */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_SBSEND))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				i2c_master_addressing(I2C0, DevAddress, I2C_TRANSMITTER);
+				timeout = 0;
+				state = I2C_CLEAR_ADDRESS_FLAG;
+			} else {
+				timedout = true;
+				done = true;
+				timeout = 0;
+				state = I2C_START;
+			}
+			break;
+		case I2C_CLEAR_ADDRESS_FLAG:
+			/* address flag set means i2c slave sends ACK */
+			while ((!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND))
+					&& (timeout < I2C_TIME_OUT )) {
+				timeout++;
+				if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) {
+					i2c_flag_clear(I2C0, I2C_FLAG_AERR);
+					i2c_stop_on_bus(I2C0);
+					/* i2c master sends STOP signal successfully */
+					while ((I2C_CTL0(I2C0) & 0x0200)
+							&& (timeout < I2C_TIME_OUT )) {
+						timeout++;
+					}
+					// Address NACK'd
+					unlock();
+					return false;
+				}
+			}
+			if (timeout < I2C_TIME_OUT) {
+				i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND);
+				timeout = 0;
+				state = I2C_STOP;
+			} else {
+				// Dont retry as this means a NAK
+				i2c_stop_on_bus(I2C0);
+				/* i2c master sends STOP signal successfully */
+				while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+					timeout++;
+				}
+				unlock();
+				return false;
+			}
+			break;
 
-    case I2C_STOP:
-      /* send a stop condition to I2C bus */
-      i2c_stop_on_bus(I2C0);
-      /* i2c master sends STOP signal successfully */
-      while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT)) {
-        timeout++;
-      }
-      if (timeout < I2C_TIME_OUT) {
-        timeout = 0;
-        state = I2C_END;
-        done = true;
-      } else {
-        timedout = true;
-        done = true;
-        timeout = 0;
-        state = I2C_START;
-      }
-      break;
-    default:
-      state = I2C_START;
-      timeout = 0;
-      break;
-    }
-  }
-  unlock();
-  return timedout == false;
+		case I2C_STOP:
+			/* send a stop condition to I2C bus */
+			i2c_stop_on_bus(I2C0);
+			/* i2c master sends STOP signal successfully */
+			while ((I2C_CTL0(I2C0) & 0x0200) && (timeout < I2C_TIME_OUT )) {
+				timeout++;
+			}
+			if (timeout < I2C_TIME_OUT) {
+				timeout = 0;
+				state = I2C_END;
+				done = true;
+			} else {
+				timedout = true;
+				done = true;
+				timeout = 0;
+				state = I2C_START;
+			}
+			break;
+		default:
+			state = I2C_START;
+			timeout = 0;
+			break;
+		}
+	}
+	unlock();
+	return timedout == false;
 }
diff --git a/workspace/TS100/Core/BSP/Pine64/Setup.cpp b/workspace/TS100/Core/BSP/Pine64/Setup.cpp
index 2b29f618..bc19dcd0 100755
--- a/workspace/TS100/Core/BSP/Pine64/Setup.cpp
+++ b/workspace/TS100/Core/BSP/Pine64/Setup.cpp
@@ -61,16 +61,20 @@ void setup_gpio() {
 	// OLED reset as output
 	gpio_init(OLED_RESET_GPIO_Port, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ,
 	OLED_RESET_Pin);
+	gpio_bit_set(SDA_GPIO_Port, SDA_Pin);
+	gpio_bit_set(SDA_GPIO_Port, SCL_Pin);
 	// I2C as AF Open Drain
-	gpio_init(SDA_GPIO_Port, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ,
+	gpio_init(SDA_GPIO_Port, GPIO_MODE_AF_OD, GPIO_OSPEED_2MHZ,
 	SDA_Pin | SCL_Pin);
 	// PWM output as AF Push Pull
-	gpio_init(PWM_Out_GPIO_Port, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, PWM_Out_Pin);
+	gpio_init(PWM_Out_GPIO_Port, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ,
+			PWM_Out_Pin);
 	// Analog Inputs ... as analog inputs
-	gpio_init(TMP36_INPUT_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_50MHZ,
+	gpio_init(TMP36_INPUT_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_2MHZ,
 	TMP36_INPUT_Pin);
-	gpio_init(TIP_TEMP_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_50MHZ, TIP_TEMP_Pin);
-	gpio_init(VIN_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_50MHZ, VIN_Pin);
+	gpio_init(TIP_TEMP_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_2MHZ,
+			TIP_TEMP_Pin);
+	gpio_init(VIN_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_2MHZ, VIN_Pin);
 
 	// Remap PB4 away from JTAG NJRST
 	gpio_pin_remap_config(GPIO_SWJ_NONJTRST_REMAP, ENABLE);
@@ -111,7 +115,7 @@ void setup_i2c() {
 	/* enable I2C0 clock */
 	rcu_periph_clock_enable(RCU_I2C0);
 	// Setup I20 at 400kHz
-	i2c_clock_config(I2C0, 400 * 1000, I2C_DTCY_16_9);
+	i2c_clock_config(I2C0, 400 * 1000, I2C_DTCY_2);
 	i2c_mode_addr_config(I2C0, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, 0x00);
 	i2c_enable(I2C0);
 	/* enable acknowledge */
@@ -132,7 +136,7 @@ void setup_adc() {
 	/* config ADC clock */
 	rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV16);
 	// Run in normal parallel + inserted parallel
-	adc_mode_config(ADC0,ADC_DAUL_REGULAL_PARALLEL_INSERTED_PARALLEL);
+	adc_mode_config(ADC0, ADC_DAUL_REGULAL_PARALLEL_INSERTED_PARALLEL);
 	adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE);
 	adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE);
 	adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE);
@@ -225,8 +229,10 @@ void setup_timers() {
 		timer_ocintpara.outputstate = TIMER_CCX_ENABLE;
 		timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocintpara);
 
-		timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, powerPWM + holdoffTicks);
-		timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM1);
+		timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0,
+				powerPWM + holdoffTicks);
+		timer_channel_output_mode_config(TIMER1, TIMER_CH_0,
+				TIMER_OC_MODE_PWM1);
 		timer_channel_output_shadow_config(TIMER1, TIMER_CH_0,
 		TIMER_OC_SHADOW_DISABLE);
 		/* CH1 used for irq */
@@ -236,7 +242,8 @@ void setup_timers() {
 		timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocintpara);
 
 		timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, 0);
-		timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0);
+		timer_channel_output_mode_config(TIMER1, TIMER_CH_1,
+				TIMER_OC_MODE_PWM0);
 		timer_channel_output_shadow_config(TIMER1, TIMER_CH_1,
 		TIMER_OC_SHADOW_DISABLE);
 		// IRQ
@@ -269,7 +276,8 @@ void setup_timers() {
 		timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
 		timer_channel_output_config(TIMER2, TIMER_CH_0, &timer_ocintpara);
 		timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, 0);
-		timer_channel_output_mode_config(TIMER2, TIMER_CH_0, TIMER_OC_MODE_PWM0);
+		timer_channel_output_mode_config(TIMER2, TIMER_CH_0,
+				TIMER_OC_MODE_PWM0);
 		timer_channel_output_shadow_config(TIMER2, TIMER_CH_0,
 		TIMER_OC_SHADOW_DISABLE);
 		timer_auto_reload_shadow_enable(TIMER2);
diff --git a/workspace/TS100/Core/BSP/Pine64/preRTOS.cpp b/workspace/TS100/Core/BSP/Pine64/preRTOS.cpp
index 68201936..013c9e4f 100755
--- a/workspace/TS100/Core/BSP/Pine64/preRTOS.cpp
+++ b/workspace/TS100/Core/BSP/Pine64/preRTOS.cpp
@@ -14,8 +14,9 @@ void preRToSInit() {
 	//Normal system bringup -- GPIO etc
 
 	hardware_init();
+	delay_ms(5);
 	gpio_bit_reset(OLED_RESET_GPIO_Port, OLED_RESET_Pin);
-	FRToSI2C::FRToSInit();
 	delay_ms(50);
 	gpio_bit_set(OLED_RESET_GPIO_Port, OLED_RESET_Pin);
+	FRToSI2C::FRToSInit();
 }
diff --git a/workspace/TS100/Core/Drivers/OLED.cpp b/workspace/TS100/Core/Drivers/OLED.cpp
index 0fd6bb3c..84e20aa8 100755
--- a/workspace/TS100/Core/Drivers/OLED.cpp
+++ b/workspace/TS100/Core/Drivers/OLED.cpp
@@ -96,12 +96,12 @@ void OLED::initialize() {
 	// Set the display to be ON once the settings block is sent and send the
 	// initialisation data to the OLED.
 
-	setDisplayState(DisplayState::ON);
 	for (int tries = 0; tries < 10; tries++) {
 		if (FRToSI2C::writeRegistersBulk(DEVICEADDR_OLED, OLED_Setup_Array, sizeof(OLED_Setup_Array) / sizeof(OLED_Setup_Array[0]))) {
 			return;
 		}
 	}
+	setDisplayState(DisplayState::ON);
 }
 void OLED::setFramebuffer(uint8_t *buffer) {
 	if (buffer == NULL) {
@@ -171,7 +171,7 @@ void OLED::transitionSecondaryFramebuffer(bool forwardNavigation) {
 
 	while (duration <= totalDuration) {
 		duration = xTaskGetTickCount() - start;
-		uint8_t progress = duration * 1000 / totalDuration;
+		uint8_t progress = duration * TICKS_SECOND / totalDuration;
 		progress = easeInOutTiming(progress);
 		progress = lerp(0, OLED_WIDTH, progress);
 		if (progress > OLED_WIDTH) {