From baa7e1bf12b58224e9461a5a605dcebabcf9504e Mon Sep 17 00:00:00 2001
From: Riku Isokoski <riksu9000@gmail.com>
Date: Mon, 9 Aug 2021 17:45:24 +0300
Subject: [PATCH] Optimize twiMaster

---
 src/drivers/TwiMaster.cpp | 43 ++++++++++++++++++---------------------
 src/drivers/TwiMaster.h   |  4 +++-
 2 files changed, 23 insertions(+), 24 deletions(-)

diff --git a/src/drivers/TwiMaster.cpp b/src/drivers/TwiMaster.cpp
index fc9edf81..e4c03d5b 100644
--- a/src/drivers/TwiMaster.cpp
+++ b/src/drivers/TwiMaster.cpp
@@ -9,21 +9,25 @@ using namespace Pinetime::Drivers;
 // TODO use DMA/IRQ
 
 TwiMaster::TwiMaster(const Modules module, const Parameters& params) : module {module}, params {params} {
+  mutex = xSemaphoreCreateBinary();
+}
+
+void TwiMaster::ConfigurePins() const {
+  NRF_GPIO->PIN_CNF[params.pinScl] =
+    (GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) |
+    (GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
+    (GPIO_PIN_CNF_DRIVE_S0D1 << GPIO_PIN_CNF_DRIVE_Pos) |
+    (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
+
+  NRF_GPIO->PIN_CNF[params.pinSda] =
+    (GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) |
+    (GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
+    (GPIO_PIN_CNF_DRIVE_S0D1 << GPIO_PIN_CNF_DRIVE_Pos) |
+    (GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
 }
 
 void TwiMaster::Init() {
-  if(mutex == nullptr)
-    mutex = xSemaphoreCreateBinary();
-  
-  NRF_GPIO->PIN_CNF[params.pinScl] =
-    ((uint32_t) GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t) GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_PULL_Pullup << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t) GPIO_PIN_CNF_DRIVE_S0D1 << GPIO_PIN_CNF_DRIVE_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
-
-  NRF_GPIO->PIN_CNF[params.pinSda] =
-    ((uint32_t) GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t) GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_PULL_Pullup << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t) GPIO_PIN_CNF_DRIVE_S0D1 << GPIO_PIN_CNF_DRIVE_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
+  ConfigurePins();
 
   switch (module) {
     case Modules::TWIM1:
@@ -179,7 +183,8 @@ void TwiMaster::Sleep() {
 }
 
 void TwiMaster::Wakeup() {
-  Init();
+  ConfigurePins();
+  twiBaseAddress->ENABLE = (TWIM_ENABLE_ENABLE_Enabled << TWIM_ENABLE_ENABLE_Pos);
   NRF_LOG_INFO("[TWIMASTER] Wakeup");
 }
 
@@ -194,16 +199,8 @@ void TwiMaster::FixHwFreezed() {
   uint32_t twi_state = NRF_TWI1->ENABLE;
   twiBaseAddress->ENABLE = TWIM_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos;
 
-  NRF_GPIO->PIN_CNF[params.pinScl] =
-    ((uint32_t) GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t) GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_PULL_Pullup << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t) GPIO_PIN_CNF_DRIVE_S0S1 << GPIO_PIN_CNF_DRIVE_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
-
-  NRF_GPIO->PIN_CNF[params.pinSda] =
-    ((uint32_t) GPIO_PIN_CNF_DIR_Input << GPIO_PIN_CNF_DIR_Pos) | ((uint32_t) GPIO_PIN_CNF_INPUT_Connect << GPIO_PIN_CNF_INPUT_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_PULL_Pullup << GPIO_PIN_CNF_PULL_Pos) | ((uint32_t) GPIO_PIN_CNF_DRIVE_S0S1 << GPIO_PIN_CNF_DRIVE_Pos) |
-    ((uint32_t) GPIO_PIN_CNF_SENSE_Disabled << GPIO_PIN_CNF_SENSE_Pos);
+  ConfigurePins();
 
   // Re-enable I²C
   twiBaseAddress->ENABLE = twi_state;
-}
\ No newline at end of file
+}
diff --git a/src/drivers/TwiMaster.h b/src/drivers/TwiMaster.h
index 6175b99b..b8d36d59 100644
--- a/src/drivers/TwiMaster.h
+++ b/src/drivers/TwiMaster.h
@@ -26,6 +26,8 @@ namespace Pinetime {
       void Sleep();
       void Wakeup();
 
+      void ConfigurePins() const;
+
     private:
       ErrorCodes Read(uint8_t deviceAddress, uint8_t* buffer, size_t size, bool stop);
       ErrorCodes Write(uint8_t deviceAddress, const uint8_t* data, size_t size, bool stop);
@@ -41,4 +43,4 @@ namespace Pinetime {
       static constexpr uint32_t HwFreezedDelay {161000};
     };
   }
-}
\ No newline at end of file
+}