cpu/mpc85xx/start.S

   1 /*
   2  * Copyright 2004 Freescale Semiconductor.
   3  * Copyright (C) 2003  Motorola,Inc.
   4  * Xianghua Xiao<X.Xiao@motorola.com>
   5  *
   6  * See file CREDITS for list of people who contributed to this
   7  * project.
   8  *
   9  * This program is free software; you can redistribute it and/or
  10  * modify it under the terms of the GNU General Public License as
  11  * published by the Free Software Foundation; either version 2 of
  12  * the License, or (at your option) any later version.
  13  *
  14  * This program is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  * GNU General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public License
  20  * along with this program; if not, write to the Free Software
  21  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  22  * MA 02111-1307 USA
  23  */
  24
  25 /* U-Boot Startup Code for Motorola 85xx PowerPC based Embedded Boards
  26  *
  27  * The processor starts at 0xfffffffc and the code is first executed in the
  28  * last 4K page(0xfffff000-0xffffffff) in flash/rom.
  29  *
  30  */
  31
  32 #include <config.h>
  33 #include <mpc85xx.h>
  34 #include <version.h>
  35
  36 #define _LINUX_CONFIG_H 1       /* avoid reading Linux autoconf.h file  */
  37
  38 #include <ppc_asm.tmpl>
  39 #include <ppc_defs.h>
  40
  41 #include <asm/cache.h>
  42 #include <asm/mmu.h>
  43
  44 #ifndef  CONFIG_IDENT_STRING
  45 #define  CONFIG_IDENT_STRING ""
  46 #endif
  47
  48 #undef  MSR_KERNEL
  49 #define MSR_KERNEL ( MSR_ME  )  /* Machine Check */
  50
  51 /*
  52  * Set up GOT: Global Offset Table
  53  *
  54  * Use r14 to access the GOT
  55  */
  56         START_GOT
  57         GOT_ENTRY(_GOT2_TABLE_)
  58         GOT_ENTRY(_FIXUP_TABLE_)
  59
  60         GOT_ENTRY(_start)
  61         GOT_ENTRY(_start_of_vectors)
  62         GOT_ENTRY(_end_of_vectors)
  63         GOT_ENTRY(transfer_to_handler)
  64
  65         GOT_ENTRY(__init_end)
  66         GOT_ENTRY(_end)
  67         GOT_ENTRY(__bss_start)
  68         END_GOT
  69
  70 /*
  71  * e500 Startup -- after reset only the last 4KB of the effective
  72  * address space is mapped in the MMU L2 TLB1 Entry0. The .bootpg
  73  * section is located at THIS LAST page and basically does three
  74  * things: clear some registers, set up exception tables and
  75  * add more TLB entries for 'larger spaces'(e.g. the boot rom) to
  76  * continue the boot procedure.
  77
  78  * Once the boot rom is mapped by TLB entries we can proceed
  79  * with normal startup.
  80  *
  81  */
  82
  83     .section .bootpg,"ax"
  84     .globl _start_e500
  85
  86 _start_e500:
  87         mfspr   r0, PVR
  88         lis     r1, PVR_85xx_REV1@h
  89         ori     r1, r1, PVR_85xx_REV1@l
  90         cmpw    r0, r1
  91         bne     1f
  92
  93         /* Semi-bogus errata fixup for Rev 1 */
  94         li      r0,0x2000
  95         mtspr   977,r0
  96
  97         /*
  98          * Before invalidating MMU L1/L2, read TLB1 Entry 0 and then
  99          * write it back immediately to fixup a Rev 1 bug (Errata CPU4)
 100          * for this initial TLB1 entry 0, otherwise the TLB1 entry 0
 101          * will be invalidated (incorrectly).
 102          */
 103         lis     r2,0x1000
 104         mtspr   MAS0,r2
 105         tlbre
 106         tlbwe
 107         isync
 108
 109 1:
 110         /*
 111          * Clear and set up some registers.
 112          * Note: Some registers need strict synchronization by
 113          * sync/mbar/msync/isync when being "mtspr".
 114          * BookE: isync before PID,tlbivax,tlbwe
 115          * BookE: isync after MSR,PID; msync_isync after tlbivax & tlbwe
 116          * E500:  msync,isync before L1CSR0
 117          * E500:  isync after BBEAR,BBTAR,BUCSR,DBCR0,DBCR1,HID0,HID1,
 118          *        L1CSR0, L1CSR1, MAS[0,1,2,3,4,6],MMUCSR0, PID[0,1,2],
 119          *        SPEFCSR
 120          */
 121
 122         /* invalidate d-cache */
 123         mfspr   r0,L1CSR0
 124         ori     r0,r0,0x0002
 125         msync
 126         isync
 127         mtspr   L1CSR0,r0
 128         isync
 129
 130         /* disable d-cache */
 131         li      r0,0x0
 132         mtspr   L1CSR0,r0
 133         isync
 134
 135         /* invalidate i-cache */
 136         mfspr   r0,L1CSR1
 137         ori     r0,r0,0x0002
 138         mtspr   L1CSR1,r0
 139         isync
 140
 141         /* disable i-cache */
 142         li      r0,0x0
 143         mtspr   L1CSR1,r0
 144         isync
 145
 146         /* clear registers */
 147         sync
 148         li      r0,0
 149         mtspr   SRR0,r0
 150         mtspr   SRR1,r0
 151         mtspr   CSRR0,r0
 152         mtspr   CSRR1,r0
 153         mtspr   MCSRR0,r0
 154         mtspr   MCSRR1,r0
 155
 156         mtspr   ESR,r0
 157         mtspr   MCSR,r0
 158         mtspr   DEAR,r0
 159
 160         mtspr   DBCR0,r0
 161         isync
 162         mtspr   DBCR1,r0
 163         isync
 164         mtspr   DBCR2,r0
 165         isync
 166         mtspr   IAC1,r0
 167         mtspr   IAC2,r0
 168         mtspr   DAC1,r0
 169         mtspr   DAC2,r0
 170
 171         mfspr   r1,DBSR
 172         mtspr   DBSR,r1         /* Clear all valid bits */
 173
 174         isync
 175         mtspr   PID0,r0
 176         isync
 177         mtspr   PID1,r0
 178         isync
 179         mtspr   PID2,r0
 180         isync
 181
 182         mtspr   TCR,r0
 183
 184         mtspr   BUCSR,r0        /* disable branch prediction */
 185         isync
 186
 187         mtspr   HID0,r0
 188         isync
 189         mtspr   HID1,r0
 190         isync
 191
 192         mtspr   MAS4,r0
 193         isync
 194         mtspr   MAS6,r0
 195         isync
 196
 197         /* Setup interrupt vectors */
 198         lis     r1,0xfff8
 199         mtspr IVPR, r1
 200
 201         li      r1,0x0100
 202         mtspr   IVOR0,r1        /* 0: Critical input */
 203         li      r1,0x0200
 204         mtspr   IVOR1,r1        /* 1: Machine check */
 205         li      r1,0x0300
 206         mtspr   IVOR2,r1        /* 2: Data storage */
 207         li      r1,0x0400
 208         mtspr   IVOR3,r1        /* 3: Instruction storage */
 209         li      r1,0x0500
 210         mtspr   IVOR4,r1        /* 4: External interrupt */
 211         li      r1,0x0600
 212         mtspr   IVOR5,r1        /* 5: Alignment */
 213         li      r1,0x0700
 214         mtspr   IVOR6,r1        /* 6: Program check */
 215         li      r1,0x0800
 216         mtspr   IVOR7,r1        /* 7: floating point unavailable */
 217         li      r1,0x0c00
 218         mtspr   IVOR8,r1        /* 8: System call */
 219         /* 9: Auxiliary processor unavailable(unsupported) */
 220         li      r1,0x1000
 221         mtspr   IVOR10,r1       /* 10: Decrementer */
 222         li      r1,0x1400
 223         mtspr   IVOR13,r1       /* 13: Data TLB error */
 224         li      r1,0x1300
 225         mtspr   IVOR14,r1       /* 14: Instruction TLB error */
 226         li      r1,0x2000
 227         mtspr   IVOR15,r1       /* 15: Debug */
 228
 229         /*
 230          * Invalidate MMU L1/L2
 231          *
 232          * Note: There is a fixup earlier for Errata CPU4 on
 233          * Rev 1 parts that must precede this MMU invalidation.
 234          */
 235         li      r2, 0x001e
 236         mtspr   MMUCSR0, r2
 237         isync
 238
 239         /* After reset, CCSRBAR is located at CFG_CCSRBAR_DEFAULT, i.e.
 240          * 0xff700000-0xff800000. We need add a TLB1 entry for this 1MB
 241          * region before we can access any CCSR registers such as L2
 242          * registers, Local Access Registers,etc. We will also re-allocate
 243          * CFG_CCSRBAR_DEFAULT to CFG_CCSRBAR immediately after TLB1 setup.
 244          *
 245          * Please refer to board-specif directory for TLB1 entry configuration.
 246          * (e.g. board/<yourboard>/init.S)
 247          *
 248          */
 249         bl      tlb1_entry
 250         mr      r5,r0
 251         li      r1,0x000f       /* max 16 TLB1 entries */
 252         mtctr   r1
 253         lwzu    r4,0(r5)        /* how many TLB1 entries we actually use */
 254
 255 0:      cmpwi   r4,0
 256         beq     1f
 257         lwzu    r0,4(r5)
 258         lwzu    r1,4(r5)
 259         lwzu    r2,4(r5)
 260         lwzu    r3,4(r5)
 261         mtspr   MAS0,r0
 262         mtspr   MAS1,r1
 263         mtspr   MAS2,r2
 264         mtspr   MAS3,r3
 265         isync
 266         msync
 267         tlbwe
 268         isync
 269         addi    r4,r4,-1
 270         bdnz    0b
 271
 272 1:
 273 #if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR)
 274         /* Special sequence needed to update CCSRBAR itself */
 275         lis     r4, CFG_CCSRBAR_DEFAULT@h
 276         ori     r4, r4, CFG_CCSRBAR_DEFAULT@l
 277
 278         lis     r5, CFG_CCSRBAR@h
 279         ori     r5, r5, CFG_CCSRBAR@l
 280         srwi    r6,r5,12
 281         stw     r6, 0(r4)
 282         isync
 283
 284         lis     r5, 0xffff
 285         ori     r5,r5,0xf000
 286         lwz     r5, 0(r5)
 287         isync
 288
 289         lis     r3, CFG_CCSRBAR@h
 290         lwz     r5, CFG_CCSRBAR@l(r3)
 291         isync
 292 #endif
 293
 294         /*  invalidate all TLB0 entries */
 295         li      r3,4
 296         li      r4,0
 297         tlbivax r4,r3
 298         /*
 299          * To avoid REV1 Errata CPU6 issues, make sure
 300          * the instruction following tlbivax is not a store.
 301          */
 302
 303
 304         /* set up local access windows, defined at board/<boardname>/init.S */
 305         lis     r7,CFG_CCSRBAR@h
 306         ori     r7,r7,CFG_CCSRBAR@l
 307
 308         bl      law_entry
 309         mr      r6,r0
 310 #if  defined(CONFIG_RAM_AS_FLASH)
 311         li      r1,0x0006
 312 #else
 313         li      r1,0x0007       /*we have 8 LAWs, but reseve one for boot-over-rio-or-pci */
 314 #endif
 315         mtctr   r1
 316         lwzu    r5,0(r6)        /* how many windows we actually use */
 317
 318 #if defined(CONFIG_RAM_AS_FLASH)
 319         li      r2,0x0c48
 320         li      r1,0x0c50
 321 #else
 322         li      r2,0x0c28       /* the first pair is reserved for boot-over-rio-or-pci */
 323         li      r1,0x0c30
 324 #endif
 325
 326 0:      cmpwi   r5,0
 327         beq     1f
 328         lwzu    r4,4(r6)
 329         lwzu    r3,4(r6)
 330         stwx    r4,r7,r2
 331         stwx    r3,r7,r1
 332         addi    r5,r5,-1
 333         addi    r2,r2,0x0020
 334         addi    r1,r1,0x0020
 335         bdnz    0b
 336
 337         /* Jump out the last 4K page and continue to 'normal' start */
 338 1:      bl      3f
 339         b       _start
 340
 341 3:      li      r0,0
 342         mtspr   SRR1,r0         /* Keep things disabled for now */
 343         mflr    r1
 344         mtspr   SRR0,r1
 345         rfi
 346
 347 /*
 348  * r3 - 1st arg to board_init(): IMMP pointer
 349  * r4 - 2nd arg to board_init(): boot flag
 350  */
 351         .text
 352         .long   0x27051956              /* U-BOOT Magic Number                  */
 353         .globl  version_string
 354 version_string:
 355         .ascii U_BOOT_VERSION
 356         .ascii " (", __DATE__, " - ", __TIME__, ")"
 357         .ascii CONFIG_IDENT_STRING, "\0"
 358
 359         . = EXC_OFF_SYS_RESET
 360         .globl  _start
 361 _start:
 362         /* Clear and set up some registers. */
 363         li      r0,0x0000
 364         lis     r1,0xffff
 365         mtspr   DEC,r0                  /* prevent dec exceptions */
 366         mttbl   r0                      /* prevent fit & wdt exceptions */
 367         mttbu   r0
 368         mtspr   TSR,r1                  /* clear all timer exception status */
 369         mtspr   TCR,r0                  /* disable all */
 370         mtspr   ESR,r0                  /* clear exception syndrome register */
 371         mtspr   MCSR,r0                 /* machine check syndrome register */
 372         mtxer   r0                      /* clear integer exception register */
 373         lis     r1,0x0002               /* set CE bit (Critical Exceptions) */
 374         ori     r1,r1,0x1200            /* set ME/DE bit */
 375         mtmsr   r1                      /* change MSR */
 376         isync
 377
 378         /* Enable Time Base and Select Time Base Clock */
 379         li      r0,0x4000               /* time base is processor clock */
 380         mtspr   HID0,r0
 381         isync
 382
 383 #if defined(CONFIG_ADDR_STREAMING)
 384         li      r0,0x3000
 385 #else
 386         li      r0,0x1000
 387 #endif
 388         mtspr   HID1,r0
 389         isync
 390
 391         /* Enable Branch Prediction */
 392 #if defined(CONFIG_BTB)
 393         li      r0,0x201                /* BBFI = 1, BPEN = 1 */
 394         mtspr   BUCSR,r0
 395         isync
 396 #endif
 397
 398 #if defined(CFG_INIT_DBCR)
 399         lis     r1,0xffff
 400         ori     r1,r1,0xffff
 401         mtspr   dbsr,r1                 /* Clear all status bits */
 402         lis     r0,CFG_INIT_DBCR@h      /* DBCR0[IDM] must be set */
 403         ori     r0,r0,CFG_INIT_DBCR@l
 404         mtspr   dbcr0,r0
 405         isync
 406 #endif
 407
 408 /* L1 DCache is used for initial RAM */
 409         mfspr   r2, L1CSR0
 410         ori     r2, r2, 0x0003
 411         oris    r2, r2, 0x0001
 412         msync
 413         isync
 414         mtspr   L1CSR0, r2      /* enable/invalidate L1 Dcache */
 415         isync
 416
 417         /* Allocate Initial RAM in data cache.
 418          */
 419         lis     r3, CFG_INIT_RAM_ADDR@h
 420         ori     r3, r3, CFG_INIT_RAM_ADDR@l
 421         li      r2, 512 /* 512*32=16K */
 422         mtctr   r2
 423         li      r0, 0
 424 1:
 425         dcbz    r0, r3
 426         dcbtls  0,r0, r3
 427         addi    r3, r3, 32
 428         bdnz    1b
 429
 430 #ifndef CFG_RAMBOOT
 431         /* Calculate absolute address in FLASH and jump there           */
 432         /*--------------------------------------------------------------*/
 433         lis     r3, CFG_MONITOR_BASE@h
 434         ori     r3, r3, CFG_MONITOR_BASE@l
 435         addi    r3, r3, in_flash - _start + EXC_OFF_SYS_RESET
 436         mtlr    r3
 437         blr
 438
 439 in_flash:
 440 #endif  /* CFG_RAMBOOT */
 441
 442         /* Setup the stack in initial RAM,could be L2-as-SRAM or L1 dcache*/
 443         lis     r1,CFG_INIT_RAM_ADDR@h
 444         ori     r1,r1,CFG_INIT_SP_OFFSET@l
 445
 446         li      r0,0
 447         stwu    r0,-4(r1)
 448         stwu    r0,-4(r1)               /* Terminate call chain */
 449
 450         stwu    r1,-8(r1)               /* Save back chain and move SP */
 451         lis     r0,RESET_VECTOR@h       /* Address of reset vector */
 452         ori     r0,r0, RESET_VECTOR@l
 453         stwu    r1,-8(r1)               /* Save back chain and move SP */
 454         stw     r0,+12(r1)              /* Save return addr (underflow vect) */
 455
 456         GET_GOT
 457         bl      cpu_init_f
 458         bl      icache_enable
 459         bl      board_init_f
 460         sync
 461
 462
 463 /* --FIXME-- machine check with MCSRRn and rfmci */
 464
 465         .globl  _start_of_vectors
 466 _start_of_vectors:
 467 #if 0
 468 /* Critical input. */
 469         CRIT_EXCEPTION(0x0100, CritcalInput, CritcalInputException)
 470 #endif
 471 /* Machine check --FIXME-- Should be MACH_EXCEPTION */
 472         CRIT_EXCEPTION(0x0200, MachineCheck, MachineCheckException)
 473
 474 /* Data Storage exception. */
 475         STD_EXCEPTION(0x0300, DataStorage, UnknownException)
 476
 477 /* Instruction Storage exception. */
 478         STD_EXCEPTION(0x0400, InstStorage, UnknownException)
 479
 480 /* External Interrupt exception. */
 481         STD_EXCEPTION(0x0500, ExtInterrupt, UnknownException)
 482
 483 /* Alignment exception. */
 484         . = 0x0600
 485 Alignment:
 486         EXCEPTION_PROLOG
 487         mfspr   r4,DAR
 488         stw     r4,_DAR(r21)
 489         mfspr   r5,DSISR
 490         stw     r5,_DSISR(r21)
 491         addi    r3,r1,STACK_FRAME_OVERHEAD
 492         li      r20,MSR_KERNEL
 493         rlwimi  r20,r23,0,16,16         /* copy EE bit from saved MSR */
 494         lwz     r6,GOT(transfer_to_handler)
 495         mtlr    r6
 496         blrl
 497 .L_Alignment:
 498         .long   AlignmentException - _start + EXC_OFF_SYS_RESET
 499         .long   int_return - _start + EXC_OFF_SYS_RESET
 500
 501 /* Program check exception */
 502         . = 0x0700
 503 ProgramCheck:
 504         EXCEPTION_PROLOG
 505         addi    r3,r1,STACK_FRAME_OVERHEAD
 506         li      r20,MSR_KERNEL
 507         rlwimi  r20,r23,0,16,16         /* copy EE bit from saved MSR */
 508         lwz     r6,GOT(transfer_to_handler)
 509         mtlr    r6
 510         blrl
 511 .L_ProgramCheck:
 512         .long   ProgramCheckException - _start + EXC_OFF_SYS_RESET
 513         .long   int_return - _start + EXC_OFF_SYS_RESET
 514
 515         /* No FPU on MPC85xx.  This exception is not supposed to happen.
 516         */
 517         STD_EXCEPTION(0x0800, FPUnavailable, UnknownException)
 518         STD_EXCEPTION(0x0900, Decrementer, timer_interrupt)
 519         STD_EXCEPTION(0x0a00, Trap_0a, UnknownException)
 520         STD_EXCEPTION(0x0b00, Trap_0b, UnknownException)
 521
 522         . = 0x0c00
 523 /*
 524  * r0 - SYSCALL number
 525  * r3-... arguments
 526  */
 527 SystemCall:
 528         addis   r11,r0,0                /* get functions table addr */
 529         ori     r11,r11,0               /* Note: this code is patched in trap_init */
 530         addis   r12,r0,0                /* get number of functions */
 531         ori     r12,r12,0
 532
 533         cmplw   0, r0, r12
 534         bge     1f
 535
 536         rlwinm  r0,r0,2,0,31            /* fn_addr = fn_tbl[r0] */
 537         add     r11,r11,r0
 538         lwz     r11,0(r11)
 539
 540         li      r20,0xd00-4             /* Get stack pointer */
 541         lwz     r12,0(r20)
 542         subi    r12,r12,12              /* Adjust stack pointer */
 543         li      r0,0xc00+_end_back-SystemCall
 544         cmplw   0, r0, r12              /* Check stack overflow */
 545         bgt     1f
 546         stw     r12,0(r20)
 547
 548         mflr    r0
 549         stw     r0,0(r12)
 550         mfspr   r0,SRR0
 551         stw     r0,4(r12)
 552         mfspr   r0,SRR1
 553         stw     r0,8(r12)
 554
 555         li      r12,0xc00+_back-SystemCall
 556         mtlr    r12
 557         mtspr   SRR0,r11
 558
 559 1:      SYNC
 560         rfi
 561 _back:
 562
 563         mfmsr   r11                     /* Disable interrupts */
 564         li      r12,0
 565         ori     r12,r12,MSR_EE
 566         andc    r11,r11,r12
 567         SYNC                            /* Some chip revs need this... */
 568         mtmsr   r11
 569         SYNC
 570
 571         li      r12,0xd00-4             /* restore regs */
 572         lwz     r12,0(r12)
 573
 574         lwz     r11,0(r12)
 575         mtlr    r11
 576         lwz     r11,4(r12)
 577         mtspr   SRR0,r11
 578         lwz     r11,8(r12)
 579         mtspr   SRR1,r11
 580
 581         addi    r12,r12,12              /* Adjust stack pointer */
 582         li      r20,0xd00-4
 583         stw     r12,0(r20)
 584
 585         SYNC
 586         rfi
 587 _end_back:
 588
 589         STD_EXCEPTION(0xd00, SingleStep, UnknownException)
 590
 591         STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
 592         STD_EXCEPTION(0xf00, Trap_0f, UnknownException)
 593
 594         STD_EXCEPTION(0x1000, PIT, PITException)
 595
 596         STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
 597         STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
 598         STD_EXCEPTION(0x1300, InstructionTLBError, UnknownException)
 599         STD_EXCEPTION(0x1400, DataTLBError, UnknownException)
 600
 601         STD_EXCEPTION(0x1500, Reserved5, UnknownException)
 602         STD_EXCEPTION(0x1600, Reserved6, UnknownException)
 603         STD_EXCEPTION(0x1700, Reserved7, UnknownException)
 604         STD_EXCEPTION(0x1800, Reserved8, UnknownException)
 605         STD_EXCEPTION(0x1900, Reserved9, UnknownException)
 606         STD_EXCEPTION(0x1a00, ReservedA, UnknownException)
 607         STD_EXCEPTION(0x1b00, ReservedB, UnknownException)
 608
 609         STD_EXCEPTION(0x1c00, DataBreakpoint, UnknownException)
 610         STD_EXCEPTION(0x1d00, InstructionBreakpoint, UnknownException)
 611         STD_EXCEPTION(0x1e00, PeripheralBreakpoint, UnknownException)
 612         STD_EXCEPTION(0x1f00, DevPortBreakpoint, UnknownException)
 613
 614         CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
 615
 616         .globl  _end_of_vectors
 617 _end_of_vectors:
 618
 619
 620         . = 0x2100
 621
 622 /*
 623  * This code finishes saving the registers to the exception frame
 624  * and jumps to the appropriate handler for the exception.
 625  * Register r21 is pointer into trap frame, r1 has new stack pointer.
 626  */
 627         .globl  transfer_to_handler
 628 transfer_to_handler:
 629         stw     r22,_NIP(r21)
 630         lis     r22,MSR_POW@h
 631         andc    r23,r23,r22
 632         stw     r23,_MSR(r21)
 633         SAVE_GPR(7, r21)
 634         SAVE_4GPRS(8, r21)
 635         SAVE_8GPRS(12, r21)
 636         SAVE_8GPRS(24, r21)
 637
 638         mflr    r23
 639         andi.   r24,r23,0x3f00          /* get vector offset */
 640         stw     r24,TRAP(r21)
 641         li      r22,0
 642         stw     r22,RESULT(r21)
 643         mtspr   SPRG2,r22               /* r1 is now kernel sp */
 644
 645         lwz     r24,0(r23)              /* virtual address of handler */
 646         lwz     r23,4(r23)              /* where to go when done */
 647         mtspr   SRR0,r24
 648         mtspr   SRR1,r20
 649         mtlr    r23
 650         SYNC
 651         rfi                             /* jump to handler, enable MMU */
 652
 653 int_return:
 654         mfmsr   r28             /* Disable interrupts */
 655         li      r4,0
 656         ori     r4,r4,MSR_EE
 657         andc    r28,r28,r4
 658         SYNC                    /* Some chip revs need this... */
 659         mtmsr   r28
 660         SYNC
 661         lwz     r2,_CTR(r1)
 662         lwz     r0,_LINK(r1)
 663         mtctr   r2
 664         mtlr    r0
 665         lwz     r2,_XER(r1)
 666         lwz     r0,_CCR(r1)
 667         mtspr   XER,r2
 668         mtcrf   0xFF,r0
 669         REST_10GPRS(3, r1)
 670         REST_10GPRS(13, r1)
 671         REST_8GPRS(23, r1)
 672         REST_GPR(31, r1)
 673         lwz     r2,_NIP(r1)     /* Restore environment */
 674         lwz     r0,_MSR(r1)
 675         mtspr   SRR0,r2
 676         mtspr   SRR1,r0
 677         lwz     r0,GPR0(r1)
 678         lwz     r2,GPR2(r1)
 679         lwz     r1,GPR1(r1)
 680         SYNC
 681         rfi
 682
 683 crit_return:
 684         mfmsr   r28             /* Disable interrupts */
 685         li      r4,0
 686         ori     r4,r4,MSR_EE
 687         andc    r28,r28,r4
 688         SYNC                    /* Some chip revs need this... */
 689         mtmsr   r28
 690         SYNC
 691         lwz     r2,_CTR(r1)
 692         lwz     r0,_LINK(r1)
 693         mtctr   r2
 694         mtlr    r0
 695         lwz     r2,_XER(r1)
 696         lwz     r0,_CCR(r1)
 697         mtspr   XER,r2
 698         mtcrf   0xFF,r0
 699         REST_10GPRS(3, r1)
 700         REST_10GPRS(13, r1)
 701         REST_8GPRS(23, r1)
 702         REST_GPR(31, r1)
 703         lwz     r2,_NIP(r1)     /* Restore environment */
 704         lwz     r0,_MSR(r1)
 705         mtspr   990,r2          /* SRR2 */
 706         mtspr   991,r0          /* SRR3 */
 707         lwz     r0,GPR0(r1)
 708         lwz     r2,GPR2(r1)
 709         lwz     r1,GPR1(r1)
 710         SYNC
 711         rfci
 712
 713 /* Cache functions.
 714 */
 715 invalidate_icache:
 716         mfspr   r0,L1CSR1
 717         ori     r0,r0,0x0002
 718         mtspr   L1CSR1,r0
 719         isync
 720         blr                             /*   entire I cache */
 721
 722 invalidate_dcache:
 723         mfspr   r0,L1CSR0
 724         ori     r0,r0,0x0002
 725         msync
 726         isync
 727         mtspr   L1CSR0,r0
 728         isync
 729         blr
 730
 731         .globl  icache_enable
 732 icache_enable:
 733         mflr    r8
 734         bl      invalidate_icache
 735         mtlr    r8
 736         isync
 737         mfspr   r4,L1CSR1
 738         ori     r4,r4,0x0001
 739         oris    r4,r4,0x0001
 740         mtspr   L1CSR1,r4
 741         isync
 742         blr
 743
 744         .globl  icache_disable
 745 icache_disable:
 746         mfspr   r0,L1CSR1
 747         lis     r1,0xfffffffe@h
 748         ori     r1,r1,0xfffffffe@l
 749         and     r0,r0,r1
 750         mtspr   L1CSR1,r0
 751         isync
 752         blr
 753
 754         .globl  icache_status
 755 icache_status:
 756         mfspr   r3,L1CSR1
 757         srwi    r3, r3, 31      /* >>31 => select bit 0 */
 758         blr
 759
 760         .globl  dcache_enable
 761 dcache_enable:
 762         mflr    r8
 763         bl      invalidate_dcache
 764         mtlr    r8
 765         isync
 766         mfspr   r0,L1CSR0
 767         ori     r0,r0,0x0001
 768         oris    r0,r0,0x0001
 769         msync
 770         isync
 771         mtspr   L1CSR0,r0
 772         isync
 773         blr
 774
 775         .globl  dcache_disable
 776 dcache_disable:
 777         mfspr   r0,L1CSR0
 778         lis     r1,0xfffffffe@h
 779         ori     r1,r1,0xfffffffe@l
 780         and     r0,r0,r1
 781         msync
 782         isync
 783         mtspr   L1CSR0,r0
 784         isync
 785         blr
 786
 787         .globl  dcache_status
 788 dcache_status:
 789         mfspr   r3,L1CSR0
 790         srwi    r3, r3, 31      /* >>31 => select bit 0 */
 791         blr
 792
 793         .globl get_pir
 794 get_pir:
 795         mfspr   r3, PIR
 796         blr
 797
 798         .globl get_pvr
 799 get_pvr:
 800         mfspr   r3, PVR
 801         blr
 802
 803         .globl get_svr
 804 get_svr:
 805         mfspr   r3, SVR
 806         blr
 807
 808         .globl wr_tcr
 809 wr_tcr:
 810         mtspr   TCR, r3
 811         blr
 812
 813 /*------------------------------------------------------------------------------- */
 814 /* Function:     in8 */
 815 /* Description:  Input 8 bits */
 816 /*------------------------------------------------------------------------------- */
 817         .globl  in8
 818 in8:
 819         lbz     r3,0x0000(r3)
 820         blr
 821
 822 /*------------------------------------------------------------------------------- */
 823 /* Function:     out8 */
 824 /* Description:  Output 8 bits */
 825 /*------------------------------------------------------------------------------- */
 826         .globl  out8
 827 out8:
 828         stb     r4,0x0000(r3)
 829         blr
 830
 831 /*------------------------------------------------------------------------------- */
 832 /* Function:     out16 */
 833 /* Description:  Output 16 bits */
 834 /*------------------------------------------------------------------------------- */
 835         .globl  out16
 836 out16:
 837         sth     r4,0x0000(r3)
 838         blr
 839
 840 /*------------------------------------------------------------------------------- */
 841 /* Function:     out16r */
 842 /* Description:  Byte reverse and output 16 bits */
 843 /*------------------------------------------------------------------------------- */
 844         .globl  out16r
 845 out16r:
 846         sthbrx  r4,r0,r3
 847         blr
 848
 849 /*------------------------------------------------------------------------------- */
 850 /* Function:     out32 */
 851 /* Description:  Output 32 bits */
 852 /*------------------------------------------------------------------------------- */
 853         .globl  out32
 854 out32:
 855         stw     r4,0x0000(r3)
 856         blr
 857
 858 /*------------------------------------------------------------------------------- */
 859 /* Function:     out32r */
 860 /* Description:  Byte reverse and output 32 bits */
 861 /*------------------------------------------------------------------------------- */
 862         .globl  out32r
 863 out32r:
 864         stwbrx  r4,r0,r3
 865         blr
 866
 867 /*------------------------------------------------------------------------------- */
 868 /* Function:     in16 */
 869 /* Description:  Input 16 bits */
 870 /*------------------------------------------------------------------------------- */
 871         .globl  in16
 872 in16:
 873         lhz     r3,0x0000(r3)
 874         blr
 875
 876 /*------------------------------------------------------------------------------- */
 877 /* Function:     in16r */
 878 /* Description:  Input 16 bits and byte reverse */
 879 /*------------------------------------------------------------------------------- */
 880         .globl  in16r
 881 in16r:
 882         lhbrx   r3,r0,r3
 883         blr
 884
 885 /*------------------------------------------------------------------------------- */
 886 /* Function:     in32 */
 887 /* Description:  Input 32 bits */
 888 /*------------------------------------------------------------------------------- */
 889         .globl  in32
 890 in32:
 891         lwz     3,0x0000(3)
 892         blr
 893
 894 /*------------------------------------------------------------------------------- */
 895 /* Function:     in32r */
 896 /* Description:  Input 32 bits and byte reverse */
 897 /*------------------------------------------------------------------------------- */
 898         .globl  in32r
 899 in32r:
 900         lwbrx   r3,r0,r3
 901         blr
 902
 903 /*------------------------------------------------------------------------------- */
 904 /* Function:     ppcDcbf */
 905 /* Description:  Data Cache block flush */
 906 /* Input:        r3 = effective address */
 907 /* Output:       none. */
 908 /*------------------------------------------------------------------------------- */
 909         .globl  ppcDcbf
 910 ppcDcbf:
 911         dcbf    r0,r3
 912         blr
 913
 914 /*------------------------------------------------------------------------------- */
 915 /* Function:     ppcDcbi */
 916 /* Description:  Data Cache block Invalidate */
 917 /* Input:        r3 = effective address */
 918 /* Output:       none. */
 919 /*------------------------------------------------------------------------------- */
 920         .globl  ppcDcbi
 921 ppcDcbi:
 922         dcbi    r0,r3
 923         blr
 924
 925 /*------------------------------------------------------------------------------- */
 926 /* Function:     ppcSync */
 927 /* Description:  Processor Synchronize */
 928 /* Input:        none. */
 929 /* Output:       none. */
 930 /*------------------------------------------------------------------------------- */
 931         .globl  ppcSync
 932 ppcSync:
 933         sync
 934         blr
 935
 936 /*------------------------------------------------------------------------------*/
 937
 938 /*
 939  * void relocate_code (addr_sp, gd, addr_moni)
 940  *
 941  * This "function" does not return, instead it continues in RAM
 942  * after relocating the monitor code.
 943  *
 944  * r3 = dest
 945  * r4 = src
 946  * r5 = length in bytes
 947  * r6 = cachelinesize
 948  */
 949         .globl  relocate_code
 950 relocate_code:
 951         mr      r1,  r3         /* Set new stack pointer                */
 952         mr      r9,  r4         /* Save copy of Init Data pointer       */
 953         mr      r10, r5         /* Save copy of Destination Address     */
 954
 955         mr      r3,  r5                         /* Destination Address  */
 956         lis     r4, CFG_MONITOR_BASE@h          /* Source      Address  */
 957         ori     r4, r4, CFG_MONITOR_BASE@l
 958         lwz     r5,GOT(__init_end)
 959         sub     r5,r5,r4
 960         li      r6, CFG_CACHELINE_SIZE          /* Cache Line Size      */
 961
 962         /*
 963          * Fix GOT pointer:
 964          *
 965          * New GOT-PTR = (old GOT-PTR - CFG_MONITOR_BASE) + Destination Address
 966          *
 967          * Offset:
 968          */
 969         sub     r15, r10, r4
 970
 971         /* First our own GOT */
 972         add     r14, r14, r15
 973         /* the the one used by the C code */
 974         add     r30, r30, r15
 975
 976         /*
 977          * Now relocate code
 978          */
 979
 980         cmplw   cr1,r3,r4
 981         addi    r0,r5,3
 982         srwi.   r0,r0,2
 983         beq     cr1,4f          /* In place copy is not necessary       */
 984         beq     7f              /* Protect against 0 count              */
 985         mtctr   r0
 986         bge     cr1,2f
 987
 988         la      r8,-4(r4)
 989         la      r7,-4(r3)
 990 1:      lwzu    r0,4(r8)
 991         stwu    r0,4(r7)
 992         bdnz    1b
 993         b       4f
 994
 995 2:      slwi    r0,r0,2
 996         add     r8,r4,r0
 997         add     r7,r3,r0
 998 3:      lwzu    r0,-4(r8)
 999         stwu    r0,-4(r7)
