3 files changed, 25 insertions, 9 deletions
diff --git a/src/leddrv.c b/src/leddrv.c
index 9eeba05..3d4f486 100644
--- a/src/leddrv.c
+++ b/src/leddrv.c
@@ -1,7 +1,9 @@
 #include "leddrv.h"
 
-#define LED_DRIVE_STRENTH 0
 #define LED_PINCOUNT (23)
+
+volatile int drive_strength;
+
 typedef enum {
 	FLOATING,
 	LOW,
@@ -28,15 +30,20 @@ static void gpio_buf_set(pinctrl_t pinctl, tristate_t state)
 	}
 }
 
+void led_setDriveStrength(int is_20mA)
+{
+	drive_strength = is_20mA;
+}
+
 static void gpio_buf_apply(
 				volatile uint8_t *gpio_base, 
 				uint32_t *port, uint32_t *cfg, 
 				uint32_t *mask)
 {
-#if LED_DRIVE_STRENTH != 0
-	uint32_t *drv = (uint32_t *)(gpio_base + GPIO_PD_DRV);
-	*drv = (*drv & ~*mask) | (*cfg & *mask);
-#endif
+	if (drive_strength) {
+		uint32_t *drv = (uint32_t *)(gpio_base + GPIO_PD_DRV);
+		*drv = (*drv & ~*mask) | (*cfg & *mask);
+	}
 	uint32_t *dir = (uint32_t *)(gpio_base + GPIO_DIR);
 	*dir = (*dir & ~*mask) | (*cfg & *mask);
 
diff --git a/src/leddrv.h b/src/leddrv.h
index b9be6ba..7474363 100644
--- a/src/leddrv.h
+++ b/src/leddrv.h
@@ -10,5 +10,6 @@ void led_init();
 void leds_releaseall();
 void led_write2dcol(int dcol, uint16_t col1_val, uint16_t col2_val);
 void led_write2row_raw(int row, int which_half, uint32_t val);
+void led_setDriveStrength(int is_20mA);
 
 #endif /* __LEDDRV_H__ */
diff --git a/src/main.c b/src/main.c
index ae818a0..c256cd1 100644
--- a/src/main.c
+++ b/src/main.c
@@ -57,6 +57,7 @@ __HIGH_CODE
 static void change_brightness()
 {
 	NEXT_STATE(brightness, 0, BRIGHTNESS_LEVELS);
+	led_setDriveStrength(brightness / 2);
 }
 
 __HIGH_CODE
@@ -95,7 +96,7 @@ int main()
 	draw2fb(fb[1], 3, 8*12);
 	fb_num = 2;
 
-	TMR0_TimerInit(SCAN_T);
+	TMR0_TimerInit(SCAN_T / 2);
 	TMR0_ITCfg(ENABLE, TMR0_3_IT_CYC_END);
 	PFIC_EnableIRQ(TMR0_IRQn);
 
@@ -124,7 +125,7 @@ void TMR0_IRQHandler(void)
 
 	if (TMR0_GetITFlag(TMR0_3_IT_CYC_END)) {
 
-		i += 2;
+		i += 1;
 		if (i >= LED_COLS) {
 			i = 0;
 			scroll++;
@@ -132,8 +133,15 @@ void TMR0_IRQHandler(void)
 				scroll = 0;
 			}
 		}
-		// This is a mess
-		led_write2dcol(i/2, fb[fb_sel][i+scroll/SCROLL_IRATIO], fb[fb_sel][i+scroll/SCROLL_IRATIO+1]);
+		
+		if (i % 2) {
+			if ((brightness + 1) % 2) 
+				leds_releaseall();
+		} else {
+			led_write2dcol(i/2, 
+				fb[fb_sel][i+scroll/SCROLL_IRATIO], 
+				fb[fb_sel][i+scroll/SCROLL_IRATIO+1]);
+		}
 
 		TMR0_ClearITFlag(TMR0_3_IT_CYC_END);
 	}