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