diff --git a/stmhal/sdcard.c b/stmhal/sdcard.c index ae98c328af..32c9df60a2 100644 --- a/stmhal/sdcard.c +++ b/stmhal/sdcard.c @@ -24,6 +24,8 @@ * THE SOFTWARE. */ +#include + #include "py/nlr.h" #include "py/runtime.h" #include "lib/fatfs/ff.h" @@ -192,11 +194,6 @@ void SDIO_IRQHandler(void) { } mp_uint_t sdcard_read_blocks(uint8_t *dest, uint32_t block_num, uint32_t num_blocks) { - // check that dest pointer is aligned on a 4-byte boundary - if (((uint32_t)dest & 3) != 0) { - return SD_ERROR; - } - // check that SD card is initialised if (sd_handle.Instance == NULL) { return SD_ERROR; @@ -204,6 +201,24 @@ mp_uint_t sdcard_read_blocks(uint8_t *dest, uint32_t block_num, uint32_t num_blo HAL_SD_ErrorTypedef err = SD_OK; + // check that dest pointer is aligned on a 4-byte boundary + uint8_t *orig_dest = NULL; + uint32_t saved_word; + if (((uint32_t)dest & 3) != 0) { + // Pointer is not aligned so it needs fixing. + // We could allocate a temporary block of RAM (as sdcard_write_blocks + // does) but instead we are going to use the dest buffer inplace. We + // are going to align the pointer, save the initial word at the aligned + // location, read into the aligned memory, move the memory back to the + // unaligned location, then restore the initial bytes at the aligned + // location. We should have no trouble doing this as those initial + // bytes at the aligned location should be able to be changed for the + // duration of this function call. + orig_dest = dest; + dest = (uint8_t*)((uint32_t)dest & ~3); + saved_word = *(uint32_t*)dest; + } + if (query_irq() == IRQ_STATE_ENABLED) { // we must disable USB irqs to prevent MSC contention with SD card uint32_t basepri = raise_irq_pri(IRQ_PRI_OTG_FS); @@ -225,15 +240,16 @@ mp_uint_t sdcard_read_blocks(uint8_t *dest, uint32_t block_num, uint32_t num_blo err = HAL_SD_ReadBlocks_BlockNumber(&sd_handle, (uint32_t*)dest, block_num, SDCARD_BLOCK_SIZE, num_blocks); } + if (orig_dest != NULL) { + // move the read data to the non-aligned position, and restore the initial bytes + memmove(orig_dest, dest, num_blocks * SDCARD_BLOCK_SIZE); + memcpy(dest, &saved_word, orig_dest - dest); + } + return err; } mp_uint_t sdcard_write_blocks(const uint8_t *src, uint32_t block_num, uint32_t num_blocks) { - // check that src pointer is aligned on a 4-byte boundary - if (((uint32_t)src & 3) != 0) { - return SD_ERROR; - } - // check that SD card is initialised if (sd_handle.Instance == NULL) { return SD_ERROR; @@ -241,6 +257,24 @@ mp_uint_t sdcard_write_blocks(const uint8_t *src, uint32_t block_num, uint32_t n HAL_SD_ErrorTypedef err = SD_OK; + // check that src pointer is aligned on a 4-byte boundary + if (((uint32_t)src & 3) != 0) { + // pointer is not aligned, so allocate a temporary block to do the write + uint8_t *src_aligned = m_new_maybe(uint8_t, SDCARD_BLOCK_SIZE); + if (src_aligned == NULL) { + return SD_ERROR; + } + for (size_t i = 0; i < num_blocks; ++i) { + memcpy(src_aligned, src + i * SDCARD_BLOCK_SIZE, SDCARD_BLOCK_SIZE); + err = sdcard_write_blocks(src_aligned, block_num + i, 1); + if (err != SD_OK) { + break; + } + } + m_del(uint8_t, src_aligned, SDCARD_BLOCK_SIZE); + return err; + } + if (query_irq() == IRQ_STATE_ENABLED) { // we must disable USB irqs to prevent MSC contention with SD card uint32_t basepri = raise_irq_pri(IRQ_PRI_OTG_FS);