twr_scheduler.c

#include <twr_scheduler.h>
#include <twr_system.h>
#include <twr_error.h>
 
static struct
{
    struct
    {
        twr_tick_t tick_execution;
        void (*task)(void *);
        void *param;
 
    } pool[TWR_SCHEDULER_MAX_TASKS];
 
    twr_tick_t tick_spin;
    twr_scheduler_task_id_t current_task_id;
    twr_scheduler_task_id_t max_task_id;
 
} _twr_scheduler;
 
void application_idle();
void application_error(twr_error_t code);
 
void twr_scheduler_init(void)
{
    memset(&_twr_scheduler, 0, sizeof(_twr_scheduler));
}
 
void twr_scheduler_run(void)
{
    static twr_scheduler_task_id_t *task_id = &_twr_scheduler.current_task_id;
 
    while (true)
    {
        _twr_scheduler.tick_spin = twr_tick_get();
 
        for (*task_id = 0; *task_id <= _twr_scheduler.max_task_id; (*task_id)++)
        {
            if (_twr_scheduler.pool[*task_id].task != NULL)
            {
                if (_twr_scheduler.tick_spin >= _twr_scheduler.pool[*task_id].tick_execution)
                {
                    _twr_scheduler.pool[*task_id].tick_execution = TWR_TICK_INFINITY;
 
                    _twr_scheduler.pool[*task_id].task(_twr_scheduler.pool[*task_id].param);
                }
            }
        }
        application_idle();
    }
}
 
twr_scheduler_task_id_t twr_scheduler_register(void (*task)(void *), void *param, twr_tick_t tick)
{
    for (twr_scheduler_task_id_t i = 0; i < TWR_SCHEDULER_MAX_TASKS; i++)
    {
        if (_twr_scheduler.pool[i].task == NULL)
        {
            _twr_scheduler.pool[i].tick_execution = tick;
            _twr_scheduler.pool[i].task = task;
            _twr_scheduler.pool[i].param = param;
 
            if (_twr_scheduler.max_task_id < i)
            {
                _twr_scheduler.max_task_id = i;
            }
 
            return i;
        }
    }
 
    application_error(TWR_ERROR_NOT_ENOUGH_TASKS);
 
    return 0;
}
 
void twr_scheduler_unregister(twr_scheduler_task_id_t task_id)
{
    if (task_id >= TWR_SCHEDULER_MAX_TASKS)
    {
        application_error(TWR_ERROR_INVALID_PARAMETER);
    }
    _twr_scheduler.pool[task_id].task = NULL;
 
    if (_twr_scheduler.max_task_id == task_id)
    {
        do
        {
            if (_twr_scheduler.max_task_id == 0)
            {
                break;
            }
 
            _twr_scheduler.max_task_id--;
 
        } while (_twr_scheduler.pool[_twr_scheduler.max_task_id].task == NULL);
    }
}
 
twr_scheduler_task_id_t twr_scheduler_get_current_task_id(void)
{
    return _twr_scheduler.current_task_id;
}
 
twr_tick_t twr_scheduler_get_spin_tick(void)
{
    return _twr_scheduler.tick_spin;
}
 
void twr_scheduler_plan_now(twr_scheduler_task_id_t task_id)
{
    if (task_id >= TWR_SCHEDULER_MAX_TASKS)
    {
        application_error(TWR_ERROR_INVALID_PARAMETER);
    }
    _twr_scheduler.pool[task_id].tick_execution = 0;
}
 
void twr_scheduler_plan_absolute(twr_scheduler_task_id_t task_id, twr_tick_t tick)
{
    if (task_id >= TWR_SCHEDULER_MAX_TASKS || _twr_scheduler.pool[task_id].task == NULL)
    {
        application_error(TWR_ERROR_INVALID_PARAMETER);
    }
    _twr_scheduler.pool[task_id].tick_execution = tick;
}
 
void twr_scheduler_plan_relative(twr_scheduler_task_id_t task_id, twr_tick_t tick)
{
    if (task_id >= TWR_SCHEDULER_MAX_TASKS || _twr_scheduler.pool[task_id].task == NULL)
    {
        application_error(TWR_ERROR_INVALID_PARAMETER);
    }
    _twr_scheduler.pool[task_id].tick_execution = _twr_scheduler.tick_spin + tick;
}
 
void twr_scheduler_plan_from_now(twr_scheduler_task_id_t task_id, twr_tick_t tick)
{
    if (task_id >= TWR_SCHEDULER_MAX_TASKS || _twr_scheduler.pool[task_id].task == NULL)
    {
        application_error(TWR_ERROR_INVALID_PARAMETER);
    }
    _twr_scheduler.pool[task_id].tick_execution = twr_tick_get() + tick;
}
 
void twr_scheduler_plan_current_now(void)
{
    _twr_scheduler.pool[_twr_scheduler.current_task_id].tick_execution = 0;
}
 
void twr_scheduler_plan_current_absolute(twr_tick_t tick)
{
    _twr_scheduler.pool[_twr_scheduler.current_task_id].tick_execution = tick;
}
 
void twr_scheduler_plan_current_relative(twr_tick_t tick)
{
    _twr_scheduler.pool[_twr_scheduler.current_task_id].tick_execution = _twr_scheduler.tick_spin + tick;
}
 
void twr_scheduler_plan_current_from_now(twr_tick_t tick)
{
    _twr_scheduler.pool[_twr_scheduler.current_task_id].tick_execution = twr_tick_get() + tick;
}