3 files changed, 32 insertions, 20 deletions

diff --git a/firmware/target/arm/as3525/ascodec-as3525.c b/firmware/target/arm/as3525/ascodec-as3525.c
index a8ad9706f8..14c3ee7a36 100644
--- a/firmware/target/arm/as3525/ascodec-as3525.c
+++ b/firmware/target/arm/as3525/ascodec-as3525.c
@@ -675,3 +675,18 @@ void ascodec_init(void)
                                     IFRTC_IRQ_RTC | IRQ_ADC);
 #endif
 }
+
+void ams_i2c_get_debug_cpsr(unsigned int *i2c_cpsr)
+{
+    int oldlevel = disable_interrupt_save(IRQ_FIQ_STATUS);
+    /* must be on to read regs */
+    bool i2c_enabled = bitset32(&CGU_PERI, CGU_I2C_AUDIO_MASTER_CLOCK_ENABLE) &
+                                           CGU_I2C_AUDIO_MASTER_CLOCK_ENABLE;
+
+    *i2c_cpsr = (I2C2_CPSR1<<8 | I2C2_CPSR0);
+
+    if (!i2c_enabled) /* put it back how we found it */
+        bitclr32(&CGU_PERI, CGU_I2C_AUDIO_MASTER_CLOCK_ENABLE);
+
+    restore_irq(oldlevel);
+}
diff --git a/firmware/target/arm/as3525/debug-as3525.c b/firmware/target/arm/as3525/debug-as3525.c
index 24cee12cf4..bb731242aa 100644
--- a/firmware/target/arm/as3525/debug-as3525.c
+++ b/firmware/target/arm/as3525/debug-as3525.c
@@ -57,8 +57,6 @@
 #define CLK_SD_MCLK_MSD      12
 #define CLK_USB              13
 
-#define I2C2_CPSR0      *((volatile unsigned int *)(I2C_AUDIO_BASE + 0x1C))
-#define I2C2_CPSR1      *((volatile unsigned int *)(I2C_AUDIO_BASE + 0x20))
 #define MCI_NAND        *((volatile unsigned long *)(NAND_FLASH_BASE + 0x04))
 #define MCI_SD          *((volatile unsigned long *)(SD_MCI_BASE + 0x04))
 
@@ -79,9 +77,8 @@ static int calc_freq(int clk)
 {
     unsigned int prediv = ((unsigned int)CGU_PROC>>2) & 0x3;
     unsigned int postdiv = ((unsigned int)CGU_PROC>>4) & 0xf;
+    unsigned int u_out_div;
 #if CONFIG_CPU == AS3525
-    int out_div;
-
     switch(clk) {
         /* clk_main = clk_int = 24MHz oscillator */
         case CLK_PLLA:
@@ -89,28 +86,27 @@ static int calc_freq(int clk)
                 return 0;
 
                  /*assume 24MHz oscillator only input available */
-            out_div = ((CGU_PLLA>>13) & 0x3);         /* bits 13:14 */
-            if (out_div == 3)                         /* for 11 NO=4  */
-                out_div=4;
-            if(out_div)                               /*  NO = 0 not allowed */
+            u_out_div = ((CGU_PLLA>>13) & 0x3);         /* bits 13:14 */
+            if (u_out_div == 3)                         /* for 11 NO=4  */
+                u_out_div=4;
+            if(u_out_div)                               /*  NO = 0 not allowed */
                 return ((2 * (CGU_PLLA & 0xff))*CLK_MAIN)/
-                                               (((CGU_PLLA>>8) & 0x1f)*out_div);
+                                               (((CGU_PLLA>>8) & 0x1f)*u_out_div);
             return 0;
        case CLK_PLLB:
             if(CGU_PLLBSUP & (1<<3))
                 return 0;
 
                  /*assume 24MHz oscillator only input available */
-            out_div = ((CGU_PLLB>>13) & 0x3);         /* bits 13:14 */
-            if (out_div == 3)                         /* for 11 NO=4  */
-                out_div=4;
-            if(out_div)                               /*  NO = 0 not allowed */
+            u_out_div = ((CGU_PLLB>>13) & 0x3);         /* bits 13:14 */
+            if (u_out_div == 3)                         /* for 11 NO=4  */
+                u_out_div=4;
+            if(u_out_div)                               /*  NO = 0 not allowed */
                 return ((2 * (CGU_PLLB & 0xff))*CLK_MAIN)/
-                                               (((CGU_PLLB>>8) & 0x1f)*out_div);
+                                               (((CGU_PLLB>>8) & 0x1f)*u_out_div);
             return 0;
 #else
     int od, f, r;
-
     /* AS3525v2  */
     switch(clk) {
         case CLK_PLLA:
@@ -182,7 +178,8 @@ static int calc_freq(int clk)
                     return 0;
             }
         case CLK_I2C:
-            return calc_freq(CLK_PCLK)/AS3525_I2C_PRESCALER;
+            ams_i2c_get_debug_cpsr(&u_out_div);
+            return calc_freq(CLK_PCLK)/(u_out_div);
         case CLK_I2SI:
             switch((CGU_AUDIO>>12) & 3) {
                 case 0:
@@ -395,9 +392,9 @@ bool dbg_hw_info(void)
         lcd_clear_display();
         line = 0;
 #endif  /*  LCD_HEIGHT < 176 */
-
-        lcd_putsf(0, line++, "I2C2_CPSR :%8x", (unsigned int)(I2C2_CPSR1<<8 |
-                                                       I2C2_CPSR0));
+        unsigned int i2c_cpsr;
+        ams_i2c_get_debug_cpsr(&i2c_cpsr);
+        lcd_putsf(0, line++, "I2C2_CPSR :%8x", i2c_cpsr);
 #if CONFIG_CPU == AS3525
         lcd_putsf(0, line++, "MCI_NAND  :%8x", (unsigned int)(MCI_NAND));
         lcd_putsf(0, line++, "MCI_SD    :%8x", (unsigned int)(MCI_SD));
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index aca30e52e1..5cdc573a1b 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -96,6 +96,6 @@ struct ams_sd_debug_info
 };
 
 void ams_sd_get_debug_info(struct ams_sd_debug_info *info);
-
+void ams_i2c_get_debug_cpsr(unsigned int *i2c_cpsr);
 
 #endif /* SYSTEM_TARGET_H */