diff --git a/litex/soc/software/bios/cmds/cmd_litedram.c b/litex/soc/software/bios/cmds/cmd_litedram.c
index 4420bacea..f0c9e46f5 100644
--- a/litex/soc/software/bios/cmds/cmd_litedram.c
+++ b/litex/soc/software/bios/cmds/cmd_litedram.c
@@ -14,50 +14,11 @@
 #include <liblitedram/sdram.h>
 #include <liblitedram/sdram_spd.h>
 #include <liblitedram/bist.h>
+#include <liblitedram/accessors.h>
 
 #include "../command.h"
 #include "../helpers.h"
 
-/**
- * Command "sdram_init"
- *
- * Initialize SDRAM (Init + Calibration)
- *
- */
-#if defined(CSR_SDRAM_BASE)
-define_command(sdram_init, sdram_init, "Initialize SDRAM (Init + Calibration)", LITEDRAM_CMDS);
-#endif
-
-/**
- * Command "sdram_cal"
- *
- * Calibrate SDRAM
- *
- */
-#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
-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
-
-/**
- * Command "sdram_test"
- *
- * Test SDRAM
- *
- */
-#if defined(CSR_SDRAM_BASE)
-static void sdram_test_handler(int nb_params, char **params)
-{
-	memtest((unsigned int *)MAIN_RAM_BASE, MAIN_RAM_SIZE/32);
-}
-define_command(sdram_test, sdram_test_handler, "Test SDRAM", LITEDRAM_CMDS);
-#endif
-
 /**
  * Command "sdram_bist"
  *
@@ -228,7 +189,44 @@ define_command(sdram_force_cmd_delay, sdram_force_cmd_delay_handler, "Force writ
 
 #endif
 
-#ifdef CSR_DDRPHY_WDLY_DQ_RST_ADDR
+#if defined(CSR_SDRAM_BASE)
+/**
+ * Command "sdram_init"
+ *
+ * Initialize SDRAM (Init + Calibration)
+ *
+ */
+define_command(sdram_init, sdram_init, "Initialize SDRAM (Init + Calibration)", LITEDRAM_CMDS);
+
+/**
+ * Command "sdram_test"
+ *
+ * Test SDRAM
+ *
+ */
+static void sdram_test_handler(int nb_params, char **params)
+{
+	memtest((unsigned int *)MAIN_RAM_BASE, MAIN_RAM_SIZE/32);
+}
+define_command(sdram_test, sdram_test_handler, "Test SDRAM", LITEDRAM_CMDS);
+
+/**
+ * Command "sdram_cal"
+ *
+ * Calibrate SDRAM
+ *
+ */
+#if defined(CSR_DDRPHY_BASE)
+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 SDRAM_PHY_WRITE_LEVELING_CAPABLE
 
 /**
  * Command "sdram_rst_dat_delay"
@@ -236,7 +234,7 @@ define_command(sdram_force_cmd_delay, sdram_force_cmd_delay_handler, "Force writ
  * Reset write leveling Dat delay
  *
  */
