From 0024679216db08366dd0358e6eedfa3022d7627e Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Sat, 10 Sep 2022 01:24:07 +0200
Subject: [PATCH] nimble/phy/nrf5x: Force FEM disable on ble_phy_disable

---
 nimble/drivers/nrf5x/src/ble_phy.c   |  4 ++++
 nimble/drivers/nrf5x/src/nrf52/phy.c | 15 +++++++++++++++
 nimble/drivers/nrf5x/src/phy_priv.h  |  1 +
 3 files changed, 20 insertions(+)

diff --git a/nimble/drivers/nrf5x/src/ble_phy.c b/nimble/drivers/nrf5x/src/ble_phy.c
index b4d64d88d..36f209c5b 100644
--- a/nimble/drivers/nrf5x/src/ble_phy.c
+++ b/nimble/drivers/nrf5x/src/ble_phy.c
@@ -1976,6 +1976,10 @@ ble_phy_disable(void)
 
     ble_phy_stop_usec_timer();
     ble_phy_disable_irq_and_ppi();
+
+#if PHY_USE_FEM
+    phy_fem_disable();
+#endif
 }
 
 /* Gets the current access address */
diff --git a/nimble/drivers/nrf5x/src/nrf52/phy.c b/nimble/drivers/nrf5x/src/nrf52/phy.c
index 763e3eef2..d4492464a 100644
--- a/nimble/drivers/nrf5x/src/nrf52/phy.c
+++ b/nimble/drivers/nrf5x/src/nrf52/phy.c
@@ -139,6 +139,21 @@ phy_fem_enable_lna(void)
     nrf_ppi_channels_enable(NRF_PPI, PPI_CHEN_CH6_Msk | PPI_CHEN_CH7_Msk);
 }
 #endif
+
+void
+phy_fem_disable(void)
+{
+#if PHY_USE_FEM_SINGLE_GPIO
+    NRF_GPIOTE->TASKS_CLR[PHY_GPIOTE_FEM] = 1;
+#else
+#if PHY_USE_FEM_PA
+    NRF_GPIOTE->TASKS_CLR[PHY_GPIOTE_FEM_PA] = 1;
+#endif
+#if PHY_USE_FEM_LNA
+    NRF_GPIOTE->TASKS_CLR[PHY_GPIOTE_FEM_LNA] = 1;
+#endif
+#endif
+}
 #endif /* PHY_USE_FEM */
 
 void
diff --git a/nimble/drivers/nrf5x/src/phy_priv.h b/nimble/drivers/nrf5x/src/phy_priv.h
index b2308e61f..1a026a6d1 100644
--- a/nimble/drivers/nrf5x/src/phy_priv.h
+++ b/nimble/drivers/nrf5x/src/phy_priv.h
@@ -68,6 +68,7 @@ void phy_fem_enable_pa(void);
 #if PHY_USE_FEM_LNA
 void phy_fem_enable_lna(void);
 #endif
+void phy_fem_disable(void);
 #endif
 
 void phy_ppi_init(void);