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