From 972f85c490d865370677bf0507216b161589c93f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 3 Nov 2016 21:54:06 +1100
Subject: [PATCH] HAL_PX4: go direct to NuttX for SPI transfers

---
 libraries/AP_HAL_PX4/SPIDevice.cpp |  2 +-
 libraries/AP_HAL_PX4/SPIWrapper.h  | 35 +++++++++++++++++++-----------
 2 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_HAL_PX4/SPIDevice.cpp b/libraries/AP_HAL_PX4/SPIDevice.cpp
index 516d61e962..f42b7c10f2 100644
--- a/libraries/AP_HAL_PX4/SPIDevice.cpp
+++ b/libraries/AP_HAL_PX4/SPIDevice.cpp
@@ -50,7 +50,7 @@ SPIDesc SPIDeviceManager::device_table[] = {
 SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
     : bus(_bus)
     , device_desc(_device_desc)
-    , px4dev(device_desc.bus, device_desc.name, device_desc.devname, device_desc.device, device_desc.mode, device_desc.lowspeed)
+    , px4dev(device_desc.bus, device_desc.device, device_desc.mode, device_desc.lowspeed)
 {
 }
 
diff --git a/libraries/AP_HAL_PX4/SPIWrapper.h b/libraries/AP_HAL_PX4/SPIWrapper.h
index b64f73e479..09747a8f68 100644
--- a/libraries/AP_HAL_PX4/SPIWrapper.h
+++ b/libraries/AP_HAL_PX4/SPIWrapper.h
@@ -7,26 +7,35 @@
 
 extern const AP_HAL::HAL& hal;
 /*
-  wrapper class for SPI to expose protected functions from PX4Firmware
+  wrapper class for SPI to expose protected functions from PX4NuttX
  */
-class PX4::PX4_SPI : public device::SPI {
+class PX4::PX4_SPI {
 public:
-    PX4_SPI(uint8_t bus, const char *name, const char *devname, enum spi_dev_e device, enum spi_mode_e mode, uint32_t frequency) :
-        SPI(name, devname, bus, device, mode, frequency) {}
+    PX4_SPI(uint8_t bus, enum spi_dev_e device, enum spi_mode_e mode, uint32_t frequency) :
+        _bus(bus), _device(device), _mode(mode), _frequency(frequency) {
+		_dev = up_spiinitialize(_bus);
+        SPI_SELECT(_dev, _device, false);
+    }
     
     bool do_transfer(uint8_t *send, uint8_t *recv, uint32_t len) {
-        if (!init_done) {
-            if (init() != 0) {
-                return false;
-            }
-            init_done = true;
-        }
-        return transfer(send, recv, len) == 0;
+        irqstate_t state = irqsave();
+        SPI_SETFREQUENCY(_dev, _frequency);
+        SPI_SETMODE(_dev, _mode);
+        SPI_SETBITS(_dev, 8);
+        SPI_SELECT(_dev, _device, true);
+        SPI_EXCHANGE(_dev, send, recv, len);
+        SPI_SELECT(_dev, _device, false);
+        irqrestore(state);
+        return true;
     }
     void set_speed(uint32_t speed_hz) {
-        set_frequency(speed_hz);
+        _frequency = speed_hz;
     }
 private:
-    bool init_done;
+    struct spi_dev_s *_dev;
+    uint8_t _bus;
+    enum spi_dev_e _device;
+    enum spi_mode_e _mode;
+    uint32_t _frequency;
 };