diff --git a/libraries/SPI/SPI.cpp b/libraries/SPI/SPI.cpp index 0f1786a4df..f67c6d69fc 100644 --- a/libraries/SPI/SPI.cpp +++ b/libraries/SPI/SPI.cpp @@ -509,9 +509,6 @@ void SPIClass::writePattern(const uint8_t * data, uint8_t size, uint32_t repeat) } /** - * Note: - * in and out need to be aligned to 32Bit - * or you get an Fatal exception (9) * @param out uint8_t * * @param in uint8_t * * @param size uint32_t @@ -538,45 +535,73 @@ void SPIClass::transferBytes(const uint8_t * out, uint8_t * in, uint32_t size) { * @param in uint8_t * * @param size uint8_t (max 64) */ -void SPIClass::transferBytes_(const uint8_t * out, uint8_t * in, uint8_t size) { + +void SPIClass::transferBytesAligned_(const uint8_t * out, uint8_t * in, uint8_t size) { + if (!size) + return; + while(SPI1CMD & SPIBUSY) {} // Set in/out Bits to transfer setDataBits(size * 8); - volatile uint32_t * fifoPtr = &SPI1W0; - uint8_t dataSize = ((size + 3) / 4); + volatile uint32_t *fifoPtr = &SPI1W0; - if(out) { - uint32_t * dataPtr = (uint32_t*) out; - while(dataSize--) { - *fifoPtr = *dataPtr; - dataPtr++; - fifoPtr++; + if (out) { + uint8_t outSize = ((size + 3) / 4); + uint32_t *dataPtr = (uint32_t*) out; + while (outSize--) { + *(fifoPtr++) = *(dataPtr++); } } else { + uint8_t outSize = ((size + 3) / 4); // no out data only read fill with dummy data! - while(dataSize--) { - *fifoPtr = 0xFFFFFFFF; - fifoPtr++; + while (outSize--) { + *(fifoPtr++) = 0xFFFFFFFF; } } SPI1CMD |= SPIBUSY; while(SPI1CMD & SPIBUSY) {} - if(in) { - uint32_t * dataPtr = (uint32_t*) in; + if (in) { + uint32_t *dataPtr = (uint32_t*) in; fifoPtr = &SPI1W0; - dataSize = ((size + 3) / 4); - while(dataSize--) { - *dataPtr = *fifoPtr; - dataPtr++; - fifoPtr++; + int inSize = size; + // Unlike outSize above, inSize tracks *bytes* since we must transfer only the requested bytes to the app to avoid overwriting other vars. + while (inSize >= 4) { + *(dataPtr++) = *(fifoPtr++); + inSize -= 4; + in += 4; + } + volatile uint8_t *fifoPtrB = (volatile uint8_t *)fifoPtr; + while (inSize--) { + *(in++) = *(fifoPtrB++); + } + } +} + + +void SPIClass::transferBytes_(const uint8_t * out, uint8_t * in, uint8_t size) { + if (!((uint32_t)out & 3) && !((uint32_t)in & 3)) { + // Input and output are both 32b aligned or NULL + transferBytesAligned_(out, in, size); + } else { + // HW FIFO has 64b limit and ::transferBytes breaks up large xfers into 64byte chunks before calling this function + // We know at this point at least one direction is misaligned, so use temporary buffer to align everything + // No need for separate out and in aligned copies, we can overwrite our out copy with the input data safely + uint8_t aligned[64]; // Stack vars will be 32b aligned + if (out) { + memcpy(aligned, out, size); + } + transferBytesAligned_(out ? aligned : nullptr, in ? aligned : nullptr, size); + if (in) { + memcpy(in, aligned, size); } } } + #if !defined(NO_GLOBAL_INSTANCES) && !defined(NO_GLOBAL_SPI) SPIClass SPI; #endif diff --git a/libraries/SPI/SPI.h b/libraries/SPI/SPI.h index 84b619f670..e8fb8a3902 100644 --- a/libraries/SPI/SPI.h +++ b/libraries/SPI/SPI.h @@ -79,6 +79,7 @@ class SPIClass { uint8_t pinSet; void writeBytes_(const uint8_t * data, uint8_t size); void transferBytes_(const uint8_t * out, uint8_t * in, uint8_t size); + void transferBytesAligned_(const uint8_t * out, uint8_t * in, uint8_t size); inline void setDataBits(uint16_t bits); };