Context switch on an interrupt vector

interrupts.c

void __attribute__((interrupt("IRQ"))) _c_interrupt_vector(void)
{
    static int lit = 0;

    /* Clear the ARM Timer interrupt - it's the only interrupt we have
       enabled, so we want don't have to work out which interrupt source
       caused us to interrupt */
    RPI_GetArmTimer()->IRQClear = 1;

    /* Flip the LED */
    if( lit )
    {
        LED_OFF();
        lit = 0;
    }
    else
    {
        LED_ON();
        lit = 1;
    }
}

start.S

// To keep this in the first portion of the binary.
.section ".text.boot"

// External values
.extern __irq_stack_top
.extern __fiq_stack_top
.extern __c_stack_top

// Make globals.
.global _start
.global _get_stack_pointer
.global _exception_table
.global _enable_interrupts

// From the ARM ARM (Architecture Reference Manual). Make sure you get the
// ARMv5 documentation which includes the ARMv6 documentation which is the
// correct processor type for the Broadcom BCM2835. The ARMv6-M manuals
// available on the ARM website are for Cortex-M parts only and are very
// different.
//
// See ARM section A2.2 (Processor Modes)
.equ    CPSR_MODE_USER,         0x10
.equ    CPSR_MODE_FIQ,          0x11
.equ    CPSR_MODE_IRQ,          0x12
.equ    CPSR_MODE_SVR,          0x13
.equ    CPSR_MODE_ABORT,        0x17
.equ    CPSR_MODE_UNDEFINED,    0x1B
.equ    CPSR_MODE_SYSTEM,       0x1F

// See ARM section A2.5 (Program status registers)
.equ    CPSR_IRQ_INHIBIT,       0x80
.equ    CPSR_FIQ_INHIBIT,       0x40
.equ    CPSR_THUMB,             0x20

_start:
  ldr pc, _reset_h
  ldr pc, _undefined_instruction_vector_h
  ldr pc, _software_interrupt_vector_h
  ldr pc, _prefetch_abort_vector_h
  ldr pc, _data_abort_vector_h
  ldr pc, _unused_handler_h
  ldr pc, _interrupt_vector_h
  ldr pc, _fast_interrupt_vector_h

_reset_h:                           .word   _reset_
_undefined_instruction_vector_h:    .word   _c_undefined_instruction_vector
_software_interrupt_vector_h:       .word   _c_software_interrupt_vector
_prefetch_abort_vector_h:           .word   _c_prefetch_abort_vector
_data_abort_vector_h:               .word   _c_data_abort_vector
_unused_handler_h:                  .word   _reset_
_interrupt_vector_h:                .word   _interrupt_vector_
_fast_interrupt_vector_h:           .word   _c_fast_interrupt_vector

_reset_:
  // We enter execution in supervisor mode. For more information on
  // processor modes see ARM Section A2.2 (Processor Modes)

  mov     r0, #0x8000
  mov     r1, #0x0000
  ldmia   r0!,{r2, r3, r4, r5, r6, r7, r8, r9}
  stmia   r1!,{r2, r3, r4, r5, r6, r7, r8, r9}
  ldmia   r0!,{r2, r3, r4, r5, r6, r7, r8, r9}
  stmia   r1!,{r2, r3, r4, r5, r6, r7, r8, r9}

  // IRQ Stack Pointer
  mov     r0, #(CPSR_MODE_IRQ | CPSR_IRQ_INHIBIT | CPSR_FIQ_INHIBIT )
  msr     cpsr_c, r0
  ldr     sp, =__irq_stack_top

  // FRQ Stack Pointer
  mov     r0, #(CPSR_MODE_FIQ | CPSR_IRQ_INHIBIT | CPSR_FIQ_INHIBIT )
  msr     cpsr_c, r0
  ldr     sp, =__fiq_stack_top

  // Service Mode Stack Pointer (C program)
  mov     r0, #(CPSR_MODE_SVR | CPSR_IRQ_INHIBIT | CPSR_FIQ_INHIBIT )
  msr     cpsr_c, r0
  ldr     sp, =__c_stack_top

  // Startup C
  bl      _c_startup

// If main does return for some reason, just catch it and stay here.
_inf_loop:
  b       _inf_loop

_get_stack_pointer:
  // Return the stack pointer value
  str     sp, [sp]
  ldr     r0, [sp]

  // Return from the function
  mov     pc, lr


_enable_interrupts:
  mrs     r0, cpsr
  mov     r1, #CPSR_IRQ_INHIBIT
  bic     r0, r0, r1
  msr     cpsr_c, r0

  // Return from the function
  mov     pc, lr

_interrupt_vector_:
  // Save current process context in process stack
  msr     cpsr_c, #(CPSR_MODE_SVR | CPSR_IRQ_INHIBIT)
  stmfd   sp!, {r0 - r3, ip, lr}

  // Save lr_irq and spsr_irq in process stack
  msr     cpsr_c, #(CPSR_MODE_SVR | CPSR_IRQ_INHIBIT)
  sub     lr, lr, #4
  mov     r1, lr
  mrs     r2, spsr
  msr     cpsr_c, #(CPSR_MODE_SVR | CPSR_IRQ_INHIBIT)
  stmfd   sp!, {r1, r2}

  // Dispatch the interrupt to platform
  bl      _c_interrupt_vector

  // Restore lr_irq and spsr_irq from process stack
  ldmfd   sp!, {r1, r2}
  msr     cpsr_c, #(CPSR_MODE_IRQ | CPSR_IRQ_INHIBIT)
  stmfd   sp!, {r1}
  msr     spsr_cxsf, r2

  // Restore process regs
  msr     cpsr_c, #(CPSR_MODE_IRQ | CPSR_IRQ_INHIBIT)
  ldmfd   sp!, {r0 - r3, ip, lr}

  // Exit from IRQ
  msr     cpsr_c, #(CPSR_MODE_IRQ | CPSR_IRQ_INHIBIT)
  ldmfd   sp!, {pc}^