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