From d595bd0baaebec0882e8cf1b1315a11bb3e35223 Mon Sep 17 00:00:00 2001
From: rasmuskleist <kleist.rasmus@gmail.com>
Date: Mon, 7 Aug 2023 10:42:16 +0200
Subject: [PATCH] Wait while busy on entry

---
 .../driver/storage/block_device_spiflash_impl.hpp     | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/modm/driver/storage/block_device_spiflash_impl.hpp b/src/modm/driver/storage/block_device_spiflash_impl.hpp
index 6f075675fa..850d55d820 100644
--- a/src/modm/driver/storage/block_device_spiflash_impl.hpp
+++ b/src/modm/driver/storage/block_device_spiflash_impl.hpp
@@ -77,6 +77,8 @@ modm::BdSpiFlash<Spi, Cs, flashSize>::read(uint8_t* buffer, bd_address_t address
 
 	if((size == 0) || (size % BlockSizeRead != 0) || (address + size > flashSize)) {
 		RF_RETURN(false);
+	} else {
+		RF_CALL(waitWhileBusy());
 	}
 
 	RF_CALL(spiOperation(Instruction::FR, address, nullptr, buffer, size, 1));
@@ -93,13 +95,14 @@ modm::BdSpiFlash<Spi, Cs, flashSize>::program(const uint8_t* buffer, bd_address_
 
 	if((size == 0) || (size % BlockSizeWrite != 0) || (address + size > flashSize)) {
 		RF_RETURN(false);
+	} else {
+		RF_CALL(waitWhileBusy());
 	}
 
 	index = 0;
 	while(index < size) {
 		RF_CALL(spiOperation(Instruction::WE));
 		RF_CALL(spiOperation(Instruction::PP, address + index, &buffer[index], nullptr, BlockSizeWrite));
-		RF_CALL(waitWhileBusy());
 		index += BlockSizeWrite;
 	}
 
@@ -116,17 +119,17 @@ modm::BdSpiFlash<Spi, Cs, flashSize>::erase(bd_address_t address, bd_size_t size
 
 	if((size == 0) || (size % BlockSizeErase != 0) || (address + size > flashSize)) {
 		RF_RETURN(false);
+	} else {
+		RF_CALL(waitWhileBusy());
 	}
 
 	if (address == 0 && size == flashSize) {
 		RF_CALL(spiOperation(Instruction::CE));
-		RF_CALL(waitWhileBusy());
 	} else {
 		index = 0;
 		while(index < size) {
 			RF_CALL(spiOperation(Instruction::WE));
 			RF_CALL(spiOperation(Instruction::SE, address + index));
-			RF_CALL(waitWhileBusy());
 			index += BlockSizeErase;
 		}
 	}
@@ -175,6 +178,8 @@ modm::BdSpiFlash<Spi, Cs, flashSize>::selectDie(uint8_t die)
 	RF_BEGIN();
 
 	RF_CALL(spiOperation(Instruction::SDS, &die, nullptr, 1));
+	RF_CALL(waitWhileBusy());
+
 	RF_CALL(spiOperation(Instruction::WE));
 	RF_CALL(spiOperation(Instruction::GBU));
 	RF_CALL(waitWhileBusy());