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