forked from haraldwolff/avr-fw-modules
344 lines
5.7 KiB
C
344 lines
5.7 KiB
C
#include <hwo/stack.h>
|
|
#include <hwo/utils.h>
|
|
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
#include <avr/wdt.h>
|
|
#include <avr/eeprom.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <sys/adc.h>
|
|
#include <sys/threads.h>
|
|
#include <sys/timer.h>
|
|
#include <sys/events.h>
|
|
#include <sys/errno.h>
|
|
#include <sys/systick.h>
|
|
|
|
#include <cpu/systick.h>
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
#pragma message "SCHEDULING: DEBUG MODE"
|
|
#else
|
|
//#pragma message "SCHEDULING: RELEASE MODE"
|
|
#endif
|
|
|
|
|
|
|
|
|
|
uint8_t* dummy_scheduler(uint8_t* oldstack);
|
|
uint8_t* scheduler(uint8_t* oldstack) __attribute__((weak,alias("dummy_scheduler")));
|
|
|
|
int __systick_interval__ = 1000;
|
|
extern int __systick_interval __attribute__((weak, alias("__systick_interval__")));
|
|
|
|
|
|
|
|
systick_t _systick_us; // Increment time by <_timebase> [us] at every call to timebase()
|
|
systick_t volatile _systick_ticks; // system uptime in [us]
|
|
uint32_t volatile _systick_secs; // system uptime in [s]
|
|
uint32_t volatile _systick_sec_frac;
|
|
|
|
int32_t _systick_correct;
|
|
int32_t _systick_correct_sum;
|
|
|
|
uint8_t *pSystickSP;
|
|
uint8_t *pUserSP;
|
|
|
|
systick_handler _systick_handler,
|
|
_systick_handler_sec;
|
|
|
|
/* System Timers Queue */
|
|
list_t sys_timers;
|
|
list_t lh_periodic;
|
|
list_t *periodic_item;
|
|
|
|
/* System Timers Queue */
|
|
list_t sys_timers;
|
|
|
|
/* ADC SW Watchdog */
|
|
uint8_t _adc_wdog;
|
|
uint8_t _adc_wdog_flag;
|
|
|
|
/* Entwicklungsunterstützung */
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
int16_t _st_lag_current,
|
|
_st_lag_min,
|
|
_st_lag_max;
|
|
|
|
#endif
|
|
|
|
|
|
|
|
void __wdt_reset(void) __attribute__((naked)) __attribute__((section (".init2")));
|
|
void __wdt_reset(void){
|
|
wdt_enable(WDTO_1S);
|
|
};
|
|
|
|
void __systick_init8(void) __attribute__((naked)) __attribute__((section (".init8")));
|
|
void __systick_init8(void)
|
|
{
|
|
pSystickSP = (uint8_t*)RAMEND;
|
|
|
|
__malloc_heap_end = (void*)(RAMEND - 128);
|
|
|
|
systick_init( __systick_interval );
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
_st_lag_min = 0x7FFF;
|
|
|
|
#endif
|
|
|
|
sei();
|
|
|
|
while (1);
|
|
};
|
|
|
|
int systick_correct(int32_t correct){
|
|
ATOMIC
|
|
_systick_correct = correct;
|
|
return ESUCCESS;
|
|
};
|
|
|
|
void systick_init(systick_t systick_us)
|
|
{
|
|
_systick_us = systick_us;
|
|
systick_timer_init(systick_us);
|
|
|
|
list_init( &sys_timers );
|
|
};
|
|
|
|
void st_timer(void){
|
|
list_t *i,*tmp;
|
|
SYSTIMER *t;
|
|
|
|
/* Handle System Timer Queue */
|
|
for_each_list_entry_save(i,tmp,&sys_timers){
|
|
t = list_entry(i,SYSTIMER,list);
|
|
|
|
if ((t->timeout) && (t->timeout < _systick_ticks)){
|
|
|
|
event_push( EV_SYS_TIMER, t->id, 0, t->handler );
|
|
|
|
if (t->repeat && t->elapse){
|
|
t->timeout = _systick_ticks + (t->elapse);
|
|
} else {
|
|
list_remove( i );
|
|
};
|
|
};
|
|
};
|
|
|
|
};
|
|
|
|
void systick(void)
|
|
{
|
|
wdt_reset();
|
|
|
|
_systick_ticks += _systick_us;
|
|
_systick_sec_frac += _systick_us;
|
|
|
|
_systick_correct_sum += _systick_correct;
|
|
if (_systick_correct_sum >= 1000) {
|
|
while (_systick_correct_sum >= 1000){
|
|
_systick_correct_sum -= 1000;
|
|
_systick_sec_frac += 1;
|
|
};
|
|
} else if (_systick_correct_sum <= -1000) {
|
|
while (_systick_correct_sum <= -1000){
|
|
_systick_correct_sum += 1000;
|
|
_systick_sec_frac -= 1;
|
|
};
|
|
};
|
|
|
|
|
|
if (_systick_sec_frac >= 1000000)
|
|
{
|
|
_systick_sec_frac -= 1000000;
|
|
_systick_secs++;
|
|
if (_systick_handler_sec)
|
|
_systick_handler_sec(_systick_us);
|
|
|
|
};
|
|
|
|
if (_systick_handler)
|
|
_systick_handler(_systick_ticks);
|
|
|
|
st_schedule();
|
|
st_timer();
|
|
|
|
if (_adc_wdog_flag){
|
|
if (_adc_wdog){
|
|
_adc_wdog--;
|
|
} else {
|
|
if (_adc_wdog_flag < 3){
|
|
adc_reinit();
|
|
} else {
|
|
// Wait for HW Watchdog...
|
|
while (1){ };
|
|
};
|
|
};
|
|
};
|
|
|
|
};
|
|
|
|
void call_sys_ctx(void (*proc)(void)){
|
|
ATOMIC
|
|
|
|
pUserSP = (uint8_t*)SP;
|
|
SP = (uint16_t)pSystickSP;
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
thread_stat_unschedule( ¤t_thread()->statistic.scheduled_time );
|
|
thread_stat_schedule( &_ts_irq_time );
|
|
|
|
#endif
|
|
|
|
if (proc)
|
|
proc();
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
thread_stat_unschedule( &_ts_irq_time );
|
|
thread_stat_schedule( ¤t_thread()->statistic.scheduled_time );
|
|
|
|
#endif
|
|
|
|
SP = (uint16_t)pUserSP;
|
|
};
|
|
|
|
void __systick_enclosed(void){
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
if (current_thread()){
|
|
thread_stat_unschedule( ¤t_thread()->statistic.scheduled_time );
|
|
};
|
|
st_reset_last_counter();
|
|
thread_stat_schedule( &_ts_sys_time );
|
|
|
|
_st_lag_current = st_current_counter();
|
|
if (_st_lag_max < _st_lag_current){
|
|
_st_lag_max = _st_lag_current;
|
|
memcpy( rt_dbg->last_late_vect, rt_dbg->last_vect, sizeof(rt_dbg->last_late_vect));
|
|
};
|
|
if (_st_lag_min > _st_lag_current){
|
|
_st_lag_min = _st_lag_current;
|
|
};
|
|
|
|
#endif
|
|
|
|
systick();
|
|
|
|
pUserSP = scheduler( pUserSP );
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
thread_stat_unschedule( &_ts_sys_time );
|
|
thread_stat_schedule( ¤t_thread()->statistic.scheduled_time );
|
|
|
|
#endif
|
|
|
|
};
|
|
|
|
ISR(SYSTICK_ISR, ISR_NAKED)
|
|
{
|
|
__asm (
|
|
"push r0\n"
|
|
"push r1\n"
|
|
"push r2\n"
|
|
"push r3\n"
|
|
"push r4\n"
|
|
"push r5\n"
|
|
"push r6\n"
|
|
"push r7\n"
|
|
"push r8\n"
|
|
"push r9\n"
|
|
"push r10\n"
|
|
"push r11\n"
|
|
"push r12\n"
|
|
"push r13\n"
|
|
"push r14\n"
|
|
"push r15\n"
|
|
"push r16\n"
|
|
"push r17\n"
|
|
"push r18\n"
|
|
"push r19\n"
|
|
"push r20\n"
|
|
"push r21\n"
|
|
"push r22\n"
|
|
"push r23\n"
|
|
"push r24\n"
|
|
"push r25\n"
|
|
"push r26\n"
|
|
"push r27\n"
|
|
"push r28\n"
|
|
"push r29\n"
|
|
"push r30\n"
|
|
"push r31\n"
|
|
|
|
"in r16, 0x3f\n" // ; Read SREG
|
|
"ori r16, 0x80\n" // I-Flag has been set, we were called by HW-IRQ
|
|
"push r16\n"
|
|
|
|
"eor r1, r1\n"
|
|
|
|
);
|
|
|
|
pUserSP = (uint8_t*)SP;
|
|
SP = (uint16_t)pSystickSP;
|
|
|
|
__systick_enclosed();
|
|
|
|
SP = (uint16_t)pUserSP;
|
|
|
|
__asm(
|
|
"pop r2\n"
|
|
|
|
"pop r31\n"
|
|
"pop r30\n"
|
|
"pop r29\n"
|
|
"pop r28\n"
|
|
"pop r27\n"
|
|
"pop r26\n"
|
|
"pop r25\n"
|
|
"pop r24\n"
|
|
"pop r23\n"
|
|
"pop r22\n"
|
|
"pop r21\n"
|
|
"pop r20\n"
|
|
"pop r19\n"
|
|
"pop r18\n"
|
|
"pop r17\n"
|
|
"pop r16\n"
|
|
"pop r15\n"
|
|
"pop r14\n"
|
|
"pop r13\n"
|
|
"pop r12\n"
|
|
"pop r11\n"
|
|
"pop r10\n"
|
|
"pop r9\n"
|
|
"pop r8\n"
|
|
"pop r7\n"
|
|
"pop r6\n"
|
|
"pop r5\n"
|
|
"pop r4\n"
|
|
"pop r3\n"
|
|
|
|
"out 0x3f, r2\n"
|
|
|
|
"pop r2\n"
|
|
"pop r1\n"
|
|
"pop r0\n"
|
|
|
|
"ret\n"
|
|
);
|
|
};
|
|
|
|
uint8_t* dummy_scheduler(uint8_t* oldstack)
|
|
{
|
|
return oldstack;
|
|
};
|