cpu/mpc5xxx/start.S

   1 /*
   2  *  Copyright (C) 1998  Dan Malek <dmalek@jlc.net>
   3  *  Copyright (C) 1999  Magnus Damm <kieraypc01.p.y.kie.era.ericsson.se>
   4  *  Copyright (C) 2000 - 2003 Wolfgang Denk <wd@denx.de>
   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 /*
  26  *  U-Boot - Startup Code for MPC5xxx CPUs
  27  */
  28 #include <config.h>
  29 #include <mpc5xxx.h>
  30 #include <version.h>
  31
  32 #define CONFIG_MPC5xxx 1        /* needed for Linux kernel header files */
  33 #define _LINUX_CONFIG_H 1       /* avoid reading Linux autoconf.h file  */
  34
  35 #include <ppc_asm.tmpl>
  36 #include <ppc_defs.h>
  37
  38 #include <asm/cache.h>
  39 #include <asm/mmu.h>
  40
  41 #ifndef  CONFIG_IDENT_STRING
  42 #define  CONFIG_IDENT_STRING ""
  43 #endif
  44
  45 /* We don't want the  MMU yet.
  46 */
  47 #undef  MSR_KERNEL
  48 /* Floating Point enable, Machine Check and Recoverable Interr. */
  49 #ifdef DEBUG
  50 #define MSR_KERNEL (MSR_FP|MSR_RI)
  51 #else
  52 #define MSR_KERNEL (MSR_FP|MSR_ME|MSR_RI)
  53 #endif
  54
  55 /*
  56  * Set up GOT: Global Offset Table
  57  *
  58  * Use r14 to access the GOT
  59  */
  60         START_GOT
  61         GOT_ENTRY(_GOT2_TABLE_)
  62         GOT_ENTRY(_FIXUP_TABLE_)
  63
  64         GOT_ENTRY(_start)
  65         GOT_ENTRY(_start_of_vectors)
  66         GOT_ENTRY(_end_of_vectors)
  67         GOT_ENTRY(transfer_to_handler)
  68
  69         GOT_ENTRY(__init_end)
  70         GOT_ENTRY(_end)
  71         GOT_ENTRY(__bss_start)
  72         END_GOT
  73
  74 /*
  75  * Version string
  76  */
  77         .data
  78         .globl  version_string
  79 version_string:
  80         .ascii U_BOOT_VERSION
  81         .ascii " (", __DATE__, " - ", __TIME__, ")"
  82         .ascii CONFIG_IDENT_STRING, "\0"
  83
  84 /*
  85  * Exception vectors
  86  */
  87         .text
  88         . = EXC_OFF_SYS_RESET
  89         .globl  _start
  90 _start:
  91         li      r21, BOOTFLAG_COLD      /* Normal Power-On              */
  92         nop
  93         b       boot_cold
  94
  95         . = EXC_OFF_SYS_RESET + 0x10
  96
  97         .globl  _start_warm
  98 _start_warm:
  99         li      r21, BOOTFLAG_WARM      /* Software reboot              */
 100         b       boot_warm
 101
 102 boot_cold:
 103 boot_warm:
 104         mfmsr   r5                      /* save msr contents            */
 105
 106 #if defined(CFG_LOWBOOT)
 107 #if defined(CFG_RAMBOOT)
 108 #error CFG_LOWBOOT is incompatible with CFG_RAMBOOT
 109 #endif /* CFG_RAMBOOT */
 110         lis     r4, CFG_DEFAULT_MBAR@h
 111         lis     r3,     START_REG(CFG_BOOTCS_START)@h
 112         ori     r3, r3, START_REG(CFG_BOOTCS_START)@l
 113         stw     r3, 0x4(r4)             /* CS0 start */
 114         lis     r3,     STOP_REG(CFG_BOOTCS_START, CFG_BOOTCS_SIZE)@h
 115         ori     r3, r3, STOP_REG(CFG_BOOTCS_START, CFG_BOOTCS_SIZE)@l
 116
 117         stw     r3, 0x8(r4)             /* CS0 stop */
 118         lis     r3,     0x00047800@h
 119         ori     r3, r3, 0x00047800@l
 120         stw     r3, 0x300(r4)           /* set timing, CS0/boot conf reg */
 121         lis     r3,     0x02010000@h
 122         ori     r3, r3, 0x02010000@l
 123         stw     r3, 0x54(r4)            /* CS0 and Boot enable, IPBI ctrl reg */
 124
 125         lis     r3,     lowboot_reentry@h
 126         ori     r3, r3, lowboot_reentry@l
 127         mtlr    r3
 128         blr                             /* jump to flash based address */
 129
 130 lowboot_reentry:
 131         lis     r3,     START_REG(CFG_BOOTCS_START)@h
 132         ori     r3, r3, START_REG(CFG_BOOTCS_START)@l
 133         stw     r3, 0x4c(r4)            /* Boot start */
 134         lis     r3,     STOP_REG(CFG_BOOTCS_START, CFG_BOOTCS_SIZE)@h
 135         ori     r3, r3, STOP_REG(CFG_BOOTCS_START, CFG_BOOTCS_SIZE)@l
 136         stw     r3, 0x50(r4)            /* Boot stop */
 137         lis     r3,     0x00047800@h
 138         ori     r3, r3, 0x00047800@l
 139         stw     r3, 0x300(r4)           /* set timing, CS0/boot conf reg */
 140         lis     r3,     0x02000001@h
 141         ori     r3, r3, 0x02000001@l
 142         stw     r3, 0x54(r4)            /* Boot enable, CS0 disable, wait state enable */
 143 #endif  /* CFG_LOWBOOT */
 144
 145 #if defined(CFG_DEFAULT_MBAR) && !defined(CFG_RAMBOOT)
 146         lis     r3, CFG_MBAR@h
 147         ori     r3, r3, CFG_MBAR@l
 148 #if defined(CONFIG_MPC5200)
 149         /* MBAR is mirrored into the MBAR SPR */
 150         mtspr   MBAR,r3
 151         rlwinm  r3, r3, 16, 16, 31
 152 #endif
 153 #if defined(CONFIG_MGT5100)
 154         rlwinm  r3, r3, 17, 15, 31
 155 #endif
 156         lis     r4, CFG_DEFAULT_MBAR@h
 157         stw     r3, 0(r4)
 158 #endif /* CFG_DEFAULT_MBAR */
 159
 160         /* Initialise the MPC5xxx processor core                        */
 161         /*--------------------------------------------------------------*/
 162
 163         bl      init_5xxx_core
 164
 165         /* initialize some things that are hard to access from C        */
 166         /*--------------------------------------------------------------*/
 167
 168         /* set up stack in on-chip SRAM */
 169         lis     r3, CFG_INIT_RAM_ADDR@h
 170         ori     r3, r3, CFG_INIT_RAM_ADDR@l
 171         ori     r1, r3, CFG_INIT_SP_OFFSET
 172         li      r0, 0                   /* Make room for stack frame header and */
 173         stwu    r0, -4(r1)              /* clear final stack frame so that      */
 174         stwu    r0, -4(r1)              /* stack backtraces terminate cleanly   */
 175
 176         /* let the C-code set up the rest                               */
 177         /*                                                              */
 178         /* Be careful to keep code relocatable !                        */
 179         /*--------------------------------------------------------------*/
 180
 181         GET_GOT                 /* initialize GOT access                */
 182
 183         /* r3: IMMR */
 184         bl      cpu_init_f      /* run low-level CPU init code (in Flash)*/
 185
 186         mr      r3, r21
 187         /* r3: BOOTFLAG */
 188         bl      board_init_f    /* run 1st part of board init code (in Flash)*/
 189
 190 /*
 191  * Vector Table
 192  */
 193
 194         .globl  _start_of_vectors
 195 _start_of_vectors:
 196
 197 /* Machine check */
 198         STD_EXCEPTION(0x200, MachineCheck, MachineCheckException)
 199
 200 /* Data Storage exception. */
 201         STD_EXCEPTION(0x300, DataStorage, UnknownException)
 202
 203 /* Instruction Storage exception. */
 204         STD_EXCEPTION(0x400, InstStorage, UnknownException)
 205
 206 /* External Interrupt exception. */
 207         STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
 208
 209 /* Alignment exception. */
 210         . = 0x600
 211 Alignment:
 212         EXCEPTION_PROLOG
 213         mfspr   r4,DAR
 214         stw     r4,_DAR(r21)
 215         mfspr   r5,DSISR
 216         stw     r5,_DSISR(r21)
 217         addi    r3,r1,STACK_FRAME_OVERHEAD
 218         li      r20,MSR_KERNEL
 219         rlwimi  r20,r23,0,16,16         /* copy EE bit from saved MSR */
 220         rlwimi  r20,r23,0,25,25         /* copy IP bit from saved MSR */
 221         lwz     r6,GOT(transfer_to_handler)
 222         mtlr    r6
 223         blrl
 224 .L_Alignment:
 225         .long   AlignmentException - _start + EXC_OFF_SYS_RESET
 226         .long   int_return - _start + EXC_OFF_SYS_RESET
 227
 228 /* Program check exception */
 229         . = 0x700
 230 ProgramCheck:
 231         EXCEPTION_PROLOG
 232         addi    r3,r1,STACK_FRAME_OVERHEAD
 233         li      r20,MSR_KERNEL
 234         rlwimi  r20,r23,0,16,16         /* copy EE bit from saved MSR */
 235         rlwimi  r20,r23,0,25,25         /* copy IP bit from saved MSR */
 236         lwz     r6,GOT(transfer_to_handler)
 237         mtlr    r6
 238         blrl
 239 .L_ProgramCheck:
 240         .long   ProgramCheckException - _start + EXC_OFF_SYS_RESET
 241         .long   int_return - _start + EXC_OFF_SYS_RESET
 242
 243         STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
 244
 245         /* I guess we could implement decrementer, and may have
 246          * to someday for timekeeping.
 247          */
 248         STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
 249
 250         STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
 251         STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
 252         STD_EXCEPTION(0xc00, SystemCall, UnknownException)
 253         STD_EXCEPTION(0xd00, SingleStep, UnknownException)
 254
 255         STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
 256         STD_EXCEPTION(0xf00, Trap_0f, UnknownException)
 257
 258         STD_EXCEPTION(0x1000, InstructionTLBMiss, UnknownException)
 259         STD_EXCEPTION(0x1100, DataLoadTLBMiss, UnknownException)
 260         STD_EXCEPTION(0x1200, DataStoreTLBMiss, UnknownException)
 261 #ifdef DEBUG
 262         . = 0x1300
 263         /*
 264          * This exception occurs when the program counter matches the
 265          * Instruction Address Breakpoint Register (IABR).
 266          *
 267          * I want the cpu to halt if this occurs so I can hunt around
 268          * with the debugger and look at things.
 269          *
 270          * When DEBUG is defined, both machine check enable (in the MSR)
 271          * and checkstop reset enable (in the reset mode register) are
 272          * turned off and so a checkstop condition will result in the cpu
 273          * halting.
 274          *
 275          * I force the cpu into a checkstop condition by putting an illegal
 276          * instruction here (at least this is the theory).
 277          *
 278          * well - that didnt work, so just do an infinite loop!
 279          */
 280 1:      b       1b
 281 #else
 282         STD_EXCEPTION(0x1300, InstructionBreakpoint, DebugException)
 283 #endif
 284         STD_EXCEPTION(0x1400, SMI, UnknownException)
 285
 286         STD_EXCEPTION(0x1500, Trap_15, UnknownException)
 287         STD_EXCEPTION(0x1600, Trap_16, UnknownException)
 288         STD_EXCEPTION(0x1700, Trap_17, UnknownException)
 289         STD_EXCEPTION(0x1800, Trap_18, UnknownException)
 290         STD_EXCEPTION(0x1900, Trap_19, UnknownException)
 291         STD_EXCEPTION(0x1a00, Trap_1a, UnknownException)
 292         STD_EXCEPTION(0x1b00, Trap_1b, UnknownException)
 293         STD_EXCEPTION(0x1c00, Trap_1c, UnknownException)
 294         STD_EXCEPTION(0x1d00, Trap_1d, UnknownException)
 295         STD_EXCEPTION(0x1e00, Trap_1e, UnknownException)
 296         STD_EXCEPTION(0x1f00, Trap_1f, UnknownException)
 297         STD_EXCEPTION(0x2000, Trap_20, UnknownException)
 298         STD_EXCEPTION(0x2100, Trap_21, UnknownException)
 299         STD_EXCEPTION(0x2200, Trap_22, UnknownException)
 300         STD_EXCEPTION(0x2300, Trap_23, UnknownException)
 301         STD_EXCEPTION(0x2400, Trap_24, UnknownException)
 302         STD_EXCEPTION(0x2500, Trap_25, UnknownException)
 303         STD_EXCEPTION(0x2600, Trap_26, UnknownException)
 304         STD_EXCEPTION(0x2700, Trap_27, UnknownException)
 305         STD_EXCEPTION(0x2800, Trap_28, UnknownException)
 306         STD_EXCEPTION(0x2900, Trap_29, UnknownException)
 307         STD_EXCEPTION(0x2a00, Trap_2a, UnknownException)
 308         STD_EXCEPTION(0x2b00, Trap_2b, UnknownException)
 309         STD_EXCEPTION(0x2c00, Trap_2c, UnknownException)
 310         STD_EXCEPTION(0x2d00, Trap_2d, UnknownException)
 311         STD_EXCEPTION(0x2e00, Trap_2e, UnknownException)
 312         STD_EXCEPTION(0x2f00, Trap_2f, UnknownException)
 313
 314
 315         .globl  _end_of_vectors
 316 _end_of_vectors:
 317
 318         . = 0x3000
 319
 320 /*
 321  * This code finishes saving the registers to the exception frame
 322  * and jumps to the appropriate handler for the exception.
 323  * Register r21 is pointer into trap frame, r1 has new stack pointer.
 324  */
 325         .globl  transfer_to_handler
 326 transfer_to_handler:
 327         stw     r22,_NIP(r21)
 328         lis     r22,MSR_POW@h
 329         andc    r23,r23,r22
 330         stw     r23,_MSR(r21)
 331         SAVE_GPR(7, r21)
 332         SAVE_4GPRS(8, r21)
 333         SAVE_8GPRS(12, r21)
 334         SAVE_8GPRS(24, r21)
 335         mflr    r23
 336         andi.   r24,r23,0x3f00          /* get vector offset */
 337         stw     r24,TRAP(r21)
 338         li      r22,0
 339         stw     r22,RESULT(r21)
 340         lwz     r24,0(r23)              /* virtual address of handler */
 341         lwz     r23,4(r23)              /* where to go when done */
 342         mtspr   SRR0,r24
 343         mtspr   SRR1,r20
 344         mtlr    r23
 345         SYNC
 346         rfi                             /* jump to handler, enable MMU */
 347
 348 int_return:
 349         mfmsr   r28             /* Disable interrupts */
 350         li      r4,0
 351         ori     r4,r4,MSR_EE
 352         andc    r28,r28,r4
 353         SYNC                    /* Some chip revs need this... */
 354         mtmsr   r28
 355         SYNC
 356         lwz     r2,_CTR(r1)
 357         lwz     r0,_LINK(r1)
 358         mtctr   r2
 359         mtlr    r0
 360         lwz     r2,_XER(r1)
 361         lwz     r0,_CCR(r1)
 362         mtspr   XER,r2
 363         mtcrf   0xFF,r0
 364         REST_10GPRS(3, r1)
 365         REST_10GPRS(13, r1)
 366         REST_8GPRS(23, r1)
 367         REST_GPR(31, r1)
 368         lwz     r2,_NIP(r1)     /* Restore environment */
 369         lwz     r0,_MSR(r1)
 370         mtspr   SRR0,r2
 371         mtspr   SRR1,r0
 372         lwz     r0,GPR0(r1)
 373         lwz     r2,GPR2(r1)
 374         lwz     r1,GPR1(r1)
 375         SYNC
 376         rfi
 377
 378 /*
 379  * This code initialises the MPC5xxx processor core
 380  * (conforms to PowerPC 603e spec)
 381  * Note: expects original MSR contents to be in r5.
 382  */
 383
 384         .globl  init_5xx_core
 385 init_5xxx_core:
 386
 387         /* Initialize machine status; enable machine check interrupt    */
 388         /*--------------------------------------------------------------*/
 389
 390         li      r3, MSR_KERNEL          /* Set ME and RI flags */
 391         rlwimi  r3, r5, 0, 25, 25       /* preserve IP bit set by HRCW */
 392 #ifdef DEBUG
 393         rlwimi  r3, r5, 0, 21, 22       /* debugger might set SE & BE bits */
 394 #endif
 395         SYNC                            /* Some chip revs need this... */
 396         mtmsr   r3
 397         SYNC
 398         mtspr   SRR1, r3                /* Make SRR1 match MSR */
 399
 400         /* Initialize the Hardware Implementation-dependent Registers   */
 401         /* HID0 also contains cache control                             */
 402         /*--------------------------------------------------------------*/
 403
 404         lis     r3, CFG_HID0_INIT@h
 405         ori     r3, r3, CFG_HID0_INIT@l
 406         SYNC
 407         mtspr   HID0, r3
 408
 409         lis     r3, CFG_HID0_FINAL@h
 410         ori     r3, r3, CFG_HID0_FINAL@l
 411         SYNC
 412         mtspr   HID0, r3
 413
 414         /* clear all BAT's                                              */
 415         /*--------------------------------------------------------------*/
 416
 417         li      r0, 0
 418         mtspr   DBAT0U, r0
 419         mtspr   DBAT0L, r0
 420         mtspr   DBAT1U, r0
 421         mtspr   DBAT1L, r0
 422         mtspr   DBAT2U, r0
 423         mtspr   DBAT2L, r0
 424         mtspr   DBAT3U, r0
 425         mtspr   DBAT3L, r0
 426         mtspr   DBAT4U, r0
 427         mtspr   DBAT4L, r0
 428         mtspr   DBAT5U, r0
 429         mtspr   DBAT5L, r0
 430         mtspr   DBAT6U, r0
 431         mtspr   DBAT6L, r0
 432         mtspr   DBAT7U, r0
 433         mtspr   DBAT7L, r0
 434         mtspr   IBAT0U, r0
 435         mtspr   IBAT0L, r0
 436         mtspr   IBAT1U, r0
 437         mtspr   IBAT1L, r0
 438         mtspr   IBAT2U, r0
 439         mtspr   IBAT2L, r0
 440         mtspr   IBAT3U, r0
 441         mtspr   IBAT3L, r0
 442         mtspr   IBAT4U, r0
 443         mtspr   IBAT4L, r0
 444         mtspr   IBAT5U, r0
 445         mtspr   IBAT5L, r0
 446         mtspr   IBAT6U, r0
 447         mtspr   IBAT6L, r0
 448         mtspr   IBAT7U, r0
 449         mtspr   IBAT7L, r0
 450         SYNC
 451
 452         /* invalidate all tlb's                                         */
 453         /*                                                              */
 454         /* From the 603e User Manual: "The 603e provides the ability to */
 455         /* invalidate a TLB entry. The TLB Invalidate Entry (tlbie)     */
 456         /* instruction invalidates the TLB entry indexed by the EA, and */
 457         /* operates on both the instruction and data TLBs simultaneously*/
 458         /* invalidating four TLB entries (both sets in each TLB). The   */
 459         /* index corresponds to bits 15-19 of the EA. To invalidate all */
 460         /* entries within both TLBs, 32 tlbie instructions should be    */
 461         /* issued, incrementing this field by one each time."           */
 462         /*                                                              */
 463         /* "Note that the tlbia instruction is not implemented on the   */
 464         /* 603e."                                                       */
 465         /*                                                              */
 466         /* bits 15-19 correspond to addresses 0x00000000 to 0x0001F000  */
 467         /* incrementing by 0x1000 each time. The code below is sort of  */
 468         /* based on code in "flush_tlbs" from arch/ppc/kernel/head.S    */
 469         /*                                                              */
 470         /*--------------------------------------------------------------*/
 471
 472         li      r3, 32
 473         mtctr   r3
 474         li      r3, 0
 475 1:      tlbie   r3
 476         addi    r3, r3, 0x1000
 477         bdnz    1b
 478         SYNC
 479
 480         /* Done!                                                        */
 481         /*--------------------------------------------------------------*/
 482
 483         blr
 484
 485 /* Cache functions.
 486  *
 487  * Note: requires that all cache bits in
 488  * HID0 are in the low half word.
 489  */
 490         .globl  icache_enable
 491 icache_enable:
 492         mfspr   r3, HID0
 493         ori     r3, r3, HID0_ICE
 494         lis     r4, 0
 495         ori     r4, r4, HID0_ILOCK
 496         andc    r3, r3, r4
 497         ori     r4, r3, HID0_ICFI
 498         isync
 499         mtspr   HID0, r4        /* sets enable and invalidate, clears lock */
 500         isync
 501         mtspr   HID0, r3        /* clears invalidate */
 502         blr
 503
 504         .globl  icache_disable
 505 icache_disable:
 506         mfspr   r3, HID0
 507         lis     r4, 0
 508         ori     r4, r4, HID0_ICE|HID0_ILOCK
 509         andc    r3, r3, r4
 510         ori     r4, r3, HID0_ICFI
 511         isync
 512         mtspr   HID0, r4        /* sets invalidate, clears enable and lock */
 513         isync
 514         mtspr   HID0, r3        /* clears invalidate */
 515         blr
 516
 517         .globl  icache_status
 518 icache_status:
 519         mfspr   r3, HID0
 520         rlwinm  r3, r3, HID0_ICE_BITPOS + 1, 31, 31
 521         blr
 522
 523         .globl  dcache_enable
 524 dcache_enable:
 525         mfspr   r3, HID0
 526         ori     r3, r3, HID0_DCE
 527         lis     r4, 0
 528         ori     r4, r4, HID0_DLOCK
 529         andc    r3, r3, r4
 530         ori     r4, r3, HID0_DCI
 531         sync
 532         mtspr   HID0, r4        /* sets enable and invalidate, clears lock */
 533         sync
 534         mtspr   HID0, r3        /* clears invalidate */
 535         blr
 536
 537         .globl  dcache_disable
 538 dcache_disable:
 539         mfspr   r3, HID0
 540         lis     r4, 0
 541         ori     r4, r4, HID0_DCE|HID0_DLOCK
 542         andc    r3, r3, r4
 543         ori     r4, r3, HID0_DCI
 544         sync
 545         mtspr   HID0, r4        /* sets invalidate, clears enable and lock */
 546         sync
 547         mtspr   HID0, r3        /* clears invalidate */
 548         blr
 549
 550         .globl  dcache_status
 551 dcache_status:
 552         mfspr   r3, HID0
 553         rlwinm  r3, r3, HID0_DCE_BITPOS + 1, 31, 31
 554         blr
 555
 556         .globl get_pvr
 557 get_pvr:
 558         mfspr   r3, PVR
 559         blr
 560
 561 /*------------------------------------------------------------------------------*/
 562
 563 /*
 564  * void relocate_code (addr_sp, gd, addr_moni)
 565  *
 566  * This "function" does not return, instead it continues in RAM
 567  * after relocating the monitor code.
 568  *
 569  * r3 = dest
 570  * r4 = src
 571  * r5 = length in bytes
 572  * r6 = cachelinesize
 573  */
 574         .globl  relocate_code
 575 relocate_code:
 576         mr      r1,  r3         /* Set new stack pointer                */
 577         mr      r9,  r4         /* Save copy of Global Data pointer     */
 578         mr      r10, r5         /* Save copy of Destination Address     */
 579
 580         mr      r3,  r5                         /* Destination Address  */
 581         lis     r4, CFG_MONITOR_BASE@h          /* Source      Address  */
 582         ori     r4, r4, CFG_MONITOR_BASE@l
 583         lwz     r5, GOT(__init_end)
 584         sub     r5, r5, r4
 585         li      r6, CFG_CACHELINE_SIZE          /* Cache Line Size      */
 586
 587         /*
 588          * Fix GOT pointer:
 589          *
 590          * New GOT-PTR = (old GOT-PTR - CFG_MONITOR_BASE) + Destination Address
 591          *
 592          * Offset:
 593          */
 594         sub     r15, r10, r4
 595
 596         /* First our own GOT */
 597         add     r14, r14, r15
 598         /* then the one used by the C code */
 599         add     r30, r30, r15
 600
 601         /*
 602          * Now relocate code
 603          */
 604
 605         cmplw   cr1,r3,r4
 606         addi    r0,r5,3
 607         srwi.   r0,r0,2
 608         beq     cr1,4f          /* In place copy is not necessary       */
 609         beq     7f              /* Protect against 0 count              */
 610         mtctr   r0
 611         bge     cr1,2f
 612
 613         la      r8,-4(r4)
 614         la      r7,-4(r3)
 615 1:      lwzu    r0,4(r8)
 616         stwu    r0,4(r7)
 617         bdnz    1b
 618         b       4f
 619
 620 2:      slwi    r0,r0,2
 621         add     r8,r4,r0
 622         add     r7,r3,r0
 623 3:      lwzu    r0,-4(r8)
 624         stwu    r0,-4(r7)
 625         bdnz    3b
 626
 627 /*
 628  * Now flush the cache: note that we must start from a cache aligned
 629  * address. Otherwise we might miss one cache line.
 630  */
 631 4:      cmpwi   r6,0
 632         add     r5,r3,r5
 633         beq     7f              /* Always flush prefetch queue in any case */
 634         subi    r0,r6,1
 635         andc    r3,r3,r0
 636         mfspr   r7,HID0         /* don't do dcbst if dcache is disabled */
 637         rlwinm  r7,r7,HID0_DCE_BITPOS+1,31,31
 638         cmpwi   r7,0
 639         beq     9f
 640         mr      r4,r3
 641 5:      dcbst   0,r4
 642         add     r4,r4,r6
 643         cmplw   r4,r5
 644         blt     5b
 645         sync                    /* Wait for all dcbst to complete on bus */
 646 9:      mfspr   r7,HID0         /* don't do icbi if icache is disabled */
 647         rlwinm  r7,r7,HID0_ICE_BITPOS+1,31,31
 648         cmpwi   r7,0
 649         beq     7f
 650         mr      r4,r3
 651 6:      icbi    0,r4
 652         add     r4,r4,r6
 653         cmplw   r4,r5
 654         blt     6b
 655 7:      sync                    /* Wait for all icbi to complete on bus */
 656         isync
 657
 658 /*
 659  * We are done. Do not return, instead branch to second part of board
 660  * initialization, now running from RAM.
 661  */
 662
 663         addi    r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
 664         mtlr    r0
 665         blr
 666
 667 in_ram:
 668
 669         /*
 670          * Relocation Function, r14 point to got2+0x8000
 671          *
 672          * Adjust got2 pointers, no need to check for 0, this code
 673          * already puts a few entries in the table.
 674          */
 675         li      r0,__got2_entries@sectoff@l
 676         la      r3,GOT(_GOT2_TABLE_)
 677         lwz     r11,GOT(_GOT2_TABLE_)
 678         mtctr   r0
 679         sub     r11,r3,r11
 680         addi    r3,r3,-4
 681 1:      lwzu    r0,4(r3)
 682         add     r0,r0,r11
 683         stw     r0,0(r3)
 684         bdnz    1b
 685
 686         /*
 687          * Now adjust the fixups and the pointers to the fixups
 688          * in case we need to move ourselves again.
 689          */
 690 2:      li      r0,__fixup_entries@sectoff@l
 691         lwz     r3,GOT(_FIXUP_TABLE_)
 692         cmpwi   r0,0
 693         mtctr   r0
 694         addi    r3,r3,-4
 695         beq     4f
 696 3:      lwzu    r4,4(r3)
 697         lwzux   r0,r4,r11
 698         add     r0,r0,r11
 699         stw     r10,0(r3)
 700         stw     r0,0(r4)
 701         bdnz    3b
 702 4:
 703 clear_bss:
 704         /*
 705          * Now clear BSS segment
 706          */
 707         lwz     r3,GOT(__bss_start)
 708         lwz     r4,GOT(_end)
 709
 710         cmplw   0, r3, r4
 711         beq     6f
 712
 713         li      r0, 0
 714 5:
 715         stw     r0, 0(r3)
 716         addi    r3, r3, 4
 717         cmplw   0, r3, r4
 718         bne     5b
 719 6:
 720
 721         mr      r3, r9          /* Global Data pointer          */
 722         mr      r4, r10         /* Destination Address          */
 723         bl      board_init_r
 724
 725         /*
 726          * Copy exception vector code to low memory
 727          *
 728          * r3: dest_addr
 729          * r7: source address, r8: end address, r9: target address
 730          */
 731         .globl  trap_init
 732 trap_init:
 733         lwz     r7, GOT(_start)
 734         lwz     r8, GOT(_end_of_vectors)
 735
 736         li      r9, 0x100               /* reset vector always at 0x100 */
 737
 738         cmplw   0, r7, r8
 739         bgelr                           /* return if r7>=r8 - just in case */
 740
 741         mflr    r4                      /* save link register           */
 742 1:
 743         lwz     r0, 0(r7)
 744         stw     r0, 0(r9)
 745         addi    r7, r7, 4
 746         addi    r9, r9, 4
 747         cmplw   0, r7, r8
 748         bne     1b
 749
 750         /*
 751          * relocate `hdlr' and `int_return' entries
 752          */
 753         li      r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
 754         li      r8, Alignment - _start + EXC_OFF_SYS_RESET
 755 2:
 756         bl      trap_reloc
 757         addi    r7, r7, 0x100           /* next exception vector        */
 758         cmplw   0, r7, r8
 759         blt     2b
 760
 761         li      r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
 762         bl      trap_reloc
 763
 764         li      r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
 765         bl      trap_reloc
 766
 767         li      r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
 768         li      r8, SystemCall - _start + EXC_OFF_SYS_RESET
 769 3:
 770         bl      trap_reloc
 771         addi    r7, r7, 0x100           /* next exception vector        */
 772         cmplw   0, r7, r8
 773         blt     3b
 774
 775         li      r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
 776         li      r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
 777 4:
 778         bl      trap_reloc
 779         addi    r7, r7, 0x100           /* next exception vector        */
 780         cmplw   0, r7, r8
 781         blt     4b
 782
 783         mfmsr   r3                      /* now that the vectors have    */
 784         lis     r7, MSR_IP@h            /* relocated into low memory    */
 785         ori     r7, r7, MSR_IP@l        /* MSR[IP] can be turned off    */
 786         andc    r3, r3, r7              /* (if it was on)               */
 787         SYNC                            /* Some chip revs need this... */
 788         mtmsr   r3
 789         SYNC
 790
 791         mtlr    r4                      /* restore link register    */
 792         blr
 793
 794         /*
 795          * Function: relocate entries for one exception vector
 796          */
 797 trap_reloc:
 798         lwz     r0, 0(r7)               /* hdlr ...                     */
 799         add     r0, r0, r3              /*  ... += dest_addr            */
 800         stw     r0, 0(r7)
 801
 802         lwz     r0, 4(r7)               /* int_return ...               */
 803         add     r0, r0, r3              /*  ... += dest_addr            */
 804         stw     r0, 4(r7)
 805
 806         blr