diff --git a/litex/soc/software/bios/main.c b/litex/soc/software/bios/main.c
index 02d690af1..f68f2168f 100644
--- a/litex/soc/software/bios/main.c
+++ b/litex/soc/software/bios/main.c
@@ -33,6 +33,7 @@
 
 #include <libbase/console.h>
 #include <libbase/crc.h>
+#include <libbase/memtest.h>
 
 #include <libbase/spiflash.h>
 #include <libbase/uart.h>
@@ -153,7 +154,7 @@ __attribute__((__used__)) int main(int i, char **c)
 
         sdr_ok = 1;
 
-#if defined(CSR_ETHMAC_BASE) || defined(CSR_SDRAM_BASE) || defined(CSR_SPIFLASH_CORE_BASE)
+#if defined(CSR_ETHMAC_BASE) || defined(MAIN_RAM_BASE) || defined(CSR_SPIFLASH_CORE_BASE)
     printf("--========== \e[1mInitialization\e[0m ============--\n");
 #ifdef CSR_ETHMAC_BASE
 	eth_init();
@@ -161,8 +162,9 @@ __attribute__((__used__)) int main(int i, char **c)
 #ifdef CSR_SDRAM_BASE
 	sdr_ok = sdram_init();
 #else
-#ifdef MAIN_RAM_TEST
-	sdr_ok = memtest();
+#ifdef MAIN_RAM_BASE
+	sdr_ok = memtest((unsigned int *) MAIN_RAM_BASE, min(MAIN_RAM_SIZE, MEMTEST_DATA_SIZE));
+	memspeed((unsigned int *) MAIN_RAM_BASE, min(MAIN_RAM_SIZE, MEMTEST_DATA_SIZE), false, 0);
 #endif
 #endif
 	if (sdr_ok != 1)
diff --git a/litex/soc/software/libbase/memtest.h b/litex/soc/software/libbase/memtest.h
index c0541f2f2..230598ce8 100644
--- a/litex/soc/software/libbase/memtest.h
+++ b/litex/soc/software/libbase/memtest.h
@@ -3,6 +3,16 @@
 
 #include <stdbool.h>
 
+#include <generated/csr.h>
+#include <generated/soc.h>
+
+#define max(x, y) (((x) > (y)) ? (x) : (y))
+#define min(x, y) (((x) < (y)) ? (x) : (y))
+
+#ifndef MEMTEST_DATA_SIZE
+#define MEMTEST_DATA_SIZE (2*1024*1024)
+#endif
+
 // Called when an error is encountered. Can return non-zero to stop the memtest.
 // `arg` can be used to pass arbitrary data to the callback via `memtest_config.arg`.
 typedef int (*on_error_callback)(unsigned int addr, unsigned int rdata, unsigned int refdata, void *arg);
diff --git a/litex/soc/software/liblitedram/sdram.c b/litex/soc/software/liblitedram/sdram.c
index 24d4b7b35..859bb121d 100644
--- a/litex/soc/software/liblitedram/sdram.c
+++ b/litex/soc/software/liblitedram/sdram.c
@@ -55,10 +55,6 @@ __attribute__((unused)) void cdelay(int i)
 /* Constants                                                             */
 /*-----------------------------------------------------------------------*/
 
-#ifndef MEMTEST_DATA_SIZE
-#define MEMTEST_DATA_SIZE (2*1024*1024)
-#endif
-
 #define DFII_PIX_DATA_BYTES SDRAM_PHY_DFI_DATABITS/8
 
 int sdram_get_databits(void) {