diff --git a/Firmware/first_lay_cal.cpp b/Firmware/first_lay_cal.cpp
index d3f9816eb..99a905fdf 100644
--- a/Firmware/first_lay_cal.cpp
+++ b/Firmware/first_lay_cal.cpp
@@ -221,26 +221,30 @@ void lay1cal_meander(float layer_height, float extrusion_width)
 
 //! @brief Print square
 //!
-//! This function needs to be called 16 times for i from 0 to 15.
+//! This function needs to be called 4 times with step of 0,4,8,12
 //!
 //! @param cmd_buffer character buffer needed to format gcodes
 //! @param i iteration
-void lay1cal_square(char *cmd_buffer, uint8_t i, float layer_height, float extrusion_width)
+void lay1cal_square(uint8_t step, float layer_height, float extrusion_width)
 {
+    char cmd_buffer[30];
     const float long_length = 20;
     const float short_length = spacing(layer_height, extrusion_width);
     const float long_extrusion = count_e(layer_height, extrusion_width, long_length);
     const float short_extrusion = count_e(layer_height, extrusion_width, short_length);
-
     static const char fmt1[] PROGMEM = "G1 X%d Y%-.2f E%-.3f";
-    sprintf_P(cmd_buffer, fmt1, 70, (35 - i*short_length * 2), long_extrusion);
-    enquecommand(cmd_buffer);
-    sprintf_P(cmd_buffer, fmt1, 70, (35 - (2 * i + 1)*short_length), short_extrusion);
-    enquecommand(cmd_buffer);
-    sprintf_P(cmd_buffer, fmt1, 50, (35 - (2 * i + 1)*short_length), long_extrusion);
-    enquecommand(cmd_buffer);
-    sprintf_P(cmd_buffer, fmt1, 50, (35 - (i + 1)*short_length * 2), short_extrusion);
-    enquecommand(cmd_buffer);
+
+    for (uint8_t i = step; i < step+4; ++i)
+    {
+        sprintf_P(cmd_buffer, fmt1, 70, (35 - i*short_length * 2), long_extrusion);
+        enquecommand(cmd_buffer);
+        sprintf_P(cmd_buffer, fmt1, 70, (35 - (2 * i + 1)*short_length), short_extrusion);
+        enquecommand(cmd_buffer);
+        sprintf_P(cmd_buffer, fmt1, 50, (35 - (2 * i + 1)*short_length), long_extrusion);
+        enquecommand(cmd_buffer);
+        sprintf_P(cmd_buffer, fmt1, 50, (35 - (i + 1)*short_length * 2), short_extrusion);
+        enquecommand(cmd_buffer);
+    }
 }
 
 void lay1cal_finish(bool mmu_enabled)
diff --git a/Firmware/first_lay_cal.h b/Firmware/first_lay_cal.h
index 3864e19e3..4580163b7 100644
--- a/Firmware/first_lay_cal.h
+++ b/Firmware/first_lay_cal.h
@@ -12,7 +12,7 @@ void lay1cal_intro_line(bool skipExtraPurge, float layer_height, float extrusion
 void lay1cal_before_meander();
 void lay1cal_meander_start(float layer_height, float extrusion_width);
 void lay1cal_meander(float layer_height, float extrusion_width);
-void lay1cal_square(char *cmd_buffer, uint8_t i, float layer_height, float extrusion_width);
+void lay1cal_square(uint8_t step, float layer_height, float extrusion_width);
 void lay1cal_finish(bool mmu_enabled);
 
 #endif /* FIRMWARE_FIRST_LAY_CAL_H_ */
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index e2c8073f8..9f8ed8f45 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -832,70 +832,49 @@ void lcd_commands()
 
         if (!blocks_queued() && cmd_buffer_empty() && !saved_printing)
         {
+            if (lcd_commands_step == 0)
+                lcd_commands_step = 12;
+            else
+                lcd_commands_step--;
+
             switch(lcd_commands_step)
             {
-            case 0:
-                lcd_commands_step = 12;
-                break;
             case 12:
                 lay1cal_wait_preheat();
-                lcd_commands_step = 11;
                 break;
             case 11:
                 extraPurgeNeeded = lay1cal_load_filament(cmd1, lay1cal_filament);
-                lcd_commands_step = 10;
                 break;
             case 10:
                 lcd_clear();
                 menu_depth = 0;
                 menu_submenu(lcd_babystep_z);
                 lay1cal_intro_line(extraPurgeNeeded, layer_height, extrusion_width);
-                lcd_commands_step = 9;
                 break;
             case 9:
                 lay1cal_before_meander();
-                lcd_commands_step = 8;
                 break;
             case 8:
                 lay1cal_meander_start(layer_height, extrusion_width);
-                lcd_commands_step = 7;
                 break;
             case 7:
                 lay1cal_meander(layer_height, extrusion_width);
-                lcd_commands_step = 6;
                 break;
             case 6:
-                for (uint8_t i = 0; i < 4; i++)
-                {
-                    lay1cal_square(cmd1, i, layer_height, extrusion_width);
-                }
-                lcd_commands_step = 5;
+                lay1cal_square(0, layer_height, extrusion_width);
                 break;
             case 5:
-                for (uint8_t i = 4; i < 8; i++)
-                {
-                    lay1cal_square(cmd1, i, layer_height, extrusion_width);
-                }
-                lcd_commands_step = 4;
+                lay1cal_square(4, layer_height, extrusion_width);
                 break;
             case 4:
-                for (uint8_t i = 8; i < 12; i++)
-                {
-                    lay1cal_square(cmd1, i, layer_height, extrusion_width);
-                }
-                lcd_commands_step = 3;
+                lay1cal_square(8, layer_height, extrusion_width);
                 break;
             case 3:
-                for (uint8_t i = 12; i < 16; i++)
-                {
-                    lay1cal_square(cmd1, i, layer_height, extrusion_width);
-                }
-                lcd_commands_step = 2;
+                lay1cal_square(12, layer_height, extrusion_width);
                 break;
             case 2:
                 lay1cal_finish(MMU2::mmu2.Enabled());
                 menu_leaving = 1; //if user dont confirm live adjust Z value by pressing the knob, we are saving last value by timeout to status screen
-                lcd_commands_step = 1;
                 break;
             case 1:
                 lcd_setstatuspgm(MSG_WELCOME);