diff --git a/common/led_matrix.c b/common/led_matrix.c
index 2a1e83d2..77e52e48 100644
--- a/common/led_matrix.c
+++ b/common/led_matrix.c
@@ -18,3 +18,32 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include "led_matrix.h"
+
+#define LED_MATRIX_TIMER_TOP F_CPU/(256*64)/LED_MATRIX_ROWS
+
+void led_matrix_init(void)
+{
+    /* Timer1 setup */
+    /* CTC mode */
+    TCCR1B |= (1<<WGM12);
+    /* Clock selelct: clk/1 */
+    TCCR1B |= (1<<CS10);
+    /* Set TOP value */
+    uint8_t sreg = SREG;
+    cli();
+    OCR1AH = (LED_MATRIX_TIMER_TOP >> 8) & 0xFF;
+    OCR1AL = LED_MATRIX_TIMER_TOP & 0xFF;
+    SREG = sreg;
+}
+
+void led_matrix_enable(void)
+{
+    /* Enable Compare Match Interrupt */
+    TIMSK1 |= _BV(OCIE1A);
+}
+
+void led_matrix_disable(void)
+{
+    /* Disable Compare Match Interrupt */
+    TIMSK1 &= ~_BV(OCIE1A);
+}
diff --git a/common/led_matrix.h b/common/led_matrix.h
index f683ff09..eb242446 100644
--- a/common/led_matrix.h
+++ b/common/led_matrix.h
@@ -31,11 +31,17 @@ typedef struct {
     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);
+#else
+#define led_matrix_rows()
+#define led_matrix_cols()
+#define led_matrix_init()
+#endif
 
 #endif