4 files changed, 145 insertions, 18 deletions

diff --git a/firmware/target/arm/as3525/debug-as3525.c b/firmware/target/arm/as3525/debug-as3525.c
index 9f7b46df8e..06c22c027c 100644
--- a/firmware/target/arm/as3525/debug-as3525.c
+++ b/firmware/target/arm/as3525/debug-as3525.c
@@ -256,6 +256,21 @@ bool __dbg_hw_info(void)
     {
         while(1)
         {
+#ifdef SANSA_C200V2
+        lcd_clear_display();
+        line = 0;
+        lcd_puts(0, line++, "[Submodel:]");
+        lcd_putsf(0, line++, "C200v2 variant %d", c200v2_variant);
+        lcd_update();
+        int btn = button_get(1);
+        if(btn == (DEBUG_CANCEL|BUTTON_REL))
+            goto end;
+        else if(btn == (BUTTON_DOWN|BUTTON_REL))
+            break;
+        }
+        while(1)
+        {
+#endif
         lcd_clear_display();
         line = 0;
         lcd_puts(0, line++, "[Clock Frequencies:]");
diff --git a/firmware/target/arm/as3525/sansa-c200v2/backlight-c200v2.c b/firmware/target/arm/as3525/sansa-c200v2/backlight-c200v2.c
index e094cca8fe..9c236d3653 100644
--- a/firmware/target/arm/as3525/sansa-c200v2/backlight-c200v2.c
+++ b/firmware/target/arm/as3525/sansa-c200v2/backlight-c200v2.c
@@ -37,26 +37,75 @@ static const int brightness_table[MAX_BRIGHTNESS_SETTING+1] = {
 
 static void _ll_backlight_on(void)
 {
-    GPIOA_PIN(5) = 1<<5;
+    if (c200v2_variant == 0) {
+        GPIOA_PIN(5) = 1<<5;
+    } else {
+        GPIOA_PIN(7) = 1<<7;
+    }
 }
 
 static void _ll_backlight_off(void)
 {
-    GPIOA_PIN(5) = 0;
+    if (c200v2_variant == 0) {
+        GPIOA_PIN(5) = 0;
+    } else {
+        GPIOA_PIN(7) = 0;
+    }
+}
+
+static void _ll_buttonlight_on(void)
+{
+    if (c200v2_variant == 1) {
+        /* Main buttonlight is on A5 */
+        GPIOA_PIN(5) = 1<<5;
+    } else {
+        /* Needed for buttonlight and MicroSD to work at the same time */
+        /* Turn ROD control on, as the OF does */
+        GPIOD_DIR |= (1<<7);
+        SD_MCI_POWER |= (1<<7);
+        GPIOD_PIN(7) = (1<<7);
+    }
+}
+
+static void _ll_buttonlight_off(void)
+{
+    if (c200v2_variant == 1) {
+        /* Main buttonlight is on A5 */
+        GPIOA_PIN(5) = 0;
+    } else {
+        /* Needed for buttonlight and MicroSD to work at the same time */
+        /* Turn ROD control off, as the OF does */
+        SD_MCI_POWER &= ~(1<<7);
+        GPIOD_PIN(7) = 0;
+        GPIOD_DIR &= ~(1<<7);
+    }
 }
 
 void _backlight_pwm(int on)
 {
     if (on) {
-        _ll_backlight_on();
+        if (backlight_is_on)
+            _ll_backlight_on();
+
+        if (buttonlight_is_on)
+            _ll_buttonlight_on();
     } else {
-        _ll_backlight_off();
+        if (backlight_is_on)
+            _ll_backlight_off();
+
+        if (buttonlight_is_on)
+            _ll_buttonlight_off();
     }
 }
 
 bool _backlight_init(void)
 {
     GPIOA_DIR |= 1<<5;
+    if (c200v2_variant == 1) {
+        /* On this variant A7 is the backlight and
+         * A5 is the buttonlight */
+        GPIOA_DIR |= 1<<7;
+    }
     return true;
 }
 
@@ -70,21 +119,40 @@ void _backlight_set_brightness(int brightness)
         _backlight_off();
 }
 
+static void _pwm_on(void)
+{
+    _set_timer2_pwm_ratio(backlight_level);
+}
+
+static void _pwm_off(void)
+{
+    if (buttonlight_is_on == 0 && backlight_is_on == 0)
+        _set_timer2_pwm_ratio(0);
+}
+
 void _backlight_on(void)
 {
+    if (backlight_is_on == 1) {
+        /* Update pwm ratio in case user changed the brightness */
+        _pwm_on();
+        return;
+    }
+
 #ifdef HAVE_LCD_ENABLE
     lcd_enable(true); /* power on lcd + visible display */
 #endif
-    if (!backlight_is_on)
-        _ll_backlight_on();
-    _set_timer2_pwm_ratio(backlight_level);
+    _ll_backlight_on();
+    _pwm_on();
     backlight_is_on = 1;
 }
 
 void _backlight_off(void)
 {
+    if (backlight_is_on == 0)
+        return;
+
     backlight_is_on = 0;
-    _set_timer2_pwm_ratio(0);
+    _pwm_off();
     _ll_backlight_off();
 #ifdef HAVE_LCD_ENABLE
     lcd_enable(false); /* power off visible display */
@@ -93,20 +161,22 @@ void _backlight_off(void)
 
 void _buttonlight_on(void)
 {
-    /* Needed for buttonlight and MicroSD to work at the same time */
-    /* Turn ROD control on, as the OF does */
-    GPIOD_DIR |= (1<<7);
-    SD_MCI_POWER |= (1<<7);
-    GPIOD_PIN(7) = (1<<7);
+    if (buttonlight_is_on == 1)
+        return;
+
+    _ll_buttonlight_on();
+    _pwm_on();
     buttonlight_is_on = 1;
 }
 
 void _buttonlight_off(void)
 {
-    /* Needed for buttonlight and MicroSD to work at the same time */
-    /* Turn ROD control off, as the OF does */
-    SD_MCI_POWER &= ~(1<<7);
-    GPIOD_PIN(7) = 0;
-    GPIOD_DIR &= ~(1<<7);
+    if (buttonlight_is_on == 0)
+        return;
+
     buttonlight_is_on = 0;
+    _pwm_off();
+    _ll_buttonlight_off();
 }
+
+/* vim:set ts=4 sw=4 et: */
diff --git a/firmware/target/arm/as3525/system-as3525.c b/firmware/target/arm/as3525/system-as3525.c
index c51d84469f..5a7d25a1f7 100644
--- a/firmware/target/arm/as3525/system-as3525.c
+++ b/firmware/target/arm/as3525/system-as3525.c
@@ -163,6 +163,41 @@ void fiq_handler(void)
     );
 }
 
+#if defined(SANSA_C200V2)
+#include "dbop-as3525.h"
+
+int c200v2_variant = 0;
+
+static void check_model_variant(void)
+{
+    unsigned int i;
+    unsigned int saved_dir = GPIOA_DIR;
+
+    /* Make A7 input */
+    GPIOA_DIR &= ~(1<<7);
+    /* wait a little to allow the pullup/pulldown resistor
+     * to charge the input capacitance */
+    for (i=0; i<1000; i++) asm volatile ("nop\n");
+    /* read the pullup/pulldown value on A7 to determine the variant */
+    if (GPIOA_PIN(7) == 0) {
+        /*
+         * Backlight on A7.
+         */
+        c200v2_variant = 1;
+    } else {
+        /*
+         * Backlight on A5.
+         */
+        c200v2_variant = 0;
+    }
+    GPIOA_DIR = saved_dir;
+}
+#else
+static inline void check_model_variant(void)
+{
+}
+#endif /* SANSA_C200V2*/
+
 #if defined(BOOTLOADER)
 static void sdram_delay(void)
 {
@@ -319,6 +354,7 @@ void system_init(void)
     fmradio_i2c_init();
 #endif
 #endif /* !BOOTLOADER */
+    check_model_variant();
 }
 
 void system_reboot(void)
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index 292ad1bbea..1173515ebb 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -32,4 +32,10 @@
 #define UNCACHED_ADDR(a) ((typeof(a)) ((uintptr_t)(a) + 0x10000000))
 #endif
 
+
+#ifdef SANSA_C200V2
+/* 0: Backlight on A5, 1: Backlight on A7 */
+extern int c200v2_variant;
+#endif
+
 #endif /* SYSTEM_TARGET_H */