From 431ba17b26ed8da3ae82e17443d8deebe3f775fb Mon Sep 17 00:00:00 2001
From: Silvano Seva <silseva@fastwebnet.it>
Date: Tue, 21 Sep 2021 12:01:31 +0200
Subject: [PATCH] Modified HR_Cx000::setInputGain() function to accept a value
 expressed in dB instead of a raw binary value to be written directly inside
 the chip's register

---
 platform/drivers/baseband/HR_Cx000.cpp    | 63 ++++++++++++++++++++---
 platform/drivers/baseband/HR_Cx000.h      |  7 ++-
 platform/drivers/baseband/radio_MD3x0.cpp |  4 +-
 3 files changed, 63 insertions(+), 11 deletions(-)

diff --git a/platform/drivers/baseband/HR_Cx000.cpp b/platform/drivers/baseband/HR_Cx000.cpp
index ba278e8c..089b16e4 100644
--- a/platform/drivers/baseband/HR_Cx000.cpp
+++ b/platform/drivers/baseband/HR_Cx000.cpp
@@ -47,18 +47,67 @@ void HR_Cx000< C6000_SpiOpModes >::setDacGain(uint8_t value)
 }
 
 template <>
-void HR_Cx000< C5000_SpiOpModes >::setInputGain(uint8_t value)
+void HR_Cx000< C5000_SpiOpModes >::setInputGain(int8_t value)
 {
-    // Mic gain is controlled by bytes 7:3 of register 0x0F (ADLinVol).
-    if(value > 31) value = 31;
-    writeReg(C5000_SpiOpModes::CONFIG, 0x0F, value << 3);
+    /*
+     * On HR_C5000 the input gain value is controlled by bits 7:3 of of register
+     * 0x0F (ADLinVol): these bits allow to set the gain in a range from +12dB
+     * to -34.5dB in steps of 1.5dB each.
+     * The equation relating the gain in dB with register value is: Gain = reg*1.5 - 34.5
+     * Its inverse is: reg = (gain + 34.5)/1.5 or, equivalently, reg = (gain/1.5) + 23
+     */
+
+    // Keep gain value in range +12dB ... -34.5dB
+    if(value > 12)    value = 12;
+    if(value < -34.5) value = -34.5;
+
+    // Apply the inverse equation to obtain gain register. Gain is multiplied
+    // by ten and divided by 15 to keep using integer operations.
+    int16_t result = (((int16_t) value) * 10)/15 + 23;
+    uint8_t regVal = ((uint8_t) result);
+
+    if(regVal > 31) regVal = 31;
+    writeReg(C5000_SpiOpModes::CONFIG, 0x0F, regVal << 3);
 }
 
 template <>
-void HR_Cx000< C6000_SpiOpModes >::setInputGain(uint8_t value)
+void HR_Cx000< C6000_SpiOpModes >::setInputGain(int8_t value)
 {
-    // TODO: Experiments needed for identification of HR_C6000 mic gain register.
-    (void) value;
+    /*
+     * On HR_C6000 the input gain in two stages: the first one is a "coarse"
+     * stage capable of doing only 0dB, -6dB and -12dB. The second stage is a
+     * "fine" stage ranging from 0 to +36dB in steps of 3dB.
+     * Both of them are controlled by register 0xE4: bits 5:4 control the first
+     * stage, while bits 4:0 control the fine stage.
+     *
+     * Allowable gain ranges are:
+     * -  0  ... +36dB when coarse stage bits are 00
+     * - -6  ... +30dB when coarse stage bits are 01
+     * - -12 ... +24dB when coarse stage bits are 10 or 11
+     *
+     * General equation for gain is reg*3 + offset, where "offset" is the gain
+     * of the coarse stage.
+     * Inverse equation, given the coarse gain, is: reg = (gain - offset)/3
+     *
+     * In this code we use -12dB and 0dB coarse gain values, leaving the fine
+     * tuning to the second stage.
+     */
+
+    uint8_t regValue = 0;
+    if(value < 24)
+    {
+        int8_t fineGain = (value + 12)/3;    // Compute fine gain, -12dB offset
+        regValue = (2 << 4)                  // Select -12dB coarse gain
+                 | ((uint8_t) fineGain);     // Set fine gain bits
+    }
+    else
+    {
+        // For values starting from +24dB set coarse gain to 0dB and use
+        // only fine gain
+        regValue = ((uint8_t) value/3);
+    }
+
+    writeReg(C6000_SpiOpModes::CONFIG, 0xE4, regValue);
 }
 
 ScopedChipSelect::ScopedChipSelect()
diff --git a/platform/drivers/baseband/HR_Cx000.h b/platform/drivers/baseband/HR_Cx000.h
index a78738e8..4219d842 100644
--- a/platform/drivers/baseband/HR_Cx000.h
+++ b/platform/drivers/baseband/HR_Cx000.h
@@ -158,9 +158,12 @@ public:
      * sources. This value affects the modulation level in analog FM mode and
      * it must be correctly tuned to avoid distorsions.
      *
-     * @param value: gain value. Allowed range is 0-31.
+     * HR_C5000 gain ranges from -34.5dB to +12dB
+     * HR_C6000 gain ranges from -12dB to +36dB
+     *
+     * @param value: gain value in dB.
      */
-    void setInputGain(uint8_t value);
+    void setInputGain(int8_t value);
 
     /**
      * Configure chipset for DMR operation.
diff --git a/platform/drivers/baseband/radio_MD3x0.cpp b/platform/drivers/baseband/radio_MD3x0.cpp
index 968f29f3..015934a5 100644
--- a/platform/drivers/baseband/radio_MD3x0.cpp
+++ b/platform/drivers/baseband/radio_MD3x0.cpp
@@ -201,7 +201,7 @@ void radio_setOpmode(const enum opmode mode)
             gpio_clearPin(DMR_SW);      // Disconnect analog paths for DMR
             gpio_setPin(FM_SW);         // Enable analog RX stage after superhet
             C5000.fmMode();             // HR_C5000 in FM mode
-            C5000.setInputGain(0x19);   // Input gain, as per TYT firmware
+            C5000.setInputGain(+3);     // Input gain in dB, as per TYT firmware
             break;
 
         case DMR:
@@ -214,7 +214,7 @@ void radio_setOpmode(const enum opmode mode)
             gpio_clearPin(DMR_SW);      // Disconnect analog paths for DMR
             gpio_setPin(FM_SW);         // Enable analog RX stage after superhet
             C5000.fmMode();             // HR_C5000 in FM mode
-            C5000.setInputGain(0x14);   // Input gain, found experimentally
+            C5000.setInputGain(-5);     // Input gain in dB, found experimentally
             break;
 
         default: