From 3060c3da3ceffd75ad9cf8802f59602f8e012e92 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Fri, 1 Jan 2016 12:35:04 -0200 Subject: [PATCH] AP_HAL_Linux: Add fake device to SPIDriver This allows us to re-use SPIDevice from SPIDeviceDriver (the to-become-SPIDeviceProperties) while the drivers are converted. We create a fake device by calling the temporary SPIDeviceManager::get_device() method passing the descriptor. The transfer and assert logic is still using the old code. Now we can interoperate SPIDeviceDriver with the ones based in SPIDevice since they are going to use the same semaphore and bus. --- libraries/AP_HAL_Linux/SPIDriver.cpp | 40 +++++++--------------------- libraries/AP_HAL_Linux/SPIDriver.h | 5 +--- 2 files changed, 10 insertions(+), 35 deletions(-) diff --git a/libraries/AP_HAL_Linux/SPIDriver.cpp b/libraries/AP_HAL_Linux/SPIDriver.cpp index 31585b45f8..f621bf8122 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIDriver.cpp @@ -14,8 +14,6 @@ #include #include "GPIO.h" -#define SPI_DEBUGGING 0 - using namespace Linux; extern const AP_HAL::HAL& hal; @@ -80,9 +78,6 @@ SPIDeviceDriver SPIDeviceManager::_device[0]; #define LINUX_SPI_DEVICE_NUM_DEVICES ARRAY_SIZE(SPIDeviceManager::_device) const uint8_t SPIDeviceManager::_n_device_desc = LINUX_SPI_DEVICE_NUM_DEVICES; -// have a separate semaphore per bus -Semaphore SPIDeviceManager::_semaphore[LINUX_SPI_MAX_BUSES]; - SPIDeviceDriver::SPIDeviceDriver(const char *name, uint16_t bus, uint16_t subdev, enum AP_HAL::SPIDeviceType type, uint8_t mode, uint8_t bitsPerWord, int16_t cs_pin, uint32_t lowspeed, uint32_t highspeed): _name(name), _bus(bus), @@ -113,9 +108,9 @@ void SPIDeviceDriver::init() } } -AP_HAL::Semaphore* SPIDeviceDriver::get_semaphore() +AP_HAL::Semaphore *SPIDeviceDriver::get_semaphore() { - return SPIDeviceManager::get_semaphore(_bus); + return _fake_dev->get_semaphore(); } bool SPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len) @@ -157,21 +152,11 @@ void SPIDeviceDriver::transfer(const uint8_t *data, uint16_t len) void SPIDeviceManager::init() { for (uint8_t i=0; i= LINUX_SPI_MAX_BUSES) { - AP_HAL::panic("SPIDriver: invalid bus number"); + _device[i]._fake_dev = SPIDeviceManager::from(hal.spi)->get_device(_device[i]); + if (!_device[i]._fake_dev) { + AP_HAL::panic("SPIDriver: couldn't use spidev%u.%u for %s", + _device[i]._bus, _device[i]._subdev, _device[i]._name); } - char path[255]; - snprintf(path, sizeof(path), "/dev/spidev%u.%u", - _device[i]._bus, _device[i]._subdev); - _device[i]._fd = open(path, O_RDWR); - if (_device[i]._fd == -1) { - printf("Unable to open %s - %s\n", path, strerror(errno)); - AP_HAL::panic("SPIDriver: unable to open SPI bus"); - } -#if SPI_DEBUGGING - printf("Opened %s\n", path); - fflush(stdout); -#endif _device[i].init(); } } @@ -243,7 +228,8 @@ bool SPIDeviceManager::transaction(SPIDeviceDriver &driver, const uint8_t *tx, u int r; // we set the mode before we assert the CS line so that the bus is // in the correct idle state before the chip is selected - r = ioctl(driver._fd, SPI_IOC_WR_MODE, &driver._mode); + int fd = driver._fake_dev->get_fd(); + r = ioctl(fd, SPI_IOC_WR_MODE, &driver._mode); if (r == -1) { hal.console->printf("SPI: error on setting mode\n"); return false; @@ -265,7 +251,7 @@ bool SPIDeviceManager::transaction(SPIDeviceDriver &driver, const uint8_t *tx, u memset(rx, 0, len); } - r = ioctl(driver._fd, SPI_IOC_MESSAGE(1), &spi); + r = ioctl(fd, SPI_IOC_MESSAGE(1), &spi); cs_release(driver._type); if (r == -1) { @@ -294,12 +280,4 @@ AP_HAL::SPIDeviceDriver *SPIDeviceManager::device(enum AP_HAL::SPIDeviceType dev return NULL; } -/* - return the bus specific semaphore - */ -AP_HAL::Semaphore *SPIDeviceManager::get_semaphore(uint16_t bus) -{ - return &_semaphore[bus]; -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_Linux/SPIDriver.h b/libraries/AP_HAL_Linux/SPIDriver.h index 8fc68403aa..2d5da64848 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.h +++ b/libraries/AP_HAL_Linux/SPIDriver.h @@ -46,7 +46,7 @@ private: uint32_t _highspeed; uint32_t _speed; enum AP_HAL::SPIDeviceType _type; - int _fd; // Per-device FD. + AP_HAL::OwnPtr _fake_dev; }; class SPIDeviceManager : public AP_HAL::SPIDeviceManager { @@ -72,8 +72,6 @@ public: void init(); AP_HAL::SPIDeviceDriver* device(enum AP_HAL::SPIDeviceType, uint8_t index = 0); - static AP_HAL::Semaphore *get_semaphore(uint16_t bus); - static void cs_assert(enum AP_HAL::SPIDeviceType type); static void cs_release(enum AP_HAL::SPIDeviceType type); static bool transaction(SPIDeviceDriver &driver, const uint8_t *tx, uint8_t *rx, uint16_t len); @@ -86,7 +84,6 @@ protected: static const uint8_t _n_device_desc; static SPIDeviceDriver _device[]; - static Semaphore _semaphore[LINUX_SPI_MAX_BUSES]; }; }