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 <timestamp.h>
  31 #include <version.h>
  32
  33 #define CONFIG_MPC5xxx 1        /* needed for Linux kernel header files */
  34 #define _LINUX_CONFIG_H 1       /* avoid reading Linux autoconf.h file  */
  35
  36 #include <ppc_asm.tmpl>
  37 #include <ppc_defs.h>
  38
  39 #include <asm/cache.h>
  40 #include <asm/mmu.h>
  41
  42 #ifndef  CONFIG_IDENT_STRING
  43 #define  CONFIG_IDENT_STRING ""
  44 #endif
  45
  46 /* We don't want the  MMU yet.
  47 */
  48 #undef  MSR_KERNEL
  49 /* Floating Point enable, Machine Check and Recoverable Interr. */
  50 #ifdef DEBUG
  51 #define MSR_KERNEL (MSR_FP|MSR_RI)
  52 #else
  53 #define MSR_KERNEL (MSR_FP|MSR_ME|MSR_RI)
  54 #endif
  55
  56 /*
  57  * Set up GOT: Global Offset Table
  58  *
  59  * Use r14 to access the GOT
  60  */
  61         START_GOT
  62         GOT_ENTRY(_GOT2_TABLE_)
  63         GOT_ENTRY(_FIXUP_TABLE_)
  64
  65         GOT_ENTRY(_start)
  66         GOT_ENTRY(_start_of_vectors)
  67         GOT_ENTRY(_end_of_vectors)
  68         GOT_ENTRY(transfer_to_handler)
  69
  70         GOT_ENTRY(__init_end)
  71         GOT_ENTRY(_end)
  72         GOT_ENTRY(__bss_start)
  73         END_GOT
  74
  75 /*
  76  * Version string
  77  */
  78         .data
  79         .globl  version_string
  80 version_string:
  81         .ascii U_BOOT_VERSION
  82         .ascii " (", U_BOOT_DATE, " - ", U_BOOT_TIME, ")"
  83         .ascii CONFIG_IDENT_STRING, "\0"
  84
  85 /*
  86  * Exception vectors
  87  */
  88         .text
  89         . = EXC_OFF_SYS_RESET
  90         .globl  _start
  91 _start:
  92         li      r21, BOOTFLAG_COLD      /* Normal Power-On              */
  93         nop
  94         b       boot_cold
  95
  96         . = EXC_OFF_SYS_RESET + 0x10
  97
  98         .globl  _start_warm
  99 _start_warm:
 100         li      r21, BOOTFLAG_WARM      /* Software reboot              */
 101         b       boot_warm
 102
 103 boot_cold:
 104 boot_warm:
 105         mfmsr   r5                      /* save msr contents            */
 106
 107         /* Move CSBoot and adjust instruction pointer                   */
 108         /*--------------------------------------------------------------*/
 109
 110 #if defined(CONFIG_SYS_LOWBOOT)
 111 # if defined(CONFIG_SYS_RAMBOOT)
 112 #  error CONFIG_SYS_LOWBOOT is incompatible with CONFIG_SYS_RAMBOOT
 113 # endif /* CONFIG_SYS_RAMBOOT */
 114 # if defined(CONFIG_MGT5100)
 115 #  error CONFIG_SYS_LOWBOOT is incompatible with MGT5100
 116 # endif /* CONFIG_MGT5100 */
 117         lis     r4, CONFIG_SYS_DEFAULT_MBAR@h
 118         lis     r3,     START_REG(CONFIG_SYS_BOOTCS_START)@h
 119         ori     r3, r3, START_REG(CONFIG_SYS_BOOTCS_START)@l
 120         stw     r3, 0x4(r4)             /* CS0 start */
 121         lis     r3,     STOP_REG(CONFIG_SYS_BOOTCS_START, CONFIG_SYS_BOOTCS_SIZE)@h
 122         ori     r3, r3, STOP_REG(CONFIG_SYS_BOOTCS_START, CONFIG_SYS_BOOTCS_SIZE)@l
 123         stw     r3, 0x8(r4)             /* CS0 stop */
 124         lis     r3,     0x02010000@h
 125         ori     r3, r3, 0x02010000@l
 126         stw     r3, 0x54(r4)            /* CS0 and Boot enable */
 127
 128         lis     r3,     lowboot_reentry@h       /* jump from bootlow address space (0x0000xxxx) */
 129         ori     r3, r3, lowboot_reentry@l       /* to the address space the linker used */
 130         mtlr    r3
 131         blr
 132
 133 lowboot_reentry:
 134         lis     r3,     START_REG(CONFIG_SYS_BOOTCS_START)@h
 135         ori     r3, r3, START_REG(CONFIG_SYS_BOOTCS_START)@l
 136         stw     r3, 0x4c(r4)            /* Boot start */
 137         lis     r3,     STOP_REG(CONFIG_SYS_BOOTCS_START, CONFIG_SYS_BOOTCS_SIZE)@h
 138         ori     r3, r3, STOP_REG(CONFIG_SYS_BOOTCS_START, CONFIG_SYS_BOOTCS_SIZE)@l
 139         stw     r3, 0x50(r4)            /* Boot stop */
 140         lis     r3,     0x02000001@h
 141         ori     r3, r3, 0x02000001@l
 142         stw     r3, 0x54(r4)            /* Boot enable, CS0 disable */
 143 #endif  /* CONFIG_SYS_LOWBOOT */
 144
 145 #if defined(CONFIG_SYS_DEFAULT_MBAR) && !defined(CONFIG_SYS_RAMBOOT)
 146         lis     r3, CONFIG_SYS_MBAR@h
 147         ori     r3, r3, CONFIG_SYS_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, CONFIG_SYS_DEFAULT_MBAR@h
 157         stw     r3, 0(r4)
 158 #endif /* CONFIG_SYS_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, CONFIG_SYS_INIT_RAM_ADDR@h
 170         ori     r3, r3, CONFIG_SYS_INIT_RAM_ADDR@l
 171         ori     r1, r3, CONFIG_SYS_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(SRR0, SRR1)
 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(SRR0, SRR1)
 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, CONFIG_SYS_HID0_INIT@h
 405         ori     r3, r3, CONFIG_SYS_HID0_INIT@l
 406         SYNC
 407         mtspr   HID0, r3
 408
 409         lis     r3, CONFIG_SYS_HID0_FINAL@h
 410         ori     r3, r3, CONFIG_SYS_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_svr
 557 get_svr:
 558         mfspr   r3, SVR
 559         blr
 560
 561         .globl get_pvr
 562 get_pvr:
 563         mfspr   r3, PVR
 564         blr
 565
 566 /*------------------------------------------------------------------------------*/
 567
 568 /*
 569  * void relocate_code (addr_sp, gd, addr_moni)
 570  *
 571  * This "function" does not return, instead it continues in RAM
 572  * after relocating the monitor code.
 573  *
 574  * r3 = dest
 575  * r4 = src
 576  * r5 = length in bytes
 577  * r6 = cachelinesize
 578  */
 579         .globl  relocate_code
 580 relocate_code:
 581         mr      r1,  r3         /* Set new stack pointer                */
 582         mr      r9,  r4         /* Save copy of Global Data pointer     */
 583         mr      r10, r5         /* Save copy of Destination Address     */
 584
 585         mr      r3,  r5                         /* Destination Address  */
 586         lis     r4, CONFIG_SYS_MONITOR_BASE@h           /* Source      Address  */
 587         ori     r4, r4, CONFIG_SYS_MONITOR_BASE@l
 588         lwz     r5, GOT(__init_end)
 589         sub     r5, r5, r4
 590         li      r6, CONFIG_SYS_CACHELINE_SIZE           /* Cache Line Size      */
 591
 592         /*
 593          * Fix GOT pointer:
 594          *
 595          * New GOT-PTR = (old GOT-PTR - CONFIG_SYS_MONITOR_BASE) + Destination Address
 596          *
 597          * Offset:
 598          */
 599         sub     r15, r10, r4
 600
 601         /* First our own GOT */
 602         add     r14, r14, r15
 603         /* then the one used by the C code */
 604         add     r30, r30, r15
 605
 606         /*
 607          * Now relocate code
 608          */
 609
 610         cmplw   cr1,r3,r4
 611         addi    r0,r5,3
 612         srwi.   r0,r0,2
 613         beq     cr1,4f          /* In place copy is not necessary       */
 614         beq     7f              /* Protect against 0 count              */
 615         mtctr   r0
 616         bge     cr1,2f
 617
 618         la      r8,-4(r4)
 619         la      r7,-4(r3)
 620 1:      lwzu    r0,4(r8)
 621         stwu    r0,4(r7)
 622         bdnz    1b
 623         b       4f
 624
 625 2:      slwi    r0,r0,2
 626         add     r8,r4,r0
 627         add     r7,r3,r0
 628 3:      lwzu    r0,-4(r8)
 629         stwu    r0,-4(r7)
 630         bdnz    3b
 631
 632 /*
 633  * Now flush the cache: note that we must start from a cache aligned
 634  * address. Otherwise we might miss one cache line.
 635  */
 636 4:      cmpwi   r6,0
 637         add     r5,r3,r5
 638         beq     7f              /* Always flush prefetch queue in any case */
 639         subi    r0,r6,1
 640         andc    r3,r3,r0
 641         mfspr   r7,HID0         /* don't do dcbst if dcache is disabled */
 642         rlwinm  r7,r7,HID0_DCE_BITPOS+1,31,31
 643         cmpwi   r7,0
 644         beq     9f
 645         mr      r4,r3
 646 5:      dcbst   0,r4
 647         add     r4,r4,r6
 648         cmplw   r4,r5
 649         blt     5b
 650         sync                    /* Wait for all dcbst to complete on bus */
 651 9:      mfspr   r7,HID0         /* don't do icbi if icache is disabled */
 652         rlwinm  r7,r7,HID0_ICE_BITPOS+1,31,31
 653         cmpwi   r7,0
 654         beq     7f
 655         mr      r4,r3
 656 6:      icbi    0,r4
 657         add     r4,r4,r6
 658         cmplw   r4,r5
 659         blt     6b
 660 7:      sync                    /* Wait for all icbi to complete on bus */
 661         isync
 662
 663 /*
 664  * We are done. Do not return, instead branch to second part of board
 665  * initialization, now running from RAM.
 666  */
 667
 668         addi    r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
 669         mtlr    r0
 670         blr
 671
 672 in_ram:
 673
 674         /*
 675          * Relocation Function, r14 point to got2+0x8000
 676          *
 677          * Adjust got2 pointers, no need to check for 0, this code
 678          * already puts a few entries in the table.
 679          */
 680         li      r0,__got2_entries@sectoff@l
 681         la      r3,GOT(_GOT2_TABLE_)
 682         lwz     r11,GOT(_GOT2_TABLE_)
 683         mtctr   r0
 684         sub     r11,r3,r11
 685         addi    r3,r3,-4
 686 1:      lwzu    r0,4(r3)
 687         cmpwi   r0,0
 688         beq-    2f
 689         add     r0,r0,r11
 690         stw     r0,0(r3)
 691 2:      bdnz    1b
 692
 693         /*
 694          * Now adjust the fixups and the pointers to the fixups
 695          * in case we need to move ourselves again.
 696          */
 697         li      r0,__fixup_entries@sectoff@l
 698         lwz     r3,GOT(_FIXUP_TABLE_)
 699         cmpwi   r0,0
 700         mtctr   r0
 701         addi    r3,r3,-4
 702         beq     4f
 703 3:      lwzu    r4,4(r3)
 704         lwzux   r0,r4,r11
 705         add     r0,r0,r11
 706         stw     r10,0(r3)
 707         stw     r0,0(r4)
 708         bdnz    3b
 709 4:
 710 clear_bss:
 711         /*
 712          * Now clear BSS segment
 713          */
 714         lwz     r3,GOT(__bss_start)
 715         lwz     r4,GOT(_end)
 716
 717         cmplw   0, r3, r4
 718         beq     6f
 719
 720         li      r0, 0
 721 5:
 722         stw     r0, 0(r3)
 723         addi    r3, r3, 4
 724         cmplw   0, r3, r4
 725         bne     5b
 726 6:
 727
 728         mr      r3, r9          /* Global Data pointer          */
 729         mr      r4, r10         /* Destination Address          */
 730         bl      board_init_r
 731
 732         /*
 733          * Copy exception vector code to low memory
 734          *
 735          * r3: dest_addr
 736          * r7: source address, r8: end address, r9: target address
 737          */
 738         .globl  trap_init
 739 trap_init:
 740         lwz     r7, GOT(_start)
 741         lwz     r8, GOT(_end_of_vectors)
 742
 743         li      r9, 0x100               /* reset vector always at 0x100 */
 744
 745         cmplw   0, r7, r8
 746         bgelr                           /* return if r7>=r8 - just in case */
 747
 748         mflr    r4                      /* save link register           */
 749 1:
 750         lwz     r0, 0(r7)
 751         stw     r0, 0(r9)
 752         addi    r7, r7, 4
 753         addi    r9, r9, 4
 754         cmplw   0, r7, r8
 755         bne     1b
 756
 757         /*
 758          * relocate `hdlr' and `int_return' entries
 759          */
 760         li      r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
 761         li      r8, Alignment - _start + EXC_OFF_SYS_RESET
 762 2:
 763         bl      trap_reloc
 764         addi    r7, r7, 0x100           /* next exception vector        */
 765         cmplw   0, r7, r8
 766         blt     2b
 767
 768         li      r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
 769         bl      trap_reloc
 770
 771         li      r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
 772         bl      trap_reloc
 773
 774         li      r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
 775         li      r8, SystemCall - _start + EXC_OFF_SYS_RESET
 776 3:
 777         bl      trap_reloc
 778         addi    r7, r7, 0x100           /* next exception vector        */
 779         cmplw   0, r7, r8
 780         blt     3b
 781
 782         li      r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
 783         li      r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
 784 4:
 785         bl      trap_reloc
 786         addi    r7, r7, 0x100           /* next exception vector        */
 787         cmplw   0, r7, r8
 788         blt     4b
 789
 790         mfmsr   r3                      /* now that the vectors have    */
 791         lis     r7, MSR_IP@h            /* relocated into low memory    */
 792         ori     r7, r7, MSR_IP@l        /* MSR[IP] can be turned off    */
 793         andc    r3, r3, r7              /* (if it was on)               */
 794         SYNC                            /* Some chip revs need this... */
 795         mtmsr   r3
 796         SYNC
 797
 798         mtlr    r4                      /* restore link register    */
 799         blr
 800
 801         /*
 802          * Function: relocate entries for one exception vector
 803          */
 804 trap_reloc:
 805         lwz     r0, 0(r7)               /* hdlr ...                     */
 806         add     r0, r0, r3              /*  ... += dest_addr            */
 807         stw     r0, 0(r7)
 808
 809         lwz     r0, 4(r7)               /* int_return ...               */
 810         add     r0, r0, r3              /*  ... += dest_addr            */
 811         stw     r0, 4(r7)
 812
 813         blr