-#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+#if defined(CSR_DDRPHY_BASE)
 static void sdram_rst_dat_delay_handler(int nb_params, char **params)
 {
 	char *c;
@@ -263,7 +261,7 @@ define_command(sdram_rst_dat_delay, sdram_rst_dat_delay_handler, "Reset write le
  * Force write leveling Dat delay
  *
  */
-#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+#if defined(CSR_DDRPHY_BASE)
 static void sdram_force_dat_delay_handler(int nb_params, char **params)
 {
 	char *c;
@@ -288,15 +286,18 @@ static void sdram_force_dat_delay_handler(int nb_params, char **params)
 	sdram_software_control_off();
 }
 define_command(sdram_force_dat_delay, sdram_force_dat_delay_handler, "Force write leveling Dat delay", LITEDRAM_CMDS);
-#endif
+#endif /* defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE) */
 
+#endif /* SDRAM_PHY_WRITE_LEVELING_CAPABLE */
+
+#if defined(SDRAM_PHY_BITSLIPS) && defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE)
 /**
  * Command "sdram_rst_bitslip"
  *
  * Reset write leveling Bitslip
  *
  */
-#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+#if defined(CSR_DDRPHY_BASE)
 static void sdram_rst_bitslip_handler(int nb_params, char **params)
 {
 	char *c;
@@ -323,7 +324,7 @@ define_command(sdram_rst_bitslip, sdram_rst_bitslip_handler, "Reset write leveli
  * Force write leveling Bitslip
  *
  */
-#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+#if defined(CSR_DDRPHY_BASE)
 static void sdram_force_bitslip_handler(int nb_params, char **params)
 {
 	char *c;
@@ -350,7 +351,7 @@ static void sdram_force_bitslip_handler(int nb_params, char **params)
 define_command(sdram_force_bitslip, sdram_force_bitslip_handler, "Force write leveling Bitslip", LITEDRAM_CMDS);
 #endif
 
-#endif
+#endif /* defined(SDRAM_PHY_BITSLIPS) && defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE) */
 
 /**
  * Command "sdram_mr_write"
@@ -358,7 +359,6 @@ define_command(sdram_force_bitslip, sdram_force_bitslip_handler, "Force write le
  * Write SDRAM Mode Register
  *
  */
-#if defined(CSR_SDRAM_BASE)
 static void sdram_mr_write_handler(int nb_params, char **params)
 {
 	char *c;
@@ -385,7 +385,8 @@ static void sdram_mr_write_handler(int nb_params, char **params)
 	sdram_software_control_off();
 }
 define_command(sdram_mr_write, sdram_mr_write_handler, "Write SDRAM Mode Register", LITEDRAM_CMDS);
-#endif
+
+#endif /* CSR_SDRAM_BASE */
 
 /**
  * Command "sdram_spd"
diff --git a/litex/soc/software/liblitedram/Makefile b/litex/soc/software/liblitedram/Makefile
index 51c3e8952..664685be3 100644
--- a/litex/soc/software/liblitedram/Makefile
+++ b/litex/soc/software/liblitedram/Makefile
@@ -1,7 +1,7 @@
 include ../include/generated/variables.mak
 include $(SOC_DIRECTORY)/software/common.mak
 
-OBJECTS = sdram.o bist.o sdram_dbg.o sdram_spd.o utils.o
+OBJECTS = sdram.o bist.o sdram_dbg.o sdram_spd.o utils.o accessors.o
 
 all: liblitedram.a
 
diff --git a/litex/soc/software/liblitedram/accessors.c b/litex/soc/software/liblitedram/accessors.c
new file mode 100644
index 000000000..47c3b733f
--- /dev/null
+++ b/litex/soc/software/liblitedram/accessors.c
@@ -0,0 +1,193 @@
+#include <stdio.h>
+
+#include <liblitedram/accessors.h>
+
+#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+
+#if defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
+
+int read_dq_delay[SDRAM_PHY_MODULES];
+void read_inc_dq_delay(int module) {
+	/* Increment delay */
+	read_dq_delay[module] = (read_dq_delay[module] + 1) & (SDRAM_PHY_DELAYS - 1);
+	ddrphy_rdly_dq_inc_write(1);
+}
+
+void read_rst_dq_delay(int module) {
+	/* Reset delay */
+	read_dq_delay[module] = 0;
+	ddrphy_rdly_dq_rst_write(1);
+}
+
+#endif // defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
+
+#if defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE)
+
+int sdram_clock_delay;
+void sdram_inc_clock_delay(void) {
+	sdram_clock_delay = (sdram_clock_delay + 1) & (SDRAM_PHY_DELAYS - 1);
+	ddrphy_cdly_inc_write(1);
+	cdelay(100);
+}
+
+void sdram_rst_clock_delay(void) {
+	sdram_clock_delay = 0;
+	ddrphy_cdly_rst_write(1);
+	cdelay(100);
+}
+
+int write_dq_delay[SDRAM_PHY_MODULES];
+void write_inc_dq_delay(int module) {
+	/* Increment DQ delay */
+	write_dq_delay[module] = (write_dq_delay[module] + 1) & (SDRAM_PHY_DELAYS - 1);
+	ddrphy_wdly_dq_inc_write(1);
+	cdelay(100);
+}
+
+void write_rst_dq_delay(int module) {
+#if defined(SDRAM_PHY_USDDRPHY) || defined(SDRAM_PHY_USPDDRPHY)
+	/* Reset DQ delay */
+	int dq_count = ddrphy_wdly_dqs_inc_count_read();
+	while (dq_count != SDRAM_PHY_DELAYS) {
+		ddrphy_wdly_dq_inc_write(1);
+		cdelay(100);
+		dq_count++;
+	}
+#else
+	/* Reset DQ delay */
+	ddrphy_wdly_dq_rst_write(1);
+	cdelay(100);
+#endif //defined(SDRAM_PHY_USDDRPHY) || defined(SDRAM_PHY_USPDDRPHY)
+	write_dq_delay[module] = 0;
+}
+
+void write_inc_dqs_delay(int module) {
+	/* Increment DQS delay */
+	ddrphy_wdly_dqs_inc_write(1);
+	cdelay(100);
+}
+
+void write_rst_dqs_delay(int module) {
+#if defined(SDRAM_PHY_USDDRPHY) || defined(SDRAM_PHY_USPDDRPHY)
+	/* Reset DQS delay */
+	while (ddrphy_wdly_dqs_inc_count_read() != 0) {
+		ddrphy_wdly_dqs_inc_write(1);
+		cdelay(100);
+	}
+#else
+	/* Reset DQS delay */
+	ddrphy_wdly_dqs_rst_write(1);
+	cdelay(100);
+#endif //defined(SDRAM_PHY_USDDRPHY) || defined(SDRAM_PHY_USPDDRPHY)
+}
+
+void write_inc_delay(int module) {
+	/* Increment DQ/DQS delay */
+	write_inc_dq_delay(module);
+	write_inc_dqs_delay(module);
+}
+
+void write_rst_delay(int module) {
+	write_rst_dq_delay(module);
+	write_rst_dqs_delay(module);
+}
+
+#endif // defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE)
+
+#if defined(SDRAM_PHY_BITSLIPS)
+
+int read_dq_bitslip[SDRAM_PHY_MODULES];
+void read_inc_dq_bitslip(int module) {
+	/* Increment bitslip */
+	read_dq_bitslip[module] = (read_dq_bitslip[module] + 1) & (SDRAM_PHY_BITSLIPS - 1);
+	ddrphy_rdly_dq_bitslip_write(1);
+}
+
+void read_rst_dq_bitslip(int module) {
+/* Reset bitslip */
+	read_dq_bitslip[module] = 0;
+	ddrphy_rdly_dq_bitslip_rst_write(1);
+}
+
+int write_dq_bitslip[SDRAM_PHY_MODULES];
+void write_inc_dq_bitslip(int module) {
+	/* Increment bitslip */
+	write_dq_bitslip[module] = (write_dq_bitslip[module] + 1) & (SDRAM_PHY_BITSLIPS - 1);
+	ddrphy_wdly_dq_bitslip_write(1);
+}
+
+void write_rst_dq_bitslip(int module) {
+	/* Increment bitslip */
+	write_dq_bitslip[module] = 0;
+	ddrphy_wdly_dq_bitslip_rst_write(1);
+}
+
+#endif // defined(SDRAM_PHY_BITSLIPS)
+
+void sdram_select(int module, int dq_line) {
+	ddrphy_dly_sel_write(1 << module);
+
+#ifdef SDRAM_DELAY_PER_DQ
+	/* Select DQ line */
+	ddrphy_dq_dly_sel_write(1 << dq_line);
+#endif
+}
+
+void sdram_deselect(int module, int dq_line) {
+	ddrphy_dly_sel_write(0);
+
+#if defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
+	/* Sync all DQSBUFM's, By toggling all dly_sel (DQSBUFM.PAUSE) lines. */
+	ddrphy_dly_sel_write(0xff);
+	ddrphy_dly_sel_write(0);
+#endif //SDRAM_PHY_ECP5DDRPHY
+
+#ifdef SDRAM_DELAY_PER_DQ
+	/* Un-select DQ line */
+	ddrphy_dq_dly_sel_write(0);
+#endif
+}
+
+void sdram_leveling_action(int module, int dq_line, action_callback action) {
+	/* Select module */
+	sdram_select(module, dq_line);
+
+	/* Action */
+	action(module);
+
+	/* Un-select module */
+	sdram_deselect(module, dq_line);
+}
+
+#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
+int _sdram_write_leveling_dat_delays[16];
+
+void sdram_write_leveling_rst_dat_delay(int module, int show) {
+	_sdram_write_leveling_dat_delays[module] = -1;
+	if (show)
+		printf("Reseting Dat delay of module %d\n", module);
+}
+
+void sdram_write_leveling_force_dat_delay(int module, int taps, int show) {
+	_sdram_write_leveling_dat_delays[module] = taps;
+	if (show)
+		printf("Forcing Dat delay of module %d to %d taps\n", module, taps);
+}
+
+#if defined(SDRAM_PHY_BITSLIPS)
+int _sdram_write_leveling_bitslips[16];
+void sdram_write_leveling_rst_bitslip(int module, int show) {
+	_sdram_write_leveling_bitslips[module] = -1;
+	if (show)
+		printf("Reseting Bitslip of module %d\n", module);
+}
+
+void sdram_write_leveling_force_bitslip(int module, int bitslip, int show) {
+	_sdram_write_leveling_bitslips[module] = bitslip;
+	if (show)
+		printf("Forcing Bitslip of module %d to %d\n", module, bitslip);
+}
+#endif // defined(SDRAM_PHY_BITSLIPS)
+#endif // SDRAM_PHY_WRITE_LEVELING_CAPABLE
+
+#endif // defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
diff --git a/litex/soc/software/liblitedram/accessors.h b/litex/soc/software/liblitedram/accessors.h
new file mode 100644
index 000000000..b411493a5
--- /dev/null
+++ b/litex/soc/software/liblitedram/accessors.h
@@ -0,0 +1,74 @@
+#ifndef __ACCESSORS_H
+#define __ACCESSORS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <generated/csr.h>
+#if defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+#include <generated/sdram_phy.h>
+
+typedef void (*action_callback)(int module);
+
+#if defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
+
+extern int read_dq_delay[SDRAM_PHY_MODULES];
+void read_inc_dq_delay(int module);
+void read_rst_dq_delay(int module);
+
+#endif // defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
+
+#if defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE)
+
+extern int sdram_clock_delay;
+void sdram_inc_clock_delay(void);
+void sdram_rst_clock_delay(void);
+
+extern int write_dq_delay[SDRAM_PHY_MODULES];
+void write_inc_dq_delay(int module);
+void write_rst_dq_delay(int module);
+
+void write_inc_dqs_delay(int module);
+void write_rst_dqs_delay(int module);
+
+void write_inc_delay(int module);
+void write_rst_delay(int module);
+
+#endif // defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE)
+
+#if defined(SDRAM_PHY_BITSLIPS)
+
+extern int read_dq_bitslip[SDRAM_PHY_MODULES];
+void read_inc_dq_bitslip(int module);
+void read_rst_dq_bitslip(int module);
+
+extern int write_dq_bitslip[SDRAM_PHY_MODULES];
+void write_inc_dq_bitslip(int module);
+void write_rst_dq_bitslip(int module);
+
+#endif // defined(SDRAM_PHY_BITSLIPS)
+
+void sdram_select(int module, int dq_line);
+void sdram_deselect(int module, int dq_line);
+void sdram_leveling_action(int module, int dq_line, action_callback action);
+
+#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
+extern int _sdram_write_leveling_dat_delays[16];
+void sdram_write_leveling_rst_dat_delay(int module, int show);
+void sdram_write_leveling_force_dat_delay(int module, int taps, int show);
+
+#if defined(SDRAM_PHY_BITSLIPS)
+extern int _sdram_write_leveling_bitslips[16];
+void sdram_write_leveling_rst_bitslip(int module, int show);
+void sdram_write_leveling_force_bitslip(int module, int bitslip, int show);
+#endif // defined(SDRAM_PHY_BITSLIPS)
+#endif // SDRAM_PHY_WRITE_LEVELING_CAPABLE
+
+#endif // defined(CSR_SDRAM_BASE) && defined(CSR_DDRPHY_BASE)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // __ACCESSORS_H
diff --git a/litex/soc/software/liblitedram/sdram.c b/litex/soc/software/liblitedram/sdram.c
index a1e0bdde3..06d01c35e 100644
--- a/litex/soc/software/liblitedram/sdram.c
+++ b/litex/soc/software/liblitedram/sdram.c
@@ -10,6 +10,7 @@
 // License: BSD
 
 #include <generated/csr.h>
+#ifdef CSR_SDRAM_BASE
 #include <generated/mem.h>
 
 #include <stdio.h>
@@ -18,21 +19,36 @@
 #include <libbase/memtest.h>
 #include <libbase/lfsr.h>
 
-#ifdef CSR_SDRAM_BASE
 #include <generated/sdram_phy.h>
-#endif
 #include <generated/mem.h>
 #include <system.h>
 
 #include <liblitedram/sdram.h>
 #include <liblitedram/sdram_dbg.h>
+#include <liblitedram/accessors.h>
 
 //#define SDRAM_TEST_DISABLE
 //#define SDRAM_WRITE_LEVELING_CMD_DELAY_DEBUG
 //#define SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG
 //#define SDRAM_LEVELING_SCAN_DISPLAY_HEX_DIV 10
 
-#ifdef CSR_SDRAM_BASE
+#ifdef SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG
+#define SDRAM_WLC_DEBUG 1
+#else
+#define SDRAM_WLC_DEBUG 0
+#endif // SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG
+
+#ifdef SDRAM_DELAY_PER_DQ
+#define DQ_COUNT SDRAM_PHY_DQ_DQS_RATIO
+#else
+#define DQ_COUNT 1
+#endif
+
+#if SDRAM_PHY_DELAYS > 32
+#define MODULO (SDRAM_PHY_DELAYS/32)
+#else
+#define MODULO (1)
+#endif // SDRAM_PHY_DELAYS > 32
 
 /*-----------------------------------------------------------------------*/
 /* Helpers                                                               */
@@ -41,14 +57,13 @@
 #define max(x, y) (((x) > (y)) ? (x) : (y))
 #define min(x, y) (((x) < (y)) ? (x) : (y))
 
-__attribute__((unused)) void cdelay(int i)
-{
+__attribute__((unused)) void cdelay(int i) {
 #ifndef CONFIG_BIOS_NO_DELAYS
 	while(i > 0) {
 		__asm__ volatile(CONFIG_CPU_NOP);
 		i--;
 	}
-#endif
+#endif // CONFIG_BIOS_NO_DELAYS
 }
 
 /*-----------------------------------------------------------------------*/
@@ -68,9 +83,9 @@ int sdram_get_freq(void) {
 int sdram_get_cl(void) {
 #ifdef SDRAM_PHY_CL
 	return SDRAM_PHY_CL;
-#else
+#else // not SDRAM_PHY_CL
 	return -1;
-#endif
+#endif // SDRAM_PHY_CL
 }
 
 int sdram_get_cwl(void) {
@@ -78,7 +93,7 @@ int sdram_get_cwl(void) {
 	return SDRAM_PHY_CWL;
 #else
 	return -1;
-#endif
+#endif // SDRAM_PHY_CWL
 }
 
 /*-----------------------------------------------------------------------*/
@@ -86,13 +101,12 @@ int sdram_get_cwl(void) {
 /*-----------------------------------------------------------------------*/
 
 #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
+#endif // CSR_DDRPHY_RDPHASE_ADDR
 }
 
 static unsigned char sdram_dfii_get_wrphase(void) {
@@ -100,27 +114,27 @@ static unsigned char sdram_dfii_get_wrphase(void) {
 	return ddrphy_wrphase_read();
 #else
 	return SDRAM_PHY_WRPHASE;
-#endif
+#endif // CSR_DDRPHY_WRPHASE_ADDR
 }
 
 static void sdram_dfii_pix_address_write(unsigned char phase, unsigned int value) {
 #if (SDRAM_PHY_PHASES > 8)
 	#error "More than 8 DFI phases not supported"
-#endif
+#endif // (SDRAM_PHY_PHASES > 8)
 	switch (phase) {
 #if (SDRAM_PHY_PHASES > 4)
 	case 7: sdram_dfii_pi7_address_write(value); break;
 	case 6: sdram_dfii_pi6_address_write(value); break;
 	case 5: sdram_dfii_pi5_address_write(value); break;
 	case 4: sdram_dfii_pi4_address_write(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 4)
 #if (SDRAM_PHY_PHASES > 2)
 	case 3: sdram_dfii_pi3_address_write(value); break;
 	case 2: sdram_dfii_pi2_address_write(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 2)
 #if (SDRAM_PHY_PHASES > 1)
 	case 1: sdram_dfii_pi1_address_write(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 1)
 	default: sdram_dfii_pi0_address_write(value);
 	}
 }
@@ -138,21 +152,21 @@ static void sdram_dfii_piwr_address_write(unsigned int value) {
 static void sdram_dfii_pix_baddress_write(unsigned char phase, unsigned int value) {
 #if (SDRAM_PHY_PHASES > 8)
 	#error "More than 8 DFI phases not supported"
-#endif
+#endif // (SDRAM_PHY_PHASES > 8)
 	switch (phase) {
 #if (SDRAM_PHY_PHASES > 4)
 	case 7: sdram_dfii_pi7_baddress_write(value); break;
 	case 6: sdram_dfii_pi6_baddress_write(value); break;
 	case 5: sdram_dfii_pi5_baddress_write(value); break;
 	case 4: sdram_dfii_pi4_baddress_write(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 4)
 #if (SDRAM_PHY_PHASES > 2)
 	case 3: sdram_dfii_pi3_baddress_write(value); break;
 	case 2: sdram_dfii_pi2_baddress_write(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 2)
 #if (SDRAM_PHY_PHASES > 1)
 	case 1: sdram_dfii_pi1_baddress_write(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 1)
 	default: sdram_dfii_pi0_baddress_write(value);
 	}
 }
@@ -170,21 +184,21 @@ static void sdram_dfii_piwr_baddress_write(unsigned int value) {
 static void command_px(unsigned char phase, unsigned int value) {
 #if (SDRAM_PHY_PHASES > 8)
 	#error "More than 8 DFI phases not supported"
-#endif
+#endif // (SDRAM_PHY_PHASES > 8)
 	switch (phase) {
 #if (SDRAM_PHY_PHASES > 4)
 	case 7: command_p7(value); break;
 	case 6: command_p6(value); break;
 	case 5: command_p5(value); break;
 	case 4: command_p4(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 4)
 #if (SDRAM_PHY_PHASES > 2)
 	case 3: command_p3(value); break;
 	case 2: command_p2(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 2)
 #if (SDRAM_PHY_PHASES > 1)
 	case 1: command_p1(value); break;
-#endif
+#endif // (SDRAM_PHY_PHASES > 1)
 	default: command_p0(value);
 	}
 }
@@ -198,8 +212,7 @@ static void command_pwr(unsigned int value) {
 	unsigned char wrphase = sdram_dfii_get_wrphase();
 	command_px(wrphase, value);
 }
-
-#endif
+#endif // CSR_DDRPHY_BASE
 
 /*-----------------------------------------------------------------------*/
 /* Software/Hardware Control                                             */
@@ -208,8 +221,7 @@ static void command_pwr(unsigned int value) {
 #define DFII_CONTROL_SOFTWARE (DFII_CONTROL_CKE|DFII_CONTROL_ODT|DFII_CONTROL_RESET_N)
 #define DFII_CONTROL_HARDWARE (DFII_CONTROL_SEL)
 
-void sdram_software_control_on(void)
-{
+void sdram_software_control_on(void) {
 	unsigned int previous;
 	previous = sdram_dfii_control_read();
 	/* Switch DFII to software control */
@@ -221,11 +233,10 @@ void sdram_software_control_on(void)
 #if CSR_DDRPHY_EN_VTC_ADDR
 	/* Disable Voltage/Temperature compensation */
 	ddrphy_en_vtc_write(0);
-#endif
+#endif // CSR_DDRPHY_EN_VTC_ADDR
 }
 
-void sdram_software_control_off(void)
-{
+void sdram_software_control_off(void) {
 	unsigned int previous;
 	previous = sdram_dfii_control_read();
 	/* Switch DFII to hardware control */
@@ -236,7 +247,7 @@ void sdram_software_control_off(void)
 #if CSR_DDRPHY_EN_VTC_ADDR
 	/* Enable Voltage/Temperature compensation */
 	ddrphy_en_vtc_write(1);
-#endif
+#endif // CSR_DDRPHY_EN_VTC_ADDR
 }
 
 /*-----------------------------------------------------------------------*/
@@ -255,8 +266,6 @@ void sdram_mode_register_write(char reg, int value) {
 /* Leveling Centering (Common for Read/Write Leveling)                   */
 /*-----------------------------------------------------------------------*/
 
-typedef void (*delay_callback)(int module);
-
 static void sdram_activate_test_row(void) {
 	sdram_dfii_pi0_address_write(0);
 	sdram_dfii_pi0_baddress_write(0);
@@ -294,15 +303,17 @@ static void print_scan_errors(unsigned int errors) {
 		printf("%x", errors);
 #else
 		printf("%d", errors == 0);
-#endif
+#endif // SDRAM_LEVELING_SCAN_DISPLAY_HEX_DIV
 }
 
 #define READ_CHECK_TEST_PATTERN_MAX_ERRORS (8*SDRAM_PHY_PHASES*DFII_PIX_DATA_BYTES/SDRAM_PHY_MODULES)
+#define MODULE_BITMASK ((1<<SDRAM_PHY_DQ_DQS_RATIO)-1)
 
-static unsigned int sdram_write_read_check_test_pattern(int module, unsigned int seed) {
-	int p, i;
+static unsigned int sdram_write_read_check_test_pattern(int module, unsigned int seed, int dq_line) {
+	int p, i, bit;
 	unsigned int errors;
 	unsigned int prv;
+	unsigned char value;
 	unsigned char tst[DFII_PIX_DATA_BYTES];
 	unsigned char prs[SDRAM_PHY_PHASES][DFII_PIX_DATA_BYTES];
 
@@ -310,8 +321,12 @@ static unsigned int sdram_write_read_check_test_pattern(int module, unsigned int
 	prv = seed;
 	for(p=0;p<SDRAM_PHY_PHASES;p++) {
 		for(i=0;i<DFII_PIX_DATA_BYTES;i++) {
-			prv = lfsr(32, prv);
-			prs[p][i] = prv;
+			value = 0;
+			for (bit=0;bit<8;bit++) {
+				prv = lfsr(32, prv);
+				value |= (prv&1) << bit;
+			}
+			prs[p][i] = value;
 		}
 	}
 
@@ -319,8 +334,9 @@ static unsigned int sdram_write_read_check_test_pattern(int module, unsigned int
 	sdram_activate_test_row();
 
 	/* Write pseudo-random sequence */
-	for(p=0;p<SDRAM_PHY_PHASES;p++)
+	for(p=0;p<SDRAM_PHY_PHASES;p++) {
 		csr_wr_buf_uint8(sdram_dfii_pix_wrdata_addr(p), prs[p], DFII_PIX_DATA_BYTES);
+	}
 	sdram_dfii_piwr_address_write(0);
 	sdram_dfii_piwr_baddress_write(0);
 	command_pwr(DFII_COMMAND_CAS|DFII_COMMAND_WE|DFII_COMMAND_CS|DFII_COMMAND_WRDATA);
@@ -328,7 +344,7 @@ static unsigned int sdram_write_read_check_test_pattern(int module, unsigned int
 
 #if defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
 	ddrphy_burstdet_clr_write(1);
-#endif
+#endif // defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
 
 	/* Read/Check pseudo-random sequence */
 	sdram_dfii_pird_address_write(0);
@@ -344,79 +360,113 @@ static unsigned int sdram_write_read_check_test_pattern(int module, unsigned int
 		/* Read back test pattern */
 		csr_rd_buf_uint8(sdram_dfii_pix_rddata_addr(p), tst, DFII_PIX_DATA_BYTES);
 		/* Verify bytes matching current 'module' */
-		for (int i = 0; i < DFII_PIX_DATA_BYTES; ++i) {
-			int j = p * DFII_PIX_DATA_BYTES + i;
-#if SDRAM_PHY_DQ_DQS_RATIO == 4
-			if (j % (SDRAM_PHY_MODULES/2) == ((SDRAM_PHY_MODULES-1)/2)-(module/2)) {
-				if(module % 2) {
-					errors += popcount((prs[p][i] & 0xf0) ^ (tst[i] & 0xf0));
-				} else {
-					errors += popcount((prs[p][i] & 0x0f) ^ (tst[i] & 0x0f));
-				}
-			}
-#else
-			if (j % SDRAM_PHY_MODULES == SDRAM_PHY_MODULES-1-module) {
-				errors += popcount(prs[p][i] ^ tst[i]);
-			}
-#endif
+		int pebo;   // module's positive_edge_byte_offset
+		int nebo;   // module's negative_edge_byte_offset, could be undefined if SDR DRAM is used
+		int ibo;    // module's in byte offset (x4 ICs)
+		int mask;   // Check data lines
+
+		mask = MODULE_BITMASK;
+
+#ifdef SDRAM_DELAY_PER_DQ
+		mask = 1 << dq_line;
+#endif // SDRAM_DELAY_PER_DQ
+
+		/* Values written into CSR are Big Endian */
+		/* SDRAM_PHY_XDR is define 1 if SDR and 2 if DDR*/
+		nebo = (DFII_PIX_DATA_BYTES / SDRAM_PHY_XDR) - 1 - (module * SDRAM_PHY_DQ_DQS_RATIO)/8;
+		pebo = nebo + DFII_PIX_DATA_BYTES / SDRAM_PHY_XDR;
+		/* When DFII_PIX_DATA_BYTES is 1 and SDRAM_PHY_XDR is 2, pebo and nebo are both -1s,
+		* but only correct value is 0. This can happen when single x4 IC is used */
+		if ((DFII_PIX_DATA_BYTES/SDRAM_PHY_XDR) == 0) {
+			pebo = 0;
+			nebo = 0;
 		}
+
+		ibo = (module * SDRAM_PHY_DQ_DQS_RATIO)%8; // Non zero only if x4 ICs are used
+
+		errors += popcount(((prs[p][pebo] >> ibo) & mask) ^
+		                   ((tst[pebo] >> ibo) & mask));
+		if (SDRAM_PHY_DQ_DQS_RATIO == 16)
+			errors += popcount(((prs[p][pebo+1] >> ibo) & mask) ^
+			                   ((tst[pebo+1] >> ibo) & mask));
+
+
+#if SDRAM_PHY_XDR == 2
+		if (DFII_PIX_DATA_BYTES == 1) // Special case for x4 single IC
+			ibo = 0x4;
+		errors += popcount(((prs[p][nebo] >> ibo) & mask) ^
+		                   ((tst[nebo] >> ibo) & mask));
+		if (SDRAM_PHY_DQ_DQS_RATIO == 16)
+			errors += popcount(((prs[p][nebo+1] >> ibo) & mask) ^
+			                   ((tst[nebo+1] >> ibo) & mask));
+#endif // SDRAM_PHY_XDR == 2
 	}
 
 #if defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
 	if (((ddrphy_burstdet_seen_read() >> module) & 0x1) != 1)
 		errors += 1;
-#endif
+#endif // defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
 
 	return errors;
 }
 
+static int _seed_array[] = {42, 84, 36, 72, 24, 48};
+static int _seed_array_length = sizeof(_seed_array) / sizeof(_seed_array[0]);
+
+static int run_test_pattern(int module, int dq_line) {
+	int errors = 0;
+	for (int i = 0; i < _seed_array_length; i++) {
+		errors += sdram_write_read_check_test_pattern(module, _seed_array[i], dq_line);
+	}
+	return errors;
+}
+
 static void sdram_leveling_center_module(
-	int module, int show_short, int show_long, delay_callback rst_delay, delay_callback inc_delay)
-{
+	int module, int show_short, int show_long, action_callback rst_delay,
+	action_callback inc_delay, int dq_line) {
+
 	int i;
 	int show;
-	int working;
+	int working, last_working;
 	unsigned int errors;
 	int delay, delay_mid, delay_range;
 	int delay_min = -1, delay_max = -1, cur_delay_min = -1;
 
 	if (show_long)
+#ifdef SDRAM_DELAY_PER_DQ
+		printf("m%d dq_line:%d: |", module, dq_line);
+#else
 		printf("m%d: |", module);
+#endif // SDRAM_DELAY_PER_DQ
 
 	/* Find smallest working delay */
 	delay = 0;
-	rst_delay(module);
+	working = 0;
+	sdram_leveling_action(module, dq_line, rst_delay);
 	while(1) {
-		errors  = sdram_write_read_check_test_pattern(module, 42);
-		errors += sdram_write_read_check_test_pattern(module, 84);
+		errors = run_test_pattern(module, dq_line);
+		last_working = working;
 		working = errors == 0;
-		show = show_long;
-#if SDRAM_PHY_DELAYS > 32
-		show = show && (delay%16 == 0);
-#endif
+		show = show_long && (delay%MODULO == 0);
 		if (show)
 			print_scan_errors(errors);
-		if(working && delay_min < 0) {
-			delay_min = delay;
+		if(working && last_working && delay_min < 0) {
+			delay_min = delay - 1; // delay on edges can be spotty
 			break;
 		}
 		delay++;
 		if(delay >= SDRAM_PHY_DELAYS)
 			break;
-		inc_delay(module);
+		sdram_leveling_action(module, dq_line, inc_delay);
 	}
 
 	delay_max = delay_min;
 	cur_delay_min = delay_min;
 	/* Find largest working delay range */
 	while(1) {
-		errors  = sdram_write_read_check_test_pattern(module, 42);
-		errors += sdram_write_read_check_test_pattern(module, 84);
+		errors = run_test_pattern(module, dq_line);
 		working = errors == 0;
-		show = show_long;
-#if SDRAM_PHY_DELAYS > 32
-		show = show && (delay%16 == 0);
-#endif
+		show = show_long && (delay%MODULO == 0);
 		if (show)
 			print_scan_errors(errors);
 
@@ -433,7 +483,7 @@ static void sdram_leveling_center_module(
 		delay++;
 		if(delay >= SDRAM_PHY_DELAYS)
 			break;
-		inc_delay(module);
+		sdram_leveling_action(module, dq_line, inc_delay);
 	}
 	if(delay_max < 0) {
 		delay_max = delay;
@@ -459,16 +509,15 @@ static void sdram_leveling_center_module(
 		int retries = 8; /* Do N configs/checks and give up if failing */
 		while (retries > 0) {
 			/* Set delay. */
-			rst_delay(module);
+			sdram_leveling_action(module, dq_line, rst_delay);
 			cdelay(100);
 			for(i = 0; i < delay_mid; i++) {
-				inc_delay(module);
+				sdram_leveling_action(module, dq_line, inc_delay);
 				cdelay(100);
 			}
 
 			/* Check */
-			errors  = sdram_write_read_check_test_pattern(module, 42);
-			errors += sdram_write_read_check_test_pattern(module, 84);
+			errors = run_test_pattern(module, dq_line);
 			if (errors == 0)
 				break;
 			retries--;
@@ -480,20 +529,17 @@ static void sdram_leveling_center_module(
 /* Write Leveling                                                        */
 /*-----------------------------------------------------------------------*/
 
-int _sdram_tck_taps;
-int _sdram_write_leveling_bitslips[16];
-
 #ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
 
+int _sdram_tck_taps;
+
 int _sdram_write_leveling_cmd_scan  = 1;
 int _sdram_write_leveling_cmd_delay = 0;
-int _sdram_write_leveling_dat_delays[16];
 
 int _sdram_write_leveling_cdly_range_start = -1;
 int _sdram_write_leveling_cdly_range_end   = -1;
 
-static void sdram_write_leveling_on(void)
-{
+static void sdram_write_leveling_on(void) {
 	// Flip write leveling bit in the Mode Register, as it is disabled by default
 	sdram_dfii_pi0_address_write(DDRX_MR_WRLVL_RESET ^ (1 << DDRX_MR_WRLVL_BIT));
 	sdram_dfii_pi0_baddress_write(DDRX_MR_WRLVL_ADDRESS);
@@ -503,13 +549,12 @@ static void sdram_write_leveling_on(void)
 	sdram_dfii_pi0_address_write((DDRX_MR_WRLVL_RESET ^ (1 << DDRX_MR_WRLVL_BIT)) ^ 0x2BF8) ;
 	sdram_dfii_pi0_baddress_write(DDRX_MR_WRLVL_ADDRESS ^ 0xF);
 	command_p0(DFII_COMMAND_RAS|DFII_COMMAND_CAS|DFII_COMMAND_WE|DFII_COMMAND_CS);
-#endif
+#endif // SDRAM_PHY_DDR4_RDIMM
 
 	ddrphy_wlevel_en_write(1);
 }
 
-static void sdram_write_leveling_off(void)
-{
+static void sdram_write_leveling_off(void) {
 	sdram_dfii_pi0_address_write(DDRX_MR_WRLVL_RESET);
 	sdram_dfii_pi0_baddress_write(DDRX_MR_WRLVL_ADDRESS);
 	command_p0(DFII_COMMAND_RAS|DFII_COMMAND_CAS|DFII_COMMAND_WE|DFII_COMMAND_CS);
@@ -518,7 +563,7 @@ static void sdram_write_leveling_off(void)
 	sdram_dfii_pi0_address_write(DDRX_MR_WRLVL_RESET ^ 0x2BF8);
 	sdram_dfii_pi0_baddress_write(DDRX_MR_WRLVL_ADDRESS ^ 0xF);
 	command_p0(DFII_COMMAND_RAS|DFII_COMMAND_CAS|DFII_COMMAND_WE|DFII_COMMAND_CS);
-#endif
+#endif // SDRAM_PHY_DDR4_RDIMM
 
 	ddrphy_wlevel_en_write(0);
 }
@@ -535,77 +580,14 @@ void sdram_write_leveling_force_cmd_delay(int taps, int show) {
 	_sdram_write_leveling_cmd_delay = taps;
 	if (show)
 		printf("Forcing Cmd delay to %d taps\n", taps);
-	ddrphy_cdly_rst_write(1);
-	cdelay(100);
+	sdram_rst_clock_delay();
 	for (i=0; i<taps; i++) {
-		ddrphy_cdly_inc_write(1);
-		cdelay(100);
+		sdram_inc_clock_delay();
 	}
 }
 
-void sdram_write_leveling_rst_dat_delay(int module, int show) {
-	_sdram_write_leveling_dat_delays[module] = -1;
-	if (show)
-		printf("Reseting Dat delay of module %d\n", module);
-}
-
-void sdram_write_leveling_force_dat_delay(int module, int taps, int show) {
-	_sdram_write_leveling_dat_delays[module] = taps;
-	if (show)
-		printf("Forcing Dat delay of module %d to %d taps\n", module, taps);
-}
-
-void sdram_write_leveling_rst_bitslip(int module, int show) {
-	_sdram_write_leveling_bitslips[module] = -1;
-	if (show)
-		printf("Reseting Bitslip of module %d\n", module);
-}
-
-void sdram_write_leveling_force_bitslip(int module, int bitslip, int show) {
-	_sdram_write_leveling_bitslips[module] = bitslip;
-	if (show)
-		printf("Forcing Bitslip of module %d to %d\n", module, bitslip);
-}
-
-static void sdram_write_leveling_rst_delay(int module) {
-	/* Select module */
-	ddrphy_dly_sel_write(1 << module);
-
-	/* Reset DQ delay */
-	ddrphy_wdly_dq_rst_write(1);
-
-#if defined(SDRAM_PHY_USDDRPHY) || defined(SDRAM_PHY_USPDDRPHY)
-	/* Reset DQS delay */
-	while (ddrphy_wdly_dqs_inc_count_read() != 0) {
-		ddrphy_wdly_dqs_inc_write(1);
-		cdelay(100);
-	}
-#else
-	/* Reset DQ/DQS delay */
-	ddrphy_wdly_dq_rst_write(1);
-	ddrphy_wdly_dqs_rst_write(1);
-	cdelay(100);
-#endif
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-}
-
-static void sdram_write_leveling_inc_delay(int module) {
-	/* Select module */
-	ddrphy_dly_sel_write(1 << module);
-
-	/* Increment DQ/DQS delay */
-	ddrphy_wdly_dq_inc_write(1);
-	ddrphy_wdly_dqs_inc_write(1);
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-}
-
-static int sdram_write_leveling_scan(int *delays, int loops, int show)
-{
-	int i, j, k;
+static int sdram_write_leveling_scan(int *delays, int loops, int show) {
+	int i, j, k, dq_line;
 
 	int err_ddrphy_wdly;
 
@@ -624,115 +606,119 @@ static int sdram_write_leveling_scan(int *delays, int loops, int show)
 	sdram_write_leveling_on();
 	cdelay(100);
 	for(i=0;i<SDRAM_PHY_MODULES;i++) {
-		if (show)
-			printf("  m%d: |", i);
-
-		/* Reset delay */
-		sdram_write_leveling_rst_delay(i);
-		cdelay(100);
-
-		/* Scan write delay taps */
-		for(j=0;j<err_ddrphy_wdly;j++) {
-			int zero_count = 0;
-			int one_count = 0;
-			int show_iter = show;
-#if SDRAM_PHY_DELAYS > 32
-			show_iter = (j%16 == 0) && show;
-#endif
-			for (k=0; k<loops; k++) {
-				ddrphy_wlevel_strobe_write(1);
-				cdelay(100);
-				csr_rd_buf_uint8(sdram_dfii_pix_rddata_addr(0), buf, DFII_PIX_DATA_BYTES);
-#if SDRAM_PHY_DQ_DQS_RATIO == 4
-				/* For x4 memories, we need to test individual nibbles, not bytes */
-
-				/* Extract the byte containing the nibble from the tested module */
-				int module_byte = buf[SDRAM_PHY_MODULES-1-(i/2)];
-				/* Shift the byte by 4 bits right if the module number is odd */
-				module_byte >>= 4 * (i % 2);
-				/* Extract the nibble from the tested module */
-				if ((module_byte & 0xf) != 0)
+		for (dq_line = 0; dq_line < DQ_COUNT; dq_line++) {
+			if (show)
+#ifdef SDRAM_DELAY_PER_DQ
+				printf("  m%d dq%d: |", i, dq_line);
 #else
-				if (buf[SDRAM_PHY_MODULES-1-i] != 0)
-#endif
-					one_count++;
-				else
-					zero_count++;
-			}
-			if (one_count > zero_count)
-				taps_scan[j] = 1;
-			else
-				taps_scan[j] = 0;
-			if (show_iter)
-				printf("%d", taps_scan[j]);
-			sdram_write_leveling_inc_delay(i);
-			cdelay(100);
-		}
-		if (show)
-			printf("|");
+				printf("  m%d: |", i);
+#endif // SDRAM_DELAY_PER_DQ
 
-		/* 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 = -1;
-		delays[i] = -1;
-		for(j=0;j<err_ddrphy_wdly+1;j++) {
-			if (one_window_active) {
-				if ((j == err_ddrphy_wdly) || (taps_scan[j] == 0)) {
-					one_window_active = 0;
-					one_window_count = j - one_window_start;
-					if (one_window_count > one_window_best_count) {
-						one_window_best_start = one_window_start;
-						one_window_best_count = one_window_count;
+			/* Reset delay */
+			sdram_leveling_action(i, dq_line, write_rst_delay);
+			cdelay(100);
+
+			/* Scan write delay taps */
+			for(j=0;j<err_ddrphy_wdly;j++) {
+				int zero_count = 0;
+				int one_count = 0;
+				int show_iter = (j%MODULO == 0) && show;
+
+				for (k=0; k<loops; k++) {
+					ddrphy_wlevel_strobe_write(1);
+					cdelay(100);
+					csr_rd_buf_uint8(sdram_dfii_pix_rddata_addr(0), buf, DFII_PIX_DATA_BYTES);
+#if SDRAM_PHY_DQ_DQS_RATIO == 4
+					/* For x4 memories, we need to test individual nibbles, not bytes */
+
+					/* Extract the byte containing the nibble from the tested module */
+					int module_byte = buf[SDRAM_PHY_MODULES-1-(i/2)];
+					/* Shift the byte by 4 bits right if the module number is odd */
+					module_byte >>= 4 * (i % 2);
+					/* Extract the nibble from the tested module */
+					if ((module_byte & 0xf) != 0)
+#else // SDRAM_PHY_DQ_DQS_RATIO != 4
+					if (buf[SDRAM_PHY_MODULES-1-i] != 0)
+#endif // SDRAM_PHY_DQ_DQS_RATIO == 4
+						one_count++;
+					else
+						zero_count++;
+				}
+				if (one_count > zero_count)
+					taps_scan[j] = 1;
+				else
+					taps_scan[j] = 0;
+				if (show_iter)
+					printf("%d", taps_scan[j]);
+				sdram_leveling_action(i, dq_line, write_inc_delay);
+				cdelay(100);
+			}
+			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 = -1;
+			delays[i] = -1;
+			for(j=0;j<err_ddrphy_wdly+1;j++) {
+				if (one_window_active) {
+					if ((j == err_ddrphy_wdly) || (taps_scan[j] == 0)) {
+						one_window_active = 0;
+						one_window_count = j - one_window_start;
+						if (one_window_count > one_window_best_count) {
+							one_window_best_start = one_window_start;
+							one_window_best_count = one_window_count;
+						}
+					}
+				} else {
+					if (j != err_ddrphy_wdly && taps_scan[j]) {
+						one_window_active = 1;
+						one_window_start = j;
 					}
 				}
-			} else {
-				if (j != err_ddrphy_wdly && taps_scan[j]) {
-					one_window_active = 1;
-					one_window_start = j;
+			}
+
+			/* Reset delay */
+			sdram_leveling_action(i, dq_line, write_rst_delay);
+			cdelay(100);
+
+			/* Use forced delay if configured */
+			if (_sdram_write_leveling_dat_delays[i] >= 0) {
+				delays[i] = _sdram_write_leveling_dat_delays[i];
+
+				/* Configure write delay */
+				for(j=0; j<delays[i]; j++)  {
+					sdram_leveling_action(i, dq_line, write_inc_delay);
+					cdelay(100);
+				}
+			/* Succeed only if the start of a 1s window has been found: */
+			} else if (
+				/* Start of 1s window directly seen after 0. */
+				((one_window_best_start) > 0 && (one_window_best_count > 0)) ||
+				/* Start of 1s window indirectly seen before 0. */
+				((one_window_best_start == 0) && (one_window_best_count > _sdram_tck_taps/4))
+			) {
+#if SDRAM_PHY_DELAYS > 32
+				/* Ensure write delay is just before transition */
+				one_window_start -= min(one_window_start, 16);
+#endif // SDRAM_PHY_DELAYS > 32
+				delays[i] = one_window_best_start;
+
+				/* Configure write delay */
+				for(j=0; j<delays[i]; j++) {
+					sdram_leveling_action(i, dq_line, write_inc_delay);
+					cdelay(100);
 				}
 			}
-		}
-
-		/* Reset delay */
-		sdram_write_leveling_rst_delay(i);
-		cdelay(100);
-
-		/* Use forced delay if configured */
-		if (_sdram_write_leveling_dat_delays[i] >= 0) {
-			delays[i] = _sdram_write_leveling_dat_delays[i];
-
-			/* Configure write delay */
-			for(j=0; j<delays[i]; j++)  {
-				sdram_write_leveling_inc_delay(i);
-				cdelay(100);
+			if (show) {
+				if (delays[i] == -1)
+					printf(" delay: -\n");
+				else
+					printf(" delay: %02d\n", delays[i]);
 			}
-		/* Succeed only if the start of a 1s window has been found: */
-		} else if (
-			/* Start of 1s window directly seen after 0. */
-			((one_window_best_start) > 0 && (one_window_best_count > 0)) ||
-			/* Start of 1s window indirectly seen before 0. */
-			((one_window_best_start == 0) && (one_window_best_count > _sdram_tck_taps/4))
-			){
-#if SDRAM_PHY_DELAYS > 32
-			/* Ensure write delay is just before transition */
-			one_window_start -= min(one_window_start, 16);
-#endif
-			delays[i] = one_window_best_start;
-
-			/* Configure write delay */
-			for(j=0; j<delays[i]; j++) {
-				sdram_write_leveling_inc_delay(i);
-				cdelay(100);
-			}
-		}
-		if (show) {
-			if (delays[i] == -1)
-				printf(" delay: -\n");
-			else
-				printf(" delay: %02d\n", delays[i]);
 		}
 	}
 
@@ -747,26 +733,21 @@ static int sdram_write_leveling_scan(int *delays, int loops, int show)
 	return ok;
 }
 
-static void sdram_write_leveling_find_cmd_delay(unsigned int *best_error, unsigned int *best_count, int *best_cdly,
-		int cdly_start, int cdly_stop, int cdly_step)
-{
+static void sdram_write_leveling_find_cmd_delay(
+	unsigned int *best_error, unsigned int *best_count, int *best_cdly,
+	int cdly_start, int cdly_stop, int cdly_step) {
 	int cdly;
-	int cdly_actual = 0;
 	int delays[SDRAM_PHY_MODULES];
 #ifndef SDRAM_WRITE_LEVELING_CMD_DELAY_DEBUG
 	int ok;
-#endif
+#endif // SDRAM_WRITE_LEVELING_CMD_DELAY_DEBUG
 
 	/* Scan through the range */
-	ddrphy_cdly_rst_write(1);
-	cdelay(100);
+	sdram_rst_clock_delay();
 	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(100);
-			cdly_actual++;
-		}
+		while (sdram_clock_delay < cdly)
+			sdram_inc_clock_delay();
 
 		/* Write level using this delay */
 #ifdef SDRAM_WRITE_LEVELING_CMD_DELAY_DEBUG
@@ -774,7 +755,7 @@ static void sdram_write_leveling_find_cmd_delay(unsigned int *best_error, unsign
 		sdram_write_leveling_scan(delays, 8, 1);
 #else
 		ok = sdram_write_leveling_scan(delays, 8, 0);
-#endif
+#endif // SDRAM_WRITE_LEVELING_CMD_DELAY_DEBUG
 		/* Use the mean of delays for error calulation */
 		int delay_mean  = 0;
 		int delay_count = 0;
@@ -804,12 +785,11 @@ static void sdram_write_leveling_find_cmd_delay(unsigned int *best_error, unsign
 		printf("Delay mean: %d/256, ideal: %d/256\n", delay_mean, ideal_delay);
 #else
 		printf("%d", ok);
-#endif
+#endif // SDRAM_WRITE_LEVELING_CMD_DELAY_DEBUG
 	}
 }
 
-int sdram_write_leveling(void)
-{
+int sdram_write_leveling(void) {
 	int delays[SDRAM_PHY_MODULES];
 	unsigned int best_error = ~0u;
 	unsigned int best_count = 0;
@@ -865,11 +845,9 @@ int sdram_write_leveling(void)
 	printf("  Setting Cmd/Clk delay to %d taps.\n", best_cdly);
 	/* Set working or forced delay */
 	if (best_cdly >= 0) {
-		ddrphy_cdly_rst_write(1);
-		cdelay(100);
+		sdram_rst_clock_delay();
 		for (int i = 0; i < best_cdly; ++i) {
-			ddrphy_cdly_inc_write(1);
-			cdelay(100);
+			sdram_inc_clock_delay();
 		}
 	}
 
@@ -881,75 +859,16 @@ int sdram_write_leveling(void)
 
 	return best_cdly >= 0;
 }
-
 #endif /*  SDRAM_PHY_WRITE_LEVELING_CAPABLE */
 
 /*-----------------------------------------------------------------------*/
 /* Read Leveling                                                         */
 /*-----------------------------------------------------------------------*/
 
-static void sdram_read_leveling_rst_delay(int module) {
-	/* Select module */
-	ddrphy_dly_sel_write(1 << module);
+#if defined(SDRAM_PHY_WRITE_DQ_DQS_TRAINING_CAPABLE) || defined(SDRAM_PHY_WRITE_LATENCY_CALIBRATION_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
 
-	/* Reset delay */
-	ddrphy_rdly_dq_rst_write(1);
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-
-#if defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
-	/* Sync all DQSBUFM's, By toggling all dly_sel (DQSBUFM.PAUSE) lines. */
-	ddrphy_dly_sel_write(0xff);
-	ddrphy_dly_sel_write(0);
-#endif
-}
-
-static void sdram_read_leveling_inc_delay(int module) {
-	/* Select module */
-	ddrphy_dly_sel_write(1 << module);
-
-	/* Increment delay */
-	ddrphy_rdly_dq_inc_write(1);
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-
-#if defined(SDRAM_PHY_ECP5DDRPHY) || defined(SDRAM_PHY_GW2DDRPHY)
-	/* Sync all DQSBUFM's, By toggling all dly_sel (DQSBUFM.PAUSE) lines. */
-	ddrphy_dly_sel_write(0xff);
-	ddrphy_dly_sel_write(0);
-#endif
-}
-
-static void sdram_read_leveling_rst_bitslip(char m)
-{
-	/* Select module */
-	ddrphy_dly_sel_write(1 << m);
-
-	/* Reset delay */
-	ddrphy_rdly_dq_bitslip_rst_write(1);
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-}
-
-
-static void sdram_read_leveling_inc_bitslip(char m)
-{
-	/* Select module */
-	ddrphy_dly_sel_write(1 << m);
-
-	/* Increment delay */
-	ddrphy_rdly_dq_bitslip_write(1);
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-}
-
-static unsigned int sdram_read_leveling_scan_module(int module, int bitslip, int show)
-{
-	const unsigned int max_errors = 2*READ_CHECK_TEST_PATTERN_MAX_ERRORS;
+static unsigned int sdram_read_leveling_scan_module(int module, int bitslip, int show, int dq_line) {
+	const unsigned int max_errors = _seed_array_length*READ_CHECK_TEST_PATTERN_MAX_ERRORS;
 	int i;
 	unsigned int score;
 	unsigned int errors;
@@ -958,22 +877,18 @@ static unsigned int sdram_read_leveling_scan_module(int module, int bitslip, int
 	score = 0;
 	if (show)
 		printf("  m%d, b%02d: |", module, bitslip);
-	sdram_read_leveling_rst_delay(module);
+	sdram_leveling_action(module, dq_line, read_rst_dq_delay);
 	for(i=0;i<SDRAM_PHY_DELAYS;i++) {
 		int working;
-		int _show = show;
-#if SDRAM_PHY_DELAYS > 32
-		_show = (i%16 == 0) & show;
-#endif
-		errors  = sdram_write_read_check_test_pattern(module, 42);
-		errors += sdram_write_read_check_test_pattern(module, 84);
+		int _show = (i%MODULO == 0) & show;
+		errors = run_test_pattern(module, dq_line);
 		working = errors == 0;
 		/* When any scan is working then the final score will always be higher then if no scan was working */
 		score += (working * max_errors*SDRAM_PHY_DELAYS) + (max_errors - errors);
 		if (_show) {
 			print_scan_errors(errors);
 		}
-		sdram_read_leveling_inc_delay(module);
+		sdram_leveling_action(module, dq_line, read_inc_dq_delay);
 	}
 	if (show)
 		printf("| ");
@@ -981,57 +896,63 @@ static unsigned int sdram_read_leveling_scan_module(int module, int bitslip, int
 	return score;
 }
 
-#endif /* CSR_DDRPHY_BASE */
+#endif // defined(SDRAM_PHY_WRITE_DQ_DQS_TRAINING_CAPABLE) || defined(SDRAM_PHY_WRITE_LATENCY_CALIBRATION_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
 
-#endif /* CSR_SDRAM_BASE */
+#ifdef SDRAM_PHY_READ_LEVELING_CAPABLE
 
-#ifdef CSR_SDRAM_BASE
-
-#if defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
-
-void sdram_read_leveling(void)
-{
+void sdram_read_leveling(void) {
 	int module;
 	int bitslip;
+	int dq_line;
 	unsigned int score;
 	unsigned int best_score;
 	int best_bitslip;
 
 	for(module=0; module<SDRAM_PHY_MODULES; module++) {
-		/* Scan possible read windows */
-		best_score = 0;
-		best_bitslip = 0;
-		sdram_read_leveling_rst_bitslip(module);
-		for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip++) {
-			/* Compute score */
-			score = sdram_read_leveling_scan_module(module, bitslip, 1);
-			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;
-				best_score = score;
+		for (dq_line = 0; dq_line < DQ_COUNT; dq_line++) {
+			/* Scan possible read windows */
+			best_score = 0;
+			best_bitslip = 0;
+			sdram_leveling_action(module, dq_line, read_rst_dq_bitslip);
+			for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip++) {
+				/* Compute score */
+				score = sdram_read_leveling_scan_module(module, bitslip, 1, dq_line);
+				sdram_leveling_center_module(module, 1, 0,
+					read_rst_dq_delay, read_inc_dq_delay, dq_line);
+				printf("\n");
+				if (score > best_score) {
+					best_bitslip = bitslip;
+					best_score = score;
+				}
+				/* Exit */
+				if (bitslip == SDRAM_PHY_BITSLIPS-1)
+					break;
+				/* Increment bitslip */
+				sdram_leveling_action(module, dq_line, read_inc_dq_bitslip);
 			}
-			/* Exit */
-			if (bitslip == SDRAM_PHY_BITSLIPS-1)
-				break;
-			/* Increment bitslip */
-			sdram_read_leveling_inc_bitslip(module);
+
+			/* Select best read window */
+#ifdef SDRAM_DELAY_PER_DQ
+			printf("  best: m%d, b%02d, dq_line%d ", module, best_bitslip, dq_line);
+#else
+			printf("  best: m%d, b%02d ", module, best_bitslip);
+#endif // SDRAM_DELAY_PER_DQ
+			sdram_leveling_action(module, dq_line, read_rst_dq_bitslip);
+			for (bitslip=0; bitslip<best_bitslip; bitslip++)
+				sdram_leveling_action(module, dq_line, read_inc_dq_bitslip);
+
+			/* Re-do leveling on best read window*/
+			sdram_leveling_center_module(module, 1, 0,
+				read_rst_dq_delay, read_inc_dq_delay, dq_line);
+			printf("\n");
 		}
-
-		/* Select best read window */
-		printf("  best: m%d, b%02d ", module, best_bitslip);
-		sdram_read_leveling_rst_bitslip(module);
-		for (bitslip=0; bitslip<best_bitslip; bitslip++)
-			sdram_read_leveling_inc_bitslip(module);
-
-		/* Re-do leveling on best read window*/
-		sdram_leveling_center_module(module, 1, 0,
-			sdram_read_leveling_rst_delay, sdram_read_leveling_inc_delay);
-		printf("\n");
 	}
 }
 
+#endif // SDRAM_PHY_READ_LEVELING_CAPABLE
+
+#endif /* CSR_DDRPHY_BASE */
+
 /*-----------------------------------------------------------------------*/
 /* Write latency calibration                                             */
 /*-----------------------------------------------------------------------*/
@@ -1042,80 +963,77 @@ static void sdram_write_latency_calibration(void) {
 	int i;
 	int module;
 	int bitslip;
+	int dq_line;
 	unsigned int score;
 	unsigned int subscore;
 	unsigned int best_score;
 	int best_bitslip;
 
-	for(module=0; module<SDRAM_PHY_MODULES; module++) {
-		/* Scan possible write windows */
-		best_score   = 0;
-		best_bitslip = -1;
-		for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip+=2) { /* +2 for tCK steps */
-#ifdef SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG
-			printf("m%d wb%02d:\n", module, bitslip);
-#endif
-			score = 0;
-			/* Select module */
-			ddrphy_dly_sel_write(1 << module);
-			/* Reset bitslip */
-			ddrphy_wdly_dq_bitslip_rst_write(1);
-			for (i=0; i<bitslip; i++) {
-				ddrphy_wdly_dq_bitslip_write(1);
+	for(module = 0; module < SDRAM_PHY_MODULES; module++) {
+		for (dq_line = 0; dq_line < DQ_COUNT; dq_line++) {
+			/* Scan possible write windows */
+			best_score   = 0;
+			best_bitslip = -1;
+			for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip+=2) { /* +2 for tCK steps */
+				if (SDRAM_WLC_DEBUG)
+					printf("m%d wb%02d:\n", module, bitslip);
+
+				sdram_leveling_action(module, dq_line, write_rst_dq_bitslip);
+				for (i=0; i<bitslip; i++) {
+					sdram_leveling_action(module, dq_line, write_inc_dq_bitslip);
+				}
+
+				score = 0;
+				sdram_leveling_action(module, dq_line, read_rst_dq_bitslip);
+
+				for(i=0; i<SDRAM_PHY_BITSLIPS; i++) {
+					/* Compute score */
+					const int debug = SDRAM_WLC_DEBUG; // Local variable should be optimized out
+					subscore = sdram_read_leveling_scan_module(module, i, debug, dq_line);
+					// If SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG was not defined, SDRAM_WLC_DEBUG will be defined as 0, so if(0) should be optimized out
+					if (debug)
+						printf("\n");
+					score = subscore > score ? subscore : score;
+					/* Increment bitslip */
+					sdram_leveling_action(module, dq_line, read_inc_dq_bitslip);
+				}
+				if (score > best_score) {
+					best_bitslip = bitslip;
+					best_score = score;
+				}
 			}
-			/* Un-select module */
-			ddrphy_dly_sel_write(0);
-			score = 0;
-			sdram_read_leveling_rst_bitslip(module);
-			for(i=0; i<SDRAM_PHY_BITSLIPS; i++) {
-				/* Compute score */
-#ifdef SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG
-				subscore = sdram_read_leveling_scan_module(module, i, 1);
-				printf("\n");
+
+#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
+			if (_sdram_write_leveling_bitslips[module] < 0)
+				bitslip = best_bitslip;
+			else
+				bitslip = _sdram_write_leveling_bitslips[module];
 #else
-				subscore = sdram_read_leveling_scan_module(module, i, 0);
-#endif
-				score = subscore > score ? subscore : score;
-				/* Increment bitslip */
-				sdram_read_leveling_inc_bitslip(module);
-			}
-			if (score > best_score) {
-				best_bitslip = bitslip;
-				best_score = score;
+			bitslip = best_bitslip;
+#endif // SDRAM_PHY_WRITE_LEVELING_CAPABLE
+			if (bitslip == -1)
+				printf("m%d:- ", module);
+			else
+#ifdef SDRAM_DELAY_PER_DQ
+				printf("m%d dq%d:%d ", module, dq_line, bitslip);
+#else
+				printf("m%d:%d ", module, bitslip);
+#endif // SDRAM_DELAY_PER_DQ
+
+			if (SDRAM_WLC_DEBUG)
+				printf("\n");
+
+			/* Reset bitslip */
+			sdram_leveling_action(module, dq_line, write_rst_dq_bitslip);
+			for (i=0; i<bitslip; i++) {
+				sdram_leveling_action(module, dq_line, write_inc_dq_bitslip);
 			}
 		}
-
-		#ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
-		if (_sdram_write_leveling_bitslips[module] < 0)
-			bitslip = best_bitslip;
-		else
-			bitslip = _sdram_write_leveling_bitslips[module];
-		#else
-			bitslip = best_bitslip;
-		#endif
-		if (bitslip == -1)
-			printf("m%d:- ", module);
-		else
-			printf("m%d:%d ", module, bitslip);
-#ifdef SDRAM_WRITE_LATENCY_CALIBRATION_DEBUG
 		printf("\n");
-#endif
-
-		/* Select best write window */
-		ddrphy_dly_sel_write(1 << module);
-
-		/* Reset bitslip */
-		ddrphy_wdly_dq_bitslip_rst_write(1);
-		for (i=0; i<bitslip; i++) {
-			ddrphy_wdly_dq_bitslip_write(1);
-		}
-		/* Un-select module */
-		ddrphy_dly_sel_write(0);
 	}
-	printf("\n");
 }
 
-#endif
+#endif // SDRAM_PHY_WRITE_LATENCY_CALIBRATION_CAPABLE
 
 /*-----------------------------------------------------------------------*/
 /* Write DQ-DQS training                                                 */
@@ -1123,77 +1041,46 @@ static void sdram_write_latency_calibration(void) {
 
 #ifdef SDRAM_PHY_WRITE_DQ_DQS_TRAINING_CAPABLE
 
-static void sdram_write_dq_dqs_training_rst_delay(int module) {
-	/* Select module */
-	ddrphy_dly_sel_write(1 << module);
-
-#if defined(SDRAM_PHY_USDDRPHY) || defined(SDRAM_PHY_USPDDRPHY)
-	/* Reset DQ delay */
-	int dq_count = ddrphy_wdly_dqs_inc_count_read();
-	while (dq_count != SDRAM_PHY_DELAYS) {
-		ddrphy_wdly_dq_inc_write(1);
-		cdelay(100);
-		dq_count++;
-	}
-#else
-	/* Reset DQ delay */
-	ddrphy_wdly_dq_rst_write(1);
-	cdelay(100);
-#endif
-
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-}
-
-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);
-	cdelay(100);
-	/* Un-select module */
-	ddrphy_dly_sel_write(0);
-}
-
-static void sdram_read_leveling_best_bitslip(int module)
-{
+static void sdram_read_leveling_best_bitslip(int module, int dq_line) {
 	unsigned int score;
 	int bitslip;
 	int best_bitslip = 0;
 	unsigned int best_score = 0;
 
-	sdram_read_leveling_rst_bitslip(module);
+	sdram_leveling_action(module, dq_line, read_rst_dq_bitslip);
 	for(bitslip=0; bitslip<SDRAM_PHY_BITSLIPS; bitslip++) {
-		score = sdram_read_leveling_scan_module(module, bitslip, 0);
+		score = sdram_read_leveling_scan_module(module, bitslip, 0, dq_line);
 		sdram_leveling_center_module(module, 0, 0,
-			sdram_read_leveling_rst_delay, sdram_read_leveling_inc_delay);
+			read_rst_dq_delay, read_inc_dq_delay, dq_line);
 		if (score > best_score) {
 			best_bitslip = bitslip;
 			best_score = score;
 		}
 		if (bitslip == SDRAM_PHY_BITSLIPS-1)
 			break;
-		sdram_read_leveling_inc_bitslip(module);
+		sdram_leveling_action(module, dq_line, read_inc_dq_bitslip);
 	}
 
 	/* Select best read window and re-center it */
-	sdram_read_leveling_rst_bitslip(module);
+	sdram_leveling_action(module, dq_line, read_rst_dq_bitslip);
 	for (bitslip=0; bitslip<best_bitslip; bitslip++)
-		sdram_read_leveling_inc_bitslip(module);
+		sdram_leveling_action(module, dq_line, read_inc_dq_bitslip);
 	sdram_leveling_center_module(module, 0, 0,
-		sdram_read_leveling_rst_delay, sdram_read_leveling_inc_delay);
+		read_rst_dq_delay, read_inc_dq_delay, dq_line);
 }
 
-static void sdram_write_dq_dqs_training(void)
-{
+static void sdram_write_dq_dqs_training(void) {
 	int module;
+	int dq_line;
 
 	for(module=0; module<SDRAM_PHY_MODULES; module++) {
-		/* Find best bitslip */
-		sdram_read_leveling_best_bitslip(module);
-		/* Center DQ-DQS window */
-		sdram_leveling_center_module(module, 1, 1,
-			sdram_write_dq_dqs_training_rst_delay, sdram_write_dq_dqs_training_inc_delay);
+		for (dq_line = 0; dq_line < DQ_COUNT; dq_line++) {
+			/* Find best bitslip */
+			sdram_read_leveling_best_bitslip(module, dq_line);
+			/* Center DQ-DQS window */
+			sdram_leveling_center_module(module, 1, 1,
+				write_rst_dq_delay, write_inc_dq_delay, dq_line);
+		}
 	}
 }
 
@@ -1203,70 +1090,80 @@ static void sdram_write_dq_dqs_training(void)
 /* Leveling                                                              */
 /*-----------------------------------------------------------------------*/
 
-int sdram_leveling(void)
-{
+int sdram_leveling(void) {
 	int module;
+	int dq_line;
 	sdram_software_control_on();
 
 	for(module=0; module<SDRAM_PHY_MODULES; module++) {
+		for (dq_line = 0; dq_line < DQ_COUNT; dq_line++) {
 #ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
-		sdram_write_leveling_rst_delay(module);
-#endif
-		sdram_read_leveling_rst_delay(module);
-		sdram_read_leveling_rst_bitslip(module);
+			sdram_leveling_action(module, dq_line, write_rst_delay);
+#ifdef SDRAM_PHY_BITSLIPS
+			sdram_leveling_action(module, dq_line, write_rst_dq_bitslip);
+#endif // SDRAM_PHY_BITSLIPS
+#endif // SDRAM_PHY_WRITE_LEVELING_CAPABLE
+
+#ifdef SDRAM_PHY_READ_LEVELING_CAPABLE
+			sdram_leveling_action(module, dq_line, read_rst_dq_delay);
+#ifdef SDRAM_PHY_BITSLIPS
+			sdram_leveling_action(module, dq_line, read_rst_dq_bitslip);
+#endif // SDRAM_PHY_BITSLIPS
+#endif // SDRAM_PHY_READ_LEVELING_CAPABLE
+		}
 	}
 
 #ifdef SDRAM_PHY_WRITE_LEVELING_CAPABLE
 	printf("Write leveling:\n");
 	sdram_write_leveling();
-#endif
+#endif // SDRAM_PHY_WRITE_LEVELING_CAPABLE
 
 #ifdef SDRAM_PHY_WRITE_LATENCY_CALIBRATION_CAPABLE
 	printf("Write latency calibration:\n");
 	sdram_write_latency_calibration();
-#endif
+#endif // SDRAM_PHY_WRITE_LATENCY_CALIBRATION_CAPABLE
 
 #ifdef SDRAM_PHY_WRITE_DQ_DQS_TRAINING_CAPABLE
 	printf("Write DQ-DQS training:\n");
 	sdram_write_dq_dqs_training();
-#endif
+#endif // SDRAM_PHY_WRITE_DQ_DQS_TRAINING_CAPABLE
 
 #ifdef SDRAM_PHY_READ_LEVELING_CAPABLE
 	printf("Read leveling:\n");
 	sdram_read_leveling();
-#endif
+#endif // SDRAM_PHY_READ_LEVELING_CAPABLE
 
 	sdram_software_control_off();
 
 	return 1;
 }
-#endif
 
 /*-----------------------------------------------------------------------*/
 /* Initialization                                                        */
 /*-----------------------------------------------------------------------*/
 
-int sdram_init(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);
+#ifdef SDRAM_PHY_BITSLIPS
 	for (i=0; i<16; i++) sdram_write_leveling_rst_bitslip(i, 0);
-#endif
+#endif // SDRAM_PHY_BITSLIPS
+#endif // SDRAM_PHY_WRITE_LEVELING_CAPABLE
 	/* Reset Read/Write phases */
 #ifdef CSR_DDRPHY_RDPHASE_ADDR
 	ddrphy_rdphase_write(SDRAM_PHY_RDPHASE);
-#endif
+#endif // CSR_DDRPHY_RDPHASE_ADDR
 #ifdef CSR_DDRPHY_WRPHASE_ADDR
 	ddrphy_wrphase_write(SDRAM_PHY_WRPHASE);
-#endif
+#endif // CSR_DDRPHY_WRPHASE_ADDR
 	/* 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;
-#endif
+#endif // SDRAM_PHY_CMD_DELAY
 	printf("Initializing SDRAM @0x%08lx...\n", MAIN_RAM_BASE);
 	sdram_software_control_on();
 #if CSR_DDRPHY_RST_ADDR
@@ -1274,30 +1171,30 @@ int sdram_init(void)
 	cdelay(1000);
 	ddrphy_rst_write(0);
 	cdelay(1000);
-#endif
+#endif // CSR_DDRPHY_RST_ADDR
 
 #ifdef CSR_DDRCTRL_BASE
 	ddrctrl_init_done_write(0);
 	ddrctrl_init_error_write(0);
-#endif
+#endif // CSR_DDRCTRL_BASE
 	init_sequence();
 #if defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
 	sdram_leveling();
-#endif
+#endif // defined(SDRAM_PHY_WRITE_LEVELING_CAPABLE) || defined(SDRAM_PHY_READ_LEVELING_CAPABLE)
 	sdram_software_control_off();
 #ifndef SDRAM_TEST_DISABLE
 	if(!memtest((unsigned int *) MAIN_RAM_BASE, MEMTEST_DATA_SIZE)) {
 #ifdef CSR_DDRCTRL_BASE
 		ddrctrl_init_error_write(1);
 		ddrctrl_init_done_write(1);
-#endif
+#endif // CSR_DDRCTRL_BASE
 		return 0;
 	}
 	memspeed((unsigned int *) MAIN_RAM_BASE, MEMTEST_DATA_SIZE, false, 0);
-#endif
+#endif // SDRAM_TEST_DISABLE
 #ifdef CSR_DDRCTRL_BASE
 	ddrctrl_init_done_write(1);
-#endif
+#endif // CSR_DDRCTRL_BASE
 
 	return 1;
 }
@@ -1314,7 +1211,7 @@ int sdram_init(void)
 #ifdef SDRAM_DEBUG_READBACK_MEM_ADDR
 #ifndef SDRAM_DEBUG_READBACK_MEM_SIZE
 #error "Provide readback memory size via SDRAM_DEBUG_READBACK_MEM_SIZE"
-#endif
+#endif // SDRAM_DEBUG_READBACK_MEM_SIZE
 #define SDRAM_DEBUG_READBACK_VERBOSE 1
 
 #define SDRAM_DEBUG_READBACK_COUNT 3
@@ -1323,11 +1220,10 @@ int sdram_init(void)
 #define _SINGLE_READBACK (SDRAM_DEBUG_READBACK_MEM_SIZE/SDRAM_DEBUG_READBACK_COUNT)
 #define _READBACK_ERRORS_SIZE (_SINGLE_READBACK - sizeof(struct readback))
 #define SDRAM_DEBUG_READBACK_LEN (_READBACK_ERRORS_SIZE / sizeof(struct memory_error))
-#endif
+#endif // SDRAM_DEBUG_READBACK_MEM_ADDR
 
 static int sdram_debug_error_stats_on_error(
-	unsigned int addr, unsigned int rdata, unsigned int refdata, void *arg)
-{
+	unsigned int addr, unsigned int rdata, unsigned int refdata, void *arg) {
 	struct error_stats *stats = (struct error_stats *) arg;
 	struct memory_error error = {
 		.addr = addr,
@@ -1364,8 +1260,7 @@ static void sdram_debug_error_stats(void) {
 
 #ifdef SDRAM_DEBUG_READBACK_MEM_ADDR
 static int sdram_debug_readback_on_error(
-	unsigned int addr, unsigned int rdata, unsigned int refdata, void *arg)
-{
+	unsigned int addr, unsigned int rdata, unsigned int refdata, void *arg) {
 	struct readback *readback = (struct readback *) arg;
 	struct memory_error error = {
 		.addr = addr,
@@ -1376,8 +1271,7 @@ static int sdram_debug_readback_on_error(
 	return readback_add(readback, SDRAM_DEBUG_READBACK_LEN, error) != 1;
 }
 
-static void sdram_debug_readback(void)
-{
+static void sdram_debug_readback(void) {
 	printf("Using storage @0x%08x with size 0x%08x for %d readbacks.\n",
 		SDRAM_DEBUG_READBACK_MEM_ADDR, SDRAM_DEBUG_READBACK_MEM_SIZE, SDRAM_DEBUG_READBACK_COUNT);
 
@@ -1426,20 +1320,19 @@ static void sdram_debug_readback(void)
 		}
 	}
 }
-#endif
+#endif // SDRAM_DEBUG_READBACK_MEM_ADDR
 
-void sdram_debug(void)
-{
+void sdram_debug(void) {
 #if defined(SDRAM_DEBUG_STATS_NUM_RUNS) && SDRAM_DEBUG_STATS_NUM_RUNS > 0
 	printf("\nError stats:\n");
 	sdram_debug_error_stats();
-#endif
+#endif // defined(SDRAM_DEBUG_STATS_NUM_RUNS) && SDRAM_DEBUG_STATS_NUM_RUNS > 0
 
 #ifdef SDRAM_DEBUG_READBACK_MEM_ADDR
 	printf("\nReadback:\n");
 	sdram_debug_readback();
-#endif
+#endif // SDRAM_DEBUG_READBACK_MEM_ADDR
 }
-#endif
+#endif // SDRAM_DEBUG
 
-#endif
+#endif // CSR_SDRAM_BASE
diff --git a/litex/soc/software/liblitedram/sdram.h b/litex/soc/software/liblitedram/sdram.h
index 8344d3ce8..22861be6b 100644
--- a/litex/soc/software/liblitedram/sdram.h
+++ b/litex/soc/software/liblitedram/sdram.h
@@ -31,10 +31,6 @@ void sdram_mode_register_write(char reg, int value);
 /*-----------------------------------------------------------------------*/
 void sdram_write_leveling_rst_cmd_delay(int show);
 void sdram_write_leveling_force_cmd_delay(int taps, int show);
-void sdram_write_leveling_rst_dat_delay(int module, int show);
-void sdram_write_leveling_force_dat_delay(int module, int taps, int show);
-void sdram_write_leveling_rst_bitslip(int module, int show);
-void sdram_write_leveling_force_bitslip(int module, int bitslip, int show);
 int sdram_write_leveling(void);
 
 /*-----------------------------------------------------------------------*/