From 4b16fa3272032a66200515750608f35dffbf1c09 Mon Sep 17 00:00:00 2001
From: Christopher Pepper <p3p@p3psoft.co.uk>
Date: Sun, 18 Jun 2017 00:36:10 +0100
Subject: [PATCH] Implement HAL and apply macros across code-base Implement AVR
 Platform

---
 .gitignore                                    |   6 +-
 Marlin/Conditionals_post.h                    |  81 ++++++-
 Marlin/Marlin.h                               |   7 +-
 Marlin/MarlinConfig.h                         |   5 +-
 Marlin/Marlin_main.cpp                        |  50 ++--
 Marlin/Sd2Card.cpp                            | 151 +------------
 Marlin/Sd2Card.h                              |  18 +-
 Marlin/SdFile.h                               |   5 +-
 Marlin/boards.h                               |  23 ++
 Marlin/buzzer.h                               |   1 -
 Marlin/circularqueue.h                        |   2 +-
 Marlin/configuration_store.cpp                |  48 +---
 Marlin/configuration_store.h                  |   2 -
 Marlin/dac_dac084s085.cpp                     | 108 +++++++++
 Marlin/dac_dac084s085.h                       |  11 +
 Marlin/hex_print_routines.cpp                 |   2 +-
 Marlin/macros.h                               |   5 -
 Marlin/pins.h                                 | 112 +++++----
 Marlin/pins_RAMPS.h                           |   6 +-
 Marlin/planner.cpp                            |   2 +-
 Marlin/printcounter.h                         |  12 +-
 Marlin/{spi.h => private_spi.h}               |   6 +-
 Marlin/serial.h                               |  26 ++-
 Marlin/src/HAL/HAL.h                          |  94 ++++++++
 Marlin/src/HAL/HAL_AVR/HAL_AVR.cpp            | 100 ++++++++
 Marlin/src/HAL/HAL_AVR/HAL_AVR.h              | 160 +++++++++++++
 .../HAL/HAL_AVR/HAL_pinsDebug_AVR.h}          |  24 +-
 Marlin/src/HAL/HAL_AVR/HAL_spi_AVR.cpp        | 213 ++++++++++++++++++
 Marlin/{ => src/HAL/HAL_AVR}/MarlinSerial.cpp |   9 +-
 Marlin/{ => src/HAL/HAL_AVR}/MarlinSerial.h   |   5 +-
 Marlin/src/HAL/HAL_AVR/ServoTimers.h          |  90 ++++++++
 .../HAL/HAL_AVR}/endstop_interrupts.h         |  12 +-
 Marlin/{ => src/HAL/HAL_AVR}/fastio_1280.h    |   2 +-
 Marlin/{ => src/HAL/HAL_AVR}/fastio_1281.h    |   2 +-
 Marlin/{ => src/HAL/HAL_AVR}/fastio_168.h     |   2 +-
 Marlin/{ => src/HAL/HAL_AVR}/fastio_644.h     |   2 +-
 Marlin/{ => src/HAL/HAL_AVR}/fastio_AT90USB.h |   2 +-
 .../HAL/HAL_AVR/fastio_AVR.h}                 |   7 +-
 Marlin/src/HAL/HAL_AVR/math_AVR.h             | 112 +++++++++
 Marlin/src/HAL/HAL_AVR/persistent_store.cpp   |  57 +++++
 .../HAL/HAL_AVR}/pinsDebug_Teensyduino.h      |   0
 .../{ => src/HAL/HAL_AVR}/pinsDebug_plus_70.h |   0
 .../HAL/HAL_AVR/servo_AVR.cpp}                | 127 +----------
 Marlin/src/HAL/HAL_AVR/spi_pins.h             |  67 ++++++
 .../HAL/HAL_AVR/watchdog_AVR.cpp}             |   7 +-
 .../HAL/HAL_AVR/watchdog_AVR.h}               |   7 +-
 Marlin/src/HAL/HAL_endstop_interrupts.h       |  53 +++++
 Marlin/src/HAL/HAL_pinsDebug.h                |  35 +++
 Marlin/src/HAL/HAL_spi_pins.h                 |  39 ++++
 Marlin/src/HAL/I2cEeprom.cpp                  | 162 +++++++++++++
 Marlin/src/HAL/SpiEeprom.cpp                  | 117 ++++++++++
 Marlin/src/HAL/math_32bit.h                   |  33 +++
 Marlin/src/HAL/persistent_store_api.h         |  19 ++
 Marlin/src/HAL/servo.cpp                      | 168 ++++++++++++++
 Marlin/{ => src/HAL}/servo.h                  |  70 +-----
 Marlin/src/HAL/servo_private.h                | 103 +++++++++
 Marlin/stepper.cpp                            | 180 +++++++--------
 Marlin/stepper.h                              | 128 ++++++-----
 Marlin/temperature.cpp                        | 118 +++++-----
 Marlin/temperature.h                          |   2 +-
 Marlin/ubl_G29.cpp                            |   2 +-
 Marlin/ubl_motion.cpp                         |   2 -
 Marlin/ultralcd.cpp                           |   4 +-
 Marlin/ultralcd_impl_DOGM.h                   |   2 +-
 Marlin/ultralcd_impl_HD44780.h                |   2 +-
 65 files changed, 2266 insertions(+), 763 deletions(-)
 create mode 100644 Marlin/dac_dac084s085.cpp
 create mode 100644 Marlin/dac_dac084s085.h
 rename Marlin/{spi.h => private_spi.h} (95%)
 create mode 100644 Marlin/src/HAL/HAL.h
 create mode 100644 Marlin/src/HAL/HAL_AVR/HAL_AVR.cpp
 create mode 100644 Marlin/src/HAL/HAL_AVR/HAL_AVR.h
 rename Marlin/{pinsDebug.h => src/HAL/HAL_AVR/HAL_pinsDebug_AVR.h} (96%)
 create mode 100644 Marlin/src/HAL/HAL_AVR/HAL_spi_AVR.cpp
 rename Marlin/{ => src/HAL/HAL_AVR}/MarlinSerial.cpp (99%)
 rename Marlin/{ => src/HAL/HAL_AVR}/MarlinSerial.h (98%)
 create mode 100644 Marlin/src/HAL/HAL_AVR/ServoTimers.h
 rename Marlin/{ => src/HAL/HAL_AVR}/endstop_interrupts.h (93%)
 rename Marlin/{ => src/HAL/HAL_AVR}/fastio_1280.h (99%)
 rename Marlin/{ => src/HAL/HAL_AVR}/fastio_1281.h (99%)
 rename Marlin/{ => src/HAL/HAL_AVR}/fastio_168.h (99%)
 rename Marlin/{ => src/HAL/HAL_AVR}/fastio_644.h (99%)
 rename Marlin/{ => src/HAL/HAL_AVR}/fastio_AT90USB.h (99%)
 rename Marlin/{fastio.h => src/HAL/HAL_AVR/fastio_AVR.h} (99%)
 create mode 100644 Marlin/src/HAL/HAL_AVR/math_AVR.h
 create mode 100644 Marlin/src/HAL/HAL_AVR/persistent_store.cpp
 rename Marlin/{ => src/HAL/HAL_AVR}/pinsDebug_Teensyduino.h (100%)
 rename Marlin/{ => src/HAL/HAL_AVR}/pinsDebug_plus_70.h (100%)
 rename Marlin/{servo.cpp => src/HAL/HAL_AVR/servo_AVR.cpp} (62%)
 create mode 100644 Marlin/src/HAL/HAL_AVR/spi_pins.h
 rename Marlin/{watchdog.cpp => src/HAL/HAL_AVR/watchdog_AVR.cpp} (95%)
 rename Marlin/{watchdog.h => src/HAL/HAL_AVR/watchdog_AVR.h} (93%)
 create mode 100644 Marlin/src/HAL/HAL_endstop_interrupts.h
 create mode 100644 Marlin/src/HAL/HAL_pinsDebug.h
 create mode 100644 Marlin/src/HAL/HAL_spi_pins.h
 create mode 100644 Marlin/src/HAL/I2cEeprom.cpp
 create mode 100644 Marlin/src/HAL/SpiEeprom.cpp
 create mode 100644 Marlin/src/HAL/math_32bit.h
 create mode 100644 Marlin/src/HAL/persistent_store_api.h
 create mode 100644 Marlin/src/HAL/servo.cpp
 rename Marlin/{ => src/HAL}/servo.h (70%)
 create mode 100644 Marlin/src/HAL/servo_private.h

diff --git a/.gitignore b/.gitignore
index 8c7053826c..88416030f3 100755
--- a/.gitignore
+++ b/.gitignore
@@ -118,7 +118,11 @@ tags
 
 # PlatformIO files/dirs
 .pio*
-lib/readme.txt
+.pioenvs
+.piolibdeps
+.clang_complete
+.gcc-flags.json
+lib/
 
 #Visual Studio
 *.sln
diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h
index 0543e34523..c50480b971 100644
--- a/Marlin/Conditionals_post.h
+++ b/Marlin/Conditionals_post.h
@@ -203,6 +203,22 @@
     #define DEFAULT_KEEPALIVE_INTERVAL 2
   #endif
 
+  #ifdef CPU_32_BIT
+    /**
+     * Hidden options for developer
+     */
+    // Double stepping start from STEP_DOUBLER_FREQUENCY + 1, quad stepping start from STEP_DOUBLER_FREQUENCY * 2 + 1
+    #ifndef STEP_DOUBLER_FREQUENCY
+      #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
+        #define STEP_DOUBLER_FREQUENCY 60000 // Hz
+      #else
+        #define STEP_DOUBLER_FREQUENCY 80000 // Hz
+      #endif
+    #endif
+    // Disable double / quad stepping
+    //#define DISABLE_MULTI_STEPPING
+  #endif
+
   /**
    * Provide a MAX_AUTORETRACT for older configs
    */
@@ -214,9 +230,17 @@
    * MAX_STEP_FREQUENCY differs for TOSHIBA
    */
   #if ENABLED(CONFIG_STEPPERS_TOSHIBA)
-    #define MAX_STEP_FREQUENCY 10000 // Max step frequency for Toshiba Stepper Controllers
+    #ifdef CPU_32_BIT
+      #define MAX_STEP_FREQUENCY STEP_DOUBLER_FREQUENCY // Max step frequency for Toshiba Stepper Controllers, 96kHz is close to maximum for an Arduino Due
+    #else
+      #define MAX_STEP_FREQUENCY 10000 // Max step frequency for Toshiba Stepper Controllers
+    #endif
   #else
-    #define MAX_STEP_FREQUENCY 40000 // Max step frequency for Ultimaker (5000 pps / half step)
+    #ifdef CPU_32_BIT
+      #define MAX_STEP_FREQUENCY (STEP_DOUBLER_FREQUENCY * 4) // Max step frequency for the Due is approx. 330kHz
+    #else
+      #define MAX_STEP_FREQUENCY 40000 // Max step frequency for Ultimaker (5000 pps / half step)
+    #endif
   #endif
 
   // MS1 MS2 Stepper Driver Microstepping mode table
@@ -224,7 +248,16 @@
   #define MICROSTEP2 HIGH,LOW
   #define MICROSTEP4 LOW,HIGH
   #define MICROSTEP8 HIGH,HIGH
+  #ifdef __SAM3X8E__
+    #if MB(ALLIGATOR)
+      #define MICROSTEP16 LOW,LOW
+      #define MICROSTEP32 HIGH,HIGH
+    #else
+      #define MICROSTEP16 HIGH,HIGH
+    #endif
+  #else
   #define MICROSTEP16 HIGH,HIGH
+  #endif
 
   /**
    * Advance calculated values
@@ -350,6 +383,10 @@
     #define BED_USES_THERMISTOR
   #endif
 
+  #ifdef __SAM3X8E__
+    #define HEATER_USES_AD595 (ENABLED(HEATER_0_USES_AD595) || ENABLED(HEATER_1_USES_AD595) || ENABLED(HEATER_2_USES_AD595) || ENABLED(HEATER_3_USES_AD595))
+  #endif
+
   /**
    * Flags for PID handling
    */
@@ -873,6 +910,18 @@
 
   // Stepper pulse duration, in cycles
   #define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND)
+  #ifdef CPU_32_BIT
+    // Add additional delay for between direction signal and pulse signal of stepper
+    #ifndef STEPPER_DIRECTION_DELAY
+      #define STEPPER_DIRECTION_DELAY 0 // time in microseconds
+    #endif
+  #endif
+
+  #ifndef __SAM3X8E__ //todo: hal: broken hal encapsulation
+    #undef UI_VOLTAGE_LEVEL
+    #undef RADDS_DISPLAY
+    #undef MOTOR_CURRENT
+  #endif
 
   #if ENABLED(SDCARD_SORT_ALPHA)
     #define HAS_FOLDER_SORTING (FOLDER_SORTING || ENABLED(SDSORT_GCODE))
@@ -928,4 +977,32 @@
     #endif
   #endif
 
+  // Use float instead of double. Needs profiling.
+  #if defined(ARDUINO_ARCH_SAM) && ENABLED(DELTA_FAST_SQRT)
+    #undef ATAN2
+    #undef FABS
+    #undef POW
+    #undef SQRT
+    #undef CEIL
+    #undef FLOOR
+    #undef LROUND
+    #undef FMOD
+    #define ATAN2(y, x) atan2f(y, x)
+    #define FABS(x) fabsf(x)
+    #define POW(x, y) powf(x, y)
+    #define SQRT(x) sqrtf(x)
+    #define CEIL(x) ceilf(x)
+    #define FLOOR(x) floorf(x)
+    #define LROUND(x) lroundf(x)
+    #define FMOD(x, y) fmodf(x, y)
+  #endif
+
+  #ifdef TEENSYDUINO
+    #undef max
+    #define max(a,b) ((a)>(b)?(a):(b))
+    #undef min
+    #define min(a,b) ((a)<(b)?(a):(b))
+
+    #define NOT_A_PIN 0 // For PINS_DEBUGGING
+  #endif
 #endif // CONDITIONALS_POST_H
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 1e65d1b4eb..4292ccddb4 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -28,20 +28,15 @@
 #include <string.h>
 #include <inttypes.h>
 
-#include <util/delay.h>
-#include <avr/pgmspace.h>
-#include <avr/eeprom.h>
-#include <avr/interrupt.h>
-
 #include "MarlinConfig.h"
 
 #ifdef DEBUG_GCODE_PARSER
   #include "gcode.h"
 #endif
+#include "src/HAL/HAL.h"
 
 #include "enum.h"
 #include "types.h"
-#include "fastio.h"
 #include "utility.h"
 #include "serial.h"
 
diff --git a/Marlin/MarlinConfig.h b/Marlin/MarlinConfig.h
index 64e0bac51f..847562b0de 100644
--- a/Marlin/MarlinConfig.h
+++ b/Marlin/MarlinConfig.h
@@ -23,18 +23,17 @@
 #ifndef MARLIN_CONFIG_H
 #define MARLIN_CONFIG_H
 
-#include "fastio.h"
 #include "macros.h"
+#include "src/HAL/HAL.h"
 #include "boards.h"
 #include "Version.h"
 #include "Configuration.h"
 #include "Conditionals_LCD.h"
 #include "Configuration_adv.h"
 #include "pins.h"
-#ifndef USBCON
+#if defined(ARDUINO_ARCH_AVR) && !defined(USBCON)
   #define HardwareSerial_h // trick to disable the standard HWserial
 #endif
-#include "Arduino.h"
 #include "Conditionals_post.h"
 #include "SanityCheck.h"
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b5f757e4ea..9b84c8e235 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -251,7 +251,9 @@
 #include "cardreader.h"
 #include "configuration_store.h"
 #include "language.h"
-#include "pins_arduino.h"
+#ifdef ARDUINO
+  #include "pins_arduino.h"
+#endif
 #include "math.h"
 #include "nozzle.h"
 #include "duration_t.h"
@@ -275,10 +277,6 @@
   #include "buzzer.h"
 #endif
 
-#if ENABLED(USE_WATCHDOG)
-  #include "watchdog.h"
-#endif
-
 #if ENABLED(MAX7219_DEBUG)
   #include "Max7219_Debug_LEDs.h"
 #endif
@@ -297,7 +295,7 @@
 #endif
 
 #if HAS_SERVOS
-  #include "servo.h"
+  #include "src/HAL/servo.h"
 #endif
 
 #if HAS_DIGIPOTSS
@@ -317,7 +315,7 @@
 #endif
 
 #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-  #include "endstop_interrupts.h"
+  #include "src/HAL/HAL_endstop_interrupts.h"
 #endif
 
 #if ENABLED(M100_FREE_MEMORY_WATCHER)
@@ -653,7 +651,7 @@ float cartes[XYZ] = { 0 };
 static bool send_ok[BUFSIZE];
 
 #if HAS_SERVOS
-  Servo servo[NUM_SERVOS];
+  HAL_SERVO_LIB servo[NUM_SERVOS];
   #define MOVE_SERVO(I, P) servo[I].move(P)
   #if HAS_Z_SERVO_ENDSTOP
     #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[0])
@@ -789,26 +787,6 @@ inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[
 
 #endif
 
-#if ENABLED(SDSUPPORT)
-  #include "SdFatUtil.h"
-  int freeMemory() { return SdFatUtil::FreeRam(); }
-#else
-extern "C" {
-  extern char __bss_end;
-  extern char __heap_start;
-  extern void* __brkval;
-
-  int freeMemory() {
-    int free_memory;
-    if ((int)__brkval == 0)
-      free_memory = ((int)&free_memory) - ((int)&__bss_end);
-    else
-      free_memory = ((int)&free_memory) - ((int)__brkval);
-    return free_memory;
-  }
-}
-#endif // !SDSUPPORT
-
 #if ENABLED(DIGIPOT_I2C)
   extern void digipot_i2c_set_current(uint8_t channel, float current);
   extern void digipot_i2c_init();
@@ -11845,7 +11823,7 @@ void ok_to_send() {
     delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + drt[C_AXIS]);
   }
 
