scheduler.c

Go to the documentation of this file.

/*
 * SPDX-License-Identifier: GPL-3.0-only
 * This file is part of Lazuli.
 */

#include <stdint.h>

#include <Lazuli/common.h>
#include <Lazuli/config.h>
#include <Lazuli/lazuli.h>
#include <Lazuli/list.h>
#include <Lazuli/mutex.h>

#include <Lazuli/sys/arch/AVR/interrupts.h>
#include <Lazuli/sys/arch/arch.h>
#include <Lazuli/sys/clock_24.h>
#include <Lazuli/sys/compiler.h>
#include <Lazuli/sys/kernel.h>
#include <Lazuli/sys/memory.h>
#include <Lazuli/sys/scheduler.h>

static Task * currentTask;

static NOINIT Lz_LinkedList readyTasks[__LZ_SCHEDULING_POLICY_ENUM_END];

static Lz_LinkedList waitingActivationTasks = LINKED_LIST_INIT;

static NOINIT Lz_LinkedList waitingInterruptsTasks[INT_TOTAL];

static Lz_LinkedList waitingTimerTasks = LINKED_LIST_INIT;

static Lz_LinkedList terminatedTasks = LINKED_LIST_INIT;

static Lz_LinkedList abortedTasks = LINKED_LIST_INIT;

static NOINIT Task *idleTask;

static PROGMEM const Lz_TaskConfiguration DefaultTaskConfiguration = {
  NULL                              ,
  LZ_CONFIG_DEFAULT_TASK_STACK_SIZE ,
  PRIORITY_RT                       ,
  0                                 ,
  0                                 ,
  0                                 
};

static void
IdleTask(void)
{
  for (;;) {
    if (LZ_CONFIG_ON_IDLE_SLEEP) {
      Arch_CpuSleep();
    }
  }
}

static void
PrepareTaskContext(Task * const task)
{
  TaskContextLayout * const contextLayout
    = (TaskContextLayout *)(ALLOW_ARITHM(task->stackPointer)
                            - sizeof(TaskContextLayout) + 1);

  contextLayout->pc = ReverseBytesOfFunctionPointer(task->entryPoint);
  contextLayout->terminationCallback =
    ReverseBytesOfFunctionPointer(Lz_Task_Terminate);

  task->stackPointer = ALLOW_ARITHM((void*)contextLayout) - 1;
}

static bool
PeriodComparer(const Task * const task1, const Task * const task2)
{
  return task1->period > task2->period;
}

static bool
PriorityComparer(const Task * const task1, const Task * const task2)
{
  return task1->priority > task2->priority;
}

static void
InsertTaskByPriority(Lz_LinkedList * const list,
                     Task * const taskToInsert,
                     bool (*compareByProperty)(const Task * const ,
                                               const Task * const))
{
  Task *task;

  /*
   * This is commented on purpose.
   * TODO: Find a neat way to perform the equivalent. This function is static
   * so we master the parameters that are passed to that function.
   * Static checking could do the job.
   *
   * if (NULL == list || NULL == taskToInsert || NULL == compareByProperty) {
   *   return;
   * }
   */

  List_ForEach (list, Task, task, stateQueue) {
    if (compareByProperty(task, taskToInsert)) {
      List_InsertBefore(list,
                        &task->stateQueue,
                        &taskToInsert->stateQueue);

      return;
    }
  }

  List_Append(list, &taskToInsert->stateQueue);
}

static Task*
PickTaskToRun(void)
{
  /*
   * We can choose an uint8_t here as Lz_SchedulingPolicy has very few possible
   * values.
   */
  uint8_t i;

  for (i = 0; i < ELEMENTS_COUNT(readyTasks); ++i) {
    const Lz_LinkedListElement * const linkedListElement
      = List_PickFirst(&readyTasks[i]);

    if (NULL != linkedListElement) {
      return CONTAINER_OF(linkedListElement, stateQueue, Task);
    }
  }

  return idleTask;
}

static void
UpdateCyclicRealTimeTasks(void)
{
  Task *loopTask;
  Lz_LinkedListElement *iterator;

  /* Here we update the time until activation of all cyclic RT tasks */
  List_ForEach(&readyTasks[CYCLIC_RT], Task, loopTask, stateQueue) {
    loopTask->timeUntilActivation--;

    if (0 == loopTask->timeUntilActivation) {
      if (loopTask->timeUntilCompletion > 0) {
        /* TODO: Missed deadline */
      } else {
        loopTask->timeUntilActivation = loopTask->period;
      }
    }
  }

  /* After updating, we check if a cyclic RT task is ready to run */
  List_RemovableForEach(&waitingActivationTasks,
                        Task,
                        loopTask,
                        stateQueue,
                        iterator) {
    loopTask->timeUntilActivation--;

    if (0 == loopTask->timeUntilActivation) {
      iterator = List_Remove(&waitingActivationTasks, &loopTask->stateQueue);
      InsertTaskByPriority(&readyTasks[CYCLIC_RT], loopTask, PeriodComparer);

      loopTask->timeUntilActivation = loopTask->period;
      loopTask->timeUntilCompletion = loopTask->completion;
    }
  }
}

