diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 8271056ec..4b0eb703b 100755
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -676,14 +676,15 @@ static inline signed char pgm_read_any(const signed char *p) { return pgm_read_b
 
 #define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \
   static const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }; \
-  static inline type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); }
+  static inline type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
+  typedef void __void_##CONFIG##__
 
-XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,   MIN_POS)
-XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,   MAX_POS)
-XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,  HOME_POS)
-XYZ_CONSTS_FROM_CONFIG(float, max_length,     MAX_LENGTH)
-XYZ_CONSTS_FROM_CONFIG(float, home_bump_mm,   HOME_BUMP_MM)
-XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR)
+XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,   MIN_POS);
+XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,   MAX_POS);
+XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,  HOME_POS);
+XYZ_CONSTS_FROM_CONFIG(float, max_length,     MAX_LENGTH);
+XYZ_CONSTS_FROM_CONFIG(float, home_bump_mm,   HOME_BUMP_MM);
+XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
 
 /**
  * ***************************************************************************
@@ -714,13 +715,13 @@ static void report_current_position();
 #if ENABLED(DEBUG_LEVELING_FEATURE)
   void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {
     serialprintPGM(prefix);
-    SERIAL_ECHOPAIR("(", x);
+    SERIAL_CHAR('(');
+    SERIAL_ECHO(x);
     SERIAL_ECHOPAIR(", ", y);
     SERIAL_ECHOPAIR(", ", z);
     SERIAL_CHAR(')');
 
-    if (suffix) serialprintPGM(suffix);
-    else SERIAL_EOL;
+    suffix ? serialprintPGM(suffix) : SERIAL_EOL;
   }
 
   void print_xyz(const char* prefix, const char* suffix, const float xyz[]) {
@@ -11567,47 +11568,46 @@ void prepare_move_to_destination() {
       #ifdef TCCR0A
         case TIMER0A:
         case TIMER0B:
-          //SET_CS(0, val);
+          //_SET_CS(0, val);
           break;
       #endif
       #ifdef TCCR1A
         case TIMER1A:
         case TIMER1B:
-          //SET_CS(1, val);
+          //_SET_CS(1, val);
           break;
       #endif
       #ifdef TCCR2
         case TIMER2:
         case TIMER2:
-          TCCR2 &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
-          TCCR2 |= val;
+          _SET_CS(2, val);
           break;
       #endif
       #ifdef TCCR2A
         case TIMER2A:
         case TIMER2B:
-          SET_CS(2, val);
+          _SET_CS(2, val);
           break;
       #endif
       #ifdef TCCR3A
         case TIMER3A:
         case TIMER3B:
         case TIMER3C:
-          SET_CS(3, val);
+          _SET_CS(3, val);
           break;
       #endif
       #ifdef TCCR4A
         case TIMER4A:
         case TIMER4B:
         case TIMER4C:
-          SET_CS(4, val);
+          _SET_CS(4, val);
           break;
       #endif
       #ifdef TCCR5A
         case TIMER5A:
         case TIMER5B:
         case TIMER5C:
-          SET_CS(5, val);
+          _SET_CS(5, val);
           break;
       #endif
     }
diff --git a/Marlin/fastio.h b/Marlin/fastio.h
index b172d8f6b..dd7d0012e 100644
--- a/Marlin/fastio.h
+++ b/Marlin/fastio.h
@@ -85,7 +85,7 @@
 #define OUT_WRITE(IO, v) do{ SET_OUTPUT(IO); WRITE(IO, v); }while(0)
 
 /**
- * Interrupt Control
+ * Timer and Interrupt Control
  */
 
 // Waveform Generation Modes
@@ -128,24 +128,86 @@ typedef enum {
   CS_EXT_RISING        //  7
 } ClockSource;
 
