git.gag.com Git - fw/openalt/blob - swi/swidispatch.s

   1 @
   2 @  This code expects to find a function named 'swiDispatchC' (which should be
   3 @  in swi.c) to dispatch any functions that aren't written in assembly.
   4 @  (What nut-case picked '@' as the comment character for .s files?)
   5 @
   6         .text
   7         .extern swiDispatchC
   8         .global swiDispatch
   9         .func   swiDispatch
  10
  11 @
  12 @  Equates for GPIO0 registers
  13 @
  14         .set    GPIO0_IOPIN, 0xe0028000
  15         .set    GPIO0_IOSET, 0xe0028004
  16         .set    GPIO0_IODIR, 0xe0028008
  17         .set    GPIO0_IOCLR, 0xe002800c
  18         .set    GPIO_IO_P11, 0x00000800
  19
  20 @
  21 @  No check for Thumb mode.  The LPC2148 demo code doesn't do Thumb
  22 @
  23 swiDispatch:
  24         stmfd   sp!, {r4, lr}           @ Save r4, lr
  25         ldr     r4, [lr, #-4]           @ Fetch the SWI instruction that invoked us
  26         bic     r4, r4, #0xff000000     @ Clear top 8 bits leaving swi "comment field"=number
  27         cmp     r4, #SWI_LAST           @ <= SWI_LAST, handled in assembly, otherwise, in C handler
  28         ldrls   pc, [pc, r4, lsl #2]    @ If <= SWI_LAST, we're handling it in assembly
  29         b       swiDispatchToC          @ Not handled in here, switch to the C handler
  30
  31 @
  32 @  Dispatch table for functions in assembly.  The swiEntriesStart label isn't
  33 @  referenced directly by the code.  Rather, it must directly follow the
  34 @  swiDispatch code, because it is relatively referenced.
  35 @
  36 swiEntriesStart:
  37         .word   swiPYP                  @ 0 - FreeRTOS Yield Process
  38         .word   LED2Set                 @ 1 - Set LED2 state, return previous state
  39         .word   LED2On                  @ 2 - Turn LED2 on, return previous state
  40         .word   LED2Off                 @ 3 - Turn LED2 off, return previous state
  41         .word   LED2Toggle              @ 4 - Toggle LED2, return previous state
  42 swiEntriesEnd:
  43         .set    SWI_LAST, ((swiEntriesEnd-swiEntriesStart)/4)-1
  44
  45 @
  46 @  FreeRTOS uses SWI to yield process, but it's greedy and wants SWI all to its
  47 @  self.  So... Special handling it is!
  48 @
  49 swiPYP:
  50         ldmfd   sp!, {r4, lr}           @ Recover r4, lr
  51         b       vPortYieldProcessor     @ Yield... yield like the wind!
  52
  53 @
  54 @  Values are returned in r0.  LED2 is hardwired in this code (bad programmer!)  Also
  55 @  remember that LED2 (and LED1) have their anodes tied to +3.3V, so we have to drive
  56 @  the port pin LOW to turn them on.  When the caller wants to turn an LED off, it
  57 @  passes in a non-zero value (keeping the same software logic as the hardware logic)
  58 @
  59 LED2Set:
  60         stmfd   sp!, {r1}               @ Save r1
  61         cmp     r0, #0                  @ Turn LED2 off?
  62         ldreq   r4, =GPIO0_IOCLR        @ Get GPIO0_IOCLR address (0 == on)
  63         ldrne   r4, =GPIO0_IOSET        @ Get GPIO0_IOSET address (!0 == off)
  64         ldr     r0, =GPIO0_IOPIN        @ Load GPIO0_IOPIN address
  65         ldr     r0, [r0]                @ Get GPIO0_IOPIN contents
  66         ldr     r1, =GPIO_IO_P11        @ Constant for port pin LED2 attached to
  67         and     r0, r0, r1              @ R0 has bit that reflects LED2 current state
  68         str     r1, [r4]                @ Set LED2 on
  69         ldmfd   sp!, {r1}               @ Recover r1
  70         b       swiExit                 @ Exit SWI handler
  71
  72 LED2On:
  73         stmfd   sp!, {r1}               @ Save r1
  74         ldr     r0, =GPIO0_IOPIN        @ Load GPIO0_IOPIN address
  75         ldr     r0, [r0]                @ Get GPIO0_IOPIN contents
  76         ldr     r1, =GPIO_IO_P11        @ Constant for port pin LED2 attached to
  77         and     r0, r0, r1              @ R0 has bit that reflects LED2 current state
  78         ldr     r4, =GPIO0_IOCLR        @ Get GPIO0_IOCLR address
  79         str     r1, [r4]                @ Set LED2 on
  80         ldmfd   sp!, {r1}               @ Recover r1
  81         b       swiExit                 @ Exit SWI handler
  82
  83 LED2Off:
  84         stmfd   sp!, {r1}               @ Save r1
  85         ldr     r0, =GPIO0_IOPIN        @ Load GPIO0_IOPIN address
  86         ldr     r0, [r0]                @ Get GPIO0_IOPIN contents
  87         ldr     r1, =GPIO_IO_P11        @ Constant for port pin LED2 attached to
  88         and     r0, r0, r1              @ R0 has bit that reflects LED2 current state
  89         ldr     r4, =GPIO0_IOSET        @ Get GPIO0_IOSET address
  90         str     r1, [r4]                @ Set LED2 off
  91         ldmfd   sp!, {r1}               @ Recover r1
  92         b       swiExit                 @ Exit SWI handler
  93
  94 LED2Toggle:
  95         stmfd   sp!, {r1}               @ Save r1
  96         ldr     r0, =GPIO0_IOPIN        @ Load GPIO0_IOPIN address
  97         ldr     r0, [r0]                @ Get GPIO0_IOPIN contents
  98         ldr     r1, =GPIO_IO_P11        @ Constant for port pin LED2 attached to
  99         ands    r0, r0, r1              @ R0 has bit that reflects LED2 current state
 100         ldrne   r4, =GPIO0_IOCLR        @ Get GPIO0_IOCLR address (!0 == off, so turn on)
 101         ldreq   r4, =GPIO0_IOSET        @ Get GPIO0_IOSET address (0 == on, so turn off)
 102         str     r1, [r4]                @ Set LED2 off
 103         ldmfd   sp!, {r1}               @ Recover r1
 104         b       swiExit                 @ Exit SWI handler
 105
 106 @
 107 @  Not an internal function, try the C dispatch handler.  r0, r1, r2 will be
 108 @  passed through as set.  r3 will contain the SWI number.  Return value will
 109 @  be in r0.
 110 @
 111 swiDispatchToC:
 112         stmfd   sp!,{r1-r12, lr}        @ Save r1..r12, lr on stack
 113         mrs     r12, spsr               @ Saved Process Status Register (SPSR) to r12
 114         stmfd   sp!, {r12}              @ Save r12
 115         ldr     r12, =swiDispatchC      @ Get the address of the C handler function
 116         mov     r3, r4                  @ Pass SWI number as parameter in r3
 117         mov     lr, pc                  @ Save current PC to lr
 118         bx      r12                     @ Branch to r12 (C handler function), return to next instruction
 119         ldmfd   sp!, {r12}              @ Recover old SPSR to r12
 120         msr     spsr_cxsf, r12          @ Set SPSR from r12
 121         ldmfd   sp!, {r1-r12, pc}^      @ Recover r1..r12, return value in r0
 122         b       swiExit                 @ And exit
 123
 124 @
 125 @  Exit SWI handler.  Recover r4, return to caller
 126 @
 127 swiExit:
 128         ldmfd   sp!, {r4, pc}^          @ Recover r4, return to caller
 129
 130         .endfunc
 131         .end