diff --git a/litex/soc/software/bios/main.c b/litex/soc/software/bios/main.c
index 0d2e6d863..423d4f28d 100644
--- a/litex/soc/software/bios/main.c
+++ b/litex/soc/software/bios/main.c
@@ -394,6 +394,7 @@ static void help(void)
 	puts("sdram_cal                       - run SDRAM calibration");
 	puts("sdram_mpr                       - read SDRAM MPR");
 	puts("sdram_mrwr reg value            - write SDRAM mode registers");
+	puts("sdram_cdly_scan enabled         - enable/disable cdly scan");
 #endif
 #ifdef CSR_SPISDCARD_BASE
         puts("spisdcardboot   - boot from SDCard via SPI hardware bitbang");
@@ -506,6 +507,11 @@ static void do_command(char *c)
 		sdrmrwr(reg, value);
 		sdrhw();
 	}
+	else if(strcmp(token, "sdram_cdly_scan") == 0) {
+		unsigned int enabled;
+		enabled = atoi(get_token(&c));
+		sdr_cdly_scan(enabled);
+	}
 #endif
 #ifdef CSR_SPISDCARD_BASE
         else if(strcmp(token, "spisdcardboot") == 0) spisdcardboot();
diff --git a/litex/soc/software/bios/sdram.c b/litex/soc/software/bios/sdram.c
index 53333cd84..ff094bd0e 100644
--- a/litex/soc/software/bios/sdram.c
+++ b/litex/soc/software/bios/sdram.c
@@ -310,7 +310,7 @@ static void write_delay_inc(int module) {
 	ddrphy_dly_sel_write(0);
 }
 
-int write_level(void)
+static int write_level_scan(int *delays, int show)
 {
 	int i, j, k;
 
@@ -322,20 +322,17 @@ int write_level(void)
 	int one_window_start, one_window_best_start;
 	int one_window_count, one_window_best_count;
 
-	int delays[SDRAM_PHY_MODULES];
-
 	unsigned char buf[DFII_PIX_DATA_BYTES];
 
 	int ok;
 
 	err_ddrphy_wdly = SDRAM_PHY_DELAYS - ddrphy_half_sys8x_taps_read();
 
-	printf("Write leveling:\n");
-
 	sdrwlon();
 	cdelay(100);
 	for(i=0;i<SDRAM_PHY_MODULES;i++) {
-		printf("m%d: |", i);
+		if (show)
+			printf("m%d: |", i);
 
 		/* rst delay */
 		write_delay_rst(i);
@@ -344,9 +341,9 @@ int write_level(void)
 		for(j=0;j<err_ddrphy_wdly;j++) {
 			int zero_count = 0;
 			int one_count = 0;
-			int show = 1;
+			int show_iter = show;
 #if SDRAM_PHY_DELAYS > 32
-			show = (j%16 == 0);
+			show_iter = (j%16 == 0) && show;
 #endif
 			for (k=0; k<128; k++) {
 				ddrphy_wlevel_strobe_write(1);
@@ -362,19 +359,20 @@ int write_level(void)
 				taps_scan[j] = 1;
 			else
 				taps_scan[j] = 0;
-			if (show)
+			if (show_iter)
 				printf("%d", taps_scan[j]);
 			write_delay_inc(i);
 			cdelay(10);
 		}
-		printf("|");
+		if (show)
+			printf("|");
 
 		/* find longer 1 window and set delay at the 0/1 transition */
 		one_window_active = 0;
 		one_window_start = 0;
 		one_window_count = 0;
 		one_window_best_start = 0;
-		one_window_best_count = 0;
+		one_window_best_count = -1;
 		delays[i] = -1;
 		for(j=0;j<err_ddrphy_wdly;j++) {
 			if (one_window_active) {
@@ -393,13 +391,17 @@ int write_level(void)
 				}
 			}
 		}
-		delays[i] = one_window_best_start;
+		/* succeed only if the start of a 1s window has been found */
+		if (one_window_best_count > 0 && one_window_best_start > 0) {
+			delays[i] = one_window_best_start;
 
-		/* configure write delay */
-		write_delay_rst(i);
-		for(j=0; j<delays[i]; j++)
-			write_delay_inc(i);
-		printf(" delay: %02d\n", delays[i]);
+			/* configure write delay */
+			write_delay_rst(i);
+			for(j=0; j<delays[i]; j++)
+				write_delay_inc(i);
+		}
+		if (show)
+			printf(" delay: %02d\n", delays[i]);
 	}
 
 	sdrwloff();
@@ -413,6 +415,106 @@ int write_level(void)
 	return ok;
 }
 
