diff --git a/.travis.yml b/.travis.yml
index 16bf75043..12b4af996 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -155,7 +155,7 @@ script:
   #   INCH_MODE_SUPPORT, TEMPERATURE_UNITS_SUPPORT
   #
   - restore_configs
-  - opt_enable EEPROM_SETTINGS EEPROM_CHITCHAT M100_FREE_MEMORY_WATCHER INCH_MODE_SUPPORT TEMPERATURE_UNITS_SUPPORT
+  - opt_enable EEPROM_SETTINGS EEPROM_CHITCHAT M100_FREE_MEMORY_WATCHER M100_FREE_MEMORY_DUMPER M100_FREE_MEMORY_CORRUPTOR INCH_MODE_SUPPORT TEMPERATURE_UNITS_SUPPORT
   - build_marlin
   #
   # Mixing Extruder
diff --git a/Marlin/M100_Free_Mem_Chk.cpp b/Marlin/M100_Free_Mem_Chk.cpp
index 01709c5d2..de61d5bb2 100644
--- a/Marlin/M100_Free_Mem_Chk.cpp
+++ b/Marlin/M100_Free_Mem_Chk.cpp
@@ -26,227 +26,238 @@
  * This code watches the free memory block between the bottom of the heap and the top of the stack.
  * This memory block is initialized and watched via the M100 command.
  *
- * M100 I Initializes the free memory block and prints vitals statistics about the area
- * M100 F Identifies how much of the free memory block remains free and unused.  It also
- *    detects and reports any corruption within the free memory block that may have
- *    happened due to errant firmware.
- * M100 D Does a hex display of the free memory block along with a flag for any errant
- *    data that does not match the expected value.
- * M100 C x Corrupts x locations within the free memory block.   This is useful to check the
- *    correctness of the M100 F and M100 D commands.
+ * M100 I   Initializes the free memory block and prints vitals statistics about the area
+ *
+ * M100 F   Identifies how much of the free memory block remains free and unused. It also
+ *          detects and reports any corruption within the free memory block that may have
+ *          happened due to errant firmware.
+ *
+ * M100 D   Does a hex display of the free memory block along with a flag for any errant
+ *          data that does not match the expected value.
+ *
+ * M100 C x Corrupts x locations within the free memory block. This is useful to check the
+ *          correctness of the M100 F and M100 D commands.
  *
  * Initial version by Roxy-3D
  */
-#define M100_FREE_MEMORY_DUMPER     // Comment out to remove Dump sub-command
-#define M100_FREE_MEMORY_CORRUPTOR    // Comment out to remove Corrupt sub-command
+#define M100_FREE_MEMORY_DUMPER     // Enable for the `M110 D` Dump sub-command
+#define M100_FREE_MEMORY_CORRUPTOR  // Enable for the `M100 C` Corrupt sub-command
 
-#include "Marlin.h"
+#include "MarlinConfig.h"
 
 #if ENABLED(M100_FREE_MEMORY_WATCHER)
+
+#define TEST_BYTE 0xE5
+
 extern char* __brkval;
 extern size_t  __heap_start, __heap_end, __flp;
 extern char __bss_end;
 
-//
-// Utility functions used by M100 to get its work done.
-//
-
+#include "Marlin.h"
 #include "hex_print_routines.h"
 
