From cd138dd6c7b099124ff7b82686c2e17f80be34bc Mon Sep 17 00:00:00 2001
From: Matthijs Kooijman <matthijs@stdin.nl>
Date: Wed, 7 Dec 2022 09:50:31 +0100
Subject: [PATCH] [STM32WLx] Apply PA clamp workaround for HP only

This modifies SX1262::begin and beginFSK to call setOutputPower after
applying the workaround, so that can undo the workaround if needed.
---
 src/modules/SX126x/STM32WLx.cpp |  4 ++++
 src/modules/SX126x/SX1262.cpp   |  8 ++++----
 src/modules/SX126x/SX126x.cpp   | 10 +++++++---
 src/modules/SX126x/SX126x.h     |  2 +-
 4 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/modules/SX126x/STM32WLx.cpp b/src/modules/SX126x/STM32WLx.cpp
index bad38dc9..744f88c3 100644
--- a/src/modules/SX126x/STM32WLx.cpp
+++ b/src/modules/SX126x/STM32WLx.cpp
@@ -64,6 +64,10 @@ int16_t STM32WLx::setOutputPower(int8_t power) {
   }
   RADIOLIB_ASSERT(state);
 
+  // Apply workaround for HP only
+  state = SX126x::fixPaClamping(use_hp);
+  RADIOLIB_ASSERT(state);
+
   // set output power
   /// \todo power ramp time configuration
   state = SX126x::setTxParams(power);
diff --git a/src/modules/SX126x/SX1262.cpp b/src/modules/SX126x/SX1262.cpp
index 1a2a60d3..a667525e 100644
--- a/src/modules/SX126x/SX1262.cpp
+++ b/src/modules/SX126x/SX1262.cpp
@@ -20,10 +20,10 @@ int16_t SX1262::begin(float freq, float bw, uint8_t sf, uint8_t cr, uint8_t sync
   state = setFrequency(freq);
   RADIOLIB_ASSERT(state);
 
-  state = setOutputPower(power);
+  state = SX126x::fixPaClamping();
   RADIOLIB_ASSERT(state);
 
-  state = SX126x::fixPaClamping();
+  state = setOutputPower(power);
   RADIOLIB_ASSERT(state);
 
   return(state);
@@ -38,10 +38,10 @@ int16_t SX1262::beginFSK(float freq, float br, float freqDev, float rxBw, int8_t
   state = setFrequency(freq);
   RADIOLIB_ASSERT(state);
 
-  state = setOutputPower(power);
+  state = SX126x::fixPaClamping();
   RADIOLIB_ASSERT(state);
 
-  state = SX126x::fixPaClamping();
+  state = setOutputPower(power);
   RADIOLIB_ASSERT(state);
 
   return(state);
diff --git a/src/modules/SX126x/SX126x.cpp b/src/modules/SX126x/SX126x.cpp
index 73e8a4df..2e95d778 100644
--- a/src/modules/SX126x/SX126x.cpp
+++ b/src/modules/SX126x/SX126x.cpp
@@ -1575,7 +1575,7 @@ int16_t SX126x::fixSensitivity() {
   return(writeRegister(RADIOLIB_SX126X_REG_SENSITIVITY_CONFIG, &sensitivityConfig, 1));
 }
 
-int16_t SX126x::fixPaClamping() {
+int16_t SX126x::fixPaClamping(bool enable) {
   // fixes overly eager PA clamping
   // see SX1262/SX1268 datasheet, chapter 15 Known Limitations, section 15.2 for details
 
@@ -1584,8 +1584,12 @@ int16_t SX126x::fixPaClamping() {
   int16_t state = readRegister(RADIOLIB_SX126X_REG_TX_CLAMP_CONFIG, &clampConfig, 1);
   RADIOLIB_ASSERT(state);
 
-  // update with the new value
-  clampConfig |= 0x1E;
+  // apply or undo workaround
+  if (enable)
+    clampConfig |= 0x1E;
+  else
+    clampConfig = (clampConfig & ~0x1E) | 0x08;
+
   return(writeRegister(RADIOLIB_SX126X_REG_TX_CLAMP_CONFIG, &clampConfig, 1));
 }
 
diff --git a/src/modules/SX126x/SX126x.h b/src/modules/SX126x/SX126x.h
index e1b603ba..97cb77e1 100644
--- a/src/modules/SX126x/SX126x.h
+++ b/src/modules/SX126x/SX126x.h
@@ -1005,7 +1005,7 @@ class SX126x: public PhysicalLayer {
 
     // fixes to errata
     int16_t fixSensitivity();
-    int16_t fixPaClamping();
+    int16_t fixPaClamping(bool enable = true);
     int16_t fixImplicitTimeout();
     int16_t fixInvertedIQ(uint8_t iqConfig);