From 698499f00df6bc7797cdf8bca212661cfd79f8da Mon Sep 17 00:00:00 2001
From: Voinea Dragos <voinea.dragos.alexandru@gmail.com>
Date: Sun, 31 Jan 2021 17:18:32 +0200
Subject: [PATCH] split timer0 and timer2 initialization. Move timer2 init
 early

---
 Firmware/Marlin_main.cpp |  2 ++
 Firmware/temperature.cpp |  5 ++++-
 Firmware/timer02.c       | 20 ++++++++++++--------
 Firmware/timer02.h       |  3 +++
 4 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index d43e05e5..68ecb1c1 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1013,6 +1013,8 @@ static void w25x20cl_err_msg()
 // are initialized by the main() routine provided by the Arduino framework.
 void setup()
 {
+	timer2_init(); // enables functional millis
+
 	mmu_init();
 
 	ultralcd_init();
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index 3b38c25c..a1281b64 100755
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -1124,7 +1124,10 @@ void tp_init()
 
   adc_init();
 
-  timer0_init();
+  timer0_init(); //enables the heatbed timer.
+
+  // timer2 already enabled earlier in the code
+  // now enable the COMPB temperature interrupt
   OCR2B = 128;
   TIMSK2 |= (1<<OCIE2B);
   
diff --git a/Firmware/timer02.c b/Firmware/timer02.c
index a866fa25..47f7d2a2 100644
--- a/Firmware/timer02.c
+++ b/Firmware/timer02.c
@@ -9,13 +9,11 @@
 
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include "macros.h"
 
 void timer0_init(void)
 {
-	//save sreg
-	uint8_t _sreg = SREG;
-	//disable interrupts for sure
-	cli();
+	CRITICAL_SECTION_START;
 
 	TCNT0  = 0;
 	// Fast PWM duty (0-255). 
@@ -25,7 +23,14 @@ void timer0_init(void)
 	TCCR0A = (1 << WGM01) | (1 << WGM00) | (1 << COM0B1) | (1 << COM0B0);  
 	TCCR0B = (1 << CS01);    // CLK/8 prescaling
 	TIMSK0 |= (1 << TOIE0);  // enable timer overflow interrupt
-	
+
+	CRITICAL_SECTION_END;
+}
+
+void timer2_init(void)
+{
+	CRITICAL_SECTION_START;
+
 	// Everything, that used to be on timer0 was moved to timer2 (delay, beeping, millis etc.)
 	//setup timer2
 	TCCR2A = 0x00; //COM_A-B=00, WGM_0-1=00
@@ -36,9 +41,8 @@ void timer0_init(void)
 	TIMSK2 &= ~(1<<OCIE2B);
 	//set timer2 OCR registers (OCRB interrupt generated 0.5ms after OVF interrupt)
 	OCR2A = 0;
-	OCR2B = 128;
-	//restore sreg (enable interrupts)
-	SREG = _sreg;
+
+	CRITICAL_SECTION_END;
 }
 
 
diff --git a/Firmware/timer02.h b/Firmware/timer02.h
index 2eef98f6..3aaa5236 100644
--- a/Firmware/timer02.h
+++ b/Firmware/timer02.h
@@ -14,6 +14,9 @@ extern "C" {
 ///! Initializes TIMER0 for fast PWM mode-driven bed heating
 extern void timer0_init(void);
 
+///! Initializes TIMER2 for time keeping and temperature interrupt
+extern void timer2_init(void);
+
 ///! Reimplemented original millis() using timer2
 extern unsigned long millis2(void);