hw/sd.c

   1 /*
   2  * SD Memory Card emulation as defined in the "SD Memory Card Physical
   3  * layer specification, Version 1.10."
   4  *
   5  * Copyright (c) 2006 Andrzej Zaborowski  <[email protected]>
   6  * Copyright (c) 2007 CodeSourcery
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  *
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in
  16  *    the documentation and/or other materials provided with the
  17  *    distribution.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS''
  20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  21  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  22  * PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR
  23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  24  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  25  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  26  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
  27  * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  28  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 #include "hw.h"
  33 #include "block.h"
  34 #include "sd.h"
  35 #include "bitmap.h"
  36
  37 //#define DEBUG_SD 1
  38
  39 #ifdef DEBUG_SD
  40 #define DPRINTF(fmt, ...) \
  41 do { fprintf(stderr, "SD: " fmt , ## __VA_ARGS__); } while (0)
  42 #else
  43 #define DPRINTF(fmt, ...) do {} while(0)
  44 #endif
  45
  46 typedef enum {
  47     sd_r0 = 0,    /* no response */
  48     sd_r1,        /* normal response command */
  49     sd_r2_i,      /* CID register */
  50     sd_r2_s,      /* CSD register */
  51     sd_r3,        /* OCR register */
  52     sd_r6 = 6,    /* Published RCA response */
  53     sd_r7,        /* Operating voltage */
  54     sd_r1b = -1,
  55     sd_illegal = -2,
  56 } sd_rsp_type_t;
  57
  58 struct SDState {
  59     enum {
  60         sd_inactive,
  61         sd_card_identification_mode,
  62         sd_data_transfer_mode,
  63     } mode;
  64     enum {
  65         sd_inactive_state = -1,
  66         sd_idle_state = 0,
  67         sd_ready_state,
  68         sd_identification_state,
  69         sd_standby_state,
  70         sd_transfer_state,
  71         sd_sendingdata_state,
  72         sd_receivingdata_state,
  73         sd_programming_state,
  74         sd_disconnect_state,
  75     } state;
  76     uint32_t ocr;
  77     uint8_t scr[8];
  78     uint8_t cid[16];
  79     uint8_t csd[16];
  80     uint16_t rca;
  81     uint32_t card_status;
  82     uint8_t sd_status[64];
  83     uint32_t vhs;
  84     bool wp_switch;
  85     unsigned long *wp_groups;
  86     uint64_t size;
  87     int blk_len;
  88     uint32_t erase_start;
  89     uint32_t erase_end;
  90     uint8_t pwd[16];
  91     int pwd_len;
  92     int function_group[6];
  93
  94     bool spi;
  95     int current_cmd;
  96     /* True if we will handle the next command as an ACMD. Note that this does
  97      * *not* track the APP_CMD status bit!
  98      */
  99     bool expecting_acmd;
 100     int blk_written;
 101     uint64_t data_start;
 102     uint32_t data_offset;
 103     uint8_t data[512];
 104     qemu_irq readonly_cb;
 105     qemu_irq inserted_cb;
 106     BlockDriverState *bdrv;
 107     uint8_t *buf;
 108
 109     bool enable;
 110 };
 111
 112 static void sd_set_mode(SDState *sd)
 113 {
 114     switch (sd->state) {
 115     case sd_inactive_state:
 116         sd->mode = sd_inactive;
 117         break;
 118
 119     case sd_idle_state:
 120     case sd_ready_state:
 121     case sd_identification_state:
 122         sd->mode = sd_card_identification_mode;
 123         break;
 124
 125     case sd_standby_state:
 126     case sd_transfer_state:
 127     case sd_sendingdata_state:
 128     case sd_receivingdata_state:
 129     case sd_programming_state:
 130     case sd_disconnect_state:
 131         sd->mode = sd_data_transfer_mode;
 132         break;
 133     }
 134 }
 135
 136 static const sd_cmd_type_t sd_cmd_type[64] = {
 137     sd_bc,   sd_none, sd_bcr,  sd_bcr,  sd_none, sd_none, sd_none, sd_ac,
 138     sd_bcr,  sd_ac,   sd_ac,   sd_adtc, sd_ac,   sd_ac,   sd_none, sd_ac,
 139     sd_ac,   sd_adtc, sd_adtc, sd_none, sd_none, sd_none, sd_none, sd_none,
 140     sd_adtc, sd_adtc, sd_adtc, sd_adtc, sd_ac,   sd_ac,   sd_adtc, sd_none,
 141     sd_ac,   sd_ac,   sd_none, sd_none, sd_none, sd_none, sd_ac,   sd_none,
 142     sd_none, sd_none, sd_bc,   sd_none, sd_none, sd_none, sd_none, sd_none,
 143     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_ac,
 144     sd_adtc, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 145 };
 146
 147 static const sd_cmd_type_t sd_acmd_type[64] = {
 148     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_ac,   sd_none,
 149     sd_none, sd_none, sd_none, sd_none, sd_none, sd_adtc, sd_none, sd_none,
 150     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_adtc, sd_ac,
 151     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 152     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 153     sd_none, sd_bcr,  sd_ac,   sd_none, sd_none, sd_none, sd_none, sd_none,
 154     sd_none, sd_none, sd_none, sd_adtc, sd_none, sd_none, sd_none, sd_none,
 155     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 156 };
 157
 158 static const int sd_cmd_class[64] = {
 159     0,  0,  0,  0,  0,  9, 10,  0,  0,  0,  0,  1,  0,  0,  0,  0,
 160     2,  2,  2,  2,  3,  3,  3,  3,  4,  4,  4,  4,  6,  6,  6,  6,
 161     5,  5, 10, 10, 10, 10,  5,  9,  9,  9,  7,  7,  7,  7,  7,  7,
 162     7,  7, 10,  7,  9,  9,  9,  8,  8, 10,  8,  8,  8,  8,  8,  8,
 163 };
 164
 165 static uint8_t sd_crc7(void *message, size_t width)
 166 {
 167     int i, bit;
 168     uint8_t shift_reg = 0x00;
 169     uint8_t *msg = (uint8_t *) message;
 170
 171     for (i = 0; i < width; i ++, msg ++)
 172         for (bit = 7; bit >= 0; bit --) {
 173             shift_reg <<= 1;
 174             if ((shift_reg >> 7) ^ ((*msg >> bit) & 1))
 175                 shift_reg ^= 0x89;
 176         }
 177
 178     return shift_reg;
 179 }
 180
 181 static uint16_t sd_crc16(void *message, size_t width)
 182 {
 183     int i, bit;
 184     uint16_t shift_reg = 0x0000;
 185     uint16_t *msg = (uint16_t *) message;
 186     width <<= 1;
 187
 188     for (i = 0; i < width; i ++, msg ++)
 189         for (bit = 15; bit >= 0; bit --) {
 190             shift_reg <<= 1;
 191             if ((shift_reg >> 15) ^ ((*msg >> bit) & 1))
 192                 shift_reg ^= 0x1011;
 193         }
 194
 195     return shift_reg;
 196 }
 197
 198 static void sd_set_ocr(SDState *sd)
 199 {
 200     /* All voltages OK, card power-up OK, Standard Capacity SD Memory Card */
 201     sd->ocr = 0x80ffff00;
 202 }
 203
 204 static void sd_set_scr(SDState *sd)
 205 {
 206     sd->scr[0] = 0x00;          /* SCR Structure */
 207     sd->scr[1] = 0x2f;          /* SD Security Support */
 208     sd->scr[2] = 0x00;
 209     sd->scr[3] = 0x00;
 210     sd->scr[4] = 0x00;
 211     sd->scr[5] = 0x00;
 212     sd->scr[6] = 0x00;
 213     sd->scr[7] = 0x00;
 214 }
 215
 216 #define MID     0xaa
 217 #define OID     "XY"
 218 #define PNM     "QEMU!"
 219 #define PRV     0x01
 220 #define MDT_YR  2006
 221 #define MDT_MON 2
 222
 223 static void sd_set_cid(SDState *sd)
 224 {
 225     sd->cid[0] = MID;           /* Fake card manufacturer ID (MID) */
 226     sd->cid[1] = OID[0];        /* OEM/Application ID (OID) */
 227     sd->cid[2] = OID[1];
 228     sd->cid[3] = PNM[0];        /* Fake product name (PNM) */
 229     sd->cid[4] = PNM[1];
 230     sd->cid[5] = PNM[2];
 231     sd->cid[6] = PNM[3];
 232     sd->cid[7] = PNM[4];
 233     sd->cid[8] = PRV;           /* Fake product revision (PRV) */
 234     sd->cid[9] = 0xde;          /* Fake serial number (PSN) */
 235     sd->cid[10] = 0xad;
 236     sd->cid[11] = 0xbe;
 237     sd->cid[12] = 0xef;
 238     sd->cid[13] = 0x00 |        /* Manufacture date (MDT) */
 239         ((MDT_YR - 2000) / 10);
 240     sd->cid[14] = ((MDT_YR % 10) << 4) | MDT_MON;
 241     sd->cid[15] = (sd_crc7(sd->cid, 15) << 1) | 1;
 242 }
 243
 244 #define HWBLOCK_SHIFT   9                       /* 512 bytes */
 245 #define SECTOR_SHIFT    5                       /* 16 kilobytes */
 246 #define WPGROUP_SHIFT   7                       /* 2 megs */
 247 #define CMULT_SHIFT     9                       /* 512 times HWBLOCK_SIZE */
 248 #define WPGROUP_SIZE    (1 << (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT))
 249
 250 static const uint8_t sd_csd_rw_mask[16] = {
 251     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 252     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xfe,
 253 };
 254
 255 static void sd_set_csd(SDState *sd, uint64_t size)
 256 {
 257     uint32_t csize = (size >> (CMULT_SHIFT + HWBLOCK_SHIFT)) - 1;
 258     uint32_t sectsize = (1 << (SECTOR_SHIFT + 1)) - 1;
 259     uint32_t wpsize = (1 << (WPGROUP_SHIFT + 1)) - 1;
 260
 261     if (size <= 0x40000000) {   /* Standard Capacity SD */
 262         sd->csd[0] = 0x00;      /* CSD structure */
 263         sd->csd[1] = 0x26;      /* Data read access-time-1 */
 264         sd->csd[2] = 0x00;      /* Data read access-time-2 */
 265         sd->csd[3] = 0x5a;      /* Max. data transfer rate */
 266         sd->csd[4] = 0x5f;      /* Card Command Classes */
 267         sd->csd[5] = 0x50 |     /* Max. read data block length */
 268             HWBLOCK_SHIFT;
 269         sd->csd[6] = 0xe0 |     /* Partial block for read allowed */
 270             ((csize >> 10) & 0x03);
 271         sd->csd[7] = 0x00 |     /* Device size */
 272             ((csize >> 2) & 0xff);
 273         sd->csd[8] = 0x3f |     /* Max. read current */
 274             ((csize << 6) & 0xc0);
 275         sd->csd[9] = 0xfc |     /* Max. write current */
 276             ((CMULT_SHIFT - 2) >> 1);
 277         sd->csd[10] = 0x40 |    /* Erase sector size */
 278             (((CMULT_SHIFT - 2) << 7) & 0x80) | (sectsize >> 1);
 279         sd->csd[11] = 0x00 |    /* Write protect group size */
 280             ((sectsize << 7) & 0x80) | wpsize;
 281         sd->csd[12] = 0x90 |    /* Write speed factor */
 282             (HWBLOCK_SHIFT >> 2);
 283         sd->csd[13] = 0x20 |    /* Max. write data block length */
 284             ((HWBLOCK_SHIFT << 6) & 0xc0);
 285         sd->csd[14] = 0x00;     /* File format group */
 286         sd->csd[15] = (sd_crc7(sd->csd, 15) << 1) | 1;
 287     } else {                    /* SDHC */
 288         size /= 512 * 1024;
 289         size -= 1;
 290         sd->csd[0] = 0x40;
 291         sd->csd[1] = 0x0e;
 292         sd->csd[2] = 0x00;
 293         sd->csd[3] = 0x32;
 294         sd->csd[4] = 0x5b;
 295         sd->csd[5] = 0x59;
 296         sd->csd[6] = 0x00;
 297         sd->csd[7] = (size >> 16) & 0xff;
 298         sd->csd[8] = (size >> 8) & 0xff;
 299         sd->csd[9] = (size & 0xff);
 300         sd->csd[10] = 0x7f;
 301         sd->csd[11] = 0x80;
 302         sd->csd[12] = 0x0a;
 303         sd->csd[13] = 0x40;
 304         sd->csd[14] = 0x00;
 305         sd->csd[15] = 0x00;
 306         sd->ocr |= 1 << 30;     /* High Capacity SD Memort Card */
 307     }
 308 }
 309
 310 static void sd_set_rca(SDState *sd)
 311 {
 312     sd->rca += 0x4567;
 313 }
 314
 315 /* Card status bits, split by clear condition:
 316  * A : According to the card current state
 317  * B : Always related to the previous command
 318  * C : Cleared by read
 319  */
 320 #define CARD_STATUS_A   0x02004100
 321 #define CARD_STATUS_B   0x00c01e00
 322 #define CARD_STATUS_C   0xfd39a028
 323
 324 static void sd_set_cardstatus(SDState *sd)
 325 {
 326     sd->card_status = 0x00000100;
 327 }
 328
 329 static void sd_set_sdstatus(SDState *sd)
 330 {
 331     memset(sd->sd_status, 0, 64);
 332 }
 333
 334 static int sd_req_crc_validate(SDRequest *req)
 335 {
 336     uint8_t buffer[5];
 337     buffer[0] = 0x40 | req->cmd;
 338     buffer[1] = (req->arg >> 24) & 0xff;
 339     buffer[2] = (req->arg >> 16) & 0xff;
 340     buffer[3] = (req->arg >> 8) & 0xff;
 341     buffer[4] = (req->arg >> 0) & 0xff;
 342     return 0;
 343     return sd_crc7(buffer, 5) != req->crc;      /* TODO */
 344 }
 345
 346 static void sd_response_r1_make(SDState *sd, uint8_t *response)
 347 {
 348     uint32_t status = sd->card_status;
 349     /* Clear the "clear on read" status bits */
 350     sd->card_status &= ~CARD_STATUS_C;
 351
 352     response[0] = (status >> 24) & 0xff;
 353     response[1] = (status >> 16) & 0xff;
 354     response[2] = (status >> 8) & 0xff;
 355     response[3] = (status >> 0) & 0xff;
 356 }
 357
 358 static void sd_response_r3_make(SDState *sd, uint8_t *response)
 359 {
 360     response[0] = (sd->ocr >> 24) & 0xff;
 361     response[1] = (sd->ocr >> 16) & 0xff;
 362     response[2] = (sd->ocr >> 8) & 0xff;
 363     response[3] = (sd->ocr >> 0) & 0xff;
 364 }
 365
 366 static void sd_response_r6_make(SDState *sd, uint8_t *response)
 367 {
 368     uint16_t arg;
 369     uint16_t status;
 370
 371     arg = sd->rca;
 372     status = ((sd->card_status >> 8) & 0xc000) |
 373              ((sd->card_status >> 6) & 0x2000) |
 374               (sd->card_status & 0x1fff);
 375     sd->card_status &= ~(CARD_STATUS_C & 0xc81fff);
 376
 377     response[0] = (arg >> 8) & 0xff;
 378     response[1] = arg & 0xff;
 379     response[2] = (status >> 8) & 0xff;
 380     response[3] = status & 0xff;
 381 }
 382
 383 static void sd_response_r7_make(SDState *sd, uint8_t *response)
 384 {
 385     response[0] = (sd->vhs >> 24) & 0xff;
 386     response[1] = (sd->vhs >> 16) & 0xff;
 387     response[2] = (sd->vhs >>  8) & 0xff;
 388     response[3] = (sd->vhs >>  0) & 0xff;
 389 }
 390
 391 static inline uint64_t sd_addr_to_wpnum(uint64_t addr)
 392 {
 393     return addr >> (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT);
 394 }
 395
 396 static void sd_reset(SDState *sd, BlockDriverState *bdrv)
 397 {
 398     uint64_t size;
 399     uint64_t sect;
 400
 401     if (bdrv) {
 402         bdrv_get_geometry(bdrv, &sect);
 403     } else {
 404         sect = 0;
 405     }
 406     size = sect << 9;
 407
 408     sect = sd_addr_to_wpnum(size) + 1;
 409
 410     sd->state = sd_idle_state;
 411     sd->rca = 0x0000;
 412     sd_set_ocr(sd);
 413     sd_set_scr(sd);
 414     sd_set_cid(sd);
 415     sd_set_csd(sd, size);
 416     sd_set_cardstatus(sd);
 417     sd_set_sdstatus(sd);
 418
 419     sd->bdrv = bdrv;
 420
 421     if (sd->wp_groups)
 422         g_free(sd->wp_groups);
 423     sd->wp_switch = bdrv ? bdrv_is_read_only(bdrv) : false;
 424     sd->wp_groups = bitmap_new(sect);
 425     memset(sd->function_group, 0, sizeof(int) * 6);
 426     sd->erase_start = 0;
 427     sd->erase_end = 0;
 428     sd->size = size;
 429     sd->blk_len = 0x200;
 430     sd->pwd_len = 0;
 431     sd->expecting_acmd = false;
 432 }
 433
 434 static void sd_cardchange(void *opaque, bool load)
 435 {
 436     SDState *sd = opaque;
 437
 438     qemu_set_irq(sd->inserted_cb, bdrv_is_inserted(sd->bdrv));
 439     if (bdrv_is_inserted(sd->bdrv)) {
 440         sd_reset(sd, sd->bdrv);
 441         qemu_set_irq(sd->readonly_cb, sd->wp_switch);
 442     }
 443 }
 444
 445 static const BlockDevOps sd_block_ops = {
 446     .change_media_cb = sd_cardchange,
 447 };
 448
 449 /* We do not model the chip select pin, so allow the board to select
 450    whether card should be in SSI or MMC/SD mode.  It is also up to the
 451    board to ensure that ssi transfers only occur when the chip select
 452    is asserted.  */
 453 SDState *sd_init(BlockDriverState *bs, bool is_spi)
 454 {
 455     SDState *sd;
 456
 457     sd = (SDState *) g_malloc0(sizeof(SDState));
 458     sd->buf = qemu_blockalign(bs, 512);
 459     sd->spi = is_spi;
 460     sd->enable = true;
 461     sd_reset(sd, bs);
 462     if (sd->bdrv) {
 463         bdrv_attach_dev_nofail(sd->bdrv, sd);
 464         bdrv_set_dev_ops(sd->bdrv, &sd_block_ops, sd);
 465     }
 466     return sd;
 467 }
 468
 469 void sd_set_cb(SDState *sd, qemu_irq readonly, qemu_irq insert)
 470 {
 471     sd->readonly_cb = readonly;
 472     sd->inserted_cb = insert;
 473     qemu_set_irq(readonly, sd->bdrv ? bdrv_is_read_only(sd->bdrv) : 0);
 474     qemu_set_irq(insert, sd->bdrv ? bdrv_is_inserted(sd->bdrv) : 0);
 475 }
 476
 477 static void sd_erase(SDState *sd)
 478 {
 479     int i, start, end;
 480     if (!sd->erase_start || !sd->erase_end) {
 481         sd->card_status |= ERASE_SEQ_ERROR;
 482         return;
 483     }
 484
 485     start = sd_addr_to_wpnum(sd->erase_start);
 486     end = sd_addr_to_wpnum(sd->erase_end);
 487     sd->erase_start = 0;
 488     sd->erase_end = 0;
 489     sd->csd[14] |= 0x40;
 490
 491     for (i = start; i <= end; i++) {
 492         if (test_bit(i, sd->wp_groups)) {
 493             sd->card_status |= WP_ERASE_SKIP;
 494         }
 495     }
 496 }
 497
 498 static uint32_t sd_wpbits(SDState *sd, uint64_t addr)
 499 {
 500     uint32_t i, wpnum;
 501     uint32_t ret = 0;
 502
 503     wpnum = sd_addr_to_wpnum(addr);
 504
 505     for (i = 0; i < 32; i++, wpnum++, addr += WPGROUP_SIZE) {
 506         if (addr < sd->size && test_bit(wpnum, sd->wp_groups)) {
 507             ret |= (1 << i);
 508         }
 509     }
 510
 511     return ret;
 512 }
 513
 514 static void sd_function_switch(SDState *sd, uint32_t arg)
 515 {
 516     int i, mode, new_func, crc;
 517     mode = !!(arg & 0x80000000);
 518
 519     sd->data[0] = 0x00;         /* Maximum current consumption */
 520     sd->data[1] = 0x01;
 521     sd->data[2] = 0x80;         /* Supported group 6 functions */
 522     sd->data[3] = 0x01;
 523     sd->data[4] = 0x80;         /* Supported group 5 functions */
 524     sd->data[5] = 0x01;
 525     sd->data[6] = 0x80;         /* Supported group 4 functions */
 526     sd->data[7] = 0x01;
 527     sd->data[8] = 0x80;         /* Supported group 3 functions */
 528     sd->data[9] = 0x01;
 529     sd->data[10] = 0x80;        /* Supported group 2 functions */
 530     sd->data[11] = 0x43;
 531     sd->data[12] = 0x80;        /* Supported group 1 functions */
 532     sd->data[13] = 0x03;
 533     for (i = 0; i < 6; i ++) {
 534         new_func = (arg >> (i * 4)) & 0x0f;
 535         if (mode && new_func != 0x0f)
 536             sd->function_group[i] = new_func;
 537         sd->data[14 + (i >> 1)] = new_func << ((i * 4) & 4);
 538     }
 539     memset(&sd->data[17], 0, 47);
 540     crc = sd_crc16(sd->data, 64);
 541     sd->data[65] = crc >> 8;
 542     sd->data[66] = crc & 0xff;
 543 }
 544
 545 static inline bool sd_wp_addr(SDState *sd, uint64_t addr)
 546 {
 547     return test_bit(sd_addr_to_wpnum(addr), sd->wp_groups);
 548 }
 549
 550 static void sd_lock_command(SDState *sd)
 551 {
 552     int erase, lock, clr_pwd, set_pwd, pwd_len;
 553     erase = !!(sd->data[0] & 0x08);
 554     lock = sd->data[0] & 0x04;
 555     clr_pwd = sd->data[0] & 0x02;
 556     set_pwd = sd->data[0] & 0x01;
 557
 558     if (sd->blk_len > 1)
 559         pwd_len = sd->data[1];
 560     else
 561         pwd_len = 0;
 562
 563     if (erase) {
 564         if (!(sd->card_status & CARD_IS_LOCKED) || sd->blk_len > 1 ||
 565                         set_pwd || clr_pwd || lock || sd->wp_switch ||
 566                         (sd->csd[14] & 0x20)) {
 567             sd->card_status |= LOCK_UNLOCK_FAILED;
 568             return;
 569         }
 570         bitmap_zero(sd->wp_groups, sd_addr_to_wpnum(sd->size) + 1);
 571         sd->csd[14] &= ~0x10;
 572         sd->card_status &= ~CARD_IS_LOCKED;
 573         sd->pwd_len = 0;
 574         /* Erasing the entire card here! */
 575         fprintf(stderr, "SD: Card force-erased by CMD42\n");
 576         return;
 577     }
 578
 579     if (sd->blk_len < 2 + pwd_len ||
 580                     pwd_len <= sd->pwd_len ||
 581                     pwd_len > sd->pwd_len + 16) {
 582         sd->card_status |= LOCK_UNLOCK_FAILED;
 583         return;
 584     }
 585
 586     if (sd->pwd_len && memcmp(sd->pwd, sd->data + 2, sd->pwd_len)) {
 587         sd->card_status |= LOCK_UNLOCK_FAILED;
 588         return;
 589     }
 590
 591     pwd_len -= sd->pwd_len;
 592     if ((pwd_len && !set_pwd) ||
 593                     (clr_pwd && (set_pwd || lock)) ||
 594                     (lock && !sd->pwd_len && !set_pwd) ||
 595                     (!set_pwd && !clr_pwd &&
 596                      (((sd->card_status & CARD_IS_LOCKED) && lock) ||
 597                       (!(sd->card_status & CARD_IS_LOCKED) && !lock)))) {
 598         sd->card_status |= LOCK_UNLOCK_FAILED;
 599         return;
 600     }
 601
 602     if (set_pwd) {
 603         memcpy(sd->pwd, sd->data + 2 + sd->pwd_len, pwd_len);
 604         sd->pwd_len = pwd_len;
 605     }
 606
 607     if (clr_pwd) {
 608         sd->pwd_len = 0;
 609     }
 610
 611     if (lock)
 612         sd->card_status |= CARD_IS_LOCKED;
 613     else
 614         sd->card_status &= ~CARD_IS_LOCKED;
 615 }
 616
 617 static sd_rsp_type_t sd_normal_command(SDState *sd,
 618                                        SDRequest req)
 619 {
 620     uint32_t rca = 0x0000;
 621     uint64_t addr = (sd->ocr & (1 << 30)) ? (uint64_t) req.arg << 9 : req.arg;
 622
 623     /* Not interpreting this as an app command */
 624     sd->card_status &= ~APP_CMD;
 625
 626     if (sd_cmd_type[req.cmd] == sd_ac || sd_cmd_type[req.cmd] == sd_adtc)
 627         rca = req.arg >> 16;
 628
 629     DPRINTF("CMD%d 0x%08x state %d\n", req.cmd, req.arg, sd->state);
 630     switch (req.cmd) {
 631     /* Basic commands (Class 0 and Class 1) */
 632     case 0:     /* CMD0:   GO_IDLE_STATE */
 633         switch (sd->state) {
 634         case sd_inactive_state:
 635             return sd->spi ? sd_r1 : sd_r0;
 636
 637         default:
 638             sd->state = sd_idle_state;
 639             sd_reset(sd, sd->bdrv);
 640             return sd->spi ? sd_r1 : sd_r0;
 641         }
 642         break;
 643
 644     case 1:     /* CMD1:   SEND_OP_CMD */
 645         if (!sd->spi)
 646             goto bad_cmd;
 647
 648         sd->state = sd_transfer_state;
 649         return sd_r1;
 650
 651     case 2:     /* CMD2:   ALL_SEND_CID */
 652         if (sd->spi)
 653             goto bad_cmd;
 654         switch (sd->state) {
 655         case sd_ready_state:
 656             sd->state = sd_identification_state;
 657             return sd_r2_i;
 658
 659         default:
 660             break;
 661         }
 662         break;
 663
 664     case 3:     /* CMD3:   SEND_RELATIVE_ADDR */
 665         if (sd->spi)
 666             goto bad_cmd;
 667         switch (sd->state) {
 668         case sd_identification_state:
 669         case sd_standby_state:
 670             sd->state = sd_standby_state;
 671             sd_set_rca(sd);
 672             return sd_r6;
 673
 674         default:
 675             break;
 676         }
 677         break;
 678
 679     case 4:     /* CMD4:   SEND_DSR */
 680         if (sd->spi)
 681             goto bad_cmd;
 682         switch (sd->state) {
 683         case sd_standby_state:
 684             break;
 685
 686         default:
 687             break;
 688         }
 689         break;
 690
 691     case 5: /* CMD5: reserved for SDIO cards */
 692         return sd_illegal;
 693
 694     case 6:     /* CMD6:   SWITCH_FUNCTION */
 695         if (sd->spi)
 696             goto bad_cmd;
 697         switch (sd->mode) {
 698         case sd_data_transfer_mode:
 699             sd_function_switch(sd, req.arg);
 700             sd->state = sd_sendingdata_state;
 701             sd->data_start = 0;
 702             sd->data_offset = 0;
 703             return sd_r1;
 704
 705         default:
 706             break;
 707         }
 708         break;
 709
 710     case 7:     /* CMD7:   SELECT/DESELECT_CARD */
 711         if (sd->spi)
 712             goto bad_cmd;
 713         switch (sd->state) {
 714         case sd_standby_state:
 715             if (sd->rca != rca)
 716                 return sd_r0;
 717
 718             sd->state = sd_transfer_state;
 719             return sd_r1b;
 720
 721         case sd_transfer_state:
 722         case sd_sendingdata_state:
 723             if (sd->rca == rca)
 724                 break;
 725
 726             sd->state = sd_standby_state;
 727             return sd_r1b;
 728
 729         case sd_disconnect_state:
 730             if (sd->rca != rca)
 731                 return sd_r0;
 732
 733             sd->state = sd_programming_state;
 734             return sd_r1b;
 735
 736         case sd_programming_state:
 737             if (sd->rca == rca)
 738                 break;
 739
 740             sd->state = sd_disconnect_state;
 741             return sd_r1b;
 742
 743         default:
 744             break;
 745         }
 746         break;
 747
 748     case 8:     /* CMD8:   SEND_IF_COND */
 749         /* Physical Layer Specification Version 2.00 command */
 750         switch (sd->state) {
 751         case sd_idle_state:
 752             sd->vhs = 0;
 753
 754             /* No response if not exactly one VHS bit is set.  */
 755             if (!(req.arg >> 8) || (req.arg >> ffs(req.arg & ~0xff)))
 756                 return sd->spi ? sd_r7 : sd_r0;
 757
 758             /* Accept.  */
 759             sd->vhs = req.arg;
 760             return sd_r7;
 761
 762         default:
 763             break;
 764         }
 765         break;
 766
 767     case 9:     /* CMD9:   SEND_CSD */
 768         switch (sd->state) {
 769         case sd_standby_state:
 770             if (sd->rca != rca)
 771                 return sd_r0;
 772
 773             return sd_r2_s;
 774
 775         case sd_transfer_state:
 776             if (!sd->spi)
 777                 break;
 778             sd->state = sd_sendingdata_state;
 779             memcpy(sd->data, sd->csd, 16);
 780             sd->data_start = addr;
 781             sd->data_offset = 0;
 782             return sd_r1;
 783
 784         default:
 785             break;
 786         }
 787         break;
 788
 789     case 10:    /* CMD10:  SEND_CID */
 790         switch (sd->state) {
 791         case sd_standby_state:
 792             if (sd->rca != rca)
 793                 return sd_r0;
 794
 795             return sd_r2_i;
 796
 797         case sd_transfer_state:
 798             if (!sd->spi)
 799                 break;
 800             sd->state = sd_sendingdata_state;
 801             memcpy(sd->data, sd->cid, 16);
 802             sd->data_start = addr;
 803             sd->data_offset = 0;
 804             return sd_r1;
 805
 806         default:
 807             break;
 808         }
 809         break;
 810
 811     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
 812         if (sd->spi)
 813             goto bad_cmd;
 814         switch (sd->state) {
 815         case sd_transfer_state:
 816             sd->state = sd_sendingdata_state;
 817             sd->data_start = req.arg;
 818             sd->data_offset = 0;
 819
 820             if (sd->data_start + sd->blk_len > sd->size)
 821                 sd->card_status |= ADDRESS_ERROR;
 822             return sd_r0;
 823
 824         default:
 825             break;
 826         }
 827         break;
 828
 829     case 12:    /* CMD12:  STOP_TRANSMISSION */
 830         switch (sd->state) {
 831         case sd_sendingdata_state:
 832             sd->state = sd_transfer_state;
 833             return sd_r1b;
 834
 835         case sd_receivingdata_state:
 836             sd->state = sd_programming_state;
 837             /* Bzzzzzzztt .... Operation complete.  */
 838             sd->state = sd_transfer_state;
 839             return sd_r1b;
 840
 841         default:
 842             break;
 843         }
 844         break;
 845
 846     case 13:    /* CMD13:  SEND_STATUS */
 847         switch (sd->mode) {
 848         case sd_data_transfer_mode:
 849             if (sd->rca != rca)
 850                 return sd_r0;
 851
 852             return sd_r1;
 853
 854         default:
 855             break;
 856         }
 857         break;
 858
 859     case 15:    /* CMD15:  GO_INACTIVE_STATE */
 860         if (sd->spi)
 861             goto bad_cmd;
 862         switch (sd->mode) {
 863         case sd_data_transfer_mode:
 864             if (sd->rca != rca)
 865                 return sd_r0;
 866
 867             sd->state = sd_inactive_state;
 868             return sd_r0;
 869
 870         default:
 871             break;
 872         }
 873         break;
 874
 875     /* Block read commands (Classs 2) */
 876     case 16:    /* CMD16:  SET_BLOCKLEN */
 877         switch (sd->state) {
 878         case sd_transfer_state:
 879             if (req.arg > (1 << HWBLOCK_SHIFT))
 880                 sd->card_status |= BLOCK_LEN_ERROR;
 881             else
 882                 sd->blk_len = req.arg;
 883
 884             return sd_r1;
 885
 886         default:
 887             break;
 888         }
 889         break;
 890
 891     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
 892         switch (sd->state) {
 893         case sd_transfer_state:
 894             sd->state = sd_sendingdata_state;
 895             sd->data_start = addr;
 896             sd->data_offset = 0;
 897
 898             if (sd->data_start + sd->blk_len > sd->size)
 899                 sd->card_status |= ADDRESS_ERROR;
 900             return sd_r1;
 901
 902         default:
 903             break;
 904         }
 905         break;
 906
 907     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
 908         switch (sd->state) {
 909         case sd_transfer_state:
 910             sd->state = sd_sendingdata_state;
 911             sd->data_start = addr;
 912             sd->data_offset = 0;
 913
 914             if (sd->data_start + sd->blk_len > sd->size)
 915                 sd->card_status |= ADDRESS_ERROR;
 916             return sd_r1;
 917
 918         default:
 919             break;
 920         }
 921         break;
 922
 923     /* Block write commands (Class 4) */
 924     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
 925         if (sd->spi)
 926             goto unimplemented_cmd;
 927         switch (sd->state) {
 928         case sd_transfer_state:
 929             /* Writing in SPI mode not implemented.  */
 930             if (sd->spi)
 931                 break;
 932             sd->state = sd_receivingdata_state;
 933             sd->data_start = addr;
 934             sd->data_offset = 0;
 935             sd->blk_written = 0;
 936
 937             if (sd->data_start + sd->blk_len > sd->size)
 938                 sd->card_status |= ADDRESS_ERROR;
 939             if (sd_wp_addr(sd, sd->data_start))
 940                 sd->card_status |= WP_VIOLATION;
 941             if (sd->csd[14] & 0x30)
 942                 sd->card_status |= WP_VIOLATION;
 943             return sd_r1;
 944
 945         default:
 946             break;
 947         }
 948         break;
 949
 950     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
 951         if (sd->spi)
 952             goto unimplemented_cmd;
 953         switch (sd->state) {
 954         case sd_transfer_state:
 955             /* Writing in SPI mode not implemented.  */
 956             if (sd->spi)
 957                 break;
 958             sd->state = sd_receivingdata_state;
 959             sd->data_start = addr;
 960             sd->data_offset = 0;
 961             sd->blk_written = 0;
 962
 963             if (sd->data_start + sd->blk_len > sd->size)
 964                 sd->card_status |= ADDRESS_ERROR;
 965             if (sd_wp_addr(sd, sd->data_start))
 966                 sd->card_status |= WP_VIOLATION;
 967             if (sd->csd[14] & 0x30)
 968                 sd->card_status |= WP_VIOLATION;
 969             return sd_r1;
 970
 971         default:
 972             break;
 973         }
 974         break;
 975
 976     case 26:    /* CMD26:  PROGRAM_CID */
 977         if (sd->spi)
 978             goto bad_cmd;
 979         switch (sd->state) {
 980         case sd_transfer_state:
 981             sd->state = sd_receivingdata_state;
 982             sd->data_start = 0;
 983             sd->data_offset = 0;
 984             return sd_r1;
 985
 986         default:
 987             break;
 988         }
 989         break;
 990
 991     case 27:    /* CMD27:  PROGRAM_CSD */
 992         if (sd->spi)
 993             goto unimplemented_cmd;
 994         switch (sd->state) {
 995         case sd_transfer_state:
 996             sd->state = sd_receivingdata_state;
 997             sd->data_start = 0;
 998             sd->data_offset = 0;
 999             return sd_r1;
1000
1001         default:
1002             break;
1003         }
1004         break;
1005
1006     /* Write protection (Class 6) */
1007     case 28:    /* CMD28:  SET_WRITE_PROT */
1008         switch (sd->state) {
1009         case sd_transfer_state:
1010             if (addr >= sd->size) {
1011                 sd->card_status |= ADDRESS_ERROR;
1012                 return sd_r1b;
1013             }
1014
1015             sd->state = sd_programming_state;
1016             set_bit(sd_addr_to_wpnum(addr), sd->wp_groups);
1017             /* Bzzzzzzztt .... Operation complete.  */
1018             sd->state = sd_transfer_state;
1019             return sd_r1b;
1020
1021         default:
1022             break;
1023         }
1024         break;
1025
1026     case 29:    /* CMD29:  CLR_WRITE_PROT */
1027         switch (sd->state) {
1028         case sd_transfer_state:
1029             if (addr >= sd->size) {
1030                 sd->card_status |= ADDRESS_ERROR;
1031                 return sd_r1b;
1032             }
1033
1034             sd->state = sd_programming_state;
1035             clear_bit(sd_addr_to_wpnum(addr), sd->wp_groups);
1036             /* Bzzzzzzztt .... Operation complete.  */
1037             sd->state = sd_transfer_state;
1038             return sd_r1b;
1039
1040         default:
1041             break;
1042         }
1043         break;
1044
1045     case 30:    /* CMD30:  SEND_WRITE_PROT */
1046         switch (sd->state) {
1047         case sd_transfer_state:
1048             sd->state = sd_sendingdata_state;
1049             *(uint32_t *) sd->data = sd_wpbits(sd, req.arg);
1050             sd->data_start = addr;
1051             sd->data_offset = 0;
1052             return sd_r1b;
1053
1054         default:
1055             break;
1056         }
1057         break;
1058
1059     /* Erase commands (Class 5) */
1060     case 32:    /* CMD32:  ERASE_WR_BLK_START */
1061         switch (sd->state) {
1062         case sd_transfer_state:
1063             sd->erase_start = req.arg;
1064             return sd_r1;
1065
1066         default:
1067             break;
1068         }
1069         break;
1070
1071     case 33:    /* CMD33:  ERASE_WR_BLK_END */
1072         switch (sd->state) {
1073         case sd_transfer_state:
1074             sd->erase_end = req.arg;
1075             return sd_r1;
1076
1077         default:
1078             break;
1079         }
1080         break;
1081
1082     case 38:    /* CMD38:  ERASE */
1083         switch (sd->state) {
1084         case sd_transfer_state:
1085             if (sd->csd[14] & 0x30) {
1086                 sd->card_status |= WP_VIOLATION;
1087                 return sd_r1b;
1088             }
1089
1090             sd->state = sd_programming_state;
1091             sd_erase(sd);
1092             /* Bzzzzzzztt .... Operation complete.  */
1093             sd->state = sd_transfer_state;
1094             return sd_r1b;
1095
1096         default:
1097             break;
1098         }
1099         break;
1100
1101     /* Lock card commands (Class 7) */
1102     case 42:    /* CMD42:  LOCK_UNLOCK */
1103         if (sd->spi)
1104             goto unimplemented_cmd;
1105         switch (sd->state) {
1106         case sd_transfer_state:
1107             sd->state = sd_receivingdata_state;
1108             sd->data_start = 0;
1109             sd->data_offset = 0;
1110             return sd_r1;
1111
1112         default:
1113             break;
1114         }
1115         break;
1116
1117     case 52:
1118     case 53:
1119         /* CMD52, CMD53: reserved for SDIO cards
1120          * (see the SDIO Simplified Specification V2.0)
1121          * Handle as illegal command but do not complain
1122          * on stderr, as some OSes may use these in their
1123          * probing for presence of an SDIO card.
1124          */
1125         return sd_illegal;
1126
1127     /* Application specific commands (Class 8) */
1128     case 55:    /* CMD55:  APP_CMD */
1129         if (sd->rca != rca)
1130             return sd_r0;
1131
1132         sd->expecting_acmd = true;
1133         sd->card_status |= APP_CMD;
1134         return sd_r1;
1135
1136     case 56:    /* CMD56:  GEN_CMD */
1137         fprintf(stderr, "SD: GEN_CMD 0x%08x\n", req.arg);
1138
1139         switch (sd->state) {
1140         case sd_transfer_state:
1141             sd->data_offset = 0;
1142             if (req.arg & 1)
1143                 sd->state = sd_sendingdata_state;
1144             else
1145                 sd->state = sd_receivingdata_state;
1146             return sd_r1;
1147
1148         default:
1149             break;
1150         }
1151         break;
1152
1153     default:
1154     bad_cmd:
1155         fprintf(stderr, "SD: Unknown CMD%i\n", req.cmd);
1156         return sd_illegal;
1157
1158     unimplemented_cmd:
1159         /* Commands that are recognised but not yet implemented in SPI mode.  */
1160         fprintf(stderr, "SD: CMD%i not implemented in SPI mode\n", req.cmd);
1161         return sd_illegal;
1162     }
1163
1164     fprintf(stderr, "SD: CMD%i in a wrong state\n", req.cmd);
1165     return sd_illegal;
1166 }
1167
1168 static sd_rsp_type_t sd_app_command(SDState *sd,
1169                                     SDRequest req)
1170 {
1171     DPRINTF("ACMD%d 0x%08x\n", req.cmd, req.arg);
1172     sd->card_status |= APP_CMD;
1173     switch (req.cmd) {
1174     case 6:     /* ACMD6:  SET_BUS_WIDTH */
1175         switch (sd->state) {
1176         case sd_transfer_state:
1177             sd->sd_status[0] &= 0x3f;
1178             sd->sd_status[0] |= (req.arg & 0x03) << 6;
1179             return sd_r1;
1180
1181         default:
1182             break;
1183         }
1184         break;
1185
1186     case 13:    /* ACMD13: SD_STATUS */
1187         switch (sd->state) {
1188         case sd_transfer_state:
1189             sd->state = sd_sendingdata_state;
1190             sd->data_start = 0;
1191             sd->data_offset = 0;
1192             return sd_r1;
1193
1194         default:
1195             break;
1196         }
1197         break;
1198
1199     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1200         switch (sd->state) {
1201         case sd_transfer_state:
1202             *(uint32_t *) sd->data = sd->blk_written;
1203
1204             sd->state = sd_sendingdata_state;
1205             sd->data_start = 0;
1206             sd->data_offset = 0;
1207             return sd_r1;
1208
1209         default:
1210             break;
1211         }
1212         break;
1213
1214     case 23:    /* ACMD23: SET_WR_BLK_ERASE_COUNT */
1215         switch (sd->state) {
1216         case sd_transfer_state:
1217             return sd_r1;
1218
1219         default:
1220             break;
1221         }
1222         break;
1223
1224     case 41:    /* ACMD41: SD_APP_OP_COND */
1225         if (sd->spi) {
1226             /* SEND_OP_CMD */
1227             sd->state = sd_transfer_state;
1228             return sd_r1;
1229         }
1230         switch (sd->state) {
1231         case sd_idle_state:
1232             /* We accept any voltage.  10000 V is nothing.  */
1233             if (req.arg)
1234                 sd->state = sd_ready_state;
1235
1236             return sd_r3;
1237
1238         default:
1239             break;
1240         }
1241         break;
1242
1243     case 42:    /* ACMD42: SET_CLR_CARD_DETECT */
1244         switch (sd->state) {
1245         case sd_transfer_state:
1246             /* Bringing in the 50KOhm pull-up resistor... Done.  */
1247             return sd_r1;
1248
1249         default:
1250             break;
1251         }
1252         break;
1253
1254     case 51:    /* ACMD51: SEND_SCR */
1255         switch (sd->state) {
1256         case sd_transfer_state:
1257             sd->state = sd_sendingdata_state;
1258             sd->data_start = 0;
1259             sd->data_offset = 0;
1260             return sd_r1;
1261
1262         default:
1263             break;
1264         }
1265         break;
1266
1267     default:
1268         /* Fall back to standard commands.  */
1269         return sd_normal_command(sd, req);
1270     }
1271
1272     fprintf(stderr, "SD: ACMD%i in a wrong state\n", req.cmd);
1273     return sd_illegal;
1274 }
1275
1276 static int cmd_valid_while_locked(SDState *sd, SDRequest *req)
1277 {
1278     /* Valid commands in locked state:
1279      * basic class (0)
1280      * lock card class (7)
1281      * CMD16
1282      * implicitly, the ACMD prefix CMD55
1283      * ACMD41 and ACMD42
1284      * Anything else provokes an "illegal command" response.
1285      */
1286     if (sd->expecting_acmd) {
1287         return req->cmd == 41 || req->cmd == 42;
1288     }
1289     if (req->cmd == 16 || req->cmd == 55) {
1290         return 1;
1291     }
1292     return sd_cmd_class[req->cmd] == 0 || sd_cmd_class[req->cmd] == 7;
1293 }
1294
1295 int sd_do_command(SDState *sd, SDRequest *req,
1296                   uint8_t *response) {
1297     int last_state;
1298     sd_rsp_type_t rtype;
1299     int rsplen;
1300
1301     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv) || !sd->enable) {
1302         return 0;
1303     }
1304
1305     if (sd_req_crc_validate(req)) {
1306         sd->card_status |= COM_CRC_ERROR;
1307         rtype = sd_illegal;
1308         goto send_response;
1309     }
1310
1311     if (sd->card_status & CARD_IS_LOCKED) {
1312         if (!cmd_valid_while_locked(sd, req)) {
1313             sd->card_status |= ILLEGAL_COMMAND;
1314             sd->expecting_acmd = false;
1315             fprintf(stderr, "SD: Card is locked\n");
1316             rtype = sd_illegal;
1317             goto send_response;
1318         }
1319     }
1320
1321     last_state = sd->state;
1322     sd_set_mode(sd);
1323
1324     if (sd->expecting_acmd) {
1325         sd->expecting_acmd = false;
1326         rtype = sd_app_command(sd, *req);
1327     } else {
1328         rtype = sd_normal_command(sd, *req);
1329     }
1330
1331     if (rtype == sd_illegal) {
1332         sd->card_status |= ILLEGAL_COMMAND;
1333     } else {
1334         /* Valid command, we can update the 'state before command' bits.
1335          * (Do this now so they appear in r1 responses.)
1336          */
1337         sd->current_cmd = req->cmd;
1338         sd->card_status &= ~CURRENT_STATE;
1339         sd->card_status |= (last_state << 9);
1340     }
1341
1342 send_response:
1343     switch (rtype) {
1344     case sd_r1:
1345     case sd_r1b:
1346         sd_response_r1_make(sd, response);
1347         rsplen = 4;
1348         break;
1349
1350     case sd_r2_i:
1351         memcpy(response, sd->cid, sizeof(sd->cid));
1352         rsplen = 16;
1353         break;
1354
1355     case sd_r2_s:
1356         memcpy(response, sd->csd, sizeof(sd->csd));
1357         rsplen = 16;
1358         break;
1359
1360     case sd_r3:
1361         sd_response_r3_make(sd, response);
1362         rsplen = 4;
1363         break;
1364
1365     case sd_r6:
1366         sd_response_r6_make(sd, response);
1367         rsplen = 4;
1368         break;
1369
1370     case sd_r7:
1371         sd_response_r7_make(sd, response);
1372         rsplen = 4;
1373         break;
1374
1375     case sd_r0:
1376     case sd_illegal:
1377     default:
1378         rsplen = 0;
1379         break;
1380     }
1381
1382     if (rtype != sd_illegal) {
1383         /* Clear the "clear on valid command" status bits now we've
1384          * sent any response
1385          */
1386         sd->card_status &= ~CARD_STATUS_B;
1387     }
1388
1389 #ifdef DEBUG_SD
1390     if (rsplen) {
1391         int i;
1392         DPRINTF("Response:");
1393         for (i = 0; i < rsplen; i++)
1394             printf(" %02x", response[i]);
1395         printf(" state %d\n", sd->state);
1396     } else {
1397         DPRINTF("No response %d\n", sd->state);
1398     }
1399 #endif
1400
1401     return rsplen;
1402 }
1403
1404 static void sd_blk_read(SDState *sd, uint64_t addr, uint32_t len)
1405 {
1406     uint64_t end = addr + len;
1407
1408     DPRINTF("sd_blk_read: addr = 0x%08llx, len = %d\n",
1409             (unsigned long long) addr, len);
1410     if (!sd->bdrv || bdrv_read(sd->bdrv, addr >> 9, sd->buf, 1) == -1) {
1411         fprintf(stderr, "sd_blk_read: read error on host side\n");
1412         return;
1413     }
1414
1415     if (end > (addr & ~511) + 512) {
1416         memcpy(sd->data, sd->buf + (addr & 511), 512 - (addr & 511));
1417
1418         if (bdrv_read(sd->bdrv, end >> 9, sd->buf, 1) == -1) {
1419             fprintf(stderr, "sd_blk_read: read error on host side\n");
1420             return;
1421         }
1422         memcpy(sd->data + 512 - (addr & 511), sd->buf, end & 511);
1423     } else
1424         memcpy(sd->data, sd->buf + (addr & 511), len);
1425 }
1426
1427 static void sd_blk_write(SDState *sd, uint64_t addr, uint32_t len)
1428 {
1429     uint64_t end = addr + len;
1430
1431     if ((addr & 511) || len < 512)
1432         if (!sd->bdrv || bdrv_read(sd->bdrv, addr >> 9, sd->buf, 1) == -1) {
1433             fprintf(stderr, "sd_blk_write: read error on host side\n");
1434             return;
1435         }
1436
1437     if (end > (addr & ~511) + 512) {
1438         memcpy(sd->buf + (addr & 511), sd->data, 512 - (addr & 511));
1439         if (bdrv_write(sd->bdrv, addr >> 9, sd->buf, 1) == -1) {
1440             fprintf(stderr, "sd_blk_write: write error on host side\n");
1441             return;
1442         }
1443
1444         if (bdrv_read(sd->bdrv, end >> 9, sd->buf, 1) == -1) {
1445             fprintf(stderr, "sd_blk_write: read error on host side\n");
1446             return;
1447         }
1448         memcpy(sd->buf, sd->data + 512 - (addr & 511), end & 511);
1449         if (bdrv_write(sd->bdrv, end >> 9, sd->buf, 1) == -1)
1450             fprintf(stderr, "sd_blk_write: write error on host side\n");
1451     } else {
1452         memcpy(sd->buf + (addr & 511), sd->data, len);
1453         if (!sd->bdrv || bdrv_write(sd->bdrv, addr >> 9, sd->buf, 1) == -1)
1454             fprintf(stderr, "sd_blk_write: write error on host side\n");
1455     }
1456 }
1457
1458 #define BLK_READ_BLOCK(a, len)  sd_blk_read(sd, a, len)
1459 #define BLK_WRITE_BLOCK(a, len) sd_blk_write(sd, a, len)
1460 #define APP_READ_BLOCK(a, len)  memset(sd->data, 0xec, len)
1461 #define APP_WRITE_BLOCK(a, len)
1462
1463 void sd_write_data(SDState *sd, uint8_t value)
1464 {
1465     int i;
1466
1467     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv) || !sd->enable)
1468         return;
1469
1470     if (sd->state != sd_receivingdata_state) {
1471         fprintf(stderr, "sd_write_data: not in Receiving-Data state\n");
1472         return;
1473     }
1474
1475     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1476         return;
1477
1478     switch (sd->current_cmd) {
1479     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
1480         sd->data[sd->data_offset ++] = value;
1481         if (sd->data_offset >= sd->blk_len) {
1482             /* TODO: Check CRC before committing */
1483             sd->state = sd_programming_state;
1484             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1485             sd->blk_written ++;
1486             sd->csd[14] |= 0x40;
1487             /* Bzzzzzzztt .... Operation complete.  */
1488             sd->state = sd_transfer_state;
1489         }
1490         break;
1491
1492     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
1493         if (sd->data_offset == 0) {
1494             /* Start of the block - lets check the address is valid */
1495             if (sd->data_start + sd->blk_len > sd->size) {
1496                 sd->card_status |= ADDRESS_ERROR;
1497                 break;
1498             }
1499             if (sd_wp_addr(sd, sd->data_start)) {
1500                 sd->card_status |= WP_VIOLATION;
1501                 break;
1502             }
1503         }
1504         sd->data[sd->data_offset++] = value;
1505         if (sd->data_offset >= sd->blk_len) {
1506             /* TODO: Check CRC before committing */
1507             sd->state = sd_programming_state;
1508             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1509             sd->blk_written++;
1510             sd->data_start += sd->blk_len;
1511             sd->data_offset = 0;
1512             sd->csd[14] |= 0x40;
1513
1514             /* Bzzzzzzztt .... Operation complete.  */
1515             sd->state = sd_receivingdata_state;
1516         }
1517         break;
1518
1519     case 26:    /* CMD26:  PROGRAM_CID */
1520         sd->data[sd->data_offset ++] = value;
1521         if (sd->data_offset >= sizeof(sd->cid)) {
1522             /* TODO: Check CRC before committing */
1523             sd->state = sd_programming_state;
1524             for (i = 0; i < sizeof(sd->cid); i ++)
1525                 if ((sd->cid[i] | 0x00) != sd->data[i])
1526                     sd->card_status |= CID_CSD_OVERWRITE;
1527
1528             if (!(sd->card_status & CID_CSD_OVERWRITE))
1529                 for (i = 0; i < sizeof(sd->cid); i ++) {
1530                     sd->cid[i] |= 0x00;
1531                     sd->cid[i] &= sd->data[i];
1532                 }
1533             /* Bzzzzzzztt .... Operation complete.  */
1534             sd->state = sd_transfer_state;
1535         }
1536         break;
1537
1538     case 27:    /* CMD27:  PROGRAM_CSD */
1539         sd->data[sd->data_offset ++] = value;
1540         if (sd->data_offset >= sizeof(sd->csd)) {
1541             /* TODO: Check CRC before committing */
1542             sd->state = sd_programming_state;
1543             for (i = 0; i < sizeof(sd->csd); i ++)
1544                 if ((sd->csd[i] | sd_csd_rw_mask[i]) !=
1545                     (sd->data[i] | sd_csd_rw_mask[i]))
1546                     sd->card_status |= CID_CSD_OVERWRITE;
1547
1548             /* Copy flag (OTP) & Permanent write protect */
1549             if (sd->csd[14] & ~sd->data[14] & 0x60)
1550                 sd->card_status |= CID_CSD_OVERWRITE;
1551
1552             if (!(sd->card_status & CID_CSD_OVERWRITE))
1553                 for (i = 0; i < sizeof(sd->csd); i ++) {
1554                     sd->csd[i] |= sd_csd_rw_mask[i];
1555                     sd->csd[i] &= sd->data[i];
1556                 }
1557             /* Bzzzzzzztt .... Operation complete.  */
1558             sd->state = sd_transfer_state;
1559         }
1560         break;
1561
1562     case 42:    /* CMD42:  LOCK_UNLOCK */
1563         sd->data[sd->data_offset ++] = value;
1564         if (sd->data_offset >= sd->blk_len) {
1565             /* TODO: Check CRC before committing */
1566             sd->state = sd_programming_state;
1567             sd_lock_command(sd);
1568             /* Bzzzzzzztt .... Operation complete.  */
1569             sd->state = sd_transfer_state;
1570         }
1571         break;
1572
1573     case 56:    /* CMD56:  GEN_CMD */
1574         sd->data[sd->data_offset ++] = value;
1575         if (sd->data_offset >= sd->blk_len) {
1576             APP_WRITE_BLOCK(sd->data_start, sd->data_offset);
1577             sd->state = sd_transfer_state;
1578         }
1579         break;
1580
1581     default:
1582         fprintf(stderr, "sd_write_data: unknown command\n");
1583         break;
1584     }
1585 }
1586
1587 uint8_t sd_read_data(SDState *sd)
1588 {
1589     /* TODO: Append CRCs */
1590     uint8_t ret;
1591     int io_len;
1592
1593     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv) || !sd->enable)
1594         return 0x00;
1595
1596     if (sd->state != sd_sendingdata_state) {
1597         fprintf(stderr, "sd_read_data: not in Sending-Data state\n");
1598         return 0x00;
1599     }
1600
1601     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1602         return 0x00;
1603
1604     io_len = (sd->ocr & (1 << 30)) ? 512 : sd->blk_len;
1605
1606     switch (sd->current_cmd) {
1607     case 6:     /* CMD6:   SWITCH_FUNCTION */
1608         ret = sd->data[sd->data_offset ++];
1609
1610         if (sd->data_offset >= 64)
1611             sd->state = sd_transfer_state;
1612         break;
1613
1614     case 9:     /* CMD9:   SEND_CSD */
1615     case 10:    /* CMD10:  SEND_CID */
1616         ret = sd->data[sd->data_offset ++];
1617
1618         if (sd->data_offset >= 16)
1619             sd->state = sd_transfer_state;
1620         break;
1621
1622     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
1623         if (sd->data_offset == 0)
1624             BLK_READ_BLOCK(sd->data_start, io_len);
1625         ret = sd->data[sd->data_offset ++];
1626
1627         if (sd->data_offset >= io_len) {
1628             sd->data_start += io_len;
1629             sd->data_offset = 0;
1630             if (sd->data_start + io_len > sd->size) {
1631                 sd->card_status |= ADDRESS_ERROR;
1632                 break;
1633             }
1634         }
1635         break;
1636
1637     case 13:    /* ACMD13: SD_STATUS */
1638         ret = sd->sd_status[sd->data_offset ++];
1639
1640         if (sd->data_offset >= sizeof(sd->sd_status))
1641             sd->state = sd_transfer_state;
1642         break;
1643
1644     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
1645         if (sd->data_offset == 0)
1646             BLK_READ_BLOCK(sd->data_start, io_len);
1647         ret = sd->data[sd->data_offset ++];
1648
1649         if (sd->data_offset >= io_len)
1650             sd->state = sd_transfer_state;
1651         break;
1652
1653     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
1654         if (sd->data_offset == 0)
1655             BLK_READ_BLOCK(sd->data_start, io_len);
1656         ret = sd->data[sd->data_offset ++];
1657
1658         if (sd->data_offset >= io_len) {
1659             sd->data_start += io_len;
1660             sd->data_offset = 0;
1661             if (sd->data_start + io_len > sd->size) {
1662                 sd->card_status |= ADDRESS_ERROR;
1663                 break;
1664             }
1665         }
1666         break;
1667
1668     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1669         ret = sd->data[sd->data_offset ++];
1670
1671         if (sd->data_offset >= 4)
1672             sd->state = sd_transfer_state;
1673         break;
1674
1675     case 30:    /* CMD30:  SEND_WRITE_PROT */
1676         ret = sd->data[sd->data_offset ++];
1677
1678         if (sd->data_offset >= 4)
1679             sd->state = sd_transfer_state;
1680         break;
1681
1682     case 51:    /* ACMD51: SEND_SCR */
1683         ret = sd->scr[sd->data_offset ++];
1684
1685         if (sd->data_offset >= sizeof(sd->scr))
1686             sd->state = sd_transfer_state;
1687         break;
1688
1689     case 56:    /* CMD56:  GEN_CMD */
1690         if (sd->data_offset == 0)
1691             APP_READ_BLOCK(sd->data_start, sd->blk_len);
1692         ret = sd->data[sd->data_offset ++];
1693
1694         if (sd->data_offset >= sd->blk_len)
1695             sd->state = sd_transfer_state;
1696         break;
1697
1698     default:
1699         fprintf(stderr, "sd_read_data: unknown command\n");
1700         return 0x00;
1701     }
1702
1703     return ret;
1704 }
1705
1706 bool sd_data_ready(SDState *sd)
1707 {
1708     return sd->state == sd_sendingdata_state;
1709 }
1710
1711 void sd_enable(SDState *sd, bool enable)
1712 {
1713     sd->enable = enable;
1714 }