hw/sd/sd.c

   1 /*
   2  * SD Memory Card emulation as defined in the "SD Memory Card Physical
   3  * layer specification, Version 1.10."
   4  *
   5  * Copyright (c) 2006 Andrzej Zaborowski  <[email protected]>
   6  * Copyright (c) 2007 CodeSourcery
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  *
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in
  16  *    the documentation and/or other materials provided with the
  17  *    distribution.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS''
  20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  21  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  22  * PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR
  23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  24  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  25  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  26  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
  27  * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  28  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 #include "hw/hw.h"
  33 #include "sysemu/block-backend.h"
  34 #include "hw/sd/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     g_free(sd->wp_groups);
 416     sd->wp_switch = sd->blk ? blk_is_read_only(sd->blk) : false;
 417     sd->wpgrps_size = sect;
 418     sd->wp_groups = bitmap_new(sd->wpgrps_size);
 419     memset(sd->function_group, 0, sizeof(sd->function_group));
 420     sd->erase_start = 0;
 421     sd->erase_end = 0;
 422     sd->size = size;
 423     sd->blk_len = 0x200;
 424     sd->pwd_len = 0;
 425     sd->expecting_acmd = false;
 426 }
 427
 428 static void sd_cardchange(void *opaque, bool load)
 429 {
 430     SDState *sd = opaque;
 431
 432     qemu_set_irq(sd->inserted_cb, blk_is_inserted(sd->blk));
 433     if (blk_is_inserted(sd->blk)) {
 434         sd_reset(sd);
 435         qemu_set_irq(sd->readonly_cb, sd->wp_switch);
 436     }
 437 }
 438
 439 static const BlockDevOps sd_block_ops = {
 440     .change_media_cb = sd_cardchange,
 441 };
 442
 443 static const VMStateDescription sd_vmstate = {
 444     .name = "sd-card",
 445     .version_id = 1,
 446     .minimum_version_id = 1,
 447     .fields = (VMStateField[]) {
 448         VMSTATE_UINT32(mode, SDState),
 449         VMSTATE_INT32(state, SDState),
 450         VMSTATE_UINT8_ARRAY(cid, SDState, 16),
 451         VMSTATE_UINT8_ARRAY(csd, SDState, 16),
 452         VMSTATE_UINT16(rca, SDState),
 453         VMSTATE_UINT32(card_status, SDState),
 454         VMSTATE_PARTIAL_BUFFER(sd_status, SDState, 1),
 455         VMSTATE_UINT32(vhs, SDState),
 456         VMSTATE_BITMAP(wp_groups, SDState, 0, wpgrps_size),
 457         VMSTATE_UINT32(blk_len, SDState),
 458         VMSTATE_UINT32(erase_start, SDState),
 459         VMSTATE_UINT32(erase_end, SDState),
 460         VMSTATE_UINT8_ARRAY(pwd, SDState, 16),
 461         VMSTATE_UINT32(pwd_len, SDState),
 462         VMSTATE_UINT8_ARRAY(function_group, SDState, 6),
 463         VMSTATE_UINT8(current_cmd, SDState),
 464         VMSTATE_BOOL(expecting_acmd, SDState),
 465         VMSTATE_UINT32(blk_written, SDState),
 466         VMSTATE_UINT64(data_start, SDState),
 467         VMSTATE_UINT32(data_offset, SDState),
 468         VMSTATE_UINT8_ARRAY(data, SDState, 512),
 469         VMSTATE_BUFFER_POINTER_UNSAFE(buf, SDState, 1, 512),
 470         VMSTATE_BOOL(enable, SDState),
 471         VMSTATE_END_OF_LIST()
 472     }
 473 };
 474
 475 /* We do not model the chip select pin, so allow the board to select
 476    whether card should be in SSI or MMC/SD mode.  It is also up to the
 477    board to ensure that ssi transfers only occur when the chip select
 478    is asserted.  */
 479 SDState *sd_init(BlockBackend *blk, bool is_spi)
 480 {
 481     SDState *sd;
 482
 483     if (blk && blk_is_read_only(blk)) {
 484         fprintf(stderr, "sd_init: Cannot use read-only drive\n");
 485         return NULL;
 486     }
 487
 488     sd = (SDState *) g_malloc0(sizeof(SDState));
 489     sd->buf = blk_blockalign(blk, 512);
 490     sd->spi = is_spi;
 491     sd->enable = true;
 492     sd->blk = blk;
 493     sd_reset(sd);
 494     if (sd->blk) {
 495         /* Attach dev if not already attached.  (This call ignores an
 496          * error return code if sd->blk is already attached.) */
 497         blk_attach_dev(sd->blk, sd);
 498         blk_set_dev_ops(sd->blk, &sd_block_ops, sd);
 499     }
 500     vmstate_register(NULL, -1, &sd_vmstate, sd);
 501     return sd;
 502 }
 503
 504 void sd_set_cb(SDState *sd, qemu_irq readonly, qemu_irq insert)
 505 {
 506     sd->readonly_cb = readonly;
 507     sd->inserted_cb = insert;
 508     qemu_set_irq(readonly, sd->blk ? blk_is_read_only(sd->blk) : 0);
 509     qemu_set_irq(insert, sd->blk ? blk_is_inserted(sd->blk) : 0);
 510 }
 511
 512 static void sd_erase(SDState *sd)
 513 {
 514     int i;
 515     uint64_t erase_start = sd->erase_start;
 516     uint64_t erase_end = sd->erase_end;
 517
 518     if (!sd->erase_start || !sd->erase_end) {
 519         sd->card_status |= ERASE_SEQ_ERROR;
 520         return;
 521     }
 522
 523     if (extract32(sd->ocr, OCR_CCS_BITN, 1)) {
 524         /* High capacity memory card: erase units are 512 byte blocks */
 525         erase_start *= 512;
 526         erase_end *= 512;
 527     }
 528
 529     erase_start = sd_addr_to_wpnum(erase_start);
 530     erase_end = sd_addr_to_wpnum(erase_end);
 531     sd->erase_start = 0;
 532     sd->erase_end = 0;
 533     sd->csd[14] |= 0x40;
 534
 535     for (i = erase_start; i <= erase_end; i++) {
 536         if (test_bit(i, sd->wp_groups)) {
 537             sd->card_status |= WP_ERASE_SKIP;
 538         }
 539     }
 540 }
 541
 542 static uint32_t sd_wpbits(SDState *sd, uint64_t addr)
 543 {
 544     uint32_t i, wpnum;
 545     uint32_t ret = 0;
 546
 547     wpnum = sd_addr_to_wpnum(addr);
 548
 549     for (i = 0; i < 32; i++, wpnum++, addr += WPGROUP_SIZE) {
 550         if (addr < sd->size && test_bit(wpnum, sd->wp_groups)) {
 551             ret |= (1 << i);
 552         }
 553     }
 554
 555     return ret;
 556 }
 557
 558 static void sd_function_switch(SDState *sd, uint32_t arg)
 559 {
 560     int i, mode, new_func, crc;
 561     mode = !!(arg & 0x80000000);
 562
 563     sd->data[0] = 0x00;         /* Maximum current consumption */
 564     sd->data[1] = 0x01;
 565     sd->data[2] = 0x80;         /* Supported group 6 functions */
 566     sd->data[3] = 0x01;
 567     sd->data[4] = 0x80;         /* Supported group 5 functions */
 568     sd->data[5] = 0x01;
 569     sd->data[6] = 0x80;         /* Supported group 4 functions */
 570     sd->data[7] = 0x01;
 571     sd->data[8] = 0x80;         /* Supported group 3 functions */
 572     sd->data[9] = 0x01;
 573     sd->data[10] = 0x80;        /* Supported group 2 functions */
 574     sd->data[11] = 0x43;
 575     sd->data[12] = 0x80;        /* Supported group 1 functions */
 576     sd->data[13] = 0x03;
 577     for (i = 0; i < 6; i ++) {
 578         new_func = (arg >> (i * 4)) & 0x0f;
 579         if (mode && new_func != 0x0f)
 580             sd->function_group[i] = new_func;
 581         sd->data[14 + (i >> 1)] = new_func << ((i * 4) & 4);
 582     }
 583     memset(&sd->data[17], 0, 47);
 584     crc = sd_crc16(sd->data, 64);
 585     sd->data[65] = crc >> 8;
 586     sd->data[66] = crc & 0xff;
 587 }
 588
 589 static inline bool sd_wp_addr(SDState *sd, uint64_t addr)
 590 {
 591     return test_bit(sd_addr_to_wpnum(addr), sd->wp_groups);
 592 }
 593
 594 static void sd_lock_command(SDState *sd)
 595 {
 596     int erase, lock, clr_pwd, set_pwd, pwd_len;
 597     erase = !!(sd->data[0] & 0x08);
 598     lock = sd->data[0] & 0x04;
 599     clr_pwd = sd->data[0] & 0x02;
 600     set_pwd = sd->data[0] & 0x01;
 601
 602     if (sd->blk_len > 1)
 603         pwd_len = sd->data[1];
 604     else
 605         pwd_len = 0;
 606
 607     if (erase) {
 608         if (!(sd->card_status & CARD_IS_LOCKED) || sd->blk_len > 1 ||
 609                         set_pwd || clr_pwd || lock || sd->wp_switch ||
 610                         (sd->csd[14] & 0x20)) {
 611             sd->card_status |= LOCK_UNLOCK_FAILED;
 612             return;
 613         }
 614         bitmap_zero(sd->wp_groups, sd->wpgrps_size);
 615         sd->csd[14] &= ~0x10;
 616         sd->card_status &= ~CARD_IS_LOCKED;
 617         sd->pwd_len = 0;
 618         /* Erasing the entire card here! */
 619         fprintf(stderr, "SD: Card force-erased by CMD42\n");
 620         return;
 621     }
 622
 623     if (sd->blk_len < 2 + pwd_len ||
 624                     pwd_len <= sd->pwd_len ||
 625                     pwd_len > sd->pwd_len + 16) {
 626         sd->card_status |= LOCK_UNLOCK_FAILED;
 627         return;
 628     }
 629
 630     if (sd->pwd_len && memcmp(sd->pwd, sd->data + 2, sd->pwd_len)) {
 631         sd->card_status |= LOCK_UNLOCK_FAILED;
 632         return;
 633     }
 634
 635     pwd_len -= sd->pwd_len;
 636     if ((pwd_len && !set_pwd) ||
 637                     (clr_pwd && (set_pwd || lock)) ||
 638                     (lock && !sd->pwd_len && !set_pwd) ||
 639                     (!set_pwd && !clr_pwd &&
 640                      (((sd->card_status & CARD_IS_LOCKED) && lock) ||
 641                       (!(sd->card_status & CARD_IS_LOCKED) && !lock)))) {
 642         sd->card_status |= LOCK_UNLOCK_FAILED;
 643         return;
 644     }
 645
 646     if (set_pwd) {
 647         memcpy(sd->pwd, sd->data + 2 + sd->pwd_len, pwd_len);
 648         sd->pwd_len = pwd_len;
 649     }
 650
 651     if (clr_pwd) {
 652         sd->pwd_len = 0;
 653     }
 654
 655     if (lock)
 656         sd->card_status |= CARD_IS_LOCKED;
 657     else
 658         sd->card_status &= ~CARD_IS_LOCKED;
 659 }
 660
 661 static sd_rsp_type_t sd_normal_command(SDState *sd,
 662                                        SDRequest req)
 663 {
 664     uint32_t rca = 0x0000;
 665     uint64_t addr = (sd->ocr & (1 << 30)) ? (uint64_t) req.arg << 9 : req.arg;
 666
 667     /* Not interpreting this as an app command */
 668     sd->card_status &= ~APP_CMD;
 669
 670     if (sd_cmd_type[req.cmd] == sd_ac || sd_cmd_type[req.cmd] == sd_adtc)
 671         rca = req.arg >> 16;
 672
 673     DPRINTF("CMD%d 0x%08x state %d\n", req.cmd, req.arg, sd->state);
 674     switch (req.cmd) {
 675     /* Basic commands (Class 0 and Class 1) */
 676     case 0:     /* CMD0:   GO_IDLE_STATE */
 677         switch (sd->state) {
 678         case sd_inactive_state:
 679             return sd->spi ? sd_r1 : sd_r0;
 680
 681         default:
 682             sd->state = sd_idle_state;
 683             sd_reset(sd);
 684             return sd->spi ? sd_r1 : sd_r0;
 685         }
 686         break;
 687
 688     case 1:     /* CMD1:   SEND_OP_CMD */
 689         if (!sd->spi)
 690             goto bad_cmd;
 691
 692         sd->state = sd_transfer_state;
 693         return sd_r1;
 694
 695     case 2:     /* CMD2:   ALL_SEND_CID */
 696         if (sd->spi)
 697             goto bad_cmd;
 698         switch (sd->state) {
 699         case sd_ready_state:
 700             sd->state = sd_identification_state;
 701             return sd_r2_i;
 702
 703         default:
 704             break;
 705         }
 706         break;
 707
 708     case 3:     /* CMD3:   SEND_RELATIVE_ADDR */
 709         if (sd->spi)
 710             goto bad_cmd;
 711         switch (sd->state) {
 712         case sd_identification_state:
 713         case sd_standby_state:
 714             sd->state = sd_standby_state;
 715             sd_set_rca(sd);
 716             return sd_r6;
 717
 718         default:
 719             break;
 720         }
 721         break;
 722
 723     case 4:     /* CMD4:   SEND_DSR */
 724         if (sd->spi)
 725             goto bad_cmd;
 726         switch (sd->state) {
 727         case sd_standby_state:
 728             break;
 729
 730         default:
 731             break;
 732         }
 733         break;
 734
 735     case 5: /* CMD5: reserved for SDIO cards */
 736         return sd_illegal;
 737
 738     case 6:     /* CMD6:   SWITCH_FUNCTION */
 739         if (sd->spi)
 740             goto bad_cmd;
 741         switch (sd->mode) {
 742         case sd_data_transfer_mode:
 743             sd_function_switch(sd, req.arg);
 744             sd->state = sd_sendingdata_state;
 745             sd->data_start = 0;
 746             sd->data_offset = 0;
 747             return sd_r1;
 748
 749         default:
 750             break;
 751         }
 752         break;
 753
 754     case 7:     /* CMD7:   SELECT/DESELECT_CARD */
 755         if (sd->spi)
 756             goto bad_cmd;
 757         switch (sd->state) {
 758         case sd_standby_state:
 759             if (sd->rca != rca)
 760                 return sd_r0;
 761
 762             sd->state = sd_transfer_state;
 763             return sd_r1b;
 764
 765         case sd_transfer_state:
 766         case sd_sendingdata_state:
 767             if (sd->rca == rca)
 768                 break;
 769
 770             sd->state = sd_standby_state;
 771             return sd_r1b;
 772
 773         case sd_disconnect_state:
 774             if (sd->rca != rca)
 775                 return sd_r0;
 776
 777             sd->state = sd_programming_state;
 778             return sd_r1b;
 779
 780         case sd_programming_state:
 781             if (sd->rca == rca)
 782                 break;
 783
 784             sd->state = sd_disconnect_state;
 785             return sd_r1b;
 786
 787         default:
 788             break;
 789         }
 790         break;
 791
 792     case 8:     /* CMD8:   SEND_IF_COND */
 793         /* Physical Layer Specification Version 2.00 command */
 794         switch (sd->state) {
 795         case sd_idle_state:
 796             sd->vhs = 0;
 797
 798             /* No response if not exactly one VHS bit is set.  */
 799             if (!(req.arg >> 8) || (req.arg >> (ctz32(req.arg & ~0xff) + 1))) {
 800                 return sd->spi ? sd_r7 : sd_r0;
 801             }
 802
 803             /* Accept.  */
 804             sd->vhs = req.arg;
 805             return sd_r7;
 806
 807         default:
 808             break;
 809         }
 810         break;
 811
 812     case 9:     /* CMD9:   SEND_CSD */
 813         switch (sd->state) {
 814         case sd_standby_state:
 815             if (sd->rca != rca)
 816                 return sd_r0;
 817
 818             return sd_r2_s;
 819
 820         case sd_transfer_state:
 821             if (!sd->spi)
 822                 break;
 823             sd->state = sd_sendingdata_state;
 824             memcpy(sd->data, sd->csd, 16);
 825             sd->data_start = addr;
 826             sd->data_offset = 0;
 827             return sd_r1;
 828
 829         default:
 830             break;
 831         }
 832         break;
 833
 834     case 10:    /* CMD10:  SEND_CID */
 835         switch (sd->state) {
 836         case sd_standby_state:
 837             if (sd->rca != rca)
 838                 return sd_r0;
 839
 840             return sd_r2_i;
 841
 842         case sd_transfer_state:
 843             if (!sd->spi)
 844                 break;
 845             sd->state = sd_sendingdata_state;
 846             memcpy(sd->data, sd->cid, 16);
 847             sd->data_start = addr;
 848             sd->data_offset = 0;
 849             return sd_r1;
 850
 851         default:
 852             break;
 853         }
 854         break;
 855
 856     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
 857         if (sd->spi)
 858             goto bad_cmd;
 859         switch (sd->state) {
 860         case sd_transfer_state:
 861             sd->state = sd_sendingdata_state;
 862             sd->data_start = req.arg;
 863             sd->data_offset = 0;
 864
 865             if (sd->data_start + sd->blk_len > sd->size)
 866                 sd->card_status |= ADDRESS_ERROR;
 867             return sd_r0;
 868
 869         default:
 870             break;
 871         }
 872         break;
 873
 874     case 12:    /* CMD12:  STOP_TRANSMISSION */
 875         switch (sd->state) {
 876         case sd_sendingdata_state:
 877             sd->state = sd_transfer_state;
 878             return sd_r1b;
 879
 880         case sd_receivingdata_state:
 881             sd->state = sd_programming_state;
 882             /* Bzzzzzzztt .... Operation complete.  */
 883             sd->state = sd_transfer_state;
 884             return sd_r1b;
 885
 886         default:
 887             break;
 888         }
 889         break;
 890
 891     case 13:    /* CMD13:  SEND_STATUS */
 892         switch (sd->mode) {
 893         case sd_data_transfer_mode:
 894             if (sd->rca != rca)
 895                 return sd_r0;
 896
 897             return sd_r1;
 898
 899         default:
 900             break;
 901         }
 902         break;
 903
 904     case 15:    /* CMD15:  GO_INACTIVE_STATE */
 905         if (sd->spi)
 906             goto bad_cmd;
 907         switch (sd->mode) {
 908         case sd_data_transfer_mode:
 909             if (sd->rca != rca)
 910                 return sd_r0;
 911
 912             sd->state = sd_inactive_state;
 913             return sd_r0;
 914
 915         default:
 916             break;
 917         }
 918         break;
 919
 920     /* Block read commands (Classs 2) */
 921     case 16:    /* CMD16:  SET_BLOCKLEN */
 922         switch (sd->state) {
 923         case sd_transfer_state:
 924             if (req.arg > (1 << HWBLOCK_SHIFT))
 925                 sd->card_status |= BLOCK_LEN_ERROR;
 926             else
 927                 sd->blk_len = req.arg;
 928
 929             return sd_r1;
 930
 931         default:
 932             break;
 933         }
 934         break;
 935
 936     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
 937         switch (sd->state) {
 938         case sd_transfer_state:
 939             sd->state = sd_sendingdata_state;
 940             sd->data_start = addr;
 941             sd->data_offset = 0;
 942
 943             if (sd->data_start + sd->blk_len > sd->size)
 944                 sd->card_status |= ADDRESS_ERROR;
 945             return sd_r1;
 946
 947         default:
 948             break;
 949         }
 950         break;
 951
 952     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
 953         switch (sd->state) {
 954         case sd_transfer_state:
 955             sd->state = sd_sendingdata_state;
 956             sd->data_start = addr;
 957             sd->data_offset = 0;
 958
 959             if (sd->data_start + sd->blk_len > sd->size)
 960                 sd->card_status |= ADDRESS_ERROR;
 961             return sd_r1;
 962
 963         default:
 964             break;
 965         }
 966         break;
 967
 968     /* Block write commands (Class 4) */
 969     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
 970         if (sd->spi)
 971             goto unimplemented_cmd;
 972         switch (sd->state) {
 973         case sd_transfer_state:
 974             /* Writing in SPI mode not implemented.  */
 975             if (sd->spi)
 976                 break;
 977             sd->state = sd_receivingdata_state;
 978             sd->data_start = addr;
 979             sd->data_offset = 0;
 980             sd->blk_written = 0;
 981
 982             if (sd->data_start + sd->blk_len > sd->size)
 983                 sd->card_status |= ADDRESS_ERROR;
 984             if (sd_wp_addr(sd, sd->data_start))
 985                 sd->card_status |= WP_VIOLATION;
 986             if (sd->csd[14] & 0x30)
 987                 sd->card_status |= WP_VIOLATION;
 988             return sd_r1;
 989
 990         default:
 991             break;
 992         }
 993         break;
 994
 995     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
 996         if (sd->spi)
 997             goto unimplemented_cmd;
 998         switch (sd->state) {
 999         case sd_transfer_state:
1000             /* Writing in SPI mode not implemented.  */
1001             if (sd->spi)
1002                 break;
1003             sd->state = sd_receivingdata_state;
1004             sd->data_start = addr;
1005             sd->data_offset = 0;
1006             sd->blk_written = 0;
1007
1008             if (sd->data_start + sd->blk_len > sd->size)
1009                 sd->card_status |= ADDRESS_ERROR;
1010             if (sd_wp_addr(sd, sd->data_start))
1011                 sd->card_status |= WP_VIOLATION;
1012             if (sd->csd[14] & 0x30)
1013                 sd->card_status |= WP_VIOLATION;
1014             return sd_r1;
1015
1016         default:
1017             break;
1018         }
1019         break;
1020
1021     case 26:    /* CMD26:  PROGRAM_CID */
1022         if (sd->spi)
1023             goto bad_cmd;
1024         switch (sd->state) {
1025         case sd_transfer_state:
1026             sd->state = sd_receivingdata_state;
1027             sd->data_start = 0;
1028             sd->data_offset = 0;
1029             return sd_r1;
1030
1031         default:
1032             break;
1033         }
1034         break;
1035
1036     case 27:    /* CMD27:  PROGRAM_CSD */
1037         if (sd->spi)
1038             goto unimplemented_cmd;
1039         switch (sd->state) {
1040         case sd_transfer_state:
1041             sd->state = sd_receivingdata_state;
1042             sd->data_start = 0;
1043             sd->data_offset = 0;
1044             return sd_r1;
1045
1046         default:
1047             break;
1048         }
1049         break;
1050
1051     /* Write protection (Class 6) */
1052     case 28:    /* CMD28:  SET_WRITE_PROT */
1053         switch (sd->state) {
1054         case sd_transfer_state:
1055             if (addr >= sd->size) {
1056                 sd->card_status |= ADDRESS_ERROR;
1057                 return sd_r1b;
1058             }
1059
1060             sd->state = sd_programming_state;
1061             set_bit(sd_addr_to_wpnum(addr), sd->wp_groups);
1062             /* Bzzzzzzztt .... Operation complete.  */
1063             sd->state = sd_transfer_state;
1064             return sd_r1b;
1065
1066         default:
1067             break;
1068         }
1069         break;
1070
1071     case 29:    /* CMD29:  CLR_WRITE_PROT */
1072         switch (sd->state) {
1073         case sd_transfer_state:
1074             if (addr >= sd->size) {
1075                 sd->card_status |= ADDRESS_ERROR;
1076                 return sd_r1b;
1077             }
1078
1079             sd->state = sd_programming_state;
1080             clear_bit(sd_addr_to_wpnum(addr), sd->wp_groups);
1081             /* Bzzzzzzztt .... Operation complete.  */
1082             sd->state = sd_transfer_state;
1083             return sd_r1b;
1084
1085         default:
1086             break;
1087         }
1088         break;
1089
1090     case 30:    /* CMD30:  SEND_WRITE_PROT */
1091         switch (sd->state) {
1092         case sd_transfer_state:
1093             sd->state = sd_sendingdata_state;
1094             *(uint32_t *) sd->data = sd_wpbits(sd, req.arg);
1095             sd->data_start = addr;
1096             sd->data_offset = 0;
1097             return sd_r1b;
1098
1099         default:
1100             break;
1101         }
1102         break;
1103
1104     /* Erase commands (Class 5) */
1105     case 32:    /* CMD32:  ERASE_WR_BLK_START */
1106         switch (sd->state) {
1107         case sd_transfer_state:
1108             sd->erase_start = req.arg;
1109             return sd_r1;
1110
1111         default:
1112             break;
1113         }
1114         break;
1115
1116     case 33:    /* CMD33:  ERASE_WR_BLK_END */
1117         switch (sd->state) {
1118         case sd_transfer_state:
1119             sd->erase_end = req.arg;
1120             return sd_r1;
1121
1122         default:
1123             break;
1124         }
1125         break;
1126
1127     case 38:    /* CMD38:  ERASE */
1128         switch (sd->state) {
1129         case sd_transfer_state:
1130             if (sd->csd[14] & 0x30) {
1131                 sd->card_status |= WP_VIOLATION;
1132                 return sd_r1b;
1133             }
1134
1135             sd->state = sd_programming_state;
1136             sd_erase(sd);
1137             /* Bzzzzzzztt .... Operation complete.  */
1138             sd->state = sd_transfer_state;
1139             return sd_r1b;
1140
1141         default:
1142             break;
1143         }
1144         break;
1145
1146     /* Lock card commands (Class 7) */
1147     case 42:    /* CMD42:  LOCK_UNLOCK */
1148         if (sd->spi)
1149             goto unimplemented_cmd;
1150         switch (sd->state) {
1151         case sd_transfer_state:
1152             sd->state = sd_receivingdata_state;
1153             sd->data_start = 0;
1154             sd->data_offset = 0;
1155             return sd_r1;
1156
1157         default:
1158             break;
1159         }
1160         break;
1161
1162     case 52:
1163     case 53:
1164         /* CMD52, CMD53: reserved for SDIO cards
1165          * (see the SDIO Simplified Specification V2.0)
1166          * Handle as illegal command but do not complain
1167          * on stderr, as some OSes may use these in their
1168          * probing for presence of an SDIO card.
1169          */
1170         return sd_illegal;
1171
1172     /* Application specific commands (Class 8) */
1173     case 55:    /* CMD55:  APP_CMD */
1174         if (sd->rca != rca)
1175             return sd_r0;
1176
1177         sd->expecting_acmd = true;
1178         sd->card_status |= APP_CMD;
1179         return sd_r1;
1180
1181     case 56:    /* CMD56:  GEN_CMD */
1182         fprintf(stderr, "SD: GEN_CMD 0x%08x\n", req.arg);
1183
1184         switch (sd->state) {
1185         case sd_transfer_state:
1186             sd->data_offset = 0;
1187             if (req.arg & 1)
1188                 sd->state = sd_sendingdata_state;
1189             else
1190                 sd->state = sd_receivingdata_state;
1191             return sd_r1;
1192
1193         default:
1194             break;
1195         }
1196         break;
1197
1198     default:
1199     bad_cmd:
1200         fprintf(stderr, "SD: Unknown CMD%i\n", req.cmd);
1201         return sd_illegal;
1202
1203     unimplemented_cmd:
1204         /* Commands that are recognised but not yet implemented in SPI mode.  */
1205         fprintf(stderr, "SD: CMD%i not implemented in SPI mode\n", req.cmd);
1206         return sd_illegal;
1207     }
1208
1209     fprintf(stderr, "SD: CMD%i in a wrong state\n", req.cmd);
1210     return sd_illegal;
1211 }
1212
1213 static sd_rsp_type_t sd_app_command(SDState *sd,
1214                                     SDRequest req)
1215 {
1216     DPRINTF("ACMD%d 0x%08x\n", req.cmd, req.arg);
1217     sd->card_status |= APP_CMD;
1218     switch (req.cmd) {
1219     case 6:     /* ACMD6:  SET_BUS_WIDTH */
1220         switch (sd->state) {
1221         case sd_transfer_state:
1222             sd->sd_status[0] &= 0x3f;
1223             sd->sd_status[0] |= (req.arg & 0x03) << 6;
1224             return sd_r1;
1225
1226         default:
1227             break;
1228         }
1229         break;
1230
1231     case 13:    /* ACMD13: SD_STATUS */
1232         switch (sd->state) {
1233         case sd_transfer_state:
1234             sd->state = sd_sendingdata_state;
1235             sd->data_start = 0;
1236             sd->data_offset = 0;
1237             return sd_r1;
1238
1239         default:
1240             break;
1241         }
1242         break;
1243
1244     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1245         switch (sd->state) {
1246         case sd_transfer_state:
1247             *(uint32_t *) sd->data = sd->blk_written;
1248
1249             sd->state = sd_sendingdata_state;
1250             sd->data_start = 0;
1251             sd->data_offset = 0;
1252             return sd_r1;
1253
1254         default:
1255             break;
1256         }
1257         break;
1258
1259     case 23:    /* ACMD23: SET_WR_BLK_ERASE_COUNT */
1260         switch (sd->state) {
1261         case sd_transfer_state:
1262             return sd_r1;
1263
1264         default:
1265             break;
1266         }
1267         break;
1268
1269     case 41:    /* ACMD41: SD_APP_OP_COND */
1270         if (sd->spi) {
1271             /* SEND_OP_CMD */
1272             sd->state = sd_transfer_state;
1273             return sd_r1;
1274         }
1275         switch (sd->state) {
1276         case sd_idle_state:
1277             /* We accept any voltage.  10000 V is nothing.
1278              *
1279              * We don't model init delay so just advance straight to ready state
1280              * unless it's an enquiry ACMD41 (bits 23:0 == 0).
1281              */
1282             if (req.arg & ACMD41_ENQUIRY_MASK) {
1283                 sd->state = sd_ready_state;
1284             }
1285
1286             return sd_r3;
1287
1288         default:
1289             break;
1290         }
1291         break;
1292
1293     case 42:    /* ACMD42: SET_CLR_CARD_DETECT */
1294         switch (sd->state) {
1295         case sd_transfer_state:
1296             /* Bringing in the 50KOhm pull-up resistor... Done.  */
1297             return sd_r1;
1298
1299         default:
1300             break;
1301         }
1302         break;
1303
1304     case 51:    /* ACMD51: SEND_SCR */
1305         switch (sd->state) {
1306         case sd_transfer_state:
1307             sd->state = sd_sendingdata_state;
1308             sd->data_start = 0;
1309             sd->data_offset = 0;
1310             return sd_r1;
1311
1312         default:
1313             break;
1314         }
1315         break;
1316
1317     default:
1318         /* Fall back to standard commands.  */
1319         return sd_normal_command(sd, req);
1320     }
1321
1322     fprintf(stderr, "SD: ACMD%i in a wrong state\n", req.cmd);
1323     return sd_illegal;
1324 }
1325
1326 static int cmd_valid_while_locked(SDState *sd, SDRequest *req)
1327 {
1328     /* Valid commands in locked state:
1329      * basic class (0)
1330      * lock card class (7)
1331      * CMD16
1332      * implicitly, the ACMD prefix CMD55
1333      * ACMD41 and ACMD42
1334      * Anything else provokes an "illegal command" response.
1335      */
1336     if (sd->expecting_acmd) {
1337         return req->cmd == 41 || req->cmd == 42;
1338     }
1339     if (req->cmd == 16 || req->cmd == 55) {
1340         return 1;
1341     }
1342     return sd_cmd_class[req->cmd] == 0 || sd_cmd_class[req->cmd] == 7;
1343 }
1344
1345 int sd_do_command(SDState *sd, SDRequest *req,
1346                   uint8_t *response) {
1347     int last_state;
1348     sd_rsp_type_t rtype;
1349     int rsplen;
1350
1351     if (!sd->blk || !blk_is_inserted(sd->blk) || !sd->enable) {
1352         return 0;
1353     }
1354
1355     if (sd_req_crc_validate(req)) {
1356         sd->card_status |= COM_CRC_ERROR;
1357         rtype = sd_illegal;
1358         goto send_response;
1359     }
1360
1361     if (sd->card_status & CARD_IS_LOCKED) {
1362         if (!cmd_valid_while_locked(sd, req)) {
1363             sd->card_status |= ILLEGAL_COMMAND;
1364             sd->expecting_acmd = false;
1365             fprintf(stderr, "SD: Card is locked\n");
1366             rtype = sd_illegal;
1367             goto send_response;
1368         }
1369     }
1370
1371     last_state = sd->state;
1372     sd_set_mode(sd);
1373
1374     if (sd->expecting_acmd) {
1375         sd->expecting_acmd = false;
1376         rtype = sd_app_command(sd, *req);
1377     } else {
1378         rtype = sd_normal_command(sd, *req);
1379     }
1380
1381     if (rtype == sd_illegal) {
1382         sd->card_status |= ILLEGAL_COMMAND;
1383     } else {
1384         /* Valid command, we can update the 'state before command' bits.
1385          * (Do this now so they appear in r1 responses.)
1386          */
1387         sd->current_cmd = req->cmd;
1388         sd->card_status &= ~CURRENT_STATE;
1389         sd->card_status |= (last_state << 9);
1390     }
1391
1392 send_response:
1393     switch (rtype) {
1394     case sd_r1:
1395     case sd_r1b:
1396         sd_response_r1_make(sd, response);
1397         rsplen = 4;
1398         break;
1399
1400     case sd_r2_i:
1401         memcpy(response, sd->cid, sizeof(sd->cid));
1402         rsplen = 16;
1403         break;
1404
1405     case sd_r2_s:
1406         memcpy(response, sd->csd, sizeof(sd->csd));
1407         rsplen = 16;
1408         break;
1409
1410     case sd_r3:
1411         sd_response_r3_make(sd, response);
1412         rsplen = 4;
1413         break;
1414
1415     case sd_r6:
1416         sd_response_r6_make(sd, response);
1417         rsplen = 4;
1418         break;
1419
1420     case sd_r7:
1421         sd_response_r7_make(sd, response);
1422         rsplen = 4;
1423         break;
1424
1425     case sd_r0:
1426     case sd_illegal:
1427     default:
1428         rsplen = 0;
1429         break;
1430     }
1431
1432     if (rtype != sd_illegal) {
1433         /* Clear the "clear on valid command" status bits now we've
1434          * sent any response
1435          */
1436         sd->card_status &= ~CARD_STATUS_B;
1437     }
1438
1439 #ifdef DEBUG_SD
1440     if (rsplen) {
1441         int i;
1442         DPRINTF("Response:");
1443         for (i = 0; i < rsplen; i++)
1444             fprintf(stderr, " %02x", response[i]);
1445         fprintf(stderr, " state %d\n", sd->state);
1446     } else {
1447         DPRINTF("No response %d\n", sd->state);
1448     }
1449 #endif
1450
1451     return rsplen;
1452 }
1453
1454 static void sd_blk_read(SDState *sd, uint64_t addr, uint32_t len)
1455 {
1456     uint64_t end = addr + len;
1457
1458     DPRINTF("sd_blk_read: addr = 0x%08llx, len = %d\n",
1459             (unsigned long long) addr, len);
1460     if (!sd->blk || blk_read(sd->blk, addr >> 9, sd->buf, 1) < 0) {
1461         fprintf(stderr, "sd_blk_read: read error on host side\n");
1462         return;
1463     }
1464
1465     if (end > (addr & ~511) + 512) {
1466         memcpy(sd->data, sd->buf + (addr & 511), 512 - (addr & 511));
1467
1468         if (blk_read(sd->blk, end >> 9, sd->buf, 1) < 0) {
1469             fprintf(stderr, "sd_blk_read: read error on host side\n");
1470             return;
1471         }
1472         memcpy(sd->data + 512 - (addr & 511), sd->buf, end & 511);
1473     } else
1474         memcpy(sd->data, sd->buf + (addr & 511), len);
1475 }
1476
1477 static void sd_blk_write(SDState *sd, uint64_t addr, uint32_t len)
1478 {
1479     uint64_t end = addr + len;
1480
1481     if ((addr & 511) || len < 512)
1482         if (!sd->blk || blk_read(sd->blk, addr >> 9, sd->buf, 1) < 0) {
1483             fprintf(stderr, "sd_blk_write: read error on host side\n");
1484             return;
1485         }
1486
1487     if (end > (addr & ~511) + 512) {
1488         memcpy(sd->buf + (addr & 511), sd->data, 512 - (addr & 511));
1489         if (blk_write(sd->blk, addr >> 9, sd->buf, 1) < 0) {
1490             fprintf(stderr, "sd_blk_write: write error on host side\n");
1491             return;
1492         }
1493
1494         if (blk_read(sd->blk, end >> 9, sd->buf, 1) < 0) {
1495             fprintf(stderr, "sd_blk_write: read error on host side\n");
1496             return;
1497         }
1498         memcpy(sd->buf, sd->data + 512 - (addr & 511), end & 511);
1499         if (blk_write(sd->blk, end >> 9, sd->buf, 1) < 0) {
1500             fprintf(stderr, "sd_blk_write: write error on host side\n");
1501         }
1502     } else {
1503         memcpy(sd->buf + (addr & 511), sd->data, len);
1504         if (!sd->blk || blk_write(sd->blk, addr >> 9, sd->buf, 1) < 0) {
1505             fprintf(stderr, "sd_blk_write: write error on host side\n");
1506         }
1507     }
1508 }
1509
1510 #define BLK_READ_BLOCK(a, len)  sd_blk_read(sd, a, len)
1511 #define BLK_WRITE_BLOCK(a, len) sd_blk_write(sd, a, len)
1512 #define APP_READ_BLOCK(a, len)  memset(sd->data, 0xec, len)
1513 #define APP_WRITE_BLOCK(a, len)
1514
1515 void sd_write_data(SDState *sd, uint8_t value)
1516 {
1517     int i;
1518
1519     if (!sd->blk || !blk_is_inserted(sd->blk) || !sd->enable)
1520         return;
1521
1522     if (sd->state != sd_receivingdata_state) {
1523         fprintf(stderr, "sd_write_data: not in Receiving-Data state\n");
1524         return;
1525     }
1526
1527     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1528         return;
1529
1530     switch (sd->current_cmd) {
1531     case 24:    /* CMD24:  WRITE_SINGLE_BLOCK */
1532         sd->data[sd->data_offset ++] = value;
1533         if (sd->data_offset >= sd->blk_len) {
1534             /* TODO: Check CRC before committing */
1535             sd->state = sd_programming_state;
1536             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1537             sd->blk_written ++;
1538             sd->csd[14] |= 0x40;
1539             /* Bzzzzzzztt .... Operation complete.  */
1540             sd->state = sd_transfer_state;
1541         }
1542         break;
1543
1544     case 25:    /* CMD25:  WRITE_MULTIPLE_BLOCK */
1545         if (sd->data_offset == 0) {
1546             /* Start of the block - let's check the address is valid */
1547             if (sd->data_start + sd->blk_len > sd->size) {
1548                 sd->card_status |= ADDRESS_ERROR;
1549                 break;
1550             }
1551             if (sd_wp_addr(sd, sd->data_start)) {
1552                 sd->card_status |= WP_VIOLATION;
1553                 break;
1554             }
1555         }
1556         sd->data[sd->data_offset++] = value;
1557         if (sd->data_offset >= sd->blk_len) {
1558             /* TODO: Check CRC before committing */
1559             sd->state = sd_programming_state;
1560             BLK_WRITE_BLOCK(sd->data_start, sd->data_offset);
1561             sd->blk_written++;
1562             sd->data_start += sd->blk_len;
1563             sd->data_offset = 0;
1564             sd->csd[14] |= 0x40;
1565
1566             /* Bzzzzzzztt .... Operation complete.  */
1567             sd->state = sd_receivingdata_state;
1568         }
1569         break;
1570
1571     case 26:    /* CMD26:  PROGRAM_CID */
1572         sd->data[sd->data_offset ++] = value;
1573         if (sd->data_offset >= sizeof(sd->cid)) {
1574             /* TODO: Check CRC before committing */
1575             sd->state = sd_programming_state;
1576             for (i = 0; i < sizeof(sd->cid); i ++)
1577                 if ((sd->cid[i] | 0x00) != sd->data[i])
1578                     sd->card_status |= CID_CSD_OVERWRITE;
1579
1580             if (!(sd->card_status & CID_CSD_OVERWRITE))
1581                 for (i = 0; i < sizeof(sd->cid); i ++) {
1582                     sd->cid[i] |= 0x00;
1583                     sd->cid[i] &= sd->data[i];
1584                 }
1585             /* Bzzzzzzztt .... Operation complete.  */
1586             sd->state = sd_transfer_state;
1587         }
1588         break;
1589
1590     case 27:    /* CMD27:  PROGRAM_CSD */
1591         sd->data[sd->data_offset ++] = value;
1592         if (sd->data_offset >= sizeof(sd->csd)) {
1593             /* TODO: Check CRC before committing */
1594             sd->state = sd_programming_state;
1595             for (i = 0; i < sizeof(sd->csd); i ++)
1596                 if ((sd->csd[i] | sd_csd_rw_mask[i]) !=
1597                     (sd->data[i] | sd_csd_rw_mask[i]))
1598                     sd->card_status |= CID_CSD_OVERWRITE;
1599
1600             /* Copy flag (OTP) & Permanent write protect */
1601             if (sd->csd[14] & ~sd->data[14] & 0x60)
1602                 sd->card_status |= CID_CSD_OVERWRITE;
1603
1604             if (!(sd->card_status & CID_CSD_OVERWRITE))
1605                 for (i = 0; i < sizeof(sd->csd); i ++) {
1606                     sd->csd[i] |= sd_csd_rw_mask[i];
1607                     sd->csd[i] &= sd->data[i];
1608                 }
1609             /* Bzzzzzzztt .... Operation complete.  */
1610             sd->state = sd_transfer_state;
1611         }
1612         break;
1613
1614     case 42:    /* CMD42:  LOCK_UNLOCK */
1615         sd->data[sd->data_offset ++] = value;
1616         if (sd->data_offset >= sd->blk_len) {
1617             /* TODO: Check CRC before committing */
1618             sd->state = sd_programming_state;
1619             sd_lock_command(sd);
1620             /* Bzzzzzzztt .... Operation complete.  */
1621             sd->state = sd_transfer_state;
1622         }
1623         break;
1624
1625     case 56:    /* CMD56:  GEN_CMD */
1626         sd->data[sd->data_offset ++] = value;
1627         if (sd->data_offset >= sd->blk_len) {
1628             APP_WRITE_BLOCK(sd->data_start, sd->data_offset);
1629             sd->state = sd_transfer_state;
1630         }
1631         break;
1632
1633     default:
1634         fprintf(stderr, "sd_write_data: unknown command\n");
1635         break;
1636     }
1637 }
1638
1639 uint8_t sd_read_data(SDState *sd)
1640 {
1641     /* TODO: Append CRCs */
1642     uint8_t ret;
1643     int io_len;
1644
1645     if (!sd->blk || !blk_is_inserted(sd->blk) || !sd->enable)
1646         return 0x00;
1647
1648     if (sd->state != sd_sendingdata_state) {
1649         fprintf(stderr, "sd_read_data: not in Sending-Data state\n");
1650         return 0x00;
1651     }
1652
1653     if (sd->card_status & (ADDRESS_ERROR | WP_VIOLATION))
1654         return 0x00;
1655
1656     io_len = (sd->ocr & (1 << 30)) ? 512 : sd->blk_len;
1657
1658     switch (sd->current_cmd) {
1659     case 6:     /* CMD6:   SWITCH_FUNCTION */
1660         ret = sd->data[sd->data_offset ++];
1661
1662         if (sd->data_offset >= 64)
1663             sd->state = sd_transfer_state;
1664         break;
1665
1666     case 9:     /* CMD9:   SEND_CSD */
1667     case 10:    /* CMD10:  SEND_CID */
1668         ret = sd->data[sd->data_offset ++];
1669
1670         if (sd->data_offset >= 16)
1671             sd->state = sd_transfer_state;
1672         break;
1673
1674     case 11:    /* CMD11:  READ_DAT_UNTIL_STOP */
1675         if (sd->data_offset == 0)
1676             BLK_READ_BLOCK(sd->data_start, io_len);
1677         ret = sd->data[sd->data_offset ++];
1678
1679         if (sd->data_offset >= io_len) {
1680             sd->data_start += io_len;
1681             sd->data_offset = 0;
1682             if (sd->data_start + io_len > sd->size) {
1683                 sd->card_status |= ADDRESS_ERROR;
1684                 break;
1685             }
1686         }
1687         break;
1688
1689     case 13:    /* ACMD13: SD_STATUS */
1690         ret = sd->sd_status[sd->data_offset ++];
1691
1692         if (sd->data_offset >= sizeof(sd->sd_status))
1693             sd->state = sd_transfer_state;
1694         break;
1695
1696     case 17:    /* CMD17:  READ_SINGLE_BLOCK */
1697         if (sd->data_offset == 0)
1698             BLK_READ_BLOCK(sd->data_start, io_len);
1699         ret = sd->data[sd->data_offset ++];
1700
1701         if (sd->data_offset >= io_len)
1702             sd->state = sd_transfer_state;
1703         break;
1704
1705     case 18:    /* CMD18:  READ_MULTIPLE_BLOCK */
1706         if (sd->data_offset == 0)
1707             BLK_READ_BLOCK(sd->data_start, io_len);
1708         ret = sd->data[sd->data_offset ++];
1709
1710         if (sd->data_offset >= io_len) {
1711             sd->data_start += io_len;
1712             sd->data_offset = 0;
1713             if (sd->data_start + io_len > sd->size) {
1714                 sd->card_status |= ADDRESS_ERROR;
1715                 break;
1716             }
1717         }
1718         break;
1719
1720     case 22:    /* ACMD22: SEND_NUM_WR_BLOCKS */
1721         ret = sd->data[sd->data_offset ++];
1722
1723         if (sd->data_offset >= 4)
1724             sd->state = sd_transfer_state;
1725         break;
1726
1727     case 30:    /* CMD30:  SEND_WRITE_PROT */
1728         ret = sd->data[sd->data_offset ++];
1729
1730         if (sd->data_offset >= 4)
1731             sd->state = sd_transfer_state;
1732         break;
1733
1734     case 51:    /* ACMD51: SEND_SCR */
1735         ret = sd->scr[sd->data_offset ++];
1736
1737         if (sd->data_offset >= sizeof(sd->scr))
1738             sd->state = sd_transfer_state;
1739         break;
1740
1741     case 56:    /* CMD56:  GEN_CMD */
1742         if (sd->data_offset == 0)
1743             APP_READ_BLOCK(sd->data_start, sd->blk_len);
1744         ret = sd->data[sd->data_offset ++];
1745
1746         if (sd->data_offset >= sd->blk_len)
1747             sd->state = sd_transfer_state;
1748         break;
1749
1750     default:
1751         fprintf(stderr, "sd_read_data: unknown command\n");
1752         return 0x00;
1753     }
1754
1755     return ret;
1756 }
1757
1758 bool sd_data_ready(SDState *sd)
1759 {
1760     return sd->state == sd_sendingdata_state;
1761 }
1762
1763 void sd_enable(SDState *sd, bool enable)
1764 {
1765     sd->enable = enable;
1766 }