static void
UpdateTasksWaitingSoftwareTimer(void)
{
  Task *task;
  Lz_LinkedListElement *iterator;

  List_RemovableForEach(&waitingTimerTasks, Task, task, stateQueue, iterator) {
    --task->timeUntilTimerExpiration;

    if (0 == task->timeUntilTimerExpiration) {
      iterator = List_Remove(&waitingTimerTasks, &task->stateQueue);
      InsertTaskByPriority(&readyTasks[PRIORITY_RT], task, PriorityComparer);
    }
  }
}

static void
ManageCyclicRealTimeTask(void)
{
  --currentTask->timeUntilCompletion;

  if (WAIT_ACTIVATION == currentTask->taskToSchedulerMessage ||
      0 == currentTask->timeUntilCompletion) {
    List_Append(&waitingActivationTasks, &currentTask->stateQueue);

    return;
  }

  InsertTaskByPriority(&readyTasks[CYCLIC_RT], currentTask, PeriodComparer);
}

static void
ManagePriorityRealTimeTask(void)
{
  bool setCurrentTaskReady = false;

  if (WAIT_INTERRUPT == currentTask->taskToSchedulerMessage) {
    uint8_t interruptCode =
      *((uint8_t*)currentTask->taskToSchedulerMessageParameter);

    /*
     * If the requested interrupt code is not an acceptable value, we abort the
     * task.
     */
    if (LZ_CONFIG_CHECK_INTERRUPT_CODE_OVER_LAST_ENTRY &&
        (interruptCode > INT_LAST_ENTRY)) {
      List_Append(&abortedTasks, &currentTask->stateQueue);

      return;
    }

    List_Prepend(&waitingInterruptsTasks[interruptCode],
                 &currentTask->stateQueue);
  } else if (WAIT_SOFTWARE_TIMER == currentTask->taskToSchedulerMessage) {
    currentTask->timeUntilTimerExpiration =
      *(lz_u_resolution_unit_t*)currentTask->taskToSchedulerMessageParameter;
    if (0 == currentTask->timeUntilTimerExpiration) {
      setCurrentTaskReady = true;
    } else {
      List_Append(&waitingTimerTasks, &currentTask->stateQueue);
    }
  } else if (LZ_CONFIG_MODULE_MUTEX_USED &&
             (WAIT_MUTEX == currentTask->taskToSchedulerMessage)) {
    Lz_Mutex * const mutex = currentTask->taskToSchedulerMessageParameter;
    List_Prepend(&mutex->waitingTasks,
                 &currentTask->stateQueue);
  } else {
    setCurrentTaskReady = true;
  }

  if (setCurrentTaskReady) {
    InsertTaskByPriority(&readyTasks[PRIORITY_RT],
                         currentTask,
                         PriorityComparer);
  }
}

static void
Schedule(void)
{
  /*
   * We call this function before adding new tasks waiting for a software timer
   * in order to avoid decrementing the expiration counter immediately after
   * adding the task to the corresponding waiting list.
   */
  UpdateTasksWaitingSoftwareTimer();

  if (currentTask != idleTask) {
    if (TERMINATE_TASK == currentTask->taskToSchedulerMessage) {
      List_Append(&terminatedTasks, &currentTask->stateQueue);
    } else {
      void (* const jumpToManager[__LZ_SCHEDULING_POLICY_ENUM_END])(void) =
        {
          ManageCyclicRealTimeTask,
          ManagePriorityRealTimeTask
        };

      jumpToManager[currentTask->schedulingPolicy]();
    }
  }

  UpdateCyclicRealTimeTasks();

  currentTask->taskToSchedulerMessage = NO_MESSAGE;

  currentTask = PickTaskToRun();
}

static Task *
CallbackRegisterUserTask(const Lz_TaskConfiguration * const taskConfiguration)
{
  Task *newTask;
  /*
   * Jump table to the appropriate comparer, depending of the desired
   * scheduling policy.
   */
  bool (* const comparers[__LZ_SCHEDULING_POLICY_ENUM_END])
    (const Task * const,
     const Task * const) = {
                            PeriodComparer,
                            PriorityComparer
  };

  if (taskConfiguration->schedulingPolicy >= __LZ_SCHEDULING_POLICY_ENUM_END) {
    return NULL;
  }

  if (CYCLIC_RT == taskConfiguration->schedulingPolicy &&
      (0 == taskConfiguration->period || 0 == taskConfiguration->completion)) {
    return NULL;
  }

  newTask = KIncrementalMalloc(sizeof(Task));
  if (NULL == newTask) {
    return NULL;
  }

  newTask->priority = taskConfiguration->priority;
  newTask->period = taskConfiguration->period;
  newTask->completion = taskConfiguration->completion;

  newTask->timeUntilActivation = newTask->period;
  newTask->timeUntilCompletion = newTask->completion;

  newTask->taskToSchedulerMessage = NO_MESSAGE;

  List_InitLinkedListElement(&newTask->stateQueue);

  InsertTaskByPriority(&readyTasks[taskConfiguration->schedulingPolicy],
                       newTask,
                       comparers[taskConfiguration->schedulingPolicy]);

  return newTask;
}

