REVONANO Milestones update

[librepilot.git] / flight / pios / common / pios_callbackscheduler.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