diff --git a/litex/soc/software/bios/cmds/cmd_litedram.c b/litex/soc/software/bios/cmds/cmd_litedram.c
index f1d8e2784..019155e32 100644
--- a/litex/soc/software/bios/cmds/cmd_litedram.c
+++ b/litex/soc/software/bios/cmds/cmd_litedram.c
@@ -25,13 +25,19 @@ define_command(sdram_init, sdram_init, "Initialize SDRAM (Init + Calibration)",
 #endif
 
 /**
- * Command "sdram_calibration"
+ * Command "sdram_cal"
  *
  * Calibrate SDRAM
  *
  */
 #if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
-define_command(sdram_cal, sdram_calibration, "Calibrate SDRAM", LITEDRAM_CMDS);
+static void sdram_cal_handler(int nb_params, char **params)
+{
+	sdram_software_control_on();
+	sdram_leveling();
+	sdram_software_control_off();
+}
+define_command(sdram_cal, sdram_cal_handler, "Calibrate SDRAM", LITEDRAM_CMDS);
 #endif
 
 #ifdef CSR_DDRPHY_CDLY_RST_ADDR
@@ -45,7 +51,9 @@ define_command(sdram_cal, sdram_calibration, "Calibrate SDRAM", LITEDRAM_CMDS);
 #if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
 static void sdram_rst_cmd_delay_handler(int nb_params, char **params)
 {
+	sdram_software_control_on();
 	sdram_write_leveling_rst_cmd_delay(1);
+	sdram_software_control_off();
 }
 define_command(sdram_rst_cmd_delay, sdram_rst_cmd_delay_handler, "Reset write leveling Cmd delay", LITEDRAM_CMDS);
 #endif
@@ -70,7 +78,9 @@ static void sdram_force_cmd_delay_handler(int nb_params, char **params)
 		printf("Incorrect taps");
 		return;
 	}
+	sdram_software_control_on();
 	sdram_write_leveling_force_cmd_delay(taps, 1);
+	sdram_software_control_off();
 }
 define_command(sdram_force_cmd_delay, sdram_force_cmd_delay_handler, "Force write leveling Cmd delay", LITEDRAM_CMDS);
 #endif
@@ -99,7 +109,9 @@ static void sdram_rst_dat_delay_handler(int nb_params, char **params)
 		printf("Incorrect module");
 		return;
 	}
+	sdram_software_control_on();
 	sdram_write_leveling_rst_dat_delay(module, 1);
+	sdram_software_control_off();
 }
 define_command(sdram_rst_dat_delay, sdram_rst_dat_delay_handler, "Reset write leveling Dat delay", LITEDRAM_CMDS);
 #endif
@@ -130,7 +142,9 @@ static void sdram_force_dat_delay_handler(int nb_params, char **params)
 		printf("Incorrect taps");
 		return;
 	}
+	sdram_software_control_on();
 	sdram_write_leveling_force_dat_delay(module, taps, 1);
+	sdram_software_control_off();
 }
 define_command(sdram_force_dat_delay, sdram_force_dat_delay_handler, "Reset write leveling Dat delay", LITEDRAM_CMDS);
 #endif
diff --git a/litex/soc/software/liblitedram/sdram.c b/litex/soc/software/liblitedram/sdram.c
index 21b1a0bbe..ca5f7a43b 100644
--- a/litex/soc/software/liblitedram/sdram.c
+++ b/litex/soc/software/liblitedram/sdram.c
@@ -61,20 +61,31 @@ void sdram_software_control_on(void)
 {
 	unsigned int previous;
 	previous = sdram_dfii_control_read();
+	/* Switch DFII to software control */
 	if (previous != DFII_CONTROL_SOFTWARE) {
 		sdram_dfii_control_write(DFII_CONTROL_SOFTWARE);
 		printf("Switching SDRAM to software control.\n");
 	}
+
+#if CSR_DDRPHY_EN_VTC_ADDR
+	/* Disable Voltage/Temperature compensation */
+	ddrphy_en_vtc_write(0);
+#endif
 }
 
 void sdram_software_control_off(void)
 {
 	unsigned int previous;
 	previous = sdram_dfii_control_read();
+	/* Switch DFII to hardware control */
 	if (previous != DFII_CONTROL_HARDWARE) {
 		sdram_dfii_control_write(DFII_CONTROL_HARDWARE);
 		printf("Switching SDRAM to hardware control.\n");
 	}
+#if CSR_DDRPHY_EN_VTC_ADDR
+	/* Enable Voltage/Temperature compensation */
+	ddrphy_en_vtc_write(1);
+#endif
 }
 
 /*-----------------------------------------------------------------------*/
@@ -136,9 +147,6 @@ void sdram_write_leveling_rst_cmd_delay(int show) {
 
 void sdram_write_leveling_force_cmd_delay(int taps, int show) {
 	_sdram_write_leveling_cmd_scan = 0;
-#if CSR_DDRPHY_EN_VTC_ADDR
-	ddrphy_en_vtc_write(0);
-#endif
 	if (show)
 		printf("Forcing Cmd delay to %d taps\n", taps);
 	ddrphy_cdly_rst_write(1);
@@ -147,9 +155,6 @@ void sdram_write_leveling_force_cmd_delay(int taps, int show) {
 		cdelay(1000);
 		taps--;
 	}
-#if CSR_DDRPHY_EN_VTC_ADDR
-	ddrphy_en_vtc_write(1);
-#endif
 }
 
 void sdram_write_leveling_rst_dat_delay(int module, int show) {
@@ -770,27 +775,6 @@ int sdram_leveling(void)
 }
 #endif
 
-/*-----------------------------------------------------------------------*/
-/* Calibration                                                           */
-/*-----------------------------------------------------------------------*/
-
-void sdram_calibration(void)
-{
-	sdram_software_control_on();
-#ifdef CSR_DDRPHY_BASE
-#if CSR_DDRPHY_EN_VTC_ADDR
-	ddrphy_en_vtc_write(0);
-#endif
-#if defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
-	sdram_leveling();
-#endif
-#if CSR_DDRPHY_EN_VTC_ADDR
-	ddrphy_en_vtc_write(1);
-#endif
-#endif
-	sdram_software_control_off();
-}
-
 /*-----------------------------------------------------------------------*/
 /* Initialization                                                        */
 /*-----------------------------------------------------------------------*/
@@ -816,7 +800,7 @@ int sdram_init(void)
 	ddrctrl_init_error_write(0);
 #endif
 	init_sequence();
-	sdram_calibration();
+	sdram_leveling();
 	sdram_software_control_off();
 	if(!memtest((unsigned int *) MAIN_RAM_BASE, MAIN_RAM_SIZE)) {
 #ifdef CSR_DDRCTRL_BASE
diff --git a/litex/soc/software/liblitedram/sdram.h b/litex/soc/software/liblitedram/sdram.h
index 9f0087fcc..886699bc5 100644
--- a/litex/soc/software/liblitedram/sdram.h
+++ b/litex/soc/software/liblitedram/sdram.h
@@ -39,11 +39,6 @@ void sdram_read_leveling(void);
 /*-----------------------------------------------------------------------*/
 int sdram_leveling(void);
 
-/*-----------------------------------------------------------------------*/
-/* Calibration                                                           */
-/*-----------------------------------------------------------------------*/
-void sdram_calibration(void);
-
 /*-----------------------------------------------------------------------*/
 /* Initialization                                                        */
 /*-----------------------------------------------------------------------*/