static Task *
CallbackRegisterIdleTask(void)
{
  idleTask = KIncrementalMalloc(sizeof(Task));
  if (NULL == idleTask) {
    return NULL;
  }

  idleTask->taskToSchedulerMessage = NO_MESSAGE;

  return idleTask;
}

static bool
RegisterTask(void (* const taskEntryPoint)(void),
             Lz_TaskConfiguration * taskConfiguration,
             const bool isIdleTask)
{
  Lz_TaskConfiguration defaultConfiguration;
  Task *newTask;
  void *taskStack;
  size_t desiredStackSize;

  if (NULL == taskConfiguration) {
    Arch_LoadFromProgmem(&DefaultTaskConfiguration,
                         &defaultConfiguration,
                         sizeof(Lz_TaskConfiguration));
    taskConfiguration = &defaultConfiguration;
  } else if (taskConfiguration->stackSize < LZ_CONFIG_DEFAULT_TASK_STACK_SIZE) {
    taskConfiguration->stackSize = LZ_CONFIG_DEFAULT_TASK_STACK_SIZE;
  }

  if (isIdleTask) {
    newTask = CallbackRegisterIdleTask();
  } else {
    newTask = CallbackRegisterUserTask(taskConfiguration);
  }

  if (NULL == newTask) {
    return false;
  }

  desiredStackSize = taskConfiguration->stackSize
    /* We add enough space to contain the context of a task on the stack */
    + sizeof(TaskContextLayout)
    /* Plus 1 call to save_context_on_stack (in startup.S) */
    + sizeof(void *);

  taskStack = KIncrementalMalloc(desiredStackSize);
  if (NULL == taskStack) {
    return false;
  }

  newTask->schedulingPolicy = taskConfiguration->schedulingPolicy;
  newTask->name = taskConfiguration->name;
  newTask->entryPoint = taskEntryPoint;
  newTask->stackSize = desiredStackSize;
  newTask->stackOrigin = ALLOW_ARITHM(taskStack) + desiredStackSize - 1;
  newTask->stackPointer = newTask->stackOrigin;
  newTask->timeUntilTimerExpiration = 0;

  PrepareTaskContext(newTask);

  return true;
}

static bool
RegisterIdleTask(void)
{
  Lz_TaskConfiguration taskConfiguration;

  Lz_TaskConfiguration_Init(&taskConfiguration);

  taskConfiguration.stackSize = LZ_CONFIG_IDLE_TASK_STACK_SIZE;

  if (LZ_CONFIG_IDLE_TASK_HAS_NAME) {
    taskConfiguration.name = LZ_CONFIG_IDLE_TASK_NAME;
  }

  return RegisterTask(IdleTask, &taskConfiguration, true);
}

void
(*ReverseBytesOfFunctionPointer(void (* const pointer)(void)))(void)
{
  const uint8_t maxIndex = sizeof(pointer) - 1;
  const uint8_t * const oldPointerPointer = (const uint8_t * const)&pointer;
  void (*newPointer)(void);
  uint8_t * const newPointerPointer = (uint8_t * const)&newPointer;
  uint8_t i;

  for (i = 0; i <= maxIndex; ++i) {
    newPointerPointer[maxIndex - i] = oldPointerPointer[i];
  }

  return newPointer;
}

void
Scheduler_Init(void)
{
  size_t i;
  const Lz_LinkedList linkedListInit = LINKED_LIST_INIT;

  Arch_InitSystemTimer();

  for (i = 0; i < ELEMENTS_COUNT(readyTasks); ++i) {
    Memory_Copy(&linkedListInit, &readyTasks[i], sizeof(linkedListInit));
  }

  for (i = 0; i < ELEMENTS_COUNT(waitingInterruptsTasks); ++i) {
    Memory_Copy(&linkedListInit,
                &waitingInterruptsTasks[i],
                sizeof(linkedListInit));
  }
}

void
Scheduler_AbortTask(void * const sp)
{
  currentTask->stackPointer = sp;

  List_Append(&abortedTasks, &currentTask->stateQueue);

  Arch_EnableInterrupts();

  for(;;);

  /*
   * TODO: we have a problem here.
   * Here, CPU is now idle.
   * But Schedule() doesn't run the elected task.
   *
   * I think one good solution is to enter idle, and wait for the next clock
   * tick.
   */
}

