1 files changed, 38 insertions, 5 deletions

diff --git a/hw/sd.c b/hw/sd.c
index e6e80db0ea..1f71d85a52 100644
--- a/hw/sd.c
+++ b/hw/sd.c
@@ -49,6 +49,7 @@ typedef enum {
     sd_r2_s,      /* CSD register */
     sd_r3,        /* OCR register */
     sd_r6 = 6,    /* Published RCA response */
+    sd_r7,        /* Operating voltage */
     sd_r1b = -1,
 } sd_rsp_type_t;
 
@@ -77,6 +78,7 @@ struct SDState {
     uint16_t rca;
     uint32_t card_status;
     uint8_t sd_status[64];
+    uint32_t vhs;
     int wp_switch;
     int *wp_groups;
     uint32_t size;
@@ -126,9 +128,9 @@ static void sd_set_status(SDState *sd)
     sd->card_status |= sd->state << 9;
 }
 
-const sd_cmd_type_t sd_cmd_type[64] = {
+static const sd_cmd_type_t sd_cmd_type[64] = {
     sd_bc,   sd_none, sd_bcr,  sd_bcr,  sd_none, sd_none, sd_none, sd_ac,
-    sd_none, sd_ac,   sd_ac,   sd_adtc, sd_ac,   sd_ac,   sd_none, sd_ac,
+    sd_bcr,  sd_ac,   sd_ac,   sd_adtc, sd_ac,   sd_ac,   sd_none, sd_ac,
     sd_ac,   sd_adtc, sd_adtc, sd_none, sd_none, sd_none, sd_none, sd_none,
     sd_adtc, sd_adtc, sd_adtc, sd_adtc, sd_ac,   sd_ac,   sd_adtc, sd_none,
     sd_ac,   sd_ac,   sd_none, sd_none, sd_none, sd_none, sd_ac,   sd_none,
@@ -137,7 +139,7 @@ const sd_cmd_type_t sd_cmd_type[64] = {
     sd_adtc, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 };
 
-const sd_cmd_type_t sd_acmd_type[64] = {
+static const sd_cmd_type_t sd_acmd_type[64] = {
     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_ac,   sd_none,
     sd_none, sd_none, sd_none, sd_none, sd_none, sd_adtc, sd_none, sd_none,
     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_adtc, sd_ac,
@@ -190,6 +192,7 @@ static uint16_t sd_crc16(void *message, size_t width)
 
 static void sd_set_ocr(SDState *sd)
 {
+    /* All voltages OK, card power-up OK, Standard Capacity SD Memory Card */
     sd->ocr = 0x80ffff80;
 }
 
@@ -349,6 +352,14 @@ static void sd_response_r6_make(SDState *sd, uint8_t *response)
     response[3] = status & 0xff;
 }
 
+static void sd_response_r7_make(SDState *sd, uint8_t *response)
+{
+    response[0] = (sd->vhs >> 24) & 0xff;
+    response[1] = (sd->vhs >> 16) & 0xff;
+    response[2] = (sd->vhs >>  8) & 0xff;
+    response[3] = (sd->vhs >>  0) & 0xff;
+}
+
 static void sd_reset(SDState *sd, BlockDriverState *bdrv)
 {
     uint32_t size;
@@ -685,6 +696,25 @@ static sd_rsp_type_t sd_normal_command(SDState *sd,
         }
         break;
 
+    case 8:	/* CMD8:   SEND_IF_COND */
+        /* Physical Layer Specification Version 2.00 command */
+        switch (sd->state) {
+        case sd_idle_state:
+            sd->vhs = 0;
+
+            /* No response if not exactly one VHS bit is set.  */
+            if (!(req.arg >> 8) || (req.arg >> ffs(req.arg & ~0xff)))
+                return sd->spi ? sd_r7 : sd_r0;
+
+            /* Accept.  */
+            sd->vhs = req.arg;
+            return sd_r7;
+
+        default:
+            break;
+        }
+        break;
+
     case 9:	/* CMD9:   SEND_CSD */
         switch (sd->state) {
         case sd_standby_state:
@@ -1238,13 +1268,11 @@ int sd_do_command(SDState *sd, struct sd_request_s *req,
 
     case sd_r2_i:
         memcpy(response, sd->cid, sizeof(sd->cid));
-        response[7] |= 1;
         rsplen = 16;
         break;
 
     case sd_r2_s:
         memcpy(response, sd->csd, sizeof(sd->csd));
-        response[7] |= 1;
         rsplen = 16;
         break;
 
@@ -1258,6 +1286,11 @@ int sd_do_command(SDState *sd, struct sd_request_s *req,
         rsplen = 4;
         break;
 
+    case sd_r7:
+        sd_response_r7_make(sd, response);
+        rsplen = 4;
+        break;
+
     case sd_r0:
     default:
         rsplen = 0;