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