HAL_Linux: fixed SPI semaphore, and panic on CS error

This adds a check for trying to assert two CS pins on the same bus
at the same time. The change involves moving the _device handles into the
DeviceManager class, and accessing via static methods.

This also moves the semaphore to be per-bus rather than per-device,
which fixes the problem with bad MS5611 transfers.

Pair-Programmed-With: Victor, Sid, Anuj and Philip

libraries/AP_HAL_Linux/SPIDriver.cpp

@@ -18,8 +18,20 @@ using namespace Linux;
 
 extern const AP_HAL::HAL& hal;
 
-LinuxSPIDeviceDriver::LinuxSPIDeviceDriver(const char *spipath, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t speed):
-    _spipath(spipath),
+LinuxSPIDeviceDriver LinuxSPIDeviceManager::_device[LINUX_SPI_DEVICE_NUM_DEVICES] = {
+    LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_MS5611,  SPI_MODE_0, 8,   7,  6*1000*1000), /* SPIDevice_MS5611 */
+    LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_MPU6000, SPI_MODE_0, 8, 113, 20*1000*1000), /* SPIDevice_MPU6000 */
+    LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_MPU9250, SPI_MODE_0, 8,  49,  6*1000*1000), /* SPIDevice_MPU9250 */
+    LinuxSPIDeviceDriver(0, LINUX_SPI_DEVICE_LSM9DS0, SPI_MODE_0, 8,   5,  6*1000*1000), /* SPIDevice_LSM9DS0 */
+    LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_FRAM,    SPI_MODE_0, 8,  44,  6*1000*1000)  /* SPIDevice_Dataflash */
+};
+
+// have a separate semaphore per bus
+LinuxSemaphore LinuxSPIDeviceManager::_semaphore[LINUX_SPI_NUM_BUSES];
+
+LinuxSPIDeviceDriver::LinuxSPIDeviceDriver(uint8_t bus, LinuxSPIDeviceType type, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t speed):
+    _bus(bus),
+    _type(type),
     _fd(-1),
     _mode(mode),
     _bitsPerWord(bitsPerWord),    
@@ -30,10 +42,11 @@ LinuxSPIDeviceDriver::LinuxSPIDeviceDriver(const char *spipath, uint8_t mode, ui
 
 void LinuxSPIDeviceDriver::init()
 {
-    _fd = open(_spipath, O_RDWR);
+    const char *path = _bus==0?"/dev/spidev1.0":"/dev/spidev2.0";
+    _fd = open(path, O_RDWR);
     if (_fd == -1) {
 #if SPI_DEBUGGING
-        hal.console->printf("LinuxSPIDeviceDriver failed opening _spipath\n");
+        hal.console->printf("LinuxSPIDeviceDriver failed opening %s\n", path);
 #endif        
         return;
     }
@@ -88,7 +101,7 @@ void LinuxSPIDeviceDriver::init()
 
     // Init the CS
     _cs = hal.gpio->channel(_cs_pin);
-    _cs->mode(GPIO_OUT);
+    _cs->mode(HAL_GPIO_OUTPUT);
     _cs->write(HIGH);       // do not hold the SPI bus initially
     return;
 
@@ -100,7 +113,7 @@ failed:
 
 AP_HAL::Semaphore* LinuxSPIDeviceDriver::get_semaphore()
 {
-    return &_semaphore;
+    return LinuxSPIDeviceManager::get_semaphore(_bus);
 }
 
 void LinuxSPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len)
@@ -122,12 +135,12 @@ void LinuxSPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t
 
 void LinuxSPIDeviceDriver::cs_assert()
 {
-    _cs->write(0);
+    LinuxSPIDeviceManager::cs_assert(_type);
 }
 
 void LinuxSPIDeviceDriver::cs_release()
 {
-    _cs->write(1);
+    LinuxSPIDeviceManager::cs_release(_type);
 }
 
 uint8_t LinuxSPIDeviceDriver::transfer(uint8_t data)
@@ -142,21 +155,38 @@ void LinuxSPIDeviceDriver::transfer(const uint8_t *data, uint16_t len)
     transaction(data, NULL, len);
 }
 
