hw/sd.c

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