128 lines
3.8 KiB
C
128 lines
3.8 KiB
C
/*
|
|
ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
|
|
|
|
This file is part of ChibiOS/RT.
|
|
|
|
ChibiOS/RT is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
ChibiOS/RT is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <ch.h>
|
|
#include <nvic.h>
|
|
|
|
/*
|
|
* System idle thread loop.
|
|
*/
|
|
void _IdleThread(void *p) {
|
|
|
|
while (TRUE) {
|
|
}
|
|
}
|
|
|
|
/*
|
|
* System console message (not implemented).
|
|
*/
|
|
void chSysPuts(char *msg) {
|
|
}
|
|
|
|
/*
|
|
* Context switch.
|
|
*/
|
|
void chSysSwitchI(Thread *otp, Thread *ntp) {
|
|
register struct intctx * volatile sp asm("sp"); /* Don't ask... */
|
|
|
|
#ifdef CH_CURRP_REGISTER_CACHE
|
|
asm ("" : : : "r4", "r5", "r6", "r8", "r9", "r10", "r11", "lr");
|
|
#else
|
|
asm ("" : : : "r4", "r5", "r6", "r7", "r8", "r9", "r10", "r11", "lr");
|
|
#endif
|
|
otp->p_ctx.r13 = sp;
|
|
sp = ntp->p_ctx.r13;
|
|
}
|
|
|
|
/*
|
|
* System halt.
|
|
*/
|
|
__attribute__((naked, weak))
|
|
void chSysHalt(void) {
|
|
|
|
while (TRUE) {
|
|
}
|
|
}
|
|
|
|
__attribute__((naked, weak))
|
|
void threadstart(void) {
|
|
|
|
chSysUnlock();
|
|
asm volatile ("mov r0, r5 \n\t" \
|
|
"blx r4 \n\t" \
|
|
"bl chThdExit ");
|
|
}
|
|
|
|
/*
|
|
* System Timer vector.
|
|
*/
|
|
void SysTickVector(void) {
|
|
|
|
chSysIRQEnterI();
|
|
chSysTimerHandlerI();
|
|
chSysIRQExitI();
|
|
}
|
|
|
|
void *retaddr;
|
|
|
|
/*
|
|
* To be invoked at the end of any interrupt handler that can trigger a
|
|
* reschedule.
|
|
*/
|
|
void chSysIRQExitI(void) {
|
|
|
|
chSysLock();
|
|
if (SCB_ICSR & ICSR_RETTOBASE_MASK) {
|
|
if (chSchRescRequiredI()) {
|
|
|
|
asm volatile ("mrs r0, PSP \n\t" \
|
|
"ldr r2, [r0, #24] \n\t" \
|
|
"orr r2, r2, #1 \n\t" \
|
|
"ldr r1, =retaddr \n\t" \
|
|
"str r2, [r1] \n\t" \
|
|
"ldr r1, =threadswitch \n\t" \
|
|
"str r1, [r0, #24] ");
|
|
return; /* Note, returns *without* re-enabling interrupts.*/
|
|
}
|
|
}
|
|
chSysUnlock();
|
|
}
|
|
|
|
/*
|
|
* This code is executed in thread mode when exiting from an ISR routine that
|
|
* requires rescheduling.
|
|
*/
|
|
__attribute__((naked, weak))
|
|
void threadswitch(void) {
|
|
|
|
asm volatile ("sub sp, sp, #4 \n\t" \
|
|
"push {r0-r3, r12, lr} \n\t" \
|
|
"mrs r0, XPSR \n\t" \
|
|
"push {r0} \n\t" \
|
|
"ldr r0, =retaddr \n\t" \
|
|
"ldr r0, [r0] \n\t" \
|
|
"str r0, [sp, #28] \n\t" \
|
|
"bl chSchDoRescheduleI \n\t" \
|
|
"pop {r0} \n\t" \
|
|
"msr XPSR, r0 \n\t" \
|
|
"pop {r0-r3, r12, lr} \n\t" \
|
|
"cpsie i \n\t" \
|
|
"pop {pc} ");
|
|
}
|