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