hw/sd.c

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