vcs.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     else
1157         rtype = sd_normal_command(sd, *req);
1158
1159     sd->current_cmd = req->cmd;
1160
1161     switch (rtype) {
1162     case sd_r1:
1163     case sd_r1b:
1164         sd_response_r1_make(sd, response, last_status);
1165         rsplen = 4;
1166         break;
1167
1168     case sd_r2_i:
1169         memcpy(response, sd->cid, sizeof(sd->cid));
1170         response[7] |= 1;
1171         rsplen = 16;
1172         break;
1173
1174     case sd_r2_s:
1175         memcpy(response, sd->csd, sizeof(sd->csd));
1176         response[7] |= 1;
1177         rsplen = 16;
1178         break;
1179
1180     case sd_r3:
1181         sd_response_r3_make(sd, response);
1182         rsplen = 4;
1183         break;
1184
1185     case sd_r6:
1186         sd_response_r6_make(sd, response);
1187         rsplen = 4;
1188         break;
1189
1190     case sd_r0:
1191     default:
1192         rsplen = 0;
1193         break;
1194     }
1195
1196     if (sd->card_status & ILLEGAL_COMMAND)
1197         rsplen = 0;
1198
1199 #ifdef DEBUG_SD
1200     if (rsplen) {
1201         int i;
1202         DPRINTF("Response:");
1203         for (i = 0; i < rsplen; i++)
1204             printf(" %02x", response[i]);
1205         printf(" state %d\n", sd->state);
1206     } else {
1207         DPRINTF("No response %d\n", sd->state);
1208     }
1209 #endif
1210
1211     return rsplen;
1212 }
1213
1214 /* No real need for 64 bit addresses here */
1215 static void sd_blk_read(BlockDriverState *bdrv,
1216                 void *data, uint32_t addr, uint32_t len)
1217 {
1218     uint8_t buf[512];
1219     uint32_t end = addr + len;
1220
1221     if (!bdrv || bdrv_read(bdrv, addr >> 9, buf, 1) == -1) {
1222         printf("sd_blk_read: read error on host side\n");
1223         return;
1224     }
1225
1226     if (end > (addr & ~511) + 512) {
1227         memcpy(data, buf + (addr & 511), 512 - (addr & 511));
1228
1229         if (bdrv_read(bdrv, end >> 9, buf, 1) == -1) {
1230             printf("sd_blk_read: read error on host side\n");
1231             return;
1232         }
1233         memcpy(data + 512 - (addr & 511), buf, end & 511);
1234     } else
1235         memcpy(data, buf + (addr & 511), len);
1236 }
1237
1238 static void sd_blk_write(BlockDriverState *bdrv,
1239                 void *data, uint32_t addr, uint32_t len)
1240 {
1241     uint8_t buf[512];
1242     uint32_t end = addr + len;
1243
1244     if ((addr & 511) || len < 512)
1245         if (!bdrv || bdrv_read(bdrv, addr >> 9, buf, 1) == -1) {
1246             printf("sd_blk_write: read error on host side\n");
1247             return;
1248         }
1249
1250     if (end > (addr & ~511) + 512) {
1251         memcpy(buf + (addr & 511), data, 512 - (addr & 511));
1252         if (bdrv_write(bdrv, addr >> 9, buf, 1) == -1) {
1253             printf("sd_blk_write: write error on host side\n");
1254             return;
1255         }
1256
1257         if (bdrv_read(bdrv, end >> 9, buf, 1) == -1) {
1258             printf("sd_blk_write: read error on host side\n");
1259             return;
1260         }
1261         memcpy(buf, data + 512 - (addr & 511), end & 511);
1262         if (bdrv_write(bdrv, end >> 9, buf, 1) == -1)
1263             printf("sd_blk_write: write error on host side\n");
1264     } else {
1265         memcpy(buf + (addr & 511), data, len);
1266         if (!bdrv || bdrv_write(bdrv, addr >> 9, buf, 1) == -1)
1267             printf("sd_blk_write: write error on host side\n");
1268     }
1269 }
1270
1271 #define BLK_READ_BLOCK(a, len)  sd_blk_read(sd->bdrv, sd->data, a, len)
1272 #define BLK_WRITE_BLOCK(a, len) sd_blk_write(sd->bdrv, sd->data, a, len)
1273 #define APP_READ_BLOCK(a, len)  memset(sd->data, 0xec, len)
1274 #define APP_WRITE_BLOCK(a, len)
1275
1276 void sd_write_data(SDState *sd, uint8_t value)
1277 {
1278     int i;
1279
1280     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv))
1281         return;
1282
1283     if (sd->state != sd_receivingdata_state) {
1284         printf("sd_write_data: not in Receiving-Data state\n");
1285         return;
1286     }
1287
1288     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1289         return;
1290
1291     switch (sd->current_cmd) {
1292     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
1293         sd->data[sd->data_offset ++] = value;
1294         if (sd->data_offset >= sd->blk_len) {
1295             /* TODO: Check CRC before committing */
1296             sd->state = sd_programming_state;
1297             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1298             sd->blk_written ++;
1299             sd->csd[14] |= 0x40;
1300             /* Bzzzzzzztt .... Operation complete.  */
1301             sd->state = sd_transfer_state;
1302         }
1303         break;
1304
1305     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
1306         sd->data[sd->data_offset ++] = value;
1307         if (sd->data_offset >= sd->blk_len) {
1308             /* TODO: Check CRC before committing */
1309             sd->state = sd_programming_state;
1310             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1311             sd->blk_written ++;
1312             sd->data_start += sd->blk_len;
1313             sd->data_offset = 0;
1314             if (sd->data_start + sd->blk_len > sd->size) {
1315                 sd->card_status |= ADDRESS_ERROR;
1316                 break;
1317             }
1318             if (sd_wp_addr(sd, sd->data_start)) {
1319                 sd->card_status |= WP_VIOLATION;
1320                 break;
1321             }
1322             sd->csd[14] |= 0x40;
1323
1324             /* Bzzzzzzztt .... Operation complete.  */
1325             sd->state = sd_receivingdata_state;
1326         }
1327         break;
1328
1329     case 26:    /* CMD26:  PROGRAM_CID */
1330         sd->data[sd->data_offset ++] = value;
1331         if (sd->data_offset >= sizeof(sd->cid)) {
1332             /* TODO: Check CRC before committing */
1333             sd->state = sd_programming_state;
1334             for (i = 0; i < sizeof(sd->cid); i ++)
1335                 if ((sd->cid[i] | 0x00) != sd->data[i])
1336                     sd->card_status |= CID_CSD_OVERWRITE;
1337
1338             if (!(sd->card_status & CID_CSD_OVERWRITE))
1339                 for (i = 0; i < sizeof(sd->cid); i ++) {
1340                     sd->cid[i] |= 0x00;
1341                     sd->cid[i] &= sd->data[i];
1342                 }
1343             /* Bzzzzzzztt .... Operation complete.  */
1344             sd->state = sd_transfer_state;
1345         }
1346         break;
1347
1348     case 27:    /* CMD27:  PROGRAM_CSD */
1349         sd->data[sd->data_offset ++] = value;
1350         if (sd->data_offset >= sizeof(sd->csd)) {
1351             /* TODO: Check CRC before committing */
1352             sd->state = sd_programming_state;
1353             for (i = 0; i < sizeof(sd->csd); i ++)
1354                 if ((sd->csd[i] | sd_csd_rw_mask[i]) !=
1355                     (sd->data[i] | sd_csd_rw_mask[i]))
1356                     sd->card_status |= CID_CSD_OVERWRITE;
1357
1358             /* Copy flag (OTP) & Permanent write protect */
1359             if (sd->csd[14] & ~sd->data[14] & 0x60)
1360                 sd->card_status |= CID_CSD_OVERWRITE;
1361
1362             if (!(sd->card_status & CID_CSD_OVERWRITE))
1363                 for (i = 0; i < sizeof(sd->csd); i ++) {
1364                     sd->csd[i] |= sd_csd_rw_mask[i];
1365                     sd->csd[i] &= sd->data[i];
1366                 }
1367             /* Bzzzzzzztt .... Operation complete.  */
1368             sd->state = sd_transfer_state;
1369         }
1370         break;
1371
1372     case 42:    /* CMD42:  LOCK_UNLOCK */
1373         sd->data[sd->data_offset ++] = value;
1374         if (sd->data_offset >= sd->blk_len) {
1375             /* TODO: Check CRC before committing */
1376             sd->state = sd_programming_state;
1377             sd_lock_command(sd);
1378             /* Bzzzzzzztt .... Operation complete.  */
1379             sd->state = sd_transfer_state;
1380         }
1381         break;
1382
1383     case 56:    /* CMD56:  GEN_CMD */
1384         sd->data[sd->data_offset ++] = value;
1385         if (sd->data_offset >= sd->blk_len) {
1386             APP_WRITE_BLOCK(sd->data_start, sd->data_offset);
1387             sd->state = sd_transfer_state;
1388         }
1389         break;
1390
1391     default:
1392         printf("sd_write_data: unknown command\n");
1393         break;
1394     }
1395 }
1396
1397 uint8_t sd_read_data(SDState *sd)
1398 {
1399     /* TODO: Append CRCs */
1400     uint8_t ret;
1401
1402     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv))
1403         return 0x00;
1404
1405     if (sd->state != sd_sendingdata_state) {
1406         printf("sd_read_data: not in Sending-Data state\n");
1407         return 0x00;
1408     }
1409
1410     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1411         return 0x00;
1412
1413     switch (sd->current_cmd) {
1414     case 6:     /* CMD6:   SWITCH_FUNCTION */
1415         ret = sd->data[sd->data_offset ++];
1416
1417         if (sd->data_offset >= 64)
1418             sd->state = sd_transfer_state;
1419         break;
1420
1421     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
1422         if (sd->data_offset == 0)
1423             BLK_READ_BLOCK(sd->data_start, sd->blk_len);
1424         ret = sd->data[sd->data_offset ++];
1425
1426         if (sd->data_offset >= sd->blk_len) {
1427             sd->data_start += sd->blk_len;
1428             sd->data_offset = 0;
1429             if (sd->data_start + sd->blk_len > sd->size) {
1430                 sd->card_status |= ADDRESS_ERROR;
1431                 break;
1432             }
1433         }
1434         break;
1435
1436     case 13:    /* ACMD13: SD_STATUS */
1437         ret = sd->sd_status[sd->data_offset ++];
1438
1439         if (sd->data_offset >= sizeof(sd->sd_status))
1440             sd->state = sd_transfer_state;
1441         break;
1442
1443     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
1444         if (sd->data_offset == 0)
1445             BLK_READ_BLOCK(sd->data_start, sd->blk_len);
1446         ret = sd->data[sd->data_offset ++];
1447
1448         if (sd->data_offset >= sd->blk_len)
1449             sd->state = sd_transfer_state;
1450         break;
1451
1452     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
1453         if (sd->data_offset == 0)
1454             BLK_READ_BLOCK(sd->data_start, sd->blk_len);
1455         ret = sd->data[sd->data_offset ++];
1456
1457         if (sd->data_offset >= sd->blk_len) {
1458             sd->data_start += sd->blk_len;
1459             sd->data_offset = 0;
1460             if (sd->data_start + sd->blk_len > sd->size) {
1461                 sd->card_status |= ADDRESS_ERROR;
1462                 break;
1463             }
1464         }
1465         break;
1466
1467     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1468         ret = sd->data[sd->data_offset ++];
1469
1470         if (sd->data_offset >= 4)
1471             sd->state = sd_transfer_state;
1472         break;
1473
1474     case 30:    /* CMD30:  SEND_WRITE_PROT */
1475         ret = sd->data[sd->data_offset ++];
1476
1477         if (sd->data_offset >= 4)
1478             sd->state = sd_transfer_state;
1479         break;
1480
1481     case 51:    /* ACMD51: SEND_SCR */
1482         ret = sd->scr[sd->data_offset ++];
1483
1484         if (sd->data_offset >= sizeof(sd->scr))
1485             sd->state = sd_transfer_state;
1486         break;
1487
1488     case 56:    /* CMD56:  GEN_CMD */
1489         if (sd->data_offset == 0)
1490             APP_READ_BLOCK(sd->data_start, sd->blk_len);
1491         ret = sd->data[sd->data_offset ++];
1492
1493         if (sd->data_offset >= sd->blk_len)
1494             sd->state = sd_transfer_state;
1495         break;
1496
1497     default:
1498         printf("sd_read_data: unknown command\n");
1499         return 0x00;
1500     }
1501
1502     return ret;
1503 }
1504
1505 int sd_data_ready(SDState *sd)
1506 {
1507     return sd->state == sd_sendingdata_state;
1508 }