AP_HAL_SITL: add simulated SPI devices

libraries/AP_HAL_SITL/HAL_SITL_Class.cpp

@@ -22,6 +22,7 @@
 #include "Util.h"
 #include "DSP.h"
 #include "CANSocketIface.h"
+#include "SPIDevice.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL_Empty/AP_HAL_Empty.h>
@@ -50,7 +51,6 @@ static DSP dspDriver;
 
 
 // use the Empty HAL for hardware we don't emulate
-static Empty::SPIDeviceManager emptySPI;
 static Empty::OpticalFlow emptyOpticalFlow;
 static Empty::Flash emptyFlash;
 
@@ -66,12 +66,13 @@ static UARTDriver sitlUart8Driver(8, &sitlState);
 
 #if defined(HAL_BUILD_AP_PERIPH)
 static Empty::I2CDeviceManager i2c_mgr_instance;
+static Empty::SPIDeviceManager spi_mgr_instance;
 #else
 static I2CDeviceManager i2c_mgr_instance;
+static SPIDeviceManager spi_mgr_instance;
 #endif
 static Util utilInstance(&sitlState);
 
-
 #if HAL_NUM_CAN_IFACES
 static HALSITL::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
 #endif
@@ -90,7 +91,7 @@ HAL_SITL::HAL_SITL() :
         &sitlUart7Driver,   /* uartH */
         &sitlUart8Driver,   /* uartI */
         &i2c_mgr_instance,
-        &emptySPI,          /* spi */
+        &spi_mgr_instance,  /* spi */
         &qspi_mgr_instance,
         &sitlAnalogIn,      /* analogin */
         &sitlStorage, /* storage */

libraries/AP_HAL_SITL/SPIDevice.cpp

