From 40050db210171dfff8a21b6c45efae37a584fc6d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Fri, 15 Apr 2016 20:42:31 -0700
Subject: [PATCH] Improve code in Sd2Card::readBlock

---
 Marlin/Sd2Card.cpp | 51 ++++++++++++++++++++--------------------------
 1 file changed, 22 insertions(+), 29 deletions(-)

diff --git a/Marlin/Sd2Card.cpp b/Marlin/Sd2Card.cpp
index 62416b068..6087aa868 100644
--- a/Marlin/Sd2Card.cpp
+++ b/Marlin/Sd2Card.cpp
@@ -383,38 +383,31 @@ fail:
  * the value zero, false, is returned for failure.
  */
 bool Sd2Card::readBlock(uint32_t blockNumber, uint8_t* dst) {
-#if ENABLED(SD_CHECK_AND_RETRY)
-  uint8_t retryCnt = 3;
   // use address if not SDHC card
   if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
-retry2:
-  retryCnt --;
-  if (cardCommand(CMD17, blockNumber)) {
-    error(SD_CARD_ERROR_CMD17);
-    if (retryCnt > 0) goto retry;
-    goto fail;
-  }
-  if (!readData(dst, 512)) {
-    if (retryCnt > 0) goto retry;
-    goto fail;
-  }
-  return true;
-retry:
-  chipSelectHigh();
-  cardCommand(CMD12, 0);//Try sending a stop command, but ignore the result.
-  errorCode_ = 0;
-  goto retry2;
-#else
-  // use address if not SDHC card
-  if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
-  if (cardCommand(CMD17, blockNumber)) {
-    error(SD_CARD_ERROR_CMD17);
-    goto fail;
-  }
-  return readData(dst, 512);
-#endif
 
-fail:
+  #if ENABLED(SD_CHECK_AND_RETRY)
+    uint8_t retryCnt = 3;
+    do {
+      if (!cardCommand(CMD17, blockNumber)) {
+        if (readData(dst, 512)) return true;
+      }
+      else
+        error(SD_CARD_ERROR_CMD17);
+
+      if (--retryCnt) break;
+
+      chipSelectHigh();
+      cardCommand(CMD12, 0); // Try sending a stop command, ignore the result.
+      errorCode_ = 0;
+    } while (true);
+  #else
+    if (cardCommand(CMD17, blockNumber))
+      error(SD_CARD_ERROR_CMD17);
+    else
+      return readData(dst, 512);
+  #endif
+
   chipSelectHigh();
   return false;
 }