1 files changed, 37 insertions, 24 deletions

diff --git a/src/drivers/SpiMaster.cpp b/src/drivers/SpiMaster.cpp
index 690a3226..19422ef3 100644
--- a/src/drivers/SpiMaster.cpp
+++ b/src/drivers/SpiMaster.cpp
@@ -94,32 +94,45 @@ bool SpiMaster::Init() {
   return true;
 }
 
-void SpiMaster::SetupWorkaroundForFtpan58(NRF_SPIM_Type* spim, uint32_t ppi_channel, uint32_t gpiote_channel) {
-  // Create an event when SCK toggles.
-  NRF_GPIOTE->CONFIG[gpiote_channel] = (GPIOTE_CONFIG_MODE_Event << GPIOTE_CONFIG_MODE_Pos) | (spim->PSEL.SCK << GPIOTE_CONFIG_PSEL_Pos) |
-                                       (GPIOTE_CONFIG_POLARITY_Toggle << GPIOTE_CONFIG_POLARITY_Pos);
+void SpiMaster::SetupWorkaroundForErratum58() {
+  nrfx_gpiote_pin_t pin = spiBaseAddress->PSEL.SCK;
+  nrfx_gpiote_in_config_t gpioteCfg = {.sense = NRF_GPIOTE_POLARITY_TOGGLE,
+                                       .pull = NRF_GPIO_PIN_NOPULL,
+                                       .is_watcher = false,
+                                       .hi_accuracy = true,
+                                       .skip_gpio_setup = true};
+  if (!workaroundActive) {
+    // Create an event when SCK toggles.
+    APP_ERROR_CHECK(nrfx_gpiote_in_init(pin, &gpioteCfg, NULL));
+    nrfx_gpiote_in_event_enable(pin, false);
+
+    // Stop the spim instance when SCK toggles.
+    nrf_ppi_channel_endpoint_setup(workaroundPpi, nrfx_gpiote_in_event_addr_get(pin), spiBaseAddress->TASKS_STOP);
+    nrf_ppi_channel_enable(workaroundPpi);
+  }
 
-  // Stop the spim instance when SCK toggles.
-  NRF_PPI->CH[ppi_channel].EEP = (uint32_t) &NRF_GPIOTE->EVENTS_IN[gpiote_channel];
-  NRF_PPI->CH[ppi_channel].TEP = (uint32_t) &spim->TASKS_STOP;
-  NRF_PPI->CHENSET = 1U << ppi_channel;
   spiBaseAddress->EVENTS_END = 0;
 
   // Disable IRQ
-  spim->INTENCLR = (1 << 6);
-  spim->INTENCLR = (1 << 1);
-  spim->INTENCLR = (1 << 19);
+  spiBaseAddress->INTENCLR = (1 << 6);
+  spiBaseAddress->INTENCLR = (1 << 1);
+  spiBaseAddress->INTENCLR = (1 << 19);
+  workaroundActive = true;
 }
 
-void SpiMaster::DisableWorkaroundForFtpan58(NRF_SPIM_Type* spim, uint32_t ppi_channel, uint32_t gpiote_channel) {
-  NRF_GPIOTE->CONFIG[gpiote_channel] = 0;
-  NRF_PPI->CH[ppi_channel].EEP = 0;
-  NRF_PPI->CH[ppi_channel].TEP = 0;
-  NRF_PPI->CHENSET = ppi_channel;
+void SpiMaster::DisableWorkaroundForErratum58() {
+  nrfx_gpiote_pin_t pin = spiBaseAddress->PSEL.SCK;
+  if (workaroundActive) {
+    nrfx_gpiote_in_uninit(pin);
+    nrf_ppi_channel_disable(workaroundPpi);
+  }
   spiBaseAddress->EVENTS_END = 0;
-  spim->INTENSET = (1 << 6);
-  spim->INTENSET = (1 << 1);
-  spim->INTENSET = (1 << 19);
+
+  // Enable IRQ
+  spiBaseAddress->INTENSET = (1 << 6);
+  spiBaseAddress->INTENSET = (1 << 1);
+  spiBaseAddress->INTENSET = (1 << 19);
+  workaroundActive = false;
 }
 
 void SpiMaster::OnEndEvent() {
@@ -176,9 +189,9 @@ bool SpiMaster::Write(uint8_t pinCsn, const uint8_t* data, size_t size, const st
   this->pinCsn = pinCsn;
 
   if (size == 1) {
-    SetupWorkaroundForFtpan58(spiBaseAddress, 0, 0);
+    SetupWorkaroundForErratum58();
   } else {
-    DisableWorkaroundForFtpan58(spiBaseAddress, 0, 0);
+    DisableWorkaroundForErratum58();
   }
 
   if (preTransactionHook != nullptr) {
@@ -201,7 +214,7 @@ bool SpiMaster::Write(uint8_t pinCsn, const uint8_t* data, size_t size, const st
     nrf_gpio_pin_set(this->pinCsn);
     currentBufferAddr = 0;
 
-    DisableWorkaroundForFtpan58(spiBaseAddress, 0, 0);
+    DisableWorkaroundForErratum58();
 
     xSemaphoreGive(mutex);
   }
@@ -213,7 +226,7 @@ bool SpiMaster::Read(uint8_t pinCsn, uint8_t* cmd, size_t cmdSize, uint8_t* data
   xSemaphoreTake(mutex, portMAX_DELAY);
 
   this->pinCsn = pinCsn;
-  DisableWorkaroundForFtpan58(spiBaseAddress, 0, 0);
+  DisableWorkaroundForErratum58();
   spiBaseAddress->INTENCLR = (1 << 6);
   spiBaseAddress->INTENCLR = (1 << 1);
   spiBaseAddress->INTENCLR = (1 << 19);
@@ -260,7 +273,7 @@ bool SpiMaster::WriteCmdAndBuffer(uint8_t pinCsn, const uint8_t* cmd, size_t cmd
   xSemaphoreTake(mutex, portMAX_DELAY);
 
   this->pinCsn = pinCsn;
-  DisableWorkaroundForFtpan58(spiBaseAddress, 0, 0);
+  DisableWorkaroundForErratum58();
   spiBaseAddress->INTENCLR = (1 << 6);
   spiBaseAddress->INTENCLR = (1 << 1);
   spiBaseAddress->INTENCLR = (1 << 19);