Debugging ARM Assembly Context Switch - c

Question

I'm working on a context switch in ARM v6 assembly. I posted about writing the switch in C, but assembly seems to be safer and more reliable. I've spent a while checking all the offsets and being careful not to delete data from registers, but the context switch just doesn't seem to work properly. I have set up and tested timer interrupts without switching context.
Here's my code:
interrupt_asm:
//store basic interrupt stuff
sub lr, lr, #4
//call the interrupt vector
push { r0-r12 }
mov r0, lr # Pass old pc
bl interrupt_vector # C function
pop { r0-r12 }
# save_current_thread:
//remember r1 so you can use it for r0
push {r1}
mov r1, r0 //store r0 so it can be restored
push {r2, r3}
bl get_current_thread //r0 now has the address of CURRENT_THREAD
pop {r2, r3}
add r0, r0, #4 // r0 = &CURRENT_THREAD.r0
str r1, [r0] // save what the r0 was
pop {r1} // restore r1
add r0, r0, #4 // r0 = &CURRENT_THREAD.r1
str r1, [r0] // save r1
//r2
add r0, r0, #4
str r2, [r0]
//r3
add r0, r0, #4
str r3, [r0]
//r4
add r0, r0, #4
str r4, [r0]
//r5
add r0, r0, #4
str r5, [r0]
//r6
add r0, r0, #4
str r6, [r0]
//r7
add r0, r0, #4
str r7, [r0]
//r8
add r0, r0, #4
str r8, [r0]
//r9
add r0, r0, #4
str r9, [r0]
//r10
add r0, r0, #4
str r10, [r0]
//r11
add r0, r0, #4
str r11, [r0]
//r12
add r0, r0, #4
str r12, [r0]
//store SVC sp, lr, and pc
mrs r1, cpsr
bic r1, r1, #0x1F
orr r1, r1, #0x13
msr cpsr_c, r1
//sp
add r0, r0, #4
str sp, [r0]
//lr
add r0, r0, #4
str lr, [r0]
//back to IRQ land
mrs r1, cpsr
bic r1, r1, #0x1F
orr r1, r1, #0x12
msr cpsr_c, r1
//pc THIS NEEDS TO BE LR
add r0, r0, #4
str lr, [r0]
//cpsr
add r0, r0, #4
mrs r1, cpsr
str r1, [r0]
//spsr
add r0, r0, #4
mrs r1, spsr
str r1, [r0]
push {r2, r3}
bl increment_thread //r0 now has the address of our next thread
pop {r2, r3}
# restore_thread:
//were pushing in order, so from the bottom up our stack is: r0, ... r12, sp, lr, pc, spsr
add r0, r0, #4 //r0 = &CURRENT_THREAD.r0
ldr r1, [r0] //r1 = thread.r0
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r1
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r2
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r3
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r4
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r5
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r6
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r7
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r8
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r9
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r10
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r11
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.r12
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.sp
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.lr
push {r1}
add r0, r0, #4
ldr r1, [r0] //r1 = thread.pc
push {r1}
add r0, r0, #8 //skip cpsr - agreed
ldr r1, [r0] //r1 = thread.spsr
push {r1}
//Our stack now looks like spsr, pc, lr, sp, r12, ... r0 (in order of popping)
pop {r1} //this was the spsr - SPSR
pop {lr} //this was the pc - PC (from thread)
pop {r2} //this was the lr - LR (from thread)
pop {r3} //this was the sp - SP (from thread)
//switch to SVC
mrs r0, cpsr
bic r0, r0, #0x1F
orr r0, r0, #0x13
msr cpsr_c, r0
msr spsr, r1 //restore spsr
mov lr, r2 //restore lr to be old lr
mov sp, r3 //restore sp
//switch to IRQ
// mrs r0, cpsr
// bic r0, r0, #0x1F
// orr r0, r0, #0x12
// msr cpsr_c, r0
cps #0x12
//our stack now just has the normal registers in it. Restore them
pop {r12}
pop {r11}
pop {r10}
pop {r9}
pop {r8}
pop {r7}
pop {r6}
pop {r5}
pop {r4}
pop {r3}
pop {r2}
pop {r2}
pop {r1}
pop {r0}
push {lr}
ldm sp!, {pc}^
A thread looks like this:
typedef struct __attribute__((packed, aligned(8))) {
void (*run)();
unsigned r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, sp, lr, pc;
unsigned cpsr, spsr;
unsigned id;
unsigned priority;
thread_status status;
wait_event wait_status;
} thread_t;
Do you have any advice about what's going on? An interrupt occurs and it never goes back to a new thread. I've debugged with GDB simulator but can't seem to nail down the issue.
The C function interrupt_vector just does this:
void interrupt_vector(unsigned pc) {
CURRENT_THREAD.pc = pc;
printf(" interrupt vector (pc = 0x%08x | thread.r0 = 0x%08x)\n", pc, CURRENT_THREAD.r0);
armtimer_clear_interrupt();
}
My other C functions are literally one line, and I've looked at their disassembly:
void increment_thread(){
// Not trying to actually increment yet
__asm__ volatile("mov r0, %0" : : "r" ((unsigned) &CURRENT_THREAD));
}
void get_current_thread(){
__asm__ volatile("mov r0, %0" : : "r" ((unsigned) &CURRENT_THREAD));
}