diff --git a/led1642gw.c b/led1642gw.c
index 23af0cf..7a96c69 100644
--- a/led1642gw.c
+++ b/led1642gw.c
@@ -29,7 +29,7 @@
  * the data registers of the LED1642 ICs.
  */
 static uint16_t ledbuffer[NUM_LED_CHANNELS];
-static uint16_t config_reg[3];
+static uint16_t config_reg[NUM_LED1642GW_ICs];
 
 /*
  * Write 16 bits of \data, with LE set high
@@ -103,7 +103,7 @@ static void write_no_command(uint16_t data)
  * Write data to CONFIG register.
  * that means setting LE high for 7 clock cycles
  */
-void led1642gw_flush_config(void)
+void led1642gw_flush_config()
 {
   uint8_t ic;
   for (ic=0; ic<(NUM_LED1642GW_ICs-1); ic++) {
@@ -141,6 +141,29 @@ void led1642gw_turn_all_off(void)
 }
 
 
+void led1642gw_set_gain(uint8_t gain)
+{
+
+  if (gain > 0x3f) {
+    gain = 0x3f;
+  }
+  
+  for (uint8_t ic=0; ic<(NUM_LED1642GW_ICs-1); ic++) {
+    config_reg[ic] &= ~(0x003f);
+    config_reg[ic] |=  gain;
+  }
+
+}
+
+
+void led1642gw_set_current_mode(uint8_t mode)
+{
+  for (uint8_t ic=0; ic<(NUM_LED1642GW_ICs-1); ic++) {
+    if (mode) { config_reg[ic] |= (1<<6); }
+    else { config_reg[ic] &= ~(1<<6); }
+  }
+}
+
 /*
  * Initialize the pins of the ATMega processor
  * to drive the data signals to the ICs
diff --git a/led1642gw.h b/led1642gw.h
index 088ccfe..233665d 100644
--- a/led1642gw.h
+++ b/led1642gw.h
@@ -23,6 +23,8 @@ void led1642gw_turn_all_off(void);
 void led1642gw_flush(void);
 void led1642gw_set_channel(uint8_t channel, uint16_t value);
 void led1642gw_clear(void);
-
+void led1642gw_flush_config(void);
+void led1642gw_set_gain(uint8_t gain);
+void led1642gw_set_current_mode(uint8_t mode);
 
 #endif // LED1642GW_H_
diff --git a/ledcontroller.c b/ledcontroller.c
index 87c693a..977f2fe 100644
--- a/ledcontroller.c
+++ b/ledcontroller.c
@@ -168,3 +168,22 @@ void led_turn_all_on(void)
 	led1642gw_turn_all_on();
 }
 
+
+void led_set_gain(uint8_t gain)
+{
+  if (gain > 0x3f) {
+    gain = 0x3f;
+  }
+  
+  led1642gw_set_gain(gain);
+  led1642gw_flush_config();
+}
+
+
+void led_set_current_mode(uint8_t mode)
+{
+  led1642gw_set_current_mode(mode);
+  led1642gw_flush_config();
+}
+
+
diff --git a/ledcontroller.h b/ledcontroller.h
index e13e2a2..934ba2c 100644
--- a/ledcontroller.h
+++ b/ledcontroller.h
@@ -21,7 +21,8 @@ void led_set(uint8_t lednum, uint16_t red, uint16_t green, uint16_t blue);
 void led_flush(void);
 void led_clear(void);
 void led_turn_all_on(void);
-
+void led_turn_all_off(void);
+void led_set_gain(uint8_t gain);
 
 #endif // LEDCONTROLLER_H
 
diff --git a/rgbyteclock.c b/rgbyteclock.c
index 5fcf1bf..25e6ceb 100644
--- a/rgbyteclock.c
+++ b/rgbyteclock.c
@@ -4,7 +4,7 @@
 
 void rgbyteclock(void)
 {
-	
+  uint8_t gain = 0;
 	while (1) {
 		
 		led_set(0, 4000, 0, 0);
@@ -13,6 +13,13 @@ void rgbyteclock(void)
 
 		led_set(1, 0, 10000, 0);
 		led_flush();
+		timer_wait(500);
+		led_set_gain(gain);
+		gain += 5;
+		if (gain > 0x3f) {
+		  gain = 0;
+		}
+		PORTC ^= (1<<PC5); // Test LED
 	}
 
 }