From 91e7807af61ba8a907f7eca8952efcb91ec81935 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sun, 5 Jun 2016 10:29:09 -0400
Subject: [PATCH] avr: Add config option to clear the CPU prescaler

Some AVR chips ship with a 1/8th clock divisor set.  Add a compile
time option to manually clear this field at startup.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/avr/Kconfig | 14 ++++++++++++++
 src/avr/timer.c |  9 +++++++++
 2 files changed, 23 insertions(+)

diff --git a/src/avr/Kconfig b/src/avr/Kconfig
index 5056e86a..24e33812 100644
--- a/src/avr/Kconfig
+++ b/src/avr/Kconfig
@@ -44,6 +44,20 @@ config CLOCK_FREQ
     default 16000000 if AVR_FREQ_16000000
     default 20000000 if AVR_FREQ_20000000
 
+config CLEAR_PRESCALER
+    bool "Manually clear the CPU prescaler field at startup"
+    default n
+    help
+         Some AVR chips ship with a "clock prescaler" that causes the
+         chip to run at 1/8th speed. Enable this setting to clear the
+         prescaler field at startup which will cause the chip to run
+         without a clock divisor.
+
+config AVR_CLKPR
+    int
+    default 0 if CLEAR_PRESCALER
+    default -1
+
 config AVR_STACK_SIZE
     int
     default 256 if MACH_atmega2560
diff --git a/src/avr/timer.c b/src/avr/timer.c
index 538f0b19..31f94caf 100644
--- a/src/avr/timer.c
+++ b/src/avr/timer.c
@@ -5,6 +5,7 @@
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
 #include <avr/interrupt.h> // TCNT1
+#include "autoconf.h" // CONFIG_AVR_CLKPR
 #include "command.h" // shutdown
 #include "irq.h" // irq_save
 #include "sched.h" // sched_timer_kick
@@ -49,6 +50,14 @@ ISR(TIMER1_COMPA_vect)
 static void
 timer_init(void)
 {
+    if (CONFIG_AVR_CLKPR != -1 && (uint8_t)CONFIG_AVR_CLKPR != CLKPR) {
+        // Program the clock prescaler
+        uint8_t flag = irq_save();
+        CLKPR = 0x80;
+        CLKPR = CONFIG_AVR_CLKPR;
+        irq_restore(flag);
+    }
+
     // no outputs
     TCCR1A = 0;
     // Normal Mode