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();
 }