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