From f883d4e7e1664d8171b2b971b648a7c51c948960 Mon Sep 17 00:00:00 2001
From: Silvano Seva <silseva@fastwebnet.it>
Date: Tue, 22 Oct 2024 18:50:35 +0200
Subject: [PATCH] CS7000: avoid spillover of FM audio when switching to
 HR_C6000 DAC output

---
 platform/drivers/audio/audio_CS7000.cpp       | 10 ++++++++--
 platform/drivers/baseband/HR_C6000_CS7000.cpp |  2 +-
 platform/drivers/baseband/radio_CS7000.cpp    |  7 +++++--
 3 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/platform/drivers/audio/audio_CS7000.cpp b/platform/drivers/audio/audio_CS7000.cpp
index f05d7bc0..b2307298 100644
--- a/platform/drivers/audio/audio_CS7000.cpp
+++ b/platform/drivers/audio/audio_CS7000.cpp
@@ -122,9 +122,12 @@ void audio_connect(const enum AudioSource source, const enum AudioSink sink)
             gpioDev_set(INT_MIC_SEL);
             break;
 
-        case PATH(SOURCE_MCU, SINK_SPK):
         case PATH(SOURCE_RTX, SINK_SPK):
             radio_enableAfOutput();
+            // Fallthrough
+
+        case PATH(SOURCE_MCU, SINK_SPK):
+            gpioDev_set(AF_MUTE);
             break;
 
         default:
@@ -159,9 +162,12 @@ void audio_disconnect(const enum AudioSource source, const enum AudioSink sink)
             gpioDev_clear(INT_MIC_SEL);
             break;
 
-        case PATH(SOURCE_MCU, SINK_SPK):
         case PATH(SOURCE_RTX, SINK_SPK):
             radio_disableAfOutput();
+            // Fallthrough
+
+        case PATH(SOURCE_MCU, SINK_SPK):
+            gpioDev_clear(AF_MUTE);
             break;
 
         default:
diff --git a/platform/drivers/baseband/HR_C6000_CS7000.cpp b/platform/drivers/baseband/HR_C6000_CS7000.cpp
index 98d40721..c2b81045 100644
--- a/platform/drivers/baseband/HR_C6000_CS7000.cpp
+++ b/platform/drivers/baseband/HR_C6000_CS7000.cpp
@@ -201,6 +201,7 @@ void HR_Cx000< M >::fmMode()
     writeReg(M::CONFIG, 0x36, 0x10); // Vocoder codec packet interface enabled
     writeReg(M::CONFIG, 0x36, 0x12); // Receiving and opening the voice channel in FM mode Codec switch, 1 means on, 0 means off.
     writeReg(M::CONFIG, 0xE0, 0x8B);
+    writeReg(M::CONFIG, 0x26, 0xFE); // Undocumented register, disable FM audio output
 }
 
 template< class M >
@@ -221,5 +222,4 @@ void HR_Cx000< M >::stopAnalogTx()
     writeReg(M::CONFIG, 0x60, 0x00);    // Stop analog transmission
     writeReg(M::CONFIG, 0xE0, 0xC9);    // Codec enabled, LineIn1, LineOut2, I2S slave mode
     writeReg(M::CONFIG, 0x34, 0x98);    // FM bpf enabled, 25kHz bandwidth
-    writeReg(M::CONFIG, 0x26, 0xFD);    // Undocumented register, enable FM receive
 }
diff --git a/platform/drivers/baseband/radio_CS7000.cpp b/platform/drivers/baseband/radio_CS7000.cpp
index 07d41558..ba49a85a 100644
--- a/platform/drivers/baseband/radio_CS7000.cpp
+++ b/platform/drivers/baseband/radio_CS7000.cpp
@@ -184,12 +184,15 @@ bool radio_checkRxDigitalSquelch()
 
 void radio_enableAfOutput()
 {
-    gpioDev_set(AF_MUTE);
+    // Undocumented register, bits [1:0] seem to enable/disable FM audio RX.
+    // 0xFD enable FM receive.
+    C6000.writeCfgRegister(0x26, 0xFD);
 }
 
 void radio_disableAfOutput()
 {
-    gpioDev_clear(AF_MUTE);
+    // Undocumented register, disable FM receive
+    C6000.writeCfgRegister(0x26, 0xFE);
 }
 
 void radio_enableRx()