From 7f3d0ce814ef2f4edf5128f3a2970f3673b9353b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tomasz=20Mo=C5=84?= <desowin@gmail.com>
Date: Mon, 7 Jun 2021 12:48:44 +0200
Subject: [PATCH] DM320: Fix SDHC response format 2 handling

Return complete R2 response to caller. Due to incorrect bitfield
handling only bits [127-96] were ever returned to caller.

Change-Id: I83726af35fb24a8051fcca8b8d019ad89d17500e
---
 firmware/target/arm/tms320dm320/sdmmc-dm320.c | 26 ++++++++++++-------
 1 file changed, 17 insertions(+), 9 deletions(-)

diff --git a/firmware/target/arm/tms320dm320/sdmmc-dm320.c b/firmware/target/arm/tms320dm320/sdmmc-dm320.c
index f479b54a91..fdc3e041a0 100644
--- a/firmware/target/arm/tms320dm320/sdmmc-dm320.c
+++ b/firmware/target/arm/tms320dm320/sdmmc-dm320.c
@@ -71,6 +71,7 @@
 #define BLOCKS_PER_BANK    0x7A7800
 
 /* command flags for send_cmd */
+#define SDHC_RESP_FMT_MASK 0x0600
 #define SDHC_RESP_FMT_NONE 0x0000
 #define SDHC_RESP_FMT_1    0x0200
 #define SDHC_RESP_FMT_2    0x0400
@@ -306,16 +307,23 @@ static int sd_command(int cmd, unsigned long arg,
     {
         /* discard response */
     }
-    else if ((cmdat & SDHC_RESP_FMT_1) || (cmdat & SDHC_RESP_FMT_3))
+    else
     {
-        response[0] = (IO_MMC_RESPONSE7 << 16) | IO_MMC_RESPONSE6;
-    }
-    else if (cmdat & SDHC_RESP_FMT_2)
-    {
-        response[0] = (IO_MMC_RESPONSE7 << 16) | IO_MMC_RESPONSE6;
-        response[1] = (IO_MMC_RESPONSE5 << 16) | IO_MMC_RESPONSE4;
-        response[2] = (IO_MMC_RESPONSE3 << 16) | IO_MMC_RESPONSE2;
-        response[3] = (IO_MMC_RESPONSE1 << 16) | IO_MMC_RESPONSE0;
+        switch (cmdat & SDHC_RESP_FMT_MASK)
+        {
+            case SDHC_RESP_FMT_1:
+            case SDHC_RESP_FMT_3:
+                response[0] = (IO_MMC_RESPONSE7 << 16) | IO_MMC_RESPONSE6;
+                break;
+            case SDHC_RESP_FMT_2:
+                response[0] = (IO_MMC_RESPONSE7 << 16) | IO_MMC_RESPONSE6;
+                response[1] = (IO_MMC_RESPONSE5 << 16) | IO_MMC_RESPONSE4;
+                response[2] = (IO_MMC_RESPONSE3 << 16) | IO_MMC_RESPONSE2;
+                response[3] = (IO_MMC_RESPONSE1 << 16) | IO_MMC_RESPONSE0;
+                break;
+            default:
+                break;
+        }
     }
 
     return 0;