diff --git a/litex/soc/software/bios/cmds/cmd_litedram.c b/litex/soc/software/bios/cmds/cmd_litedram.c
index 019155e32..1d9d51997 100644
--- a/litex/soc/software/bios/cmds/cmd_litedram.c
+++ b/litex/soc/software/bios/cmds/cmd_litedram.c
@@ -40,6 +40,58 @@ static void sdram_cal_handler(int nb_params, char **params)
 define_command(sdram_cal, sdram_cal_handler, "Calibrate SDRAM", LITEDRAM_CMDS);
 #endif
 
+#ifdef CSR_DDRPHY_RDPHASE_ADDR
+/**
+ * Command "sdram_force_rdphase"
+ *
+ * Force read phase
+ *
+ */
+static void sdram_force_rdphase_handler(int nb_params, char **params)
+{
+	char *c;
+	int phase;
+	if (nb_params < 1) {
+		printf("sdram_force_rdphase <phase>");
+		return;
+	}
+	phase = strtoul(params[0], &c, 0);
+	if (*c != 0) {
+		printf("Incorrect phase");
+		return;
+	}
+	printf("Forcing read phase to %d\n", phase);
+	ddrphy_rdphase_write(phase);
+}
+define_command(sdram_force_rdphase, sdram_force_rdphase_handler, "Force read phase", LITEDRAM_CMDS);
+#endif
+
+#ifdef CSR_DDRPHY_WRPHASE_ADDR
+/**
+ * Command "sdram_force_wrphase"
+ *
+ * Force write phase
+ *
+ */
+static void sdram_force_wrphase_handler(int nb_params, char **params)
+{
+	char *c;
+	int phase;
+	if (nb_params < 1) {
+		printf("sdram_force_wrphase <phase>");
+		return;
+	}
+	phase = strtoul(params[0], &c, 0);
+	if (*c != 0) {
+		printf("Incorrect phase");
+		return;
+	}
+	printf("Forcing write phase to %d\n", phase);
+	ddrphy_wrphase_write(phase);
+}
+define_command(sdram_force_wrphase, sdram_force_wrphase_handler, "Force write phase", LITEDRAM_CMDS);
+#endif
+
 #ifdef CSR_DDRPHY_CDLY_RST_ADDR
 
 /**
@@ -113,7 +165,7 @@ static void sdram_rst_dat_delay_handler(int nb_params, char **params)
 	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);
+define_command(sdram_rst_dat_delay, sdram_rst_dat_delay_handler, "Force write leveling Dat delay", LITEDRAM_CMDS);
 #endif
 
 /**
diff --git a/litex/soc/software/liblitedram/sdram.c b/litex/soc/software/liblitedram/sdram.c
index 0d55db41f..0298717bf 100644
--- a/litex/soc/software/liblitedram/sdram.c
+++ b/litex/soc/software/liblitedram/sdram.c
@@ -50,6 +50,120 @@ int sdram_get_freq(void) {
 	return SDRAM_PHY_XDR*SDRAM_PHY_PHASES*CONFIG_CLOCK_FREQUENCY;
 }
 
+/*-----------------------------------------------------------------------*/
+/* DFII                                                                  */
+/*-----------------------------------------------------------------------*/
+
+#ifdef CSR_DDRPHY_BASE
+
+static unsigned char sdram_dfii_get_rdphase(void) {
+#ifdef CSR_DDRPHY_RDPHASE_ADDR
+	return ddrphy_rdphase_read();
+#else
+	return SDRAM_PHY_RDPHASE;
+#endif
+}
+
+static unsigned char sdram_dfii_get_wrphase(void) {
+#ifdef CSR_DDRPHY_WRPHASE_ADDR
+	return ddrphy_wrphase_read();
+#else
+	return SDRAM_PHY_WRPHASE;
+#endif
+}
+
+static void sdram_dfii_pix_address_write(unsigned char phase, unsigned int value) {
+	switch (phase) {
+#if (SDRAM_PHY_PHASES > 2)
+	case 3:
+		sdram_dfii_pi3_address_write(value);
+		break;
+	case 2:
+		sdram_dfii_pi2_address_write(value);
+		break;
+#endif
+#if (SDRAM_PHY_PHASES > 1)
+	case 1:
+		sdram_dfii_pi1_address_write(value);
+		break;
+#endif
+	default:
+		sdram_dfii_pi0_address_write(value);
+	}
+}
+
+static void sdram_dfii_pird_address_write(unsigned int value) {
+	unsigned char rdphase = sdram_dfii_get_rdphase();
+	sdram_dfii_pix_address_write(rdphase, value);
+}
+
+static void sdram_dfii_piwr_address_write(unsigned int value) {
+	unsigned char wrphase = sdram_dfii_get_wrphase();
+	sdram_dfii_pix_address_write(wrphase, value);
+}
+
+static void sdram_dfii_pix_baddress_write(unsigned char phase, unsigned int value) {
+	switch (phase) {
+#if (SDRAM_PHY_PHASES > 2)
+	case 3:
+		sdram_dfii_pi3_baddress_write(value);
+		break;
+	case 2:
+		sdram_dfii_pi2_baddress_write(value);
+		break;
+#endif
+#if (SDRAM_PHY_PHASES > 1)
+	case 1:
+		sdram_dfii_pi1_baddress_write(value);
+		break;
+#endif
+	default:
+		sdram_dfii_pi0_baddress_write(value);
+	}
+}
+
+static void sdram_dfii_pird_baddress_write(unsigned int value) {
+	unsigned char rdphase = sdram_dfii_get_rdphase();
+	sdram_dfii_pix_baddress_write(rdphase, value);
+}
+
+static void sdram_dfii_piwr_baddress_write(unsigned int value) {
+	unsigned char wrphase = sdram_dfii_get_wrphase();
+	sdram_dfii_pix_baddress_write(wrphase, value);
+}
+
+static void command_px(unsigned char phase, unsigned int value) {
+	switch (phase) {
+#if (SDRAM_PHY_PHASES > 2)
+	case 3:
+		command_p3(value);
+		break;
+	case 2:
+		command_p2(value);
+		break;
+#endif
+#if (SDRAM_PHY_PHASES > 1)
+	case 1:
+		command_p1(value);
+		break;
+#endif
+	default:
+		command_p0(value);
+	}
+}
+
+static void command_prd(unsigned int value) {
+	unsigned char rdphase = sdram_dfii_get_rdphase();
+	command_px(rdphase, value);
+}
+
+static void command_pwr(unsigned int value) {
+	unsigned char wrphase = sdram_dfii_get_wrphase();
+	command_px(wrphase, value);
+}
+
+#endif
+
 /*-----------------------------------------------------------------------*/
 /* Software/Hardware Control                                             */
 /*-----------------------------------------------------------------------*/
@@ -778,11 +892,20 @@ int sdram_leveling(void)
 
 int sdram_init(void)
 {
+	/* Reset Cmd/Dat delays */
 #ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
 	int i;
 	sdram_write_leveling_rst_cmd_delay(0);
 	for (i=0; i<16; i++) sdram_write_leveling_rst_dat_delay(i, 0);
 #endif
+	/* Reset Read/Write phases */
+#ifdef CSR_DDRPHY_RDPHASE_ADDR
+	ddrphy_rdphase_write(SDRAM_PHY_RDPHASE);
+#endif
+#ifdef CSR_DDRPHY_WRPHASE_ADDR
+	ddrphy_wrphase_write(SDRAM_PHY_WRPHASE);
+#endif
+	/* Set Cmd delay if enforced at build time */
 #ifdef SDRAM_PHY_CMD_DELAY
 	_sdram_write_leveling_cmd_scan  = 0;
 	_sdram_write_leveling_cmd_delay = SDRAM_PHY_CMD_DELAY;