[PATCH 0/4] EmbeddedPkg/DwEmmc: Fix bugs causing DwEmmc to fail to initialize

[PATCH 0/4] EmbeddedPkg/DwEmmc: Fix bugs causing DwEmmc to fail to initialize

[tien.hock.loh]

From: Loh Tien Hock <tien.hock.loh@intel.com>

This patch series fixes bugs with DwEmmc driver, namely:
* Added CMD6 handling
* Fixed workaround querying SendCommand using delays
* Fix DMA transfer length causing buffer underrun in FIFO
* Check DMA completion before returning from SendCommand

Loh, Tien Hock (4):
  EmbeddedPkg/DwEmmc: Remove unnecessary MicroSecondDelay
  EmbeddedPkg/DwEmmc: Fix SendCommand parameters
  EmbeddedPkg/DwEmmc: Fix DMA transfer length
  EmbeddedPkg/DwEmmc: Check DMA completion in SendCommand

 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 122 +++++++++++++++++++++++-------
 1 file changed, 95 insertions(+), 27 deletions(-)

-- 
2.2.2

[PATCH 1/4] EmbeddedPkg/DwEmmc: Remove unnecessary MicroSecondDelay

[tien.hock.loh]

From: "Loh, Tien Hock" <tien.hock.loh@intel.com>

Existing implementation checks for error regardless of if
DWEMMC_INT_CMD_DONE is set, causing the loop check to errors out
even when it shouldn't if the MicroSecondDelay doesn't do long
enough delays. This removes MicroSecondDelay and updates the
function to check for CMD_DONE before doing any error checking.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>
---
 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
index 0437e30..6d0f472 100644
--- a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
+++ b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
@@ -290,17 +290,15 @@ SendCommand (
   ErrMask = DWEMMC_INT_EBE | DWEMMC_INT_HLE | DWEMMC_INT_RTO |
             DWEMMC_INT_RCRC | DWEMMC_INT_RE;
   ErrMask |= DWEMMC_INT_DCRC | DWEMMC_INT_DRT | DWEMMC_INT_SBE;
+
   do {
-    MicroSecondDelay(500);
     Data = MmioRead32 (DWEMMC_RINTSTS);
-
-    if (Data & ErrMask) {
-      return EFI_DEVICE_ERROR;
-    }
-    if (Data & DWEMMC_INT_DTO) {     // Transfer Done
-      break;
-    }
   } while (!(Data & DWEMMC_INT_CMD_DONE));
+
+  if (Data & ErrMask) {
+    return EFI_DEVICE_ERROR;
+  }
+
   return EFI_SUCCESS;
 }
 
-- 
2.2.2

[PATCH 2/4] EmbeddedPkg/DwEmmc: Fix SendCommand parameters

[tien.hock.loh]

From: "Loh, Tien Hock" <tien.hock.loh@intel.com>

Only send BIT_CMD_CHECK_RESPONSE_CRC if MMC commands needs it.

Fixes parameters to ACMD6 where if CMD is application command, ie. CMD55 is
sent before ACMD6, to do response instead of data transfer.

Added CMD51 handling as CMD51 is a data transfer, and needs BIT_CMD_READ
and BIT_CMD_DATA_EXPECTED to be set.

Updates DwEmmcReceiveResponse to SendCommand only if IsPendingReadCommand
or IsPendingWriteCommand is true.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>
---
 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 59 +++++++++++++++++++++++--------
 1 file changed, 45 insertions(+), 14 deletions(-)

diff --git a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
index 6d0f472..600ab01 100644
--- a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
+++ b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
@@ -45,6 +45,7 @@ DWEMMC_IDMAC_DESCRIPTOR   *gpIdmacDesc;
 EFI_GUID mDwEmmcDevicePathGuid = EFI_CALLER_ID_GUID;
 STATIC UINT32 mDwEmmcCommand;
 STATIC UINT32 mDwEmmcArgument;
