hw/sd.c

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