-  #if ENABLED(DELTA_FAST_SQRT)
+  #if ENABLED(DELTA_FAST_SQRT) && defined(ARDUINO_ARCH_AVR)
     /**
      * Fast inverse sqrt from Quake III Arena
      * See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
@@ -13357,17 +13335,22 @@ void setup() {
   #endif
 
   MYSERIAL.begin(BAUDRATE);
+  while(!MYSERIAL);
   SERIAL_PROTOCOLLNPGM("start");
   SERIAL_ECHO_START();
 
   // Check startup - does nothing if bootloader sets MCUSR to 0
-  byte mcu = MCUSR;
+  byte mcu = HAL_get_reset_source();
   if (mcu &  1) SERIAL_ECHOLNPGM(MSG_POWERUP);
   if (mcu &  2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
   if (mcu &  4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
   if (mcu &  8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
   if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
-  MCUSR = 0;
+  HAL_clear_reset_source();
+
+  #if ENABLED(USE_WATCHDOG) //reinit watchdog after HAL_get_reset_source call
+    watchdog_init();
+  #endif
 
   SERIAL_ECHOPGM(MSG_MARLIN);
   SERIAL_CHAR(' ');
@@ -13406,10 +13389,6 @@ void setup() {
 
   thermalManager.init();    // Initialize temperature loop
 
-  #if ENABLED(USE_WATCHDOG)
-    watchdog_init();
-  #endif
-
   stepper.init();    // Initialize stepper, this enables interrupts!
   servo_init();
 
@@ -13619,4 +13598,3 @@ void loop() {
   endstops.report_state();
   idle();
 }
-
diff --git a/Marlin/Sd2Card.cpp b/Marlin/Sd2Card.cpp
index 2afe9a8b48..512871f218 100644
--- a/Marlin/Sd2Card.cpp
+++ b/Marlin/Sd2Card.cpp
@@ -31,136 +31,6 @@
 #if ENABLED(SDSUPPORT)
 #include "Sd2Card.h"
 
-#if ENABLED(USE_WATCHDOG)
-  #include "watchdog.h"
-#endif
-
-//------------------------------------------------------------------------------
-#if DISABLED(SOFTWARE_SPI)
-  // functions for hardware SPI
-  //------------------------------------------------------------------------------
-  // make sure SPCR rate is in expected bits
-  #if (SPR0 != 0 || SPR1 != 1)
-    #error "unexpected SPCR bits"
-  #endif
-  /**
-   * Initialize hardware SPI
-   * Set SCK rate to F_CPU/pow(2, 1 + spiRate) for spiRate [0,6]
-   */
-  static void spiInit(uint8_t spiRate) {
-    // See avr processor documentation
-    SPCR = _BV(SPE) | _BV(MSTR) | (spiRate >> 1);
-    SPSR = spiRate & 1 || spiRate == 6 ? 0 : _BV(SPI2X);
-  }
-  //------------------------------------------------------------------------------
-  /** SPI receive a byte */
-  static uint8_t spiRec() {
-    SPDR = 0xFF;
-    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-    return SPDR;
-  }
-  //------------------------------------------------------------------------------
-  /** SPI read data - only one call so force inline */
-  static inline __attribute__((always_inline))
-  void spiRead(uint8_t* buf, uint16_t nbyte) {
-    if (nbyte-- == 0) return;
-    SPDR = 0xFF;
-    for (uint16_t i = 0; i < nbyte; i++) {
-      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-      buf[i] = SPDR;
-      SPDR = 0xFF;
-    }
-    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-    buf[nbyte] = SPDR;
-  }
-  //------------------------------------------------------------------------------
-  /** SPI send a byte */
-  static void spiSend(uint8_t b) {
-    SPDR = b;
-    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-  }
-  //------------------------------------------------------------------------------
-  /** SPI send block - only one call so force inline */
-  static inline __attribute__((always_inline))
-  void spiSendBlock(uint8_t token, const uint8_t* buf) {
-    SPDR = token;
-    for (uint16_t i = 0; i < 512; i += 2) {
-      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-      SPDR = buf[i];
-      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-      SPDR = buf[i + 1];
-    }
-    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
-  }
-       //------------------------------------------------------------------------------
-#else  // SOFTWARE_SPI
-       //------------------------------------------------------------------------------
-  /** nop to tune soft SPI timing */
-  #define nop asm volatile ("nop\n\t")
-  //------------------------------------------------------------------------------
-  /** Soft SPI receive byte */
-  static uint8_t spiRec() {
-    uint8_t data = 0;
-    // no interrupts during byte receive - about 8 us
-    cli();
-    // output pin high - like sending 0xFF
-    WRITE(SPI_MOSI_PIN, HIGH);
-
-    for (uint8_t i = 0; i < 8; i++) {
-      WRITE(SPI_SCK_PIN, HIGH);
-
-      // adjust so SCK is nice
-      nop;
-      nop;
-
-      data <<= 1;
-
-      if (READ(SPI_MISO_PIN)) data |= 1;
-
-      WRITE(SPI_SCK_PIN, LOW);
-    }
-    // enable interrupts
-    sei();
-    return data;
-  }
-  //------------------------------------------------------------------------------
-  /** Soft SPI read data */
-  static void spiRead(uint8_t* buf, uint16_t nbyte) {
-    for (uint16_t i = 0; i < nbyte; i++)
-      buf[i] = spiRec();
-  }
-  //------------------------------------------------------------------------------
-  /** Soft SPI send byte */
-  static void spiSend(uint8_t data) {
-    // no interrupts during byte send - about 8 us
-    cli();
-    for (uint8_t i = 0; i < 8; i++) {
-      WRITE(SPI_SCK_PIN, LOW);
-
-      WRITE(SPI_MOSI_PIN, data & 0x80);
-
-      data <<= 1;
-
-      WRITE(SPI_SCK_PIN, HIGH);
-    }
-    // hold SCK high for a few ns
-    nop;
-    nop;
-    nop;
-    nop;
-
-    WRITE(SPI_SCK_PIN, LOW);
-    // enable interrupts
-    sei();
-  }
-  //------------------------------------------------------------------------------
-  /** Soft SPI send block */
-  void spiSendBlock(uint8_t token, const uint8_t* buf) {
-    spiSend(token);
-    for (uint16_t i = 0; i < 512; i++)
-      spiSend(buf[i]);
-  }
-#endif  // SOFTWARE_SPI
 //------------------------------------------------------------------------------
 // send command and return error code.  Return zero for OK
 uint8_t Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) {
@@ -310,23 +180,12 @@ bool Sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin) {
   #endif
 
   // set pin modes
-  pinMode(chipSelectPin_, OUTPUT);
-  chipSelectHigh();
-  SET_INPUT(SPI_MISO_PIN);
-  SET_OUTPUT(SPI_MOSI_PIN);
-  SET_OUTPUT(SPI_SCK_PIN);
+//todo: should use chipSelectPin ?
+  spiBegin();
 
-  #if DISABLED(SOFTWARE_SPI)
-    // SS must be in output mode even it is not chip select
-    SET_OUTPUT(SS_PIN);
-    // set SS high - may be chip select for another SPI device
-    #if SET_SPI_SS_HIGH
-      WRITE(SS_PIN, HIGH);
-    #endif  // SET_SPI_SS_HIGH
-    // set SCK rate for initialization commands
-    spiRate_ = SPI_SD_INIT_RATE;
-    spiInit(spiRate_);
-  #endif  // SOFTWARE_SPI
+  // set SCK rate for initialization commands
+  spiRate_ = SPI_SD_INIT_RATE;
+  spiInit(spiRate_);
 
   // must supply min of 74 clock cycles with CS high.
   for (uint8_t i = 0; i < 10; i++) spiSend(0xFF);
diff --git a/Marlin/Sd2Card.h b/Marlin/Sd2Card.h
index 1fbd527473..536737355d 100644
--- a/Marlin/Sd2Card.h
+++ b/Marlin/Sd2Card.h
@@ -39,18 +39,6 @@
 #include "SdFatConfig.h"
 #include "SdInfo.h"
 //------------------------------------------------------------------------------
-// SPI speed is F_CPU/2^(1 + index), 0 <= index <= 6
-/** Set SCK to max rate of F_CPU/2. See Sd2Card::setSckRate(). */
-uint8_t const SPI_FULL_SPEED = 0;
-/** Set SCK rate to F_CPU/4. See Sd2Card::setSckRate(). */
-uint8_t const SPI_HALF_SPEED = 1;
-/** Set SCK rate to F_CPU/8. See Sd2Card::setSckRate(). */
-uint8_t const SPI_QUARTER_SPEED = 2;
-/** Set SCK rate to F_CPU/16. See Sd2Card::setSckRate(). */
-uint8_t const SPI_EIGHTH_SPEED = 3;
-/** Set SCK rate to F_CPU/32. See Sd2Card::setSckRate(). */
-uint8_t const SPI_SIXTEENTH_SPEED = 4;
-//------------------------------------------------------------------------------
 /** init timeout ms */
 uint16_t const SD_INIT_TIMEOUT = 2000;
 /** erase timeout ms */
@@ -133,6 +121,9 @@ uint8_t const SD_CARD_TYPE_SDHC = 3;
 //------------------------------------------------------------------------------
 // SPI pin definitions - do not edit here - change in SdFatConfig.h
 //
+#define SD_CHIP_SELECT_PIN SS_PIN
+
+#if 0
 #if DISABLED(SOFTWARE_SPI)
   // hardware pin defs
   /** The default chip select pin for the SD card is SS. */
@@ -156,6 +147,9 @@ uint8_t const SD_CARD_TYPE_SDHC = 3;
   /** SPI Clock pin */
   #define SPI_SCK_PIN SOFT_SPI_SCK_PIN
 #endif  // SOFTWARE_SPI
+
+#endif
+
 //------------------------------------------------------------------------------
 /**
  * \class Sd2Card
diff --git a/Marlin/SdFile.h b/Marlin/SdFile.h
index 53f38255cb..d9dbdc370e 100644
--- a/Marlin/SdFile.h
+++ b/Marlin/SdFile.h
@@ -34,7 +34,8 @@
 
 #if ENABLED(SDSUPPORT)
 #include "SdBaseFile.h"
-#include <Print.h>
+//todo: HAL: create wrapper for Print?
+//#include <Print.h>
 #ifndef SdFile_h
 #define SdFile_h
 //------------------------------------------------------------------------------
@@ -42,7 +43,7 @@
  * \class SdFile
  * \brief SdBaseFile with Print.
  */
-class SdFile : public SdBaseFile, public Print {
+class SdFile : public SdBaseFile/*, public Print*/ {
  public:
   SdFile() {}
   SdFile(const char* name, uint8_t oflag);
diff --git a/Marlin/boards.h b/Marlin/boards.h
index 398165e46c..7f564a7ca3 100644
--- a/Marlin/boards.h
+++ b/Marlin/boards.h
@@ -76,6 +76,7 @@
 #define BOARD_BRAINWAVE         82   // Brainwave (AT90USB646)
 #define BOARD_SAV_MKI           83   // SAV Mk-I (AT90USB1286)
 #define BOARD_TEENSY2           84   // Teensy++2.0 (AT90USB1286) - CLI compile: HARDWARE_MOTHERBOARD=84  make
+#define BOARD_TEENSY35_36       841  // Teensy3.5 and Teensy3.6
 #define BOARD_BRAINWAVE_PRO     85   // Brainwave Pro (AT90USB1286)
 #define BOARD_GEN3_PLUS         9    // Gen3+
 #define BOARD_GEN3_MONOLITHIC   22   // Gen3 Monolithic Electronics
@@ -100,6 +101,28 @@
 #define BOARD_BAM_DICE_DUE      402  // 2PrintBeta BAM&DICE Due with STK drivers
 #define BOARD_BQ_ZUM_MEGA_3D    503  // bq ZUM Mega 3D
 #define BOARD_ZRIB_V20          504  // zrib V2.0 control board (Chinese knock off RAMPS replica)
+//ARM 32
+#define BOARD_DUE3DOM          1411  // DUE3DOM for Arduino DUE
+#define BOARD_DUE3DOM_MINI     1412  // DUE3DOM MINI for Arduino DUE
+#define BOARD_RADDS            1502  // RADDS
+#define BOARD_RAMPS_FD_V1      1503  // RAMPS-FD v1
+#define BOARD_RAMPS_FD_V2      1504  // RAMPS-FD v2
+#define BOARD_RAMPS_SMART_EFB  1523  // RAMPS-SMART (Power outputs: Hotend, Fan, Bed)
+#define BOARD_RAMPS_SMART_EEB  1524  // RAMPS-SMART (Power outputs: Hotend0, Hotend1, Bed)
+#define BOARD_RAMPS_SMART_EFF  1525  // RAMPS-SMART (Power outputs: Hotend, Fan0, Fan1)
+#define BOARD_RAMPS_SMART_EEF  1526  // RAMPS-SMART (Power outputs: Hotend0, Hotend1, Fan)
+#define BOARD_RAMPS_SMART_SF   1528  // RAMPS-SMART (Power outputs: Spindle, Controller Fan)
+#define BOARD_RAMPS_DUO_EFB    1533  // RAMPS Duo (Power outputs: Hotend, Fan, Bed)
+#define BOARD_RAMPS_DUO_EEB    1534  // RAMPS Duo (Power outputs: Hotend0, Hotend1, Bed)
+#define BOARD_RAMPS_DUO_EFF    1535  // RAMPS Duo (Power outputs: Hotend, Fan0, Fan1)
+#define BOARD_RAMPS_DUO_EEF    1536  // RAMPS Duo (Power outputs: Hotend0, Hotend1, Fan)
+#define BOARD_RAMPS_DUO_SF     1538  // RAMPS Duo (Power outputs: Spindle, Controller Fan)
+#define BOARD_RAMPS4DUE_EFB    1543  // RAMPS4DUE (Power outputs: Hotend, Fan, Bed)
+#define BOARD_RAMPS4DUE_EEB    1544  // RAMPS4DUE (Power outputs: Hotend0, Hotend1, Bed)
+#define BOARD_RAMPS4DUE_EFF    1545  // RAMPS4DUE (Power outputs: Hotend, Fan0, Fan1)
+#define BOARD_RAMPS4DUE_EEF    1546  // RAMPS4DUE (Power outputs: Hotend0, Hotend1, Fan)
+#define BOARD_RAMPS4DUE_SF     1548  // RAMPS4DUE (Power outputs: Spindle, Controller Fan)
+#define BOARD_ALLIGATOR        1602  // Alligator Board R2
 
 #define MB(board) (MOTHERBOARD==BOARD_##board)
 
diff --git a/Marlin/buzzer.h b/Marlin/buzzer.h
index 530b729683..ef183ead44 100644
--- a/Marlin/buzzer.h
+++ b/Marlin/buzzer.h
@@ -24,7 +24,6 @@
 #define __BUZZER_H__
 
 #include "types.h"
-#include "fastio.h"
 #include "circularqueue.h"
 #include "temperature.h"
 
diff --git a/Marlin/circularqueue.h b/Marlin/circularqueue.h
index 9aafb99aca..d10e79bac9 100644
--- a/Marlin/circularqueue.h
+++ b/Marlin/circularqueue.h
@@ -23,7 +23,7 @@
 #ifndef __CIRCULARQUEUE_H__
 #define __CIRCULARQUEUE_H__
 
-#include <Arduino.h>
+#include <stdint.h>
 
 /**
  * @brief   Circular Queue class
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index 5ebbe85fb5..d90cefdb7b 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -252,12 +252,14 @@ void MarlinSettings::postprocess() {
 }
 
 #if ENABLED(EEPROM_SETTINGS)
+  #include "src/HAL/persistent_store_api.h"
 
   #define DUMMY_PID_VALUE 3000.0f
-  #define EEPROM_START() int eeprom_index = EEPROM_OFFSET
+  #define EEPROM_START() int eeprom_index = EEPROM_OFFSET; HAL::PersistentStore::access_start()
+  #define EEPROM_FINISH() HAL::PersistentStore::access_finish()
   #define EEPROM_SKIP(VAR) eeprom_index += sizeof(VAR)
-  #define EEPROM_WRITE(VAR) write_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc)
-  #define EEPROM_READ(VAR) read_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc)
+  #define EEPROM_WRITE(VAR) HAL::PersistentStore::write_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc)
+  #define EEPROM_READ(VAR) HAL::PersistentStore::read_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc)
   #define EEPROM_ASSERT(TST,ERR) if (!(TST)) do{ SERIAL_ERROR_START(); SERIAL_ERRORLNPGM(ERR); eeprom_read_error = true; }while(0)
 
   const char version[4] = EEPROM_VERSION;
@@ -268,39 +270,6 @@ void MarlinSettings::postprocess() {
     int MarlinSettings::meshes_begin;
   #endif
 
-  void MarlinSettings::write_data(int &pos, const uint8_t *value, uint16_t size, uint16_t *crc) {
-    if (eeprom_error) return;
-    while (size--) {
-      uint8_t * const p = (uint8_t * const)pos;
-      uint8_t v = *value;
-      // EEPROM has only ~100,000 write cycles,
-      // so only write bytes that have changed!
-      if (v != eeprom_read_byte(p)) {
-        eeprom_write_byte(p, v);
-        if (eeprom_read_byte(p) != v) {
-          SERIAL_ECHO_START();
-          SERIAL_ECHOLNPGM(MSG_ERR_EEPROM_WRITE);
-          eeprom_error = true;
-          return;
-        }
-      }
-      crc16(crc, &v, 1);
-      pos++;
-      value++;
-    };
-  }
-
-  void MarlinSettings::read_data(int &pos, uint8_t* value, uint16_t size, uint16_t *crc) {
-    if (eeprom_error) return;
-    do {
-      uint8_t c = eeprom_read_byte((unsigned char*)pos);
-      *value = c;
-      crc16(crc, &c, 1);
-      pos++;
-      value++;
-    } while (--size);
-  }
-
   /**
    * M500 - Store Configuration
    */
@@ -668,7 +637,7 @@ void MarlinSettings::postprocess() {
       if (ubl.state.storage_slot >= 0)
         store_mesh(ubl.state.storage_slot);
     #endif
-
+    EEPROM_FINISH();
     return !eeprom_error;
   }
 
@@ -1067,6 +1036,7 @@ void MarlinSettings::postprocess() {
     #if ENABLED(EEPROM_CHITCHAT) && DISABLED(DISABLE_M503)
       report();
     #endif
+    EEPROM_FINISH();
 
     return !eeprom_error;
   }
@@ -1107,7 +1077,7 @@ void MarlinSettings::postprocess() {
         uint16_t crc = 0;
         int pos = meshes_end - (slot + 1) * sizeof(ubl.z_values);
 
-        write_data(pos, (uint8_t *)&ubl.z_values, sizeof(ubl.z_values), &crc);
+        HAL::PersistentStore::write_data(pos, (uint8_t *)&ubl.z_values, sizeof(ubl.z_values), &crc);
 
         // Write crc to MAT along with other data, or just tack on to the beginning or end
 
@@ -1138,7 +1108,7 @@ void MarlinSettings::postprocess() {
         uint16_t crc = 0;
         int pos = meshes_end - (slot + 1) * sizeof(ubl.z_values);
         uint8_t * const dest = into ? (uint8_t*)into : (uint8_t*)&ubl.z_values;
-        read_data(pos, dest, sizeof(ubl.z_values), &crc);
+        HAL::PersistentStore::read_data(pos, dest, sizeof(ubl.z_values), &crc);
 
         // Compare crc with crc from MAT, or read from end
 
diff --git a/Marlin/configuration_store.h b/Marlin/configuration_store.h
index 99e9511209..33709bab91 100644
--- a/Marlin/configuration_store.h
+++ b/Marlin/configuration_store.h
@@ -72,8 +72,6 @@ class MarlinSettings {
 
       #endif
 
-      static void write_data(int &pos, const uint8_t *value, uint16_t size, uint16_t *crc);
-      static void read_data(int &pos, uint8_t *value, uint16_t size, uint16_t *crc);
     #endif
 };
 
diff --git a/Marlin/dac_dac084s085.cpp b/Marlin/dac_dac084s085.cpp
new file mode 100644
index 0000000000..bca359821b
--- /dev/null
+++ b/Marlin/dac_dac084s085.cpp
@@ -0,0 +1,108 @@
+/***************************************************************
+ *
+ * External DAC for Alligator Board
+ *
+ ****************************************************************/
+#include "Marlin.h"
+
+#if MB(ALLIGATOR)
+  #include "stepper.h"
+  #include "dac_dac084s085.h"
+
+  dac084s085::dac084s085() {
+    return ;
+  }
+
+  void dac084s085::begin() {
+    uint8_t externalDac_buf[2] = {0x20,0x00};//all off
+
+    // All SPI chip-select HIGH
+    pinMode (DAC0_SYNC, OUTPUT);
+    digitalWrite( DAC0_SYNC , HIGH );
+    #if EXTRUDERS > 1
+      pinMode (DAC1_SYNC, OUTPUT);
+      digitalWrite( DAC1_SYNC , HIGH );
+    #endif
+    digitalWrite( SPI_EEPROM1_CS , HIGH );
+    digitalWrite( SPI_EEPROM2_CS , HIGH );
+    digitalWrite( SPI_FLASH_CS , HIGH );
+    digitalWrite( SS_PIN , HIGH );
+    spiBegin();
+
+    //init onboard DAC
+    delayMicroseconds(2U);
+    digitalWrite( DAC0_SYNC , LOW );
+    delayMicroseconds(2U);
+    digitalWrite( DAC0_SYNC , HIGH );
+    delayMicroseconds(2U);
+    digitalWrite( DAC0_SYNC , LOW );
+
+    spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
+    digitalWrite( DAC0_SYNC , HIGH );
+
+    #if EXTRUDERS > 1
+      //init Piggy DAC
+      delayMicroseconds(2U);
+      digitalWrite( DAC1_SYNC , LOW );
+      delayMicroseconds(2U);
+      digitalWrite( DAC1_SYNC , HIGH );
+      delayMicroseconds(2U);
+      digitalWrite( DAC1_SYNC , LOW );
+
+      spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
+      digitalWrite( DAC1_SYNC , HIGH );
+    #endif
+
+    return;
+  }
+
+  void dac084s085::setValue(uint8_t channel, uint8_t value) {
+    if(channel >= 7) // max channel (X,Y,Z,E0,E1,E2,E3)
+      return;
+    if(value > 255) value = 255;
+
+    uint8_t externalDac_buf[2] = {0x10,0x00};
+
+    if(channel > 3)
+      externalDac_buf[0] |= (7 - channel << 6);
+    else
+      externalDac_buf[0] |= (3 - channel << 6);
+
+    externalDac_buf[0] |= (value>>4);
+    externalDac_buf[1] |= (value<<4);
+    
+    // All SPI chip-select HIGH
+    digitalWrite( DAC0_SYNC , HIGH );
+    #if EXTRUDERS > 1
+      digitalWrite( DAC1_SYNC , HIGH );
+    #endif
+    digitalWrite( SPI_EEPROM1_CS , HIGH );
+    digitalWrite( SPI_EEPROM2_CS , HIGH );
+    digitalWrite( SPI_FLASH_CS , HIGH );
+    digitalWrite( SS_PIN , HIGH );
+
+    if(channel > 3) { // DAC Piggy E1,E2,E3
+
+      digitalWrite(DAC1_SYNC , LOW);
+      delayMicroseconds(2U);
+      digitalWrite(DAC1_SYNC , HIGH);
+      delayMicroseconds(2U);
+      digitalWrite(DAC1_SYNC , LOW);
+    }
+
+    else { // DAC onboard X,Y,Z,E0
+
+      digitalWrite(DAC0_SYNC , LOW);
+      delayMicroseconds(2U);
+      digitalWrite(DAC0_SYNC , HIGH);
+      delayMicroseconds(2U);
+      digitalWrite(DAC0_SYNC , LOW);
+    }
+
+    delayMicroseconds(2U);
+    spiSend(SPI_CHAN_DAC,externalDac_buf , 2);
+
+    return;
+  }
+
+#endif
diff --git a/Marlin/dac_dac084s085.h b/Marlin/dac_dac084s085.h
new file mode 100644
index 0000000000..797103ce19
--- /dev/null
+++ b/Marlin/dac_dac084s085.h
@@ -0,0 +1,11 @@
+#ifndef dac084s085_h
+#define dac084s085_h
+
+class dac084s085 {
+  public:
+    dac084s085();
+    static void begin(void);
+    static void setValue(uint8_t channel, uint8_t value);
+};
+
+#endif //dac084s085_h
diff --git a/Marlin/hex_print_routines.cpp b/Marlin/hex_print_routines.cpp
index 65b97fc18c..4c6d3b5881 100644
--- a/Marlin/hex_print_routines.cpp
+++ b/Marlin/hex_print_routines.cpp
@@ -43,7 +43,7 @@ char* hex_word(const uint16_t w) {
 }
 
 char* hex_address(const void * const w) {
-  (void)hex_word((uint16_t)w);
+  (void)hex_word((int)w);
   return _hex;
 }
 
diff --git a/Marlin/macros.h b/Marlin/macros.h
index 3b79ba9b83..dd5ca4f474 100644
--- a/Marlin/macros.h
+++ b/Marlin/macros.h
@@ -36,11 +36,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
diff --git a/Marlin/pins.h b/Marlin/pins.h
index fbf2331da1..194172e892 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -231,6 +231,72 @@
 #elif MB(5DPRINT)
   #include "pins_5DPRINT.h"           // AT90USB1286
 
+//
+// 32-bit Boards
+//
+
+#elif MB(TEENSY35_36)
+  #include "pins_TEENSY35_36.h"
+#elif MB(DUE3DOM)
+  #include "pins_DUE3DOM.h"
+#elif MB(DUE3DOM_MINI)
+  #include "pins_DUE3DOM_MINI.h"
+#elif MB(RADDS)
+  #include "pins_RADDS.h"
+#elif MB(RAMPS_FD_V1)
+  #include "pins_RAMPS_FD.h"
+#elif MB(RAMPS_FD_V2)
+  #include "pins_RAMPS_FD_V2.h"
+#elif MB(RAMPS_SMART_EFB)
+  #define IS_RAMPS_EFB
+  #include "pins_RAMPS_SMART.h"
+#elif MB(RAMPS_SMART_EEB)
+  #define IS_RAMPS_EEB
+  #include "pins_RAMPS_SMART.h"
+#elif MB(RAMPS_SMART_EFF)
+  #define IS_RAMPS_EFF
+  #include "pins_RAMPS_SMART.h"
+#elif MB(RAMPS_SMART_EEF)
+  #define IS_RAMPS_EEF
+  #include "pins_RAMPS_SMART.h"
+#elif MB(RAMPS_SMART_SF)
+  #define IS_RAMPS_SF
+  #include "pins_RAMPS_SMART.h"
+#elif MB(RAMPS_DUO_EFB)
+  #define IS_RAMPS_EFB
+  #include "pins_RAMPS_DUO.h"
+#elif MB(RAMPS_DUO_EEB)
+  #define IS_RAMPS_EEB
+  #include "pins_RAMPS_DUO.h"
+#elif MB(RAMPS_DUO_EFF)
+  #define IS_RAMPS_EFF
+  #include "pins_RAMPS_DUO.h"
+#elif MB(RAMPS_DUO_EEF)
+  #define IS_RAMPS_EEF
+  #include "pins_RAMPS_DUO.h"
+#elif MB(RAMPS_DUO_SF)
+  #define IS_RAMPS_SF
+  #include "pins_RAMPS_DUO.h"
+#elif MB(RAMPS4DUE_EFB)
+  #define IS_RAMPS_EFB
+  #include "pins_RAMPS4DUE.h"
+#elif MB(RAMPS4DUE_EEB)
+  #define IS_RAMPS_EEB
+  #include "pins_RAMPS4DUE.h"
+#elif MB(RAMPS4DUE_EFF)
+  #define IS_RAMPS_EFF
+  #include "pins_RAMPS4DUE.h"
+#elif MB(RAMPS4DUE_EEF)
+  #define IS_RAMPS_EEF
+  #include "pins_RAMPS4DUE.h"
+#elif MB(RAMPS4DUE_SF)
+  #define IS_RAMPS_SF
+  #include "pins_RAMPS4DUE.h"
+#elif MB(ALLIGATOR)
+  #include "pins_ALLIGATOR_R2.h"
+#elif MB(RAMPS_FD_V1) || MB(RAMPS_FD_V2)
+  #include "pins_RAMPS_FD_v1.h"
+
 #else
   #error "Unknown MOTHERBOARD value set in Configuration.h"
 #endif
@@ -606,47 +672,9 @@
 
 #define HAS_DIGIPOTSS (PIN_EXISTS(DIGIPOTSS))
 
-/**
- * Define SPI Pins: SCK, MISO, MOSI, SS
- */
-#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)
-  #define AVR_SCK_PIN  13
-  #define AVR_MISO_PIN 12
-  #define AVR_MOSI_PIN 11
-  #define AVR_SS_PIN   10
-#elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__)
-  #define AVR_SCK_PIN  7
-  #define AVR_MISO_PIN 6
-  #define AVR_MOSI_PIN 5
-  #define AVR_SS_PIN   4
-#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-  #define AVR_SCK_PIN  52
-  #define AVR_MISO_PIN 50
-  #define AVR_MOSI_PIN 51
-  #define AVR_SS_PIN   53
-#elif defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__)
-  #define AVR_SCK_PIN  21
-  #define AVR_MISO_PIN 23
-  #define AVR_MOSI_PIN 22
-  #define AVR_SS_PIN   20
-#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
-  #define AVR_SCK_PIN  10
-  #define AVR_MISO_PIN 12
-  #define AVR_MOSI_PIN 11
-  #define AVR_SS_PIN   16
-#endif
-
-#ifndef SCK_PIN
-  #define SCK_PIN  AVR_SCK_PIN
-#endif
-#ifndef MISO_PIN
-  #define MISO_PIN AVR_MISO_PIN
-#endif
-#ifndef MOSI_PIN
-  #define MOSI_PIN AVR_MOSI_PIN
-#endif
-#ifndef SS_PIN
-  #define SS_PIN   AVR_SS_PIN
-#endif
+// Note: default SPI pins are defined in the HAL
+
+#include "src/HAL/HAL_spi_pins.h"
+
 
 #endif // __PINS_H__
diff --git a/Marlin/pins_RAMPS.h b/Marlin/pins_RAMPS.h
index 9b18461137..dd3cdef9c8 100644
--- a/Marlin/pins_RAMPS.h
+++ b/Marlin/pins_RAMPS.h
@@ -44,8 +44,10 @@
  *         7 | 11
  */
 
-#if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__)
-  #error "Oops!  Make sure you have 'Arduino Mega' selected from the 'Tools -> Boards' menu."
+#if !defined(IS_RAMPS_SMART) && !defined(IS_RAMPS_DUO) && !defined(IS_RAMPS4DUE)
+  #if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__)
+    #error "Oops!  Make sure you have 'Arduino Mega' selected from the 'Tools -> Boards' menu."
+  #endif
 #endif
 
 #ifndef BOARD_NAME
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 349e8affde..5f93c6d8a2 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -1222,7 +1222,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   }
   block->acceleration_steps_per_s2 = accel;
   block->acceleration = accel / steps_per_mm;
-  block->acceleration_rate = (long)(accel * 16777216.0 / ((F_CPU) * 0.125)); // * 8.388608
+  block->acceleration_rate = (long)(accel * 16777216.0 / (HAL_STEPPER_TIMER_RATE)); // 16777216 = <<24
 
   // Initial limit on the segment entry velocity
   float vmax_junction;
diff --git a/Marlin/printcounter.h b/Marlin/printcounter.h
index 0eeded9d5a..e5c2908392 100644
--- a/Marlin/printcounter.h
+++ b/Marlin/printcounter.h
@@ -26,8 +26,6 @@
 #include "macros.h"
 #include "language.h"
 #include "stopwatch.h"
-#include <avr/eeprom.h>
-
 
 // Print debug messages with M111 S2
 //#define DEBUG_PRINTCOUNTER
@@ -51,8 +49,14 @@ class PrintCounter: public Stopwatch {
      * @brief EEPROM address
      * @details Defines the start offset address where the data is stored.
      */
-    const uint16_t address = 0x32;
-
+    #if ENABLED(I2C_EEPROM) || ENABLED(SPI_EEPROM)
+      // round up address to next page boundary (assuming 32 byte pages)
+      const uint32_t address = 0x40;
+    #elif defined(CPU_32_BIT)
+      const uint32_t address = 0x32;
+    #else
+      const uint16_t address = 0x32;
+    #endif
     /**
      * @brief Interval in seconds between counter updates
      * @details This const value defines what will be the time between each
diff --git a/Marlin/spi.h b/Marlin/private_spi.h
similarity index 95%
rename from Marlin/spi.h
rename to Marlin/private_spi.h
index c4b86005ad..45cab24435 100644
--- a/Marlin/spi.h
+++ b/Marlin/private_spi.h
@@ -20,8 +20,8 @@
  *
  */
 
-#ifndef __SPI_H__
-#define __SPI_H__
+#ifndef __PRIVATE_SPI_H__
+#define __PRIVATE_SPI_H__
 
 #include <stdint.h>
 #include "softspi.h"
@@ -54,4 +54,4 @@ class SPI<MISO_PIN, MOSI_PIN, SCK_PIN> {
 
 };
 
-#endif // __SPI_H__
+#endif // __PRIVATE_SPI_H__
diff --git a/Marlin/serial.h b/Marlin/serial.h
index 8be90c06a5..f23d9f7a1a 100644
--- a/Marlin/serial.h
+++ b/Marlin/serial.h
@@ -23,20 +23,26 @@
 #ifndef __SERIAL_H__
 #define __SERIAL_H__
 
-#include "MarlinConfig.h"
+#include "src/HAL/HAL.h"
 
-#ifdef USBCON
-  #include "HardwareSerial.h"
-  #if ENABLED(BLUETOOTH)
-    #define MYSERIAL bluetoothSerial
+//todo: HAL: breaks encapsulation
+// For AVR only, define a serial interface based on configuration
+#ifdef ARDUINO_ARCH_AVR
+  #ifdef USBCON
+    #include "HardwareSerial.h"
+    #if ENABLED(BLUETOOTH)
+      #define MYSERIAL bluetoothSerial
+    #else
+      #define MYSERIAL Serial
+    #endif // BLUETOOTH
   #else
-    #define MYSERIAL Serial
-  #endif // BLUETOOTH
-#else
-  #include "MarlinSerial.h"
-  #define MYSERIAL customizedSerial
+    #include "src/HAL/HAL_AVR/MarlinSerial.h"
+    #define MYSERIAL customizedSerial
+  #endif
 #endif
 
+#include "MarlinConfig.h"
+
 extern const char echomagic[] PROGMEM;
 extern const char errormagic[] PROGMEM;
 
diff --git a/Marlin/src/HAL/HAL.h b/Marlin/src/HAL/HAL.h
new file mode 100644
index 0000000000..5d076ac568
--- /dev/null
+++ b/Marlin/src/HAL/HAL.h
@@ -0,0 +1,94 @@
+/* **************************************************************************
+
+ 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 wrapper
+ *
+ * Supports platforms :
+ *    ARDUINO_ARCH_SAM : For Arduino Due and other boards based on Atmel SAM3X8E
+ *    ARDUINO_ARCH_AVR : For all Atmel AVR boards
+ */
+
+#ifndef _HAL_H
+#define _HAL_H
+
+#include <stdint.h>
+
+/**
+ * SPI speed where 0 <= index <= 6
+ *
+ * Approximate rates :
+ *
+ *  0 :  8 - 10 MHz
+ *  1 :  4 - 5 MHz
+ *  2 :  2 - 2.5 MHz
+ *  3 :  1 - 1.25 MHz
+ *  4 :  500 - 625 kHz
+ *  5 :  250 - 312 kHz
+ *  6 :  125 - 156 kHz
+ *
+ *  On AVR, actual speed is F_CPU/2^(1 + index).
+ *  On other platforms, speed should be in range given above where possible.
+ */
+
+/** Set SCK to max rate */
+uint8_t const SPI_FULL_SPEED = 0;
+/** Set SCK rate to half max rate. */
+uint8_t const SPI_HALF_SPEED = 1;
+/** Set SCK rate to quarter max rate. */
+uint8_t const SPI_QUARTER_SPEED = 2;
+/** Set SCK rate to 1/8 max rate. */
+uint8_t const SPI_EIGHTH_SPEED = 3;
+/** Set SCK rate to 1/16 of max rate. */
+uint8_t const SPI_SIXTEENTH_SPEED = 4;
+/** Set SCK rate to 1/32 of max rate. */
+uint8_t const SPI_SPEED_5 = 5;
+/** Set SCK rate to 1/64 of max rate. */
+uint8_t const SPI_SPEED_6 = 6;
+
+// Standard SPI functions
+/** Initialise SPI bus */
+void spiBegin(void);
+/** Configure SPI for specified SPI speed */
+void spiInit(uint8_t spiRate);
+/** Write single byte to SPI */
+void spiSend(uint8_t b);
+/** Read single byte from SPI */
+uint8_t spiRec(void);
+/** Read from SPI into buffer */
+void spiRead(uint8_t* buf, uint16_t nbyte);
+/** Write token and then write from 512 byte buffer to SPI (for SD card) */
+void spiSendBlock(uint8_t token, const uint8_t* buf);
+
+#ifdef ARDUINO_ARCH_AVR
+  #include "HAL_AVR/HAL_AVR.h"
+#elif defined(ARDUINO_ARCH_SAM)
+  #define CPU_32_BIT
+  #include "HAL_DUE/HAL_Due.h"
+  #include "math_32bit.h"
+#elif defined(__MK64FX512__) || defined(__MK66FX1M0__)
+  #define CPU_32_BIT
+  #include "HAL_TEENSY35_36/HAL_Teensy.h"
+  #include "math_32bit.h"
+#else
+  #error Unsupported Platform!
+#endif
+
+#endif /* HAL_H_ */
diff --git a/Marlin/src/HAL/HAL_AVR/HAL_AVR.cpp b/Marlin/src/HAL/HAL_AVR/HAL_AVR.cpp
new file mode 100644
index 0000000000..c572b2ccab
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/HAL_AVR.cpp
@@ -0,0 +1,100 @@
+/* **************************************************************************
+
+ 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
+ *
+ * For ARDUINO_ARCH_AVR
+ */
+
+
+#ifdef ARDUINO_ARCH_AVR
+
+// --------------------------------------------------------------------------
+// Includes
+// --------------------------------------------------------------------------
+
+#include "../HAL.h"
+#include "../../../macros.h"
+
+// --------------------------------------------------------------------------
+// Externals
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Local defines
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Types
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Variables
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Public Variables
+// --------------------------------------------------------------------------
+
+//uint8_t MCUSR;
+
+// --------------------------------------------------------------------------
+// Private Variables
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Function prototypes
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Private functions
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Public functions
+// --------------------------------------------------------------------------
+
+#if ENABLED(SDSUPPORT)
+  #include "../../../SdFatUtil.h"
+  int freeMemory() { return SdFatUtil::FreeRam(); }
+#else
+
+extern "C" {
+  extern char __bss_end;
+  extern char __heap_start;
+  extern void* __brkval;
+
+  int freeMemory() {
+    int free_memory;
+    if ((int)__brkval == 0)
+      free_memory = ((int)&free_memory) - ((int)&__bss_end);
+    else
+      free_memory = ((int)&free_memory) - ((int)__brkval);
+    return free_memory;
+  }
+}
+
+#endif //!SDSUPPORT
+
+#endif
+
diff --git a/Marlin/src/HAL/HAL_AVR/HAL_AVR.h b/Marlin/src/HAL/HAL_AVR/HAL_AVR.h
new file mode 100644
index 0000000000..008f80f5fa
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/HAL_AVR.h
@@ -0,0 +1,160 @@
+/* **************************************************************************
+
+ 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
+ *
+ * For ARDUINO_ARCH_AVR
+ */
+
+
+#ifndef _HAL_AVR_H
+#define _HAL_AVR_H
+
+// --------------------------------------------------------------------------
+// Includes
+// --------------------------------------------------------------------------
+
+#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>
+
+#include "fastio_AVR.h"
+#include "watchdog_AVR.h"
+#include "math_AVR.h"
+
+// --------------------------------------------------------------------------
+// Defines
+// --------------------------------------------------------------------------
+
+//#define analogInputToDigitalPin(IO) IO
+
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
+  #define CRITICAL_SECTION_END    SREG = _sreg;
+#endif
+
+
+// On AVR this is in math.h?
+//#define square(x) ((x)*(x))
+
+// --------------------------------------------------------------------------
+// Types
+// --------------------------------------------------------------------------
+
+#define HAL_TIMER_TYPE uint16_t
+#define HAL_TIMER_TYPE_MAX 0xFFFF
+
+#define HAL_SERVO_LIB Servo
+
+// --------------------------------------------------------------------------
+// Public Variables
+// --------------------------------------------------------------------------
+
+//extern uint8_t MCUSR;
+
+// --------------------------------------------------------------------------
+// Public functions
+// --------------------------------------------------------------------------
+
+//void cli(void);
+
+//void _delay_ms(int delay);
+
+inline void HAL_clear_reset_source(void) { MCUSR = 0; }
+inline uint8_t HAL_get_reset_source(void) { return MCUSR; }
+
+extern "C" {
+  int freeMemory(void);
+}
+
+// eeprom
+//void eeprom_write_byte(unsigned char *pos, unsigned char value);
+//unsigned char eeprom_read_byte(unsigned char *pos);
+
+
+// timers
+#define STEP_TIMER_NUM OCR1A
+#define TEMP_TIMER_NUM 0
+#define TEMP_TIMER_FREQUENCY (F_CPU / 64.0 / 256.0)
+
+#define HAL_TIMER_RATE ((F_CPU) / 8.0)
+#define HAL_STEPPER_TIMER_RATE HAL_TIMER_RATE
+#define STEPPER_TIMER_PRESCALE INT0_PRESCALER
+
+#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
+
+#define ENABLE_TEMPERATURE_INTERRUPT()  SBI(TIMSK0, OCIE0B)
+#define DISABLE_TEMPERATURE_INTERRUPT() CBI(TIMSK0, OCIE0B)
+
+//void HAL_timer_start (uint8_t timer_num, uint32_t frequency);
+#define HAL_timer_start (timer_num,frequency)
+
+//void HAL_timer_set_count (uint8_t timer_num, uint16_t count);
+#define HAL_timer_set_count(timer,count) timer = (count)
+
+#define HAL_timer_get_current_count(timer) timer
+
+//void HAL_timer_isr_prologue (uint8_t timer_num);
+#define HAL_timer_isr_prologue(timer_num)
+
+#define HAL_STEP_TIMER_ISR ISR(TIMER1_COMPA_vect)
+#define HAL_TEMP_TIMER_ISR ISR(TIMER0_COMPB_vect)
+
+#define HAL_ENABLE_ISRs() do { cli(); if (thermalManager.in_temp_isr)DISABLE_TEMPERATURE_INTERRUPT(); else ENABLE_TEMPERATURE_INTERRUPT(); ENABLE_STEPPER_DRIVER_INTERRUPT(); } while(0)
+
+// ADC
+#ifdef DIDR2
+  #define HAL_ANALOG_SELECT(pin) do{ if (pin < 8) SBI(DIDR0, pin); else SBI(DIDR2, pin - 8); }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
+
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+
+#endif // _HAL_AVR_H
diff --git a/Marlin/pinsDebug.h b/Marlin/src/HAL/HAL_AVR/HAL_pinsDebug_AVR.h
similarity index 96%
rename from Marlin/pinsDebug.h
rename to Marlin/src/HAL/HAL_AVR/HAL_pinsDebug_AVR.h
index 56f0916d20..51108196a1 100644
--- a/Marlin/pinsDebug.h
+++ b/Marlin/src/HAL/HAL_AVR/HAL_pinsDebug_AVR.h
@@ -20,6 +20,16 @@
  *
  */
 
+#ifndef HAL_PINSDEBUG_AVR_H
+
+void HAL_print_analog_pin(char buffer[], int8_t pin) {
+  sprintf(buffer, "(A%2d)  ", int(pin - analogInputToDigitalPin(0)));
+}
+
+void HAL_analog_pin_state(char buffer[], int8_t pin) {
+  sprintf(buffer, "Analog in =% 5d", analogRead(pin - analogInputToDigitalPin(0)));
+}
+
 bool endstop_monitor_flag = false;
 
 #define NAME_FORMAT "%-35s"   // one place to specify the format of all the sources of names
@@ -46,7 +56,7 @@ bool endstop_monitor_flag = false;
 #define REPORT_NAME_DIGITAL(NAME, COUNTER) _ADD_PIN(#NAME, COUNTER)
 #define REPORT_NAME_ANALOG(NAME, COUNTER) _ADD_PIN(#NAME, COUNTER)
 
-#include "pinsDebug_list.h"
+#include "../../../pinsDebug_list.h"
 #line 51
 
 // manually add pins that have names that are macros which don't play well with these macros
@@ -97,7 +107,7 @@ const PinInfo pin_array[] PROGMEM = {
     #endif
   #endif
 
-  #include "pinsDebug_list.h"
+  #include "../../../pinsDebug_list.h"
   #line 102
 
 };
@@ -146,7 +156,7 @@ const PinInfo pin_array[] PROGMEM = {
  * Print a pin's PWM status.
  * Return true if it's currently a PWM pin.
  */
-static bool pwm_status(uint8_t pin) {
+static bool HAL_pwm_status(uint8_t pin) {
   char buffer[20];   // for the sprintf statements
 
   switch (digitalPinToTimer_DEBUG(pin)) {
@@ -338,7 +348,7 @@ void timer_prefix(uint8_t T, char L, uint8_t N) {  // T - timer    L - pwm  N -
   if (TEST(*TMSK, TOIE)) err_prob_interrupt();
 }
 
-static void pwm_details(uint8_t pin) {
+static void HAL_pwm_details(uint8_t pin) {
   switch (digitalPinToTimer_DEBUG(pin)) {
 
     #if defined(TCCR0A) && defined(COM0A1)
@@ -514,7 +524,7 @@ inline void report_pin_state_extended(int8_t pin, bool ignore, bool extended = f
                 print_input_or_output(false);
                 SERIAL_PROTOCOL(digitalRead_mod(pin));
               }
-              else if (pwm_status(pin)) {
+              else if (HAL_pwm_status(pin)) {
                 // do nothing
               }
               else {
@@ -522,7 +532,7 @@ inline void report_pin_state_extended(int8_t pin, bool ignore, bool extended = f
                 SERIAL_PROTOCOL(digitalRead_mod(pin));
               }
             }
-            if (!multi_name_pin && extended) pwm_details(pin);  // report PWM capabilities only on the first pass & only if doing an extended report
+            if (!multi_name_pin && extended) HAL_pwm_details(pin);  // report PWM capabilities only on the first pass & only if doing an extended report
           }
         }
       }
@@ -581,3 +591,5 @@ inline void report_pin_state_extended(int8_t pin, bool ignore, bool extended = f
     SERIAL_EOL();
   }
 }
+
+#endif //HAL_PINSDEBUG_AVR_H
\ No newline at end of file
diff --git a/Marlin/src/HAL/HAL_AVR/HAL_spi_AVR.cpp b/Marlin/src/HAL/HAL_AVR/HAL_spi_AVR.cpp
new file mode 100644
index 0000000000..128e73555c
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/HAL_spi_AVR.cpp
@@ -0,0 +1,213 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * Originally from Arduino Sd2Card Library
+ * Copyright (C) 2009 by William Greiman
+ */
+ 
+/**
+ * Description: HAL for AVR - SPI functions
+ *
+ * For ARDUINO_ARCH_AVR
+ */
+
+#ifdef ARDUINO_ARCH_AVR
+
+// --------------------------------------------------------------------------
+// Includes
+// --------------------------------------------------------------------------
+
+#include "../../../MarlinConfig.h"
+
+// --------------------------------------------------------------------------
+// Public Variables
+// --------------------------------------------------------------------------
+
+
+// --------------------------------------------------------------------------
+// Public functions
+// --------------------------------------------------------------------------
+
+void spiBegin (void) {
+  SET_OUTPUT(SS_PIN);
+  WRITE(SS_PIN, HIGH);
+  SET_OUTPUT(SCK_PIN);
+  SET_INPUT(MISO_PIN);
+  SET_OUTPUT(MOSI_PIN);
+
+  #if DISABLED(SOFTWARE_SPI)
+    // SS must be in output mode even it is not chip select
+    SET_OUTPUT(SS_PIN);
+    // set SS high - may be chip select for another SPI device
+    #if SET_SPI_SS_HIGH
+      WRITE(SS_PIN, HIGH);
+    #endif  // SET_SPI_SS_HIGH
+    // set a default rate
+    spiInit(1);
+  #endif  // SOFTWARE_SPI
+}
+
+
+//------------------------------------------------------------------------------
+#if DISABLED(SOFTWARE_SPI)
+  // functions for hardware SPI
+  //------------------------------------------------------------------------------
+  // make sure SPCR rate is in expected bits
+  #if (SPR0 != 0 || SPR1 != 1)
+    #error "unexpected SPCR bits"
+  #endif
+
+  /**
+   * Initialize hardware SPI
+   * Set SCK rate to F_CPU/pow(2, 1 + spiRate) for spiRate [0,6]
+   */
+  void spiInit(uint8_t spiRate) {
+    // See avr processor documentation
+    CBI(
+      #ifdef PRR
+        PRR
+      #elif defined(PRR0)
+        PRR0
+      #endif
+        , PRSPI);
+
+    SPCR = _BV(SPE) | _BV(MSTR) | (spiRate >> 1);
+    SPSR = spiRate & 1 || spiRate == 6 ? 0 : _BV(SPI2X);
+  }
+  //------------------------------------------------------------------------------
+  /** SPI receive a byte */
+  uint8_t spiRec(void) {
+    SPDR = 0XFF;
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+    return SPDR;
+  }
+  //------------------------------------------------------------------------------
+  /** SPI read data  */
+  void spiRead(uint8_t* buf, uint16_t nbyte) {
+    if (nbyte-- == 0) return;
+    SPDR = 0XFF;
+    for (uint16_t i = 0; i < nbyte; i++) {
+      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+      buf[i] = SPDR;
+      SPDR = 0XFF;
+    }
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+    buf[nbyte] = SPDR;
+  }
+  //------------------------------------------------------------------------------
+  /** SPI send a byte */
+  void spiSend(uint8_t b) {
+    SPDR = b;
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+  }
+  //------------------------------------------------------------------------------
+  /** SPI send block  */
+  void spiSendBlock(uint8_t token, const uint8_t* buf) {
+    SPDR = token;
+    for (uint16_t i = 0; i < 512; i += 2) {
+      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+      SPDR = buf[i];
+      while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+      SPDR = buf[i + 1];
+    }
+    while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ }
+  }
+       //------------------------------------------------------------------------------
+#else  // SOFTWARE_SPI
+       //------------------------------------------------------------------------------
+  /** nop to tune soft SPI timing */
+  #define nop asm volatile ("\tnop\n")
+
+  /** Set SPI rate */
+  void spiInit(uint8_t spiRate) {
+    // nothing to do
+    UNUSED(spiRate);
+  }
+
+  //------------------------------------------------------------------------------
+  /** Soft SPI receive byte */
+  uint8_t spiRec() {
+    uint8_t data = 0;
+    // no interrupts during byte receive - about 8 us
+    cli();
+    // output pin high - like sending 0XFF
+    WRITE(MOSI_PIN, HIGH);
+
+    for (uint8_t i = 0; i < 8; i++) {
+      WRITE(SCK_PIN, HIGH);
+
+      // adjust so SCK is nice
+      nop;
+      nop;
+
+      data <<= 1;
+
+      if (READ(MISO_PIN)) data |= 1;
+
+      WRITE(SCK_PIN, LOW);
+    }
+    // enable interrupts
+    sei();
+    return data;
+  }
+  //------------------------------------------------------------------------------
+  /** Soft SPI read data */
+  void spiRead(uint8_t* buf, uint16_t nbyte) {
+    for (uint16_t i = 0; i < nbyte; i++)
+      buf[i] = spiRec();
+  }
+  //------------------------------------------------------------------------------
+  /** Soft SPI send byte */
+  void spiSend(uint8_t data) {
+    // no interrupts during byte send - about 8 us
+    cli();
+    for (uint8_t i = 0; i < 8; i++) {
+      WRITE(SCK_PIN, LOW);
+
+      WRITE(MOSI_PIN, data & 0X80);
+
+      data <<= 1;
+
+      WRITE(SCK_PIN, HIGH);
+    }
+    // hold SCK high for a few ns
+    nop;
+    nop;
+    nop;
+    nop;
+
+    WRITE(SCK_PIN, LOW);
+    // enable interrupts
+    sei();
+  }
+  //------------------------------------------------------------------------------
+  /** Soft SPI send block */
+  void spiSendBlock(uint8_t token, const uint8_t* buf) {
+    spiSend(token);
+    for (uint16_t i = 0; i < 512; i++)
+      spiSend(buf[i]);
+  }
+#endif  // SOFTWARE_SPI
+
+
+#endif // ARDUINO_ARCH_AVR
diff --git a/Marlin/MarlinSerial.cpp b/Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp
similarity index 99%
rename from Marlin/MarlinSerial.cpp
rename to Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp
index 235e018094..b4f03f5f07 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp
@@ -28,9 +28,10 @@
  * Modified 28 September 2010 by Mark Sproul
  * Modified 14 February 2016 by Andreas Hardtung (added tx buffer)
  */
+#ifdef ARDUINO_ARCH_AVR
 
 #include "MarlinSerial.h"
-#include "Marlin.h"
+#include "../../../Marlin.h"
 
 // Disable HardwareSerial.cpp to support chips without a UART (Attiny, etc.)
 
@@ -46,8 +47,8 @@
 
   #if ENABLED(EMERGENCY_PARSER)
 
-    #include "stepper.h"
-    #include "language.h"
+    #include "../../../stepper.h"
+    #include "../../../language.h"
 
     // Currently looking for: M108, M112, M410
     // If you alter the parser please don't forget to update the capabilities in Conditionals_post.h
@@ -515,3 +516,5 @@
 #if defined(USBCON) && ENABLED(BLUETOOTH)
   HardwareSerial bluetoothSerial;
 #endif
+
+#endif
diff --git a/Marlin/MarlinSerial.h b/Marlin/src/HAL/HAL_AVR/MarlinSerial.h
similarity index 98%
rename from Marlin/MarlinSerial.h
rename to Marlin/src/HAL/HAL_AVR/MarlinSerial.h
index 8456a7237e..cf96330f56 100644
--- a/Marlin/MarlinSerial.h
+++ b/Marlin/src/HAL/HAL_AVR/MarlinSerial.h
@@ -32,7 +32,9 @@
 #ifndef MARLINSERIAL_H
 #define MARLINSERIAL_H
 
-#include "MarlinConfig.h"
+#include "../../../MarlinConfig.h"
+
+#include <WString.h>
 
 #ifndef SERIAL_PORT
   #define SERIAL_PORT 0
@@ -160,6 +162,7 @@
       static void println(unsigned long, int = DEC);
       static void println(double, int = 2);
       static void println(void);
+      operator bool() { return true; }
   };
 
   extern MarlinSerial customizedSerial;
diff --git a/Marlin/src/HAL/HAL_AVR/ServoTimers.h b/Marlin/src/HAL/HAL_AVR/ServoTimers.h
new file mode 100644
index 0000000000..231d1a6d26
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/ServoTimers.h
@@ -0,0 +1,90 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+/*
+  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library 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
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/*
+ * Defines for 16 bit timers used with  Servo library
+ *
+ * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
+ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+ * _Nbr_16timers indicates how many 16 bit timers are available.
+ */
+
+/**
+ * AVR Only definitions
+ * --------------------
+ */
+
+#define TRIM_DURATION       2    // compensation ticks to trim adjust for digitalWrite delays
+#define PRESCALER           8   // timer prescaler
+
+// Say which 16 bit timers can be used and in what order
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  //#define _useTimer1
+  #define _useTimer3
+  #define _useTimer4
+  #if !HAS_MOTOR_CURRENT_PWM
+    #define _useTimer5 // Timer 5 is used for motor current PWM and can't be used for servos.
+  #endif
+#elif defined(__AVR_ATmega32U4__)
+  #define _useTimer3
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+  #define _useTimer3
+#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__)
+  #define _useTimer3
+#else
+  // everything else
+#endif
+
+typedef enum {
+  #if ENABLED(_useTimer1)
+    _timer1,
+  #endif
+  #if ENABLED(_useTimer3)
+    _timer3,
+  #endif
+  #if ENABLED(_useTimer4)
+    _timer4,
+  #endif
+  #if ENABLED(_useTimer5)
+    _timer5,
+  #endif
+  _Nbr_16timers
+} timer16_Sequence_t;
diff --git a/Marlin/endstop_interrupts.h b/Marlin/src/HAL/HAL_AVR/endstop_interrupts.h
similarity index 93%
rename from Marlin/endstop_interrupts.h
rename to Marlin/src/HAL/HAL_AVR/endstop_interrupts.h
index 7d37c77c66..10aa652963 100644
--- a/Marlin/endstop_interrupts.h
+++ b/Marlin/src/HAL/HAL_AVR/endstop_interrupts.h
@@ -38,7 +38,7 @@
 #ifndef _ENDSTOP_INTERRUPTS_H_
 #define _ENDSTOP_INTERRUPTS_H_
 
-#include "macros.h"
+#include "../../../macros.h"
 
 /**
  * Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h)
@@ -72,8 +72,6 @@
                                     0 )
 #endif
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
-                            // Must be reset to 0 by the test function when finished.
 
 // Install Pin change interrupt for a pin. Can be called multiple times.
 void pciSetup(byte pin) {
@@ -82,14 +80,6 @@ void pciSetup(byte pin) {
   SBI(PCICR, digitalPinToPCICRbit(pin)); // enable interrupt for the group
 }
 
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
-
-// Use one Routine to handle each group
-// One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
 
 // Handlers for pin change interrupts
 #ifdef PCINT0_vect
diff --git a/Marlin/fastio_1280.h b/Marlin/src/HAL/HAL_AVR/fastio_1280.h
similarity index 99%
rename from Marlin/fastio_1280.h
rename to Marlin/src/HAL/HAL_AVR/fastio_1280.h
index f9f56c8a92..ed96002f27 100644
--- a/Marlin/fastio_1280.h
+++ b/Marlin/src/HAL/HAL_AVR/fastio_1280.h
@@ -31,7 +31,7 @@
 #ifndef _FASTIO_1280
 #define _FASTIO_1280
 
-#include "fastio.h"
+#include "fastio_AVR.h"
 
 // change for your board
 #define DEBUG_LED   DIO21
diff --git a/Marlin/fastio_1281.h b/Marlin/src/HAL/HAL_AVR/fastio_1281.h
similarity index 99%
rename from Marlin/fastio_1281.h
rename to Marlin/src/HAL/HAL_AVR/fastio_1281.h
index 85d2c4ef7c..0bbea73ed6 100644
--- a/Marlin/fastio_1281.h
+++ b/Marlin/src/HAL/HAL_AVR/fastio_1281.h
@@ -31,7 +31,7 @@
 #ifndef _FASTIO_1281
 #define _FASTIO_1281
 
-#include "fastio.h"
+#include "fastio_AVR.h"
 
 // change for your board
 #define DEBUG_LED   DIO46
diff --git a/Marlin/fastio_168.h b/Marlin/src/HAL/HAL_AVR/fastio_168.h
similarity index 99%
rename from Marlin/fastio_168.h
rename to Marlin/src/HAL/HAL_AVR/fastio_168.h
index 4ee67a32b7..89304defe2 100644
--- a/Marlin/fastio_168.h
+++ b/Marlin/src/HAL/HAL_AVR/fastio_168.h
@@ -31,7 +31,7 @@
 #ifndef _FASTIO_168
 #define _FASTIO_168
 
-#include "fastio.h"
+#include "fastio_AVR.h"
 
 #define DEBUG_LED   AIO5
 
diff --git a/Marlin/fastio_644.h b/Marlin/src/HAL/HAL_AVR/fastio_644.h
similarity index 99%
rename from Marlin/fastio_644.h
rename to Marlin/src/HAL/HAL_AVR/fastio_644.h
index 6465738d03..c0a453397d 100644
--- a/Marlin/fastio_644.h
+++ b/Marlin/src/HAL/HAL_AVR/fastio_644.h
@@ -31,7 +31,7 @@
 #ifndef _FASTIO_644
 #define _FASTIO_644
 
-#include "fastio.h"
+#include "fastio_AVR.h"
 
 #define DEBUG_LED   DIO0
 
diff --git a/Marlin/fastio_AT90USB.h b/Marlin/src/HAL/HAL_AVR/fastio_AT90USB.h
similarity index 99%
rename from Marlin/fastio_AT90USB.h
rename to Marlin/src/HAL/HAL_AVR/fastio_AT90USB.h
index 68463e18a5..803944e4df 100644
--- a/Marlin/fastio_AT90USB.h
+++ b/Marlin/src/HAL/HAL_AVR/fastio_AT90USB.h
@@ -32,7 +32,7 @@
 #ifndef _FASTIO_AT90USB
 #define _FASTIO_AT90USB
 
-#include "fastio.h"
+#include "fastio_AVR.h"
 
 // change for your board
 #define DEBUG_LED   DIO31 /* led D5 red */
diff --git a/Marlin/fastio.h b/Marlin/src/HAL/HAL_AVR/fastio_AVR.h
similarity index 99%
rename from Marlin/fastio.h
rename to Marlin/src/HAL/HAL_AVR/fastio_AVR.h
index d5ae8f2cce..035e3a5d12 100644
--- a/Marlin/fastio.h
+++ b/Marlin/src/HAL/HAL_AVR/fastio_AVR.h
@@ -30,7 +30,7 @@
 #define _FASTIO_ARDUINO_H
 
 #include <avr/io.h>
-#include "macros.h"
+#include "../../../macros.h"
 
 #define AVR_AT90USB1286_FAMILY (defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1286P__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB646P__)  || defined(__AVR_AT90USB647__))
 #define AVR_ATmega1284_FAMILY (defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__))
@@ -38,7 +38,6 @@
 #define AVR_ATmega2561_FAMILY (defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__))
 #define AVR_ATmega328_FAMILY (defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328p__))
 
-
 /**
  * Include Ports and Functions
  */
@@ -57,7 +56,7 @@
 #endif
 
 #ifndef _BV
-  #define _BV(PIN) (1UL << PIN)
+  #define _BV(bit) (1UL << (bit))
 #endif
 
 /**
@@ -269,7 +268,7 @@ typedef enum {
   #define PWM_CHK_MOTOR_CURRENT(p) false
 #endif
 
-#if defined(NUM_SERVOS)
+#ifdef NUM_SERVOS
   #if AVR_ATmega2560_FAMILY
     #define PWM_CHK_SERVO(p) ( p == 5 || NUM_SERVOS > 12 && p == 6 || NUM_SERVOS > 24 && p == 46)  //PWMS 3A, 4A & 5A
   #elif AVR_ATmega2561_FAMILY
diff --git a/Marlin/src/HAL/HAL_AVR/math_AVR.h b/Marlin/src/HAL/HAL_AVR/math_AVR.h
new file mode 100644
index 0000000000..9d210eed38
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/math_AVR.h
@@ -0,0 +1,112 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+#ifndef MATH_AVR_H
+#define MATH_AVR_H
+
+/**
+ * Optimized math functions for AVR
+ */
+
+// intRes = longIn1 * longIn2 >> 24
+// uses:
+// r26 to store 0
+// r27 to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
+// note that the lower two bytes and the upper byte of the 48bit result are not calculated.
+// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
+// B0 A0 are bits 24-39 and are the returned value
+// C1 B1 A1 is longIn1
+// D2 C2 B2 A2 is longIn2
+//
+#define MultiU24X32toH16(intRes, longIn1, longIn2) \
+  asm volatile ( \
+                 "clr r26 \n\t" \
+                 "mul %A1, %B2 \n\t" \
+                 "mov r27, r1 \n\t" \
+                 "mul %B1, %C2 \n\t" \
+                 "movw %A0, r0 \n\t" \
+                 "mul %C1, %C2 \n\t" \
+                 "add %B0, r0 \n\t" \
+                 "mul %C1, %B2 \n\t" \
+                 "add %A0, r0 \n\t" \
+                 "adc %B0, r1 \n\t" \
+                 "mul %A1, %C2 \n\t" \
+                 "add r27, r0 \n\t" \
+                 "adc %A0, r1 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "mul %B1, %B2 \n\t" \
+                 "add r27, r0 \n\t" \
+                 "adc %A0, r1 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "mul %C1, %A2 \n\t" \
+                 "add r27, r0 \n\t" \
+                 "adc %A0, r1 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "mul %B1, %A2 \n\t" \
+                 "add r27, r1 \n\t" \
+                 "adc %A0, r26 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "lsr r27 \n\t" \
+                 "adc %A0, r26 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "mul %D2, %A1 \n\t" \
+                 "add %A0, r0 \n\t" \
+                 "adc %B0, r1 \n\t" \
+                 "mul %D2, %B1 \n\t" \
+                 "add %B0, r0 \n\t" \
+                 "clr r1 \n\t" \
+                 : \
+                 "=&r" (intRes) \
+                 : \
+                 "d" (longIn1), \
+                 "d" (longIn2) \
+                 : \
+                 "r26" , "r27" \
+               )
+
+// intRes = intIn1 * intIn2 >> 16
+// uses:
+// r26 to store 0
+// r27 to store the byte 1 of the 24 bit result
+#define MultiU16X8toH16(intRes, charIn1, intIn2) \
+  asm volatile ( \
+                 "clr r26 \n\t" \
+                 "mul %A1, %B2 \n\t" \
+                 "movw %A0, r0 \n\t" \
+                 "mul %A1, %A2 \n\t" \
+                 "add %A0, r1 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "lsr r0 \n\t" \
+                 "adc %A0, r26 \n\t" \
+                 "adc %B0, r26 \n\t" \
+                 "clr r1 \n\t" \
+                 : \
+                 "=&r" (intRes) \
+                 : \
+                 "d" (charIn1), \
+                 "d" (intIn2) \
+                 : \
+                 "r26" \
+               )
+
+
+#endif
diff --git a/Marlin/src/HAL/HAL_AVR/persistent_store.cpp b/Marlin/src/HAL/HAL_AVR/persistent_store.cpp
new file mode 100644
index 0000000000..84d0b947f8
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/persistent_store.cpp
@@ -0,0 +1,57 @@
+#include "../persistent_store_api.h"
+
+#include "../../../types.h"
+#include "../../../language.h"
+#include "../../../serial.h"
+#include "../../../utility.h"
+
+#ifdef ARDUINO_ARCH_AVR
+#if ENABLED(EEPROM_SETTINGS)
+
+namespace HAL {
+namespace PersistentStore {
+
+bool access_start() {
+  return true;
+}
+
+bool access_finish(){
+  return true;
+}
+
+bool write_data(int &pos, const uint8_t *value, uint16_t size, uint16_t *crc) {
+  while (size--) {
+    uint8_t * const p = (uint8_t * const)pos;
+    uint8_t v = *value;
+    // EEPROM has only ~100,000 write cycles,
+    // so only write bytes that have changed!
+    if (v != eeprom_read_byte(p)) {
+      eeprom_write_byte(p, v);
+      if (eeprom_read_byte(p) != v) {
+        SERIAL_ECHO_START();
+        SERIAL_ECHOLNPGM(MSG_ERR_EEPROM_WRITE);
+        return false;
+      }
+    }
+    crc16(crc, &v, 1);
+    pos++;
+    value++;
+  };
+  return true;
+}
+
+void read_data(int &pos, uint8_t* value, uint16_t size, uint16_t *crc) {
+  do {
+    uint8_t c = eeprom_read_byte((unsigned char*)pos);
+    *value = c;
+    crc16(crc, &c, 1);
+    pos++;
+    value++;
+  } while (--size);
+}
+
+}
+}
+
+#endif // EEPROM_SETTINGS
+#endif // ARDUINO_ARCH_AVR
diff --git a/Marlin/pinsDebug_Teensyduino.h b/Marlin/src/HAL/HAL_AVR/pinsDebug_Teensyduino.h
similarity index 100%
rename from Marlin/pinsDebug_Teensyduino.h
rename to Marlin/src/HAL/HAL_AVR/pinsDebug_Teensyduino.h
diff --git a/Marlin/pinsDebug_plus_70.h b/Marlin/src/HAL/HAL_AVR/pinsDebug_plus_70.h
similarity index 100%
rename from Marlin/pinsDebug_plus_70.h
rename to Marlin/src/HAL/HAL_AVR/pinsDebug_plus_70.h
diff --git a/Marlin/servo.cpp b/Marlin/src/HAL/HAL_AVR/servo_AVR.cpp
similarity index 62%
rename from Marlin/servo.cpp
rename to Marlin/src/HAL/HAL_AVR/servo_AVR.cpp
index d6b7b700f9..ab0a7ee617 100644
--- a/Marlin/servo.cpp
+++ b/Marlin/src/HAL/HAL_AVR/servo_AVR.cpp
@@ -20,6 +20,7 @@
  *
  */
 
+
 /**
  * servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
  * Copyright (c) 2009 Michael Margolis.  All right reserved.
@@ -50,36 +51,21 @@
  * detach()              - Stop an attached servo from pulsing its i/o pin.
  *
  */
-#include "MarlinConfig.h"
+
+#ifdef ARDUINO_ARCH_AVR
+
+#include "../../../MarlinConfig.h"
 
 #if HAS_SERVOS
 
 #include <avr/interrupt.h>
 #include <Arduino.h>
 
-#include "servo.h"
-
-#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
-#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
-
-#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
-
-//#define NBR_TIMERS        ((MAX_SERVOS) / (SERVOS_PER_TIMER))
+#include "../servo.h"
+#include "../servo_private.h"
 
-static ServoInfo_t servo_info[MAX_SERVOS];                  // static array of servo info structures
 static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
 
-uint8_t ServoCount = 0;                                     // the total number of attached servos
-
-
-// convenience macros
-#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo
-#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER))       // returns the index of the servo on this timer
-#define SERVO_INDEX(_timer,_channel)  ((_timer*(SERVOS_PER_TIMER)) + _channel)     // macro to access servo index by timer and channel
-#define SERVO(_timer,_channel)  (servo_info[SERVO_INDEX(_timer,_channel)])       // macro to access servo class by timer and channel
-
-#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
-#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
 
 /************ static functions common to all instances ***********************/
 
@@ -138,8 +124,9 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t
 
 #endif // WIRING
 
+/****************** end of static functions ******************************/
 
-static void initISR(timer16_Sequence_t timer) {
+void initISR(timer16_Sequence_t timer) {
   #if ENABLED(_useTimer1)
     if (timer == _timer1) {
       TCCR1A = 0;             // normal counting mode
@@ -198,7 +185,7 @@ static void initISR(timer16_Sequence_t timer) {
   #endif
 }
 
-static void finISR(timer16_Sequence_t timer) {
+void finISR(timer16_Sequence_t timer) {
   // Disable use of the given timer
   #ifdef WIRING
     if (timer == _timer1) {
@@ -227,96 +214,6 @@ static void finISR(timer16_Sequence_t timer) {
   #endif
 }
 
-static bool isTimerActive(timer16_Sequence_t timer) {
-  // returns true if any servo is active on this timer
-  for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++) {
-    if (SERVO(timer, channel).Pin.isActive)
-      return true;
-  }
-  return false;
-}
-
-
-/****************** end of static functions ******************************/
-
-Servo::Servo() {
-  if (ServoCount < MAX_SERVOS) {
-    this->servoIndex = ServoCount++;                    // assign a servo index to this instance
-    servo_info[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
-  }
-  else
-    this->servoIndex = INVALID_SERVO;  // too many servos
-}
-
-int8_t Servo::attach(int pin) {
-  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
-}
-
-int8_t Servo::attach(int pin, int min, int max) {
-
-  if (this->servoIndex >= MAX_SERVOS) return -1;
-
-  if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin;
-  pinMode(servo_info[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
-
-  // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
-  this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
-  this->max = (MAX_PULSE_WIDTH - max) / 4;
-
-  // initialize the timer if it has not already been initialized
-  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
-  if (!isTimerActive(timer)) initISR(timer);
-  servo_info[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
-
-  return this->servoIndex;
-}
-
-void Servo::detach() {
-  servo_info[this->servoIndex].Pin.isActive = false;
-  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
-  if (!isTimerActive(timer)) finISR(timer);
-}
-
-void Servo::write(int value) {
-  if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
-    value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
-  }
-  this->writeMicroseconds(value);
-}
-
-void Servo::writeMicroseconds(int value) {
-  // calculate and store the values for the given channel
-  byte channel = this->servoIndex;
-  if (channel < MAX_SERVOS) {  // ensure channel is valid
-    // ensure pulse width is valid
-    value = constrain(value, SERVO_MIN(), SERVO_MAX()) - (TRIM_DURATION);
-    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
-
-    CRITICAL_SECTION_START;
-    servo_info[channel].ticks = value;
-    CRITICAL_SECTION_END;
-  }
-}
-
-// return the value as degrees
-int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
-
-int Servo::readMicroseconds() {
-  return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[this->servoIndex].ticks) + TRIM_DURATION;
-}
-
-bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
-
-void Servo::move(int value) {
-  constexpr uint16_t servo_delay[] = SERVO_DELAY;
-  static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
-  if (this->attach(0) >= 0) {
-    this->write(value);
-    delay(servo_delay[this->servoIndex]);
-    #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
-      this->detach();
-    #endif
-  }
-}
+#endif // HAS_SERVOS
 
-#endif
+#endif // ARDUINO_ARCH_AVR
diff --git a/Marlin/src/HAL/HAL_AVR/spi_pins.h b/Marlin/src/HAL/HAL_AVR/spi_pins.h
new file mode 100644
index 0000000000..444534880c
--- /dev/null
+++ b/Marlin/src/HAL/HAL_AVR/spi_pins.h
@@ -0,0 +1,67 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#ifndef SPI_PINS_H_
+#define SPI_PINS_H_
+
+/**
+ * Define SPI Pins: SCK, MISO, MOSI, SS
+ */
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__)
+  #define AVR_SCK_PIN  13
+  #define AVR_MISO_PIN 12
+  #define AVR_MOSI_PIN 11
+  #define AVR_SS_PIN   10
+#elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__)
+  #define AVR_SCK_PIN  7
+  #define AVR_MISO_PIN 6
+  #define AVR_MOSI_PIN 5
+  #define AVR_SS_PIN   4
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  #define AVR_SCK_PIN  52
+  #define AVR_MISO_PIN 50
+  #define AVR_MOSI_PIN 51
+  #define AVR_SS_PIN   53
+#elif defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__)
+  #define AVR_SCK_PIN  21
+  #define AVR_MISO_PIN 23
+  #define AVR_MOSI_PIN 22
+  #define AVR_SS_PIN   20
+#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+  #define AVR_SCK_PIN  10
+  #define AVR_MISO_PIN 12
+  #define AVR_MOSI_PIN 11
+  #define AVR_SS_PIN   16
+#endif
+
+#ifndef SCK_PIN
+  #define SCK_PIN  AVR_SCK_PIN
+#endif
+#ifndef MISO_PIN
+  #define MISO_PIN AVR_MISO_PIN
+#endif
+#ifndef MOSI_PIN
+  #define MOSI_PIN AVR_MOSI_PIN
+#endif
+#ifndef SS_PIN
+  #define SS_PIN   AVR_SS_PIN
+#endif
+
+
+#endif /* SPI_PINS_H_ */
diff --git a/Marlin/watchdog.cpp b/Marlin/src/HAL/HAL_AVR/watchdog_AVR.cpp
similarity index 95%
rename from Marlin/watchdog.cpp
rename to Marlin/src/HAL/HAL_AVR/watchdog_AVR.cpp
index 5e42b5faca..40d7bdf724 100644
--- a/Marlin/watchdog.cpp
+++ b/Marlin/src/HAL/HAL_AVR/watchdog_AVR.cpp
@@ -20,11 +20,13 @@
  *
  */
 
-#include "Marlin.h"
+#ifdef ARDUINO_ARCH_AVR
+
+#include "../../../Marlin.h"
 
 #if ENABLED(USE_WATCHDOG)
 
-#include "watchdog.h"
+#include "watchdog_AVR.h"
 
 // Initialize watchdog with a 4 sec interrupt time
 void watchdog_init() {
@@ -54,3 +56,4 @@ void watchdog_init() {
 #endif // WATCHDOG_RESET_MANUAL
 
 #endif // USE_WATCHDOG
+#endif // ARDUINO_ARCH_AVR
diff --git a/Marlin/watchdog.h b/Marlin/src/HAL/HAL_AVR/watchdog_AVR.h
similarity index 93%
rename from Marlin/watchdog.h
rename to Marlin/src/HAL/HAL_AVR/watchdog_AVR.h
index 2c04b58972..e3572fe749 100644
--- a/Marlin/watchdog.h
+++ b/Marlin/src/HAL/HAL_AVR/watchdog_AVR.h
@@ -20,10 +20,11 @@
  *
  */
 
-#ifndef WATCHDOG_H
-#define WATCHDOG_H
+#ifndef WATCHDOG_AVR_H
+#define WATCHDOG_AVR_H
+
+//#include "../../../Marlin.h"
 
-#include "Marlin.h"
 #include <avr/wdt.h>
 
 // Initialize watchdog with a 4 second interrupt time
diff --git a/Marlin/src/HAL/HAL_endstop_interrupts.h b/Marlin/src/HAL/HAL_endstop_interrupts.h
new file mode 100644
index 0000000000..ab0652ea76
--- /dev/null
+++ b/Marlin/src/HAL/HAL_endstop_interrupts.h
@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+
+#ifndef HAL_ENDSTOP_INTERRUPTS_H_
+#define HAL_ENDSTOP_INTERRUPTS_H_
+
+volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+                            // Must be reset to 0 by the test function when finished.
+
+// This is what is really done inside the interrupts.
+FORCE_INLINE void endstop_ISR_worker( void ) {
+  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
+}
+
+// One ISR for all EXT-Interrupts
+void endstop_ISR(void) { endstop_ISR_worker(); }
+
+#ifdef ARDUINO_ARCH_AVR
+
+  #include "HAL_AVR/endstop_interrupts.h"
+
+#elif defined(ARDUINO_ARCH_SAM)
+
+  #include "HAL_DUE/endstop_interrupts.h"
+
+#elif IS_32BIT_TEENSY
+
+  #include "HAL_TEENSY35_36/endstop_interrupts.h"
+
+#else
+
+  #error Unsupported Platform!
+
+#endif
+
+#endif /* HAL_ENDSTOP_INTERRUPTS_H_ */
diff --git a/Marlin/src/HAL/HAL_pinsDebug.h b/Marlin/src/HAL/HAL_pinsDebug.h
new file mode 100644
index 0000000000..48c95438e9
--- /dev/null
+++ b/Marlin/src/HAL/HAL_pinsDebug.h
@@ -0,0 +1,35 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+#ifndef HAL_PINSDEBUG_H
+
+#ifdef ARDUINO_ARCH_AVR
+  #include "HAL_AVR/HAL_pinsDebug_AVR.h"
+#elif defined(ARDUINO_ARCH_SAM)
+  #include "HAL_DUE/HAL_pinsDebug_Due.h"
+#elif IS_32BIT_TEENSY
+  #include "HAL_TEENSY35_36/HAL_pinsDebug_Teensy.h"
+#else
+  #error Unsupported Platform!
+#endif
+
+#endif
diff --git a/Marlin/src/HAL/HAL_spi_pins.h b/Marlin/src/HAL/HAL_spi_pins.h
new file mode 100644
index 0000000000..aac2540291
--- /dev/null
+++ b/Marlin/src/HAL/HAL_spi_pins.h
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+
+#ifndef HAL_SPI_PINS_H_
+#define HAL_SPI_PINS_H_
+
+#include "MarlinConfig.h"
+
+#ifdef ARDUINO_ARCH_SAM
+  #include "HAL_DUE/spi_pins.h"
+
+#elif defined(IS_32BIT_TEENSY)
+  #include "HAL_TEENSY35_36/spi_pins.h"
+
+#elif defined(ARDUINO_ARCH_AVR)
+  #include "HAL_AVR/spi_pins.h"
+
+#else
+  #error "Unsupported Platform!"
+#endif
+
+#endif /* HAL_SPI_PINS_H_ */
diff --git a/Marlin/src/HAL/I2cEeprom.cpp b/Marlin/src/HAL/I2cEeprom.cpp
new file mode 100644
index 0000000000..8bdd00967f
--- /dev/null
+++ b/Marlin/src/HAL/I2cEeprom.cpp
@@ -0,0 +1,162 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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: functions for I2C connected external EEPROM.
+ * Not platform dependent.
+ */
+
+#include "../../MarlinConfig.h"
+
+#if ENABLED(I2C_EEPROM)
+
+// --------------------------------------------------------------------------
+// Includes
+// --------------------------------------------------------------------------
+
+#include "HAL.h"
+#include <Wire.h>
+
+// --------------------------------------------------------------------------
+// Externals
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Local defines
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Types
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Variables
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Public Variables
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Private Variables
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Function prototypes
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Private functions
+// --------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------
+// Public functions
+// --------------------------------------------------------------------------
+
+static bool eeprom_initialised = false;
+static uint8_t eeprom_device_address = 0x50;
+
+static void eeprom_init(void) {
+  if (!eeprom_initialised) {
+    Wire.begin();
+    eeprom_initialised = true;
+  }
+}
+
+void eeprom_write_byte(unsigned char *pos, unsigned char value) {
+  unsigned eeprom_address = (unsigned) pos;
+
+  eeprom_init();
+
+  Wire.beginTransmission(eeprom_device_address);
+  Wire.write((int)(eeprom_address >> 8));   // MSB
+  Wire.write((int)(eeprom_address & 0xFF)); // LSB
+  Wire.write(value);
+  Wire.endTransmission();
+
+  // wait for write cycle to complete
+  // this could be done more efficiently with "acknowledge polling"
+  delay(5);
+}
+
+// WARNING: address is a page address, 6-bit end will wrap around
+// also, data can be maximum of about 30 bytes, because the Wire library has a buffer of 32 bytes
+void eeprom_update_block(const void* pos, void* eeprom_address, size_t n) {
+  uint8_t eeprom_temp[32] = {0};
+  uint8_t flag = 0;
+
+  eeprom_init();
+
+  Wire.beginTransmission(eeprom_device_address);
+  Wire.write((int)((unsigned)eeprom_address >> 8));   // MSB
+  Wire.write((int)((unsigned)eeprom_address & 0xFF)); // LSB
+  Wire.endTransmission();
+  Wire.requestFrom(eeprom_device_address, (byte)n);
+  for (byte c = 0; c < n; c++) {
+    if (Wire.available()) eeprom_temp[c] = Wire.read();
+    flag |= (eeprom_temp[c] ^ *((uint8_t*)pos + c));
+  }
+
+  if (flag) {
+    Wire.beginTransmission(eeprom_device_address);
+    Wire.write((int)((unsigned)eeprom_address >> 8));   // MSB
+    Wire.write((int)((unsigned)eeprom_address & 0xFF)); // LSB
+    Wire.write((uint8_t*)(pos), n);
+    Wire.endTransmission();
+
+    // wait for write cycle to complete
+    // this could be done more efficiently with "acknowledge polling"
+    delay(5);
+  }
+}
+
+
+unsigned char eeprom_read_byte(unsigned char *pos) {
+  byte data = 0xFF;
+  unsigned eeprom_address = (unsigned) pos;
+
+  eeprom_init ();
+
+  Wire.beginTransmission(eeprom_device_address);
+  Wire.write((int)(eeprom_address >> 8));   // MSB
+  Wire.write((int)(eeprom_address & 0xFF)); // LSB
+  Wire.endTransmission();
+  Wire.requestFrom(eeprom_device_address, (byte)1);
+  if (Wire.available())
+    data = Wire.read();
+  return data;
+}
+
+// maybe let's not read more than 30 or 32 bytes at a time!
+void eeprom_read_block(void* pos, const void* eeprom_address, size_t n) {
+  eeprom_init();
+
+  Wire.beginTransmission(eeprom_device_address);
+  Wire.write((int)((unsigned)eeprom_address >> 8));   // MSB
+  Wire.write((int)((unsigned)eeprom_address & 0xFF)); // LSB
+  Wire.endTransmission();
+  Wire.requestFrom(eeprom_device_address, (byte)n);
+  for (byte c = 0; c < n; c++ )
+    if (Wire.available()) *((uint8_t*)pos + c) = Wire.read();
+}
+
+
+#endif // ENABLED(I2C_EEPROM)
+
diff --git a/Marlin/src/HAL/SpiEeprom.cpp b/Marlin/src/HAL/SpiEeprom.cpp
new file mode 100644
index 0000000000..365d91d191
--- /dev/null
+++ b/Marlin/src/HAL/SpiEeprom.cpp
@@ -0,0 +1,117 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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: functions for SPI connected external EEPROM.
+ * Not platform dependent.
+ */
+
+#include "../../MarlinConfig.h"
+
+#if ENABLED(SPI_EEPROM)
+
+#include "HAL.h"
+
+#define CMD_WREN  6   // WREN
+#define CMD_READ  2   // WRITE
+#define CMD_WRITE 2   // WRITE
+
+uint8_t eeprom_read_byte(uint8_t* pos) {
+  uint8_t v;
+  uint8_t eeprom_temp[3];
+
+  // set read location
+  // begin transmission from device
+  eeprom_temp[0] = CMD_READ;
+  eeprom_temp[1] = ((unsigned)pos>>8) & 0xFF; // addr High
+  eeprom_temp[2] = (unsigned)pos& 0xFF;       // addr Low
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  digitalWrite(SPI_EEPROM1_CS, LOW);
+  spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3);
+
+  v = spiRec(SPI_CHAN_EEPROM1);
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  return v;
+}
+
+
+void eeprom_read_block(void* dest, const void* eeprom_address, size_t n) {
+  uint8_t eeprom_temp[3];
+
+  // set read location
+  // begin transmission from device
+  eeprom_temp[0] = CMD_READ;
+  eeprom_temp[1] = ((unsigned)eeprom_address>>8) & 0xFF; // addr High
+  eeprom_temp[2] = (unsigned)eeprom_address& 0xFF;       // addr Low
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  digitalWrite(SPI_EEPROM1_CS, LOW);
+  spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3);
+
+  uint8_t *p_dest = (uint8_t *)dest;
+  while (n--)
+    *p_dest++ = spiRec(SPI_CHAN_EEPROM1);
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+}
+
+void eeprom_write_byte(uint8_t* pos, uint8_t value) {
+  uint8_t eeprom_temp[3];
+
+  /*write enable*/
+  eeprom_temp[0] = CMD_WREN;
+  digitalWrite(SPI_EEPROM1_CS, LOW);
+  spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 1);
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  delay(1);
+
+  /*write addr*/
+  eeprom_temp[0] = CMD_WRITE;
+  eeprom_temp[1] = ((unsigned)pos>>8) & 0xFF;  //addr High
+  eeprom_temp[2] = (unsigned)pos & 0xFF;       //addr Low
+  digitalWrite(SPI_EEPROM1_CS, LOW);
+  spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3);
+
+  spiSend(SPI_CHAN_EEPROM1, value);
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  delay(7);   // wait for page write to complete
+}
+
+void eeprom_update_block(const void* src, void* eeprom_address, size_t n) {
+  uint8_t eeprom_temp[3];
+
+  /*write enable*/
+  eeprom_temp[0] = CMD_WREN;
+  digitalWrite(SPI_EEPROM1_CS, LOW);
+  spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 1);
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  delay(1);
+
+  /*write addr*/
+  eeprom_temp[0] = CMD_WRITE;
+  eeprom_temp[1] = ((unsigned)eeprom_address>>8) & 0xFF;  //addr High
+  eeprom_temp[2] = (unsigned)eeprom_address & 0xFF;       //addr Low
+  digitalWrite(SPI_EEPROM1_CS, LOW);
+  spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3);
+
+  spiSend(SPI_CHAN_EEPROM1, (const uint8_t*)src, n);
+  digitalWrite(SPI_EEPROM1_CS, HIGH);
+  delay(7);   // wait for page write to complete
+}
+
+
+#endif // ENABLED(SPI_EEPROM)
diff --git a/Marlin/src/HAL/math_32bit.h b/Marlin/src/HAL/math_32bit.h
new file mode 100644
index 0000000000..8586c00006
--- /dev/null
+++ b/Marlin/src/HAL/math_32bit.h
@@ -0,0 +1,33 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+#ifndef MATH_32BIT_H
+#define MATH_32BIT_H
+
+/**
+ * Math helper functions for 32 bit CPUs
+ */
+
+#define MultiU32X32toH32(intRes, longIn1, longIn2) intRes = ((uint64_t)longIn1 * longIn2 + 0x80000000) >> 32
+#define MultiU32X24toH32(intRes, longIn1, longIn2) intRes = ((uint64_t)longIn1 * longIn2 + 0x00800000) >> 24
+
+#endif
diff --git a/Marlin/src/HAL/persistent_store_api.h b/Marlin/src/HAL/persistent_store_api.h
new file mode 100644
index 0000000000..7d548704d8
--- /dev/null
+++ b/Marlin/src/HAL/persistent_store_api.h
@@ -0,0 +1,19 @@
+#ifndef _PERSISTENT_STORE_H_
+#define _PERSISTENT_STORE_H_
+
+#include <stddef.h>
+#include <stdint.h>
+
+namespace HAL {
+namespace PersistentStore {
+
+bool access_start();
+bool access_finish();
+bool write_data(int &pos, const uint8_t *value, uint16_t size, uint16_t *crc);
+void read_data(int &pos, uint8_t* value, uint16_t size, uint16_t *crc) ;
+
+}
+}
+
+
+#endif /* _PERSISTANT_STORE_H_ */
diff --git a/Marlin/src/HAL/servo.cpp b/Marlin/src/HAL/servo.cpp
new file mode 100644
index 0000000000..0e8af80843
--- /dev/null
+++ b/Marlin/src/HAL/servo.cpp
@@ -0,0 +1,168 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+
+/**
+ * servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+ * Copyright (c) 2009 Michael Margolis.  All right reserved.
+ */
+
+/**
+ * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
+ * The servos are pulsed in the background using the value most recently written using the write() method
+ *
+ * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
+ * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
+ *
+ * The methods are:
+ *
+ * Servo - Class for manipulating servo motors connected to Arduino pins.
+ *
+ * attach(pin)           - Attach a servo motor to an i/o pin.
+ * attach(pin, min, max) - Attach to a pin, setting min and max values in microseconds
+ *                         Default min is 544, max is 2400
+ *
+ * write()               - Set the servo angle in degrees. (Invalid angles —over MIN_PULSE_WIDTH— are treated as µs.)
+ * writeMicroseconds()   - Set the servo pulse width in microseconds.
+ * move(pin, angle)      - Sequence of attach(pin), write(angle), delay(SERVO_DELAY).
+ *                         With DEACTIVATE_SERVOS_AFTER_MOVE it detaches after SERVO_DELAY.
+ * read()                - Get the last-written servo pulse width as an angle between 0 and 180.
+ * readMicroseconds()    - Get the last-written servo pulse width in microseconds.
+ * attached()            - Return true if a servo is attached.
+ * detach()              - Stop an attached servo from pulsing its i/o pin.
+ *
+ */
+
+
+#include "../../MarlinConfig.h"
+
+#include "HAL.h"
+
+#if HAS_SERVOS && !IS_32BIT_TEENSY
+
+//#include <Arduino.h>
+
+#include "servo.h"
+#include "servo_private.h"
+
+ServoInfo_t servo_info[MAX_SERVOS];                  // static array of servo info structures
+uint8_t ServoCount = 0;                              // the total number of attached servos
+
+
+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
+
+/************ static functions common to all instances ***********************/
+
+static boolean isTimerActive(timer16_Sequence_t timer) {
+  // returns true if any servo is active on this timer
+  for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++) {
+    if (SERVO(timer, channel).Pin.isActive)
+      return true;
+  }
+  return false;
+}
+
+/****************** end of static functions ******************************/
+
+Servo::Servo() {
+  if (ServoCount < MAX_SERVOS) {
+    this->servoIndex = ServoCount++;                    // assign a servo index to this instance
+    servo_info[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
+  }
+  else
+    this->servoIndex = INVALID_SERVO;  // too many servos
+}
+
+int8_t Servo::attach(int pin) {
+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+}
+
+int8_t Servo::attach(int pin, int min, int max) {
+
+  if (this->servoIndex >= MAX_SERVOS) return -1;
+
+  if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin;
+  pinMode(servo_info[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
+
+  // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
+  this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
+  this->max = (MAX_PULSE_WIDTH - max) / 4;
+
+  // initialize the timer if it has not already been initialized
+  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if (!isTimerActive(timer)) initISR(timer);
+  servo_info[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
+
+  return this->servoIndex;
+}
+
+void Servo::detach() {
+  servo_info[this->servoIndex].Pin.isActive = false;
+  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if (!isTimerActive(timer)) finISR(timer);
+}
+
+void Servo::write(int value) {
+  if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+    value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
+  }
+  this->writeMicroseconds(value);
+}
+
+void Servo::writeMicroseconds(int value) {
+  // calculate and store the values for the given channel
+  byte channel = this->servoIndex;
+  if (channel < MAX_SERVOS) {  // ensure channel is valid
+    // ensure pulse width is valid
+    value = constrain(value, SERVO_MIN(), SERVO_MAX()) - (TRIM_DURATION);
+    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
+
+    CRITICAL_SECTION_START;
+    servo_info[channel].ticks = value;
+    CRITICAL_SECTION_END;
+  }
+}
+
+// return the value as degrees
+int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
+
+int Servo::readMicroseconds() {
+  return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[this->servoIndex].ticks) + TRIM_DURATION;
+}
+
+bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
+
+void Servo::move(int value) {
+  constexpr uint16_t servo_delay[] = SERVO_DELAY;
+  static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
+  if (this->attach(0) >= 0) {
+    this->write(value);
+    delay(servo_delay[this->servoIndex]);
+    #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
+      this->detach();
+    #endif
+  }
+}
+
+#endif // HAS_SERVOS
+
diff --git a/Marlin/servo.h b/Marlin/src/HAL/servo.h
similarity index 70%
rename from Marlin/servo.h
rename to Marlin/src/HAL/servo.h
index ad75b6baab..8d099f311f 100644
--- a/Marlin/servo.h
+++ b/Marlin/src/HAL/servo.h
@@ -69,74 +69,20 @@
 #ifndef servo_h
 #define servo_h
 
-#include <inttypes.h>
+#if IS_32BIT_TEENSY
+  #include "HAL_TEENSY35_36/HAL_Servo_Teensy.h" // Teensy HAL uses an inherited library
+#else
 
-/**
- * Defines for 16 bit timers used with  Servo library
- *
- * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
- * timer16_Sequence_t enumerates the sequence that the timers should be allocated
- * _Nbr_16timers indicates how many 16 bit timers are available.
- *
- */
+#include <inttypes.h>
 
-// Say which 16 bit timers can be used and in what order
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-  //#define _useTimer1 // Timer 1 is used by the stepper ISR
-  #define _useTimer3
-  #define _useTimer4
-  #if !HAS_MOTOR_CURRENT_PWM
-    #define _useTimer5 // Timer 5 is used for motor current PWM and can't be used for servos.
-  #endif
-#elif defined(__AVR_ATmega32U4__)
-  #define _useTimer3
-#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
-  #define _useTimer3
-#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__)
-  #define _useTimer3
+#if defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_ARCH_SAM)
+  // we're good to go
 #else
-  // everything else
+  #error "This library only supports boards with an AVR or SAM3X processor."
 #endif
 
-typedef enum {
-  #if ENABLED(_useTimer1)
-    _timer1,
-  #endif
-  #if ENABLED(_useTimer3)
-    _timer3,
-  #endif
-  #if ENABLED(_useTimer4)
-    _timer4,
-  #endif
-  #if ENABLED(_useTimer5)
-    _timer5,
-  #endif
-  _Nbr_16timers
-} timer16_Sequence_t;
-
-
 #define Servo_VERSION           2     // software version of this library
 
-#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
-#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
-#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
-#define REFRESH_INTERVAL    20000     // minimum time to refresh servos in microseconds
-
-#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer
-#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
-
-#define INVALID_SERVO         255     // flag indicating an invalid servo index
-
-typedef struct {
-  uint8_t nbr        : 6 ;            // a pin number from 0 to 63
-  uint8_t isActive   : 1 ;            // true if this channel is enabled, pin not pulsed if false
-} ServoPin_t;
-
-typedef struct {
-  ServoPin_t Pin;
-  unsigned int ticks;
-} ServoInfo_t;
-
 class Servo {
   public:
     Servo();
@@ -158,4 +104,6 @@ class Servo {
     int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
 };
 
+#endif // !TEENSY
+
 #endif
diff --git a/Marlin/src/HAL/servo_private.h b/Marlin/src/HAL/servo_private.h
new file mode 100644
index 0000000000..3d8c50c0df
--- /dev/null
+++ b/Marlin/src/HAL/servo_private.h
@@ -0,0 +1,103 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * 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/>.
+ *
+ */
+
+/**
+  servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library 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
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#ifndef servo_private_h
+#define servo_private_h
+
+#include <inttypes.h>
+
+// Architecture specific include
+#ifdef ARDUINO_ARCH_AVR
+  #include "HAL_AVR/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_SAM)
+  #include "HAL_DUE/ServoTimers.h"
+#else
+  #error "This library only supports boards with an AVR or SAM3X processor."
+#endif
+
+// Macros
+
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+#define REFRESH_INTERVAL    20000     // minimum time to refresh servos in microseconds
+
+#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer
+#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
+
+#define INVALID_SERVO         255     // flag indicating an invalid servo index
+
+//
+#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / PRESCALER)     // converts microseconds to tick (PRESCALER depends on architecture)
+#define ticksToUs(_ticks) (( (unsigned)_ticks * PRESCALER)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
+
+//#define NBR_TIMERS        ((MAX_SERVOS) / (SERVOS_PER_TIMER))
+
+// convenience macros
+#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo
+#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER))       // returns the index of the servo on this timer
+#define SERVO_INDEX(_timer,_channel)  ((_timer*(SERVOS_PER_TIMER)) + _channel)     // macro to access servo index by timer and channel
+#define SERVO(_timer,_channel)  (servo_info[SERVO_INDEX(_timer,_channel)])       // macro to access servo class by timer and channel
+
+// Types
+
+typedef struct {
+  uint8_t nbr        : 6 ;            // a pin number from 0 to 63
+  uint8_t isActive   : 1 ;            // true if this channel is enabled, pin not pulsed if false
+} ServoPin_t;
+
+typedef struct {
+  ServoPin_t Pin;
+  unsigned int ticks;
+} ServoInfo_t;
+
+// Global variables
+
+extern uint8_t ServoCount;
+extern ServoInfo_t servo_info[MAX_SERVOS];
+
+// Public functions
+
+extern void initISR(timer16_Sequence_t timer);
+extern void finISR(timer16_Sequence_t timer);
+
+
+#endif
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 600bc39257..c3d9808ec0 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -48,11 +48,16 @@
 #include "stepper.h"
 #include "endstops.h"
 #include "planner.h"
+#if MB(ALLIGATOR)
+  #include "dac_dac084s085.h"
+#endif
 #include "temperature.h"
 #include "ultralcd.h"
 #include "language.h"
 #include "cardreader.h"
-#include "speed_lookuptable.h"
+#ifdef ARDUINO_ARCH_AVR
+  #include "speed_lookuptable.h"
+#endif
 
 #if HAS_DIGIPOTSS
   #include <SPI.h>
@@ -99,11 +104,11 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
 
 #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
 
-  constexpr uint16_t ADV_NEVER = 65535;
+  constexpr HAL_TIMER_TYPE ADV_NEVER = HAL_TIMER_TYPE_MAX;
 
-  uint16_t Stepper::nextMainISR = 0,
-           Stepper::nextAdvanceISR = ADV_NEVER,
-           Stepper::eISR_Rate = ADV_NEVER;
+  HAL_TIMER_TYPE Stepper::nextMainISR = 0,
+         Stepper::nextAdvanceISR = ADV_NEVER,
+         Stepper::eISR_Rate = ADV_NEVER;
 
   #if ENABLED(LIN_ADVANCE)
     volatile int Stepper::e_steps[E_STEPPERS];
@@ -144,9 +149,9 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
   long Stepper::counter_m[MIXING_STEPPERS];
 #endif
 
-unsigned short Stepper::acc_step_rate; // needed for deceleration start point
+HAL_TIMER_TYPE Stepper::acc_step_rate; // needed for deceleration start point
 uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
-unsigned short Stepper::OCR1A_nominal;
+HAL_TIMER_TYPE Stepper::OCR1A_nominal;
 
 volatile long Stepper::endstops_trigsteps[XYZ];
 
@@ -213,66 +218,7 @@ volatile long Stepper::endstops_trigsteps[XYZ];
   #define E_APPLY_STEP(v,Q) E_STEP_WRITE(v)
 #endif
 
-// intRes = longIn1 * longIn2 >> 24
-// uses:
-// r26 to store 0
-// r27 to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
-// note that the lower two bytes and the upper byte of the 48bit result are not calculated.
-// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
-// B0 A0 are bits 24-39 and are the returned value
-// C1 B1 A1 is longIn1
-// D2 C2 B2 A2 is longIn2
-//
-#define MultiU24X32toH16(intRes, longIn1, longIn2) \
-  asm volatile ( \
-                 "clr r26 \n\t" \
-                 "mul %A1, %B2 \n\t" \
-                 "mov r27, r1 \n\t" \
-                 "mul %B1, %C2 \n\t" \
-                 "movw %A0, r0 \n\t" \
-                 "mul %C1, %C2 \n\t" \
-                 "add %B0, r0 \n\t" \
-                 "mul %C1, %B2 \n\t" \
-                 "add %A0, r0 \n\t" \
-                 "adc %B0, r1 \n\t" \
-                 "mul %A1, %C2 \n\t" \
-                 "add r27, r0 \n\t" \
-                 "adc %A0, r1 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "mul %B1, %B2 \n\t" \
-                 "add r27, r0 \n\t" \
-                 "adc %A0, r1 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "mul %C1, %A2 \n\t" \
-                 "add r27, r0 \n\t" \
-                 "adc %A0, r1 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "mul %B1, %A2 \n\t" \
-                 "add r27, r1 \n\t" \
-                 "adc %A0, r26 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "lsr r27 \n\t" \
-                 "adc %A0, r26 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "mul %D2, %A1 \n\t" \
-                 "add %A0, r0 \n\t" \
-                 "adc %B0, r1 \n\t" \
-                 "mul %D2, %B1 \n\t" \
-                 "add %B0, r0 \n\t" \
-                 "clr r1 \n\t" \
-                 : \
-                 "=&r" (intRes) \
-                 : \
-                 "d" (longIn1), \
-                 "d" (longIn2) \
-                 : \
-                 "r26" , "r27" \
-               )
-
-// Some useful constants
-
-#define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
+
 
 /**
  *         __________________________
@@ -345,6 +291,8 @@ void Stepper::set_directions() {
  * Stepper Driver Interrupt
  *
  * Directly pulses the stepper motors at high frequency.
+ *
+ * AVR :
  * Timer 1 runs at a base frequency of 2MHz, with this ISR using OCR1A compare mode.
  *
  * OCR1A   Frequency
@@ -355,7 +303,9 @@ void Stepper::set_directions() {
  *  2000     1 KHz - sleep rate
  *  4000   500  Hz - init rate
  */
-ISR(TIMER1_COMPA_vect) {
+
+HAL_STEP_TIMER_ISR {
+  HAL_timer_isr_prologue(STEP_TIMER_NUM);
   #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
     Stepper::advance_isr_scheduler();
   #else
@@ -363,23 +313,23 @@ ISR(TIMER1_COMPA_vect) {
   #endif
 }
 
-#define _ENABLE_ISRs() do { cli(); if (thermalManager.in_temp_isr) CBI(TIMSK0, OCIE0B); else SBI(TIMSK0, OCIE0B); ENABLE_STEPPER_DRIVER_INTERRUPT(); } while(0)
-
 void Stepper::isr() {
 
-  uint16_t ocr_val;
+  HAL_TIMER_TYPE ocr_val;
 
   #define ENDSTOP_NOMINAL_OCR_VAL 3000    // check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
   #define OCR_VAL_TOLERANCE 1000          // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms
 
   #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
     // Disable Timer0 ISRs and enable global ISR again to capture UART events (incoming chars)
-    CBI(TIMSK0, OCIE0B); // Temperature ISR
+    DISABLE_TEMPERATURE_INTERRUPT(); // Temperature ISR
     DISABLE_STEPPER_DRIVER_INTERRUPT();
-    sei();
+    #if !defined(CPU_32_BIT)
+      sei();
+    #endif
   #endif
 
-  #define _SPLIT(L) (ocr_val = (uint16_t)L)
+  #define _SPLIT(L) (ocr_val = (HAL_TIMER_TYPE)L)
   #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
     #define SPLIT(L) _SPLIT(L)
   #else                 // sample endstops in between step pulses
@@ -405,10 +355,13 @@ void Stepper::isr() {
       }
 
       _NEXT_ISR(ocr_val);
+      #ifdef CPU_32_BIT
+        //todo: HAL?
+      #else
+        NOLESS(OCR1A, TCNT1 + 16);
+      #endif
 
-      NOLESS(OCR1A, TCNT1 + 16);
-
-      _ENABLE_ISRs(); // re-enable ISRs
+      HAL_ENABLE_ISRs(); // re-enable ISRs
       return;
     }
   #endif
@@ -420,8 +373,8 @@ void Stepper::isr() {
     #ifdef SD_FINISHED_RELEASECOMMAND
       if (!cleaning_buffer_counter && (SD_FINISHED_STEPPERRELEASE)) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
     #endif
-    _NEXT_ISR(200); // Run at max speed - 10 KHz
-    _ENABLE_ISRs(); // re-enable ISRs
+    _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 10000); // Run at max speed - 10 KHz
+    HAL_ENABLE_ISRs(); // re-enable ISRs
     return;
   }
 
@@ -450,8 +403,8 @@ void Stepper::isr() {
       #if ENABLED(Z_LATE_ENABLE)
         if (current_block->steps[Z_AXIS] > 0) {
           enable_Z();
-          _NEXT_ISR(2000); // Run at slow speed - 1 KHz
-          _ENABLE_ISRs(); // re-enable ISRs
+          _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
+          HAL_ENABLE_ISRs(); // re-enable ISRs
           return;
         }
       #endif
@@ -461,8 +414,8 @@ void Stepper::isr() {
       // #endif
     }
     else {
-      _NEXT_ISR(2000); // Run at slow speed - 1 KHz
-      _ENABLE_ISRs(); // re-enable ISRs
+      _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
+      HAL_ENABLE_ISRs(); // re-enable ISRs
       return;
     }
   }
@@ -613,7 +566,7 @@ void Stepper::isr() {
      * 10µs = 160 or 200 cycles.
      */
     #if EXTRA_CYCLES_XYZE > 20
-      uint32_t pulse_start = TCNT0;
+      uint32_t pulse_start = HAL_timer_get_current_count(STEP_TIMER_NUM);
     #endif
 
     #if HAS_X_STEP
@@ -645,8 +598,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_current_count(STEP_TIMER_NUM) - pulse_start) * (STEPPER_TIMER_PRESCALE)) { /* nada */ }
+      pulse_start = HAL_timer_get_current_count(STEP_TIMER_NUM);
     #elif EXTRA_CYCLES_XYZE > 0
       DELAY_NOPS(EXTRA_CYCLES_XYZE);
     #endif
@@ -686,7 +639,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_current_count(STEP_TIMER_NUM) - pulse_start) * (STEPPER_TIMER_PRESCALE)) { /* nada */ }
     #elif EXTRA_CYCLES_XYZE > 0
       if (i) DELAY_NOPS(EXTRA_CYCLES_XYZE);
     #endif
@@ -716,14 +669,18 @@ void Stepper::isr() {
   // Calculate new timer value
   if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
 
-    MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+    #ifdef CPU_32_BIT
+      MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+    #else
+      MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+    #endif
     acc_step_rate += current_block->initial_rate;
 
     // upper limit
     NOMORE(acc_step_rate, current_block->nominal_rate);
 
     // step_rate to timer interval
-    const uint16_t timer = calc_timer(acc_step_rate);
+    const HAL_TIMER_TYPE timer = calc_timer(acc_step_rate);
 
     SPLIT(timer);  // split step into multiple ISRs if larger than  ENDSTOP_NOMINAL_OCR_VAL
     _NEXT_ISR(ocr_val);
@@ -764,12 +721,17 @@ void Stepper::isr() {
     #endif // ADVANCE or LIN_ADVANCE
 
     #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
+      // TODO: HAL
       eISR_Rate = adv_rate(e_steps[TOOL_E_INDEX], timer, step_loops);
     #endif
   }
   else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
-    uint16_t step_rate;
-    MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
+    HAL_TIMER_TYPE step_rate;
+    #ifdef CPU_32_BIT
+      MultiU32X24toH32(step_rate, deceleration_time, current_block->acceleration_rate);
+    #else
+      MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
+    #endif
 
     if (step_rate < acc_step_rate) { // Still decelerating?
       step_rate = acc_step_rate - step_rate;
@@ -779,7 +741,7 @@ void Stepper::isr() {
       step_rate = current_block->final_rate;
 
     // step_rate to timer interval
-    const uint16_t timer = calc_timer(step_rate);
+    const HAL_TIMER_TYPE timer = calc_timer(step_rate);
 
     SPLIT(timer);  // split step into multiple ISRs if larger than  ENDSTOP_NOMINAL_OCR_VAL
     _NEXT_ISR(ocr_val);
@@ -834,13 +796,19 @@ void Stepper::isr() {
 
     SPLIT(OCR1A_nominal);  // split step into multiple ISRs if larger than  ENDSTOP_NOMINAL_OCR_VAL
     _NEXT_ISR(ocr_val);
-
     // ensure we're running at the correct step rate, even if we just came off an acceleration
     step_loops = step_loops_nominal;
   }
 
   #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
-    NOLESS(OCR1A, TCNT1 + 16);
+    #ifdef CPU_32_BIT
+      // Make sure stepper interrupt does not monopolise CPU by adjusting count to give about 8 us room
+      uint32_t stepper_timer_count = HAL_timer_get_count(STEP_TIMER_NUM);
+      uint32_t stepper_timer_current_count = HAL_timer_get_current_count(STEP_TIMER_NUM) + 8 * HAL_TICKS_PER_US;
+      HAL_timer_set_count(STEP_TIMER_NUM, stepper_timer_count < stepper_timer_current_count ? stepper_timer_current_count : stepper_timer_count);
+    #else
+      NOLESS(OCR1A, TCNT1 + 16);
+    #endif
   #endif
 
   // If current block is finished, reset pointer
@@ -849,7 +817,7 @@ void Stepper::isr() {
     planner.discard_current_block();
   }
   #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
-    _ENABLE_ISRs(); // re-enable ISRs
+    HAL_ENABLE_ISRs(); // re-enable ISRs
   #endif
 }
 
@@ -951,7 +919,7 @@ void Stepper::isr() {
 
   void Stepper::advance_isr_scheduler() {
     // Disable Timer0 ISRs and enable global ISR again to capture UART events (incoming chars)
-    CBI(TIMSK0, OCIE0B); // Temperature ISR
+    DISABLE_TEMPERATURE_INTERRUPT(); // Temperature ISR
     DISABLE_STEPPER_DRIVER_INTERRUPT();
     sei();
 
@@ -984,7 +952,7 @@ void Stepper::isr() {
     NOLESS(OCR1A, TCNT1 + 16);
 
     // Restore original ISR settings
-    _ENABLE_ISRs();
+    HAL_ENABLE_ISRs();
   }
 
 #endif // ADVANCE or LIN_ADVANCE
@@ -996,6 +964,15 @@ void Stepper::init() {
     digipot_init();
   #endif
 
+  #if MB(ALLIGATOR)
+    const float motor_current[] = MOTOR_CURRENT;
+    unsigned int digipot_motor = 0;
+    for (uint8_t i = 0; i < 3 + EXTRUDERS; i++) {
+      digipot_motor = 255 * (motor_current[i] / 2.5);
+      dac084s085::setValue(i, digipot_motor);
+    }
+  #endif//MB(ALLIGATOR)
+
   // Init Microstepping Pins
   #if HAS_MICROSTEPS
     microstep_init();
@@ -1152,6 +1129,7 @@ void Stepper::init() {
     E_AXIS_INIT(4);
   #endif
 
+#ifdef ARDUINO_ARCH_AVR
   // waveform generation = 0100 = CTC
   SET_WGM(1, CTC_OCRnA);
 
@@ -1168,6 +1146,11 @@ void Stepper::init() {
   // Init Stepper ISR to 122 Hz for quick starting
   OCR1A = 0x4000;
   TCNT1 = 0;
+#else
+  // Init Stepper ISR to 122 Hz for quick starting
+  HAL_timer_start (STEP_TIMER_NUM, 122);
+#endif
+
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 
   #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
@@ -1663,6 +1646,9 @@ void Stepper::report_positions() {
       case 4: microstep_ms(driver, MICROSTEP4); break;
       case 8: microstep_ms(driver, MICROSTEP8); break;
       case 16: microstep_ms(driver, MICROSTEP16); break;
+      #if MB(ALLIGATOR)
+        case 32: microstep_ms(driver, MICROSTEP32); break;
+      #endif
     }
   }
 
diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index 3ca1926193..b46273b07c 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -52,31 +52,6 @@
 class Stepper;
 extern Stepper stepper;
 
-// intRes = intIn1 * intIn2 >> 16
-// uses:
-// r26 to store 0
-// r27 to store the byte 1 of the 24 bit result
-#define MultiU16X8toH16(intRes, charIn1, intIn2) \
-  asm volatile ( \
-                 "clr r26 \n\t" \
-                 "mul %A1, %B2 \n\t" \
-                 "movw %A0, r0 \n\t" \
-                 "mul %A1, %A2 \n\t" \
-                 "add %A0, r1 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "lsr r0 \n\t" \
-                 "adc %A0, r26 \n\t" \
-                 "adc %B0, r26 \n\t" \
-                 "clr r1 \n\t" \
-                 : \
-                 "=&r" (intRes) \
-                 : \
-                 "d" (charIn1), \
-                 "d" (intIn2) \
-                 : \
-                 "r26" \
-               )
-
 class Stepper {
 
   public:
@@ -112,8 +87,9 @@ class Stepper {
     static volatile uint32_t step_events_completed; // The number of step events executed in the current block
 
     #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
-      static uint16_t nextMainISR, nextAdvanceISR, eISR_Rate;
+      static HAL_TIMER_TYPE nextMainISR, nextAdvanceISR, eISR_Rate;
       #define _NEXT_ISR(T) nextMainISR = T
+
       #if ENABLED(LIN_ADVANCE)
         static volatile int e_steps[E_STEPPERS];
         static int final_estep_rate;
@@ -127,14 +103,14 @@ class Stepper {
         static long old_advance;
       #endif
     #else
-      #define _NEXT_ISR(T) OCR1A = T
+      #define _NEXT_ISR(T) HAL_timer_set_count(STEP_TIMER_NUM, T);
     #endif // ADVANCE or LIN_ADVANCE
 
     static long acceleration_time, deceleration_time;
     //unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
-    static unsigned short acc_step_rate; // needed for deceleration start point
+    static HAL_TIMER_TYPE acc_step_rate; // needed for deceleration start point
     static uint8_t step_loops, step_loops_nominal;
-    static unsigned short OCR1A_nominal;
+    static HAL_TIMER_TYPE OCR1A_nominal;
 
     static volatile long endstops_trigsteps[XYZ];
     static volatile long endstops_stepsTotal, endstops_stepsDone;
@@ -285,43 +261,71 @@ class Stepper {
 
   private:
 
-    static FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
-      unsigned short timer;
+    static FORCE_INLINE HAL_TIMER_TYPE calc_timer(HAL_TIMER_TYPE step_rate) {
+      HAL_TIMER_TYPE timer;
 
       NOMORE(step_rate, MAX_STEP_FREQUENCY);
 
-      if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
-        step_rate >>= 2;
-        step_loops = 4;
-      }
-      else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
-        step_rate >>= 1;
-        step_loops = 2;
-      }
-      else {
-        step_loops = 1;
-      }
+      // TODO: HAL: tidy this up, use condtionals_post.h
+      #ifdef CPU_32_BIT
+        #if ENABLED(DISABLE_MULTI_STEPPING)
+          step_loops = 1;
+        #else
+          if (step_rate > STEP_DOUBLER_FREQUENCY * 2) { // If steprate > (STEP_DOUBLER_FREQUENCY * 2) kHz >> step 4 times
+            step_rate >>= 2;
+            step_loops = 4;
+          }
+          else if (step_rate > STEP_DOUBLER_FREQUENCY) { // If steprate > STEP_DOUBLER_FREQUENCY kHz >> step 2 times
+            step_rate >>= 1;
+            step_loops = 2;
+          }
+          else {
+            step_loops = 1;
+          }
+        #endif
+      #else
+        if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
+          step_rate >>= 2;
+          step_loops = 4;
+        }
+        else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
+          step_rate >>= 1;
+          step_loops = 2;
+        }
+        else {
+          step_loops = 1;
+        }
+      #endif
+
+      #ifdef CPU_32_BIT
+        // In case of high-performance processor, it is able to calculate in real-time 
+        timer = (uint32_t)(HAL_STEPPER_TIMER_RATE) / step_rate;
+        if (timer < (HAL_STEPPER_TIMER_RATE / (STEP_DOUBLER_FREQUENCY * 2))) { // (STEP_DOUBLER_FREQUENCY * 2 kHz - this should never happen)
+          timer = (HAL_STEPPER_TIMER_RATE / (STEP_DOUBLER_FREQUENCY * 2));
+        }
+      #else
+        NOLESS(step_rate, F_CPU / 500000);
+        step_rate -= F_CPU / 500000; // Correct for minimal speed
+        if (step_rate >= (8 * 256)) { // higher step rate
+          unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate >> 8)][0];
+          unsigned char tmp_step_rate = (step_rate & 0x00ff);
+          unsigned short gain = (unsigned short)pgm_read_word_near(table_address + 2);
+          MultiU16X8toH16(timer, tmp_step_rate, gain);
+          timer = (unsigned short)pgm_read_word_near(table_address) - timer;
+        }
+        else { // lower step rates
+          unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
+          table_address += ((step_rate) >> 1) & 0xfffc;
+          timer = (unsigned short)pgm_read_word_near(table_address);
+          timer -= (((unsigned short)pgm_read_word_near(table_address + 2) * (unsigned char)(step_rate & 0x0007)) >> 3);
+        }
+        if (timer < 100) { // (20kHz - this should never happen)
+          timer = 100;
+          MYSERIAL.print(MSG_STEPPER_TOO_HIGH);
+          MYSERIAL.println(step_rate);
+        }
+      #endif
 
-      NOLESS(step_rate, F_CPU / 500000);
-      step_rate -= F_CPU / 500000; // Correct for minimal speed
-      if (step_rate >= (8 * 256)) { // higher step rate
-        unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate >> 8)][0];
-        unsigned char tmp_step_rate = (step_rate & 0x00FF);
-        unsigned short gain = (unsigned short)pgm_read_word_near(table_address + 2);
-        MultiU16X8toH16(timer, tmp_step_rate, gain);
-        timer = (unsigned short)pgm_read_word_near(table_address) - timer;
-      }
-      else { // lower step rates
-        unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
-        table_address += ((step_rate) >> 1) & 0xFFFC;
-        timer = (unsigned short)pgm_read_word_near(table_address);
-        timer -= (((unsigned short)pgm_read_word_near(table_address + 2) * (unsigned char)(step_rate & 0x0007)) >> 3);
-      }
-      if (timer < 100) { // (20kHz - this should never happen)
-        timer = 100;
-        MYSERIAL.print(MSG_STEPPER_TOO_HIGH);
-        MYSERIAL.println(step_rate);
-      }
       return timer;
     }
 
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 261e12cff5..e2d88775bf 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -32,7 +32,7 @@
 #include "language.h"
 
 #if ENABLED(HEATER_0_USES_MAX6675)
-  #include "spi.h"
+  #include "private_spi.h"
 #endif
 
 #if ENABLED(BABYSTEPPING)
@@ -43,10 +43,6 @@
   #include "endstops.h"
 #endif
 
-#if ENABLED(USE_WATCHDOG)
-  #include "watchdog.h"
-#endif
-
 #ifdef K1 // Defined in Configuration.h in the PID settings
   #define K2 (1.0-K1)
 #endif
@@ -1054,38 +1050,39 @@ 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 - 8); }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_0
-    ANALOG_SELECT(TEMP_0_PIN);
+    HAL_ANALOG_SELECT(TEMP_0_PIN);
   #endif
   #if HAS_TEMP_1
-    ANALOG_SELECT(TEMP_1_PIN);
+    HAL_ANALOG_SELECT(TEMP_1_PIN);
   #endif
   #if HAS_TEMP_2
-    ANALOG_SELECT(TEMP_2_PIN);
+    HAL_ANALOG_SELECT(TEMP_2_PIN);
   #endif
   #if HAS_TEMP_3
-    ANALOG_SELECT(TEMP_3_PIN);
+    HAL_ANALOG_SELECT(TEMP_3_PIN);
   #endif
   #if HAS_TEMP_4
-    ANALOG_SELECT(TEMP_4_PIN);
+    HAL_ANALOG_SELECT(TEMP_4_PIN);
   #endif
   #if HAS_TEMP_BED
-    ANALOG_SELECT(TEMP_BED_PIN);
+    HAL_ANALOG_SELECT(TEMP_BED_PIN);
   #endif
   #if ENABLED(FILAMENT_WIDTH_SENSOR)
-    ANALOG_SELECT(FILWIDTH_PIN);
+    HAL_ANALOG_SELECT(FILWIDTH_PIN);
+  #endif
+
+// todo: HAL: fix abstraction
+  #ifdef ARDUINO_ARCH_AVR
+    // Use timer0 for temperature measurement
+    // Interleave temperature interrupt with millies interrupt
+    OCR0B = 128;
+    SBI(TIMSK0, OCIE0B);
+  #else
+    HAL_timer_start (TEMP_TIMER_NUM, TEMP_TIMER_FREQUENCY);
+    HAL_timer_enable_interrupt (TEMP_TIMER_NUM);
   #endif
 
   #if HAS_AUTO_FAN_0
@@ -1139,11 +1136,6 @@ void Temperature::init() {
     #endif
   #endif
 
-  // Use timer0 for temperature measurement
-  // Interleave temperature interrupt with millies interrupt
-  OCR0B = 128;
-  SBI(TIMSK0, OCIE0B);
-
   // Wait for temperature measurement to settle
   delay(250);
 
@@ -1415,12 +1407,12 @@ void Temperature::disable_all_heaters() {
     uint32_t max6675_temp = 2000;
     #define MAX6675_ERROR_MASK 7
     #define MAX6675_DISCARD_BITS 18
-    #define MAX6675_SPEED_BITS (_BV(SPR1)) // clock ÷ 64
+    #define MAX6675_SPEED_BITS 3  // (_BV(SPR1)) // clock ÷ 64
   #else
     uint16_t max6675_temp = 2000;
     #define MAX6675_ERROR_MASK 4
     #define MAX6675_DISCARD_BITS 3
-    #define MAX6675_SPEED_BITS (_BV(SPR0)) // clock ÷ 16
+    #define MAX6675_SPEED_BITS 2 // (_BV(SPR0)) // clock ÷ 16
   #endif
 
   int Temperature::read_max6675() {
@@ -1433,14 +1425,8 @@ void Temperature::disable_all_heaters() {
 
     next_max6675_ms = ms + MAX6675_HEAT_INTERVAL;
 
-    CBI(
-      #ifdef PRR
-        PRR
-      #elif defined(PRR0)
-        PRR0
-      #endif
-        , PRSPI);
-    SPCR = _BV(MSTR) | _BV(SPE) | MAX6675_SPEED_BITS;
+    spiBegin();
+    spiInit(MAX6675_SPEED_BITS);
 
     WRITE(MAX6675_SS, 0); // enable TT_MAX6675
 
@@ -1451,7 +1437,7 @@ void Temperature::disable_all_heaters() {
     // Read a big-endian temperature value
     max6675_temp = 0;
     for (uint8_t i = sizeof(max6675_temp); i--;) {
-      max6675_temp |= max6675_spi.receive();
+      max6675_temp |= spiRec();
       if (i > 0) max6675_temp <<= 8; // shift left if not the last byte
     }
 
@@ -1607,7 +1593,10 @@ 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) { Temperature::isr(); }
+HAL_TEMP_TIMER_ISR {
+  HAL_timer_isr_prologue (TEMP_TIMER_NUM);
+  Temperature::isr();
+}
 
 volatile bool Temperature::in_temp_isr = false;
 
@@ -1618,8 +1607,10 @@ void Temperature::isr() {
   in_temp_isr = true;
 
   // Allow UART and stepper ISRs
-  CBI(TIMSK0, OCIE0B); //Disable Temperature ISR
-  sei();
+  DISABLE_TEMPERATURE_INTERRUPT(); //Disable Temperature ISR
+  #if !defined(CPU_32_BIT)
+    sei();
+  #endif
 
   static int8_t temp_count = -1;
   static ADCSensorState adc_sensor_state = StartupDelay;
@@ -1914,13 +1905,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: {
@@ -1940,66 +1924,66 @@ void Temperature::isr() {
 
     #if HAS_TEMP_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_TEMP_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_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_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_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_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
@@ -2130,7 +2114,9 @@ void Temperature::isr() {
     }
   #endif
 
-  cli();
+  #if !defined(CPU_32_BIT)
+    cli();
+  #endif
   in_temp_isr = false;
-  SBI(TIMSK0, OCIE0B); //re-enable Temperature ISR
+  ENABLE_TEMPERATURE_INTERRUPT(); //re-enable Temperature ISR
 }
diff --git a/Marlin/temperature.h b/Marlin/temperature.h
index 6ce644d88e..89a917992f 100644
--- a/Marlin/temperature.h
+++ b/Marlin/temperature.h
@@ -125,7 +125,7 @@ class Temperature {
     #endif
 
     #if ENABLED(PIDTEMP) || ENABLED(PIDTEMPBED)
-      #define PID_dT ((OVERSAMPLENR * float(ACTUAL_ADC_SAMPLES)) / (F_CPU / 64.0 / 256.0))
+      #define PID_dT ((OVERSAMPLENR * float(ACTUAL_ADC_SAMPLES)) / TEMP_TIMER_FREQUENCY)
     #endif
 
     #if ENABLED(PIDTEMP)
diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp
index 4c10c41cfe..95fa2494bb 100644
--- a/Marlin/ubl_G29.cpp
+++ b/Marlin/ubl_G29.cpp
@@ -1371,7 +1371,7 @@
         z_values[x][y] -= tmp_z_values[x][y];
   }
 
-  mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], const bool far_flag) {
+  mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, uint16_t bits[16], const bool far_flag) {
     mesh_index_pair out_mesh;
     out_mesh.x_index = out_mesh.y_index = -1;
 
diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp
index be5fe3c200..13684b7012 100644
--- a/Marlin/ubl_motion.cpp
+++ b/Marlin/ubl_motion.cpp
@@ -27,7 +27,6 @@
   #include "ubl.h"
   #include "planner.h"
   #include "stepper.h"
-  #include <avr/io.h>
   #include <math.h>
 
   extern float destination[XYZE];
@@ -737,4 +736,3 @@
   #endif // UBL_DELTA
 
 #endif // AUTO_BED_LEVELING_UBL
-
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 353db25c5b..57c3e3227d 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -184,6 +184,7 @@ uint16_t max_display_update_time = 0;
     void menu_action_setting_edit_callback_ ## _name(const char * const pstr, _type * const ptr, const _type minValue, const _type maxValue, const screenFunc_t callback, const bool live=false); \
     typedef void _name##_void
 
+  DECLARE_MENU_EDIT_TYPE(uint32_t, long5);
   DECLARE_MENU_EDIT_TYPE(int16_t, int3);
   DECLARE_MENU_EDIT_TYPE(uint8_t, int8);
   DECLARE_MENU_EDIT_TYPE(float, float3);
@@ -193,7 +194,6 @@ uint16_t max_display_update_time = 0;
   DECLARE_MENU_EDIT_TYPE(float, float51);
   DECLARE_MENU_EDIT_TYPE(float, float52);
   DECLARE_MENU_EDIT_TYPE(float, float62);
-  DECLARE_MENU_EDIT_TYPE(uint32_t, long5);
 
   void menu_action_setting_edit_bool(const char* pstr, bool* ptr);
   void menu_action_setting_edit_callback_bool(const char* pstr, bool* ptr, screenFunc_t callbackFunc);
@@ -4039,6 +4039,7 @@ void kill_screen(const char* lcd_msg) {
     } \
     typedef void _name
 
+  DEFINE_MENU_EDIT_TYPE(uint32_t, long5, ftostr5rj, 0.01);
   DEFINE_MENU_EDIT_TYPE(int16_t, int3, itostr3, 1);
   DEFINE_MENU_EDIT_TYPE(uint8_t, int8, i8tostr3, 1);
   DEFINE_MENU_EDIT_TYPE(float, float3, ftostr3, 1.0);
@@ -4048,7 +4049,6 @@ void kill_screen(const char* lcd_msg) {
   DEFINE_MENU_EDIT_TYPE(float, float51, ftostr51sign, 10.0);
   DEFINE_MENU_EDIT_TYPE(float, float52, ftostr52sign, 100.0);
   DEFINE_MENU_EDIT_TYPE(float, float62, ftostr62rj, 100.0);
-  DEFINE_MENU_EDIT_TYPE(uint32_t, long5, ftostr5rj, 0.01);
 
   /**
    *
diff --git a/Marlin/ultralcd_impl_DOGM.h b/Marlin/ultralcd_impl_DOGM.h
index 3cbced445f..df68ded62d 100644
--- a/Marlin/ultralcd_impl_DOGM.h
+++ b/Marlin/ultralcd_impl_DOGM.h
@@ -869,6 +869,7 @@ static void lcd_implementation_status_screen() {
     } \
     typedef void _name##_void
 
+  DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(uint32_t, long5, ftostr5rj);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(int16_t, int3, itostr3);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(uint8_t, int8, i8tostr3);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float3, ftostr3);
@@ -878,7 +879,6 @@ static void lcd_implementation_status_screen() {
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float51, ftostr51sign);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float52, ftostr52sign);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float62, ftostr62rj);
-  DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(uint32_t, long5, ftostr5rj);
 
   #define lcd_implementation_drawmenu_setting_edit_bool(sel, row, pstr, pstr2, data) lcd_implementation_drawmenu_setting_edit_generic_P(sel, row, pstr, (*(data))?PSTR(MSG_ON):PSTR(MSG_OFF))
   #define lcd_implementation_drawmenu_setting_edit_callback_bool(sel, row, pstr, pstr2, data, callback) lcd_implementation_drawmenu_setting_edit_generic_P(sel, row, pstr, (*(data))?PSTR(MSG_ON):PSTR(MSG_OFF))
diff --git a/Marlin/ultralcd_impl_HD44780.h b/Marlin/ultralcd_impl_HD44780.h
index e8ecd9cf4f..8bcfee94c7 100644
--- a/Marlin/ultralcd_impl_HD44780.h
+++ b/Marlin/ultralcd_impl_HD44780.h
@@ -976,6 +976,7 @@ static void lcd_implementation_status_screen() {
     } \
     typedef void _name##_void
 
+  DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(uint32_t, long5, ftostr5rj);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(int16_t, int3, itostr3);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(uint8_t, int8, i8tostr3);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float3, ftostr3);
@@ -985,7 +986,6 @@ static void lcd_implementation_status_screen() {
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float51, ftostr51sign);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float52, ftostr52sign);
   DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(float, float62, ftostr62rj);
-  DEFINE_LCD_IMPLEMENTATION_DRAWMENU_SETTING_EDIT_TYPE(uint32_t, long5, ftostr5rj);
 
   #define lcd_implementation_drawmenu_setting_edit_bool(sel, row, pstr, pstr2, data) lcd_implementation_drawmenu_setting_edit_generic_P(sel, row, pstr, '>', (*(data))?PSTR(MSG_ON):PSTR(MSG_OFF))
   #define lcd_implementation_drawmenu_setting_edit_callback_bool(sel, row, pstr, pstr2, data, callback) lcd_implementation_drawmenu_setting_edit_generic_P(sel, row, pstr, '>', (*(data))?PSTR(MSG_ON):PSTR(MSG_OFF))
-- 
GitLab