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