/*
 * This function is executed on the current task's stack. So go easy with stack
 * usage.
 */
void
Scheduler_HandleInterrupt(const uint8_t interruptCode)
{
  Task *loopTask;
  Lz_LinkedListElement *iterator;

  if (LZ_CONFIG_CHECK_INTERRUPT_CODE_OVER_LAST_ENTRY) {
    if (interruptCode > INT_LAST_ENTRY) {
      Kernel_Panic();
    }
  }

  List_RemovableForEach(&waitingInterruptsTasks[interruptCode],
                        Task,
                        loopTask,
                        stateQueue,
                        iterator) {
    iterator = List_Remove(&waitingInterruptsTasks[interruptCode],
                           &loopTask->stateQueue);
    InsertTaskByPriority(&readyTasks[PRIORITY_RT], loopTask, PriorityComparer);
  }
}

void
Scheduler_HandleClockTick(void * const sp)
{
  currentTask->stackPointer = sp;

  if (LZ_CONFIG_MODULE_CLOCK_24_USED) {
    Clock24_Increment();
  }

  Schedule();

  Arch_RestoreContextAndReturnFromInterrupt(currentTask->stackPointer);
}

void
Scheduler_WakeupTasksWaitingMutex(Lz_Mutex * const mutex)
{
  Task *loopTask;
  Lz_LinkedListElement *iterator;

  /* TODO: Ugly */
  if (!LZ_CONFIG_MODULE_MUTEX_USED) {
    UNUSED(mutex);
    UNUSED(loopTask);
    UNUSED(iterator);

    return;
  }

  List_RemovableForEach(&mutex->waitingTasks,
                        Task,
                        loopTask,
                        stateQueue,
                        iterator) {
    iterator = List_Remove(&mutex->waitingTasks,
                           &loopTask->stateQueue);
    InsertTaskByPriority(&readyTasks[PRIORITY_RT], loopTask, PriorityComparer);
  }
}

Task*
Scheduler_GetCurrentTask(void)
{
  return currentTask;
}

void
Scheduler_SleepUntilEndOfTimeSlice(void)
{
  /*
   * We use this do-while loop because the task can be woken up by any interrupt
   * source. In the case of standard interrupts (i.e. not a clock tick)
   * the task needs to sleep again after the interruption has been managed.
   * In other words, currentTask->taskToSchedulerMessage will be equal to
   * NO_MESSAGE only if the task's time slice has finished.
   */
  do {
    Arch_CpuSleep();
  } while (NO_MESSAGE != currentTask->taskToSchedulerMessage);
}

void
Lz_TaskConfiguration_Init(Lz_TaskConfiguration * const taskConfiguration)
{
  if (NULL == taskConfiguration) {
    return;
  }

  Arch_LoadFromProgmem(&DefaultTaskConfiguration,
                       taskConfiguration,
                       sizeof(Lz_TaskConfiguration));
}

bool
Lz_RegisterTask(void (* const taskEntryPoint)(void),
                Lz_TaskConfiguration * taskConfiguration)
{
  return RegisterTask(taskEntryPoint, taskConfiguration, false);
}

void
Lz_Run(void)
{
  if (!RegisterIdleTask()) {
    Kernel_Panic();
  }

  currentTask = PickTaskToRun();

  Arch_StartSystemTimer();

  Arch_RestoreContextAndReturnFromInterrupt(currentTask->stackPointer);
}

const char *
Lz_Task_GetName(void)
{
  return currentTask->name;
}

void
Lz_Task_WaitActivation(void)
{
  currentTask->taskToSchedulerMessage = WAIT_ACTIVATION;

  Scheduler_SleepUntilEndOfTimeSlice();
}

void
Lz_Task_WaitInterrupt(uint8_t interruptCode)
{
  /* TODO: Check if the calling task's scheduling policy is PRIORITY_RT */

  currentTask->taskToSchedulerMessageParameter = &interruptCode;
  currentTask->taskToSchedulerMessage = WAIT_INTERRUPT;

  Scheduler_SleepUntilEndOfTimeSlice();
}

void
Lz_WaitTimer(lz_u_resolution_unit_t units)
{
  /* TODO: Check if the calling task's scheduling policy is PRIORITY_RT */

  currentTask->taskToSchedulerMessageParameter = &units;
  currentTask->taskToSchedulerMessage = WAIT_SOFTWARE_TIMER;

  Scheduler_SleepUntilEndOfTimeSlice();
}

void
Lz_Task_Terminate(void)
{
  currentTask->taskToSchedulerMessage = TERMINATE_TASK;

  Scheduler_SleepUntilEndOfTimeSlice();
}