From 23b54526703b5c23a8c42a08073ff88624b7f5b8 Mon Sep 17 00:00:00 2001
From: "Ben V. Brown" <Ralim@Ralimtek.com>
Date: Fri, 18 Sep 2020 21:59:38 +1000
Subject: [PATCH] Port across init reg helper

---
 .../TS100/Core/BSP/Miniware/I2C_Wrapper.cpp   | 44 +++++++++++++------
 .../TS100/Core/BSP/Pine64/I2C_Wrapper.cpp     |  4 +-
 2 files changed, 32 insertions(+), 16 deletions(-)

diff --git a/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp b/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp
index 5eb60c81..95ae98b1 100644
--- a/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp
@@ -9,6 +9,8 @@
 #include "Setup.h"
 SemaphoreHandle_t FRToSI2C::I2CSemaphore = nullptr;
 StaticSemaphore_t FRToSI2C::xSemaphoreBuffer;
+SemaphoreHandle_t FRToSI2C::I2CSemaphore2 = nullptr;
+StaticSemaphore_t FRToSI2C::xSemaphoreBuffer2;
 
 void FRToSI2C::CpltCallback() {
 	hi2c1.State = HAL_I2C_STATE_READY;  // Force state reset (even if tx error)
@@ -17,13 +19,11 @@ void FRToSI2C::CpltCallback() {
 	}
 }
 
-bool FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
-		uint8_t *pData, uint16_t Size) {
+bool FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData, uint16_t Size) {
 
 	if (!lock())
 		return false;
-	if (HAL_I2C_Mem_Read(&hi2c1, DevAddress, MemAddress,
-	I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {
+	if (HAL_I2C_Mem_Read(&hi2c1, DevAddress, MemAddress, I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {
 
 		I2C_Unstick();
 		unlock();
@@ -43,13 +43,11 @@ uint8_t FRToSI2C::I2C_RegisterRead(uint8_t add, uint8_t reg) {
 	Mem_Read(add, reg, tx_data, 1);
 	return tx_data[0];
 }
-bool FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
-		uint8_t *pData, uint16_t Size) {
+bool FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData, uint16_t Size) {
 
 	if (!lock())
 		return false;
-	if (HAL_I2C_Mem_Write(&hi2c1, DevAddress, MemAddress,
-	I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {
+	if (HAL_I2C_Mem_Write(&hi2c1, DevAddress, MemAddress, I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {
 
 		I2C_Unstick();
 		unlock();
@@ -63,8 +61,7 @@ bool FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
 bool FRToSI2C::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
 	if (!lock())
 		return false;
-	if (HAL_I2C_Master_Transmit_DMA(&hi2c1, DevAddress, pData, Size)
-			!= HAL_OK) {
+	if (HAL_I2C_Master_Transmit_DMA(&hi2c1, DevAddress, pData, Size) != HAL_OK) {
 		I2C_Unstick();
 		unlock();
 		return false;
@@ -76,8 +73,7 @@ bool FRToSI2C::probe(uint16_t DevAddress) {
 	if (!lock())
 		return false;
 	uint8_t buffer[1];
-	bool worked = HAL_I2C_Mem_Read(&hi2c1, DevAddress, 0x0F,
-			I2C_MEMADD_SIZE_8BIT, buffer, 1, 1000) == HAL_OK;
+	bool worked = HAL_I2C_Mem_Read(&hi2c1, DevAddress, 0x0F, I2C_MEMADD_SIZE_8BIT, buffer, 1, 1000) == HAL_OK;
 	unlock();
 	return worked;
 }
@@ -87,11 +83,31 @@ void FRToSI2C::I2C_Unstick() {
 }
 
 void FRToSI2C::unlock() {
-
 	xSemaphoreGive(I2CSemaphore);
 }
 
 bool FRToSI2C::lock() {
-
 	return xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE;
 }
+bool FRToSI2C::lock2() {
+	if (I2CSemaphore2 == nullptr)
+		return true;
+	return xSemaphoreTake(I2CSemaphore2,1000) == pdTRUE;
+}
+
+void FRToSI2C::unlock2() {
+	if (I2CSemaphore2 == nullptr)
+		return;
+	xSemaphoreGive(I2CSemaphore2);
+}
+
+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;
+}
diff --git a/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp b/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp
index 2fba6154..2588c008 100644
--- a/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp
+++ b/workspace/TS100/Core/BSP/Pine64/I2C_Wrapper.cpp
@@ -7,9 +7,9 @@
 #include "BSP.h"
 #include "Setup.h"
 #include <I2C_Wrapper.hpp>
-SemaphoreHandle_t FRToSI2C::I2CSemaphore;
+SemaphoreHandle_t FRToSI2C::I2CSemaphore = nullptr;
 StaticSemaphore_t FRToSI2C::xSemaphoreBuffer;
-SemaphoreHandle_t FRToSI2C::I2CSemaphore2;
+SemaphoreHandle_t FRToSI2C::I2CSemaphore2 = nullptr;
 StaticSemaphore_t FRToSI2C::xSemaphoreBuffer2;
 #define FLAG_TIMEOUT 1000