AP_HAL_Linux: Implement threaded SPI

libraries/AP_HAL_Linux/SPIDevice.cpp

@@ -32,6 +32,10 @@
 #include <AP_HAL/utility/OwnPtr.h>
 
 #include "GPIO.h"
+#include "PollerThread.h"
+#include "Scheduler.h"
+#include "Semaphores.h"
+#include "Thread.h"
 #include "Util.h"
 
 namespace Linux {
@@ -134,12 +138,20 @@ const uint8_t SPIDeviceManager::_n_device_desc = LINUX_SPI_DEVICE_NUM_DEVICES;
 
 
 /* Private struct to maintain for each bus */
-class SPIBus {
+class SPIBus : public TimerPollable::WrapperCb {
 public:
     ~SPIBus();
 
+    /*
+     * TimerPollable::WrapperCb methods to take
+     * and release semaphore while calling the callback
+     */
+    void start_cb() override;
+    void end_cb() override;
+
     int open(uint16_t bus_, uint16_t kernel_cs_);
 
+    PollerThread thread;
     Semaphore sem;
     int fd = -1;
     uint16_t bus;
@@ -154,6 +166,16 @@ SPIBus::~SPIBus()
     }
 }
 
+void SPIBus::start_cb()
+{
+    sem.take(HAL_SEMAPHORE_BLOCK_FOREVER);
+}
+
+void SPIBus::end_cb()
+{
+    sem.give();
+}
+
 
 int SPIBus::open(uint16_t bus_, uint16_t kernel_cs_)
 {
@@ -336,6 +358,33 @@ int SPIDevice::get_fd()
     return _bus.fd;
 }
 
+AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(
+    uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    TimerPollable *p = _bus.thread.add_timer(cb, &_bus, period_usec);
+    if (!p) {
+        AP_HAL::panic("Could not create periodic callback");
+    }
+
+    if (!_bus.thread.is_started()) {
+        char name[16];
+        snprintf(name, sizeof(name), "ap-spi-%u", _bus.bus);
+
+        _bus.thread.set_stack_size(AP_LINUX_SENSORS_STACK_SIZE);
+        _bus.thread.start(name, AP_LINUX_SENSORS_SCHED_POLICY,
+                          AP_LINUX_SENSORS_SCHED_PRIO);
+    }
+
+    return static_cast<AP_HAL::Device::PeriodicHandle>(p);
+}
+
+bool SPIDevice::adjust_periodic_callback(
+    AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
+{
+    return _bus.thread.adjust_timer(static_cast<TimerPollable*>(h), period_usec);
+}
+
+
 AP_HAL::OwnPtr<AP_HAL::SPIDevice>
 SPIDeviceManager::get_device(const char *name)
 {

libraries/AP_HAL_Linux/SPIDevice.h

@@ -52,10 +52,11 @@ public:
 
     /* See AP_HAL::Device::register_periodic_callback() */
     AP_HAL::Device::PeriodicHandle register_periodic_callback(
-        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override
-    {
-        return nullptr;
-    }
+        uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
+
+    /* See AP_HAL::Device::adjust_periodic_callback() */
+    bool adjust_periodic_callback(
+        AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
 
     /* See AP_HAL::Device::get_fd() */
     int get_fd() override;