-#define SET_WGM(T,V) do{ \
+// Clock Sources (Timer 2 only)
+typedef enum {
+  CS2_NONE,            //  0
+  CS2_PRESCALER_1,     //  1
+  CS2_PRESCALER_8,     //  2
+  CS2_PRESCALER_32,    //  3
+  CS2_PRESCALER_64,    //  4
+  CS2_PRESCALER_128,   //  5
+  CS2_PRESCALER_256,   //  6
+  CS2_PRESCALER_1024   //  7
+} ClockSource2;
+
+// Get interrupt bits in an orderly way
+#define GET_WGM(T)   (((TCCR##T##A >> WGM##T##0) & 0x3) | ((TCCR##T##B >> WGM##T##2 << 2) & 0xC))
+#define GET_CS(T)    ((TCCR##T##B >> CS##T##0) & 0x7)
+#define GET_COM(T,Q) ((TCCR##T##Q >> COM##T##Q##0) & 0x3)
+#define GET_COMA(T)  GET_COM(T,A)
+#define GET_COMB(T)  GET_COM(T,B)
+#define GET_COMC(T)  GET_COM(T,C)
+#define GET_ICNC(T)  (!!(TCCR##T##B & _BV(ICNC##T)))
+#define GET_ICES(T)  (!!(TCCR##T##B & _BV(ICES##T)))
+#define GET_FOC(T,Q) (!!(TCCR##T##C & _BV(FOC##T##Q)))
+#define GET_FOCA(T)  GET_FOC(T,A)
+#define GET_FOCB(T)  GET_FOC(T,B)
+#define GET_FOCC(T)  GET_FOC(T,C)
+
+// Set Wave Generation Mode bits
+#define _SET_WGM(T,V) do{ \
     TCCR##T##A = (TCCR##T##A & ~(0x3 << WGM##T##0)) | (( int(V)       & 0x3) << WGM##T##0); \
     TCCR##T##B = (TCCR##T##B & ~(0x3 << WGM##T##2)) | (((int(V) >> 2) & 0x3) << WGM##T##2); \
   }while(0)
-
-#define SET_CS(T,V) do{ \
-    TCCR##T##B = (TCCR##T##B & ~(0x7 << CS10)) | ((int(V) & 0x7) << CS10); \
-  }while(0)
-
-#define SET_COM(T,Q,V) do{ \
-    TCCR##T##Q = (TCCR##T##Q & ~(0x3 << COM1##Q##0)) | ((int(V) & 0x3) << COM1##Q##0); \
-  }while(0)
+#define SET_WGM(T,V) _SET_WGM(T,WGM_##V)
+
+// Set Clock Select bits
+#define _SET_CS(T,V) (TCCR##T##B = (TCCR##T##B & ~(0x7 << CS##T##0)) | ((int(V) & 0x7) << CS##T##0))
+#define _SET_CS0(V) _SET_CS(0,V)
+#define _SET_CS1(V) _SET_CS(1,V)
+#ifdef TCCR2
+  #define _SET_CS2(V) (TCCR2 = (TCCR2 & ~(0x7 << CS20)) | (int(V) << CS20))
+#else
+  #define _SET_CS2(V) _SET_CS(2,V)
+#endif
+#define _SET_CS3(V) _SET_CS(3,V)
+#define _SET_CS4(V) _SET_CS(4,V)
+#define _SET_CS5(V) _SET_CS(5,V)
+#define SET_CS0(V) _SET_CS0(CS_##V)
+#define SET_CS1(V) _SET_CS1(CS_##V)
+#ifdef TCCR2
+  #define SET_CS2(V) _SET_CS2(CS2_##V)
+#else
+  #define SET_CS2(V) _SET_CS2(CS_##V)
+#endif
+#define SET_CS3(V) _SET_CS3(CS_##V)
+#define SET_CS4(V) _SET_CS4(CS_##V)
+#define SET_CS5(V) _SET_CS5(CS_##V)
+#define SET_CS(T,V) SET_CS##T(V)
+
+// Set Compare Mode bits
+#define _SET_COM(T,Q,V) (TCCR##T##Q = (TCCR##T##Q & ~(0x3 << COM##T##Q##0)) | (int(V) << COM##T##Q##0))
+#define _SET_COMA(T,V) _SET_COM(T,A,V)
+#define _SET_COMB(T,V) _SET_COM(T,B,V)
+#define _SET_COMC(T,V) _SET_COM(T,C,V)
+#define _SET_COMS(T,V1,V2,V3) do{ _SET_COMA(T,V1); _SET_COMB(T,V2); _SET_COMC(T,V3); }while(0)
+#define SET_COM(T,Q,V) _SET_COM(T,Q,COM_##V)
 #define SET_COMA(T,V) SET_COM(T,A,V)
 #define SET_COMB(T,V) SET_COM(T,B,V)