@@ -0,0 +1,263 @@
+/*
+ * Copyright (C) 2021  Peter Barker. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "SPIDevice.h"
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
+
+#include <SITL/SITL.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace HALSITL;
+
+/*
+ * SPIBus
+ */
+class SPIBus {
+    friend class SPIDeviceManager;
+public:
+
+    SPIBus(uint8_t _bus) :
+        bus{_bus}
+        { }
+
+    uint8_t bus;
+    Semaphore sem;
+    int ioctl(uint8_t cs_pin, uint8_t ioctl_number, void *data) {
+        return _ioctl(cs_pin, ioctl_number, data);
+    }
+    // void _timer_tick(); // in lieu of a thread-per-bus
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb);
+
+    class SPIBus *next;
+
+private:
+    int _ioctl(uint8_t cs_pin, uint8_t ioctl_number, void *data);
+
+    struct callback_info {
+        struct callback_info *next;
+        AP_HAL::Device::PeriodicCb cb;
+        uint32_t period_usec;
+        uint64_t next_usec;
+    } *callbacks;
+
+    static uint8_t spi_buscount;
+
+};
+
+uint8_t SPIBus::spi_buscount;
+
+// FIXME: do the whole subdev dance here and get an actual FD to do an ioctl on?
+int SPIBus::_ioctl(uint8_t cs_pin, uint8_t ioctl_number, void *data)
+{
+    SITL::SIM *sitl = AP::sitl();
+    return sitl->spi_ioctl(bus, cs_pin, ioctl_number, data);
+}
+
+AP_HAL::Device::PeriodicHandle SPIBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    // mostly swiped from ChibiOS:
+    SPIBus::callback_info *callback = new SPIBus::callback_info;
+    if (callback == nullptr) {
+        return nullptr;
+    }
+    callback->cb = cb;
+    callback->period_usec = period_usec;
+    callback->next_usec = AP_HAL::micros64() + period_usec;
+
+    // add to linked list of callbacks on thread
+    callback->next = callbacks;
+    callbacks = callback;
+    return callback;
+}
+
+// void SPIBus::_timer_tick()
+// {
+//     const uint64_t now = AP_HAL::micros64();
+//     for (struct callback_info *ci = callbacks; ci != nullptr; ci = ci->next) {
+//         if (ci->next_usec < now) {
+//             WITH_SEMAPHORE(sem);
+//             ci->cb();
+//             ci->next_usec += ci->period_usec;
+//         }
+//     }
+// }
+
+/*
+ * SPIDeviceManager
+ */
+SPIBus *SPIDeviceManager::buses;
+
+SPIDeviceManager::SPIDeviceManager()
+{
+    for (uint8_t i=0; i<ARRAY_SIZE(buses); i++) {
+        buses[i].bus = i;
+    }
+}
+
+static const struct SPIDriverInfo {
+    // SPIDriver *driver;
+    uint8_t busid; // used for device IDs in parameters
+    // uint8_t dma_channel_rx;
+    // uint8_t dma_channel_tx;
+} spi_devices[] = {
+    { 0, },
+};
+
+// name, bus, cs_pin
+SPIDesc SPIDeviceManager::device_table[] = {
+    { "ramtron", 0, 0 },
+};
+
+AP_HAL::OwnPtr<AP_HAL::SPIDevice>
+SPIDeviceManager::get_device(const char *name)
+{
+    // this was swiped from AP_HAL_ChibiOS
+
+    /* Find the bus description in the table */
+    uint8_t i;
+    for (i = 0; i<ARRAY_SIZE(device_table); i++) {
+        if (strcmp(device_table[i].name, name) == 0) {
+            break;
+        }
+    }
+    if (i == ARRAY_SIZE(device_table)) {
+        return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
+    }
+
+    SPIDesc &desc = device_table[i];
+
+    // find the bus
+    SPIBus *busp;
+    for (busp = buses; busp; busp = (SPIBus *)busp->next) {
+        if (busp->bus == desc.bus) {
+            break;
+        }
+    }
+    if (busp == nullptr) {
+        // create a new one
+        busp = new SPIBus(desc.bus);
+        if (busp == nullptr) {
+            return nullptr;
+        }
+        busp->next = buses;
+        busp->bus = desc.bus;
+
+        buses = busp;
+    }
+
+    return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(*busp, desc));
+}
+
+// void SPIDeviceManager::_timer_tick()
+// {
+//     for (auto &bus : buses) {
+//         bus._timer_tick();
+//     }
+// }
+
+/*
+ * SPIDevice
+ */
+
+SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
+    : bus(_bus)
+    , device_desc(_device_desc)
+{
+    set_device_bus(spi_devices[_bus.bus].busid);
+    set_device_address(_device_desc.cs_pin);
+}
+
+AP_HAL::Semaphore *SPIDevice::get_semaphore()
+{
+    return &bus.sem;
+}
+
+/*
+  low level transfer function - swiped from the Linux HAL
+ */
+// this is copied into the SITL SPI layer
+struct spi_ioc_transfer {
+	uint64_t tx_buf;
+	uint64_t rx_buf;
+
+	uint32_t len;
+	// uint32_t speed_hz;
+
+	// uint16_t delay_usecs;
+	// uint8_t bits_per_word;
+	// uint8_t cs_change;
+};
+
+#define SPI_TRANSACTION_1LONG 17
+#define SPI_TRANSACTION_2LONG 18
+
+bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
+                         uint8_t *recv, uint32_t recv_len)
+{
+    bus.sem.check_owner();
+
+    struct spi_ioc_transfer msgs[2] = { };
+    unsigned nmsgs = 0;
+
+    if (send && send_len != 0) {
+        msgs[nmsgs].tx_buf = (uint64_t) send;
+        msgs[nmsgs].rx_buf = 0;
+        msgs[nmsgs].len = send_len;
+        nmsgs++;
+    }
+
+    if (recv && recv_len != 0) {
+        msgs[nmsgs].tx_buf = 0;
+        msgs[nmsgs].rx_buf = (uint64_t) recv;
+        msgs[nmsgs].len = recv_len;
+        nmsgs++;
+    }
+
+    if (!nmsgs) {
+        return false;
+    }
+
+    uint8_t ioctl_number;
+    switch (nmsgs) {
+    case 1:
+        ioctl_number = SPI_TRANSACTION_1LONG;
+        break;
+    case 2:
+        ioctl_number = SPI_TRANSACTION_2LONG;
+        break;
+    default:
+        abort();
+    }
+    const int r = bus.ioctl(device_desc.cs_pin, ioctl_number, &msgs);
+
+    if (r == -1) {
+        hal.console->printf("SPIDevice: error transferring data\n");
+        return false;
+    }
+
+    return true;
+}
+
+AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    return bus.register_periodic_callback(period_usec, cb);
+}
+
+#endif // HAL_BOARD_SITL

libraries/AP_HAL_SITL/SPIDevice.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2021 Peter Barker. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <inttypes.h>
+
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/SPIDevice.h>
+
+#include "Semaphores.h"
+
+class SPIBus;
+
+namespace HALSITL {
+
+struct SPIDesc {
+    SPIDesc(const char *_name, uint8_t _bus, uint8_t _cs_pin)
+        : name(_name), bus(_bus), cs_pin(_cs_pin)
+    { }
+
+    const char *name;
+    uint8_t bus;
+    uint8_t cs_pin;  // cs
+};
+
+class SPIDevice : public AP_HAL::SPIDevice {
+public:
+    SPIDevice(SPIBus &_bus, SPIDesc &_device_desc);
+
+    // no concept of speed in SITL yet
+    bool set_speed(AP_HAL::Device::Speed speed) override { return true; }
+
+    bool transfer(const uint8_t *send, uint32_t send_len,
+                  uint8_t *recv, uint32_t recv_len) override;
+
+    bool transfer_fullduplex(const uint8_t *send, uint8_t *recv,
+                             uint32_t len) override {
+        abort();
+    }
+
+
+    AP_HAL::Semaphore *get_semaphore() override;
+
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(
+        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
+
+private:
+    SPIBus &bus;
+    SPIDesc &device_desc;
+
+    Semaphore _semaphore;
+
+    bool do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len);
+};
+
+class SPIDeviceManager : public AP_HAL::SPIDeviceManager {
+public:
+
+    SPIDeviceManager();
+
+    AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) override;
+
+    static SPIDesc device_table[];
+    static SPIBus *buses;
+};
+
+}