libdi/stubasm.S

   1 #include <ogc/machine/asm.h>
   2 #include "stubasm.h"
   3
   4 #define         CTX_SAVE        0x80004004
   5 #define         RET_ADDR        0x80004000
   6
   7 #define         CACHE_SCRATCH   0x81000000
   8
   9         .extern __InitPS
  10         .extern __InitCache
  11         .extern __InitSystem
  12         .extern DCDisable
  13         .extern ICDisable
  14         .extern L2Disable
  15         .extern DCFlashInvalidate
  16         .extern ICFlashInvalidate
  17         .extern L2GlobalInvalidate
  18         .extern DCFlushRangeNoSync
  19         .extern ICInvalidateRange
  20
  21         .globl __distub_take_plunge
  22         .globl __distub_enable_caches
  23         .globl __distub_disable_caches
  24
  25         .text
  26
  27 __distub_take_plunge:
  28         # save LR + r31
  29         mflr    r0
  30         stw     r0, 4(sp)
  31         stwu    sp, -16(sp)
  32         stw     r31, 12(sp)
  33
  34         # save context address
  35         lis     r4, CTX_SAVE@h
  36         stw     r3, CTX_SAVE@l(r4)
  37
  38         # save MSR
  39         mfmsr   r4
  40         stw     r4, MSR_OFF(r3)
  41
  42         # disable interrupts
  43         rlwinm  r4,r4,0,17,15
  44         mtmsr   r4
  45
  46         # save GPRs
  47         stw     r1, R1_OFF(r3)
  48         stw     r2, R2_OFF(r3)
  49         stw     r13, R13_OFF(r3)
  50         stw     r14, R14_OFF(r3)
  51         stw     r15, R15_OFF(r3)
  52         stw     r16, R16_OFF(r3)
  53         stw     r17, R17_OFF(r3)
  54         stw     r18, R18_OFF(r3)
  55         stw     r19, R19_OFF(r3)
  56         stw     r20, R20_OFF(r3)
  57         stw     r21, R21_OFF(r3)
  58         stw     r22, R22_OFF(r3)
  59         stw     r23, R23_OFF(r3)
  60         stw     r24, R24_OFF(r3)
  61         stw     r25, R25_OFF(r3)
  62         stw     r26, R26_OFF(r3)
  63         stw     r27, R27_OFF(r3)
  64         stw     r28, R28_OFF(r3)
  65         stw     r29, R29_OFF(r3)
  66         stw     r30, R30_OFF(r3)
  67         stw     r31, R31_OFF(r3)
  68
  69         # save SPRGx
  70         mfspr   r4, SPRG0
  71         stw     r4, SPRG0_OFF(r3)
  72         mfspr   r4, SPRG1
  73         stw     r4, SPRG1_OFF(r3)
  74         mfspr   r4, SPRG2
  75         stw     r4, SPRG2_OFF(r3)
  76         mfspr   r4, SPRG3
  77         stw     r4, SPRG3_OFF(r3)
  78
  79         # save HIDx
  80         mfspr   r4, HID0
  81         stw     r4, HID0_OFF(r3)
  82         mfspr   r4, HID1
  83         stw     r4, HID1_OFF(r3)
  84         mfspr   r4, HID2
  85         stw     r4, HID2_OFF(r3)
  86         mfspr   r4, HID4
  87         stw     r4, HID4_OFF(r3)
  88
  89         # save WPAR
  90         mfspr   r4, WPAR
  91         stw     r4, WPAR_OFF(r3)
  92
  93         # save low MEM1
  94         li      r5, MEM1_WORDS
  95         mtctr   r5
  96
  97         addi    r6, r3, MEM1_OFF - 4
  98         lis     r5,0x8000
  99         addi    r5, r5, -4
 100 1:
 101         lwzu    r7, 4(r5)
 102         stwu    r7, 4(r6)
 103         bdnz    1b
 104
 105         # flush and disable caches
 106         bl      __distub_disable_caches
 107
 108         # set up return address for stub
 109         lis     r5, __distub_return@h
 110         ori     r5, r5, __distub_return@l
 111         lis     r4, RET_ADDR@h
 112         stw     r5, RET_ADDR@l(r4)
 113
 114         sync
 115
 116         # take the plunge
 117 __distub_wait:
 118         b       __distub_wait
 119
 120 __distub_return:
 121         # get the context address
 122         lis     r4, CTX_SAVE@h
 123         lwz     r3, CTX_SAVE@l(r4)
 124
 125         # restore GPRs
 126         lwz     r1, R1_OFF(r3)
 127         lwz     r2, R2_OFF(r3)
 128         lwz     r13, R13_OFF(r3)
 129         lwz     r14, R14_OFF(r3)
 130         lwz     r15, R15_OFF(r3)
 131         lwz     r16, R16_OFF(r3)
 132         lwz     r17, R17_OFF(r3)
 133         lwz     r18, R18_OFF(r3)
 134         lwz     r19, R19_OFF(r3)
 135         lwz     r20, R20_OFF(r3)
 136         lwz     r21, R21_OFF(r3)
 137         lwz     r22, R22_OFF(r3)
 138         lwz     r23, R23_OFF(r3)
 139         lwz     r24, R24_OFF(r3)
 140         lwz     r25, R25_OFF(r3)
 141         lwz     r26, R26_OFF(r3)
 142         lwz     r27, R27_OFF(r3)
 143         lwz     r28, R28_OFF(r3)
 144         lwz     r29, R29_OFF(r3)
 145         lwz     r30, R30_OFF(r3)
 146         lwz     r31, R31_OFF(r3)
 147
 148         # initialize system partially
 149         bl      __InitPS
 150         bl      __InitCache
 151         bl      __InitSystem
 152
 153         # get the context address (again), this time into r31
 154         lis     r4, CTX_SAVE@h
 155         lwz     r31, CTX_SAVE@l(r4)
 156
 157         # save IOS version
 158         lis     r5, 0x8000
 159         lwz     r3, 0x3140(r5)
 160
 161         # restore low MEM1
 162         li      r5, MEM1_WORDS
 163         mtctr   r5
 164
 165         addi    r6, r31, MEM1_OFF - 4
 166         lis     r5,0x8000
 167         addi    r5, r5, -4
 168 1:
 169         lwzu    r7, 4(r6)
 170         stwu    r7, 4(r5)
 171         bdnz    1b
 172
 173         # restore IOS version
 174         lis     r5, 0x8000
 175         stw     r3, 0x3140(r5)
 176
 177         # flush low MEM1
 178         lis     r3, 0x8000
 179         li      r4, (MEM1_WORDS * 4)
 180         bl      DCFlushRangeNoSync
 181         lis     r3, 0x8000
 182         li      r4, (MEM1_WORDS * 4)
 183         bl      ICInvalidateRange
 184         sync
 185
 186         # restore SPRGx
 187         lwz     r4, SPRG0_OFF(r31)
 188         mtspr   SPRG0, r4
 189         lwz     r4, SPRG1_OFF(r31)
 190         mtspr   SPRG1, r4
 191         lwz     r4, SPRG2_OFF(r31)
 192         mtspr   SPRG2, r4
 193         lwz     r4, SPRG3_OFF(r31)
 194         mtspr   SPRG3, r4
 195
 196         # restore HIDx
 197         lwz     r4, HID0_OFF(r31)
 198         mtspr   HID0, r4
 199         lwz     r4, HID1_OFF(r31)
 200         mtspr   HID1, r4
 201         lwz     r4, HID2_OFF(r31)
 202         mtspr   HID2, r4
 203         lwz     r4, HID4_OFF(r31)
 204         mtspr   HID4, r4
 205
 206         # restore WPAR
 207         lwz     r4, WPAR_OFF(r31)
 208         mtspr   WPAR, r4
 209
 210         # restore MSR (and interrupts)
 211         lwz     r4, MSR_OFF(r31)
 212         mtmsr   r4
 213
 214         # we grab the LR and r31 back from the (now configured) stack
 215         lwz     r0, 20(sp)
 216         lwz     r31, 12(sp)
 217         addi    sp, sp, 16
 218         mtlr    r0
 219         blr
 220
 221 __distub_ledon:
 222         # tray on!
 223         lis     r3, 0xcd80
 224         lwz     r4, 0xc0(r3)
 225         ori     r4, r4, 0x20
 226         stw     r4, 0xc0(r3)
 227         blr
 228
 229 __distub_ledoff:
 230         # tray on!
 231         lis     r3, 0xcd80
 232         lwz     r4, 0xc0(r3)
 233         li      r5, 0x20
 234         andc    r4, r4, r5
 235         stw     r4, 0xc0(r3)
 236         blr
 237
 238 __distub_ledblink:
 239         # tray blink!
 240         lis     r3, 0xcd80
 241         lwz     r4, 0xc0(r3)
 242         xori    r4, r4, 0x20
 243         stw     r4, 0xc0(r3)
 244         blr
 245
 246 __distub_disable_caches:
 247         # save LR + r31
 248         mflr    r0
 249         stw     r0, 4(sp)
 250         stwu    sp, -16(sp)
 251         stw     r31, 12(sp)
 252
 253         # disable interrupts
 254         mfmsr   r31
 255         rlwinm  r4,r31,0,17,15
 256         mtmsr   r4
 257
 258         sync
 259
 260         # Flush and disable L1/L2 data caches
 261
 262         # Load cache
 263         lis     r2,0x0001
 264         mtctr   r2
 265         lis     r4,CACHE_SCRATCH@h
 266         ori     r4,r4,CACHE_SCRATCH@l
 267 1:
 268         lwz     r6,0(r4)
 269         addi    r4,r4,0x20      # Move to next block
 270         bdnz    1b              # decrement ctr, branch if necessary
 271         isync
 272
 273         # Flush cache
 274         lis     r2,0x0001
 275         mtctr   r2
 276         lis     r4,CACHE_SCRATCH@h
 277         ori     r4,r4,CACHE_SCRATCH@l
 278         sync
 279 1:
 280         dcbf    r0,r4
 281         addi    r4,r4,0x20      # move to next block
 282         bdnz    1b              # decrement & branch as appropriate
 283
 284         bl      DCDisable
 285         bl      ICDisable
 286         bl      L2Disable
 287         bl      DCFlashInvalidate
 288         bl      ICFlashInvalidate
 289         bl      L2GlobalInvalidate
 290
 291         # restore interrupts
 292         mtmsr   r31
 293
 294         # restore LR + r31
 295         lwz     r0, 20(sp)
 296         lwz     r31, 12(sp)
 297         addi    sp, sp, 16
 298         mtlr    r0
 299         blr
 300
 301 /*
 302 __distub_enable_caches:
 303         # save LR + r31
 304         mflr    r0
 305         stw     r0, 4(sp)
 306         stwu    sp, -16(sp)
 307         stw     r31, 12(sp)
 308
 309         # disable interrupts
 310         mfmsr   r31
 311         rlwinm  r4,r31,0,17,15
 312         mtmsr   r4
 313
 314         # initialize caches
 315         bl      __CacheInit
 316
 317         # reenable interrupts
 318         mtmsr   r31
 319
 320         # restore LR + r31
 321         lwz     r0, 20(sp)
 322         lwz     r31, 12(sp)
 323         addi    sp, sp, 16
 324         mtlr    r0
 325         blr*/
 326