From 37a9a78725d1d45d6d7468e11a5e28c9dace8cf9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Nov 2016 10:39:01 +1100 Subject: [PATCH] HAL_PX4: avoid bounce buffers for SPI when possible --- libraries/AP_HAL_PX4/SPIDevice.cpp | 7 ++++++- libraries/AP_HAL_PX4/SPIDevice.h | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_PX4/SPIDevice.cpp b/libraries/AP_HAL_PX4/SPIDevice.cpp index 8e77af4a95..5b0ba6f1ca 100644 --- a/libraries/AP_HAL_PX4/SPIDevice.cpp +++ b/libraries/AP_HAL_PX4/SPIDevice.cpp @@ -146,7 +146,7 @@ bool SPIDevice::set_speed(AP_HAL::Device::Speed speed) /* low level transfer function */ -void SPIDevice::do_transfer(uint8_t *send, uint8_t *recv, uint32_t len) +void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) { /* to accomodate the method in PX4 drivers of using interrupt @@ -187,6 +187,11 @@ void SPIDevice::do_transfer(uint8_t *send, uint8_t *recv, uint32_t len) bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) { + if (send_len == recv_len && send == recv) { + // simplest cases, needed for DMA + do_transfer(send, recv, recv_len); + return true; + } uint8_t buf[send_len+recv_len]; if (send_len > 0) { memcpy(buf, send, send_len); diff --git a/libraries/AP_HAL_PX4/SPIDevice.h b/libraries/AP_HAL_PX4/SPIDevice.h index 39fd035039..6596ca0699 100644 --- a/libraries/AP_HAL_PX4/SPIDevice.h +++ b/libraries/AP_HAL_PX4/SPIDevice.h @@ -62,7 +62,7 @@ public: bool set_speed(AP_HAL::Device::Speed speed) override; // low level transfer function - void do_transfer(uint8_t *send, uint8_t *recv, uint32_t len); + void do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len); /* See AP_HAL::Device::transfer() */ bool transfer(const uint8_t *send, uint32_t send_len,