avr-fw-modules/core/src/systick.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( &current_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( &current_thread()->statistic.scheduled_time );

#endif

	SP = (uint16_t)pUserSP;
};

void __systick_enclosed(void){
	
#if defined(DEBUG_SCHEDULING)

	if (current_thread()){
		thread_stat_unschedule( &current_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( &current_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;
};