1000         bdnz    3b
1001
1002 /*
1003  * Now flush the cache: note that we must start from a cache aligned
1004  * address. Otherwise we might miss one cache line.
1005  */
1006 4:      cmpwi   r6,0
1007         add     r5,r3,r5
1008         beq     7f              /* Always flush prefetch queue in any case */
1009         subi    r0,r6,1
1010         andc    r3,r3,r0
1011         mr      r4,r3
1012 5:      dcbst   0,r4
1013         add     r4,r4,r6
1014         cmplw   r4,r5
1015         blt     5b
1016         sync                    /* Wait for all dcbst to complete on bus */
1017         mr      r4,r3
1018 6:      icbi    0,r4
1019         add     r4,r4,r6
1020         cmplw   r4,r5
1021         blt     6b
1022 7:      sync                    /* Wait for all icbi to complete on bus */
1023         isync
1024
1025 /*
1026  * We are done. Do not return, instead branch to second part of board
1027  * initialization, now running from RAM.
1028  */
1029
1030         addi    r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
1031         mtlr    r0
1032         blr                             /* NEVER RETURNS! */
1033
1034 in_ram:
1035
1036         /*
1037          * Relocation Function, r14 point to got2+0x8000
1038          *
1039          * Adjust got2 pointers, no need to check for 0, this code
1040          * already puts a few entries in the table.
1041          */
1042         li      r0,__got2_entries@sectoff@l
1043         la      r3,GOT(_GOT2_TABLE_)
1044         lwz     r11,GOT(_GOT2_TABLE_)
1045         mtctr   r0
1046         sub     r11,r3,r11
1047         addi    r3,r3,-4
1048 1:      lwzu    r0,4(r3)
1049         add     r0,r0,r11
1050         stw     r0,0(r3)
1051         bdnz    1b
1052
1053         /*
1054          * Now adjust the fixups and the pointers to the fixups
1055          * in case we need to move ourselves again.
1056          */
1057 2:      li      r0,__fixup_entries@sectoff@l
1058         lwz     r3,GOT(_FIXUP_TABLE_)
1059         cmpwi   r0,0
1060         mtctr   r0
1061         addi    r3,r3,-4
1062         beq     4f
1063 3:      lwzu    r4,4(r3)
1064         lwzux   r0,r4,r11
1065         add     r0,r0,r11
1066         stw     r10,0(r3)
1067         stw     r0,0(r4)
1068         bdnz    3b
1069 4:
1070 clear_bss:
1071         /*
1072          * Now clear BSS segment
1073          */
1074         lwz     r3,GOT(__bss_start)
1075         lwz     r4,GOT(_end)
1076
1077         cmplw   0, r3, r4
1078         beq     6f
1079
1080         li      r0, 0
1081 5:
1082         stw     r0, 0(r3)
1083         addi    r3, r3, 4
1084         cmplw   0, r3, r4
1085         bne     5b
1086 6:
1087
1088         mr      r3, r9          /* Init Data pointer            */
1089         mr      r4, r10         /* Destination Address          */
1090         bl      board_init_r
1091
1092         /*
1093          * Copy exception vector code to low memory
1094          *
1095          * r3: dest_addr
1096          * r7: source address, r8: end address, r9: target address
1097          */
1098         .globl  trap_init
1099 trap_init:
1100         lwz     r7, GOT(_start)
1101         lwz     r8, GOT(_end_of_vectors)
1102
1103         li      r9, 0x100               /* reset vector always at 0x100 */
1104
1105         cmplw   0, r7, r8
1106         bgelr                           /* return if r7>=r8 - just in case */
1107
1108         mflr    r4                      /* save link register           */
1109 1:
1110         lwz     r0, 0(r7)
1111         stw     r0, 0(r9)
1112         addi    r7, r7, 4
1113         addi    r9, r9, 4
1114         cmplw   0, r7, r8
1115         bne     1b
1116
1117         /*
1118          * relocate `hdlr' and `int_return' entries
1119          */
1120         li      r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
1121         li      r8, Alignment - _start + EXC_OFF_SYS_RESET
1122 2:
1123         bl      trap_reloc
1124         addi    r7, r7, 0x100           /* next exception vector        */
1125         cmplw   0, r7, r8
1126         blt     2b
1127
1128         li      r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
1129         bl      trap_reloc
1130
1131         li      r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
1132         bl      trap_reloc
1133
1134         li      r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
1135         li      r8, SystemCall - _start + EXC_OFF_SYS_RESET
1136 3:
1137         bl      trap_reloc
1138         addi    r7, r7, 0x100           /* next exception vector        */
1139         cmplw   0, r7, r8
1140         blt     3b
1141
1142         li      r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
1143         li      r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
1144 4:
1145         bl      trap_reloc
1146         addi    r7, r7, 0x100           /* next exception vector        */
1147         cmplw   0, r7, r8
1148         blt     4b
1149
1150         mtlr    r4                      /* restore link register        */
1151         blr
1152
1153         /*
1154          * Function: relocate entries for one exception vector
1155          */
1156 trap_reloc:
1157         lwz     r0, 0(r7)               /* hdlr ...                     */
1158         add     r0, r0, r3              /*  ... += dest_addr            */
1159         stw     r0, 0(r7)
1160
1161         lwz     r0, 4(r7)               /* int_return ...               */
1162         add     r0, r0, r3              /*  ... += dest_addr            */
1163         stw     r0, 4(r7)
1164
1165         blr
1166
1167 #ifdef CFG_INIT_RAM_LOCK
1168 .globl unlock_ram_in_cache
1169 unlock_ram_in_cache:
1170         /* invalidate the INIT_RAM section */
1171         lis     r3, (CFG_INIT_RAM_ADDR & ~31)@h
1172         ori     r3, r3, (CFG_INIT_RAM_ADDR & ~31)@l
1173         li      r2,512
1174         mtctr   r2
1175 1:      icbi    r0, r3
1176         dcbi    r0, r3
1177         addi    r3, r3, 32
1178         bdnz    1b
1179         sync                    /* Wait for all icbi to complete on bus */
1180         isync
1181         blr
1182 #endif