diff --git a/lib/stm32/st_usbfs_v2.c b/lib/stm32/st_usbfs_v2.c index 0ef1c24e..f6a41783 100644 --- a/lib/stm32/st_usbfs_v2.c +++ b/lib/stm32/st_usbfs_v2.c @@ -79,15 +79,23 @@ void st_usbfs_copy_to_pm(volatile void *vPM, const void *buf, uint16_t len) */ void st_usbfs_copy_from_pm(void *buf, const volatile void *vPM, uint16_t len) { - uint16_t *lbuf = buf; const volatile uint16_t *PM = vPM; uint8_t odd = len & 1; + len >>= 1; - for (len >>= 1; len; PM++, lbuf++, len--) { - *lbuf = *PM; + if (((uintptr_t) buf) & 0x01) { + for (; len; PM++, len--) { + uint16_t value = *PM; + *(uint8_t *) buf++ = value; + *(uint8_t *) buf++ = value >> 8; + } + } else { + for (; len; PM++, buf += 2, len--) { + *(uint16_t *) buf = *PM; + } } if (odd) { - *(uint8_t *) lbuf = *(uint8_t *) PM; + *(uint8_t *) buf = *(uint8_t *) PM; } }