diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 6c60f3ff97..e3808c80e8 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -41,7 +41,6 @@ #include "px4io_serial.h" -IOPacket PX4IO_serial::_io_buffer; static PX4IO_serial *g_interface; device::Device @@ -95,8 +94,9 @@ PX4IO_serial::~PX4IO_serial() } int -PX4IO_serial::init() +PX4IO_serial::init(IOPacket *io_buffer) { + _io_buffer_ptr = io_buffer; /* create semaphores */ // in case the sub-class impl fails, the semaphore is cleaned up by destructor. px4_sem_init(&_bus_semaphore, 0, 1); @@ -120,26 +120,26 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) int result; for (unsigned retries = 0; retries < 3; retries++) { - _io_buffer.count_code = count | PKT_CODE_WRITE; - _io_buffer.page = page; - _io_buffer.offset = offset; - memcpy((void *)&_io_buffer.regs[0], (void *)values, (2 * count)); + _io_buffer_ptr->count_code = count | PKT_CODE_WRITE; + _io_buffer_ptr->page = page; + _io_buffer_ptr->offset = offset; + memcpy((void *)&_io_buffer_ptr->regs[0], (void *)values, (2 * count)); for (unsigned i = count; i < PKT_MAX_REGS; i++) { - _io_buffer.regs[i] = 0x55aa; + _io_buffer_ptr->regs[i] = 0x55aa; } - _io_buffer.crc = 0; - _io_buffer.crc = crc_packet(&_io_buffer); + _io_buffer_ptr->crc = 0; + _io_buffer_ptr->crc = crc_packet(_io_buffer_ptr); /* start the transaction and wait for it to complete */ - result = _bus_exchange(&_io_buffer); + result = _bus_exchange(_io_buffer_ptr); /* successful transaction? */ if (result == OK) { /* check result in packet */ - if (PKT_CODE(_io_buffer) == PKT_CODE_ERROR) { + if (PKT_CODE(*_io_buffer_ptr) == PKT_CODE_ERROR) { /* IO didn't like it - no point retrying */ result = -EINVAL; @@ -178,21 +178,21 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) for (unsigned retries = 0; retries < 3; retries++) { - _io_buffer.count_code = count | PKT_CODE_READ; - _io_buffer.page = page; - _io_buffer.offset = offset; + _io_buffer_ptr->count_code = count | PKT_CODE_READ; + _io_buffer_ptr->page = page; + _io_buffer_ptr->offset = offset; - _io_buffer.crc = 0; - _io_buffer.crc = crc_packet(&_io_buffer); + _io_buffer_ptr->crc = 0; + _io_buffer_ptr->crc = crc_packet(_io_buffer_ptr); /* start the transaction and wait for it to complete */ - result = _bus_exchange(&_io_buffer); + result = _bus_exchange(_io_buffer_ptr); /* successful transaction? */ if (result == OK) { /* check result in packet */ - if (PKT_CODE(_io_buffer) == PKT_CODE_ERROR) { + if (PKT_CODE(*_io_buffer_ptr) == PKT_CODE_ERROR) { /* IO didn't like it - no point retrying */ result = -EINVAL; @@ -200,7 +200,7 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) /* compare the received count with the expected count */ - } else if (PKT_COUNT(_io_buffer) != count) { + } else if (PKT_COUNT(*_io_buffer_ptr) != count) { /* IO returned the wrong number of registers - no point retrying */ result = -EIO; @@ -211,7 +211,7 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) } else { /* copy back the result */ - memcpy(values, &_io_buffer.regs[0], (2 * count)); + memcpy(values, &_io_buffer_ptr->regs[0], (2 * count)); } break; diff --git a/src/drivers/px4io/px4io_serial.h b/src/drivers/px4io/px4io_serial.h index 2a8515a0ca..dc2f35c14a 100644 --- a/src/drivers/px4io/px4io_serial.h +++ b/src/drivers/px4io/px4io_serial.h @@ -67,11 +67,18 @@ public: PX4IO_serial(); virtual ~PX4IO_serial(); - virtual int init(); + virtual int init() = 0; virtual int read(unsigned offset, void *data, unsigned count = 1); virtual int write(unsigned address, void *data, unsigned count = 1); protected: + /** + * Does the PX4IO_serial instance initialization. + * @param io_buffer The IO buffer that should be used for transfers. + * @return 0 on success. + */ + int init(IOPacket *io_buffer); + /** * Start the transaction with IO and wait for it to complete. */ @@ -100,7 +107,7 @@ private: * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) * transfers? Could cause issues with any regs expecting to be written atomically... */ - static IOPacket _io_buffer; // XXX static to ensure DMA-able memory + IOPacket *_io_buffer_ptr; /** bus-ownership lock */ px4_sem_t _bus_semaphore; @@ -171,6 +178,11 @@ private: /* do not allow top copying this class */ PX4IO_serial_f4(PX4IO_serial_f4 &); PX4IO_serial_f4 &operator = (const PX4IO_serial_f4 &); + + /** + * IO Buffer storage + */ + static IOPacket _io_buffer_storage; // XXX static to ensure DMA-able memory }; #elif defined(CONFIG_ARCH_CHIP_STM32F7) @@ -236,6 +248,11 @@ private: /* do not allow top copying this class */ PX4IO_serial_f7(PX4IO_serial_f7 &); PX4IO_serial_f7 &operator = (const PX4IO_serial_f7 &); + + /** + * IO Buffer storage + */ + static uint8_t _io_buffer_storage[]; }; #else diff --git a/src/drivers/px4io/px4io_serial_f4.cpp b/src/drivers/px4io/px4io_serial_f4.cpp index 4d01510c56..8ada27f0a1 100644 --- a/src/drivers/px4io/px4io_serial_f4.cpp +++ b/src/drivers/px4io/px4io_serial_f4.cpp @@ -51,6 +51,8 @@ #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) +IOPacket PX4IO_serial_f4::_io_buffer_storage; + PX4IO_serial_f4::PX4IO_serial_f4() : _tx_dma(nullptr), _rx_dma(nullptr), @@ -108,7 +110,7 @@ PX4IO_serial_f4::init() /* initialize base implementation */ int r; - if ((r = PX4IO_serial::init()) != 0) { + if ((r = PX4IO_serial::init(&_io_buffer_storage)) != 0) { return r; } diff --git a/src/drivers/px4io/px4io_serial_f7.cpp b/src/drivers/px4io/px4io_serial_f7.cpp index 286cf04173..3ad7be7418 100644 --- a/src/drivers/px4io/px4io_serial_f7.cpp +++ b/src/drivers/px4io/px4io_serial_f7.cpp @@ -57,6 +57,13 @@ #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) +#define CACHE_LINE_SIZE 32 + +#define ROUND_UP_TO_POW2_CT(size, alignment) (((uintptr_t)((size) + ((alignment) - 1u))) & (~((uintptr_t)((alignment) - 1u)))) +#define ALIGNED_IO_BUFFER_SIZE ROUND_UP_TO_POW2_CT(sizeof(IOPacket), CACHE_LINE_SIZE) + +uint8_t PX4IO_serial_f7::_io_buffer_storage[ALIGNED_IO_BUFFER_SIZE + CACHE_LINE_SIZE]; + PX4IO_serial_f7::PX4IO_serial_f7() : _tx_dma(nullptr), _rx_dma(nullptr), @@ -114,7 +121,7 @@ PX4IO_serial_f7::init() /* initialize base implementation */ int r; - if ((r = PX4IO_serial::init()) != 0) { + if ((r = PX4IO_serial::init((IOPacket *)ROUND_UP_TO_POW2_CT((uintptr_t)_io_buffer_storage, CACHE_LINE_SIZE))) != 0) { return r; } @@ -282,7 +289,7 @@ PX4IO_serial_f7::_bus_exchange(IOPacket *_packet) /* Clean _current_packet, so DMA can see the data */ arch_clean_dcache((uintptr_t)_current_packet, - (uintptr_t)_current_packet + sizeof(*_current_packet)); + (uintptr_t)_current_packet + ALIGNED_IO_BUFFER_SIZE); /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ @@ -452,7 +459,7 @@ PX4IO_serial_f7::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* Invalidate _current_packet, so we get fresh data from RAM */ arch_invalidate_dcache((uintptr_t)_current_packet, - (uintptr_t)_current_packet + sizeof(*_current_packet)); + (uintptr_t)_current_packet + ALIGNED_IO_BUFFER_SIZE); /* verify that the received packet is complete */ size_t length = sizeof(*_current_packet) - stm32_dmaresidual(_rx_dma);