2 files changed, 24 insertions, 4 deletions

diff --git a/firmware/target/arm/as3525/ata_sd_as3525.c b/firmware/target/arm/as3525/ata_sd_as3525.c
index 92dc0b746b..57dc307e7f 100644
--- a/firmware/target/arm/as3525/ata_sd_as3525.c
+++ b/firmware/target/arm/as3525/ata_sd_as3525.c
@@ -812,6 +812,11 @@ void sd_enable(bool on)
 #if defined(HAVE_BUTTON_LIGHT) && defined(HAVE_MULTIDRIVE)
     extern int buttonlight_is_on;
 #endif
+
+#ifdef HAVE_HOTSWAP
+    static bool cpu_boosted = false;
+#endif
+
     if (sd_enabled == on)
         return; /* nothing to do */
     if(on)
@@ -830,6 +835,14 @@ void sd_enable(bool on)
         CGU_IDE |= (1<<7)  /* AHB interface enable */  |
                    (1<<6)  /* interface enable */;
         sd_enabled = true;
+
+#ifdef HAVE_HOTSWAP
+        if(card_detect_target())  /* If SD card present Boost cpu for voltage */
+        {
+            cpu_boosted = true;
+            cpu_boost(true);
+        }
+#endif
     }
     else
     {
@@ -844,6 +857,14 @@ void sd_enable(bool on)
 #endif /* HAVE_MULTIDRIVE */
         CGU_IDE &= ~((1<<7)|(1<<6));
         sd_enabled = false;
+
+#ifdef HAVE_HOTSWAP
+        if(cpu_boosted)
+        {
+            cpu_boost(false);
+            cpu_boosted = false;
+        }
+#endif
     }
 }
 
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index d68f2c4f00..ba817b4ad3 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -335,14 +335,13 @@ void set_cpu_frequency(long frequency)
 {
     if(frequency == CPUFREQ_MAX)
     {
-#ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
         /* Increasing frequency so boost voltage before change */
         ascodec_write(AS3514_CVDD_DCDC3, (AS314_CP_DCDC3_SETTING | CVDD_1_20));
 
         /* Wait for voltage to be at least 1.20v before making fclk > 200 MHz */
         while(adc_read(ADC_CVDD) < 480) /* 480 * .0025 = 1.20V */
             ;
-#endif
+
         asm volatile(
             "mrc p15, 0, r0, c1, c0  \n"
 
@@ -365,10 +364,10 @@ void set_cpu_frequency(long frequency)
             "bic r0, r0, #3<<30      \n"     /* fastbus clocking */
             "mcr p15, 0, r0, c1, c0  \n"
             : : : "r0" );
-#ifdef HAVE_ADJUSTABLE_CPU_VOLTAGE
+
         /* Decreasing frequency so reduce voltage after change */
         ascodec_write(AS3514_CVDD_DCDC3, (AS314_CP_DCDC3_SETTING | CVDD_1_10));
-#endif
+
         cpu_frequency = CPUFREQ_NORMAL;
     }
 }