diff --git a/common.mk b/common.mk
index c8809c0c..bef00a3c 100644
--- a/common.mk
+++ b/common.mk
@@ -69,6 +69,11 @@ ifdef KEYMAP_EX_ENABLE
     OPT_DEFS += -DKEYMAP_EX_ENABLE
 endif
 
+ifdef LED_MATRIX_ENABLE
+    SRC += $(COMMON_DIR)/led_matrix.c
+    OPT_DEFS += -DLED_MATRIX_ENABLE
+endif
+
 # Version string
 OPT_DEFS += -DVERSION=$(shell (git describe --always --dirty || echo 'unknown') 2> /dev/null)
 
diff --git a/common/keyboard.c b/common/keyboard.c
index 48a67e4a..c5de65ef 100644
--- a/common/keyboard.c
+++ b/common/keyboard.c
@@ -62,6 +62,11 @@ void keyboard_init(void)
 {
     timer_init();
     matrix_init();
+
+#ifdef LED_MATRIX_ENABLE
+    led_matrix_init();
+#endif
+
 #ifdef PS2_MOUSE_ENABLE
     ps2_mouse_init();
 #endif
diff --git a/common/led_matrix.c b/common/led_matrix.c
index 77e52e48..e5a4629f 100644
--- a/common/led_matrix.c
+++ b/common/led_matrix.c
@@ -17,12 +17,17 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include <util/delay.h>
 #include "led_matrix.h"
 
 #define LED_MATRIX_TIMER_TOP F_CPU/(256*64)/LED_MATRIX_ROWS
 
+static led_matrix_element_t led_matrix[LED_MATRIX_ROWS][LED_MATRIX_COLS];
+
 void led_matrix_init(void)
 {
+    led_matrix_unselect_rows();
+    led_matrix_write_cols(0);
     /* Timer1 setup */
     /* CTC mode */
     TCCR1B |= (1<<WGM12);
@@ -47,3 +52,40 @@ void led_matrix_disable(void)
     /* Disable Compare Match Interrupt */
     TIMSK1 &= ~_BV(OCIE1A);
 }
+
+inline 
+led_matrix_row_t led_matrix_make_cols(uint8_t row, uint8_t pwm)
+{
+    led_matrix_row_t cols = 0;
+    for (uint8_t i = 0; i < LED_MATRIX_COLS; i++) {
+        cols |= ((led_matrix[row][i].value < pwm ? 1 : 0) << i);
+    }
+    return cols;
+}
+
+inline
+void led_matrix_set_value(uint8_t row, uint8_t col, uint8_t value)
+{
+    led_matrix[row][col].value = value;
+}
+
+inline
+void led_matrix_set_delta(uint8_t row, uint8_t col, int8_t delta)
+{
+    led_matrix[row][col].delta = delta;
+}
+
+ISR(TIMER1_COMPA_vect)
+{
+    static uint8_t row = 0;
+    static uint8_t pwm = 0;
+
+    row = (row + 1) % LED_MATRIX_ROWS;
+    pwm++;
+
+    led_matrix_select_row(row);
+    _delay_us(10);
+    led_matrix_write_cols(led_matrix_make_cols(row, pwm));
+    _delay_us(10);
+    led_matrix_unselect_rows();
+}
diff --git a/common/led_matrix.h b/common/led_matrix.h
index eb242446..513f2c8d 100644
--- a/common/led_matrix.h
+++ b/common/led_matrix.h
@@ -21,27 +21,45 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <stdint.h>
 #include <stdbool.h>
 
+#if (LED_MATRIX_COLS <= 8)
+typedef  uint8_t    led_matrix_row_t;
+#elif (LED_MATRIX_COLS <= 16)
+typedef  uint16_t   led_matrix_row_t;
+#elif (LED_MATRIX_COLS <= 32)
+typedef  uint32_t   led_matrix_row_t;
+#else
+#error "LED_MATRIX_COLS: invalid value"
+#endif
+
 typedef struct {
     union {
         int8_t delta;
         struct {
             bool direction:1;
-        }
-    }
+        };
+    };
     uint8_t value;
 } led_matrix_element_t;
 
 #ifdef LED_MATRIX_ENABLE
-uint8_t led_matrix_rows(void);
-uint8_t led_matrix_cols(void);
 void led_matrix_init(void);
 void led_matrix_enable(void);
 void led_matrix_disable(void);
-uint8_t led_matrix_scan(void);
+void led_matrix_init_cols(void);
+led_matrix_row_t led_matrix_make_cols(uint8_t row, uint8_t pwm);
+void led_matrix_set_value(uint8_t row, uint8_t col, uint8_t value);
+void led_matrix_set_delta(uint8_t row, uint8_t col, int8_t delta);
+extern void led_matrix_write_cols(led_matrix_row_t cols);
+extern void led_matrix_unselect_rows(void);
+extern void led_matrix_select_row(uint8_t row);
 #else
-#define led_matrix_rows()
-#define led_matrix_cols()
 #define led_matrix_init()
+#define led_matrix_enable()
+#define led_matrix_disable()
+#define led_matrix_init_cols()
+#define led_matrix_write_cols()
+#define led_matrix_unselect_rows()
+#define led_matrix_select_row()
 #endif
 
 #endif