From fe500463fd1e511487ffaa1505ad1b61628d0021 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@gmail.com>
Date: Sun, 4 Jan 2015 23:39:08 +0100
Subject: [PATCH 1/2] Quick fix for #1970 - I don't have any 2561 for testing,
 I let you test and fix if needed!

---
 radio/src/targets/stock/board_stock.cpp | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/radio/src/targets/stock/board_stock.cpp b/radio/src/targets/stock/board_stock.cpp
index c7114a979..919b2858c 100644
--- a/radio/src/targets/stock/board_stock.cpp
+++ b/radio/src/targets/stock/board_stock.cpp
@@ -130,10 +130,14 @@ inline void boardInit()
   #endif
 #elif defined(PWM_BACKLIGHT)
   /** Smartieparts LED Backlight is connected to PORTB/pin7, which can be used as pwm output of timer2 **/
-  #if defined(SP22)
-    TCCR2  = (0b011 << CS20)|(1<<WGM20)|(1<<COM21)|(1<<COM20); // inv. pwm mode, clk/64
+  #if defined(CPUM2561)
+    TCCR0A = (1<<WGM00)|(1<<COM0A0);
+    TCCR0B = (0b111<<CS00);
+    DDRB |= (1<<DDB7);
+  #elif defined(SP22)
+    TCCR2  = (0b011<<CS20)|(1<<WGM20)|(1<<COM21)|(1<<COM20); // inv. pwm mode, clk/64
   #else
-    TCCR2  = (0b011 << CS20)|(1<<WGM20)|(1<<COM21); // pwm mode, clk/64
+    TCCR2  = (0b011<<CS20)|(1<<WGM20)|(1<<COM21); // pwm mode, clk/64
   #endif
   #if !defined(CPUM2561)
     TIMSK |= (1<<OCIE0) | (1<<TOIE0); // Enable Output-Compare and Overflow interrrupts
@@ -326,13 +330,20 @@ bool getBackLightState()
   return (bl_target==g_eeGeneral.blOnBright);
 }
 
-void backlightFade() //called from per10ms()
+void backlightFade() // called from per10ms()
 {
   if (bl_target != bl_current) {
+#if defined(CPUM2561)
+    if (bl_target > bl_current)
+      OCR0A = pgm_read_byte(&pwmtable[++bl_current]);
+    else
+      OCR0A = pgm_read_byte(&pwmtable[--bl_current]);
+#else
     if (bl_target > bl_current)
       OCR2 = pgm_read_byte(&pwmtable[++bl_current]);
     else
       OCR2 = pgm_read_byte(&pwmtable[--bl_current]);
+#endif
   }
 }
 

From a7179f5b77ab2b4bdad60477055893ca6a13f865 Mon Sep 17 00:00:00 2001
From: Ken <Ken@vb>
Date: Thu, 8 Jan 2015 00:52:21 -0500
Subject: [PATCH 2/2] Tested fix for PWM_BACKLIGHT on ATmega2561 MCUs

The functionality of timer0 and timer2 are switched between ATmega64/128
and 641/1281/2561. When compiled for ATmega2561, this commit sets up timer0
in the same way as timer2 on ATmega64/128 (phase correct PWM, clk/64 count
rate, inverted if SP22 is defined).

The previous quick fix had a typo in the initialization of TCCR0B (0b111 is
an invalid value for the clock select bits CS2:0). It also did not respect
the SP22 define which indicates the PWM must be inverted. Smartieparts
v2.2 or later programmer boards have an inverted backlight driver. Finally,
it is unnecessary to set DDB7 as it is already set in boardInit() (line
101).
---
 radio/src/targets/stock/board_stock.cpp | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/radio/src/targets/stock/board_stock.cpp b/radio/src/targets/stock/board_stock.cpp
index 919b2858c..6ff848a10 100644
--- a/radio/src/targets/stock/board_stock.cpp
+++ b/radio/src/targets/stock/board_stock.cpp
@@ -131,15 +131,18 @@ inline void boardInit()
 #elif defined(PWM_BACKLIGHT)
   /** Smartieparts LED Backlight is connected to PORTB/pin7, which can be used as pwm output of timer2 **/
   #if defined(CPUM2561)
-    TCCR0A = (1<<WGM00)|(1<<COM0A0);
-    TCCR0B = (0b111<<CS00);
-    DDRB |= (1<<DDB7);
-  #elif defined(SP22)
-    TCCR2  = (0b011<<CS20)|(1<<WGM20)|(1<<COM21)|(1<<COM20); // inv. pwm mode, clk/64
+    #if defined(SP22)
+      TCCR0A = (1<<WGM00)|(1<<COM0A1)|(1<<COM0A0); // inv. pwm mode, clk/64
+    #else
+      TCCR0A = (1<<WGM00)|(1<<COM0A1); // pwm mode, clk/64
+    #endif
+    TCCR0B = (0b011<<CS00);
   #else
-    TCCR2  = (0b011<<CS20)|(1<<WGM20)|(1<<COM21); // pwm mode, clk/64
-  #endif
-  #if !defined(CPUM2561)
+    #if defined(SP22)
+      TCCR2  = (0b011<<CS20)|(1<<WGM20)|(1<<COM21)|(1<<COM20); // inv. pwm mode, clk/64
+    #else
+      TCCR2  = (0b011<<CS20)|(1<<WGM20)|(1<<COM21); // pwm mode, clk/64
+    #endif
     TIMSK |= (1<<OCIE0) | (1<<TOIE0); // Enable Output-Compare and Overflow interrrupts
   #endif
 #else