mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-05 13:24:11 +01:00
550 lines
19 KiB
C
550 lines
19 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @file pios_callbackscheduler.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
|
|
* @brief Scheduler to run callback functions from a shared context with given priorities.
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program 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 program 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, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include <pios.h>
|
|
#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
|
|
|
|
#include <utlist.h>
|
|
#include <uavobjectmanager.h>
|
|
#include <taskinfo.h>
|
|
|
|
// Private constants
|
|
#define STACK_SAFETYCOUNT 16
|
|
#define STACK_SIZE (300 + STACK_SAFETYSIZE)
|
|
#define STACK_SAFETYSIZE 8
|
|
#define MAX_SLEEP 1000
|
|
|
|
// Private types
|
|
/**
|
|
* task information
|
|
*/
|
|
struct DelayedCallbackTaskStruct {
|
|
DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1];
|
|
DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1];
|
|
xTaskHandle callbackSchedulerTaskHandle;
|
|
char name[3];
|
|
uint32_t stackSize;
|
|
DelayedCallbackPriorityTask priorityTask;
|
|
xSemaphoreHandle signal;
|
|
struct DelayedCallbackTaskStruct *next;
|
|
};
|
|
|
|
/**
|
|
* callback information
|
|
*/
|
|
struct DelayedCallbackInfoStruct {
|
|
DelayedCallback cb;
|
|
int16_t callbackID;
|
|
bool volatile waiting;
|
|
uint32_t volatile scheduletime;
|
|
uint32_t stackSize;
|
|
int32_t stackFree;
|
|
int32_t stackNotFree;
|
|
uint16_t stackSafetyCount;
|
|
uint16_t currentSafetyCount;
|
|
uint32_t runCount;
|
|
struct DelayedCallbackTaskStruct *task;
|
|
struct DelayedCallbackInfoStruct *next;
|
|
};
|
|
|
|
|
|
// Private variables
|
|
static struct DelayedCallbackTaskStruct *schedulerTasks;
|
|
static xSemaphoreHandle mutex;
|
|
static bool schedulerStarted;
|
|
|
|
// Private functions
|
|
static void CallbackSchedulerTask(void *task);
|
|
static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority);
|
|
|
|
/**
|
|
* Initialize the scheduler
|
|
* must be called before any other functions are called
|
|
* \return Success (0), failure (-1)
|
|
*/
|
|
int32_t PIOS_CALLBACKSCHEDULER_Initialize()
|
|
{
|
|
// Initialize variables
|
|
schedulerTasks = NULL;
|
|
schedulerStarted = false;
|
|
|
|
// Create mutex
|
|
mutex = xSemaphoreCreateRecursiveMutex();
|
|
if (mutex == NULL) {
|
|
return -1;
|
|
}
|
|
|
|
// Done
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Start all scheduler tasks
|
|
* Will instantiate all scheduler tasks registered so far. Although new
|
|
* callbacks CAN be registered beyond that point, any further scheduling tasks
|
|
* will be started the moment of instantiation. It is not possible to increase
|
|
* the STACK requirements of a scheduler task after this function has been
|
|
* run. No callbacks will be run before this function is called, although
|
|
* they can be marked for later execution by executing the dispatch function.
|
|
* \return Success (0), failure (-1)
|
|
*/
|
|
int32_t PIOS_CALLBACKSCHEDULER_Start()
|
|
{
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// only call once
|
|
PIOS_Assert(schedulerStarted == false);
|
|
|
|
// start tasks
|
|
struct DelayedCallbackTaskStruct *cursor = NULL;
|
|
int t = 0;
|
|
LL_FOREACH(schedulerTasks, cursor) {
|
|
xTaskCreate(
|
|
CallbackSchedulerTask,
|
|
cursor->name,
|
|
1 + (cursor->stackSize / 4),
|
|
cursor,
|
|
cursor->priorityTask,
|
|
&cursor->callbackSchedulerTaskHandle
|
|
);
|
|
if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) {
|
|
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle);
|
|
}
|
|
t++;
|
|
}
|
|
|
|
schedulerStarted = true;
|
|
|
|
xSemaphoreGiveRecursive(mutex);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Schedule dispatching a callback at some point in the future. The function returns immediately.
|
|
* \param[in] *cbinfo the callback handle
|
|
* \param[in] milliseconds How far in the future to dispatch the callback
|
|
* \param[in] updatemode What to do if the callback is already scheduled but not dispatched yet.
|
|
* The options are:
|
|
* UPDATEMODE_NONE: An existing schedule will not be touched, the call will have no effect at all if there's an existing schedule.
|
|
* UPDATEMODE_SOONER: The callback will be rescheduled only if the new schedule triggers before the original one would have triggered.
|
|
* UPDATEMODE_LATER: The callback will be rescheduled only if the new schedule triggers after the original one would have triggered.
|
|
* UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
|
|
* \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
|
|
*/
|
|
int32_t PIOS_CALLBACKSCHEDULER_Schedule(
|
|
DelayedCallbackInfo *cbinfo,
|
|
int32_t milliseconds,
|
|
DelayedCallbackUpdateMode updatemode)
|
|
{
|
|
int32_t result = 0;
|
|
|
|
PIOS_Assert(cbinfo);
|
|
|
|
if (milliseconds <= 0) {
|
|
milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t
|
|
}
|
|
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
uint32_t new = xTaskGetTickCount() + (milliseconds / portTICK_RATE_MS);
|
|
if (!new) {
|
|
new = 1; // zero has a special meaning, schedule at time 1 instead
|
|
}
|
|
|
|
int32_t diff = new - cbinfo->scheduletime;
|
|
if ((!cbinfo->scheduletime)
|
|
|| ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0)
|
|
|| ((updatemode & CALLBACK_UPDATEMODE_LATER) && diff > 0)
|
|
) {
|
|
// the scheduletime may be updated
|
|
if (!cbinfo->scheduletime) {
|
|
result = 1;
|
|
} else {
|
|
result = 2;
|
|
}
|
|
cbinfo->scheduletime = new;
|
|
|
|
// scheduler needs to be notified to adapt sleep times
|
|
xSemaphoreGive(cbinfo->task->signal);
|
|
}
|
|
|
|
xSemaphoreGiveRecursive(mutex);
|
|
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* Dispatch an event by invoking the supplied callback. The function
|
|
* returns immediately, the callback is invoked from the event task.
|
|
* \param[in] cbinfo the callback handle
|
|
* \return Success (-1), failure (0)
|
|
*/
|
|
int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo)
|
|
{
|
|
PIOS_Assert(cbinfo);
|
|
|
|
// no semaphore needed for the callback
|
|
cbinfo->waiting = true;
|
|
// but the scheduler as a whole needs to be notified
|
|
return xSemaphoreGive(cbinfo->task->signal);
|
|
}
|
|
|
|
/**
|
|
* Dispatch an event by invoking the supplied callback. The function
|
|
* returns immediately, the callback is invoked from the event task.
|
|
* \param[in] cbinfo the callback handle
|
|
* \param[in] pxHigherPriorityTaskWoken
|
|
* xSemaphoreGiveFromISR() will set *pxHigherPriorityTaskWoken to pdTRUE if
|
|
* giving the semaphore caused a task to unblock, and the unblocked task has a
|
|
* priority higher than the currently running task. If xSemaphoreGiveFromISR()
|
|
* sets this value to pdTRUE then a context switch should be requested before
|
|
* the interrupt is exited.
|
|
* From FreeRTOS Docu: Context switching from an ISR uses port specific syntax.
|
|
* Check the demo task for your port to find the syntax required.
|
|
* \return Success (-1), failure (0)
|
|
*/
|
|
int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
|
|
{
|
|
PIOS_Assert(cbinfo);
|
|
|
|
// no semaphore needed for the callback
|
|
cbinfo->waiting = true;
|
|
// but the scheduler as a whole needs to be notified
|
|
return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken);
|
|
}
|
|
|
|
/**
|
|
* Register a new callback to be called by a delayed callback scheduler task.
|
|
* If a scheduler task with the specified task priority does not exist yet, it
|
|
* will be created.
|
|
* \param[in] cb The callback to be invoked
|
|
* \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task.
|
|
* \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified,
|
|
* further callbacks created with the same priorityTask will all be handled by the same delayed callback scheduler task
|
|
* and scheduled according to their individual callback priorities
|
|
* \param[in] stacksize The stack requirements of the callback when called by the scheduler.
|
|
* \return CallbackInfo Pointer on success, NULL if failed.
|
|
*/
|
|
DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
|
|
DelayedCallback cb,
|
|
DelayedCallbackPriority priority,
|
|
DelayedCallbackPriorityTask priorityTask,
|
|
int16_t callbackID,
|
|
uint32_t stacksize)
|
|
{
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
|
|
// add callback schedulers own stack requirements
|
|
stacksize += STACK_SIZE;
|
|
|
|
// find appropriate scheduler task matching priorityTask
|
|
struct DelayedCallbackTaskStruct *task = NULL;
|
|
int t = 0;
|
|
LL_FOREACH(schedulerTasks, task) {
|
|
if (task->priorityTask == priorityTask) {
|
|
break; // found
|
|
}
|
|
t++;
|
|
}
|
|
// if given priorityTask does not exist, create it
|
|
if (!task) {
|
|
// allocate memory if possible
|
|
task = (struct DelayedCallbackTaskStruct *)pios_malloc(sizeof(struct DelayedCallbackTaskStruct));
|
|
if (!task) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
|
|
// initialize structure
|
|
for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) {
|
|
task->callbackQueue[p] = NULL;
|
|
task->queueCursor[p] = NULL;
|
|
}
|
|
task->name[0] = 'C';
|
|
task->name[1] = 'a' + t;
|
|
task->name[2] = 0;
|
|
task->stackSize = stacksize;
|
|
task->priorityTask = priorityTask;
|
|
task->next = NULL;
|
|
|
|
// create the signaling semaphore
|
|
vSemaphoreCreateBinary(task->signal);
|
|
if (!task->signal) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL;
|
|
}
|
|
|
|
// add to list of scheduler tasks
|
|
LL_APPEND(schedulerTasks, task);
|
|
|
|
// Previously registered tasks are spawned when PIOS_CALLBACKSCHEDULER_Start() is called.
|
|
// Tasks registered afterwards need to spawn upon creation.
|
|
if (schedulerStarted) {
|
|
xTaskCreate(
|
|
CallbackSchedulerTask,
|
|
task->name,
|
|
1 + (task->stackSize / 4),
|
|
task,
|
|
task->priorityTask,
|
|
&task->callbackSchedulerTaskHandle
|
|
);
|
|
if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) {
|
|
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!schedulerStarted && stacksize > task->stackSize) {
|
|
task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack
|
|
}
|
|
|
|
|
|
if (stacksize > task->stackSize) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL; // error - not enough memory
|
|
}
|
|
|
|
// initialize callback scheduling info
|
|
DelayedCallbackInfo *info = (DelayedCallbackInfo *)pios_malloc(sizeof(DelayedCallbackInfo));
|
|
if (!info) {
|
|
xSemaphoreGiveRecursive(mutex);
|
|
return NULL; // error - not enough memory
|
|
}
|
|
info->next = NULL;
|
|
info->waiting = false;
|
|
info->scheduletime = 0;
|
|
info->task = task;
|
|
info->cb = cb;
|
|
info->callbackID = callbackID;
|
|
info->runCount = 0;
|
|
info->stackSize = stacksize - STACK_SIZE;
|
|
info->stackNotFree = info->stackSize;
|
|
info->stackFree = 0;
|
|
info->stackSafetyCount = STACK_SAFETYCOUNT;
|
|
info->currentSafetyCount = 0;
|
|
|
|
// add to scheduling queue
|
|
LL_APPEND(task->callbackQueue[priority], info);
|
|
|
|
xSemaphoreGiveRecursive(mutex);
|
|
|
|
return info;
|
|
}
|
|
|
|
/**
|
|
* Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
|
|
*
|
|
* @param[in] callback Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
|
|
* @param context Context information optionally provided to the callback.
|
|
*/
|
|
void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context)
|
|
{
|
|
if (!callback) {
|
|
return;
|
|
}
|
|
|
|
struct pios_callback_info info;
|
|
|
|
struct DelayedCallbackTaskStruct *task = NULL;
|
|
LL_FOREACH(schedulerTasks, task) {
|
|
int prio;
|
|
|
|
for (prio = 0; prio < (CALLBACK_PRIORITY_LOW + 1); prio++) {
|
|
struct DelayedCallbackInfoStruct *cbinfo;
|
|
LL_FOREACH(task->callbackQueue[prio], cbinfo) {
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
|
info.is_running = true;
|
|
info.stack_remaining = cbinfo->stackNotFree;
|
|
info.running_time_count = cbinfo->runCount;
|
|
xSemaphoreGiveRecursive(mutex);
|
|
callback(cbinfo->callbackID, &info, context);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Stack magic, find how much stack is being used without affecting performance
|
|
*/
|
|
static void markStack(DelayedCallbackInfo *current)
|
|
{
|
|
register int8_t t;
|
|
register int32_t halfWayMark;
|
|
volatile unsigned char *marker;
|
|
|
|
if (current->stackNotFree < 0) {
|
|
return;
|
|
}
|
|
|
|
// end of stack watermark
|
|
marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
|
|
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
|
|
*(marker + t) = '#';
|
|
}
|
|
// shifted watermarks
|
|
halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
|
|
marker = (unsigned char *)((size_t)marker + halfWayMark);
|
|
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
|
|
*(marker + t) = '#';
|
|
}
|
|
}
|
|
static void checkStack(DelayedCallbackInfo *current)
|
|
{
|
|
register int8_t t;
|
|
register int32_t halfWayMark;
|
|
volatile unsigned char *marker;
|
|
|
|
if (current->stackNotFree < 0) {
|
|
return;
|
|
}
|
|
|
|
// end of stack watermark
|
|
marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
|
|
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
|
|
if (*(marker + t) != '#') {
|
|
current->stackNotFree = -1; // stack overflow, disable all further checks
|
|
return;
|
|
}
|
|
}
|
|
// shifted watermarks
|
|
halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
|
|
marker = (unsigned char *)((size_t)marker + halfWayMark);
|
|
for (t = -STACK_SAFETYSIZE; t < 0; t++) {
|
|
if (*(marker + t) != '#') {
|
|
current->stackNotFree = halfWayMark; // tainted mark, this place is definitely used stack
|
|
current->currentSafetyCount = 0;
|
|
if (current->stackNotFree <= current->stackFree) {
|
|
current->stackFree = 0; // if it was supposed to be free, restart search between here and bottom of stack
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (current->currentSafetyCount < 0xffff) {
|
|
current->currentSafetyCount++; // mark has not been tainted, increase safety counter
|
|
}
|
|
if (current->currentSafetyCount >= current->stackSafetyCount) { // if the safety counter is above the limit, then
|
|
if (halfWayMark == current->stackFree) { // check if search already converged, if so increase the limit to find very rare stack usage incidents
|
|
current->stackSafetyCount = current->currentSafetyCount;
|
|
} else {
|
|
current->stackFree = halfWayMark; // otherwise just mark this position as free stack to narrow search
|
|
current->currentSafetyCount = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Scheduler subtask
|
|
* \param[in] task The scheduler task in question
|
|
* \param[in] priority The scheduling priority of the callback to search for
|
|
* \return wait time until next scheduled callback is due - 0 if a callback has just been executed
|
|
*/
|
|
static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority)
|
|
{
|
|
int32_t result = MAX_SLEEP;
|
|
int32_t diff = 0;
|
|
|
|
// no such queue
|
|
if (priority > CALLBACK_PRIORITY_LOW) {
|
|
return result;
|
|
}
|
|
|
|
// queue is empty, search a lower priority queue
|
|
if (task->callbackQueue[priority] == NULL) {
|
|
return runNextCallback(task, priority + 1);
|
|
}
|
|
|
|
DelayedCallbackInfo *current = task->queueCursor[priority];
|
|
DelayedCallbackInfo *next;
|
|
do {
|
|
if (current == NULL) {
|
|
next = task->callbackQueue[priority]; // loop around the end of the list
|
|
// also attempt to run a callback that has lower priority
|
|
// every time the queue is completely traversed
|
|
diff = runNextCallback(task, priority + 1);
|
|
if (!diff) {
|
|
task->queueCursor[priority] = next; // the recursive call has executed a callback
|
|
return 0;
|
|
}
|
|
if (diff < result) {
|
|
result = diff; // adjust sleep time
|
|
}
|
|
} else {
|
|
next = current->next;
|
|
xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected
|
|
if (current->scheduletime) {
|
|
diff = current->scheduletime - xTaskGetTickCount();
|
|
if (diff <= 0) {
|
|
current->waiting = true;
|
|
} else if (diff < result) {
|
|
result = diff; // adjust sleep time
|
|
}
|
|
}
|
|
if (current->waiting) {
|
|
task->queueCursor[priority] = next;
|
|
current->scheduletime = 0; // any schedules are reset
|
|
current->waiting = false; // the flag is reset just before execution.
|
|
xSemaphoreGiveRecursive(mutex);
|
|
|
|
/* callback gets invoked here - check stack sizes */
|
|
markStack(current);
|
|
|
|
current->cb(); // call the callback
|
|
|
|
checkStack(current);
|
|
|
|
current->runCount++;
|
|
|
|
return 0;
|
|
}
|
|
xSemaphoreGiveRecursive(mutex);
|
|
}
|
|
current = next;
|
|
} while (current != task->queueCursor[priority]);
|
|
// once the list has been traversed entirely without finding any to be executed task, abort (nothing to do)
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* Scheduler task, responsible of invoking callbacks.
|
|
* \param[in] task The scheduling task being run
|
|
*/
|
|
static void CallbackSchedulerTask(void *task)
|
|
{
|
|
uint32_t delay = 0;
|
|
|
|
while (1) {
|
|
delay = runNextCallback((struct DelayedCallbackTaskStruct *)task, CALLBACK_PRIORITY_CRITICAL);
|
|
if (delay) {
|
|
// nothing to do but sleep
|
|
xSemaphoreTake(((struct DelayedCallbackTaskStruct *)task)->signal, delay);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
|