diff --git a/Marlin/src/sd/Sd2Card.cpp b/Marlin/src/sd/Sd2Card.cpp index 44cc9f2995784..c8c7e9301e05f 100644 --- a/Marlin/src/sd/Sd2Card.cpp +++ b/Marlin/src/sd/Sd2Card.cpp @@ -195,39 +195,42 @@ void DiskIODriver_SPI_SD::chipSelect() { bool DiskIODriver_SPI_SD::erase(uint32_t firstBlock, uint32_t lastBlock) { if (ENABLED(SDCARD_READONLY)) return false; - csd_t csd; - if (!readCSD(&csd)) goto FAIL; - - // check for single block erase - if (!csd.v1.erase_blk_en) { - // erase size mask - uint8_t m = (csd.v1.sector_size_high << 1) | csd.v1.sector_size_low; - if ((firstBlock & m) || ((lastBlock + 1) & m)) { - // error card can't erase specified area - error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK); - goto FAIL; + bool success = false; + do { + + csd_t csd; + if (!readCSD(&csd)) break; + + // check for single block erase + if (!csd.v1.erase_blk_en) { + // erase size mask + uint8_t m = (csd.v1.sector_size_high << 1) | csd.v1.sector_size_low; + if ((firstBlock & m) || ((lastBlock + 1) & m)) { + // error card can't erase specified area + error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK); + break; + } } - } - if (type_ != SD_CARD_TYPE_SDHC) { firstBlock <<= 9; lastBlock <<= 9; } - if (cardCommand(CMD32, firstBlock) || cardCommand(CMD33, lastBlock) || cardCommand(CMD38, 0)) { - error(SD_CARD_ERROR_ERASE); - goto FAIL; - } - #ifdef SD_ERASE_TIMEOUT - if (!waitNotBusy(SD_ERASE_TIMEOUT)) { - error(SD_CARD_ERROR_ERASE_TIMEOUT); - goto FAIL; + if (type_ != SD_CARD_TYPE_SDHC) { firstBlock <<= 9; lastBlock <<= 9; } + if (cardCommand(CMD32, firstBlock) || cardCommand(CMD33, lastBlock) || cardCommand(CMD38, 0)) { + error(SD_CARD_ERROR_ERASE); + break; } - #else - while (spiRec() != 0xFF) {} - #endif + #ifdef SD_ERASE_TIMEOUT + if (!waitNotBusy(SD_ERASE_TIMEOUT)) { + error(SD_CARD_ERROR_ERASE_TIMEOUT); + break; + } + #else + while (spiRec() != 0xFF) {} + #endif - chipDeselect(); - return true; + success = true; + + } while (0); - FAIL: chipDeselect(); - return false; + return success; } /** @@ -713,29 +716,33 @@ bool DiskIODriver_SPI_SD::writeStop() { if (ENABLED(SDCARD_READONLY)) return false; chipSelect(); - #ifdef SD_WRITE_TIMEOUT - if (!waitNotBusy(SD_WRITE_TIMEOUT)) { - error(SD_CARD_ERROR_STOP_TRAN); - goto FAIL; - } - #else - while (spiRec() != 0xFF) {} - #endif - spiSend(STOP_TRAN_TOKEN); - #ifdef SD_WRITE_TIMEOUT - if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto FAIL; - #else - while (spiRec() != 0xFF) {} - #endif - chipDeselect(); - return true; + bool success = false; + do { - #ifdef SD_WRITE_TIMEOUT - FAIL: - chipDeselect(); - return false; - #endif + #ifdef SD_WRITE_TIMEOUT + if (!waitNotBusy(SD_WRITE_TIMEOUT)) { + error(SD_CARD_ERROR_STOP_TRAN); + break; + } + #else + while (spiRec() != 0xFF) {} + #endif + + spiSend(STOP_TRAN_TOKEN); + + #ifdef SD_WRITE_TIMEOUT + if (!waitNotBusy(SD_WRITE_TIMEOUT)) break; + #else + while (spiRec() != 0xFF) {} + #endif + + success = true; + + } while(0); + + chipDeselect(); + return success; } #endif // NEED_SD2CARD_SPI