From 7cc1ad923787da39f0d5cb6bc68fecf877939729 Mon Sep 17 00:00:00 2001
From: Victor Mateus Oliveira <rhapsodyv@gmail.com>
Date: Tue, 1 Sep 2020 14:26:28 -0300
Subject: [PATCH 01/11] new custom gcode to help developers test their code
 while working in marlin

---
 Marlin/Configuration_adv.h          |  3 +++
 Marlin/src/gcode/dev_custom_gcode.h | 39 +++++++++++++++++++++++++++++
 Marlin/src/gcode/gcode.cpp          |  8 ++++++
 Marlin/src/gcode/gcode.h            |  5 ++++
 4 files changed, 55 insertions(+)
 create mode 100644 Marlin/src/gcode/dev_custom_gcode.h

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index e4b3fbf2ae1b..f971732b1015 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -3524,3 +3524,6 @@
 
 // Enable Marlin dev mode which adds some special commands
 //#define MARLIN_DEV_MODE
+
+// Enable Marlin Custom Dev GCode M9999. It's to help developers test their code and new features.
+//#define DEV_CUSTOM_GCODE
diff --git a/Marlin/src/gcode/dev_custom_gcode.h b/Marlin/src/gcode/dev_custom_gcode.h
new file mode 100644
index 000000000000..69a2ea4fe301
--- /dev/null
+++ b/Marlin/src/gcode/dev_custom_gcode.h
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 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 <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+
+#if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
+  #include "gcode.h"
+  /**
+   * M9999: Custom Development GCode. Allow developers to talk to marlin when testing new features and bugs.
+   *
+   * You can edit and do whenever you want here, to test you development
+   */
+  void GcodeSuite::M9999() {
+    parser.seen('V');
+    int value = parser.value_int();
+    //call my test code
+    (value);
+  }
+#endif
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index c0a795025fc7..5ce1f04a5f61 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -924,6 +924,10 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
         case 7219: M7219(); break;                                // M7219: Set LEDs, columns, and rows
       #endif
 
+      #if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
+        case 9999: M9999(); break;                                // M9999: Custom Development GCode
+      #endif
+
       default: parser.unknown_command_warning(); break;
     }
     break;
@@ -1032,3 +1036,7 @@ void GcodeSuite::process_subcommands_now(char * gcode) {
   }
 
 #endif // HOST_KEEPALIVE_FEATURE
+
+#if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
+  #include "dev_custom_gcode.h"
+#endif
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index 23bf2c0ce5b1..5e1e77cece87 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -284,6 +284,7 @@
  * M995 - Touch screen calibration for TFT display
  * M997 - Perform in-application firmware update
  * M999 - Restart after being stopped by error
+ * M9999 - Custom Development GCode. Allow developers to talk to marlin when testing new features and bugs.
  *
  * "T" Codes
  *
