From 63af814d4dd82420092a05c29a670487fe43c3da Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sat, 9 Jun 2018 21:28:37 -0500
Subject: [PATCH] Add HAL_timer_start to HAL.h

Co-Authored-By: ejtagle <ejtagle@hotmail.com>
---
 Marlin/HAL.h           | 34 ++++++++++++++++++++++++++++++++--
 Marlin/stepper.cpp     | 16 +---------------
 Marlin/temperature.cpp |  4 +---
 3 files changed, 34 insertions(+), 20 deletions(-)

diff --git a/Marlin/HAL.h b/Marlin/HAL.h
index c7b9a5d69..05bca1c0f 100644
--- a/Marlin/HAL.h
+++ b/Marlin/HAL.h
@@ -100,7 +100,8 @@ inline uint8_t HAL_get_reset_source(void) { return MCUSR; }
 #define STEPPER_TIMER_PRESCALE  8
 #define STEP_TIMER_MIN_INTERVAL 8 // minimum time in µs between stepper interrupts
 
-#define TEMP_TIMER_FREQUENCY    ((F_CPU) / 64.0 / 256.0)
+#define TEMP_TIMER_PRESCALE     64
+#define TEMP_TIMER_FREQUENCY    ((F_CPU) / float(TEMP_TIMER_PRESCALE) / 256.0)
 
 #define TIMER_OCR_1             OCR1A
 #define TIMER_COUNTER_1         TCNT1
@@ -118,7 +119,36 @@ inline uint8_t HAL_get_reset_source(void) { return MCUSR; }
 #define DISABLE_TEMPERATURE_INTERRUPT()    CBI(TIMSK0, OCIE0B)
 #define TEMPERATURE_ISR_ENABLED()         TEST(TIMSK0, OCIE0B)
 
-#define HAL_timer_start(timer_num, frequency)
+FORCE_INLINE void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
+  UNUSED(frequency);
+  switch (timer_num) {
+    case STEP_TIMER_NUM:
+      // waveform generation = 0100 = CTC
+      SET_WGM(1, CTC_OCRnA);
+
+      // output mode = 00 (disconnected)
+      SET_COMA(1, NORMAL);
+
+      // Set the timer pre-scaler
+      // Generally we use a divider of 8, resulting in a 2MHz timer
+      // frequency on a 16MHz MCU. If you are going to change this, be
+      // sure to regenerate speed_lookuptable.h with
+      // create_speed_lookuptable.py
+      SET_CS(1, PRESCALER_8);  //  CS 2 = 1/8 prescaler
+
+      // Init Stepper ISR to 122 Hz for quick starting
+      // (F_CPU) / (STEPPER_TIMER_PRESCALE) / frequency
+      OCR1A = 0x4000;
+      TCNT1 = 0;
+      break;
+
+    case TEMP_TIMER_NUM:
+      // Use timer0 for temperature measurement
+      // Interleave temperature interrupt with millies interrupt
+      OCR0B = 128;
+      break;
+  }
+}
 
 #define _CAT(a, ...) a ## __VA_ARGS__
 #define HAL_timer_set_compare(timer, compare) (_CAT(TIMER_OCR_, timer) = compare)
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 7a201c3f9..c03a58d84 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -1947,22 +1947,8 @@ void Stepper::init() {
     E_AXIS_INIT(4);
   #endif
 
-  // waveform generation = 0100 = CTC
-  SET_WGM(1, CTC_OCRnA);
-
-  // output mode = 00 (disconnected)
-  SET_COMA(1, NORMAL);
-
-  // Set the timer pre-scaler
-  // Generally we use a divider of 8, resulting in a 2MHz timer
-  // frequency on a 16MHz MCU. If you are going to change this, be
-  // sure to regenerate speed_lookuptable.h with
-  // create_speed_lookuptable.py
-  SET_CS(1, PRESCALER_8);  //  CS 2 = 1/8 prescaler
-
   // Init Stepper ISR to 122 Hz for quick starting
-  OCR1A = 0x4000;
-  TCNT1 = 0;
+  HAL_timer_start(STEP_TIMER_NUM, 122); // OCR1A = 0x4000
 
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 0681263f6..fb60b7d44 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -1209,9 +1209,7 @@ void Temperature::init() {
     HAL_ANALOG_SELECT(FILWIDTH_PIN);
   #endif
 
-  // Use timer0 for temperature measurement
-  // Interleave temperature interrupt with millies interrupt
-  OCR0B = 128;
+  HAL_timer_start(TEMP_TIMER_NUM, TEMP_TIMER_FREQUENCY);
   ENABLE_TEMPERATURE_INTERRUPT();
 
   #if HAS_AUTO_FAN_0