diff --git a/litex/boards/targets/kcu105.py b/litex/boards/targets/kcu105.py
index 645962616..75e6805ab 100755
--- a/litex/boards/targets/kcu105.py
+++ b/litex/boards/targets/kcu105.py
@@ -93,6 +93,7 @@ class BaseSoC(SoCSDRAM):
                 sys_clk_freq = sys_clk_freq)
             self.add_csr("ddrphy")
             self.add_constant("USDDRPHY", None)
+            self.add_constant("USDDRPHY_DEBUG", None)
             sdram_module = EDY4016A(sys_clk_freq, "1:4")
             self.register_sdram(self.ddrphy,
                 geom_settings       = sdram_module.geom_settings,
diff --git a/litex/soc/software/bios/main.c b/litex/soc/software/bios/main.c
index 73cfd8e45..4c3824c78 100644
--- a/litex/soc/software/bios/main.c
+++ b/litex/soc/software/bios/main.c
@@ -388,6 +388,12 @@ static void help(void)
 	puts("sdinit         - SDCard initialization");
 	puts("sdtest <loops> - SDCard test");
 #endif
+#ifdef USDDRPHY_DEBUG
+	puts("");
+	puts("sdram_cal                       - run SDRAM calibration");
+	puts("sdram_mpr                       - read SDRAM MPR");
+	puts("sdram_mrwr reg value            - write SDRAM mode registers");
+#endif
 }
 
 static char *get_token(char **str)
@@ -479,7 +485,22 @@ static void do_command(char *c)
 	else if(strcmp(token, "sdinit") == 0) sdcard_init();
 	else if(strcmp(token, "sdtest") == 0) sdcard_test(atoi(get_token(&c)));
 #endif
-
+#ifdef USDDRPHY_DEBUG
+	else if(strcmp(token, "sdram_cal") == 0)
+		sdrcal();
+	else if(strcmp(token, "sdram_mpr") == 0)
+		sdrmpr();
+	else if(strcmp(token, "sdram_mrwr") == 0) {
+		unsigned int reg;
+		unsigned int value;
+		reg = atoi(get_token(&c));
+		value = atoi(get_token(&c));
+		sdrsw();
+		printf("Writing 0x%04x to SDRAM mode register %d\n", value, reg);
+		sdrmrwr(reg, value);
+		sdrhw();
+	}
+#endif
 	else if(strcmp(token, "") != 0)
 		printf("Command not found\n");
 }
diff --git a/litex/soc/software/bios/sdram.c b/litex/soc/software/bios/sdram.c
index 58e7b16dd..5c9ec6802 100644
--- a/litex/soc/software/bios/sdram.c
+++ b/litex/soc/software/bios/sdram.c
@@ -1017,4 +1017,85 @@ int sdrinit(void)
 	return 1;
 }
 
+#ifdef USDDRPHY_DEBUG
+
+#define MPR0_SEL (0 << 0)
+#define MPR1_SEL (1 << 0)
+#define MPR2_SEL (2 << 0)
+#define MPR3_SEL (3 << 0)
+
+#define MPR_ENABLE (1 << 2)
+
+#define MPR_READ_SERIAL    (0 << 11)
+#define MPR_READ_PARALLEL  (1 << 11)
+#define MPR_READ_STAGGERED (2 << 11)
+
+void sdrcal(void)
+{
+#ifdef CSR_DDRPHY_BASE
+#if CSR_DDRPHY_EN_VTC_ADDR
+	ddrphy_en_vtc_write(0);
+#endif
+	sdrlevel();
+#if CSR_DDRPHY_EN_VTC_ADDR
+	ddrphy_en_vtc_write(1);
+#endif
+#endif
+	sdrhw();
+}
+
+void sdrmrwr(char reg, int value) {
+	sdram_dfii_pi0_address_write(value);
+	sdram_dfii_pi0_baddress_write(reg);
+	command_p0(DFII_COMMAND_RAS|DFII_COMMAND_CAS|DFII_COMMAND_WE|DFII_COMMAND_CS);
+}
+
+static void sdrmpron(char mpr)
+{
+	sdrmrwr(3, MPR_READ_SERIAL | MPR_ENABLE | mpr);
+}
+
+static void sdrmproff(void)
+{
+	sdrmrwr(3, 0);
+}
+
+void sdrmpr(void)
+{
+	int module, phase;
+	printf("Read SDRAM MPR...\n");
+
+	/* rst phy */
+	for(module=0; module<NBMODULES; module++) {
+#ifdef CSR_DDRPHY_WLEVEL_EN_ADDR
+		write_delay_rst(module);
+#endif
+		read_delay_rst(module);
+		read_bitslip_rst(module);
+	}
+
+	/* software control */
+	sdrsw();
+
+	printf("Reads with MPR0 (0b01010101) enabled...\n");
+	sdrmpron(MPR0_SEL);
+	command_prd(DFII_COMMAND_CAS|DFII_COMMAND_CS|DFII_COMMAND_RDDATA);
+	cdelay(15);
+	for (module=0; module < NBMODULES; module++) {
+		printf("m%d: ", module);
+		for(phase=0; phase<DFII_NPHASES; phase++) {
+			printf("%d", MMPTR(sdram_dfii_pix_rddata_addr[phase]+4*(NBMODULES-module-1)) & 0x1);
+			printf("%d", MMPTR(sdram_dfii_pix_rddata_addr[phase]+4*(2*NBMODULES-module-1)) & 0x1);
+		}
+		printf("\n");
+	}
+	sdrmproff();
+
+	/* hardware control */
+	sdrhw();
+}
+
+#endif
+
+
 #endif
diff --git a/litex/soc/software/bios/sdram.h b/litex/soc/software/bios/sdram.h
index 93034a548..7844c1e87 100644
--- a/litex/soc/software/bios/sdram.h
+++ b/litex/soc/software/bios/sdram.h
@@ -27,4 +27,10 @@ int memtest_silent(void);
 int memtest(void);
 int sdrinit(void);
 
+#ifdef USDDRPHY_DEBUG
+void sdrcal(void);
+void sdrmrwr(char reg, int value);
+void sdrmpr(void);
+#endif
+
 #endif /* __SDRAM_H */