hw/sd.c

   1 /*
   2  * SD Memory Card emulation as defined in the "SD Memory Card Physical
   3  * layer specification, Version 1.10."
   4  *
   5  * Copyright (c) 2006 Andrzej Zaborowski  <[email protected]>
   6  * Copyright (c) 2007 CodeSourcery
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  *
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in
  16  *    the documentation and/or other materials provided with the
  17  *    distribution.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS''
  20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  21  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  22  * PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR
  23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  24  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  25  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  26  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
  27  * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  28  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 #include "hw.h"
  33 #include "block.h"
  34 #include "sd.h"
  35
  36 //#define DEBUG_SD 1
  37
  38 #ifdef DEBUG_SD
  39 #define DPRINTF(fmt, ...) \
  40 do { fprintf(stderr, "SD: " fmt , ## __VA_ARGS__); } while (0)
  41 #else
  42 #define DPRINTF(fmt, ...) do {} while(0)
  43 #endif
  44
  45 typedef enum {
  46     sd_r0 = 0,    /* no response */
  47     sd_r1,        /* normal response command */
  48     sd_r2_i,      /* CID register */
  49     sd_r2_s,      /* CSD register */
  50     sd_r3,        /* OCR register */
  51     sd_r6 = 6,    /* Published RCA response */
  52     sd_r7,        /* Operating voltage */
  53     sd_r1b = -1,
  54     sd_illegal = -2,
  55 } sd_rsp_type_t;
  56
  57 struct SDState {
  58     enum {
  59         sd_inactive,
  60         sd_card_identification_mode,
  61         sd_data_transfer_mode,
  62     } mode;
  63     enum {
  64         sd_inactive_state = -1,
  65         sd_idle_state = 0,
  66         sd_ready_state,
  67         sd_identification_state,
  68         sd_standby_state,
  69         sd_transfer_state,
  70         sd_sendingdata_state,
  71         sd_receivingdata_state,
  72         sd_programming_state,
  73         sd_disconnect_state,
  74     } state;
  75     uint32_t ocr;
  76     uint8_t scr[8];
  77     uint8_t cid[16];
  78     uint8_t csd[16];
  79     uint16_t rca;
  80     uint32_t card_status;
  81     uint8_t sd_status[64];
  82     uint32_t vhs;
  83     int wp_switch;
  84     int *wp_groups;
  85     uint64_t size;
  86     int blk_len;
  87     uint32_t erase_start;
  88     uint32_t erase_end;
  89     uint8_t pwd[16];
  90     int pwd_len;
  91     int function_group[6];
  92
  93     int spi;
  94     int current_cmd;
  95     /* True if we will handle the next command as an ACMD. Note that this does
  96      * *not* track the APP_CMD status bit!
  97      */
  98     int expecting_acmd;
  99     int blk_written;
 100     uint64_t data_start;
 101     uint32_t data_offset;
 102     uint8_t data[512];
 103     qemu_irq readonly_cb;
 104     qemu_irq inserted_cb;
 105     BlockDriverState *bdrv;
 106     uint8_t *buf;
 107
 108     int enable;
 109 };
 110
 111 static void sd_set_mode(SDState *sd)
 112 {
 113     switch (sd->state) {
 114     case sd_inactive_state:
 115         sd->mode = sd_inactive;
 116         break;
 117
 118     case sd_idle_state:
 119     case sd_ready_state:
 120     case sd_identification_state:
 121         sd->mode = sd_card_identification_mode;
 122         break;
 123
 124     case sd_standby_state:
 125     case sd_transfer_state:
 126     case sd_sendingdata_state:
 127     case sd_receivingdata_state:
 128     case sd_programming_state:
 129     case sd_disconnect_state:
 130         sd->mode = sd_data_transfer_mode;
 131         break;
 132     }
 133 }
 134
 135 static const sd_cmd_type_t sd_cmd_type[64] = {
 136     sd_bc,   sd_none, sd_bcr,  sd_bcr,  sd_none, sd_none, sd_none, sd_ac,
 137     sd_bcr,  sd_ac,   sd_ac,   sd_adtc, sd_ac,   sd_ac,   sd_none, sd_ac,
 138     sd_ac,   sd_adtc, sd_adtc, sd_none, sd_none, sd_none, sd_none, sd_none,
 139     sd_adtc, sd_adtc, sd_adtc, sd_adtc, sd_ac,   sd_ac,   sd_adtc, sd_none,
 140     sd_ac,   sd_ac,   sd_none, sd_none, sd_none, sd_none, sd_ac,   sd_none,
 141     sd_none, sd_none, sd_bc,   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_ac,
 143     sd_adtc, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 144 };
 145
 146 static const sd_cmd_type_t sd_acmd_type[64] = {
 147     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_ac,   sd_none,
 148     sd_none, sd_none, sd_none, sd_none, sd_none, sd_adtc, sd_none, sd_none,
 149     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_adtc, sd_ac,
 150     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 151     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 152     sd_none, sd_bcr,  sd_ac,   sd_none, sd_none, sd_none, sd_none, sd_none,
 153     sd_none, sd_none, sd_none, sd_adtc, sd_none, sd_none, sd_none, sd_none,
 154     sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none, sd_none,
 155 };
 156
 157 static const int sd_cmd_class[64] = {
 158     0,  0,  0,  0,  0,  9, 10,  0,  0,  0,  0,  1,  0,  0,  0,  0,
 159     2,  2,  2,  2,  3,  3,  3,  3,  4,  4,  4,  4,  6,  6,  6,  6,
 160     5,  5, 10, 10, 10, 10,  5,  9,  9,  9,  7,  7,  7,  7,  7,  7,
 161     7,  7, 10,  7,  9,  9,  9,  8,  8, 10,  8,  8,  8,  8,  8,  8,
 162 };
 163
 164 static uint8_t sd_crc7(void *message, size_t width)
 165 {
 166     int i, bit;
 167     uint8_t shift_reg = 0x00;
 168     uint8_t *msg = (uint8_t *) message;
 169
 170     for (i = 0; i < width; i ++, msg ++)
 171         for (bit = 7; bit >= 0; bit --) {
 172             shift_reg <<= 1;
 173             if ((shift_reg >> 7) ^ ((*msg >> bit) & 1))
 174                 shift_reg ^= 0x89;
 175         }
 176
 177     return shift_reg;
 178 }
 179
 180 static uint16_t sd_crc16(void *message, size_t width)
 181 {
 182     int i, bit;
 183     uint16_t shift_reg = 0x0000;
 184     uint16_t *msg = (uint16_t *) message;
 185     width <<= 1;
 186
 187     for (i = 0; i < width; i ++, msg ++)
 188         for (bit = 15; bit >= 0; bit --) {
 189             shift_reg <<= 1;
 190             if ((shift_reg >> 15) ^ ((*msg >> bit) & 1))
 191                 shift_reg ^= 0x1011;
 192         }
 193
 194     return shift_reg;
 195 }
 196
 197 static void sd_set_ocr(SDState *sd)
 198 {
 199     /* All voltages OK, card power-up OK, Standard Capacity SD Memory Card */
 200     sd->ocr = 0x80ffff00;
 201 }
 202
 203 static void sd_set_scr(SDState *sd)
 204 {
 205     sd->scr[0] = 0x00;          /* SCR Structure */
 206     sd->scr[1] = 0x2f;          /* SD Security Support */
 207     sd->scr[2] = 0x00;
 208     sd->scr[3] = 0x00;
 209     sd->scr[4] = 0x00;
 210     sd->scr[5] = 0x00;
 211     sd->scr[6] = 0x00;
 212     sd->scr[7] = 0x00;
 213 }
 214
 215 #define MID     0xaa
 216 #define OID     "XY"
 217 #define PNM     "QEMU!"
 218 #define PRV     0x01
 219 #define MDT_YR  2006
 220 #define MDT_MON 2
 221
 222 static void sd_set_cid(SDState *sd)
 223 {
 224     sd->cid[0] = MID;           /* Fake card manufacturer ID (MID) */
 225     sd->cid[1] = OID[0];        /* OEM/Application ID (OID) */
 226     sd->cid[2] = OID[1];
 227     sd->cid[3] = PNM[0];        /* Fake product name (PNM) */
 228     sd->cid[4] = PNM[1];
 229     sd->cid[5] = PNM[2];
 230     sd->cid[6] = PNM[3];
 231     sd->cid[7] = PNM[4];
 232     sd->cid[8] = PRV;           /* Fake product revision (PRV) */
 233     sd->cid[9] = 0xde;          /* Fake serial number (PSN) */
 234     sd->cid[10] = 0xad;
 235     sd->cid[11] = 0xbe;
 236     sd->cid[12] = 0xef;
 237     sd->cid[13] = 0x00 |        /* Manufacture date (MDT) */
 238         ((MDT_YR - 2000) / 10);
 239     sd->cid[14] = ((MDT_YR % 10) << 4) | MDT_MON;
 240     sd->cid[15] = (sd_crc7(sd->cid, 15) << 1) | 1;
 241 }
 242
 243 #define HWBLOCK_SHIFT   9                       /* 512 bytes */
 244 #define SECTOR_SHIFT    5                       /* 16 kilobytes */
 245 #define WPGROUP_SHIFT   7                       /* 2 megs */
 246 #define CMULT_SHIFT     9                       /* 512 times HWBLOCK_SIZE */
 247 #define WPGROUP_SIZE    (1 << (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT))
 248
 249 static const uint8_t sd_csd_rw_mask[16] = {
 250     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 251     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xfe,
 252 };
 253
 254 static void sd_set_csd(SDState *sd, uint64_t size)
 255 {
 256     uint32_t csize = (size >> (CMULT_SHIFT + HWBLOCK_SHIFT)) - 1;
 257     uint32_t sectsize = (1 << (SECTOR_SHIFT + 1)) - 1;
 258     uint32_t wpsize = (1 << (WPGROUP_SHIFT + 1)) - 1;
 259
 260     if (size <= 0x40000000) {   /* Standard Capacity SD */
 261         sd->csd[0] = 0x00;      /* CSD structure */
 262         sd->csd[1] = 0x26;      /* Data read access-time-1 */
 263         sd->csd[2] = 0x00;      /* Data read access-time-2 */
 264         sd->csd[3] = 0x5a;      /* Max. data transfer rate */
 265         sd->csd[4] = 0x5f;      /* Card Command Classes */
 266         sd->csd[5] = 0x50 |     /* Max. read data block length */
 267             HWBLOCK_SHIFT;
 268         sd->csd[6] = 0xe0 |     /* Partial block for read allowed */
 269             ((csize >> 10) & 0x03);
 270         sd->csd[7] = 0x00 |     /* Device size */
 271             ((csize >> 2) & 0xff);
 272         sd->csd[8] = 0x3f |     /* Max. read current */
 273             ((csize << 6) & 0xc0);
 274         sd->csd[9] = 0xfc |     /* Max. write current */
 275             ((CMULT_SHIFT - 2) >> 1);
 276         sd->csd[10] = 0x40 |    /* Erase sector size */
 277             (((CMULT_SHIFT - 2) << 7) & 0x80) | (sectsize >> 1);
 278         sd->csd[11] = 0x00 |    /* Write protect group size */
 279             ((sectsize << 7) & 0x80) | wpsize;
 280         sd->csd[12] = 0x90 |    /* Write speed factor */
 281             (HWBLOCK_SHIFT >> 2);
 282         sd->csd[13] = 0x20 |    /* Max. write data block length */
 283             ((HWBLOCK_SHIFT << 6) & 0xc0);
 284         sd->csd[14] = 0x00;     /* File format group */
 285         sd->csd[15] = (sd_crc7(sd->csd, 15) << 1) | 1;
 286     } else {                    /* SDHC */
 287         size /= 512 * 1024;
 288         size -= 1;
 289         sd->csd[0] = 0x40;
 290         sd->csd[1] = 0x0e;
 291         sd->csd[2] = 0x00;
 292         sd->csd[3] = 0x32;
 293         sd->csd[4] = 0x5b;
 294         sd->csd[5] = 0x59;
 295         sd->csd[6] = 0x00;
 296         sd->csd[7] = (size >> 16) & 0xff;
 297         sd->csd[8] = (size >> 8) & 0xff;
 298         sd->csd[9] = (size & 0xff);
 299         sd->csd[10] = 0x7f;
 300         sd->csd[11] = 0x80;
 301         sd->csd[12] = 0x0a;
 302         sd->csd[13] = 0x40;
 303         sd->csd[14] = 0x00;
 304         sd->csd[15] = 0x00;
 305         sd->ocr |= 1 << 30;     /* High Capacity SD Memort Card */
 306     }
 307 }
 308
 309 static void sd_set_rca(SDState *sd)
 310 {
 311     sd->rca += 0x4567;
 312 }
 313
 314 /* Card status bits, split by clear condition:
 315  * A : According to the card current state
 316  * B : Always related to the previous command
 317  * C : Cleared by read
 318  */
 319 #define CARD_STATUS_A   0x02004100
 320 #define CARD_STATUS_B   0x00c01e00
 321 #define CARD_STATUS_C   0xfd39a028
 322
 323 static void sd_set_cardstatus(SDState *sd)
 324 {
 325     sd->card_status = 0x00000100;
 326 }
 327
 328 static void sd_set_sdstatus(SDState *sd)
 329 {
 330     memset(sd->sd_status, 0, 64);
 331 }
 332
 333 static int sd_req_crc_validate(SDRequest *req)
 334 {
 335     uint8_t buffer[5];
 336     buffer[0] = 0x40 | req->cmd;
 337     buffer[1] = (req->arg >> 24) & 0xff;
 338     buffer[2] = (req->arg >> 16) & 0xff;
 339     buffer[3] = (req->arg >> 8) & 0xff;
 340     buffer[4] = (req->arg >> 0) & 0xff;
 341     return 0;
 342     return sd_crc7(buffer, 5) != req->crc;      /* TODO */
 343 }
 344
 345 static void sd_response_r1_make(SDState *sd, uint8_t *response)
 346 {
 347     uint32_t status = sd->card_status;
 348     /* Clear the "clear on read" status bits */
 349     sd->card_status &= ~CARD_STATUS_C;
 350
 351     response[0] = (status >> 24) & 0xff;
 352     response[1] = (status >> 16) & 0xff;
 353     response[2] = (status >> 8) & 0xff;
 354     response[3] = (status >> 0) & 0xff;
 355 }
 356
 357 static void sd_response_r3_make(SDState *sd, uint8_t *response)
 358 {
 359     response[0] = (sd->ocr >> 24) & 0xff;
 360     response[1] = (sd->ocr >> 16) & 0xff;
 361     response[2] = (sd->ocr >> 8) & 0xff;
 362     response[3] = (sd->ocr >> 0) & 0xff;
 363 }
 364
 365 static void sd_response_r6_make(SDState *sd, uint8_t *response)
 366 {
 367     uint16_t arg;
 368     uint16_t status;
 369
 370     arg = sd->rca;
 371     status = ((sd->card_status >> 8) & 0xc000) |
 372              ((sd->card_status >> 6) & 0x2000) |
 373               (sd->card_status & 0x1fff);
 374     sd->card_status &= ~(CARD_STATUS_C & 0xc81fff);
 375
 376     response[0] = (arg >> 8) & 0xff;
 377     response[1] = arg & 0xff;
 378     response[2] = (status >> 8) & 0xff;
 379     response[3] = status & 0xff;
 380 }
 381
 382 static void sd_response_r7_make(SDState *sd, uint8_t *response)
 383 {
 384     response[0] = (sd->vhs >> 24) & 0xff;
 385     response[1] = (sd->vhs >> 16) & 0xff;
 386     response[2] = (sd->vhs >>  8) & 0xff;
 387     response[3] = (sd->vhs >>  0) & 0xff;
 388 }
 389
 390 static void sd_reset(SDState *sd, BlockDriverState *bdrv)
 391 {
 392     uint64_t size;
 393     uint64_t sect;
 394
 395     if (bdrv) {
 396         bdrv_get_geometry(bdrv, &sect);
 397     } else {
 398         sect = 0;
 399     }
 400     size = sect << 9;
 401
 402     sect = (size >> (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT)) + 1;
 403
 404     sd->state = sd_idle_state;
 405     sd->rca = 0x0000;
 406     sd_set_ocr(sd);
 407     sd_set_scr(sd);
 408     sd_set_cid(sd);
 409     sd_set_csd(sd, size);
 410     sd_set_cardstatus(sd);
 411     sd_set_sdstatus(sd);
 412
 413     sd->bdrv = bdrv;
 414
 415     if (sd->wp_groups)
 416         g_free(sd->wp_groups);
 417     sd->wp_switch = bdrv ? bdrv_is_read_only(bdrv) : 0;
 418     sd->wp_groups = (int *) g_malloc0(sizeof(int) * sect);
 419     memset(sd->function_group, 0, sizeof(int) * 6);
 420     sd->erase_start = 0;
 421     sd->erase_end = 0;
 422     sd->size = size;
 423     sd->blk_len = 0x200;
 424     sd->pwd_len = 0;
 425     sd->expecting_acmd = 0;
 426 }
 427
 428 static void sd_cardchange(void *opaque, bool load)
 429 {
 430     SDState *sd = opaque;
 431
 432     qemu_set_irq(sd->inserted_cb, bdrv_is_inserted(sd->bdrv));
 433     if (bdrv_is_inserted(sd->bdrv)) {
 434         sd_reset(sd, sd->bdrv);
 435         qemu_set_irq(sd->readonly_cb, sd->wp_switch);
 436     }
 437 }
 438
 439 static const BlockDevOps sd_block_ops = {
 440     .change_media_cb = sd_cardchange,
 441 };
 442
 443 /* We do not model the chip select pin, so allow the board to select
 444    whether card should be in SSI or MMC/SD mode.  It is also up to the
 445    board to ensure that ssi transfers only occur when the chip select
 446    is asserted.  */
 447 SDState *sd_init(BlockDriverState *bs, int is_spi)
 448 {
 449     SDState *sd;
 450
 451     sd = (SDState *) g_malloc0(sizeof(SDState));
 452     sd->buf = qemu_blockalign(bs, 512);
 453     sd->spi = is_spi;
 454     sd->enable = 1;
 455     sd_reset(sd, bs);
 456     if (sd->bdrv) {
 457         bdrv_attach_dev_nofail(sd->bdrv, sd);
 458         bdrv_set_dev_ops(sd->bdrv, &sd_block_ops, sd);
 459     }
 460     return sd;
 461 }
 462
 463 void sd_set_cb(SDState *sd, qemu_irq readonly, qemu_irq insert)
 464 {
 465     sd->readonly_cb = readonly;
 466     sd->inserted_cb = insert;
 467     qemu_set_irq(readonly, sd->bdrv ? bdrv_is_read_only(sd->bdrv) : 0);
 468     qemu_set_irq(insert, sd->bdrv ? bdrv_is_inserted(sd->bdrv) : 0);
 469 }
 470
 471 static void sd_erase(SDState *sd)
 472 {
 473     int i, start, end;
 474     if (!sd->erase_start || !sd->erase_end) {
 475         sd->card_status |= ERASE_SEQ_ERROR;
 476         return;
 477     }
 478
 479     start = sd->erase_start >>
 480             (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT);
 481     end = sd->erase_end >>
 482             (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT);
 483     sd->erase_start = 0;
 484     sd->erase_end = 0;
 485     sd->csd[14] |= 0x40;
 486
 487     for (i = start; i <= end; i ++)
 488         if (sd->wp_groups[i])
 489             sd->card_status |= WP_ERASE_SKIP;
 490 }
 491
 492 static uint32_t sd_wpbits(SDState *sd, uint64_t addr)
 493 {
 494     uint32_t i, wpnum;
 495     uint32_t ret = 0;
 496
 497     wpnum = addr >> (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT);
 498
 499     for (i = 0; i < 32; i ++, wpnum ++, addr += WPGROUP_SIZE)
 500         if (addr < sd->size && sd->wp_groups[wpnum])
 501             ret |= (1 << i);
 502
 503     return ret;
 504 }
 505
 506 static void sd_function_switch(SDState *sd, uint32_t arg)
 507 {
 508     int i, mode, new_func, crc;
 509     mode = !!(arg & 0x80000000);
 510
 511     sd->data[0] = 0x00;         /* Maximum current consumption */
 512     sd->data[1] = 0x01;
 513     sd->data[2] = 0x80;         /* Supported group 6 functions */
 514     sd->data[3] = 0x01;
 515     sd->data[4] = 0x80;         /* Supported group 5 functions */
 516     sd->data[5] = 0x01;
 517     sd->data[6] = 0x80;         /* Supported group 4 functions */
 518     sd->data[7] = 0x01;
 519     sd->data[8] = 0x80;         /* Supported group 3 functions */
 520     sd->data[9] = 0x01;
 521     sd->data[10] = 0x80;        /* Supported group 2 functions */
 522     sd->data[11] = 0x43;
 523     sd->data[12] = 0x80;        /* Supported group 1 functions */
 524     sd->data[13] = 0x03;
 525     for (i = 0; i < 6; i ++) {
 526         new_func = (arg >> (i * 4)) & 0x0f;
 527         if (mode && new_func != 0x0f)
 528             sd->function_group[i] = new_func;
 529         sd->data[14 + (i >> 1)] = new_func << ((i * 4) & 4);
 530     }
 531     memset(&sd->data[17], 0, 47);
 532     crc = sd_crc16(sd->data, 64);
 533     sd->data[65] = crc >> 8;
 534     sd->data[66] = crc & 0xff;
 535 }
 536
 537 static inline int sd_wp_addr(SDState *sd, uint32_t addr)
 538 {
 539     return sd->wp_groups[addr >>
 540             (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT)];
 541 }
 542
 543 static void sd_lock_command(SDState *sd)
 544 {
 545     int erase, lock, clr_pwd, set_pwd, pwd_len;
 546     erase = !!(sd->data[0] & 0x08);
 547     lock = sd->data[0] & 0x04;
 548     clr_pwd = sd->data[0] & 0x02;
 549     set_pwd = sd->data[0] & 0x01;
 550
 551     if (sd->blk_len > 1)
 552         pwd_len = sd->data[1];
 553     else
 554         pwd_len = 0;
 555
 556     if (erase) {
 557         if (!(sd->card_status & CARD_IS_LOCKED) || sd->blk_len > 1 ||
 558                         set_pwd || clr_pwd || lock || sd->wp_switch ||
 559                         (sd->csd[14] & 0x20)) {
 560             sd->card_status |= LOCK_UNLOCK_FAILED;
 561             return;
 562         }
 563         memset(sd->wp_groups, 0, sizeof(int) * (sd->size >>
 564                         (HWBLOCK_SHIFT + SECTOR_SHIFT + WPGROUP_SHIFT)));
 565         sd->csd[14] &= ~0x10;
 566         sd->card_status &= ~CARD_IS_LOCKED;
 567         sd->pwd_len = 0;
 568         /* Erasing the entire card here! */
 569         fprintf(stderr, "SD: Card force-erased by CMD42\n");
 570         return;
 571     }
 572
 573     if (sd->blk_len < 2 + pwd_len ||
 574                     pwd_len <= sd->pwd_len ||
 575                     pwd_len > sd->pwd_len + 16) {
 576         sd->card_status |= LOCK_UNLOCK_FAILED;
 577         return;
 578     }
 579
 580     if (sd->pwd_len && memcmp(sd->pwd, sd->data + 2, sd->pwd_len)) {
 581         sd->card_status |= LOCK_UNLOCK_FAILED;
 582         return;
 583     }
 584
 585     pwd_len -= sd->pwd_len;
 586     if ((pwd_len && !set_pwd) ||
 587                     (clr_pwd && (set_pwd || lock)) ||
 588                     (lock && !sd->pwd_len && !set_pwd) ||
 589                     (!set_pwd && !clr_pwd &&
 590                      (((sd->card_status & CARD_IS_LOCKED) && lock) ||
 591                       (!(sd->card_status & CARD_IS_LOCKED) && !lock)))) {
 592         sd->card_status |= LOCK_UNLOCK_FAILED;
 593         return;
 594     }
 595
 596     if (set_pwd) {
 597         memcpy(sd->pwd, sd->data + 2 + sd->pwd_len, pwd_len);
 598         sd->pwd_len = pwd_len;
 599     }
 600
 601     if (clr_pwd) {
 602         sd->pwd_len = 0;
 603     }
 604
 605     if (lock)
 606         sd->card_status |= CARD_IS_LOCKED;
 607     else
 608         sd->card_status &= ~CARD_IS_LOCKED;
 609 }
 610
 611 static sd_rsp_type_t sd_normal_command(SDState *sd,
 612                                        SDRequest req)
 613 {
 614     uint32_t rca = 0x0000;
 615     uint64_t addr = (sd->ocr & (1 << 30)) ? (uint64_t) req.arg << 9 : req.arg;
 616
 617     /* Not interpreting this as an app command */
 618     sd->card_status &= ~APP_CMD;
 619
 620     if (sd_cmd_type[req.cmd] == sd_ac || sd_cmd_type[req.cmd] == sd_adtc)
 621         rca = req.arg >> 16;
 622
 623     DPRINTF("CMD%d 0x%08x state %d\n", req.cmd, req.arg, sd->state);
 624     switch (req.cmd) {
 625     /* Basic commands (Class 0 and Class 1) */
 626     case 0:     /* CMD0:   GO_IDLE_STATE */
 627         switch (sd->state) {
 628         case sd_inactive_state:
 629             return sd->spi ? sd_r1 : sd_r0;
 630
 631         default:
 632             sd->state = sd_idle_state;
 633             sd_reset(sd, sd->bdrv);
 634             return sd->spi ? sd_r1 : sd_r0;
 635         }
 636         break;
 637
 638     case 1:     /* CMD1:   SEND_OP_CMD */
 639         if (!sd->spi)
 640             goto bad_cmd;
 641
 642         sd->state = sd_transfer_state;
 643         return sd_r1;
 644
 645     case 2:     /* CMD2:   ALL_SEND_CID */
 646         if (sd->spi)
 647             goto bad_cmd;
 648         switch (sd->state) {
 649         case sd_ready_state:
 650             sd->state = sd_identification_state;
 651             return sd_r2_i;
 652
 653         default:
 654             break;
 655         }
 656         break;
 657
 658     case 3:     /* CMD3:   SEND_RELATIVE_ADDR */
 659         if (sd->spi)
 660             goto bad_cmd;
 661         switch (sd->state) {
 662         case sd_identification_state:
 663         case sd_standby_state:
 664             sd->state = sd_standby_state;
 665             sd_set_rca(sd);
 666             return sd_r6;
 667
 668         default:
 669             break;
 670         }
 671         break;
 672
 673     case 4:     /* CMD4:   SEND_DSR */
 674         if (sd->spi)
 675             goto bad_cmd;
 676         switch (sd->state) {
 677         case sd_standby_state:
 678             break;
 679
 680         default:
 681             break;
 682         }
 683         break;
 684
 685     case 5: /* CMD5: reserved for SDIO cards */
 686         return sd_illegal;
 687
 688     case 6:     /* CMD6:   SWITCH_FUNCTION */
 689         if (sd->spi)
 690             goto bad_cmd;
 691         switch (sd->mode) {
 692         case sd_data_transfer_mode:
 693             sd_function_switch(sd, req.arg);
 694             sd->state = sd_sendingdata_state;
 695             sd->data_start = 0;
 696             sd->data_offset = 0;
 697             return sd_r1;
 698
 699         default:
 700             break;
 701         }
 702         break;
 703
 704     case 7:     /* CMD7:   SELECT/DESELECT_CARD */
 705         if (sd->spi)
 706             goto bad_cmd;
 707         switch (sd->state) {
 708         case sd_standby_state:
 709             if (sd->rca != rca)
 710                 return sd_r0;
 711
 712             sd->state = sd_transfer_state;
 713             return sd_r1b;
 714
 715         case sd_transfer_state:
 716         case sd_sendingdata_state:
 717             if (sd->rca == rca)
 718                 break;
 719
 720             sd->state = sd_standby_state;
 721             return sd_r1b;
 722
 723         case sd_disconnect_state:
 724             if (sd->rca != rca)
 725                 return sd_r0;
 726
 727             sd->state = sd_programming_state;
 728             return sd_r1b;
 729
 730         case sd_programming_state:
 731             if (sd->rca == rca)
 732                 break;
 733
 734             sd->state = sd_disconnect_state;
 735             return sd_r1b;
 736
 737         default:
 738             break;
 739         }
 740         break;
 741
 742     case 8:     /* CMD8:   SEND_IF_COND */
 743         /* Physical Layer Specification Version 2.00 command */
 744         switch (sd->state) {
 745         case sd_idle_state:
 746             sd->vhs = 0;
 747
 748             /* No response if not exactly one VHS bit is set.  */
 749             if (!(req.arg >> 8) || (req.arg >> ffs(req.arg & ~0xff)))
 750                 return sd->spi ? sd_r7 : sd_r0;
 751
 752             /* Accept.  */
 753             sd->vhs = req.arg;
 754             return sd_r7;
 755
 756         default:
 757             break;
 758         }
 759         break;
 760
 761     case 9:     /* CMD9:   SEND_CSD */
 762         switch (sd->state) {
 763         case sd_standby_state:
 764             if (sd->rca != rca)
 765                 return sd_r0;
 766
 767             return sd_r2_s;
 768
 769         case sd_transfer_state:
 770             if (!sd->spi)
 771                 break;
 772             sd->state = sd_sendingdata_state;
 773             memcpy(sd->data, sd->csd, 16);
 774             sd->data_start = addr;
 775             sd->data_offset = 0;
 776             return sd_r1;
 777
 778         default:
 779             break;
 780         }
 781         break;
 782
 783     case 10:    /* CMD10:  SEND_CID */
 784         switch (sd->state) {
 785         case sd_standby_state:
 786             if (sd->rca != rca)
 787                 return sd_r0;
 788
 789             return sd_r2_i;
 790
 791         case sd_transfer_state:
 792             if (!sd->spi)
 793                 break;
 794             sd->state = sd_sendingdata_state;
 795             memcpy(sd->data, sd->cid, 16);
 796             sd->data_start = addr;
 797             sd->data_offset = 0;
 798             return sd_r1;
 799
 800         default:
 801             break;
 802         }
 803         break;
 804
 805     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
 806         if (sd->spi)
 807             goto bad_cmd;
 808         switch (sd->state) {
 809         case sd_transfer_state:
 810             sd->state = sd_sendingdata_state;
 811             sd->data_start = req.arg;
 812             sd->data_offset = 0;
 813
 814             if (sd->data_start + sd->blk_len > sd->size)
 815                 sd->card_status |= ADDRESS_ERROR;
 816             return sd_r0;
 817
 818         default:
 819             break;
 820         }
 821         break;
 822
 823     case 12:    /* CMD12:  STOP_TRANSMISSION */
 824         switch (sd->state) {
 825         case sd_sendingdata_state:
 826             sd->state = sd_transfer_state;
 827             return sd_r1b;
 828
 829         case sd_receivingdata_state:
 830             sd->state = sd_programming_state;
 831             /* Bzzzzzzztt .... Operation complete.  */
 832             sd->state = sd_transfer_state;
 833             return sd_r1b;
 834
 835         default:
 836             break;
 837         }
 838         break;
 839
 840     case 13:    /* CMD13:  SEND_STATUS */
 841         switch (sd->mode) {
 842         case sd_data_transfer_mode:
 843             if (sd->rca != rca)
 844                 return sd_r0;
 845
 846             return sd_r1;
 847
 848         default:
 849             break;
 850         }
 851         break;
 852
 853     case 15:    /* CMD15:  GO_INACTIVE_STATE */
 854         if (sd->spi)
 855             goto bad_cmd;
 856         switch (sd->mode) {
 857         case sd_data_transfer_mode:
 858             if (sd->rca != rca)
 859                 return sd_r0;
 860
 861             sd->state = sd_inactive_state;
 862             return sd_r0;
 863
 864         default:
 865             break;
 866         }
 867         break;
 868
 869     /* Block read commands (Classs 2) */
 870     case 16:    /* CMD16:  SET_BLOCKLEN */
 871         switch (sd->state) {
 872         case sd_transfer_state:
 873             if (req.arg > (1 << HWBLOCK_SHIFT))
 874                 sd->card_status |= BLOCK_LEN_ERROR;
 875             else
 876                 sd->blk_len = req.arg;
 877
 878             return sd_r1;
 879
 880         default:
 881             break;
 882         }
 883         break;
 884
 885     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
 886         switch (sd->state) {
 887         case sd_transfer_state:
 888             sd->state = sd_sendingdata_state;
 889             sd->data_start = addr;
 890             sd->data_offset = 0;
 891
 892             if (sd->data_start + sd->blk_len > sd->size)
 893                 sd->card_status |= ADDRESS_ERROR;
 894             return sd_r1;
 895
 896         default:
 897             break;
 898         }
 899         break;
 900
 901     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
 902         switch (sd->state) {
 903         case sd_transfer_state:
 904             sd->state = sd_sendingdata_state;
 905             sd->data_start = addr;
 906             sd->data_offset = 0;
 907
 908             if (sd->data_start + sd->blk_len > sd->size)
 909                 sd->card_status |= ADDRESS_ERROR;
 910             return sd_r1;
 911
 912         default:
 913             break;
 914         }
 915         break;
 916
 917     /* Block write commands (Class 4) */
 918     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
 919         if (sd->spi)
 920             goto unimplemented_cmd;
 921         switch (sd->state) {
 922         case sd_transfer_state:
 923             /* Writing in SPI mode not implemented.  */
 924             if (sd->spi)
 925                 break;
 926             sd->state = sd_receivingdata_state;
 927             sd->data_start = addr;
 928             sd->data_offset = 0;
 929             sd->blk_written = 0;
 930
 931             if (sd->data_start + sd->blk_len > sd->size)
 932                 sd->card_status |= ADDRESS_ERROR;
 933             if (sd_wp_addr(sd, sd->data_start))
 934                 sd->card_status |= WP_VIOLATION;
 935             if (sd->csd[14] & 0x30)
 936                 sd->card_status |= WP_VIOLATION;
 937             return sd_r1;
 938
 939         default:
 940             break;
 941         }
 942         break;
 943
 944     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
 945         if (sd->spi)
 946             goto unimplemented_cmd;
 947         switch (sd->state) {
 948         case sd_transfer_state:
 949             /* Writing in SPI mode not implemented.  */
 950             if (sd->spi)
 951                 break;
 952             sd->state = sd_receivingdata_state;
 953             sd->data_start = addr;
 954             sd->data_offset = 0;
 955             sd->blk_written = 0;
 956
 957             if (sd->data_start + sd->blk_len > sd->size)
 958                 sd->card_status |= ADDRESS_ERROR;
 959             if (sd_wp_addr(sd, sd->data_start))
 960                 sd->card_status |= WP_VIOLATION;
 961             if (sd->csd[14] & 0x30)
 962                 sd->card_status |= WP_VIOLATION;
 963             return sd_r1;
 964
 965         default:
 966             break;
 967         }
 968         break;
 969
 970     case 26:    /* CMD26:  PROGRAM_CID */
 971         if (sd->spi)
 972             goto bad_cmd;
 973         switch (sd->state) {
 974         case sd_transfer_state:
 975             sd->state = sd_receivingdata_state;
 976             sd->data_start = 0;
 977             sd->data_offset = 0;
 978             return sd_r1;
 979
 980         default:
 981             break;
 982         }
 983         break;
 984
 985     case 27:    /* CMD27:  PROGRAM_CSD */
 986         if (sd->spi)
 987             goto unimplemented_cmd;
 988         switch (sd->state) {
 989         case sd_transfer_state:
 990             sd->state = sd_receivingdata_state;
 991             sd->data_start = 0;
 992             sd->data_offset = 0;
 993             return sd_r1;
 994
 995         default:
 996             break;
 997         }
 998         break;
 999
1000     /* Write protection (Class 6) */
1001     case 28:    /* CMD28:  SET_WRITE_PROT */
1002         switch (sd->state) {
1003         case sd_transfer_state:
1004             if (addr >= sd->size) {
1005                 sd->card_status |= ADDRESS_ERROR;
1006                 return sd_r1b;
1007             }
1008
1009             sd->state = sd_programming_state;
1010             sd->wp_groups[addr >> (HWBLOCK_SHIFT +
1011                             SECTOR_SHIFT + WPGROUP_SHIFT)] = 1;
1012             /* Bzzzzzzztt .... Operation complete.  */
1013             sd->state = sd_transfer_state;
1014             return sd_r1b;
1015
1016         default:
1017             break;
1018         }
1019         break;
1020
1021     case 29:    /* CMD29:  CLR_WRITE_PROT */
1022         switch (sd->state) {
1023         case sd_transfer_state:
1024             if (addr >= sd->size) {
1025                 sd->card_status |= ADDRESS_ERROR;
1026                 return sd_r1b;
1027             }
1028
1029             sd->state = sd_programming_state;
1030             sd->wp_groups[addr >> (HWBLOCK_SHIFT +
1031                             SECTOR_SHIFT + WPGROUP_SHIFT)] = 0;
1032             /* Bzzzzzzztt .... Operation complete.  */
1033             sd->state = sd_transfer_state;
1034             return sd_r1b;
1035
1036         default:
1037             break;
1038         }
1039         break;
1040
1041     case 30:    /* CMD30:  SEND_WRITE_PROT */
1042         switch (sd->state) {
1043         case sd_transfer_state:
1044             sd->state = sd_sendingdata_state;
1045             *(uint32_t *) sd->data = sd_wpbits(sd, req.arg);
1046             sd->data_start = addr;
1047             sd->data_offset = 0;
1048             return sd_r1b;
1049
1050         default:
1051             break;
1052         }
1053         break;
1054
1055     /* Erase commands (Class 5) */
1056     case 32:    /* CMD32:  ERASE_WR_BLK_START */
1057         switch (sd->state) {
1058         case sd_transfer_state:
1059             sd->erase_start = req.arg;
1060             return sd_r1;
1061
1062         default:
1063             break;
1064         }
1065         break;
1066
1067     case 33:    /* CMD33:  ERASE_WR_BLK_END */
1068         switch (sd->state) {
1069         case sd_transfer_state:
1070             sd->erase_end = req.arg;
1071             return sd_r1;
1072
1073         default:
1074             break;
1075         }
1076         break;
1077
1078     case 38:    /* CMD38:  ERASE */
1079         switch (sd->state) {
1080         case sd_transfer_state:
1081             if (sd->csd[14] & 0x30) {
1082                 sd->card_status |= WP_VIOLATION;
1083                 return sd_r1b;
1084             }
1085
1086             sd->state = sd_programming_state;
1087             sd_erase(sd);
1088             /* Bzzzzzzztt .... Operation complete.  */
1089             sd->state = sd_transfer_state;
1090             return sd_r1b;
1091
1092         default:
1093             break;
1094         }
1095         break;
1096
1097     /* Lock card commands (Class 7) */
1098     case 42:    /* CMD42:  LOCK_UNLOCK */
1099         if (sd->spi)
1100             goto unimplemented_cmd;
1101         switch (sd->state) {
1102         case sd_transfer_state:
1103             sd->state = sd_receivingdata_state;
1104             sd->data_start = 0;
1105             sd->data_offset = 0;
1106             return sd_r1;
1107
1108         default:
1109             break;
1110         }
1111         break;
1112
1113     case 52:
1114     case 53:
1115         /* CMD52, CMD53: reserved for SDIO cards
1116          * (see the SDIO Simplified Specification V2.0)
1117          * Handle as illegal command but do not complain
1118          * on stderr, as some OSes may use these in their
1119          * probing for presence of an SDIO card.
1120          */
1121         return sd_illegal;
1122
1123     /* Application specific commands (Class 8) */
1124     case 55:    /* CMD55:  APP_CMD */
1125         if (sd->rca != rca)
1126             return sd_r0;
1127
1128         sd->expecting_acmd = 1;
1129         sd->card_status |= APP_CMD;
1130         return sd_r1;
1131
1132     case 56:    /* CMD56:  GEN_CMD */
1133         fprintf(stderr, "SD: GEN_CMD 0x%08x\n", req.arg);
1134
1135         switch (sd->state) {
1136         case sd_transfer_state:
1137             sd->data_offset = 0;
1138             if (req.arg & 1)
1139                 sd->state = sd_sendingdata_state;
1140             else
1141                 sd->state = sd_receivingdata_state;
1142             return sd_r1;
1143
1144         default:
1145             break;
1146         }
1147         break;
1148
1149     default:
1150     bad_cmd:
1151         fprintf(stderr, "SD: Unknown CMD%i\n", req.cmd);
1152         return sd_illegal;
1153
1154     unimplemented_cmd:
1155         /* Commands that are recognised but not yet implemented in SPI mode.  */
1156         fprintf(stderr, "SD: CMD%i not implemented in SPI mode\n", req.cmd);
1157         return sd_illegal;
1158     }
1159
1160     fprintf(stderr, "SD: CMD%i in a wrong state\n", req.cmd);
1161     return sd_illegal;
1162 }
1163
1164 static sd_rsp_type_t sd_app_command(SDState *sd,
1165                                     SDRequest req)
1166 {
1167     DPRINTF("ACMD%d 0x%08x\n", req.cmd, req.arg);
1168     sd->card_status |= APP_CMD;
1169     switch (req.cmd) {
1170     case 6:     /* ACMD6:  SET_BUS_WIDTH */
1171         switch (sd->state) {
1172         case sd_transfer_state:
1173             sd->sd_status[0] &= 0x3f;
1174             sd->sd_status[0] |= (req.arg & 0x03) << 6;
1175             return sd_r1;
1176
1177         default:
1178             break;
1179         }
1180         break;
1181
1182     case 13:    /* ACMD13: SD_STATUS */
1183         switch (sd->state) {
1184         case sd_transfer_state:
1185             sd->state = sd_sendingdata_state;
1186             sd->data_start = 0;
1187             sd->data_offset = 0;
1188             return sd_r1;
1189
1190         default:
1191             break;
1192         }
1193         break;
1194
1195     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1196         switch (sd->state) {
1197         case sd_transfer_state:
1198             *(uint32_t *) sd->data = sd->blk_written;
1199
1200             sd->state = sd_sendingdata_state;
1201             sd->data_start = 0;
1202             sd->data_offset = 0;
1203             return sd_r1;
1204
1205         default:
1206             break;
1207         }
1208         break;
1209
1210     case 23:    /* ACMD23: SET_WR_BLK_ERASE_COUNT */
1211         switch (sd->state) {
1212         case sd_transfer_state:
1213             return sd_r1;
1214
1215         default:
1216             break;
1217         }
1218         break;
1219
1220     case 41:    /* ACMD41: SD_APP_OP_COND */
1221         if (sd->spi) {
1222             /* SEND_OP_CMD */
1223             sd->state = sd_transfer_state;
1224             return sd_r1;
1225         }
1226         switch (sd->state) {
1227         case sd_idle_state:
1228             /* We accept any voltage.  10000 V is nothing.  */
1229             if (req.arg)
1230                 sd->state = sd_ready_state;
1231
1232             return sd_r3;
1233
1234         default:
1235             break;
1236         }
1237         break;
1238
1239     case 42:    /* ACMD42: SET_CLR_CARD_DETECT */
1240         switch (sd->state) {
1241         case sd_transfer_state:
1242             /* Bringing in the 50KOhm pull-up resistor... Done.  */
1243             return sd_r1;
1244
1245         default:
1246             break;
1247         }
1248         break;
1249
1250     case 51:    /* ACMD51: SEND_SCR */
1251         switch (sd->state) {
1252         case sd_transfer_state:
1253             sd->state = sd_sendingdata_state;
1254             sd->data_start = 0;
1255             sd->data_offset = 0;
1256             return sd_r1;
1257
1258         default:
1259             break;
1260         }
1261         break;
1262
1263     default:
1264         /* Fall back to standard commands.  */
1265         return sd_normal_command(sd, req);
1266     }
1267
1268     fprintf(stderr, "SD: ACMD%i in a wrong state\n", req.cmd);
1269     return sd_illegal;
1270 }
1271
1272 static int cmd_valid_while_locked(SDState *sd, SDRequest *req)
1273 {
1274     /* Valid commands in locked state:
1275      * basic class (0)
1276      * lock card class (7)
1277      * CMD16
1278      * implicitly, the ACMD prefix CMD55
1279      * ACMD41 and ACMD42
1280      * Anything else provokes an "illegal command" response.
1281      */
1282     if (sd->expecting_acmd) {
1283         return req->cmd == 41 || req->cmd == 42;
1284     }
1285     if (req->cmd == 16 || req->cmd == 55) {
1286         return 1;
1287     }
1288     return sd_cmd_class[req->cmd] == 0 || sd_cmd_class[req->cmd] == 7;
1289 }
1290
1291 int sd_do_command(SDState *sd, SDRequest *req,
1292                   uint8_t *response) {
1293     int last_state;
1294     sd_rsp_type_t rtype;
1295     int rsplen;
1296
1297     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv) || !sd->enable) {
1298         return 0;
1299     }
1300
1301     if (sd_req_crc_validate(req)) {
1302         sd->card_status |= COM_CRC_ERROR;
1303         rtype = sd_illegal;
1304         goto send_response;
1305     }
1306
1307     if (sd->card_status & CARD_IS_LOCKED) {
1308         if (!cmd_valid_while_locked(sd, req)) {
1309             sd->card_status |= ILLEGAL_COMMAND;
1310             sd->expecting_acmd = 0;
1311             fprintf(stderr, "SD: Card is locked\n");
1312             rtype = sd_illegal;
1313             goto send_response;
1314         }
1315     }
1316
1317     last_state = sd->state;
1318     sd_set_mode(sd);
1319
1320     if (sd->expecting_acmd) {
1321         sd->expecting_acmd = 0;
1322         rtype = sd_app_command(sd, *req);
1323     } else {
1324         rtype = sd_normal_command(sd, *req);
1325     }
1326
1327     if (rtype == sd_illegal) {
1328         sd->card_status |= ILLEGAL_COMMAND;
1329     } else {
1330         /* Valid command, we can update the 'state before command' bits.
1331          * (Do this now so they appear in r1 responses.)
1332          */
1333         sd->current_cmd = req->cmd;
1334         sd->card_status &= ~CURRENT_STATE;
1335         sd->card_status |= (last_state << 9);
1336     }
1337
1338 send_response:
1339     switch (rtype) {
1340     case sd_r1:
1341     case sd_r1b:
1342         sd_response_r1_make(sd, response);
1343         rsplen = 4;
1344         break;
1345
1346     case sd_r2_i:
1347         memcpy(response, sd->cid, sizeof(sd->cid));
1348         rsplen = 16;
1349         break;
1350
1351     case sd_r2_s:
1352         memcpy(response, sd->csd, sizeof(sd->csd));
1353         rsplen = 16;
1354         break;
1355
1356     case sd_r3:
1357         sd_response_r3_make(sd, response);
1358         rsplen = 4;
1359         break;
1360
1361     case sd_r6:
1362         sd_response_r6_make(sd, response);
1363         rsplen = 4;
1364         break;
1365
1366     case sd_r7:
1367         sd_response_r7_make(sd, response);
1368         rsplen = 4;
1369         break;
1370
1371     case sd_r0:
1372     case sd_illegal:
1373     default:
1374         rsplen = 0;
1375         break;
1376     }
1377
1378     if (rtype != sd_illegal) {
1379         /* Clear the "clear on valid command" status bits now we've
1380          * sent any response
1381          */
1382         sd->card_status &= ~CARD_STATUS_B;
1383     }
1384
1385 #ifdef DEBUG_SD
1386     if (rsplen) {
1387         int i;
1388         DPRINTF("Response:");
1389         for (i = 0; i < rsplen; i++)
1390             printf(" %02x", response[i]);
1391         printf(" state %d\n", sd->state);
1392     } else {
1393         DPRINTF("No response %d\n", sd->state);
1394     }
1395 #endif
1396
1397     return rsplen;
1398 }
1399
1400 static void sd_blk_read(SDState *sd, uint64_t addr, uint32_t len)
1401 {
1402     uint64_t end = addr + len;
1403
1404     DPRINTF("sd_blk_read: addr = 0x%08llx, len = %d\n",
1405             (unsigned long long) addr, len);
1406     if (!sd->bdrv || bdrv_read(sd->bdrv, addr >> 9, sd->buf, 1) == -1) {
1407         fprintf(stderr, "sd_blk_read: read error on host side\n");
1408         return;
1409     }
1410
1411     if (end > (addr & ~511) + 512) {
1412         memcpy(sd->data, sd->buf + (addr & 511), 512 - (addr & 511));
1413
1414         if (bdrv_read(sd->bdrv, end >> 9, sd->buf, 1) == -1) {
1415             fprintf(stderr, "sd_blk_read: read error on host side\n");
1416             return;
1417         }
1418         memcpy(sd->data + 512 - (addr & 511), sd->buf, end & 511);
1419     } else
1420         memcpy(sd->data, sd->buf + (addr & 511), len);
1421 }
1422
1423 static void sd_blk_write(SDState *sd, uint64_t addr, uint32_t len)
1424 {
1425     uint64_t end = addr + len;
1426
1427     if ((addr & 511) || len < 512)
1428         if (!sd->bdrv || bdrv_read(sd->bdrv, addr >> 9, sd->buf, 1) == -1) {
1429             fprintf(stderr, "sd_blk_write: read error on host side\n");
1430             return;
1431         }
1432
1433     if (end > (addr & ~511) + 512) {
1434         memcpy(sd->buf + (addr & 511), sd->data, 512 - (addr & 511));
1435         if (bdrv_write(sd->bdrv, addr >> 9, sd->buf, 1) == -1) {
1436             fprintf(stderr, "sd_blk_write: write error on host side\n");
1437             return;
1438         }
1439
1440         if (bdrv_read(sd->bdrv, end >> 9, sd->buf, 1) == -1) {
1441             fprintf(stderr, "sd_blk_write: read error on host side\n");
1442             return;
1443         }
1444         memcpy(sd->buf, sd->data + 512 - (addr & 511), end & 511);
1445         if (bdrv_write(sd->bdrv, end >> 9, sd->buf, 1) == -1)
1446             fprintf(stderr, "sd_blk_write: write error on host side\n");
1447     } else {
1448         memcpy(sd->buf + (addr & 511), sd->data, len);
1449         if (!sd->bdrv || bdrv_write(sd->bdrv, addr >> 9, sd->buf, 1) == -1)
1450             fprintf(stderr, "sd_blk_write: write error on host side\n");
1451     }
1452 }
1453
1454 #define BLK_READ_BLOCK(a, len)  sd_blk_read(sd, a, len)
1455 #define BLK_WRITE_BLOCK(a, len) sd_blk_write(sd, a, len)
1456 #define APP_READ_BLOCK(a, len)  memset(sd->data, 0xec, len)
1457 #define APP_WRITE_BLOCK(a, len)
1458
1459 void sd_write_data(SDState *sd, uint8_t value)
1460 {
1461     int i;
1462
1463     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv) || !sd->enable)
1464         return;
1465
1466     if (sd->state != sd_receivingdata_state) {
1467         fprintf(stderr, "sd_write_data: not in Receiving-Data state\n");
1468         return;
1469     }
1470
1471     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1472         return;
1473
1474     switch (sd->current_cmd) {
1475     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
1476         sd->data[sd->data_offset ++] = value;
1477         if (sd->data_offset >= sd->blk_len) {
1478             /* TODO: Check CRC before committing */
1479             sd->state = sd_programming_state;
1480             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1481             sd->blk_written ++;
1482             sd->csd[14] |= 0x40;
1483             /* Bzzzzzzztt .... Operation complete.  */
1484             sd->state = sd_transfer_state;
1485         }
1486         break;
1487
1488     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
1489         if (sd->data_offset == 0) {
1490             /* Start of the block - lets check the address is valid */
1491             if (sd->data_start + sd->blk_len > sd->size) {
1492                 sd->card_status |= ADDRESS_ERROR;
1493                 break;
1494             }
1495             if (sd_wp_addr(sd, sd->data_start)) {
1496                 sd->card_status |= WP_VIOLATION;
1497                 break;
1498             }
1499         }
1500         sd->data[sd->data_offset++] = value;
1501         if (sd->data_offset >= sd->blk_len) {
1502             /* TODO: Check CRC before committing */
1503             sd->state = sd_programming_state;
1504             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1505             sd->blk_written++;
1506             sd->data_start += sd->blk_len;
1507             sd->data_offset = 0;
1508             sd->csd[14] |= 0x40;
1509
1510             /* Bzzzzzzztt .... Operation complete.  */
1511             sd->state = sd_receivingdata_state;
1512         }
1513         break;
1514
1515     case 26:    /* CMD26:  PROGRAM_CID */
1516         sd->data[sd->data_offset ++] = value;
1517         if (sd->data_offset >= sizeof(sd->cid)) {
1518             /* TODO: Check CRC before committing */
1519             sd->state = sd_programming_state;
1520             for (i = 0; i < sizeof(sd->cid); i ++)
1521                 if ((sd->cid[i] | 0x00) != sd->data[i])
1522                     sd->card_status |= CID_CSD_OVERWRITE;
1523
1524             if (!(sd->card_status & CID_CSD_OVERWRITE))
1525                 for (i = 0; i < sizeof(sd->cid); i ++) {
1526                     sd->cid[i] |= 0x00;
1527                     sd->cid[i] &= sd->data[i];
1528                 }
1529             /* Bzzzzzzztt .... Operation complete.  */
1530             sd->state = sd_transfer_state;
1531         }
1532         break;
1533
1534     case 27:    /* CMD27:  PROGRAM_CSD */
1535         sd->data[sd->data_offset ++] = value;
1536         if (sd->data_offset >= sizeof(sd->csd)) {
1537             /* TODO: Check CRC before committing */
1538             sd->state = sd_programming_state;
1539             for (i = 0; i < sizeof(sd->csd); i ++)
1540                 if ((sd->csd[i] | sd_csd_rw_mask[i]) !=
1541                     (sd->data[i] | sd_csd_rw_mask[i]))
1542                     sd->card_status |= CID_CSD_OVERWRITE;
1543
1544             /* Copy flag (OTP) & Permanent write protect */
1545             if (sd->csd[14] & ~sd->data[14] & 0x60)
1546                 sd->card_status |= CID_CSD_OVERWRITE;
1547
1548             if (!(sd->card_status & CID_CSD_OVERWRITE))
1549                 for (i = 0; i < sizeof(sd->csd); i ++) {
1550                     sd->csd[i] |= sd_csd_rw_mask[i];
1551                     sd->csd[i] &= sd->data[i];
1552                 }
1553             /* Bzzzzzzztt .... Operation complete.  */
1554             sd->state = sd_transfer_state;
1555         }
1556         break;
1557
1558     case 42:    /* CMD42:  LOCK_UNLOCK */
1559         sd->data[sd->data_offset ++] = value;
1560         if (sd->data_offset >= sd->blk_len) {
1561             /* TODO: Check CRC before committing */
1562             sd->state = sd_programming_state;
1563             sd_lock_command(sd);
1564             /* Bzzzzzzztt .... Operation complete.  */
1565             sd->state = sd_transfer_state;
1566         }
1567         break;
1568
1569     case 56:    /* CMD56:  GEN_CMD */
1570         sd->data[sd->data_offset ++] = value;
1571         if (sd->data_offset >= sd->blk_len) {
1572             APP_WRITE_BLOCK(sd->data_start, sd->data_offset);
1573             sd->state = sd_transfer_state;
1574         }
1575         break;
1576
1577     default:
1578         fprintf(stderr, "sd_write_data: unknown command\n");
1579         break;
1580     }
1581 }
1582
1583 uint8_t sd_read_data(SDState *sd)
1584 {
1585     /* TODO: Append CRCs */
1586     uint8_t ret;
1587     int io_len;
1588
1589     if (!sd->bdrv || !bdrv_is_inserted(sd->bdrv) || !sd->enable)
1590         return 0x00;
1591
1592     if (sd->state != sd_sendingdata_state) {
1593         fprintf(stderr, "sd_read_data: not in Sending-Data state\n");
1594         return 0x00;
1595     }
1596
1597     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1598         return 0x00;
1599
1600     io_len = (sd->ocr & (1 << 30)) ? 512 : sd->blk_len;
1601
1602     switch (sd->current_cmd) {
1603     case 6:     /* CMD6:   SWITCH_FUNCTION */
1604         ret = sd->data[sd->data_offset ++];
1605
1606         if (sd->data_offset >= 64)
1607             sd->state = sd_transfer_state;
1608         break;
1609
1610     case 9:     /* CMD9:   SEND_CSD */
1611     case 10:    /* CMD10:  SEND_CID */
1612         ret = sd->data[sd->data_offset ++];
1613
1614         if (sd->data_offset >= 16)
1615             sd->state = sd_transfer_state;
1616         break;
1617
1618     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
1619         if (sd->data_offset == 0)
1620             BLK_READ_BLOCK(sd->data_start, io_len);
1621         ret = sd->data[sd->data_offset ++];
1622
1623         if (sd->data_offset >= io_len) {
1624             sd->data_start += io_len;
1625             sd->data_offset = 0;
1626             if (sd->data_start + io_len > sd->size) {
1627                 sd->card_status |= ADDRESS_ERROR;
1628                 break;
1629             }
1630         }
1631         break;
1632
1633     case 13:    /* ACMD13: SD_STATUS */
1634         ret = sd->sd_status[sd->data_offset ++];
1635
1636         if (sd->data_offset >= sizeof(sd->sd_status))
1637             sd->state = sd_transfer_state;
1638         break;
1639
1640     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
1641         if (sd->data_offset == 0)
1642             BLK_READ_BLOCK(sd->data_start, io_len);
1643         ret = sd->data[sd->data_offset ++];
1644
1645         if (sd->data_offset >= io_len)
1646             sd->state = sd_transfer_state;
1647         break;
1648
1649     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
1650         if (sd->data_offset == 0)
1651             BLK_READ_BLOCK(sd->data_start, io_len);
1652         ret = sd->data[sd->data_offset ++];
1653
1654         if (sd->data_offset >= io_len) {
1655             sd->data_start += io_len;
1656             sd->data_offset = 0;
1657             if (sd->data_start + io_len > sd->size) {
1658                 sd->card_status |= ADDRESS_ERROR;
1659                 break;
1660             }
1661         }
1662         break;
1663
1664     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1665         ret = sd->data[sd->data_offset ++];
1666
1667         if (sd->data_offset >= 4)
1668             sd->state = sd_transfer_state;
1669         break;
1670
1671     case 30:    /* CMD30:  SEND_WRITE_PROT */
1672         ret = sd->data[sd->data_offset ++];
1673
1674         if (sd->data_offset >= 4)
1675             sd->state = sd_transfer_state;
1676         break;
1677
1678     case 51:    /* ACMD51: SEND_SCR */
1679         ret = sd->scr[sd->data_offset ++];
1680
1681         if (sd->data_offset >= sizeof(sd->scr))
1682             sd->state = sd_transfer_state;
1683         break;
1684
1685     case 56:    /* CMD56:  GEN_CMD */
1686         if (sd->data_offset == 0)
1687             APP_READ_BLOCK(sd->data_start, sd->blk_len);
1688         ret = sd->data[sd->data_offset ++];
1689
1690         if (sd->data_offset >= sd->blk_len)
1691             sd->state = sd_transfer_state;
1692         break;
1693
1694     default:
1695         fprintf(stderr, "sd_read_data: unknown command\n");
1696         return 0x00;
1697     }
1698
1699     return ret;
1700 }
1701
1702 int sd_data_ready(SDState *sd)
1703 {
1704     return sd->state == sd_sendingdata_state;
1705 }
1706
1707 void sd_enable(SDState *sd, int enable)
1708 {
1709     sd->enable = enable;
1710 }