-#define SET_COMS(T,V1,V2) do{ SET_COMA(T,V1); SET_COMB(T,V2); }while(0)
+#define SET_COMC(T,V) SET_COM(T,C,V)
+#define SET_COMS(T,V1,V2,V3) do{ SET_COMA(T,V1); SET_COMB(T,V2); SET_COMC(T,V3); }while(0)
+
+// Set Noise Canceler bit
+#define SET_ICNC(T,V) (TCCR##T##B = (V) ? TCCR##T##B | _BV(ICNC##T) : TCCR##T##B & ~_BV(ICNC##T))
+
+// Set Input Capture Edge Select bit
+#define SET_ICES(T,V) (TCCR##T##B = (V) ? TCCR##T##B | _BV(ICES##T) : TCCR##T##B & ~_BV(ICES##T))
 
-#define SET_ICNC(T,V) (TCCR##T##B = (TCCR##T##B & ~_BV(7) | ((V) & 1) << 7))
-#define SET_ICES(T,V) (TCCR##T##B = (TCCR##T##B & ~_BV(6) | ((V) & 1) << 6))
+// Set Force Output Compare bit
+#define SET_FOC(T,Q,V) (TCCR##T##C = (V) ? TCCR##T##C | _BV(FOC##T##Q) : TCCR##T##C & ~_BV(FOC##T##Q))
+#define SET_FOCA(T,V) SET_FOC(T,A,V)
+#define SET_FOCB(T,V) SET_FOC(T,B,V)
+#define SET_FOCC(T,V) SET_FOC(T,C,V)
 
 /**
  * Ports and Functions
@@ -177,8 +239,8 @@ typedef enum {
   #define DEBUG_LED   AIO5
 
   /**
-  pins
-  */
+   * Pins Info
+   */
 
   #define DIO0_PIN    PIND0
   #define DIO0_RPORT  PIND
@@ -513,9 +575,10 @@ typedef enum {
   #define OC2B        DIO14
 
   #define DEBUG_LED   DIO0
+
   /**
-  pins
-  */
+   * Pins Info
+   */
 
   #define DIO0_PIN    PINB0
   #define DIO0_RPORT  PINB
@@ -757,8 +820,6 @@ typedef enum {
   #define AIO7_DDR    DDRA
   #define AIO7_PWM    NULL
 
-
-
   #undef PA0
   #define PA0_PIN     PINA0
   #define PA0_RPORT   PINA
@@ -1023,8 +1084,9 @@ typedef enum {
   #define DEBUG_LED   DIO21
 
   /**
-  pins
-  */
+   * Pins Info
+   */
+
   #define DIO0_PIN    PINE0
   #define DIO0_RPORT  PINE
   #define DIO0_WPORT  PORTE
@@ -2075,7 +2137,7 @@ typedef enum {
   #define DEBUG_LED   DIO31 /* led D5 red */
 
   /**
-   * pins
+   * Pins Info
    */
 
   //#define AT90USBxx_TEENSYPP_ASSIGNMENTS // Use Teensy++ 2.0 assignments
@@ -3402,8 +3464,9 @@ typedef enum {
   #define DEBUG_LED   DIO46
 
   /**
-  pins
-  */
+   * Pins Info
+   */
+
   #define DIO0_PIN    PINE0
   #define DIO0_RPORT  PINE
   #define DIO0_WPORT  PORTE
@@ -3728,9 +3791,6 @@ typedef enum {
   #define DIO53_DDR   DDRF
   #define DIO53_PWM   NULL
 
-
-
-
   #undef PA0
   #define PA0_PIN     PINA0
   #define PA0_RPORT   PINA
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index ed36a3124..5cbd8bddb 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -1081,17 +1081,17 @@ void Stepper::init() {
   #endif
 
   // waveform generation = 0100 = CTC
-  SET_WGM(1, WGM_CTC_OCRnA);
+  SET_WGM(1, CTC_OCRnA);
 
   // output mode = 00 (disconnected)
-  SET_COMS(1, COM_NORMAL, COM_NORMAL);
+  SET_COMS(1, NORMAL, NORMAL, NORMAL);
 
   // Set the timer pre-scaler
   // Generally we use a divider of 8, resulting in a 2MHz timer
   // frequency on a 16MHz MCU. If you are going to change this, be
   // sure to regenerate speed_lookuptable.h with
   // create_speed_lookuptable.py
-  SET_CS(1, CS_PRESCALER_8);  //  CS 2 = 1/8 prescaler
+  SET_CS(1, PRESCALER_8);  //  CS 2 = 1/8 prescaler
 
   // Init Stepper ISR to 122 Hz for quick starting
   OCR1A = 0x4000;