diff --git a/litex/soc/software/liblitespi/spiflash.c b/litex/soc/software/liblitespi/spiflash.c
index 6f8c47a63..54214472e 100644
--- a/litex/soc/software/liblitespi/spiflash.c
+++ b/litex/soc/software/liblitespi/spiflash.c
@@ -93,6 +93,14 @@ static void spiflash_master_write(uint32_t val, size_t len, size_t width, uint32
 
 #endif
 
+void spiflash_memspeed(void) {
+	/* Test Sequential Read accesses */
+	memspeed((unsigned int *) SPIFLASH_BASE, 4096, 1, 0);
+
+	/* Test Random Read accesses */
+	memspeed((unsigned int *) SPIFLASH_BASE, 4096, 1, 1);
+}
+
 void spiflash_init(void)
 {
 	printf("\nInitializing %s SPI Flash @0x%08lx...\n", SPIFLASH_MODULE_NAME, SPIFLASH_BASE);
@@ -123,8 +131,8 @@ void spiflash_init(void)
 	spiflash_freq_init();
 #endif
 
-	memspeed((unsigned int *) SPIFLASH_BASE, SPIFLASH_SIZE/16, 1, 0);
-	memspeed((unsigned int *) SPIFLASH_BASE, SPIFLASH_SIZE/16, 1, 1);
+	/* Test SPI Flash speed */
+	spiflash_memspeed();
 }
 
 #endif
diff --git a/litex/soc/software/liblitespi/spiflash.h b/litex/soc/software/liblitespi/spiflash.h
index fd846aced..e76a7d2a2 100644
--- a/litex/soc/software/liblitespi/spiflash.h
+++ b/litex/soc/software/liblitespi/spiflash.h
@@ -1,13 +1,12 @@
 #ifndef __LITESPI_FLASH_H
 #define __LITESPI_FLASH_H
 
-#include <generated/csr.h>
-
-#define SPI_FLASH_BLOCK_SIZE	256
-#define CRC32_ERASED_FLASH	0xFEA8A821
+#define SPI_FLASH_BLOCK_SIZE 256
+#define CRC32_ERASED_FLASH	 0xFEA8A821
 
 int spiflash_freq_init(void);
 void spiflash_dummy_bits_setup(unsigned int dummy_bits);
+void spiflash_memspeed(void);
 void spiflash_init(void);
 
 #endif /* __LITESPI_FLASH_H */