diff options
-rw-r--r-- | firmware/target/arm/ata-sd-pp.c | 59 |
1 files changed, 43 insertions, 16 deletions
diff --git a/firmware/target/arm/ata-sd-pp.c b/firmware/target/arm/ata-sd-pp.c index 4711c98769..cde474ca29 100644 --- a/firmware/target/arm/ata-sd-pp.c +++ b/firmware/target/arm/ata-sd-pp.c | |||
@@ -481,15 +481,47 @@ static inline void copy_read_sectors_slow(unsigned char** buf) | |||
481 | /* Writes have to be kept slow for now */ | 481 | /* Writes have to be kept slow for now */ |
482 | static inline void copy_write_sectors(const unsigned char** buf) | 482 | static inline void copy_write_sectors(const unsigned char** buf) |
483 | { | 483 | { |
484 | int cnt = FIFO_LEN; | 484 | int cnt = FIFO_LEN - 1; |
485 | unsigned t; | 485 | unsigned t; |
486 | long time; | ||
486 | 487 | ||
487 | do | 488 | time = USEC_TIMER + 3; |
489 | if (((intptr_t)*buf & 3) == 0) | ||
490 | { | ||
491 | asm volatile ( | ||
492 | "ldmia %[buf]!, { r3, r5, r7, r9 } \r\n" | ||
493 | "mov r4, r3, lsr #16 \r\n" | ||
494 | "mov r6, r5, lsr #16 \r\n" | ||
495 | "mov r8, r7, lsr #16 \r\n" | ||
496 | "mov r10, r9, lsr #16 \r\n" | ||
497 | "stmia %[data], { r3-r10 } \r\n" | ||
498 | "ldmia %[buf]!, { r3, r5, r7, r9 } \r\n" | ||
499 | "mov r4, r3, lsr #16 \r\n" | ||
500 | "mov r6, r5, lsr #16 \r\n" | ||
501 | "mov r8, r7, lsr #16 \r\n" | ||
502 | "mov %[t], r9, lsr #16 \r\n" | ||
503 | "stmia %[data], { r3-r9 } \r\n" | ||
504 | : [buf]"+&r"(*buf), [t]"=&r"(t) | ||
505 | : [data]"r"(&MMC_DATA_FIFO) | ||
506 | : "r3", "r4", "r5", "r6", "r7", "r8", "r9", "r10" | ||
507 | ); | ||
508 | } | ||
509 | else | ||
488 | { | 510 | { |
511 | do | ||
512 | { | ||
513 | t = *(*buf)++; | ||
514 | t |= *(*buf)++ << 8; | ||
515 | MMC_DATA_FIFO = t; | ||
516 | } while (--cnt > 0); /* tail loop is faster */ | ||
489 | t = *(*buf)++; | 517 | t = *(*buf)++; |
490 | t |= *(*buf)++ << 8; | 518 | t |= *(*buf)++ << 8; |
491 | MMC_DATA_FIFO = t; | 519 | } |
492 | } while (--cnt > 0); /* tail loop is faster */ | 520 | /* Don't write the last word before at least 3 usec have elapsed since FIFO_EMPTY */ |
521 | /* This prevents the 'two bytes inserted' bug. */ | ||
522 | |||
523 | while (!TIME_AFTER(USEC_TIMER, time)); | ||
524 | MMC_DATA_FIFO = t; | ||
493 | } | 525 | } |
494 | 526 | ||
495 | static int sd_select_bank(unsigned char bank) | 527 | static int sd_select_bank(unsigned char bank) |
@@ -988,15 +1020,13 @@ sd_write_retry: | |||
988 | { | 1020 | { |
989 | /* SDHC */ | 1021 | /* SDHC */ |
990 | ret = sd_command(SD_WRITE_MULTIPLE_BLOCK, start, NULL, | 1022 | ret = sd_command(SD_WRITE_MULTIPLE_BLOCK, start, NULL, |
991 | 0x1c00 | CMDAT_BUSY | CMDAT_WR_RD | | 1023 | CMDAT_WR_RD | CMDAT_DATA_EN | CMDAT_RES_TYPE1); |
992 | CMDAT_DATA_EN | CMDAT_RES_TYPE1); | ||
993 | } | 1024 | } |
994 | else | 1025 | else |
995 | #endif | 1026 | #endif |
996 | { | 1027 | { |
997 | ret = sd_command(SD_WRITE_MULTIPLE_BLOCK, start*BLOCK_SIZE, NULL, | 1028 | ret = sd_command(SD_WRITE_MULTIPLE_BLOCK, start*BLOCK_SIZE, NULL, |
998 | 0x1c00 | CMDAT_BUSY | CMDAT_WR_RD | | 1029 | CMDAT_WR_RD | CMDAT_DATA_EN | CMDAT_RES_TYPE1); |
999 | CMDAT_DATA_EN | CMDAT_RES_TYPE1); | ||
1000 | } | 1030 | } |
1001 | if (ret < 0) | 1031 | if (ret < 0) |
1002 | goto sd_write_error; | 1032 | goto sd_write_error; |
@@ -1011,18 +1041,15 @@ sd_write_retry: | |||
1011 | MMC_SD_STATE = SD_PRG; | 1041 | MMC_SD_STATE = SD_PRG; |
1012 | } | 1042 | } |
1013 | 1043 | ||
1014 | udelay(2); /* needed here (loop is too fast :-) */ | 1044 | copy_write_sectors(&buf); /* Copy one chunk of 16 words */ |
1045 | /* TODO: Switch bank if necessary */ | ||
1015 | 1046 | ||
1016 | /* Wait for the FIFO to empty */ | 1047 | /* Wait for the FIFO to empty */ |
1017 | if (sd_poll_status(STAT_XMIT_FIFO_EMPTY, 0x80000)) | 1048 | if (!sd_poll_status(STAT_XMIT_FIFO_EMPTY, 0x80000)) |
1018 | { | 1049 | { |
1019 | copy_write_sectors(&buf); /* Copy one chunk of 16 words */ | 1050 | ret = -EC_FIFO_WR_EMPTY; |
1020 | /* TODO: Switch bank if necessary */ | 1051 | goto sd_write_error; |
1021 | continue; | ||
1022 | } | 1052 | } |
1023 | |||
1024 | ret = -EC_FIFO_WR_EMPTY; | ||
1025 | goto sd_write_error; | ||
1026 | } | 1053 | } |
1027 | 1054 | ||
1028 | last_disk_activity = current_tick; | 1055 | last_disk_activity = current_tick; |