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