diff --git a/litex/soc/software/liblitedram/sdram.c b/litex/soc/software/liblitedram/sdram.c
index 3a2b49f6b..79a85ab7e 100644
--- a/litex/soc/software/liblitedram/sdram.c
+++ b/litex/soc/software/liblitedram/sdram.c
@@ -802,9 +802,13 @@ static int sdram_read_leveling_scan_module(int module, int bitslip, int show)
 	return score;
 }
 
-static void sdram_read_leveling_module(int module, int show, int show_long)
+typedef void (*delay_callback)(int module);
+
+static void sdram_leveling_center_module(
+	int module, int show_short, int show_long, delay_callback rst_delay, delay_callback inc_delay)
 {
 	int i;
+	int show;
 	int working;
 	int delay, delay_mid, delay_range;
 	int delay_min = -1, delay_min_next = -1, delay_max = -1;
@@ -814,11 +818,15 @@ static void sdram_read_leveling_module(int module, int show, int show_long)
 
 	/* Find smallest working delay */
 	delay = 0;
-	sdram_read_leveling_rst_delay(module);
+	rst_delay(module);
 	while(1) {
 		working  = sdram_write_read_check_test_pattern(module, 42);
 		working &= sdram_write_read_check_test_pattern(module, 84);
-		if (show_long)
+		show = show_long;
+#if SDRAM_PHY_DELAYS > 32
+		show = show && (delay%16 == 0);
+#endif
+		if (show)
 			printf(working ? "1" : "0");
 		if(working && delay_min < 0) {
 			delay_min = delay;
@@ -827,25 +835,29 @@ static void sdram_read_leveling_module(int module, int show, int show_long)
 		delay++;
 		if(delay >= SDRAM_PHY_DELAYS)
 			break;
-		sdram_read_leveling_inc_delay(module);
+		inc_delay(module);
 	}
 
 	/* Get a bit further into the working zone */
 #if SDRAM_PHY_DELAYS > 32
 	for(i=0;i<16;i++) {
 		delay += 1;
-		sdram_read_leveling_inc_delay(module);
+		inc_delay(module);
 	}
 #else
 	delay++;
-	sdram_read_leveling_inc_delay(module);
+	inc_delay(module);
 #endif
 
 	/* Find largest working delay */
 	while(1) {
 		working  = sdram_write_read_check_test_pattern(module, 42);
 		working &= sdram_write_read_check_test_pattern(module, 84);
-		if (show_long)
+		show = show_long;
+#if SDRAM_PHY_DELAYS > 32
+		show = show && (delay%16 == 0);
+#endif
+		if (show)
 			printf(working ? "1" : "0");
 		if(!working && delay_max < 0) {
 			delay_max = delay;
@@ -859,7 +871,7 @@ static void sdram_read_leveling_module(int module, int show, int show_long)
 		delay++;
 		if(delay >= SDRAM_PHY_DELAYS)
 			break;
-		sdram_read_leveling_inc_delay(module);
+		inc_delay(module);
 	}
 	if(delay_max < 0) {
 		delay_max = delay;
@@ -876,7 +888,7 @@ static void sdram_read_leveling_module(int module, int show, int show_long)
 
 	delay_mid = (delay_min+delay_max)/2 % SDRAM_PHY_DELAYS;
 	delay_range = (delay_max-delay_min)/2;
-	if (show) {
+	if (show_short) {
 		if (delay_min < 0)
 			printf("delays: -");
 		else
@@ -887,13 +899,14 @@ static void sdram_read_leveling_module(int module, int show, int show_long)
 		printf("\n");
 
 	/* Set delay to the middle */
-	sdram_read_leveling_rst_delay(module);
+	rst_delay(module);
     cdelay(100);
 	for(i = 0; i < delay_mid; i++) {
-		sdram_read_leveling_inc_delay(module);
+		inc_delay(module);
 		cdelay(100);
 	}
 }
+
 #endif /* CSR_DDRPHY_BASE */
 
 #endif /* CSR_SDRAM_BASE */
@@ -918,7 +931,8 @@ void sdram_read_leveling(void)
 		for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip++) {
 			/* Compute score */
 			score = sdram_read_leveling_scan_module(module, bitslip, 1);
-			sdram_read_leveling_module(module, 1, 0);
+			sdram_leveling_center_module(module, 1, 0,
+				sdram_read_leveling_rst_delay, sdram_read_leveling_inc_delay);
 			printf("\n");
 			if (score > best_score) {
 				best_bitslip = bitslip;
@@ -938,7 +952,8 @@ void sdram_read_leveling(void)
 			sdram_read_leveling_inc_bitslip(module);
 
 		/* Re-do leveling on best read window*/
-		sdram_read_leveling_module(module, 1, 0);
+		sdram_leveling_center_module(module, 1, 0,
+			sdram_read_leveling_rst_delay, sdram_read_leveling_inc_delay);
 		printf("\n");
 	}
 }
@@ -1017,100 +1032,21 @@ static void sdram_write_latency_calibration(void) {
 
 #ifdef SDRAM_PHY_WRITE_DQ_DQS_TRAINING_CAPABLE
 
-static void sdram_write_dq_dqs_training_module(int module, int show, int show_long)
-{
-	int i;
-	int working;
-	int delay, delay_mid, delay_range;
-	int delay_min = -1, delay_min_next = -1, delay_max = -1;
-
-	if (show_long)
-		printf("m%d: |", module);
-
+static void sdram_write_dq_dqs_training_rst_delay(int module) {
+	/* Select module */
 	ddrphy_dly_sel_write(1 << module);
+	/* Reset delay */
+	ddrphy_wdly_dq_rst_write(1);
+	/* Un-select module */
+	ddrphy_dly_sel_write(0);
+}
 
-	/* Find smallest working delay */
-	delay = 0;
-	ddrphy_wdly_dq_rst_write(module);
-	while(1) {
-		working  = sdram_write_read_check_test_pattern(module, 42);
-		working &= sdram_write_read_check_test_pattern(module, 84);
-		if (show_long)
-			printf(working ? "1" : "0");
-		if(working && delay_min < 0) {
-			delay_min = delay;
-			break;
-		}
-		delay++;
-		if(delay >= SDRAM_PHY_DELAYS)
-			break;
-		ddrphy_wdly_dq_inc_write(module);
-	}
-
-	/* Get a bit further into the working zone */
-#if SDRAM_PHY_DELAYS > 32
-	for(i=0;i<16;i++) {
-		delay += 1;
-		ddrphy_wdly_dq_inc_write(module);
-	}
-#else
-	delay++;
-	ddrphy_wdly_dq_inc_write(module);
-#endif
-
-	/* Find largest working delay */
-	while(1) {
-		working  = sdram_write_read_check_test_pattern(module, 42);
-		working &= sdram_write_read_check_test_pattern(module, 84);
-		if (show_long)
-			printf(working ? "1" : "0");
-		if(!working && delay_max < 0) {
-			delay_max = delay;
-		}
-		/* Store next working delay to include wrapping around */
-		if (!working) {
-			delay_min_next = -1;
-		} else if(working && delay_min_next < 0) {
-			delay_min_next = delay;
-		}
-		delay++;
-		if(delay >= SDRAM_PHY_DELAYS)
-			break;
-		ddrphy_wdly_dq_inc_write(module);
-	}
-	if(delay_max < 0) {
-		delay_max = delay;
-	}
-
-	/* Extend the range if it wraps around */
-	if (delay_min_next > 0) {
-		delay_min = delay_min_next;
-		delay_max += SDRAM_PHY_DELAYS;
-	}
-
-	if (show_long)
-		printf("| ");
-
-	delay_mid = (delay_min+delay_max)/2 % SDRAM_PHY_DELAYS;
-	delay_range = (delay_max-delay_min)/2;
-	if (show) {
-		if (delay_min < 0)
-			printf("delays: -");
-		else
-			printf("delays: %02d+-%02d", delay_mid, delay_range);
-	}
-
-	if (show_long)
-		printf("\n");
-
-	/* Set delay to the middle */
-	ddrphy_wdly_dq_rst_write(module);
-	cdelay(100);
-	for(i = 0; i < delay_mid; i++) {
-		ddrphy_wdly_dq_inc_write(module);
-		cdelay(100);
-	}
-
+static void sdram_write_dq_dqs_training_inc_delay(int module) {
+	/* Select module */
+	ddrphy_dly_sel_write(1 << module);
+	/* Increment delay */
+	ddrphy_wdly_dq_inc_write(1);
+	/* Un-select module */
 	ddrphy_dly_sel_write(0);
 }
 
@@ -1124,7 +1060,8 @@ static void sdram_read_leveling_best_bitslip(int module)
 	sdram_read_leveling_rst_bitslip(module);
 	for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip++) {
 		score = sdram_read_leveling_scan_module(module, bitslip, 0);
-		sdram_read_leveling_module(module, 0, 0);
+		sdram_leveling_center_module(module, 0, 0,
+			sdram_read_leveling_rst_delay, sdram_read_leveling_inc_delay);
 		if (score > best_score) {
 			best_bitslip = bitslip;
 			best_score = score;
@@ -1154,7 +1091,8 @@ static void sdram_write_dq_dqs_training(void)
 			/* Find best bitslip */
 			sdram_read_leveling_best_bitslip(module);
 			/* Center DQ-DQS window */
-			sdram_write_dq_dqs_training_module(module, show, show);
+			sdram_leveling_center_module(module, show, show,
+				sdram_write_dq_dqs_training_rst_delay, sdram_write_dq_dqs_training_inc_delay);
 		}
 	}
 }