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