-char* top_of_stack();
-int how_many_E5s_are_here(char*);
-int free_memory_is_corrupted();         // int not bool!!!!  it will tell us how many blocks of
-                                        // free memory it found.
-void gcode_M100() {
-  static bool m100_not_initialized = true;
-  char* sp, *ptr;
-  int i, j, n;
-  //
-  // M100 D dumps the free memory block from __brkval to the stack pointer.
-  // malloc() eats memory from the start of the block and the stack grows
-  // up from the bottom of the block.    Solid 0xE5's indicate nothing has
-  // used that memory yet.   There should not be anything but 0xE5's within
-  // the block of 0xE5's.  If there is, that would indicate memory corruption
-  // probably caused by bad pointers.  Any unexpected values will be flagged in
-  // the right hand column to help spotting them.
-  //
-    SERIAL_ECHOPAIR("\n__brkval : 0x", hex_word((uint16_t)__brkval) );
-    SERIAL_ECHOPAIR("\n__bss_end : 0x", hex_word((uint16_t)&__bss_end));
-  //
-  // With out malloc() we need to be smart and use &__bss_end
-  //
-    ptr = __brkval ? __brkval : &__bss_end;    
-    SERIAL_ECHOPAIR("\nstart of free space : 0x", hex_word((uint16_t)ptr));
+//
+// Utility functions
+//
 
-    sp = top_of_stack();
-    SERIAL_ECHOLNPAIR("\nStack Pointer : 0x", hex_word((uint16_t)sp));
-
-  #if ENABLED(M100_FREE_MEMORY_DUMPER) // Disable to remove Dump sub-command
-    if (code_seen('D')) {
-      //
-      // We want to start and end the dump on a nice 16 byte boundry even though
-      // the values we are using are not 16 byte aligned.
-      //
-      ptr = (char*) ((uint16_t) ptr & 0xfff0);
-      sp  = (char*) ((uint16_t) sp  | 0x000f);
-
-      n = sp - ptr;
-      //
-      // This is the main loop of the Dump command.
-      //
-      while (ptr < sp) {
-        print_hex_word((uint16_t)ptr); // Print the address
-        SERIAL_CHAR(':');
-        for (i = 0; i < 16; i++) {      // and 16 data bytes
-          if (i==8)
-            SERIAL_CHAR('-');
-          print_hex_byte(*(ptr + i));
-          SERIAL_CHAR(' ');
-        }
-        SERIAL_CHAR('|');         // now show where non 0xE5's are
-        for (i = 0; i < 16; i++)
-          SERIAL_CHAR((*(ptr + i) == (char)0xe5) ? ' ' : '?');
-        SERIAL_EOL;
-        ptr += 16;
-        idle();
-      }
-      return;
-    }
-  #endif
-  //
-  // M100 F   requests the code to return the number of free bytes in the memory pool along with
-  // other vital statistics that define the memory pool.
-  //
-  if (code_seen('F')) {
-    int max_cnt = -1, block_cnt = 0;
-    uint16_t max_addr=0;
-    ptr =  __brkval ? __brkval : &__bss_end;
-    sp = top_of_stack();
-    n = sp - ptr;
-    // Scan through the range looking for the biggest block of 0xE5's we can find
-    for (i = 0; i < n; i++) {
-      if (*(ptr + i) == (char)0xe5) {
-        j = how_many_E5s_are_here(ptr + i);
-        if (j > 8) {
-          SERIAL_ECHOPAIR("Found ", j);
-          SERIAL_ECHOLNPAIR(" bytes free at 0x", hex_word((uint16_t)(ptr + i)));
-          if (j > max_cnt) {  
-            max_cnt  = j;    
-            max_addr = (uint16_t) ptr + i;
-          }
-          i += j;
-          block_cnt++;
-        }
-      }
-    }
-    if (block_cnt > 1) {
-      SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
-      SERIAL_ECHOPAIR("\nLargest free block is ", max_cnt);
-      SERIAL_ECHOLNPAIR(" bytes big at 0x", hex_word(max_addr));
-    }
-    SERIAL_ECHOLNPAIR("free_memory_is_corrupted() = ", free_memory_is_corrupted()); 
-    return;
-  }
-  //
-  // M100 C x  Corrupts x locations in the free memory pool and reports the locations of the corruption.
-  // This is useful to check the correctness of the M100 D and the M100 F commands.
-  //
-  #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
-    if (code_seen('C')) {
-      int x = code_value_int(); // x gets the # of locations to corrupt within the memory pool
-      SERIAL_ECHOLNPGM("Corrupting free memory block.\n");
-      ptr += 8;
-      sp = top_of_stack();
-      n = sp - ptr - 250;    // -250 just to keep us from finding interrupt activity that
-                            // has altered the stack.
-      j = n / (x + 1);
-      for (i = 1; i <= x; i++) {
-        *(ptr + (i * j)) = i;
-        SERIAL_ECHOPAIR("\nCorrupting address: 0x", hex_word((uint16_t)(ptr + i * j)));
-      }
-      SERIAL_ECHOLNPGM("\n");
-      return;
-    }
-  #endif
-  //
-  // M100 I    Initializes the free memory pool so it can be watched and prints vital
-  // statistics that define the free memory pool.
-  //
-  if (m100_not_initialized || code_seen('I')) {            // If no sub-command is specified, the first time
-    SERIAL_ECHOLNPGM("Initializing free memory block.\n"); // this happens, it will Initialize.
-                                                           // Repeated M100 with no sub-command will not destroy the
-                                                           // state of the initialized free memory pool.
-    ptr += 8;
-    SERIAL_ECHOLNPGM("\n");
-    n = sp - ptr - 250;    // -250 just to keep us from finding interrupt activity that
-    // has altered the stack.
-    SERIAL_ECHO(n);
-    SERIAL_ECHOLNPGM(" bytes of memory initialized.\n");
-    for (i = 0; i < n; i++)
-      *(ptr + i) = (char)0xe5;
-    for (i = 0; i < n; i++) {
-      if (*(ptr + i) != (char)0xe5) {
-        SERIAL_ECHOPAIR("? address : ", hex_word(ptr+i) );
-        SERIAL_ECHOPAIR("=", hex_byte(*(ptr + i)) );
-        SERIAL_ECHOLNPGM("\n");
-      }
-    }
-    m100_not_initialized = false;
-    return;
-  }
-  return;
-}
-
-// top_of_stack() returns the location of a variable on its stack frame.  The value returned is above
-// the stack once the function returns to the caller.
+#define END_OF_HEAP() (__brkval ? __brkval : &__bss_end)
 
