forked from haraldwolff/avr-fw-modules
121 lines
1.6 KiB
C
121 lines
1.6 KiB
C
#include <hwo/threads.h>
|
|
#include <sys/atomic.h>
|
|
|
|
extern uint8_t *pSystickSP;
|
|
extern uint8_t *pUserSP;
|
|
|
|
uint8_t* scheduler(uint8_t* oldstack);
|
|
|
|
void yield(void) __attribute__((naked));
|
|
void yield(void)
|
|
{
|
|
__asm (
|
|
"push r0\n"
|
|
"push r1\n"
|
|
"push r2\n"
|
|
|
|
"in r2, 0x3f\n" // ; Read SREG
|
|
"cli\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"
|
|
|
|
"push r2\n"
|
|
|
|
"eor r1, r1\n"
|
|
|
|
);
|
|
|
|
pUserSP = (uint8_t*)SP;
|
|
SP = (uint16_t)pSystickSP;
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
thread_stat_unschedule( ¤t_thread()->statistic.scheduled_time );
|
|
thread_stat_schedule( &_ts_sys_time );
|
|
|
|
#endif
|
|
|
|
pUserSP = scheduler( pUserSP );
|
|
|
|
#if defined(DEBUG_SCHEDULING)
|
|
|
|
thread_stat_unschedule( &_ts_sys_time );
|
|
thread_stat_schedule( ¤t_thread()->statistic.scheduled_time );
|
|
|
|
#endif
|
|
|
|
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"
|
|
);
|
|
};
|
|
|
|
|