diff --git a/litex/soc/software/liblitespi/spiflash.c b/litex/soc/software/liblitespi/spiflash.c
index 53bb936ef..b6cd68915 100644
--- a/litex/soc/software/liblitespi/spiflash.c
+++ b/litex/soc/software/liblitespi/spiflash.c
@@ -74,23 +74,42 @@ void spiflash_dummy_bits_setup(unsigned int dummy_bits)
 
 #ifdef CSR_SPIFLASH_CORE_MASTER_CS_ADDR
 
+static void spiflash_len_mask_width_write(uint32_t len, uint32_t width, uint32_t mask)
+{
+	uint32_t tmp = len & ((2 ^ CSR_SPIFLASH_CORE_MASTER_PHYCONFIG_LEN_SIZE) - 1);
+	uint32_t word = tmp << CSR_SPIFLASH_CORE_MASTER_PHYCONFIG_LEN_OFFSET;
+	tmp = width & ((2 ^ CSR_SPIFLASH_CORE_MASTER_PHYCONFIG_WIDTH_SIZE) - 1);
+	word |= tmp << CSR_SPIFLASH_CORE_MASTER_PHYCONFIG_WIDTH_OFFSET;
+	tmp = mask & ((2 ^ CSR_SPIFLASH_CORE_MASTER_PHYCONFIG_MASK_SIZE) - 1);
+	word |= tmp << CSR_SPIFLASH_CORE_MASTER_PHYCONFIG_MASK_OFFSET;
+	spiflash_core_master_phyconfig_write(word);
+}
+
+static bool spiflash_tx_ready(void)
+{
+	return (spiflash_core_master_status_read() >> CSR_SPIFLASH_CORE_MASTER_STATUS_TX_READY_OFFSET) & 1;
+}
+
+static bool spiflash_rx_ready(void)
+{
+	return (spiflash_core_master_status_read() >> CSR_SPIFLASH_CORE_MASTER_STATUS_RX_READY_OFFSET) & 1;
+}
+
 static void spiflash_master_write(uint32_t val, size_t len, size_t width, uint32_t mask)
 {
 	/* Be sure to empty RX queue before doing Xfer. */
-	while (spiflash_core_master_status_rx_ready_read())
+	while (spiflash_rx_ready())
 		spiflash_core_master_rxtx_read();
 
 	/* Configure Master */
-	spiflash_core_master_phyconfig_len_write(8 * len);
-	spiflash_core_master_phyconfig_mask_write(mask);
-	spiflash_core_master_phyconfig_width_write(width);
+	spiflash_len_mask_width_write(8*len, width, mask);
 
 	/* Set CS. */
 	spiflash_core_master_cs_write(1);
 
 	/* Do Xfer. */
 	spiflash_core_master_rxtx_write(val);
-	while (!spiflash_core_master_status_rx_ready_read());
+	while (!spiflash_rx_ready());
 
 	/* Clear CS. */
 	spiflash_core_master_cs_write(0);
@@ -102,21 +121,19 @@ static volatile uint8_t r_buf[SPI_FLASH_BLOCK_SIZE + 4];
 static uint32_t transfer_byte(uint8_t b)
 {
 	/* wait for tx ready */
-	while (!spiflash_core_master_status_tx_ready_read());
+	while (!spiflash_tx_ready());
 
 	spiflash_core_master_rxtx_write((uint32_t)b);
 
 	/* wait for rx ready */
-	while (!spiflash_core_master_status_rx_ready_read());
+	while (!spiflash_rx_ready());
 
 	return spiflash_core_master_rxtx_read();
 }
 
 static void transfer_cmd(volatile uint8_t *bs, volatile uint8_t *resp, int len)
 {
-	spiflash_core_master_phyconfig_len_write(8);
-	spiflash_core_master_phyconfig_width_write(1);
-	spiflash_core_master_phyconfig_mask_write(1);
+	spiflash_len_mask_width_write(8, 1, 1);
 	spiflash_core_master_cs_write(1);
 
 	flush_cpu_dcache();