src/lib/DEVICE/device.cpp

   1 #include "targets.h"
   2 #include "common.h"
   3 #include "logging.h"
   4 #include "helpers.h"
   5 #include "device.h"
   6
   7 ///////////////////////////////////////
   8 // Even though we aren't using anything this keeps the PIO dependency analyzer happy!
   9
  10 #if defined(RADIO_SX127X)
  11 #include "SX127xDriver.h"
  12 #elif defined(RADIO_LR1121)
  13 #include "LR1121Driver.h"
  14 #elif defined(RADIO_SX128X)
  15 #include "SX1280Driver.h"
  16 #else
  17 #error Invalid radio configuration!
  18 #endif
  19
  20 #if defined(PLATFORM_ESP32)
  21 #include <soc/soc_caps.h>
  22 #define MULTICORE (SOC_CPU_CORES_NUM > 1)
  23 #endif
  24
  25 ///////////////////////////////////////
  26
  27 static device_affinity_t *uiDevices;
  28 static uint8_t deviceCount;
  29
  30 static uint32_t eventFired[2] = {0, 0};
  31 static bool lastModelMatch[2] = {false, false};
  32
  33 static unsigned long deviceTimeout[16] = {0};
  34
  35 #if MULTICORE
  36 static TaskHandle_t xDeviceTask = NULL;
  37 static SemaphoreHandle_t taskSemaphore;
  38 static SemaphoreHandle_t completeSemaphore;
  39 [[noreturn]] static void deviceTask(void *pvArgs);
  40 #define CURRENT_CORE  xPortGetCoreID()
  41 #else
  42 #define CURRENT_CORE -1
  43 #endif
  44
  45 void devicesRegister(device_affinity_t *devices, uint8_t count)
  46 {
  47     uiDevices = devices;
  48     deviceCount = count;
  49
  50     #if MULTICORE
  51         taskSemaphore = xSemaphoreCreateBinary();
  52         completeSemaphore = xSemaphoreCreateBinary();
  53         disableCore0WDT();
  54         xTaskCreatePinnedToCore(deviceTask, "deviceTask", 32768, NULL, 0, &xDeviceTask, 0);
  55     #endif
  56 }
  57
  58 void devicesInit()
  59 {
  60     int32_t core = CURRENT_CORE;
  61
  62     for(size_t i=0 ; i<deviceCount ; i++) {
  63         if (uiDevices[i].core == core || core == -1) {
  64             if (uiDevices[i].device->initialize && !(uiDevices[i].device->initialize)()) {
  65                 uiDevices[i].device->start = nullptr;
  66                 uiDevices[i].device->event = nullptr;
  67                 uiDevices[i].device->timeout = nullptr;
  68             }
  69         }
  70     }
  71     #if MULTICORE
  72     if (core == 1)
  73     {
  74         xSemaphoreGive(taskSemaphore);
  75         xSemaphoreTake(completeSemaphore, portMAX_DELAY);
  76     }
  77     #endif
  78 }
  79
  80 void devicesStart()
  81 {
  82     int32_t core = CURRENT_CORE;
  83     unsigned long now = millis();
  84
  85     for(size_t i=0 ; i<deviceCount ; i++)
  86     {
  87         if (uiDevices[i].core == core || core == -1) {
  88             deviceTimeout[i] = 0xFFFFFFFF;
  89             if (uiDevices[i].device->start)
  90             {
  91                 int delay = (uiDevices[i].device->start)();
  92                 deviceTimeout[i] = delay == DURATION_NEVER ? 0xFFFFFFFF : now + delay;
  93             }
  94         }
  95     }
  96     #if MULTICORE
  97     if (core == 1)
  98     {
  99         xSemaphoreGive(taskSemaphore);
 100         xSemaphoreTake(completeSemaphore, portMAX_DELAY);
 101     }
 102     #endif
 103 }
 104
 105 void devicesStop()
 106 {
 107     #if MULTICORE
 108     vTaskDelete(xDeviceTask);
 109     #endif
 110 }
 111
 112 void devicesTriggerEvent(uint32_t events)
 113 {
 114     eventFired[0] |= events;
 115     eventFired[1] |= events;
 116     #if MULTICORE
 117     // Release the semaphore so the tasks on core 0 run now
 118     xSemaphoreGive(taskSemaphore);
 119     #endif
 120 }
 121
 122 static int _devicesUpdate(unsigned long now)
 123 {
 124     const int32_t core = CURRENT_CORE;
 125     const int32_t coreMulti = (core == -1) ? 0 : core;
 126
 127     bool newModelMatch = connectionHasModelMatch && teamraceHasModelMatch;
 128     uint32_t events = eventFired[coreMulti];
 129     eventFired[coreMulti] = 0;
 130     bool handleEvents = events != 0 || lastModelMatch[coreMulti] != newModelMatch;
 131     lastModelMatch[coreMulti] = newModelMatch;
 132
 133     if (handleEvents)
 134     {
 135         for(size_t i=0 ; i<deviceCount ; i++)
 136         {
 137             if ((uiDevices[i].core == core || core == -1) && (uiDevices[i].device->event && (uiDevices[i].device->subscribe & events) != 0))
 138             {
 139                 int delay = (uiDevices[i].device->event)();
 140                 if (delay != DURATION_IGNORE)
 141                 {
 142                     deviceTimeout[i] = delay == DURATION_NEVER ? 0xFFFFFFFF : now + delay;
 143                 }
 144             }
 145         }
 146     }
 147
 148     int smallest_delay = DURATION_NEVER;
 149     for(size_t i=0 ; i<deviceCount ; i++)
 150     {
 151         if ((uiDevices[i].core == core || core == -1) && uiDevices[i].device->timeout)
 152         {
 153             int delay = deviceTimeout[i] == 0xFFFFFFFF ? DURATION_NEVER : (int)(deviceTimeout[i]-now);
 154             if (now >= deviceTimeout[i])
 155             {
 156                 delay = (uiDevices[i].device->timeout)();
 157                 deviceTimeout[i] = delay == DURATION_NEVER ? 0xFFFFFFFF : now + delay;
 158             }
 159             if (delay != DURATION_NEVER)
 160             {
 161                 smallest_delay = (smallest_delay == DURATION_NEVER) ? delay : std::min(smallest_delay, delay);
 162             }
 163         }
 164     }
 165     return smallest_delay;
 166 }
 167
 168 void devicesUpdate(unsigned long now)
 169 {
 170     _devicesUpdate(now);
 171 }
 172
 173 #if MULTICORE
 174 [[noreturn]] static void deviceTask(void *pvArgs)
 175 {
 176     xSemaphoreTake(taskSemaphore, portMAX_DELAY);
 177     devicesInit();
 178     xSemaphoreGive(completeSemaphore);
 179     xSemaphoreTake(taskSemaphore, portMAX_DELAY);
 180     devicesStart();
 181     xSemaphoreGive(completeSemaphore);
 182     for (;;)
 183     {
 184         int delay = _devicesUpdate(millis());
 185         // sleep the core until the desired time, or it's awakened by an event
 186         xSemaphoreTake(taskSemaphore, delay == DURATION_NEVER ? portMAX_DELAY : pdMS_TO_TICKS(delay));
 187     }
 188 }
 189 #endif