+STATIC BOOLEAN mIsACmd = FALSE;
 
 EFI_STATUS
 DwEmmcReadBlockData (
@@ -321,68 +322,93 @@ DwEmmcSendCommand (
     break;
   case MMC_INDX(2):
     Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_LONG_RESPONSE |
-           BIT_CMD_CHECK_RESPONSE_CRC | BIT_CMD_SEND_INIT;
+           BIT_CMD_SEND_INIT;
     break;
   case MMC_INDX(3):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT |
            BIT_CMD_SEND_INIT;
     break;
+  case MMC_INDX(6):
+    if(mIsACmd) {
+      Cmd = BIT_CMD_RESPONSE_EXPECT ;
+    }
+    else {
+      Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_DATA_EXPECTED |
+            BIT_CMD_READ;
+    }
+    break;
   case MMC_INDX(7):
     if (Argument)
-        Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC;
+        Cmd = BIT_CMD_RESPONSE_EXPECT;
     else
         Cmd = 0;
     break;
   case MMC_INDX(8):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
-           BIT_CMD_DATA_EXPECTED | BIT_CMD_READ |
+    Cmd = BIT_CMD_RESPONSE_EXPECT |
            BIT_CMD_WAIT_PRVDATA_COMPLETE;
     break;
   case MMC_INDX(9):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT |
            BIT_CMD_LONG_RESPONSE;
     break;
   case MMC_INDX(12):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT  |
            BIT_CMD_STOP_ABORT_CMD;
     break;
   case MMC_INDX(13):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT  |
            BIT_CMD_WAIT_PRVDATA_COMPLETE;
     break;
   case MMC_INDX(16):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT  |
            BIT_CMD_DATA_EXPECTED | BIT_CMD_READ |
            BIT_CMD_WAIT_PRVDATA_COMPLETE;
     break;
   case MMC_INDX(17):
   case MMC_INDX(18):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT |
            BIT_CMD_DATA_EXPECTED | BIT_CMD_READ |
            BIT_CMD_WAIT_PRVDATA_COMPLETE;
     break;
   case MMC_INDX(24):
   case MMC_INDX(25):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT  |
            BIT_CMD_DATA_EXPECTED | BIT_CMD_WRITE |
            BIT_CMD_WAIT_PRVDATA_COMPLETE;
     break;
   case MMC_INDX(30):
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC |
+    Cmd = BIT_CMD_RESPONSE_EXPECT  |
            BIT_CMD_DATA_EXPECTED;
     break;
+  case MMC_INDX(51):
+    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_DATA_EXPECTED |
+           BIT_CMD_READ | BIT_CMD_WAIT_PRVDATA_COMPLETE;
+    break;
   default:
-    Cmd = BIT_CMD_RESPONSE_EXPECT | BIT_CMD_CHECK_RESPONSE_CRC;
+    Cmd = BIT_CMD_RESPONSE_EXPECT ;
     break;
   }
 
   Cmd |= MMC_GET_INDX(MmcCmd) | BIT_CMD_USE_HOLD_REG | BIT_CMD_START;
+
+  if(MMC_INDX(55) == MMC_GET_INDX(MmcCmd))
+    mIsACmd = TRUE;
+  else
+    mIsACmd = FALSE;
+
+  if (!(MmcCmd & MMC_CMD_NO_CRC_RESPONSE)) {
+    Cmd |= BIT_CMD_CHECK_RESPONSE_CRC;
+  }
+
   if (IsPendingReadCommand (Cmd) || IsPendingWriteCommand (Cmd)) {
     mDwEmmcCommand = Cmd;
     mDwEmmcArgument = Argument;
   } else {
+    mDwEmmcCommand = Cmd;
+    mDwEmmcArgument = Argument;
     Status = SendCommand (Cmd, Argument);
   }
+
   return Status;
 }
 
@@ -393,6 +419,11 @@ DwEmmcReceiveResponse (
   IN UINT32*                    Buffer
   )
 {
+  EFI_STATUS Status = EFI_SUCCESS;
+
+  if(IsPendingReadCommand (mDwEmmcCommand) || IsPendingWriteCommand(mDwEmmcCommand))
+    Status = SendCommand (mDwEmmcCommand, mDwEmmcArgument);
+
   if (Buffer == NULL) {
     return EFI_INVALID_PARAMETER;
   }
@@ -410,7 +441,7 @@ DwEmmcReceiveResponse (
     Buffer[2] = MmioRead32 (DWEMMC_RESP2);
     Buffer[3] = MmioRead32 (DWEMMC_RESP3);
   }
-  return EFI_SUCCESS;
+  return Status;
 }
 
 VOID
-- 
2.2.2