+// Location of a variable on its stack frame. Returns a value above
+// the stack (once the function returns to the caller).
 char* top_of_stack() {
   char x;
   return &x + 1; // x is pulled on return;
 }
 
-// how_many_E5s_are_here() is a utility function to easily find out how many 0xE5's are
-// at the specified location.  Having this logic as a function simplifies the search code.
-//
-int how_many_E5s_are_here(char* p) {
-  int n;
-  for (n = 0; n < 32000; n++) {
-    if (*(p + n) != (char)0xe5)
-      return n - 1;
-  }
+// Count the number of test bytes at the specified location.
+int16_t count_test_bytes(const char * const ptr) {
+  for (uint16_t i = 0; i < 32000; i++)
+    if (ptr[i] != TEST_BYTE)
+      return i - 1;
+
   return -1;
 }
 
-
-int free_memory_is_corrupted() {
-  char *sp, *ptr;
-  int block_cnt = 0, i, j, n;
-
-    ptr = __brkval ? __brkval : &__bss_end;    
-    sp = top_of_stack();
-
-    n = sp - ptr;
-
-    // Scan through the range looking for the biggest block of 0xE5's we can find
-    for (i = 0; i < n; i++) {
-      if (*(ptr + i) == (char)0xe5) {
-        j = how_many_E5s_are_here(ptr + i);
-        if (j > 8) {
-//        SERIAL_ECHOPAIR("Found ", j);
-//        SERIAL_ECHOLNPAIR(" bytes free at 0x", hex_word((uint16_t)(ptr + i)));
-
-          i += j;
-          block_cnt++;
-        }
+// Return a count of free memory blocks.
+uint16_t free_memory_is_corrupted(char * const ptr, const uint16_t size) {
+  // Find the longest block of test bytes in the given buffer
+  uint16_t block_cnt = 0;
+  for (uint16_t i = 0; i < size; i++) {
+    if (ptr[i] == TEST_BYTE) {
+      const uint16_t j = count_test_bytes(ptr + i);
+      if (j > 8) {
+        //SERIAL_ECHOPAIR("Found ", j);
+        //SERIAL_ECHOLNPAIR(" bytes free at 0x", hex_word((uint16_t)ptr + i));
+        i += j;
+        block_cnt++;
       }
     }
+  }
+  //if (block_cnt > 1) {
+  //  SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+  //  SERIAL_ECHOLNPAIR("\nLargest free block is ", max_cnt);
+  //}
+  return block_cnt;
+}
 
-//  if (block_cnt > 1) {
-//    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
-//   SERIAL_ECHOLNPAIR("\nLargest free block is ", max_cnt);
-//  }
-    return block_cnt;
+//
+// M100 sub-commands
+//
+
+#if ENABLED(M100_FREE_MEMORY_DUMPER)
+  /**
+   * M100 D
+   *  Dump the free memory block from __brkval to the stack pointer.
+   *  malloc() eats memory from the start of the block and the stack grows
+   *  up from the bottom of the block. Solid test bytes indicate nothing has
+   *  used that memory yet. There should not be anything but test bytes within
+   *  the block. If so, it may indicate memory corruption due to a bad pointer.
+   *  Unexpected bytes are flagged in the right column.
+   */
+  void dump_free_memory(char *ptr, char *sp) {
+    //
+    // Start and end the dump on a nice 16 byte boundary
+    // (even though the values are not 16-byte aligned).
+    //
+    ptr = (char*)((uint16_t)ptr & 0xFFF0); // Align to 16-byte boundary
+    sp  = (char*)((uint16_t)sp  | 0x000F); // Align sp to the 15th byte (at or above sp)
+
+    // Dump command main loop
+    while (ptr < sp) {
+      print_hex_word((uint16_t)ptr);      // Print the address
+      SERIAL_CHAR(':');
+      for (uint8_t i = 0; i < 16; i++) {  // and 16 data bytes
+        if (i == 8) SERIAL_CHAR('-');
+        print_hex_byte(ptr[i]);
+        SERIAL_CHAR(' ');
+      }
+      SERIAL_CHAR('|');                   // Point out non test bytes
+      for (uint8_t i = 0; i < 16; i++)
+        SERIAL_CHAR(ptr[i] == TEST_BYTE ? ' ' : '?');
+      SERIAL_EOL;
+      ptr += 16;
+      idle();
+    }
+  }
+#endif // M100_FREE_MEMORY_DUMPER
+
+/**
+ * M100 F
+ *  Return the number of free bytes in the memory pool,
+ *  with other vital statistics defining the pool.
+ */
+void free_memory_pool_report(const char * const ptr, const uint16_t size) {
+  int16_t max_cnt = -1;
+  uint16_t block_cnt = 0;
+  char *max_addr = NULL;
+  // Find the longest block of test bytes in the buffer
+  for (uint16_t i = 0; i < size; i++) {
+    char * const addr = ptr + i;
+    if (*addr == TEST_BYTE) {
+      const uint16_t j = count_test_bytes(addr);
+      if (j > 8) {
+        SERIAL_ECHOPAIR("Found ", j);
+        SERIAL_ECHOLNPAIR(" bytes free at 0x", hex_word((uint16_t)addr));
+        if (j > max_cnt) {
+          max_cnt  = j;
+          max_addr = addr;
+        }
+        i += j;
+        block_cnt++;
+      }
+    }
+  }
+  if (block_cnt > 1) {
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+    SERIAL_ECHOPAIR("\nLargest free block is ", max_cnt);
+    SERIAL_ECHOLNPAIR(" bytes at 0x", hex_word((uint16_t)max_addr));
+  }
+  SERIAL_ECHOLNPAIR("free_memory_is_corrupted() = ", free_memory_is_corrupted(ptr, size));
+}
+
+#if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+  /**
+   * M100 C<num>
+   *  Corrupt <num> locations in the free memory pool and report the corrupt addresses.
+   *  This is useful to check the correctness of the M100 D and the M100 F commands.
+   */
+  void corrupt_free_memory(char *ptr, const uint16_t size) {
+    if (code_seen('C')) {
+      ptr += 8;
+      const uint16_t near_top = top_of_stack() - ptr - 250, // -250 to avoid interrupt activity that's altered the stack.
+                     j = near_top / (size + 1);
+
+      SERIAL_ECHOLNPGM("Corrupting free memory block.\n");
+      for (uint16_t i = 1; i <= size; i++) {
+        char * const addr = ptr + i * j;
+        *addr = i;
+        SERIAL_ECHOPAIR("\nCorrupting address: 0x", hex_word((uint16_t)addr));
+      }
+      SERIAL_EOL;
+    }
+  }
+#endif // M100_FREE_MEMORY_CORRUPTOR
+
+/**
+ * M100 I
+ *  Init memory for the M100 tests. (Automatically applied on the first M100.)
+ */
+void init_free_memory(char *ptr, int16_t size) {
+  SERIAL_ECHOLNPGM("Initializing free memory block.\n\n");
+
+  size -= 250;    // -250 to avoid interrupt activity that's altered the stack.
+  if (size < 0) return;
+
+  ptr += 8;
+  memset(ptr, TEST_BYTE, size);
+
+  SERIAL_ECHO(size);
+  SERIAL_ECHOLNPGM(" bytes of memory initialized.\n");
+
+  for (uint16_t i = 0; i < size; i++) {
+    if (ptr[i] != TEST_BYTE) {
+      SERIAL_ECHOPAIR("? address : 0x", hex_word((uint16_t)ptr + i));
+      SERIAL_ECHOPAIR("=", hex_byte(ptr[i]));
+      SERIAL_EOL; SERIAL_EOL;
+    }
+  }
+}
+
+/**
+ * M100: Free Memory Check
+ */
+void gcode_M100() {
+  SERIAL_ECHOPAIR("\n__brkval : 0x", hex_word((uint16_t)__brkval));
+  SERIAL_ECHOPAIR("\n__bss_end : 0x", hex_word((uint16_t)&__bss_end));
+
+  char *ptr = END_OF_HEAP(), *sp = top_of_stack();
+
+  SERIAL_ECHOPAIR("\nstart of free space : 0x", hex_word((uint16_t)ptr));
+  SERIAL_ECHOLNPAIR("\nStack Pointer : 0x", hex_word((uint16_t)sp));
+
+  // Always init on the first invocation of M100
+  static bool m100_not_initialized = true;
+  if (m100_not_initialized || code_seen('I')) {
+    m100_not_initialized = false;
+    init_free_memory(ptr, sp - ptr);
   }
 
-#endif
+  #if ENABLED(M100_FREE_MEMORY_DUMPER)
 
+    if (code_seen('D'))
+      return dump_free_memory(ptr, sp);
+
+  #endif
+
+  if (code_seen('F'))
+    return free_memory_pool_report(ptr, sp - ptr);
+
+  #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
+
+    if (code_seen('C'))
+      return corrupt_free_memory(ptr, code_value_int());
+
+  #endif
+}
+
+#endif // M100_FREE_MEMORY_WATCHER
diff --git a/Marlin/hex_print_routines.cpp b/Marlin/hex_print_routines.cpp
index 9218f9f84..02b07754d 100644
--- a/Marlin/hex_print_routines.cpp
+++ b/Marlin/hex_print_routines.cpp
@@ -43,7 +43,7 @@ char* hex_word(const uint16_t w) {
   return _hex;
 }
 
-void print_hex_nybble(const uint8_t n) { SERIAL_CHAR(hex_nybble(n));  }
+void print_hex_nybble(const uint8_t n) { SERIAL_CHAR(hex_nybble(n)); }
 void print_hex_byte(const uint8_t b)   { SERIAL_ECHO(hex_byte(b)); }
 void print_hex_word(const uint16_t w)  { SERIAL_ECHO(hex_word(w)); }
 
diff --git a/Marlin/hex_print_routines.h b/Marlin/hex_print_routines.h
index 5956a72ee..c5c4f759f 100644
--- a/Marlin/hex_print_routines.h
+++ b/Marlin/hex_print_routines.h
@@ -42,4 +42,4 @@ void print_hex_byte(const uint8_t b);
 void print_hex_word(const uint16_t w);
 
 #endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER
-#endif // HEX_PRINT_ROUTINES_H
\ No newline at end of file
+#endif // HEX_PRINT_ROUTINES_H