hw/sd.c

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