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