diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 402233a2e6..54d64cf915 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -259,12 +259,12 @@
 
 #ifndef ENDSTOPPULLUPS
   // fine Enstop settings: Individual Pullups. will be ignored if ENDSTOPPULLUPS is defined
-  #define ENDSTOPPULLUP_XMAX
-  #define ENDSTOPPULLUP_YMAX
-  #define ENDSTOPPULLUP_ZMAX
-  #define ENDSTOPPULLUP_XMIN
-  #define ENDSTOPPULLUP_YMIN
-  //#define ENDSTOPPULLUP_ZMIN
+  // #define ENDSTOPPULLUP_XMAX
+  // #define ENDSTOPPULLUP_YMAX
+  // #define ENDSTOPPULLUP_ZMAX
+  // #define ENDSTOPPULLUP_XMIN
+  // #define ENDSTOPPULLUP_YMIN
+  // #define ENDSTOPPULLUP_ZMIN
 #endif
 
 #ifdef ENDSTOPPULLUPS
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 5f1c77a056..909949eb91 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -18,12 +18,6 @@
 //#define WATCH_TEMP_PERIOD 40000 //40 seconds
 //#define WATCH_TEMP_INCREASE 10  //Heat up at least 10 degree in 20 seconds
 
-// Wait for Cooldown
-// This defines if the M109 call should not block if it is cooling down.
-// example: From a current temp of 220, you set M109 S200. 
-// if CooldownNoWait is defined M109 will not wait for the cooldown to finish
-#define CooldownNoWait true
-
 #ifdef PIDTEMP
   // this adds an experimental additional term to the heatingpower, proportional to the extrusion speed.
   // if Kc is choosen well, the additional required power due to increased melting should be compensated.
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 79911fccff..7053a96ec7 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -67,17 +67,9 @@
 // G91 - Use Relative Coordinates
 // G92 - Set current position to cordinates given
 
-//RepRap M Codes
+// M Codes
 // M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
 // M1   - Same as M0
-// M104 - Set extruder target temp
-// M105 - Read current temp
-// M106 - Fan on
-// M107 - Fan off
-// M109 - Wait for extruder current temp to reach target temp.
-// M114 - Display current position
-
-//Custom M Codes
 // M17  - Enable/Power all stepper motors
 // M18  - Disable all stepper motors; same as M84
 // M20  - List SD card
@@ -101,6 +93,12 @@
 //        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
 // M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
 // M92  - Set axis_steps_per_unit - same syntax as G92
+// M104 - Set extruder target temp
+// M105 - Read current temp
+// M106 - Fan on
+// M107 - Fan off
+// M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
+//        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
 // M114 - Output current position to serial port
 // M115 - Capabilities string
 // M117 - display message
@@ -110,7 +108,8 @@
 // M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
 // M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
 // M140 - Set bed target temp
-// M190 - Wait for bed current temp to reach target temp.
+// M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
+//        Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
 // M200 - Set filament diameter
 // M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
 // M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
@@ -242,6 +241,9 @@ bool Stopped=false;
   Servo servos[NUM_SERVOS];
 #endif
 
+bool CooldownNoWait = true;
+bool target_direction;
+
 //===========================================================================
 //=============================ROUTINES=============================
 //===========================================================================
@@ -1161,7 +1163,13 @@ void process_commands()
       #ifdef AUTOTEMP
         autotemp_enabled=false;
       #endif
-      if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
+      if (code_seen('S')) { 
+        setTargetHotend(code_value(), tmp_extruder);
+        CooldownNoWait = true;
+      } else if (code_seen('R')) {
+        setTargetHotend(code_value(), tmp_extruder);
+        CooldownNoWait = false;
+      }
       #ifdef AUTOTEMP
         if (code_seen('S')) autotemp_min=code_value();
         if (code_seen('B')) autotemp_max=code_value();
@@ -1176,7 +1184,7 @@ void process_commands()
       codenum = millis();
 
       /* See if we are heating up or cooling down */
-      bool target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
+      target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling
 
       #ifdef TEMP_RESIDENCY_TIME
         long residencyStart;
@@ -1232,9 +1240,18 @@ void process_commands()
     case 190: // M190 - Wait for bed heater to reach target.
     #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
         LCD_MESSAGEPGM(MSG_BED_HEATING);
-        if (code_seen('S')) setTargetBed(code_value());
+        if (code_seen('S')) { 
+          setTargetBed(code_value());
+          CooldownNoWait = true;
+        } else if (code_seen('R')) {
+          setTargetBed(code_value());
+          CooldownNoWait = false;
+        }
         codenum = millis();
-        while(isHeatingBed())
+        
+        target_direction = isHeatingBed(); // true if heating, false if cooling
+        
+        while ( target_direction ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) )
         {
           if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
           {
diff --git a/README.md b/README.md
index 97c847732e..033224865c 100644
--- a/README.md
+++ b/README.md
@@ -142,17 +142,9 @@ Implemented G Codes:
 *  G91 - Use Relative Coordinates
 *  G92 - Set current position to cordinates given
 
-RepRap M Codes
+M Codes
 *  M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
 *  M1   - Same as M0
-*  M104 - Set extruder target temp
-*  M105 - Read current temp
-*  M106 - Fan on
-*  M107 - Fan off
-*  M109 - Wait for extruder current temp to reach target temp.
-*  M114 - Display current position
-
-Custom M Codes
 *  M17  - Enable/Power all stepper motors
 *  M18  - Disable all stepper motors; same as M84
 *  M20  - List SD card
@@ -175,6 +167,12 @@ Custom M Codes
 *  M84  - Disable steppers until next move, or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
 *  M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
 *  M92  - Set axis_steps_per_unit - same syntax as G92
+*  M104 - Set extruder target temp
+*  M105 - Read current temp
+*  M106 - Fan on
+*  M107 - Fan off
+*  M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
+*         Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
 *  M114 - Output current position to serial port
 *  M115 - Capabilities string
 *  M117 - display message
@@ -184,7 +182,8 @@ Custom M Codes
 *  M128 - EtoP Open (BariCUDA EtoP = electricity to air pressure transducer by jmil)
 *  M129 - EtoP Closed (BariCUDA EtoP = electricity to air pressure transducer by jmil)
 *  M140 - Set bed target temp
-*  M190 - Wait for bed current temp to reach target temp.
+*  M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
+*         Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
 *  M200 - Set filament diameter
 *  M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
 *  M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!