[PATCH 3/4] EmbeddedPkg/DwEmmc: Fix DMA transfer length

[tien.hock.loh]

From: "Loh, Tien Hock" <tien.hock.loh@intel.com>

DMA should not transfer more than requested length otherwise FIFO might run
into buffer underrun and causes errors in future transfers.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>
---
 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
index 600ab01..c232309 100644
--- a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
+++ b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
@@ -496,7 +496,13 @@ PrepareDmaData (
 
   Cnt = (Length + DWEMMC_DMA_BUF_SIZE - 1) / DWEMMC_DMA_BUF_SIZE;
   Blks = (Length + DWEMMC_BLOCK_SIZE - 1) / DWEMMC_BLOCK_SIZE;
-  Length = DWEMMC_BLOCK_SIZE * Blks;
+
+  if(Length < DWEMMC_BLOCK_SIZE) {
+    Length = Length;
+  }
+  else {
+    Length = DWEMMC_BLOCK_SIZE * Blks;
+  }
 
   for (Idx = 0; Idx < Cnt; Idx++) {
     (IdmacDesc + Idx)->Des0 = DWEMMC_IDMAC_DES0_OWN | DWEMMC_IDMAC_DES0_CH |
@@ -534,11 +540,18 @@ StartDma (
   Data |= DWEMMC_CTRL_INT_EN | DWEMMC_CTRL_DMA_EN | DWEMMC_CTRL_IDMAC_EN;
   MmioWrite32 (DWEMMC_CTRL, Data);
   Data = MmioRead32 (DWEMMC_BMOD);
+
   Data |= DWEMMC_IDMAC_ENABLE | DWEMMC_IDMAC_FB;
   MmioWrite32 (DWEMMC_BMOD, Data);
 
-  MmioWrite32 (DWEMMC_BLKSIZ, DWEMMC_BLOCK_SIZE);
-  MmioWrite32 (DWEMMC_BYTCNT, Length);
+  if(Length < DWEMMC_BLOCK_SIZE) {
+    MmioWrite32 (DWEMMC_BLKSIZ, Length);
+    MmioWrite32 (DWEMMC_BYTCNT, Length);
+  }
+  else {
+    MmioWrite32 (DWEMMC_BLKSIZ, DWEMMC_BLOCK_SIZE);
+    MmioWrite32 (DWEMMC_BYTCNT, Length);
+  }
 }
 
 EFI_STATUS
-- 
2.2.2

[PATCH 4/4] EmbeddedPkg/DwEmmc: Check DMA completion in SendCommand

[tien.hock.loh]

From: "Loh, Tien Hock" <tien.hock.loh@intel.com>

DwEmmcReadBlockData and DwEmmcWriteBlockData needs to check for the
transfer completion before returning. This also adds error checking
to the DMA transfer.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>
---
 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 30 ++++++++++++++++++++++++++++--
 1 file changed, 28 insertions(+), 2 deletions(-)

diff --git a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
index c232309..baf299d 100644
--- a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
+++ b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
@@ -563,8 +563,9 @@ DwEmmcReadBlockData (
   )
 {
   EFI_STATUS  Status;
-  UINT32      DescPages, CountPerPage, Count;
+  UINT32      DescPages, CountPerPage, Count, ErrMask;
   EFI_TPL     Tpl;
+  UINTN Rintsts;
 
   Tpl = gBS->RaiseTPL (TPL_NOTIFY);
 
@@ -587,6 +588,18 @@ DwEmmcReadBlockData (
     DEBUG ((DEBUG_ERROR, "Failed to read data, mDwEmmcCommand:%x, mDwEmmcArgument:%x, Status:%r\n", mDwEmmcCommand, mDwEmmcArgument, Status));
     goto out;
   }
+
+  while(!((MmioRead32(DWEMMC_RINTSTS) & (DWEMMC_INT_DTO)))) {
+    Rintsts = MmioRead32 (DWEMMC_RINTSTS);
+  }
+  ErrMask = DWEMMC_INT_EBE | DWEMMC_INT_HLE | DWEMMC_INT_RTO |
+            DWEMMC_INT_RCRC | DWEMMC_INT_RE | DWEMMC_INT_DCRC |
+            DWEMMC_INT_DRT | DWEMMC_INT_SBE;
+
+  if (Rintsts & ErrMask) {
+    Status = EFI_DEVICE_ERROR;
+    goto out;
+  }
 out:
   // Restore Tpl
   gBS->RestoreTPL (Tpl);
@@ -602,8 +615,9 @@ DwEmmcWriteBlockData (
   )
 {
   EFI_STATUS  Status;
-  UINT32      DescPages, CountPerPage, Count;
+  UINT32      DescPages, CountPerPage, Count, ErrMask;
   EFI_TPL     Tpl;
+  UINTN Rintsts;
 
   Tpl = gBS->RaiseTPL (TPL_NOTIFY);
 
@@ -626,6 +640,18 @@ DwEmmcWriteBlockData (
     DEBUG ((DEBUG_ERROR, "Failed to write data, mDwEmmcCommand:%x, mDwEmmcArgument:%x, Status:%r\n", mDwEmmcCommand, mDwEmmcArgument, Status));
     goto out;
   }
+
+  while(!((MmioRead32(DWEMMC_RINTSTS) & (DWEMMC_INT_DTO)))) {
+    Rintsts = MmioRead32 (DWEMMC_RINTSTS);
+  }
+  ErrMask = DWEMMC_INT_EBE | DWEMMC_INT_HLE | DWEMMC_INT_RTO |
+            DWEMMC_INT_RCRC | DWEMMC_INT_RE | DWEMMC_INT_DCRC |
+            DWEMMC_INT_DRT | DWEMMC_INT_SBE;
+
+  if (Rintsts & ErrMask) {
+    Status = EFI_DEVICE_ERROR;
+    goto out;
+  }
 out:
   // Restore Tpl
   gBS->RestoreTPL (Tpl);
-- 
2.2.2

[PATCH 0/4] EmbeddedPkg/DwEmmc: Fix bugs causing DwEmmc to fail to initialize

[tien.hock.loh]

From: Loh Tien Hock <tien.hock.loh@intel.com>

This patch series fixes bugs with DwEmmc driver, namely:
* Added CMD6 handling
* Fixed workaround querying SendCommand using delays
* Fix DMA transfer length causing buffer underrun in FIFO
* Check DMA completion before returning from SendCommand

Loh, Tien Hock (4):
  EmbeddedPkg/DwEmmc: Remove unnecessary MicroSecondDelay
  EmbeddedPkg/DwEmmc: Fix SendCommand parameters
  EmbeddedPkg/DwEmmc: Fix DMA transfer length
  EmbeddedPkg/DwEmmc: Check DMA completion in SendCommand

 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 122 +++++++++++++++++++++++-------
 1 file changed, 95 insertions(+), 27 deletions(-)

-- 
2.2.2

[PATCH 3/4] EmbeddedPkg/DwEmmc: Fix DMA transfer length

[tien.hock.loh]

From: "Loh, Tien Hock" <tien.hock.loh@intel.com>

DMA should not transfer more than requested length otherwise FIFO might run
into buffer underrun and causes errors in future transfers.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>
---
 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
index 600ab01..c232309 100644
--- a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
+++ b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
@@ -496,7 +496,13 @@ PrepareDmaData (
 
   Cnt = (Length + DWEMMC_DMA_BUF_SIZE - 1) / DWEMMC_DMA_BUF_SIZE;
   Blks = (Length + DWEMMC_BLOCK_SIZE - 1) / DWEMMC_BLOCK_SIZE;
-  Length = DWEMMC_BLOCK_SIZE * Blks;
+
+  if(Length < DWEMMC_BLOCK_SIZE) {
+    Length = Length;
+  }
+  else {
+    Length = DWEMMC_BLOCK_SIZE * Blks;
+  }
 
   for (Idx = 0; Idx < Cnt; Idx++) {
     (IdmacDesc + Idx)->Des0 = DWEMMC_IDMAC_DES0_OWN | DWEMMC_IDMAC_DES0_CH |
@@ -534,11 +540,18 @@ StartDma (
   Data |= DWEMMC_CTRL_INT_EN | DWEMMC_CTRL_DMA_EN | DWEMMC_CTRL_IDMAC_EN;
   MmioWrite32 (DWEMMC_CTRL, Data);
   Data = MmioRead32 (DWEMMC_BMOD);
+
   Data |= DWEMMC_IDMAC_ENABLE | DWEMMC_IDMAC_FB;
   MmioWrite32 (DWEMMC_BMOD, Data);
 
-  MmioWrite32 (DWEMMC_BLKSIZ, DWEMMC_BLOCK_SIZE);
-  MmioWrite32 (DWEMMC_BYTCNT, Length);
+  if(Length < DWEMMC_BLOCK_SIZE) {
+    MmioWrite32 (DWEMMC_BLKSIZ, Length);
+    MmioWrite32 (DWEMMC_BYTCNT, Length);
+  }
+  else {
+    MmioWrite32 (DWEMMC_BLKSIZ, DWEMMC_BLOCK_SIZE);
+    MmioWrite32 (DWEMMC_BYTCNT, Length);
+  }
 }
 
 EFI_STATUS
-- 
2.2.2

[PATCH 0/4] EmbeddedPkg/DwEmmc: Fix bugs causing DwEmmc to fail to initialize

[tien.hock.loh]

From: "Tien Hock, Loh" <tien.hock.loh@intel.com>

This patch series fixes bugs with DwEmmc driver, namely:
* Added CMD6 handling
* Fixed workaround querying SendCommand using delays
* Fix DMA transfer length causing buffer underrun in FIFO
* Check DMA completion before returning from SendCommand

Loh, Tien Hock (3):
  EmbeddedPkg/DwEmmc: Remove unnecessary MicroSecondDelay
  EmbeddedPkg/DwEmmc: Fix SendCommand parameters
  EmbeddedPkg/DwEmmc: Fix DMA transfer length

Tien Hock, Loh (1):
  EmbeddedPkg/DwEmmc: Check DMA completion in SendCommand

 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 120 +++++++++++++++++++++++-------
 1 file changed, 93 insertions(+), 27 deletions(-)

-- 
2.2.2

[PATCH 3/4] EmbeddedPkg/DwEmmc: Fix DMA transfer length

[tien.hock.loh]

From: "Loh, Tien Hock" <tien.hock.loh@intel.com>

DMA should not transfer more than requested length otherwise FIFO might run
into buffer underrun and causes errors in future transfers.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>
---
 EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
index 600ab01..c232309 100644
--- a/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
+++ b/EmbeddedPkg/Drivers/DwEmmcDxe/DwEmmcDxe.c
@@ -496,7 +496,13 @@ PrepareDmaData (
 
   Cnt = (Length + DWEMMC_DMA_BUF_SIZE - 1) / DWEMMC_DMA_BUF_SIZE;
   Blks = (Length + DWEMMC_BLOCK_SIZE - 1) / DWEMMC_BLOCK_SIZE;
-  Length = DWEMMC_BLOCK_SIZE * Blks;
+
+  if(Length < DWEMMC_BLOCK_SIZE) {
+    Length = Length;
+  }
+  else {
+    Length = DWEMMC_BLOCK_SIZE * Blks;
+  }
 
   for (Idx = 0; Idx < Cnt; Idx++) {
     (IdmacDesc + Idx)->Des0 = DWEMMC_IDMAC_DES0_OWN | DWEMMC_IDMAC_DES0_CH |
@@ -534,11 +540,18 @@ StartDma (
   Data |= DWEMMC_CTRL_INT_EN | DWEMMC_CTRL_DMA_EN | DWEMMC_CTRL_IDMAC_EN;
   MmioWrite32 (DWEMMC_CTRL, Data);
   Data = MmioRead32 (DWEMMC_BMOD);
+
   Data |= DWEMMC_IDMAC_ENABLE | DWEMMC_IDMAC_FB;
   MmioWrite32 (DWEMMC_BMOD, Data);
 
-  MmioWrite32 (DWEMMC_BLKSIZ, DWEMMC_BLOCK_SIZE);
-  MmioWrite32 (DWEMMC_BYTCNT, Length);
+  if(Length < DWEMMC_BLOCK_SIZE) {
+    MmioWrite32 (DWEMMC_BLKSIZ, Length);
+    MmioWrite32 (DWEMMC_BYTCNT, Length);
+  }
+  else {
+    MmioWrite32 (DWEMMC_BLKSIZ, DWEMMC_BLOCK_SIZE);
+    MmioWrite32 (DWEMMC_BYTCNT, Length);
+  }
 }
 
 EFI_STATUS
-- 
2.2.2