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