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