diff --git a/Marlin/HAL.h b/Marlin/HAL.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4c2f805a2f65c93fb48e5af72f05e69fc9f16dc
--- /dev/null
+++ b/Marlin/HAL.h
@@ -0,0 +1,305 @@
+/* **************************************************************************
+
+ Marlin 3D Printer Firmware
+ Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+
+ Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+****************************************************************************/
+
+/**
+ * Description: HAL for __AVR__
+ */
+
+#ifndef _HAL_AVR_H_
+#define _HAL_AVR_H_
+
+// --------------------------------------------------------------------------
+// Includes
+// --------------------------------------------------------------------------
+
+#include "fastio.h"
+
+#include <stdint.h>
+#include <Arduino.h>
+#include <util/delay.h>
+#include <avr/eeprom.h>
+#include <avr/pgmspace.h>
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+// --------------------------------------------------------------------------
+// Defines
+// --------------------------------------------------------------------------
+
+//#define analogInputToDigitalPin(IO) IO
+
+// Bracket code that shouldn't be interrupted
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
+  #define CRITICAL_SECTION_END    SREG = _sreg;
+#endif
+
+// --------------------------------------------------------------------------
+// Types
+// --------------------------------------------------------------------------
+
+typedef uint16_t hal_timer_t;
+#define HAL_TIMER_TYPE_MAX 0xFFFF
+
+typedef int8_t pin_t;
+
+#define HAL_SERVO_LIB Servo
+
+// --------------------------------------------------------------------------
+// Public Variables
+// --------------------------------------------------------------------------
+
+//extern uint8_t MCUSR;
+
+// --------------------------------------------------------------------------
+// Public functions
+// --------------------------------------------------------------------------
+
+//void cli(void);
+
+//void _delay_ms(const int delay);
+
+inline void HAL_clear_reset_source(void) { MCUSR = 0; }
+inline uint8_t HAL_get_reset_source(void) { return MCUSR; }
+
+// eeprom
+//void eeprom_write_byte(unsigned char *pos, unsigned char value);
+//unsigned char eeprom_read_byte(unsigned char *pos);
+
+// timers
+#define HAL_TIMER_RATE          ((F_CPU) / 8)    // i.e., 2MHz or 2.5MHz
+
+#define STEP_TIMER_NUM          1
+#define TEMP_TIMER_NUM          0
+#define PULSE_TIMER_NUM         TEMP_TIMER_NUM
+
+#define HAL_STEPPER_TIMER_RATE  HAL_TIMER_RATE
+#define HAL_TICKS_PER_US        ((HAL_STEPPER_TIMER_RATE) / 1000000) // Cannot be of type double
+#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 TIMER_OCR_1             OCR1A
+#define TIMER_COUNTER_1         TCNT1
+
+#define TIMER_OCR_0             OCR0A
+#define TIMER_COUNTER_0         TCNT0
+
+#define PULSE_TIMER_PRESCALE    8
+
+#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
+#define STEPPER_ISR_ENABLED()             TEST(TIMSK1, OCIE1A)
+
+#define ENABLE_TEMPERATURE_INTERRUPT()     SBI(TIMSK0, OCIE0B)
+#define DISABLE_TEMPERATURE_INTERRUPT()    CBI(TIMSK0, OCIE0B)
+#define TEMPERATURE_ISR_ENABLED()         TEST(TIMSK0, OCIE0B)
+
+#define HAL_timer_start(timer_num, frequency)
+
+#define _CAT(a, ...) a ## __VA_ARGS__
+#define HAL_timer_set_compare(timer, compare) (_CAT(TIMER_OCR_, timer) = compare)
+#define HAL_timer_restrain(timer, interval_ticks) NOLESS(_CAT(TIMER_OCR_, timer), _CAT(TIMER_COUNTER_, timer) + interval_ticks)
+
+#define HAL_timer_get_compare(timer) _CAT(TIMER_OCR_, timer)
+#define HAL_timer_get_count(timer) _CAT(TIMER_COUNTER_, timer)
+
+/**
+ * On AVR there is no hardware prioritization and preemption of
+ * interrupts, so this emulates it. The UART has first priority
+ * (otherwise, characters will be lost due to UART overflow).
+ * Then: Stepper, Endstops, Temperature, and -finally- all others.
+ */
+#define HAL_timer_isr_prologue(TIMER_NUM)
+#define HAL_timer_isr_epilogue(TIMER_NUM)
+
+/* 18 cycles maximum latency */
+#define HAL_STEP_TIMER_ISR \
+extern "C" void TIMER1_COMPA_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER1_COMPA_vect_bottom (void) asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER1_COMPA_vect (void) { \
+  __asm__ __volatile__ ( \
+    A("push r16")                      /* 2 Save R16 */ \
+    A("in r16, __SREG__")              /* 1 Get SREG */ \
+    A("push r16")                      /* 2 Save SREG into stack */ \
+    A("lds r16, %[timsk0]")            /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+    A("push r16")                      /* 2 Save TIMSK0 into the stack */ \
+    A("andi r16,~%[msk0]")             /* 1 Disable the temperature ISR */ \
+    A("sts %[timsk0], r16")            /* 2 And set the new value */ \
+    A("lds r16, %[timsk1]")            /* 2 Load into R0 the stepper timer Interrupt mask register [TIMSK1] */ \
+    A("andi r16,~%[msk1]")             /* 1 Disable the stepper ISR */ \
+    A("sts %[timsk1], r16")            /* 2 And set the new value */ \
+    A("sei")                           /* 1 Enable global interrupts - stepper and temperature ISRs are disabled, so no risk of reentry or being preempted by the temperature ISR */    \
+    A("push r16")                      /* 2 Save TIMSK1 into stack */ \
+    A("in r16, 0x3B")                  /* 1 Get RAMPZ register */ \
+    A("push r16")                      /* 2 Save RAMPZ into stack */ \
+    A("in r16, 0x3C")                  /* 1 Get EIND register */ \
+    A("push r0")                       /* C runtime can modify all the following registers without restoring them */ \
+    A("push r1")                       \
+    A("push r18")                      \
+    A("push r19")                      \
+    A("push r20")                      \
+    A("push r21")                      \
+    A("push r22")                      \
+    A("push r23")                      \
+    A("push r24")                      \
+    A("push r25")                      \
+    A("push r26")                      \
+    A("push r27")                      \
+    A("push r30")                      \
+    A("push r31")                      \
+    A("clr r1")                        /* C runtime expects this register to be 0 */ \
+    A("call TIMER1_COMPA_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */   \
+    A("pop r31")                       \
+    A("pop r30")                       \
+    A("pop r27")                       \
+    A("pop r26")                       \
+    A("pop r25")                       \
+    A("pop r24")                       \
+    A("pop r23")                       \
+    A("pop r22")                       \
+    A("pop r21")                       \
+    A("pop r20")                       \
+    A("pop r19")                       \
+    A("pop r18")                       \
+    A("pop r1")                        \
+    A("pop r0")                        \
+    A("out 0x3C, r16")                 /* 1 Restore EIND register */ \
+    A("pop r16")                       /* 2 Get the original RAMPZ register value */ \
+    A("out 0x3B, r16")                 /* 1 Restore RAMPZ register to its original value */ \
+    A("pop r16")                       /* 2 Get the original TIMSK1 value but with stepper ISR disabled */ \
+    A("ori r16,%[msk1]")               /* 1 Reenable the stepper ISR */ \
+    A("cli")                           /* 1 Disable global interrupts - Reenabling Stepper ISR can reenter amd temperature can reenter, and we want that, if it happens, after this ISR has ended */ \
+    A("sts %[timsk1], r16")            /* 2 And restore the old value - This reenables the stepper ISR */ \
+    A("pop r16")                       /* 2 Get the temperature timer Interrupt mask register [TIMSK0] */ \
+    A("sts %[timsk0], r16")            /* 2 And restore the old value - This reenables the temperature ISR */ \
+    A("pop r16")                       /* 2 Get the old SREG value */ \
+    A("out __SREG__, r16")             /* 1 And restore the SREG value */ \
+    A("pop r16")                       /* 2 Restore R16 value */ \
+    A("reti")                          /* 4 Return from interrupt */ \
+    :                                   \
+    : [timsk0] "i" ((uint16_t)&TIMSK0), \
+      [timsk1] "i" ((uint16_t)&TIMSK1), \
+      [msk0] "M" ((uint8_t)(1<<OCIE0B)),\
+      [msk1] "M" ((uint8_t)(1<<OCIE1A)) \
+    : \
+  ); \
+} \
+void TIMER1_COMPA_vect_bottom(void)
+
+/* 14 cycles maximum latency */
+#define HAL_TEMP_TIMER_ISR \
+extern "C" void TIMER0_COMPB_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER0_COMPB_vect_bottom(void)  asm ("TIMER0_COMPB_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER0_COMPB_vect (void) { \
+  __asm__ __volatile__ ( \
+    A("push r16")                       /* 2 Save R16 */ \
+    A("in r16, __SREG__")               /* 1 Get SREG */ \
+    A("push r16")                       /* 2 Save SREG into stack */ \
+    A("lds r16, %[timsk0]")             /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+    A("andi r16,~%[msk0]")              /* 1 Disable the temperature ISR */ \
+    A("sts %[timsk0], r16")             /* 2 And set the new value */ \
+    A("sei")                            /* 1 Enable global interrupts - It is safe, as the temperature ISR is disabled, so we cannot reenter it */    \
+    A("push r16")                       /* 2 Save TIMSK0 into stack */ \
+    A("in r16, 0x3B")                   /* 1 Get RAMPZ register */ \
+    A("push r16")                       /* 2 Save RAMPZ into stack */ \
+    A("in r16, 0x3C")                   /* 1 Get EIND register */ \
+    A("push r0")                        /* C runtime can modify all the following registers without restoring them */ \
+    A("push r1")                        \
+    A("push r18")                       \
+    A("push r19")                       \
+    A("push r20")                       \
+    A("push r21")                       \
+    A("push r22")                       \
+    A("push r23")                       \
+    A("push r24")                       \
+    A("push r25")                       \
+    A("push r26")                       \
+    A("push r27")                       \
+    A("push r30")                       \
+    A("push r31")                       \
+    A("clr r1")                         /* C runtime expects this register to be 0 */ \
+    A("call TIMER0_COMPB_vect_bottom")  /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */   \
+    A("pop r31")                        \
+    A("pop r30")                        \
+    A("pop r27")                        \
+    A("pop r26")                        \
+    A("pop r25")                        \
+    A("pop r24")                        \
+    A("pop r23")                        \
+    A("pop r22")                        \
+    A("pop r21")                        \
+    A("pop r20")                        \
+    A("pop r19")                        \
+    A("pop r18")                        \
+    A("pop r1")                         \
+    A("pop r0")                         \
+    A("out 0x3C, r16")                  /* 1 Restore EIND register */ \
+    A("pop r16")                        /* 2 Get the original RAMPZ register value */ \
+    A("out 0x3B, r16")                  /* 1 Restore RAMPZ register to its original value */ \
+    A("pop r16")                        /* 2 Get the original TIMSK0 value but with temperature ISR disabled */ \
+    A("ori r16,%[msk0]")                /* 1 Enable temperature ISR */ \
+    A("cli")                            /* 1 Disable global interrupts - We must do this, as we will reenable the temperature ISR, and we don´t want to reenter this handler until the current one is done */ \
+    A("sts %[timsk0], r16")             /* 2 And restore the old value */ \
+    A("pop r16")                        /* 2 Get the old SREG */ \
+    A("out __SREG__, r16")              /* 1 And restore the SREG value */ \
+    A("pop r16")                        /* 2 Restore R16 */ \
+    A("reti")                           /* 4 Return from interrupt */ \
+    :                                   \
+    : [timsk0] "i"((uint16_t)&TIMSK0),  \
+      [msk0] "M" ((uint8_t)(1<<OCIE0B)) \
+    : \
+  ); \
+} \
+void TIMER0_COMPB_vect_bottom(void)
+
+// ADC
+#ifdef DIDR2
+  #define HAL_ANALOG_SELECT(pin) do{ if (pin < 8) SBI(DIDR0, pin); else SBI(DIDR2, pin & 0x07); }while(0)
+#else
+  #define HAL_ANALOG_SELECT(pin) do{ SBI(DIDR0, pin); }while(0)
+#endif
+
+inline void HAL_adc_init(void) {
+  ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07;
+  DIDR0 = 0;
+  #ifdef DIDR2
+    DIDR2 = 0;
+  #endif
+}
+
+#define SET_ADMUX_ADCSRA(pin) ADMUX = _BV(REFS0) | (pin & 0x07); SBI(ADCSRA, ADSC)
+#ifdef MUX5
+  #define HAL_START_ADC(pin) if (pin > 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
+#else
+  #define HAL_START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
+#endif
+
+#define HAL_READ_ADC ADC
+
+#define GET_PIN_MAP_PIN(index) index
+#define GET_PIN_MAP_INDEX(pin) pin
+#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval)
+
+#define HAL_SENSITIVE_PINS 0, 1
+
+#endif // _HAL_AVR_H_
diff --git a/Marlin/MarlinConfig.h b/Marlin/MarlinConfig.h
index 5f77dba0e47b1fa78a813fef61f3fa49158228e7..47e6020d9915637e6a200b6e3220b8ef72213b61 100644
--- a/Marlin/MarlinConfig.h
+++ b/Marlin/MarlinConfig.h
@@ -23,21 +23,25 @@
 #ifndef MARLIN_CONFIG_H
 #define MARLIN_CONFIG_H
 
-#include "fastio.h"
-#include "macros.h"
 #include "boards.h"
+#include "macros.h"
 #include "Version.h"
 #include "Configuration.h"
 #include "Conditionals_LCD.h"
 #include "Configuration_adv.h"
-#include "pins.h"
+
 #if defined(__AVR__) && !defined(USBCON)
   #define HardwareSerial_h // trick to disable the standard HWserial
 #endif
-#include "Arduino.h"
+
+#include "types.h"
+#include "HAL.h"
+#include "pins.h"
 #include "Conditionals_post.h"
 #include "SanityCheck.h"
-
-#include <avr/pgmspace.h>
+#include "enum.h"
+#include "language.h"
+#include "utility.h"
+#include "serial.h"
 
 #endif // MARLIN_CONFIG_H
diff --git a/Marlin/endstops.h b/Marlin/endstops.h
index 96cb3d089c20f5b467a56d485d875d2cf15ce324..fdaf02ea7f63b0838f6145712939a96ae23ea305 100644
--- a/Marlin/endstops.h
+++ b/Marlin/endstops.h
@@ -27,7 +27,6 @@
 #ifndef __ENDSTOPS_H__
 #define __ENDSTOPS_H__
 
-#include "enum.h"
 #include "MarlinConfig.h"
 
 class Endstops {
diff --git a/Marlin/fastio.h b/Marlin/fastio.h
index 03a3a903663df20b77509b40f49efe4fdb0e4e9a..839db9929379fca73ca3379598f8224b81b32e5d 100644
--- a/Marlin/fastio.h
+++ b/Marlin/fastio.h
@@ -28,7 +28,6 @@
 
 #include <stdint.h>
 
-typedef int8_t pin_t;
 #ifndef _FASTIO_ARDUINO_H_
 #define _FASTIO_ARDUINO_H_
 
diff --git a/Marlin/macros.h b/Marlin/macros.h
index 6b8e8781259ae137afaec0583d62055ce73c5135..1f72ba38d0a713d1d33160f8e0bb9409898222b8 100644
--- a/Marlin/macros.h
+++ b/Marlin/macros.h
@@ -47,12 +47,6 @@
 #define _O2          __attribute__((optimize("O2")))
 #define _O3          __attribute__((optimize("O3")))
 
-// Bracket code that shouldn't be interrupted
-#ifndef CRITICAL_SECTION_START
-  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
-  #define CRITICAL_SECTION_END    SREG = _sreg;
-#endif
-
 // Clock speed factors
 #define CYCLES_PER_MICROSECOND (F_CPU / 1000000L) // 16 or 20
 #define INT0_PRESCALER 8
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 4791661404033b652e433a77c8dc5dba8755ab71..1a2eefe3bba04f872c29ddbc2210dd234d3d813e 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -1394,7 +1394,7 @@ void Stepper::isr() {
      * 10µs = 160 or 200 cycles.
      */
     #if EXTRA_CYCLES_XYZE > 20
-      uint32_t pulse_start = TCNT0;
+      hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
     #endif
 
     #if HAS_X_STEP
@@ -1459,8 +1459,8 @@ void Stepper::isr() {
 
     // For minimum pulse time wait before stopping pulses
     #if EXTRA_CYCLES_XYZE > 20
-      while (EXTRA_CYCLES_XYZE > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
-      pulse_start = TCNT0;
+      while (EXTRA_CYCLES_XYZE > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
+      pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
     #elif EXTRA_CYCLES_XYZE > 0
       DELAY_NS(EXTRA_CYCLES_XYZE * NANOSECONDS_PER_CYCLE);
     #endif
@@ -1495,7 +1495,7 @@ void Stepper::isr() {
 
     // For minimum pulse time wait after stopping pulses also
     #if EXTRA_CYCLES_XYZE > 20
-      if (i) while (EXTRA_CYCLES_XYZE > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
+      if (i) while (EXTRA_CYCLES_XYZE > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
     #elif EXTRA_CYCLES_XYZE > 0
       if (i) DELAY_NS(EXTRA_CYCLES_XYZE * NANOSECONDS_PER_CYCLE);
     #endif
@@ -1736,8 +1736,8 @@ void Stepper::isr() {
 
       // For minimum pulse time wait before stopping pulses
       #if EXTRA_CYCLES_E > 20
-        while (EXTRA_CYCLES_E > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
-        pulse_start = TCNT0;
+        while (EXTRA_CYCLES_E > (hal_timer_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
+        pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
       #elif EXTRA_CYCLES_E > 0
         DELAY_NS(EXTRA_CYCLES_E * NANOSECONDS_PER_CYCLE);
       #endif
@@ -1760,7 +1760,7 @@ void Stepper::isr() {
 
       // For minimum pulse time wait before looping
       #if EXTRA_CYCLES_E > 20
-        if (e_steps) while (EXTRA_CYCLES_E > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
+        if (e_steps) while (EXTRA_CYCLES_E > (hal_timer_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
       #elif EXTRA_CYCLES_E > 0
         if (e_steps) DELAY_NS(EXTRA_CYCLES_E * NANOSECONDS_PER_CYCLE);
       #endif
@@ -2056,11 +2056,16 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
 }
 
 void Stepper::report_positions() {
-  CRITICAL_SECTION_START;
+
+  // Protect the access to the position.
+  const bool was_enabled = STEPPER_ISR_ENABLED();
+  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
   const int32_t xpos = count_position[X_AXIS],
                 ypos = count_position[Y_AXIS],
                 zpos = count_position[Z_AXIS];
-  CRITICAL_SECTION_END;
+
+  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 
   #if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA
     SERIAL_PROTOCOLPGM(MSG_COUNT_A);
@@ -2101,8 +2106,8 @@ void Stepper::report_positions() {
   #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
 
   #if EXTRA_CYCLES_BABYSTEP > 20
-    #define _SAVE_START const uint32_t pulse_start = TCNT0
-    #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
+    #define _SAVE_START const hal_timer_t pulse_start = HAL_timer_get_count(STEP_TIMER_NUM)
+    #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(STEP_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
   #else
     #define _SAVE_START NOOP
     #if EXTRA_CYCLES_BABYSTEP > 0
diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index 5dec78390546177370c576b67fe39e0cbbff28e0..198da72dd7d186f64f274d5218e52ae5ff6d0fd0 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -52,11 +52,6 @@
 class Stepper;
 extern Stepper stepper;
 
-#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
-#define STEPPER_ISR_ENABLED()             TEST(TIMSK1, OCIE1A)
-#define HAL_STEPPER_TIMER_RATE            ((F_CPU) * 0.125)
-
 // intRes = intIn1 * intIn2 >> 16
 // uses:
 // r26 to store 0
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index e94c6b03b16aed0cdb587c871430ea68c8fe1191..ca344aab1868df99f34acfaf958544a9ad75e243 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -1066,9 +1066,7 @@ void Temperature::updateTemperaturesFromRawValues() {
     watchdog_reset();
   #endif
 
-  CRITICAL_SECTION_START;
   temp_meas_ready = false;
-  CRITICAL_SECTION_END;
 }
 
 
@@ -1179,43 +1177,38 @@ void Temperature::init() {
 
   #endif // HEATER_0_USES_MAX6675
 
-  #ifdef DIDR2
-    #define ANALOG_SELECT(pin) do{ if (pin < 8) SBI(DIDR0, pin); else SBI(DIDR2, pin & 0x07); }while(0)
-  #else
-    #define ANALOG_SELECT(pin) do{ SBI(DIDR0, pin); }while(0)
-  #endif
+  HAL_adc_init();
 
-  // Set analog inputs
-  ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07;
-  DIDR0 = 0;
-  #ifdef DIDR2
-    DIDR2 = 0;
-  #endif
   #if HAS_TEMP_ADC_0
-    ANALOG_SELECT(TEMP_0_PIN);
+    HAL_ANALOG_SELECT(TEMP_0_PIN);
   #endif
   #if HAS_TEMP_ADC_1
-    ANALOG_SELECT(TEMP_1_PIN);
+    HAL_ANALOG_SELECT(TEMP_1_PIN);
   #endif
   #if HAS_TEMP_ADC_2
-    ANALOG_SELECT(TEMP_2_PIN);
+    HAL_ANALOG_SELECT(TEMP_2_PIN);
   #endif
   #if HAS_TEMP_ADC_3
-    ANALOG_SELECT(TEMP_3_PIN);
+    HAL_ANALOG_SELECT(TEMP_3_PIN);
   #endif
   #if HAS_TEMP_ADC_4
-    ANALOG_SELECT(TEMP_4_PIN);
+    HAL_ANALOG_SELECT(TEMP_4_PIN);
   #endif
   #if HAS_HEATED_BED
-    ANALOG_SELECT(TEMP_BED_PIN);
+    HAL_ANALOG_SELECT(TEMP_BED_PIN);
   #endif
   #if HAS_TEMP_CHAMBER
-    ANALOG_SELECT(TEMP_CHAMBER_PIN);
+    HAL_ANALOG_SELECT(TEMP_CHAMBER_PIN);
   #endif
   #if ENABLED(FILAMENT_WIDTH_SENSOR)
-    ANALOG_SELECT(FILWIDTH_PIN);
+    HAL_ANALOG_SELECT(FILWIDTH_PIN);
   #endif
 
+  // Use timer0 for temperature measurement
+  // Interleave temperature interrupt with millies interrupt
+  OCR0B = 128;
+  ENABLE_TEMPERATURE_INTERRUPT();
+
   #if HAS_AUTO_FAN_0
     #if E0_AUTO_FAN_PIN == FAN1_PIN
       SET_OUTPUT(E0_AUTO_FAN_PIN);
@@ -1277,11 +1270,6 @@ void Temperature::init() {
     #endif
   #endif
 
-  // Use timer0 for temperature measurement
-  // Interleave temperature interrupt with millies interrupt
-  OCR0B = 128;
-  ENABLE_TEMPERATURE_INTERRUPT();
-
   // Wait for temperature measurement to settle
   delay(250);
 
@@ -1793,23 +1781,12 @@ void Temperature::set_current_temp_raw() {
  *  - For PINS_DEBUGGING, monitor and report endstop pins
  *  - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged
  */
-ISR(TIMER0_COMPB_vect) {
-  /**
-   * AVR has no hardware interrupt preemption, so emulate priorization
-   * and preemption of this ISR by all others by disabling the timer
-   * interrupt generation capability and reenabling global interrupts.
-   * Any interrupt can then interrupt this handler and preempt it.
-   * This ISR becomes the lowest priority one so the UART, Endstops
-   * and Stepper ISRs can all preempt it.
-   */
-  DISABLE_TEMPERATURE_INTERRUPT();
-  sei();
+HAL_TEMP_TIMER_ISR {
+  HAL_timer_isr_prologue(TEMP_TIMER_NUM);
 
   Temperature::isr();
 
-  // Disable global interrupts and reenable this ISR
-  cli();
-  ENABLE_TEMPERATURE_INTERRUPT();
+  HAL_timer_isr_epilogue(TEMP_TIMER_NUM);
 }
 
 void Temperature::isr() {
@@ -2107,13 +2084,6 @@ void Temperature::isr() {
    * This gives each ADC 0.9765ms to charge up.
    */
 
-  #define SET_ADMUX_ADCSRA(pin) ADMUX = _BV(REFS0) | (pin & 0x07); SBI(ADCSRA, ADSC)
-  #ifdef MUX5
-    #define START_ADC(pin) if (pin > 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
-  #else
-    #define START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
-  #endif
-
   switch (adc_sensor_state) {
 
     case SensorsReady: {
@@ -2133,25 +2103,25 @@ void Temperature::isr() {
 
     #if HAS_TEMP_ADC_0
       case PrepareTemp_0:
-        START_ADC(TEMP_0_PIN);
+        HAL_START_ADC(TEMP_0_PIN);
         break;
       case MeasureTemp_0:
-        raw_temp_value[0] += ADC;
+        raw_temp_value[0] += HAL_READ_ADC;
         break;
     #endif
 
     #if HAS_HEATED_BED
       case PrepareTemp_BED:
-        START_ADC(TEMP_BED_PIN);
+        HAL_START_ADC(TEMP_BED_PIN);
         break;
       case MeasureTemp_BED:
-        raw_temp_bed_value += ADC;
+        raw_temp_bed_value += HAL_READ_ADC;
         break;
     #endif
 
     #if HAS_TEMP_CHAMBER
       case PrepareTemp_CHAMBER:
-        START_ADC(TEMP_CHAMBER_PIN);
+        HAL_START_ADC(TEMP_CHAMBER_PIN);
         break;
       case MeasureTemp_CHAMBER:
         raw_temp_chamber_value += ADC;
@@ -2160,55 +2130,55 @@ void Temperature::isr() {
 
     #if HAS_TEMP_ADC_1
       case PrepareTemp_1:
-        START_ADC(TEMP_1_PIN);
+        HAL_START_ADC(TEMP_1_PIN);
         break;
       case MeasureTemp_1:
-        raw_temp_value[1] += ADC;
+        raw_temp_value[1] += HAL_READ_ADC;
         break;
     #endif
 
     #if HAS_TEMP_ADC_2
       case PrepareTemp_2:
-        START_ADC(TEMP_2_PIN);
+        HAL_START_ADC(TEMP_2_PIN);
         break;
       case MeasureTemp_2:
-        raw_temp_value[2] += ADC;
+        raw_temp_value[2] += HAL_READ_ADC;
         break;
     #endif
 
     #if HAS_TEMP_ADC_3
       case PrepareTemp_3:
-        START_ADC(TEMP_3_PIN);
+        HAL_START_ADC(TEMP_3_PIN);
         break;
       case MeasureTemp_3:
-        raw_temp_value[3] += ADC;
+        raw_temp_value[3] += HAL_READ_ADC;
         break;
     #endif
 
     #if HAS_TEMP_ADC_4
       case PrepareTemp_4:
-        START_ADC(TEMP_4_PIN);
+        HAL_START_ADC(TEMP_4_PIN);
         break;
       case MeasureTemp_4:
-        raw_temp_value[4] += ADC;
+        raw_temp_value[4] += HAL_READ_ADC;
         break;
     #endif
 
     #if ENABLED(FILAMENT_WIDTH_SENSOR)
       case Prepare_FILWIDTH:
-        START_ADC(FILWIDTH_PIN);
+        HAL_START_ADC(FILWIDTH_PIN);
       break;
       case Measure_FILWIDTH:
-        if (ADC > 102) { // Make sure ADC is reading > 0.5 volts, otherwise don't read.
+        if (HAL_READ_ADC > 102) { // Make sure ADC is reading > 0.5 volts, otherwise don't read.
           raw_filwidth_value -= (raw_filwidth_value >> 7); // Subtract 1/128th of the raw_filwidth_value
-          raw_filwidth_value += ((unsigned long)ADC << 7); // Add new ADC reading, scaled by 128
+          raw_filwidth_value += ((unsigned long)HAL_READ_ADC << 7); // Add new ADC reading, scaled by 128
         }
       break;
     #endif
 
     #if ENABLED(ADC_KEYPAD)
       case Prepare_ADC_KEY:
-        START_ADC(ADC_KEYPAD_PIN);
+        HAL_START_ADC(ADC_KEYPAD_PIN);
         break;
       case Measure_ADC_KEY:
         if (ADCKey_count < 16) {