From 836d13fc4b6d945c600bc269b58fec8e6e755be9 Mon Sep 17 00:00:00 2001 From: "William D. Jones" Date: Wed, 23 Jan 2019 02:04:40 -0500 Subject: [PATCH] stm32f4: Fix FIFO write logic (requires 32 bit writes). --- src/portable/stm/stm32f4/dcd_stm32f4.c | 27 ++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/portable/stm/stm32f4/dcd_stm32f4.c b/src/portable/stm/stm32f4/dcd_stm32f4.c index 53e0868a..2abf0465 100644 --- a/src/portable/stm/stm32f4/dcd_stm32f4.c +++ b/src/portable/stm/stm32f4/dcd_stm32f4.c @@ -495,8 +495,31 @@ void OTG_FS_IRQHandler(void) { xfer->queued_len = xfer->total_len - remaining; uint16_t to_xfer_size = (remaining > xfer->max_size) ? xfer->max_size : remaining; - for(uint16_t i = 0; i < to_xfer_size; i++) { - (* tx_fifo) = xfer->buffer[xfer->queued_len + i]; + uint8_t to_xfer_rem = to_xfer_size % 4; + uint16_t to_xfer_size_aligned = to_xfer_size - to_xfer_rem; + + // Buffer might not be aligned to 32b, so we need to force alignment + // by copying to a temp var. + uint8_t * base = (xfer->buffer + xfer->queued_len); + for(uint16_t i = 0; i < to_xfer_size_aligned; i += 4) { + uint32_t tmp = base[i] | (base[i + 1] << 8) | (base[i + 2] << 16) | (base[i + 3] << 24); + (* tx_fifo) = tmp; + } + + // Do not read beyond end of buffer if not divisible by 4. + if(to_xfer_rem != 0) { + uint32_t tmp = 0; + uint8_t * last_32b_bound = base + to_xfer_size_aligned; + + tmp |= last_32b_bound[0]; + if(to_xfer_rem > 1) { + tmp |= (last_32b_bound[1] << 8); + } + if(to_xfer_rem > 2) { + tmp |= (last_32b_bound[2] << 16); + } + + (* tx_fifo) = tmp; } } }