+static void write_level_cdly_range(unsigned int *best_error, int *best_cdly,
+		int cdly_start, int cdly_stop, int cdly_step)
+{
+	int cdly;
+	int cdly_actual = 0;
+	int delays[SDRAM_PHY_MODULES];
+
+	/* scan through the range */
+	ddrphy_cdly_rst_write(1);
+	for (cdly = cdly_start; cdly < cdly_stop; cdly += cdly_step) {
+		/* increment cdly to current value */
+		while (cdly_actual < cdly) {
+			ddrphy_cdly_inc_write(1);
+			cdelay(10);
+			cdly_actual++;
+		}
+
+		/* write level using this delay */
+		if (write_level_scan(delays, 0)) {
+			/* use the mean of delays for error calulation */
+			int delay_mean = 0;
+			for (int i=0; i < SDRAM_PHY_MODULES; ++i) {
+				delay_mean += delays[i];
+			}
+			delay_mean /= SDRAM_PHY_MODULES;
+
+			/* we want it to be in the middle */
+			int ideal_delay = (SDRAM_PHY_DELAYS - ddrphy_half_sys8x_taps_read()) / 2;
+			int error = ideal_delay - delay_mean;
+			if (error < 0)
+				error *= -1;
+
+			if (error < *best_error) {
+				printf("+");
+				*best_cdly = cdly;
+				*best_error = error;
+			} else {
+				printf("-");
+			}
+		} else {
+			printf(".");
+		}
+	}
+}
+
+int write_level(void)
+{
+	int delays[SDRAM_PHY_MODULES];
+	unsigned int best_error = ~0u;
+	int best_cdly = -1;
+	int cdly_range_start;
+	int cdly_range_end;
+	int cdly_range_step;
+
+	printf("cdly scan: ");
+
+	/* Center write leveling by varying cdly. Searching through all possible
+	 * values is slow, but we can use a simple optimization method of iterativly
+	 * scanning smaller ranges with decreasing step */
+	cdly_range_start = 0;
+	cdly_range_end = 512;
+	cdly_range_step = 64;
+	while (cdly_range_step > 0) {
+		printf("|");
+		write_level_cdly_range(&best_error, &best_cdly,
+				cdly_range_start, cdly_range_end, cdly_range_step);
+
+		/* small optimization - stop if we have zero error */
+		if (best_error == 0)
+			break;
+
+		/* use best result as the middle of next range */
+		cdly_range_start = best_cdly - cdly_range_step;
+		cdly_range_end = best_cdly + cdly_range_step + 1;
+		if (cdly_range_start < 0)
+			cdly_range_start = 0;
+		if (cdly_range_end > 512)
+			cdly_range_end = 512;
+
+		cdly_range_step /= 4;
+	}
+	printf("| best: %d\n", best_cdly);
+
+	/* if we found any working delay then set it */
+	if (best_cdly >= 0) {
+		ddrphy_cdly_rst_write(1);
+		for (int i = 0; i < best_cdly; ++i) {
+			ddrphy_cdly_inc_write(1);
+			cdelay(10);
+		}
+	}
+
+	/* re-run write leveling the final time */
+	if (!write_level_scan(delays, 1))
+		return 0;
+
+	return best_cdly >= 0;
+}
+
+
 #endif /*  SDRAM_PHY_WRITE_LEVELING_CAPABLE */
 
 static void read_delay_rst(int module) {
@@ -905,7 +1007,8 @@ int memtest(void)
 #ifdef CSR_SDRAM_BASE
 
 #if defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
-int sdrlevel(void)
+
+static void read_leveling(void)
 {
 	int module;
 	int bitslip;
@@ -913,23 +1016,6 @@ int sdrlevel(void)
 	int best_score;
 	int best_bitslip;
 
-	sdrsw();
-
-	for(module=0; module<SDRAM_PHY_MODULES; module++) {
-#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
-		write_delay_rst(module);
-#endif
-		read_delay_rst(module);
-		read_bitslip_rst(module);
-	}
-
-#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
-	if(!write_level())
-		return 0;
-#endif
-
-#ifdef SDRAM_PHY_READ_LEVELING_CAPABLE
-	printf("Read leveling:\n");
 	for(module=0; module<SDRAM_PHY_MODULES; module++) {
 		/* scan possible read windows */
 		best_score = 0;
@@ -960,8 +1046,40 @@ int sdrlevel(void)
 		read_level(module);
 		printf("\n");
 	}
+}
+
+int _write_level_cdly_scan = 1;
+
+int sdrlevel(void)
+{
+	int module;
+	sdrsw();
+
+	for(module=0; module<SDRAM_PHY_MODULES; module++) {
+#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
+		write_delay_rst(module);
+#endif
+		read_delay_rst(module);
+		read_bitslip_rst(module);
+	}
+
+#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
+	printf("Write leveling:\n");
+	if (_write_level_cdly_scan) {
+		if(!write_level())
+			return 0;
+	} else {
+		/* use only the current cdly */
+		int delays[SDRAM_PHY_MODULES];
+		if (!write_level_scan(delays, 1))
+			return 0;
+	}
 #endif
 
+#ifdef SDRAM_PHY_READ_LEVELING_CAPABLE
+	printf("Read leveling:\n");
+	read_leveling();
+#endif
 
 	return 1;
 }
@@ -978,12 +1096,12 @@ int sdrinit(void)
 
 	init_sequence();
 #ifdef CSR_DDRPHY_BASE
-#if CSR_DDRPHY_EN_VTC_ADDR
-	ddrphy_en_vtc_write(0);
-#endif
 #ifdef DDRPHY_CMD_DELAY
 	ddrphy_cdly(DDRPHY_CMD_DELAY);
 #endif
+#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)
 	sdrlevel();
 #endif
@@ -1087,6 +1205,12 @@ void sdrmpr(void)
 	sdrhw();
 }
 
+void sdr_cdly_scan(int enabled)
+{
+	printf("Turning cdly scan %s\n", enabled ? "ON" : "OFF");
+	_write_level_cdly_scan = enabled;
+}
+
 #endif
 
 #endif
diff --git a/litex/soc/software/bios/sdram.h b/litex/soc/software/bios/sdram.h
index 15629eba8..a25711919 100644
--- a/litex/soc/software/bios/sdram.h
+++ b/litex/soc/software/bios/sdram.h
@@ -29,6 +29,7 @@ void ddrphy_cdly(unsigned int delay);
 void sdrcal(void);
 void sdrmrwr(char reg, int value);
 void sdrmpr(void);
+void sdr_cdly_scan(int enabled);
 #endif
 
 #endif /* __SDRAM_H */