122 lines
3.3 KiB
ArmAsm
122 lines
3.3 KiB
ArmAsm
#include <p32xxxx.h>
|
|
#include "../common/mips.inc"
|
|
#include "../common/stackframe.h"
|
|
|
|
.section ".text", "ax"
|
|
.set noat
|
|
.set noreorder
|
|
|
|
/*
|
|
* rt_base_t rt_hw_interrupt_disable()
|
|
*/
|
|
.globl rt_hw_interrupt_disable
|
|
rt_hw_interrupt_disable:
|
|
mfc0 v0, CP0_STATUS /* v0 = status */
|
|
addiu v1, zero, -2 /* v1 = 0-2 = 0xFFFFFFFE */
|
|
and v1, v0, v1 /* v1 = v0 & 0xFFFFFFFE */
|
|
mtc0 v1, CP0_STATUS /* status = v1 */
|
|
jr ra
|
|
nop
|
|
|
|
/*
|
|
* void rt_hw_interrupt_enable(rt_base_t level)
|
|
*/
|
|
.globl rt_hw_interrupt_enable
|
|
rt_hw_interrupt_enable:
|
|
mtc0 a0, CP0_STATUS
|
|
jr ra
|
|
nop
|
|
|
|
/*
|
|
* void rt_hw_context_switch_to(rt_uint32 to)/*
|
|
* a0 --> to
|
|
*/
|
|
.globl rt_hw_context_switch_to
|
|
rt_hw_context_switch_to:
|
|
lw sp, 0(a0) /* get new task stack pointer */
|
|
|
|
RESTORE_ALL_AND_RET
|
|
|
|
/*
|
|
* void rt_hw_context_switch(rt_uint32 from, rt_uint32 to)
|
|
* a0 --> from
|
|
* a1 --> to
|
|
*/
|
|
.globl rt_hw_context_switch
|
|
rt_hw_context_switch:
|
|
mtc0 ra, CP0_EPC
|
|
SAVE_ALL
|
|
|
|
sw sp, 0(a0) /* store sp in preempted tasks TCB */
|
|
lw sp, 0(a1) /* get new task stack pointer */
|
|
|
|
RESTORE_ALL_AND_RET
|
|
|
|
/*
|
|
* void rt_hw_context_switch_interrupt(rt_uint32 from, rt_uint32 to)/*
|
|
*/
|
|
.globl rt_thread_switch_interrupt_flag
|
|
.globl rt_interrupt_from_thread
|
|
.globl rt_interrupt_to_thread
|
|
.globl rt_hw_context_switch_interrupt
|
|
rt_hw_context_switch_interrupt:
|
|
la t0, rt_thread_switch_interrupt_flag
|
|
lw t1, 0(t0)
|
|
nop
|
|
bnez t1, _reswitch
|
|
nop
|
|
li t1, 0x01 /* set rt_thread_switch_interrupt_flag to 1 */
|
|
sw t1, 0(t0)
|
|
la t0, rt_interrupt_from_thread /* set rt_interrupt_from_thread */
|
|
sw a0, 0(t0)
|
|
_reswitch:
|
|
la t0, rt_interrupt_to_thread /* set rt_interrupt_to_thread */
|
|
sw a1, 0(t0)
|
|
|
|
/* trigger the soft exception (causes context switch) */
|
|
mfc0 t0, CP0_CAUSE /* t0 = Cause */
|
|
ori t0, t0, (1<<8) /* t0 |= (1<<8) */
|
|
mtc0 t0, CP0_CAUSE /* cause = t0 */
|
|
addiu t1, zero, -257 /* t1 = ~(1<<8) */
|
|
and t0, t0, t1 /* t0 &= t1 */
|
|
mtc0 t0, CP0_CAUSE /* cause = t0 */
|
|
jr ra
|
|
nop
|
|
|
|
/*
|
|
* void __ISR(_CORE_SOFTWARE_0_VECTOR, ipl2) CoreSW0Handler(void)
|
|
*/
|
|
.section ".text", "ax"
|
|
.set noreorder
|
|
.set noat
|
|
.ent CoreSW0Handler
|
|
|
|
.globl CoreSW0Handler
|
|
CoreSW0Handler:
|
|
SAVE_ALL
|
|
|
|
/* mCS0ClearIntFlag(); */
|
|
la t0, IFS0CLR /* t0 = IFS0CLR */
|
|
addiu t1,zero,0x02 /* t1 = (1<<2) */
|
|
sw t1, 0(t0) /* IFS0CLR = t1 */
|
|
|
|
la k0, rt_thread_switch_interrupt_flag
|
|
sw zero, 0(k0) /* clear flag */
|
|
|
|
/*
|
|
* switch to the new thread
|
|
*/
|
|
la k0, rt_interrupt_from_thread
|
|
lw k1, 0(k0)
|
|
nop
|
|
sw sp, 0(k1) /* store sp in preempted tasks's TCB */
|
|
|
|
la k0, rt_interrupt_to_thread
|
|
lw k1, 0(k0)
|
|
nop
|
|
lw sp, 0(k1) /* get new task's stack pointer */
|
|
|
|
RESTORE_ALL_AND_RET
|
|
|
|
.end CoreSW0Handler
|