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