Don't rely on the generic scheduler code to always have a timer no more than 1ms in the future. Instead, create an avr specific timer that will be called every 0x8000 ticks. This simplifies the generic code and it reduces the amount of code that needs to be run every millisecond. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
287 lines
7.5 KiB
C
287 lines
7.5 KiB
C
// Basic scheduling functions and startup/shutdown code.
|
|
//
|
|
// Copyright (C) 2016,2017 Kevin O'Connor <kevin@koconnor.net>
|
|
//
|
|
// This file may be distributed under the terms of the GNU GPLv3 license.
|
|
|
|
#include <setjmp.h> // setjmp
|
|
#include "autoconf.h" // CONFIG_*
|
|
#include "board/io.h" // readb
|
|
#include "board/irq.h" // irq_save
|
|
#include "board/misc.h" // timer_from_us
|
|
#include "board/pgm.h" // READP
|
|
#include "command.h" // shutdown
|
|
#include "sched.h" // sched_check_periodic
|
|
#include "stepper.h" // stepper_event
|
|
|
|
|
|
/****************************************************************
|
|
* Timers
|
|
****************************************************************/
|
|
|
|
static struct timer periodic_timer, *timer_list = &periodic_timer;
|
|
static struct timer sentinel_timer, deleted_timer;
|
|
|
|
// The periodic_timer simplifies the timer code by ensuring there is
|
|
// always a timer on the timer list and that there is always a timer
|
|
// not far in the future.
|
|
static uint_fast8_t
|
|
periodic_event(struct timer *t)
|
|
{
|
|
periodic_timer.waketime += timer_from_us(100000);
|
|
sentinel_timer.waketime = periodic_timer.waketime + 0x80000000;
|
|
return SF_RESCHEDULE;
|
|
}
|
|
|
|
static struct timer periodic_timer = {
|
|
.func = periodic_event,
|
|
.next = &sentinel_timer,
|
|
};
|
|
|
|
// The sentinel timer is always the last timer on timer_list - its
|
|
// presence allows the code to avoid checking for NULL while
|
|
// traversing timer_list. Since sentinel_timer.waketime is always
|
|
// equal to (periodic_timer.waketime + 0x80000000) any added timer
|
|
// must always have a waketime less than one of these two timers.
|
|
static uint_fast8_t
|
|
sentinel_event(struct timer *t)
|
|
{
|
|
shutdown("sentinel timer called");
|
|
}
|
|
|
|
static struct timer sentinel_timer = {
|
|
.func = sentinel_event,
|
|
.waketime = 0x80000000,
|
|
};
|
|
|
|
// Find position for a timer in timer_list and insert it
|
|
static void __always_inline
|
|
insert_timer(struct timer *t, uint32_t waketime)
|
|
{
|
|
struct timer *prev, *pos = timer_list;
|
|
for (;;) {
|
|
prev = pos;
|
|
if (CONFIG_MACH_AVR)
|
|
// micro optimization for AVR - reduces register pressure
|
|
asm("" : "+r"(prev));
|
|
pos = pos->next;
|
|
if (timer_is_before(waketime, pos->waketime))
|
|
break;
|
|
}
|
|
t->next = pos;
|
|
prev->next = t;
|
|
}
|
|
|
|
// Schedule a function call at a supplied time.
|
|
void
|
|
sched_add_timer(struct timer *add)
|
|
{
|
|
uint32_t waketime = add->waketime;
|
|
irqstatus_t flag = irq_save();
|
|
if (unlikely(timer_is_before(waketime, timer_list->waketime))) {
|
|
// This timer is before all other scheduled timers
|
|
struct timer *tl = timer_list;
|
|
if (timer_is_before(waketime, timer_read_time() + timer_from_us(2000)))
|
|
try_shutdown("Timer too close");
|
|
if (tl == &deleted_timer)
|
|
add->next = deleted_timer.next;
|
|
else
|
|
add->next = tl;
|
|
deleted_timer.waketime = waketime;
|
|
deleted_timer.next = add;
|
|
timer_list = &deleted_timer;
|
|
timer_kick();
|
|
} else {
|
|
insert_timer(add, waketime);
|
|
}
|
|
irq_restore(flag);
|
|
}
|
|
|
|
// The deleted timer is used when deleting an active timer.
|
|
static uint_fast8_t
|
|
deleted_event(struct timer *t)
|
|
{
|
|
return SF_DONE;
|
|
}
|
|
|
|
static struct timer deleted_timer = {
|
|
.func = deleted_event,
|
|
};
|
|
|
|
// Remove a timer that may be live.
|
|
void
|
|
sched_del_timer(struct timer *del)
|
|
{
|
|
irqstatus_t flag = irq_save();
|
|
if (timer_list == del) {
|
|
// Deleting the next active timer - replace with deleted_timer
|
|
deleted_timer.waketime = del->waketime;
|
|
deleted_timer.next = del->next;
|
|
timer_list = &deleted_timer;
|
|
} else {
|
|
// Find and remove from timer list (if present)
|
|
struct timer *pos;
|
|
for (pos = timer_list; pos->next; pos = pos->next) {
|
|
if (pos->next == del) {
|
|
pos->next = del->next;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
irq_restore(flag);
|
|
}
|
|
|
|
// Invoke the next timer - called from board hardware irq code.
|
|
unsigned int
|
|
sched_timer_dispatch(void)
|
|
{
|
|
// Invoke timer callback
|
|
struct timer *t = timer_list;
|
|
uint_fast8_t res;
|
|
uint32_t updated_waketime;
|
|
if (CONFIG_INLINE_STEPPER_HACK && likely(!t->func)) {
|
|
res = stepper_event(t);
|
|
updated_waketime = t->waketime;
|
|
} else {
|
|
res = t->func(t);
|
|
updated_waketime = t->waketime;
|
|
}
|
|
|
|
// Update timer_list (rescheduling current timer if necessary)
|
|
unsigned int next_waketime = updated_waketime;
|
|
if (unlikely(res == SF_DONE)) {
|
|
next_waketime = t->next->waketime;
|
|
timer_list = t->next;
|
|
} else if (!timer_is_before(updated_waketime, t->next->waketime)) {
|
|
next_waketime = t->next->waketime;
|
|
timer_list = t->next;
|
|
insert_timer(t, updated_waketime);
|
|
}
|
|
|
|
return next_waketime;
|
|
}
|
|
|
|
// Remove all user timers on a shutdown
|
|
static void
|
|
sched_timer_reset(void)
|
|
{
|
|
timer_list = &deleted_timer;
|
|
deleted_timer.waketime = periodic_timer.waketime;
|
|
deleted_timer.next = &periodic_timer;
|
|
periodic_timer.next = &sentinel_timer;
|
|
timer_kick();
|
|
}
|
|
|
|
|
|
/****************************************************************
|
|
* Task waking
|
|
****************************************************************/
|
|
|
|
// Note that a task is ready to run
|
|
void
|
|
sched_wake_task(struct task_wake *w)
|
|
{
|
|
writeb(&w->wake, 1);
|
|
}
|
|
|
|
// Check if a task is ready to run (as indicated by sched_wake_task)
|
|
uint8_t
|
|
sched_check_wake(struct task_wake *w)
|
|
{
|
|
if (!readb(&w->wake))
|
|
return 0;
|
|
writeb(&w->wake, 0);
|
|
return 1;
|
|
}
|
|
|
|
|
|
/****************************************************************
|
|
* Shutdown processing
|
|
****************************************************************/
|
|
|
|
static uint_fast8_t shutdown_status, shutdown_reason;
|
|
|
|
// Return true if the machine is in an emergency stop state
|
|
uint8_t
|
|
sched_is_shutdown(void)
|
|
{
|
|
return !!shutdown_status;
|
|
}
|
|
|
|
// Transition out of shutdown state
|
|
void
|
|
sched_clear_shutdown(void)
|
|
{
|
|
if (!shutdown_status)
|
|
shutdown("Shutdown cleared when not shutdown");
|
|
if (shutdown_status == 2)
|
|
// Ignore attempt to clear shutdown if still processing shutdown
|
|
return;
|
|
shutdown_status = 0;
|
|
}
|
|
|
|
// Invoke all shutdown functions (as declared by DECL_SHUTDOWN)
|
|
static void
|
|
run_shutdown(int reason)
|
|
{
|
|
uint32_t cur = timer_read_time();
|
|
if (!shutdown_status)
|
|
shutdown_reason = reason;
|
|
shutdown_status = 2;
|
|
sched_timer_reset();
|
|
extern void ctr_run_shutdownfuncs(void);
|
|
ctr_run_shutdownfuncs();
|
|
shutdown_status = 1;
|
|
irq_enable();
|
|
|
|
sendf("shutdown clock=%u static_string_id=%hu", cur, shutdown_reason);
|
|
}
|
|
|
|
// Report the last shutdown reason code
|
|
void
|
|
sched_report_shutdown(void)
|
|
{
|
|
sendf("is_shutdown static_string_id=%hu", shutdown_reason);
|
|
}
|
|
|
|
// Shutdown the machine if not already in the process of shutting down
|
|
void
|
|
sched_try_shutdown(uint_fast8_t reason)
|
|
{
|
|
if (shutdown_status != 2)
|
|
sched_shutdown(reason);
|
|
}
|
|
|
|
static jmp_buf shutdown_jmp;
|
|
|
|
// Force the machine to immediately run the shutdown handlers
|
|
void
|
|
sched_shutdown(uint_fast8_t reason)
|
|
{
|
|
irq_disable();
|
|
longjmp(shutdown_jmp, reason);
|
|
}
|
|
|
|
|
|
/****************************************************************
|
|
* Startup and background task processing
|
|
****************************************************************/
|
|
|
|
// Auto-generated code in out/compile_time_requests.c
|
|
extern void ctr_run_initfuncs(void);
|
|
extern void ctr_run_taskfuncs(void);
|
|
|
|
// Main loop of program
|
|
void
|
|
sched_main(void)
|
|
{
|
|
ctr_run_initfuncs();
|
|
|
|
int ret = setjmp(shutdown_jmp);
|
|
if (ret)
|
|
run_shutdown(ret);
|
|
|
|
for (;;)
|
|
ctr_run_taskfuncs();
|
|
}
|