-LinuxSPIDeviceManager::LinuxSPIDeviceManager() :
-    _device_ms5611("/dev/spidev2.0", SPI_MODE_0, 8, 7, 6*1000*1000), /* SPIDevice_MS5611 */
-    _device_mpu6000("/dev/spidev2.0", SPI_MODE_0, 8, 113, 20*1000*1000), /* SPIDevice_MPU6000 */
-    _device_mpu9250("/dev/spidev2.0", SPI_MODE_0, 8, 49, 6*1000*1000), /* SPIDevice_MPU9250 */
-    _device_lsm9ds0("/dev/spidev1.0", SPI_MODE_0, 8, 5, 6*1000*1000), /* SPIDevice_LSM9DS0 */
-    _device_fram("/dev/spidev2.0", SPI_MODE_0, 8, 44, 6*1000*1000) /* SPIDevice_Dataflash */
-{}
-
 void LinuxSPIDeviceManager::init(void *)
 {
-    _device_ms5611.init();
-    _device_mpu6000.init();
-    _device_mpu9250.init();
-    _device_lsm9ds0.init();
-    _device_fram.init();
+    for (uint8_t i=0; i<LINUX_SPI_DEVICE_NUM_DEVICES; i++) {
+        _device[i].init();
+    }
+}
+
+void LinuxSPIDeviceManager::cs_assert(LinuxSPIDeviceType type)
+{
+    for (uint8_t i=0; i<LINUX_SPI_DEVICE_NUM_DEVICES; i++) {
+        if (_device[i].get_bus() != _device[type].get_bus()) {
+            // not the same bus
+            continue;
+        }
+        if (i != type) {
+            if (_device[i].get_cs()->read() != 1) {
+                hal.scheduler->panic("two CS enabled at once");
+            }
+        }
+    }
+    _device[type].get_cs()->write(0);
+}
+
+void LinuxSPIDeviceManager::cs_release(LinuxSPIDeviceType type)
+{
+    for (uint8_t i=0; i<LINUX_SPI_DEVICE_NUM_DEVICES; i++) {
+        if (_device[i].get_bus() != _device[type].get_bus()) {
+            // not the same bus
+            continue;
+        }
+        _device[i].get_cs()->write(1);
+    }
 }
 
 /*
@@ -166,18 +196,26 @@ AP_HAL::SPIDeviceDriver* LinuxSPIDeviceManager::device(enum AP_HAL::SPIDevice de
 {
     switch (dev) {
         case AP_HAL::SPIDevice_MPU6000:
-            return &_device_mpu6000;            
+            return &_device[LINUX_SPI_DEVICE_MPU6000];
         case AP_HAL::SPIDevice_MPU9250:
-            return &_device_mpu9250;            
+            return &_device[LINUX_SPI_DEVICE_MPU9250];
         case AP_HAL::SPIDevice_MS5611:
-            return &_device_ms5611;
+            return &_device[LINUX_SPI_DEVICE_MS5611];
         case AP_HAL::SPIDevice_LSM9DS0:
-            return &_device_lsm9ds0;                        
+            return &_device[LINUX_SPI_DEVICE_LSM9DS0];       
         case AP_HAL::SPIDevice_Dataflash:
-            return &_device_fram;                        
+            return &_device[LINUX_SPI_DEVICE_FRAM];   
 
     }
     return NULL;
 }
 
+/*
+  return the bus specific semaphore
+ */
+AP_HAL::Semaphore *LinuxSPIDeviceManager::get_semaphore(uint8_t bus)
+{
+    return &_semaphore[bus];
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_Linux/SPIDriver.h

@@ -5,39 +5,56 @@
 #include <AP_HAL_Linux.h>
 #include "Semaphores.h"
 
+enum LinuxSPIDeviceType {
+    LINUX_SPI_DEVICE_MS5611  = 0,
+    LINUX_SPI_DEVICE_MPU6000 = 1,
+    LINUX_SPI_DEVICE_MPU9250 = 2,
+    LINUX_SPI_DEVICE_LSM9DS0 = 3,
+    LINUX_SPI_DEVICE_FRAM    = 4,
+    LINUX_SPI_DEVICE_NUM_DEVICES = 5
+};
+
+#define LINUX_SPI_NUM_BUSES 2
+
 class Linux::LinuxSPIDeviceDriver : public AP_HAL::SPIDeviceDriver {
 public:
-    LinuxSPIDeviceDriver(const char *spipath, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t speed);
+    LinuxSPIDeviceDriver(uint8_t bus, LinuxSPIDeviceType type, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t speed);
     void init();
-    AP_HAL::Semaphore* get_semaphore();
+    AP_HAL::Semaphore *get_semaphore();
     void transaction(const uint8_t *tx, uint8_t *rx, uint16_t len);
 
     void cs_assert();
     void cs_release();
     uint8_t transfer (uint8_t data);
     void transfer (const uint8_t *data, uint16_t len);
+
+    uint8_t get_bus(void) const { return _bus; }
+    AP_HAL::DigitalSource *get_cs(void) const { return _cs; }
+
 private:
-    LinuxSemaphore _semaphore;
-    const char *_spipath;
     int _fd;
     uint8_t _cs_pin;
     AP_HAL::DigitalSource *_cs;
     uint8_t _mode;
     uint8_t _bitsPerWord;
     uint32_t _speed;
+    LinuxSPIDeviceType _type;
+    uint8_t _bus;
 };
 
 class Linux::LinuxSPIDeviceManager : public AP_HAL::SPIDeviceManager {
 public:
-    LinuxSPIDeviceManager();
     void init(void *);
     AP_HAL::SPIDeviceDriver* device(enum AP_HAL::SPIDevice);
+
+    static AP_HAL::Semaphore *get_semaphore(uint8_t bus);
+
+    static void cs_assert(enum LinuxSPIDeviceType type);
+    static void cs_release(enum LinuxSPIDeviceType type);
+
 private:
-    LinuxSPIDeviceDriver _device_ms5611;
-    LinuxSPIDeviceDriver _device_mpu6000;
-    LinuxSPIDeviceDriver _device_mpu9250;
-    LinuxSPIDeviceDriver _device_lsm9ds0;
-    LinuxSPIDeviceDriver _device_fram;
+    static LinuxSPIDeviceDriver _device[LINUX_SPI_DEVICE_NUM_DEVICES];
+    static LinuxSemaphore _semaphore[LINUX_SPI_NUM_BUSES];
 };
 
 #endif // __AP_HAL_LINUX_SPIDRIVER_H__