kthacker | 6de6775 | 2006-04-17 15:02:26 +0000 | [diff] [blame^] | 1 | ;--------------------------------------------------------------------- |
| 2 | .importzp ptr1 |
| 3 | .importzp sp |
| 4 | .import __ZP_START__ |
| 5 | |
| 6 | |
| 7 | .import _mtarch_asm_threadstack |
| 8 | .import _mtarch_asm_threadsp |
| 9 | .import _mtarch_asm_threadspreg |
| 10 | .import _mtarch_asm_threadzp |
| 11 | |
| 12 | .export _mtarch_asm_exec |
| 13 | .export _mtarch_yield |
| 14 | .export _mtarch_asm_start |
| 15 | .export _mtarch_pstart, _mtarch_pstop |
| 16 | |
| 17 | ;--------------------------------------------------------------------- |
| 18 | .bss |
| 19 | kernelsp: .res 2 |
| 20 | kernelspreg: .res 1 |
| 21 | |
| 22 | oldirq: .res 2 |
| 23 | |
| 24 | zpsize = 32 |
| 25 | .code |
| 26 | ;--------------------------------------------------------------------- |
| 27 | ;; Switch to thread defined by threadsp, threadstack and threadspreg. |
| 28 | ;; The kernel stack is swapped onto the threadstack, and the |
| 29 | ;; sp and spreg are saved to the local variables "kernelsp" and |
| 30 | ;; "kernelspreg". Also, the zeropage variables are saved. |
| 31 | |
| 32 | _mtarch_asm_exec: |
| 33 | sei |
| 34 | ;; Save current stack pointer |
| 35 | lda sp |
| 36 | sta kernelsp |
| 37 | lda sp+1 |
| 38 | sta kernelsp+1 |
| 39 | |
| 40 | tsx |
| 41 | stx kernelspreg |
| 42 | |
| 43 | lda _mtarch_asm_threadzp |
| 44 | sta ptr1 |
| 45 | lda _mtarch_asm_threadzp+1 |
| 46 | sta ptr1+1 |
| 47 | |
| 48 | ldy #0 |
| 49 | : |
| 50 | lda <__ZP_START__,y |
| 51 | tax |
| 52 | lda (ptr1),y |
| 53 | sta <__ZP_START__,y |
| 54 | txa |
| 55 | sta (ptr1),y |
| 56 | iny |
| 57 | cpy #zpsize |
| 58 | bne :- |
| 59 | |
| 60 | lda _mtarch_asm_threadstack |
| 61 | sta ptr1 |
| 62 | lda _mtarch_asm_threadstack+1 |
| 63 | sta ptr1+1 |
| 64 | |
| 65 | ldy kernelspreg ; determine the smallest of the two stack pointers, |
| 66 | cpy _mtarch_asm_threadspreg ; as we only need to swap the used part of the stack |
| 67 | bcc :+ |
| 68 | ldy _mtarch_asm_threadspreg |
| 69 | |
| 70 | : |
| 71 | lda $0100,y |
| 72 | tax |
| 73 | lda (ptr1),y |
| 74 | sta $0100,y |
| 75 | txa |
| 76 | sta (ptr1),y |
| 77 | iny |
| 78 | bne :- |
| 79 | |
| 80 | lda _mtarch_asm_threadsp |
| 81 | sta sp |
| 82 | lda _mtarch_asm_threadsp+1 |
| 83 | sta sp+1 |
| 84 | |
| 85 | ldx _mtarch_asm_threadspreg |
| 86 | txs |
| 87 | |
| 88 | ; jsr _mtarch_pstart |
| 89 | |
| 90 | lda $0314 |
| 91 | sta oldirq |
| 92 | lda $0315 |
| 93 | sta oldirq+1 |
| 94 | |
| 95 | lda #<irq |
| 96 | sta $0314 |
| 97 | lda #>irq |
| 98 | sta $0315 |
| 99 | |
| 100 | |
| 101 | pla |
| 102 | tay |
| 103 | pla |
| 104 | tax |
| 105 | pla |
| 106 | |
| 107 | rti |
| 108 | |
| 109 | ;; Switch from thread defined by threadsp, threadstack and threadspreg. |
| 110 | ;; The kernel stack is swapped back from the threadstack, and the |
| 111 | ;; sp and spreg are restored from the local variables "kernelsp" and |
| 112 | ;; "kernelspreg". |
| 113 | yield: |
| 114 | sei |
| 115 | lda sp |
| 116 | sta _mtarch_asm_threadsp |
| 117 | lda sp+1 |
| 118 | sta _mtarch_asm_threadsp+1 |
| 119 | |
| 120 | tsx |
| 121 | stx _mtarch_asm_threadspreg |
| 122 | |
| 123 | lda _mtarch_asm_threadzp |
| 124 | sta ptr1 |
| 125 | lda _mtarch_asm_threadzp+1 |
| 126 | sta ptr1+1 |
| 127 | |
| 128 | ldy kernelspreg ; determine the smallest of the two stack pointers, |
| 129 | cpy _mtarch_asm_threadspreg ; as we only need to swap the used part of the stack |
| 130 | bcc :+ |
| 131 | ldy _mtarch_asm_threadspreg |
| 132 | |
| 133 | : |
| 134 | lda <__ZP_START__,y |
| 135 | tax |
| 136 | lda (ptr1),y |
| 137 | sta <__ZP_START__,y |
| 138 | txa |
| 139 | sta (ptr1),y |
| 140 | iny |
| 141 | cpy #zpsize |
| 142 | bne :- |
| 143 | |
| 144 | lda _mtarch_asm_threadstack |
| 145 | sta ptr1 |
| 146 | lda _mtarch_asm_threadstack+1 |
| 147 | sta ptr1+1 |
| 148 | |
| 149 | ldy #0 |
| 150 | : |
| 151 | lda $0100,y |
| 152 | tax |
| 153 | lda (ptr1),y |
| 154 | sta $0100,y |
| 155 | txa |
| 156 | sta (ptr1),y |
| 157 | iny |
| 158 | bne :- |
| 159 | |
| 160 | lda kernelsp |
| 161 | sta sp |
| 162 | lda kernelsp+1 |
| 163 | sta sp+1 |
| 164 | |
| 165 | ldx kernelspreg |
| 166 | txs |
| 167 | |
| 168 | cli |
| 169 | |
| 170 | rts |
| 171 | ;--------------------------------------------------------------------- |
| 172 | ;; Simulate an IRQ by pushing CPU status and CPu registers |
| 173 | ;; onto the stack. Then call the yield function to yield the |
| 174 | ;; process. |
| 175 | _mtarch_yield: |
| 176 | php |
| 177 | pha |
| 178 | txa |
| 179 | pha |
| 180 | tya |
| 181 | pha |
| 182 | |
| 183 | tsx |
| 184 | |
| 185 | ; the rts adds 1 to the PC |
| 186 | ; saved on the stack. We want |
| 187 | lda $0105,x ; the stack to look like is would |
| 188 | clc ; do inside of an interrupt. |
| 189 | adc #1 ; (this is what the 'rts' does, |
| 190 | sta $0105,x ; but not the 'rti') |
| 191 | lda $0106,x |
| 192 | adc #0 |
| 193 | sta $0106,x |
| 194 | |
| 195 | |
| 196 | jmp yield |
| 197 | ;--------------------------------------------------------------------- |
| 198 | _mtarch_asm_start: |
| 199 | lda _mtarch_asm_threadzp |
| 200 | sta ptr1 |
| 201 | lda _mtarch_asm_threadzp+1 |
| 202 | sta ptr1+1 |
| 203 | |
| 204 | ldy #0 |
| 205 | : |
| 206 | lda <__ZP_START__,y |
| 207 | sta (ptr1),y |
| 208 | iny |
| 209 | cpy #zpsize |
| 210 | bne :- |
| 211 | rts |
| 212 | |
| 213 | ;--------------------------------------------------------------------- |
| 214 | irq: |
| 215 | lda oldirq |
| 216 | sta $0314 |
| 217 | lda oldirq+1 |
| 218 | sta $0315 |
| 219 | jmp yield |
| 220 | ;--------------------------------------------------------------------- |
| 221 | ;; Setup preemption IRQ |
| 222 | _mtarch_pstart: |
| 223 | sei |
| 224 | lda $0314 |
| 225 | sta oldirq |
| 226 | lda $0315 |
| 227 | sta oldirq+1 |
| 228 | |
| 229 | lda #<irq |
| 230 | sta $0314 |
| 231 | lda #>irq |
| 232 | sta $0315 |
| 233 | cli |
| 234 | rts |
| 235 | ;--------------------------------------------------------------------- |
| 236 | _mtarch_pstop: |
| 237 | sei |
| 238 | lda oldirq |
| 239 | sta $0314 |
| 240 | lda oldirq+1 |
| 241 | sta $0315 |
| 242 | cli |
| 243 | rts |
| 244 | ;--------------------------------------------------------------------- |
| 245 | |