@@ -875,6 +876,10 @@ class GcodeSuite {
     static void M1000();
   #endif
 
+  #if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
+    static void M9999();
+  #endif
+
   TERN_(SDSUPPORT, static void M1001());
 
   TERN_(MAX7219_GCODE, static void M7219());

From c0e8b3b5a15f6cf93252bacfdd42b9186bac22ca Mon Sep 17 00:00:00 2001
From: Victor Mateus Oliveira <rhapsodyv@gmail.com>
Date: Tue, 1 Sep 2020 14:27:18 -0300
Subject: [PATCH 02/11] to avoid devs commit their custom tests

---
 .gitignore | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/.gitignore b/.gitignore
index c163d339dff3..6dc5701c2357 100755
--- a/.gitignore
+++ b/.gitignore
@@ -184,3 +184,6 @@ cmake-build-*
 
 #Python
 __pycache__
+
+#Don't commit custom dev code
+Marlin/src/gcode/dev_custom_gcode.h
\ No newline at end of file

From 76e837d0a0f7bf2ce3defad054aea96f6e1d9221 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Tue, 1 Sep 2020 22:30:09 -0500
Subject: [PATCH 03/11] Allow for T-1 (select no tool)

---
 Marlin/src/gcode/control/T.cpp |  2 +-
 Marlin/src/gcode/gcode.h       |  2 +-
 Marlin/src/gcode/parser.cpp    | 20 ++++++++++++++------
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/Marlin/src/gcode/control/T.cpp b/Marlin/src/gcode/control/T.cpp
index 46cdfebf11ce..2513a81e752a 100644
--- a/Marlin/src/gcode/control/T.cpp
+++ b/Marlin/src/gcode/control/T.cpp
@@ -46,7 +46,7 @@
  *   Tx   Same as T?, but nozzle doesn't have to be preheated. Tc requires a preheated nozzle to finish filament load.
  *   Tc   Load to nozzle after filament was prepared by Tc and nozzle is already heated.
  */
-void GcodeSuite::T(const uint8_t tool_index) {
+void GcodeSuite::T(const int8_t tool_index) {
 
   DEBUG_SECTION(log_T, "T", DEBUGGING(LEVELING));
   if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("...(", tool_index, ")");
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index 5e1e77cece87..df653d80c787 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -886,7 +886,7 @@ class GcodeSuite {
 
   TERN_(CONTROLLER_FAN_EDITABLE, static void M710());
 
-  static void T(const uint8_t tool_index);
+  static void T(const int8_t tool_index);
 
 };
 
diff --git a/Marlin/src/gcode/parser.cpp b/Marlin/src/gcode/parser.cpp
index 91a24d1dd0eb..8079fff75e03 100644
--- a/Marlin/src/gcode/parser.cpp
+++ b/Marlin/src/gcode/parser.cpp
@@ -155,14 +155,15 @@ void GCodeParser::parse(char *p) {
     #endif
   #endif
 
+  #if ENABLED(MARLIN_DEV_MODE) || ANY(SWITCHING_TOOLHEAD, MAGNETIC_SWITCHING_TOOLHEAD, ELECTROMAGNETIC_SWITCHING_TOOLHEAD)
+    #define SIGNED_CODENUM 1
+  #endif
+
   // Bail if the letter is not G, M, or T
   // (or a valid parameter for the current motion mode)
   switch (letter) {
 
     case 'G': case 'M': case 'T':
-    #if ENABLED(CANCEL_OBJECTS)
-      case 'O':
-    #endif
       // Skip spaces to get the numeric part
       while (*p == ' ') p++;
 
@@ -178,22 +179,29 @@ void GCodeParser::parse(char *p) {
       #endif
 
       // Bail if there's no command code number
-      if (!NUMERIC(*p)) return;
+      if (!TERN(SIGNED_CODENUM,NUMERIC_SIGNED,NUMERIC)(*p)) return;
 
       // Save the command letter at this point
       // A '?' signifies an unknown command
       command_letter = letter;
 
+      int sign = 1; // Allow for a negative code like D-1 or T-1
+      if (ENABLED(SIGNED_CODENUM) && *p == '-') { sign = -1; ++p; }
+
       // Get the code number - integer digits only
       codenum = 0;
-      do { codenum *= 10, codenum += *p++ - '0'; } while (NUMERIC(*p));
+
+      do { codenum = codenum * 10 + *p++ - '0'; } while (NUMERIC(*p));
+
+      // Apply the sign, if any
+      codenum *= sign;
 
       // Allow for decimal point in command
       #if ENABLED(USE_GCODE_SUBCODES)
         if (*p == '.') {
           p++;
           while (NUMERIC(*p))
-          subcode *= 10, subcode += *p++ - '0';
+            subcode = subcode * 10 + *p++ - '0';
         }
       #endif
 

From 14dd81d6a174e932b597be20114f81b77babd461 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <thinkyhead@users.noreply.github.com>
Date: Wed, 2 Sep 2020 00:57:36 -0500
Subject: [PATCH 04/11] Update .gitignore

---
 .gitignore | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/.gitignore b/.gitignore
index 6dc5701c2357..c163d339dff3 100755
--- a/.gitignore
+++ b/.gitignore
@@ -184,6 +184,3 @@ cmake-build-*
 
 #Python
 __pycache__
-
-#Don't commit custom dev code
-Marlin/src/gcode/dev_custom_gcode.h
\ No newline at end of file

From 09e6f185103e9f3e58c390da6fbd9947106925f6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Tue, 1 Sep 2020 22:31:02 -0500
Subject: [PATCH 05/11] Make 'Dnnn' standard with MARLIN_DEV_MODE

---
 Marlin/Configuration_adv.h                    |  3 ---
 Marlin/src/gcode/gcode.cpp                    | 12 ++++------
 Marlin/src/gcode/gcode.h                      |  8 +++----
 .../gcode/{dev_custom_gcode.h => gcode_d.cpp} | 22 ++++++++++---------
 Marlin/src/gcode/parser.cpp                   |  1 +
 5 files changed, 20 insertions(+), 26 deletions(-)
 rename Marlin/src/gcode/{dev_custom_gcode.h => gcode_d.cpp} (69%)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index f971732b1015..e4b3fbf2ae1b 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -3524,6 +3524,3 @@
 
 // Enable Marlin dev mode which adds some special commands
 //#define MARLIN_DEV_MODE
-
-// Enable Marlin Custom Dev GCode M9999. It's to help developers test their code and new features.
-//#define DEV_CUSTOM_GCODE
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index 5ce1f04a5f61..b27453952e73 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -924,16 +924,16 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
         case 7219: M7219(); break;                                // M7219: Set LEDs, columns, and rows
       #endif
 
-      #if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
-        case 9999: M9999(); break;                                // M9999: Custom Development GCode
-      #endif
-
       default: parser.unknown_command_warning(); break;
     }
     break;
 
     case 'T': T(parser.codenum); break;                           // Tn: Tool Change
 
+    #if ENABLED(MARLIN_DEV_MODE)
+      case 'D': D(parser.codenum); break;                         // Dn: Debug codes
+    #endif
+
     default:
       #if ENABLED(WIFI_CUSTOM_COMMAND)
         if (wifi_custom_command(parser.command_ptr)) break;
@@ -1036,7 +1036,3 @@ void GcodeSuite::process_subcommands_now(char * gcode) {
   }
 
 #endif // HOST_KEEPALIVE_FEATURE
-
-#if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
-  #include "dev_custom_gcode.h"
-#endif
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index df653d80c787..1940ed693e4d 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -284,7 +284,7 @@
  * M995 - Touch screen calibration for TFT display
  * M997 - Perform in-application firmware update
  * M999 - Restart after being stopped by error
- * M9999 - Custom Development GCode. Allow developers to talk to marlin when testing new features and bugs.
+ * D... - Custom Development G-code. Add hooks to 'gcode_D.cpp' for developers to test features. (Requires MARLIN_DEV_MODE)
  *
  * "T" Codes
  *
@@ -405,6 +405,8 @@ class GcodeSuite {
 
 private:
 
+  TERN_(MARLIN_DEV_MODE, static void D());
+
   static void G0_G1(
     #if IS_SCARA || defined(G0_FEEDRATE)
       const bool fast_move=false
@@ -876,10 +878,6 @@ class GcodeSuite {
     static void M1000();
   #endif
 
-  #if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
-    static void M9999();
-  #endif
-
   TERN_(SDSUPPORT, static void M1001());
 
   TERN_(MAX7219_GCODE, static void M7219());
diff --git a/Marlin/src/gcode/dev_custom_gcode.h b/Marlin/src/gcode/gcode_d.cpp
similarity index 69%
rename from Marlin/src/gcode/dev_custom_gcode.h
rename to Marlin/src/gcode/gcode_d.cpp
index 69a2ea4fe301..afd17d82060b 100644
--- a/Marlin/src/gcode/dev_custom_gcode.h
+++ b/Marlin/src/gcode/gcode_d.cpp
@@ -19,21 +19,23 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  *
  */
-#pragma once
+#include "../inc/MarlinConfigPre.h"
 
-#include "../inc/MarlinConfig.h"
+#if ENABLED(MARLIN_DEV_MODE)
 
-#if BOTH(MARLIN_DEV_MODE, DEV_CUSTOM_GCODE)
   #include "gcode.h"
   /**
-   * M9999: Custom Development GCode. Allow developers to talk to marlin when testing new features and bugs.
+   * Dn: G-code for development and testing
    *
-   * You can edit and do whenever you want here, to test you development
+   * See https://reprap.org/wiki/G-code#D:_Debug_codes
+   *
+   * Put whatever else you need here to test ongoing development.
    */
-  void GcodeSuite::M9999() {
-    parser.seen('V');
-    int value = parser.value_int();
-    //call my test code
-    (value);
+  void GcodeSuite::D(const int8_t dcode) {
+    switch (dcode) {
+      case -1: for (;;) { /*spin*/ }
+      case 0:
+    }
   }
+
 #endif
diff --git a/Marlin/src/gcode/parser.cpp b/Marlin/src/gcode/parser.cpp
index 8079fff75e03..67deccad15b7 100644
--- a/Marlin/src/gcode/parser.cpp
+++ b/Marlin/src/gcode/parser.cpp
@@ -163,6 +163,7 @@ void GCodeParser::parse(char *p) {
   // (or a valid parameter for the current motion mode)
   switch (letter) {
 
+    TERN_(MARLIN_DEV_MODE, case 'D':)
     case 'G': case 'M': case 'T':
       // Skip spaces to get the numeric part
       while (*p == ' ') p++;

From 54e07fa14c4e6719915b60d6d5dcf3ded861aceb Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 2 Sep 2020 00:52:52 -0500
Subject: [PATCH 06/11] Work on some D-codes support

---
 Marlin/src/HAL/AVR/HAL.h         |  2 +
 Marlin/src/HAL/DUE/HAL.h         |  2 +
 Marlin/src/HAL/ESP32/HAL.h       |  2 +
 Marlin/src/HAL/LINUX/HAL.h       |  2 +
 Marlin/src/HAL/LPC1768/HAL.h     |  2 +
 Marlin/src/HAL/SAMD51/HAL.h      |  2 +
 Marlin/src/HAL/STM32/HAL.h       |  2 +
 Marlin/src/HAL/STM32F1/HAL.h     |  2 +
 Marlin/src/HAL/STM32_F4_F7/HAL.h |  2 +
 Marlin/src/HAL/TEENSY31_32/HAL.h |  2 +
 Marlin/src/HAL/TEENSY35_36/HAL.h |  2 +
 Marlin/src/core/macros.h         |  1 +
 Marlin/src/feature/e_parser.h    | 11 ++--
 Marlin/src/gcode/gcode_d.cpp     | 92 +++++++++++++++++++++++++++++++-
 Marlin/src/gcode/parser.cpp      | 23 +++-----
 Marlin/src/gcode/parser.h        | 27 ++++++++--
 Marlin/src/lcd/ultralcd.cpp      |  6 ++-
 17 files changed, 152 insertions(+), 30 deletions(-)

diff --git a/Marlin/src/HAL/AVR/HAL.h b/Marlin/src/HAL/AVR/HAL.h
index 609375e386e8..611b4ad22284 100644
--- a/Marlin/src/HAL/AVR/HAL.h
+++ b/Marlin/src/HAL/AVR/HAL.h
@@ -144,6 +144,8 @@ void HAL_init();
 inline void HAL_clear_reset_source() { MCUSR = 0; }
 inline uint8_t HAL_get_reset_source() { return MCUSR; }
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-function"
 extern "C" {
diff --git a/Marlin/src/HAL/DUE/HAL.h b/Marlin/src/HAL/DUE/HAL.h
index 31409c76dd56..3d0c97afb6b9 100644
--- a/Marlin/src/HAL/DUE/HAL.h
+++ b/Marlin/src/HAL/DUE/HAL.h
@@ -130,6 +130,8 @@ void sei();                     // Enable interrupts
 void HAL_clear_reset_source();  // clear reset reason
 uint8_t HAL_get_reset_source(); // get reset reason
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 //
 // ADC
 //
diff --git a/Marlin/src/HAL/ESP32/HAL.h b/Marlin/src/HAL/ESP32/HAL.h
index c91f9efff0b9..355214d8628f 100644
--- a/Marlin/src/HAL/ESP32/HAL.h
+++ b/Marlin/src/HAL/ESP32/HAL.h
@@ -98,6 +98,8 @@ void HAL_clear_reset_source();
 // reset reason
 uint8_t HAL_get_reset_source();
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 void _delay_ms(int delay);
 
 #pragma GCC diagnostic push
diff --git a/Marlin/src/HAL/LINUX/HAL.h b/Marlin/src/HAL/LINUX/HAL.h
index 96e121d9153e..02fc848981c0 100644
--- a/Marlin/src/HAL/LINUX/HAL.h
+++ b/Marlin/src/HAL/LINUX/HAL.h
@@ -102,6 +102,8 @@ uint16_t HAL_adc_get_result();
 inline void HAL_clear_reset_source(void) {}
 inline uint8_t HAL_get_reset_source(void) { return RST_POWER_ON; }
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 /* ---------------- Delay in cycles */
 FORCE_INLINE static void DELAY_CYCLES(uint64_t x) {
   Clock::delayCycles(x);
diff --git a/Marlin/src/HAL/LPC1768/HAL.h b/Marlin/src/HAL/LPC1768/HAL.h
index 0153bacf42dd..0986df02155e 100644
--- a/Marlin/src/HAL/LPC1768/HAL.h
+++ b/Marlin/src/HAL/LPC1768/HAL.h
@@ -223,6 +223,8 @@ void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255,
 void HAL_clear_reset_source(void);
 uint8_t HAL_get_reset_source(void);
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 // Add strcmp_P if missing
 #ifndef strcmp_P
   #define strcmp_P(a, b) strcmp((a), (b))
diff --git a/Marlin/src/HAL/SAMD51/HAL.h b/Marlin/src/HAL/SAMD51/HAL.h
index ea0f694cdc02..4df28bacab62 100644
--- a/Marlin/src/HAL/SAMD51/HAL.h
+++ b/Marlin/src/HAL/SAMD51/HAL.h
@@ -112,6 +112,8 @@ typedef int8_t pin_t;
 void HAL_clear_reset_source();  // clear reset reason
 uint8_t HAL_get_reset_source(); // get reset reason
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 //
 // ADC
 //
diff --git a/Marlin/src/HAL/STM32/HAL.h b/Marlin/src/HAL/STM32/HAL.h
index 08081331b736..17b818caba34 100644
--- a/Marlin/src/HAL/STM32/HAL.h
+++ b/Marlin/src/HAL/STM32/HAL.h
@@ -177,6 +177,8 @@ void HAL_clear_reset_source();
 // Reset reason
 uint8_t HAL_get_reset_source();
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 void _delay_ms(const int delay);
 
 extern "C" char* _sbrk(int incr);
diff --git a/Marlin/src/HAL/STM32F1/HAL.h b/Marlin/src/HAL/STM32F1/HAL.h
index 5a0b0460092b..71ea137a9adb 100644
--- a/Marlin/src/HAL/STM32F1/HAL.h
+++ b/Marlin/src/HAL/STM32F1/HAL.h
@@ -219,6 +219,8 @@ void HAL_clear_reset_source();
 // Reset reason
 uint8_t HAL_get_reset_source();
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 void _delay_ms(const int delay);
 
 #pragma GCC diagnostic push
diff --git a/Marlin/src/HAL/STM32_F4_F7/HAL.h b/Marlin/src/HAL/STM32_F4_F7/HAL.h
index 5601400c5ac9..055e871b1949 100644
--- a/Marlin/src/HAL/STM32_F4_F7/HAL.h
+++ b/Marlin/src/HAL/STM32_F4_F7/HAL.h
@@ -183,6 +183,8 @@ void HAL_clear_reset_source();
 // Reset reason
 uint8_t HAL_get_reset_source();
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 void _delay_ms(const int delay);
 
 /*
diff --git a/Marlin/src/HAL/TEENSY31_32/HAL.h b/Marlin/src/HAL/TEENSY31_32/HAL.h
index 890930f7f8be..f0d252968089 100644
--- a/Marlin/src/HAL/TEENSY31_32/HAL.h
+++ b/Marlin/src/HAL/TEENSY31_32/HAL.h
@@ -94,6 +94,8 @@ void HAL_clear_reset_source();
 // Get the reason for the reset
 uint8_t HAL_get_reset_source();
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); }
 
 #pragma GCC diagnostic push
diff --git a/Marlin/src/HAL/TEENSY35_36/HAL.h b/Marlin/src/HAL/TEENSY35_36/HAL.h
index 5442ae2d3b5f..28e0a862c5eb 100644
--- a/Marlin/src/HAL/TEENSY35_36/HAL.h
+++ b/Marlin/src/HAL/TEENSY35_36/HAL.h
@@ -99,6 +99,8 @@ void HAL_clear_reset_source();
 // Reset reason
 uint8_t HAL_get_reset_source();
 
+inline void HAL_reboot() {}  // reboot the board or restart the bootloader
+
 FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); }
 
 #pragma GCC diagnostic push
diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h
index 5fc1081019b5..72644b195ccd 100644
--- a/Marlin/src/core/macros.h
+++ b/Marlin/src/core/macros.h
@@ -215,6 +215,7 @@
 #define WITHIN(N,L,H)       ((N) >= (L) && (N) <= (H))
 #define NUMERIC(a)          WITHIN(a, '0', '9')
 #define DECIMAL(a)          (NUMERIC(a) || a == '.')
+#define HEXCHR(a)           (NUMERIC(a) ? (a) - '0' : WITHIN(a, 'a', 'f') ? ((a) - 'a' + 10)  : WITHIN(a, 'A', 'F') ? ((a) - 'A' + 10) : -1)
 #define NUMERIC_SIGNED(a)   (NUMERIC(a) || (a) == '-' || (a) == '+')
 #define DECIMAL_SIGNED(a)   (DECIMAL(a) || (a) == '-' || (a) == '+')
 #define COUNT(a)            (sizeof(a)/sizeof(*a))
diff --git a/Marlin/src/feature/e_parser.h b/Marlin/src/feature/e_parser.h
index 8d11463ecdc2..085cbd4eab0e 100644
--- a/Marlin/src/feature/e_parser.h
+++ b/Marlin/src/feature/e_parser.h
@@ -88,10 +88,8 @@ class EmergencyParser {
 
       case EP_N:
         switch (c) {
-          case '0': case '1': case '2':
-          case '3': case '4': case '5':
-          case '6': case '7': case '8':
-          case '9': case '-': case ' ':   break;
+          case '0' ... '9':
+          case '-': case ' ':   break;
           case 'M': state = EP_M;      break;
           default:  state = EP_IGNORE;
         }
@@ -153,10 +151,7 @@ class EmergencyParser {
       case EP_M876S:
         switch (c) {
           case ' ': break;
-          case '0': case '1': case '2':
-          case '3': case '4': case '5':
-          case '6': case '7': case '8':
-          case '9':
+          case '0' ... '9':
             state = EP_M876SN;
             M876_reason = (uint8_t)(c - '0');
             break;
diff --git a/Marlin/src/gcode/gcode_d.cpp b/Marlin/src/gcode/gcode_d.cpp
index afd17d82060b..50a9696f79cf 100644
--- a/Marlin/src/gcode/gcode_d.cpp
+++ b/Marlin/src/gcode/gcode_d.cpp
@@ -24,6 +24,10 @@
 #if ENABLED(MARLIN_DEV_MODE)
 
   #include "gcode.h"
+  #include "../module/settings.h"
+  #include "../libs/hex_print.h"
+  #include "../HAL/shared/eeprom_if.h"
+
   /**
    * Dn: G-code for development and testing
    *
@@ -33,8 +37,94 @@
    */
   void GcodeSuite::D(const int8_t dcode) {
     switch (dcode) {
-      case -1: for (;;) { /*spin*/ }
+
+      case -1:
+        for (;;); // forever
+
       case 0:
+        HAL_reboot();
+        break;
+
+      case 1:
+        // TODO: Zero or pattern-fill the EEPROM data
+        //settings.reset();
+        //settings.save();
+        HAL_reboot();
+        break;
+
+      case 2: { // D2 Read / Write SRAM
+        #define SRAM_SIZE 8192
+        uint8_t *adr = parser.uhexval('A');
+        uint16_t len = parser.ushortval('C', 1);
+        NOMORE(adr, SRAM_SIZE - 1);
+        NOMORE(len, SRAM_SIZE - adr);
+        if (parser.seenval('X')) {
+          // TODO: Write the hex bytes after the X
+          //while (len--) {
+          //}
+        }
+        else {
+          while (len--) print_hex_byte(adr++);
+          SERIAL_EOL();
+        }
+      } break;
+
+      case 3: { // D3 Read / Write EEPROM
+        uint8_t *adr = parser.uhexval('A');
+        uint16_t len = parser.ushortval('C', 1);
+        NOMORE(adr, MARLIN_EEPROM_SIZE - 1);
+        NOMORE(len, MARLIN_EEPROM_SIZE - adr);
+        if (parser.seenval('X')) {
+          // TODO: Write the hex bytes after the X
+          //while (len--) {
+          //}
+        }
+        else {
+          while (len--) {
+            // TODO: Read bytes from EEPROM
+            print_hex_byte(eeprom_read_byte(adr++));
+          }
+          SERIAL_EOL();
+        }
+      } break;
+
+      case 4: { // D4 Read / Write PIN
+        const uint8_t pin = parser.byteval('P');
+        const bool is_out = parser.boolval('F'),
+                   val = parser.byteval('V', LOW);
+        if (parser.seenval('X')) {
+          // TODO: Write the hex bytes after the X
+          //while (len--) {
+          //}
+        }
+        else {
+          while (len--) {
+            // TODO: Read bytes from EEPROM
+            print_hex_byte(eeprom_read_byte(adr++));
+          }
+          SERIAL_EOL();
+        }
+      } break;
+
+      case 5: { // D4 Read / Write onboard Flash
+        #define FLASH_SIZE 1024
+        uint16_t adr = parser.uhexval('A');
+        uint16_t len = parser.ushortval('C', 1);
+        NOMORE(adr, FLASH_SIZE - 1);
+        NOMORE(len, FLASH_SIZE - adr);
+        if (parser.seenval('X')) {
+          // TODO: Write the hex bytes after the X
+          //while (len--) {
+          //}
+        }
+        else {
+          while (len--) {
+            // TODO: Read bytes from EEPROM
+            print_hex_byte(eeprom_read_byte(adr++));
+          }
+          SERIAL_EOL();
+        }
+      } break;
     }
   }
 
diff --git a/Marlin/src/gcode/parser.cpp b/Marlin/src/gcode/parser.cpp
index 67deccad15b7..b6b8a995a023 100644
--- a/Marlin/src/gcode/parser.cpp
+++ b/Marlin/src/gcode/parser.cpp
@@ -147,14 +147,6 @@ void GCodeParser::parse(char *p) {
     starpos[1] = '\0';
   }
 
-  #if ENABLED(GCODE_MOTION_MODES)
-    #if ENABLED(ARC_SUPPORT)
-      #define GTOP 3
-    #else
-      #define GTOP 1
-    #endif
-  #endif
-
   #if ENABLED(MARLIN_DEV_MODE) || ANY(SWITCHING_TOOLHEAD, MAGNETIC_SWITCHING_TOOLHEAD, ELECTROMAGNETIC_SWITCHING_TOOLHEAD)
     #define SIGNED_CODENUM 1
   #endif
@@ -210,11 +202,8 @@ void GCodeParser::parse(char *p) {
       while (*p == ' ') p++;
 
       #if ENABLED(GCODE_MOTION_MODES)
-        if (letter == 'G' && (codenum <= GTOP || codenum == 5
-                                #if ENABLED(G38_PROBE_TARGET)
-                                  || codenum == 38
-                                #endif
-                             )
+        if (letter == 'G'
+          && (codenum <= TERN(ARC_SUPPORT, 3, 1) || codenum == 5 || TERN0(G38_PROBE_TARGET, codenum == 38))
         ) {
           motion_mode_codenum = codenum;
           TERN_(USE_GCODE_SUBCODES, motion_mode_subcode = subcode);
@@ -225,12 +214,12 @@ void GCodeParser::parse(char *p) {
 
     #if ENABLED(GCODE_MOTION_MODES)
       #if ENABLED(ARC_SUPPORT)
-        case 'I': case 'J': case 'R':
+        case 'I' ... 'J': case 'R':
           if (motion_mode_codenum != 2 && motion_mode_codenum != 3) return;
       #endif
-      case 'P': case 'Q':
+      case 'P' ... 'Q':
         if (motion_mode_codenum != 5) return;
-      case 'X': case 'Y': case 'Z': case 'E': case 'F':
+      case 'X' ... 'Z': case 'E' ... 'F':
         if (motion_mode_codenum < 0) return;
         command_letter = 'G';
         codenum = motion_mode_codenum;
@@ -256,7 +245,7 @@ void GCodeParser::parse(char *p) {
     #if ENABLED(EXPECTED_PRINTER_CHECK)
       case 16:
     #endif
-    case 23: case 28: case 30: case 117: case 118: case 928:
+    case 23: case 28: case 30: case 117 ... 118: case 928:
       string_arg = unescape_string(p);
       return;
     default: break;
diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
index eb614c33b4c9..324a4d512b8a 100644
--- a/Marlin/src/gcode/parser.h
+++ b/Marlin/src/gcode/parser.h
@@ -114,6 +114,11 @@ class GCodeParser {
     return valid_signless(p) || ((p[0] == '-' || p[0] == '+') && valid_signless(&p[1])); // [-+]?.?[0-9]
   }
 
+  FORCE_INLINE static bool valid_number(const char * const p) {
+    // TODO: With MARLIN_DEV_MODE allow HEX values starting with "x"
+    return valid_float(p);
+  }
+
   #if ENABLED(FASTER_GCODE_PARSER)
 
     FORCE_INLINE static bool valid_int(const char * const p) {
@@ -142,8 +147,12 @@ class GCodeParser {
       if (ind >= COUNT(param)) return false; // Only A-Z
       const bool b = TEST32(codebits, ind);
       if (b) {
-        char * const ptr = command_ptr + param[ind];
-        value_ptr = param[ind] && valid_float(ptr) ? ptr : nullptr;
+        if (param[ind]) {
+          char * const ptr = command_ptr + param[ind];
+          value_ptr = valid_number(ptr) ? ptr : nullptr;
+        }
+        else
+          value_ptr = nullptr;
       }
       return b;
     }
@@ -198,7 +207,7 @@ class GCodeParser {
     static inline bool seen(const char c) {
       char *p = strgchr(command_args, c);
       const bool b = !!p;
-      if (b) value_ptr = valid_float(&p[1]) ? &p[1] : nullptr;
+      if (b) value_ptr = valid_number(&p[1]) ? &p[1] : nullptr;
       return b;
     }
 
@@ -401,6 +410,18 @@ class GCodeParser {
   static inline float    linearval(const char c, const float dval=0)    { return seenval(c) ? value_linear_units() : dval; }
   static inline float    celsiusval(const char c, const float dval=0)   { return seenval(c) ? value_celsius()      : dval; }
 
+  #if ENABLED(MARLIN_DEV_MODE)
+
+    static inline const uint8_t* hex_adr_val(const char c, const uint8_t * const dval=nullptr) {
+      if (!seen(c) || *value_ptr != 'x') return dval;
+      char *vp = value_ptr + 1;
+      uint8_t *out = nullptr;
+      for (; HEXCHR(*vp) >= 0, vp++)
+        out = (out << 8) | HEXCHR(*vp);
+      return out;
+    }
+
+  #endif
 };
 
 extern GCodeParser parser;
diff --git a/Marlin/src/lcd/ultralcd.cpp b/Marlin/src/lcd/ultralcd.cpp
index b30f6e49c9e0..65aff7deaa63 100644
--- a/Marlin/src/lcd/ultralcd.cpp
+++ b/Marlin/src/lcd/ultralcd.cpp
@@ -123,11 +123,15 @@ constexpr uint8_t epps = ENCODER_PULSES_PER_STEP;
 #include "lcdprint.h"
 
 #include "../sd/cardreader.h"
-#include "../module/settings.h"
+
 #include "../module/temperature.h"
 #include "../module/planner.h"
 #include "../module/motion.h"
 
+#if HAS_LCD_MENU
+  #include "../module/settings.h"
+#endif
+
 #if ENABLED(AUTO_BED_LEVELING_UBL)
   #include "../feature/bedlevel/bedlevel.h"
 #endif

From 82cb51b37a8d088b731814d6176468474496c507 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 2 Sep 2020 01:19:35 -0500
Subject: [PATCH 07/11] Wrap sign define and use

---
 Marlin/src/gcode/parser.cpp | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/Marlin/src/gcode/parser.cpp b/Marlin/src/gcode/parser.cpp
index b6b8a995a023..8dad98295297 100644
--- a/Marlin/src/gcode/parser.cpp
+++ b/Marlin/src/gcode/parser.cpp
@@ -178,16 +178,18 @@ void GCodeParser::parse(char *p) {
       // A '?' signifies an unknown command
       command_letter = letter;
 
-      int sign = 1; // Allow for a negative code like D-1 or T-1
-      if (ENABLED(SIGNED_CODENUM) && *p == '-') { sign = -1; ++p; }
+      {
+        int sign = 1; // Allow for a negative code like D-1 or T-1
+        if (ENABLED(SIGNED_CODENUM) && *p == '-') { sign = -1; ++p; }
 
-      // Get the code number - integer digits only
-      codenum = 0;
+        // Get the code number - integer digits only
+        codenum = 0;
 
-      do { codenum = codenum * 10 + *p++ - '0'; } while (NUMERIC(*p));
+        do { codenum = codenum * 10 + *p++ - '0'; } while (NUMERIC(*p));
 
-      // Apply the sign, if any
-      codenum *= sign;
+        // Apply the sign, if any
+        codenum *= sign;
+      }
 
       // Allow for decimal point in command
       #if ENABLED(USE_GCODE_SUBCODES)

From e2202d4876537751d3d4f117a1eef63c46704ada Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 2 Sep 2020 01:24:11 -0500
Subject: [PATCH 08/11] Fix loop

---
 Marlin/src/gcode/parser.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
index 324a4d512b8a..0d5c3797f61d 100644
--- a/Marlin/src/gcode/parser.h
+++ b/Marlin/src/gcode/parser.h
@@ -414,9 +414,8 @@ class GCodeParser {
 
     static inline const uint8_t* hex_adr_val(const char c, const uint8_t * const dval=nullptr) {
       if (!seen(c) || *value_ptr != 'x') return dval;
-      char *vp = value_ptr + 1;
       uint8_t *out = nullptr;
-      for (; HEXCHR(*vp) >= 0, vp++)
+      for (char *vp = value_ptr + 1; HEXCHR(*vp) >= 0; vp++)
         out = (out << 8) | HEXCHR(*vp);
       return out;
     }

From 32a0822bbf0263f1dbdf6ebbcf3b7f3fb5dbfbfc Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Wed, 2 Sep 2020 01:35:10 -0500
Subject: [PATCH 09/11] ptr to int

---
 Marlin/src/gcode/parser.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
index 0d5c3797f61d..746b3fd01e71 100644
--- a/Marlin/src/gcode/parser.h
+++ b/Marlin/src/gcode/parser.h
@@ -416,7 +416,7 @@ class GCodeParser {
       if (!seen(c) || *value_ptr != 'x') return dval;
       uint8_t *out = nullptr;
       for (char *vp = value_ptr + 1; HEXCHR(*vp) >= 0; vp++)
-        out = (out << 8) | HEXCHR(*vp);
+        out = (uint8_t*)((uintptr_t(out) << 8) | HEXCHR(*vp));
       return out;
     }
 

From f851ca1dc49521f30a80805fd957975232a6fcd2 Mon Sep 17 00:00:00 2001
From: Victor Mateus Oliveira <rhapsodyv@gmail.com>
Date: Wed, 2 Sep 2020 09:42:49 -0300
Subject: [PATCH 10/11] fix compiling

---
 Marlin/src/gcode/gcode.h     |  2 +-
 Marlin/src/gcode/gcode_d.cpp | 45 +++++++++++++++++++-----------------
 Marlin/src/gcode/parser.h    |  2 +-
 3 files changed, 26 insertions(+), 23 deletions(-)

diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index 1940ed693e4d..9afdcdef8983 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -405,7 +405,7 @@ class GcodeSuite {
 
 private:
 
-  TERN_(MARLIN_DEV_MODE, static void D());
+  TERN_(MARLIN_DEV_MODE, static void D(const int16_t dcode));
 
   static void G0_G1(
     #if IS_SCARA || defined(G0_FEEDRATE)
diff --git a/Marlin/src/gcode/gcode_d.cpp b/Marlin/src/gcode/gcode_d.cpp
index 50a9696f79cf..1d5c0f8b7d8a 100644
--- a/Marlin/src/gcode/gcode_d.cpp
+++ b/Marlin/src/gcode/gcode_d.cpp
@@ -35,7 +35,7 @@
    *
    * Put whatever else you need here to test ongoing development.
    */
-  void GcodeSuite::D(const int8_t dcode) {
+  void GcodeSuite::D(const int16_t dcode) {
     switch (dcode) {
 
       case -1:
@@ -54,26 +54,29 @@
 
       case 2: { // D2 Read / Write SRAM
         #define SRAM_SIZE 8192
-        uint8_t *adr = parser.uhexval('A');
+        uint8_t *adr = parser.hex_adr_val('A');
         uint16_t len = parser.ushortval('C', 1);
-        NOMORE(adr, SRAM_SIZE - 1);
-        NOMORE(len, SRAM_SIZE - adr);
+        NOMORE(*adr, SRAM_SIZE - 1);
+        NOMORE(len, SRAM_SIZE - (uint16_t)adr);
         if (parser.seenval('X')) {
           // TODO: Write the hex bytes after the X
           //while (len--) {
           //}
         }
         else {
-          while (len--) print_hex_byte(adr++);
+          while (len--) print_hex_byte(*(adr++));
           SERIAL_EOL();
         }
       } break;
 
       case 3: { // D3 Read / Write EEPROM
-        uint8_t *adr = parser.uhexval('A');
+        uint8_t *adr = parser.hex_adr_val('A');
         uint16_t len = parser.ushortval('C', 1);
-        NOMORE(adr, MARLIN_EEPROM_SIZE - 1);
-        NOMORE(len, MARLIN_EEPROM_SIZE - adr);
+        #ifndef MARLIN_EEPROM_SIZE
+          #define MARLIN_EEPROM_SIZE size_t(E2END + 1)
+        #endif
+        NOMORE(*adr, MARLIN_EEPROM_SIZE - 1);
+        NOMORE(len, MARLIN_EEPROM_SIZE - (uint16_t)adr);
         if (parser.seenval('X')) {
           // TODO: Write the hex bytes after the X
           //while (len--) {
@@ -82,46 +85,46 @@
         else {
           while (len--) {
             // TODO: Read bytes from EEPROM
-            print_hex_byte(eeprom_read_byte(adr++));
+            // print_hex_byte(eeprom_read_byte(adr++));
           }
           SERIAL_EOL();
         }
       } break;
 
       case 4: { // D4 Read / Write PIN
-        const uint8_t pin = parser.byteval('P');
-        const bool is_out = parser.boolval('F'),
-                   val = parser.byteval('V', LOW);
+        // const uint8_t pin = parser.byteval('P');
+        // const bool is_out = parser.boolval('F'),
+        //            val = parser.byteval('V', LOW);
         if (parser.seenval('X')) {
           // TODO: Write the hex bytes after the X
           //while (len--) {
           //}
         }
         else {
-          while (len--) {
+          // while (len--) {
             // TODO: Read bytes from EEPROM
-            print_hex_byte(eeprom_read_byte(adr++));
-          }
+            // print_hex_byte(eeprom_read_byte(*(adr++));
+          // }
           SERIAL_EOL();
         }
       } break;
 
       case 5: { // D4 Read / Write onboard Flash
         #define FLASH_SIZE 1024
-        uint16_t adr = parser.uhexval('A');
+        uint8_t *adr = parser.hex_adr_val('A');
         uint16_t len = parser.ushortval('C', 1);
-        NOMORE(adr, FLASH_SIZE - 1);
-        NOMORE(len, FLASH_SIZE - adr);
+        NOMORE(*adr, FLASH_SIZE - 1);
+        NOMORE(len, FLASH_SIZE - (uint16_t)adr);
         if (parser.seenval('X')) {
           // TODO: Write the hex bytes after the X
           //while (len--) {
           //}
         }
         else {
-          while (len--) {
+          // while (len--) {
             // TODO: Read bytes from EEPROM
-            print_hex_byte(eeprom_read_byte(adr++));
-          }
+            // print_hex_byte(eeprom_read_byte(adr++));
+          // }
           SERIAL_EOL();
         }
       } break;
diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
index 746b3fd01e71..5717ea17cd92 100644
--- a/Marlin/src/gcode/parser.h
+++ b/Marlin/src/gcode/parser.h
@@ -412,7 +412,7 @@ class GCodeParser {
 
   #if ENABLED(MARLIN_DEV_MODE)
 
-    static inline const uint8_t* hex_adr_val(const char c, const uint8_t * const dval=nullptr) {
+    static inline uint8_t* hex_adr_val(const char c, uint8_t * const dval=nullptr) {
       if (!seen(c) || *value_ptr != 'x') return dval;
       uint8_t *out = nullptr;
       for (char *vp = value_ptr + 1; HEXCHR(*vp) >= 0; vp++)

From 6baeefd7667866662a1845501324eff98db3cc6e Mon Sep 17 00:00:00 2001
From: Victor Mateus Oliveira <rhapsodyv@gmail.com>
Date: Wed, 2 Sep 2020 21:14:44 -0300
Subject: [PATCH 11/11] fill some g codes (not tested yet)... used
 persistentStore to simplify the eeprom access.. will it works?

---
 Marlin/src/gcode/gcode_d.cpp | 85 ++++++++++++++++++++++++++----------
 Marlin/src/gcode/parser.h    |  8 ++++
 2 files changed, 70 insertions(+), 23 deletions(-)

diff --git a/Marlin/src/gcode/gcode_d.cpp b/Marlin/src/gcode/gcode_d.cpp
index 1d5c0f8b7d8a..4bc3b6c6c377 100644
--- a/Marlin/src/gcode/gcode_d.cpp
+++ b/Marlin/src/gcode/gcode_d.cpp
@@ -45,47 +45,85 @@
         HAL_reboot();
         break;
 
-      case 1:
-        // TODO: Zero or pattern-fill the EEPROM data
-        //settings.reset();
-        //settings.save();
+      case 1: {
+        // Zero or pattern-fill the EEPROM data
+        #if ENABLED(EEPROM_SETTINGS)
+          persistentStore.access_start();
+          size_t total = persistentStore.capacity();
+          int pos = 0;
+          const uint8_t value = 0x0;
+          while(total--) {
+            persistentStore.write_data(pos, &value, 1);
+          }
+          persistentStore.access_finish();
+        #else
+          settings.reset();
+          settings.save();
+        #endif
         HAL_reboot();
-        break;
+      } break;
 
       case 2: { // D2 Read / Write SRAM
         #define SRAM_SIZE 8192
-        uint8_t *adr = parser.hex_adr_val('A');
+        uint8_t *pointer = parser.hex_adr_val('A');
         uint16_t len = parser.ushortval('C', 1);
-        NOMORE(*adr, SRAM_SIZE - 1);
-        NOMORE(len, SRAM_SIZE - (uint16_t)adr);
+        uintptr_t addr = (uintptr_t)pointer;
+        NOMORE(addr, (size_t)(SRAM_SIZE - 1));
+        NOMORE(len, SRAM_SIZE - addr);
         if (parser.seenval('X')) {
-          // TODO: Write the hex bytes after the X
-          //while (len--) {
-          //}
+          // Write the hex bytes after the X
+          uint16_t val = parser.hex_val('X');
+          while (len--) {
+            *pointer = val;
+            pointer++;
+          }
         }
         else {
-          while (len--) print_hex_byte(*(adr++));
+          while (len--) print_hex_byte(*(pointer++));
           SERIAL_EOL();
         }
       } break;
 
       case 3: { // D3 Read / Write EEPROM
-        uint8_t *adr = parser.hex_adr_val('A');
+        uint8_t *pointer = parser.hex_adr_val('A');
         uint16_t len = parser.ushortval('C', 1);
+        uintptr_t addr = (uintptr_t)pointer;
         #ifndef MARLIN_EEPROM_SIZE
           #define MARLIN_EEPROM_SIZE size_t(E2END + 1)
         #endif
-        NOMORE(*adr, MARLIN_EEPROM_SIZE - 1);
-        NOMORE(len, MARLIN_EEPROM_SIZE - (uint16_t)adr);
+        NOMORE(addr, (size_t)(MARLIN_EEPROM_SIZE - 1));
+        NOMORE(len, MARLIN_EEPROM_SIZE - addr);
         if (parser.seenval('X')) {
-          // TODO: Write the hex bytes after the X
-          //while (len--) {
-          //}
+          uint16_t val = parser.hex_val('X');
+          #if ENABLED(EEPROM_SETTINGS)
+            persistentStore.access_start();
+            while(len--) {
+              int pos = 0;
+              persistentStore.write_data(pos, (uint8_t *)&val, sizeof(val));
+            }
+            SERIAL_EOL();
+            persistentStore.access_finish();
+          #else
+            SERIAL_ECHOLN("NO EEPROM");
+          #endif
         }
         else {
           while (len--) {
-            // TODO: Read bytes from EEPROM
-            // print_hex_byte(eeprom_read_byte(adr++));
+            // Read bytes from EEPROM
+            #if ENABLED(EEPROM_SETTINGS)
+              persistentStore.access_start();
+              uint8_t val;
+              while(len--) {
+                int pos = 0;
+                if (!persistentStore.read_data(pos, (uint8_t *)&val, sizeof(val))) {
+                  print_hex_byte(val);
+                }
+              }
+              SERIAL_EOL();
+              persistentStore.access_finish();
+            #else
+              SERIAL_ECHOLN("NO EEPROM");
+            #endif
           }
           SERIAL_EOL();
         }
@@ -111,10 +149,11 @@
 
       case 5: { // D4 Read / Write onboard Flash
         #define FLASH_SIZE 1024
-        uint8_t *adr = parser.hex_adr_val('A');
+        uint8_t *pointer = parser.hex_adr_val('A');
         uint16_t len = parser.ushortval('C', 1);
-        NOMORE(*adr, FLASH_SIZE - 1);
-        NOMORE(len, FLASH_SIZE - (uint16_t)adr);
+        uintptr_t addr = (uintptr_t)pointer;
+        NOMORE(addr, (size_t)(FLASH_SIZE - 1));
+        NOMORE(len, FLASH_SIZE - addr);
         if (parser.seenval('X')) {
           // TODO: Write the hex bytes after the X
           //while (len--) {
diff --git a/Marlin/src/gcode/parser.h b/Marlin/src/gcode/parser.h
index 5717ea17cd92..42b85ca27195 100644
--- a/Marlin/src/gcode/parser.h
+++ b/Marlin/src/gcode/parser.h
@@ -420,6 +420,14 @@ class GCodeParser {
       return out;
     }
 
+    static inline uint16_t hex_val(const char c, uint16_t const dval=0) {
+      if (!seen(c) || *value_ptr != 'x') return dval;
+      uint16_t out = 0;
+      for (char *vp = value_ptr + 1; HEXCHR(*vp) >= 0; vp++)
+        out = ((out) << 8) | HEXCHR(*vp);
+      return out;
+    }
+
   #endif
 };