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