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