I changed the previous cooperative threads into preemptive threads.
It changes contexts during timer interrupts every 16ms with non-inteligent round robin scheduling.
I noticed that ARM swaps R13/R14 during IRQ, and needed to go back and force between IRQ and SVC mode to get the original (banked) registers as below. When IRQ_hander C function returns non null, it’ll switch contexts.
_IRQ_iterrupt:
//-- irq mode
sub lr, lr, #4 // in IRQ mode, r14_irq(lr_irq) points to PC+#4 in user mode
// save context
push {r0-r12, lr} // save user mode registers
mrs r0, spsr // spsr -> r0
cps #0x13
//-- svc mode
mov r1, sp
mov r2, lr
cps #0x12
//-- irq mode
push {r0-r2} // save spsr, user mode sp, lr
// call IRQ_hander(user-mode-sp)
mov r0, r2
bl IRQ_handler
cmp r0, #0
bne _IRQ_interrupt_context_switch
pop {r0-r2} // restore spsr, user mode sp, lr
msr spsr, r0 // r0 -> spsr
cps #0x13
//-- svc mode
mov sp, r1 // restore sp
mov lr, r2 // restore lr
cps #0x12
//-- irq mode
pop {r0-r12,lr}
movs pc, lr
_IRQ_interrupt_context_switch:
//-- irq mode
// r0 is next thread's SP
pop {r1-r3} // restore spsr, user mode sp, lr
// save registers in user mode stack
// r1: user mode cpsr
// r2: user mode sp
// r3: user mode lr
sub r2, r2, #4
str r1, [r2] // spsr
ldr r4, [r13, #4*13]
sub r2, r2, #4
str r4, [r2] // user mode pc (r14_irq)
sub r2, r2, #4
str r3, [r2] // user mode lr
ldr r4, [r13, #4*12]
sub r2, r2, #4
str r4, [r2] // user mode r12
ldr r4, [r13, #4*11]
sub r2, r2, #4
str r4, [r2] // user mode r11
ldr r4, [r13, #4*10]
sub r2, r2, #4
str r4, [r2] // user mode r10
ldr r4, [r13, #4*9]
sub r2, r2, #4
str r4, [r2] // user mode r9
ldr r4, [r13, #4*8]
sub r2, r2, #4
str r4, [r2] // user mode r8
ldr r4, [r13, #4*7]
sub r2, r2, #4
str r4, [r2] // user mode r7
ldr r4, [r13, #4*6]
sub r2, r2, #4
str r4, [r2] // user mode r6
ldr r4, [r13, #4*5]
sub r2, r2, #4
str r4, [r2] // user mode r5
ldr r4, [r13, #4*4]
sub r2, r2, #4
str r4, [r2] // user mode r4
ldr r4, [r13, #4*3]
sub r2, r2, #4
str r4, [r2] // user mode r3
ldr r4, [r13, #4*2]
sub r2, r2, #4
str r4, [r2] // user mode r2
ldr r4, [r13, #4*1]
sub r2, r2, #4
str r4, [r2] // user mode r1
ldr r4, [r13]
sub r2, r2, #4
str r4, [r2] // user mode r0
mov r4, sp // r4 <- r13_irq
msr spsr, r1 // r1 -> spsr
cps #0x13 //@ svc mode
//-- svc mode
// change user mode stack to next thread's stack
mov sp, r0
// push into r13_irq (r4)
ldr r2, [sp, #4*14] // user mode pc
sub r4, r4, #4
str r2, [r4]
ldr r2, [sp, #4*15] // spsr
sub r4, r4, #4
str r2, [r4]
// restore registers
pop {r0-r12,lr}
add sp, sp, #4*2 // pop pc, spsr
cps #0x12 //@ irq mode
//-- irq mode
sub sp, sp, #4*2
pop {lr}
// enable irq and restore spsr
bic lr, lr, #0x80
msr spsr, lr // lr -> spsr
// restore lr (user mode pc)
pop {lr}
// discard pushed registers
add r13, r13, #4*14
// movs pc, * ... mov's' pc restores status register
movs pc, lr
I also tried to implement and add critical section but the one in ARM reference’s hanged probably because I haven’t setup MMU.
// http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/ch01s03s02.html
.equ locked, 1
.equ unlocked, 0
// BUG: lock_mutex hangs
// LDREX doesn't work when MMU is disabled. Don't use this.
// Declare for use from C as extern void lock_mutex(void * mutex);
.global _lock_mutex_mmu
_lock_mutex_mmu:
LDR r1, =locked
1:
LDREX r2, [r0]
CMP r2, r1 // Test if mutex is locked or unlocked
BEQ 2f
STREXNE r2, r1, [r0] // Not locked, attempt to lock it
CMPNE r2, #1 // Check if Store-Exclusive failed
BEQ 1b // Failed - retry from 1
// Lock acquired
DMB // Required before accessing protected resource
BX lr
2:
// Take appropriate action while waiting for mutex to become unlocked
//wfi
nop
B 1b // Retry from 1
// BUG: unlock_mutex
// Declare for use from C as extern void unlock_mutex(void * mutex);
.global _unlock_mutex_mmu
_unlock_mutex_mmu:
LDR r1, =unlocked
DMB // Required before releasing protected resource
STR r1, [r0] // Unlock mutex
// SIGNAL_UPDATE: none
BX lr
// BUG: SWP version
// http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dht0008a/CJHBGBBJ.html
// still hangs
.global _lock_mutex_swp
_lock_mutex_swp:
LDR r2, =locked
SWP r1, r2, [r0] // Swap R2 with location [R0], [R0] value placed in R1
CMP r1, r2 // Check if memory value was ‘locked’
BEQ _lock_mutex_swp // If so, retry immediately
BX lr // If not, lock successful, return
// BUG: not really excusive when context swithes after ldr befor str
.global _lock_mutex_simple
_lock_mutex_simple:
ldr r1, =unlocked
ldr r3, =locked
ldr r2, [r0]
cmp r2, r3
beq _lock_mutex_simple
str r1, [r0]
bx lr
.global _unlock_mutex_simple
_unlock_mutex_simple:
LDR r1, =unlocked
STR r1, [r0] // Write value ‘unlocked’ to location [R0]
BX lr
Today’s code -> https://github.com/sokoide/rpi-baremetal -> 009_context_switch2
