From 84dec5da1065e1d506d2b4bd18294e5837ebe976 Mon Sep 17 00:00:00 2001
From: InsanityAutomation
 <38436470+InsanityAutomation@users.noreply.github.com>
Date: Wed, 18 Mar 2020 17:12:51 -0400
Subject: [PATCH] Add M42 M, improve M43 (#17173)

---
 Marlin/src/gcode/config/M43.cpp  |  2 ++
 Marlin/src/gcode/control/M42.cpp | 25 ++++++++++++++++++++-----
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp
index 661c41319..2c28da62f 100644
--- a/Marlin/src/gcode/config/M43.cpp
+++ b/Marlin/src/gcode/config/M43.cpp
@@ -67,6 +67,7 @@ inline void toggle_pins() {
     else {
       watchdog_refresh();
       report_pin_state_extended(pin, ignore_protection, true, PSTR("Pulsing   "));
+      const bool prior_mode = GET_PINMODE(pin);
       #if AVR_AT90USB1286_FAMILY // Teensy IDEs don't know about these pins so must use FASTIO
         if (pin == TEENSY_E2) {
           SET_OUTPUT(TEENSY_E2);
@@ -95,6 +96,7 @@ inline void toggle_pins() {
           watchdog_refresh();
         }
       }
+      pinMode(pin, prior_mode);
     }
     SERIAL_EOL();
   }
diff --git a/Marlin/src/gcode/control/M42.cpp b/Marlin/src/gcode/control/M42.cpp
index 7106ce550..74625b031 100644
--- a/Marlin/src/gcode/control/M42.cpp
+++ b/Marlin/src/gcode/control/M42.cpp
@@ -37,16 +37,33 @@
  *
  *  S<byte> Pin status from 0 - 255
  *  I       Flag to ignore Marlin's pin protection
+ *
+ *  M<mode> Pin mode: 0=INPUT  1=OUTPUT  2=INPUT_PULLUP  3=INPUT_PULLDOWN
  */
 void GcodeSuite::M42() {
-  if (!parser.seenval('S')) return;
-  const byte pin_status = parser.value_byte();
-
   const int pin_index = PARSED_PIN_INDEX('P', GET_PIN_MAP_INDEX(LED_PIN));
   if (pin_index < 0) return;
 
   const pin_t pin = GET_PIN_MAP_PIN(pin_index);
 
+  if (!parser.boolval('I') && pin_is_protected(pin)) return protected_pin_err();
+
+  if (parser.seenval('M')) {
+    switch (parser.value_byte()) {
+      case 0: pinMode(pin, INPUT); break;
+      case 1: pinMode(pin, OUTPUT); break;
+      case 2: pinMode(pin, INPUT_PULLUP); break;
+      #ifdef INPUT_PULLDOWN
+        case 3: pinMode(pin, INPUT_PULLDOWN); break;
+      #endif
+      default: SERIAL_ECHOLNPGM("Invalid Pin Mode");
+    }
+    return;
+  }
+
+  if (!parser.seenval('S')) return;
+  const byte pin_status = parser.value_byte();
+
   #if FAN_COUNT > 0
     switch (pin) {
       #if HAS_FAN0
@@ -76,8 +93,6 @@ void GcodeSuite::M42() {
     }
   #endif
 
-  if (!parser.boolval('I') && pin_is_protected(pin)) return protected_pin_err();
-
   pinMode(pin, OUTPUT);
   extDigitalWrite(pin, pin_status);
   analogWrite(pin, pin_status);