diff --git a/xs/src/libslic3r/GCode.cpp b/xs/src/libslic3r/GCode.cpp
index 7a935b3640..2203846329 100644
--- a/xs/src/libslic3r/GCode.cpp
+++ b/xs/src/libslic3r/GCode.cpp
@@ -98,7 +98,7 @@ OozePrevention::pre_toolchange(GCode &gcodegen)
     if (gcodegen.config.standby_temperature_delta.value != 0) {
         // we assume that heating is always slower than cooling, so no need to block
         gcode += gcodegen.writer.set_temperature
-            (this->_get_temp(gcodegen) + gcodegen.config.standby_temperature_delta.value, false);
+            (this->_get_temp(gcodegen) + gcodegen.config.standby_temperature_delta.value, false, gcodegen.writer.extruder()->id);
     }
     
     return gcode;
@@ -110,7 +110,7 @@ OozePrevention::post_toolchange(GCode &gcodegen)
     std::string gcode;
     
     if (gcodegen.config.standby_temperature_delta.value != 0) {
-        gcode += gcodegen.writer.set_temperature(this->_get_temp(gcodegen), true);
+        gcode += gcodegen.writer.set_temperature(this->_get_temp(gcodegen), true, gcodegen.writer.extruder()->id);
     }
     
     return gcode;
diff --git a/xs/src/libslic3r/GCodeWriter.cpp b/xs/src/libslic3r/GCodeWriter.cpp
index d256ab2af0..a7a52cc1af 100644
--- a/xs/src/libslic3r/GCodeWriter.cpp
+++ b/xs/src/libslic3r/GCodeWriter.cpp
@@ -78,11 +78,9 @@ GCodeWriter::postamble() const
 std::string
 GCodeWriter::set_temperature(unsigned int temperature, bool wait, int tool) const
 {
-    if (wait && (FLAVOR_IS(gcfMakerWare) || FLAVOR_IS(gcfSailfish)))
-        return "";
     
     std::string code, comment;
-    if (wait && FLAVOR_IS_NOT(gcfTeacup)) {
+    if (wait && FLAVOR_IS_NOT(gcfTeacup) && FLAVOR_IS_NOT(gcfMakerWare) && FLAVOR_IS_NOT(gcfSailfish)) {
         code = "M109";
         comment = "set temperature and wait for it to be reached";
     } else {
@@ -106,6 +104,9 @@ GCodeWriter::set_temperature(unsigned int temperature, bool wait, int tool) cons
     if (FLAVOR_IS(gcfTeacup) && wait)
         gcode << "M116 ; wait for temperature to be reached\n";
     
+    if (wait && tool !=-1 && (FLAVOR_IS(gcfMakerWare) || FLAVOR_IS(gcfSailfish)))
+        gcode << "M6 T" << tool << " ; wait for temperature to be reached\n";
+    
     return gcode.str();
 }