* Patch by Marc Singer, 29 May 2003:
[platform/kernel/u-boot.git] / cpu / mpc824x / 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,2001,2002 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 /* U-Boot - Startup Code for PowerPC based Embedded Boards
26  *
27  *
28  * The processor starts at 0x00000100 and the code is executed
29  * from flash. The code is organized to be at an other address
30  * in memory, but as long we don't jump around before relocating.
31  * board_init lies at a quite high address and when the cpu has
32  * jumped there, everything is ok.
33  * This works because the cpu gives the FLASH (CS0) the whole
34  * address space at startup, and board_init lies as a echo of
35  * the flash somewhere up there in the memorymap.
36  *
37  * board_init will change CS0 to be positioned at the correct
38  * address and (s)dram will be positioned at address 0
39  */
40 #include <config.h>
41 #include <mpc824x.h>
42 #include <version.h>
43
44 #define _LINUX_CONFIG_H 1       /* avoid reading Linux autoconf.h file  */
45
46 #include <ppc_asm.tmpl>
47 #include <ppc_defs.h>
48
49 #include <asm/cache.h>
50 #include <asm/mmu.h>
51
52 #ifndef CONFIG_IDENT_STRING
53 #define CONFIG_IDENT_STRING ""
54 #endif
55
56 /* We don't want the MMU yet.
57 */
58 #undef  MSR_KERNEL
59 /* FP, Machine Check and Recoverable Interr. */
60 #define MSR_KERNEL ( MSR_FP | MSR_ME | MSR_RI )
61
62 /*
63  * Set up GOT: Global Offset Table
64  *
65  * Use r14 to access the GOT
66  */
67         START_GOT
68         GOT_ENTRY(_GOT2_TABLE_)
69         GOT_ENTRY(_FIXUP_TABLE_)
70
71         GOT_ENTRY(_start)
72         GOT_ENTRY(_start_of_vectors)
73         GOT_ENTRY(_end_of_vectors)
74         GOT_ENTRY(transfer_to_handler)
75
76         GOT_ENTRY(__init_end)
77         GOT_ENTRY(_end)
78         GOT_ENTRY(__bss_start)
79 #if defined(CONFIG_FADS)
80         GOT_ENTRY(environment)
81 #endif
82         END_GOT
83
84 /*
85  * r3 - 1st arg to board_init(): IMMP pointer
86  * r4 - 2nd arg to board_init(): boot flag
87  */
88         .text
89         .long   0x27051956              /* U-Boot Magic Number                  */
90         .globl  version_string
91 version_string:
92         .ascii U_BOOT_VERSION
93         .ascii " (", __DATE__, " - ", __TIME__, ")"
94         .ascii CONFIG_IDENT_STRING, "\0"
95
96         . = EXC_OFF_SYS_RESET
97         .globl  _start
98 _start:
99         li      r21, BOOTFLAG_COLD      /* Normal Power-On: Boot from FLASH     */
100         b       boot_cold
101
102         . = EXC_OFF_SYS_RESET + 0x10
103
104         .globl  _start_warm
105 _start_warm:
106         li      r21, BOOTFLAG_WARM      /* Software reboot                      */
107         b       boot_warm
108
109 boot_cold:
110 boot_warm:
111
112         /* Initialize machine status; enable machine check interrupt            */
113         /*----------------------------------------------------------------------*/
114         li      r3, MSR_KERNEL          /* Set FP, ME, RI flags */
115         mtmsr   r3
116         mtspr   SRR1, r3                /* Make SRR1 match MSR */
117
118         addis   r0,0,0x0000             /* lets make sure that r0 is really 0 */
119         mtspr   HID0, r0                /* disable I and D caches */
120
121         mfspr   r3, ICR                 /* clear Interrupt Cause Register */
122
123         mfmsr   r3                      /* turn off address translation */
124         addis   r4,0,0xffff
125         ori     r4,r4,0xffcf
126         and     r3,r3,r4
127         mtmsr   r3
128         isync
129         sync                            /* the MMU should be off... */
130
131
132 in_flash:
133 #if defined(CONFIG_BMW)
134         bl early_init_f /* Must be ASM: no stack yet! */
135 #endif
136         /*
137          * Setup BATs - cannot be done in C since we don't have a stack yet
138          */
139         bl      setup_bats
140
141         /* Enable MMU.
142          */
143         mfmsr   r3
144         ori     r3, r3, (MSR_IR | MSR_DR)
145         mtmsr   r3
146 #if !defined(CONFIG_BMW)
147         /* Enable and invalidate data cache.
148          */
149         mfspr   r3, HID0
150         mr      r2, r3
151         ori     r3, r3, HID0_DCE | HID0_DCI
152         ori     r2, r2, HID0_DCE
153         sync
154         mtspr   HID0, r3
155         mtspr   HID0, r2
156         sync
157
158         /* Allocate Initial RAM in data cache.
159          */
160         lis     r3, CFG_INIT_RAM_ADDR@h
161         ori     r3, r3, CFG_INIT_RAM_ADDR@l
162         li      r2, 128
163         mtctr   r2
164 1:
165         dcbz    r0, r3
166         addi    r3, r3, 32
167         bdnz    1b
168
169         /* Lock way0 in data cache.
170          */
171         mfspr   r3, 1011
172         lis     r2, 0xffff
173         ori     r2, r2, 0xff1f
174         and     r3, r3, r2
175         ori     r3, r3, 0x0080
176         sync
177         mtspr   1011, r3
178 #endif /* !CONFIG_BMW */
179         /*
180          * Thisk the stack pointer *somewhere* sensible. Doesnt
181          * matter much where as we'll move it when we relocate
182          */
183         lis     r1, (CFG_INIT_RAM_ADDR + CFG_GBL_DATA_OFFSET)@h
184         ori     r1, r1, (CFG_INIT_RAM_ADDR + CFG_GBL_DATA_OFFSET)@l
185
186         li      r0, 0                   /* Make room for stack frame header and */
187         stwu    r0, -4(r1)              /* clear final stack frame so that      */
188         stwu    r0, -4(r1)              /* stack backtraces terminate cleanly   */
189
190         /* let the C-code set up the rest                                       */
191         /*                                                                      */
192         /* Be careful to keep code relocatable !                                */
193         /*----------------------------------------------------------------------*/
194
195         GET_GOT                 /* initialize GOT access                        */
196
197         /* r3: IMMR */
198         bl      cpu_init_f      /* run low-level CPU init code     (from Flash) */
199
200         mr      r3, r21
201         /* r3: BOOTFLAG */
202         bl      board_init_f    /* run 1st part of board init code (from Flash) */
203
204
205
206         .globl  _start_of_vectors
207 _start_of_vectors:
208
209 /* Machine check */
210         STD_EXCEPTION(EXC_OFF_MACH_CHCK, MachineCheck, MachineCheckException)
211
212 /* Data Storage exception.  "Never" generated on the 860. */
213         STD_EXCEPTION(EXC_OFF_DATA_STOR, DataStorage, UnknownException)
214
215 /* Instruction Storage exception.  "Never" generated on the 860. */
216         STD_EXCEPTION(EXC_OFF_INS_STOR, InstStorage, UnknownException)
217
218 /* External Interrupt exception. */
219         STD_EXCEPTION(EXC_OFF_EXTERNAL, ExtInterrupt, external_interrupt)
220
221 /* Alignment exception. */
222         . = EXC_OFF_ALIGN
223 Alignment:
224         EXCEPTION_PROLOG
225         mfspr   r4,DAR
226         stw     r4,_DAR(r21)
227         mfspr   r5,DSISR
228         stw     r5,_DSISR(r21)
229         addi    r3,r1,STACK_FRAME_OVERHEAD
230         li      r20,MSR_KERNEL
231         rlwimi  r20,r23,0,16,16         /* copy EE bit from saved MSR */
232         lwz     r6,GOT(transfer_to_handler)
233         mtlr    r6
234         blrl
235 .L_Alignment:
236         .long   AlignmentException - _start + EXC_OFF_SYS_RESET
237         .long   int_return - _start + EXC_OFF_SYS_RESET
238
239 /* Program check exception */
240         . = EXC_OFF_PROGRAM
241 ProgramCheck:
242         EXCEPTION_PROLOG
243         addi    r3,r1,STACK_FRAME_OVERHEAD
244         li      r20,MSR_KERNEL
245         rlwimi  r20,r23,0,16,16         /* copy EE bit from saved MSR */
246         lwz     r6,GOT(transfer_to_handler)
247         mtlr    r6
248         blrl
249 .L_ProgramCheck:
250         .long   ProgramCheckException - _start + EXC_OFF_SYS_RESET
251         .long   int_return - _start + EXC_OFF_SYS_RESET
252
253         /* No FPU on MPC8xx. This exception is not supposed to happen.
254         */
255         STD_EXCEPTION(EXC_OFF_FPUNAVAIL, FPUnavailable, UnknownException)
256
257         /* I guess we could implement decrementer, and may have
258          * to someday for timekeeping.
259          */
260         STD_EXCEPTION(EXC_OFF_DECR, Decrementer, timer_interrupt)
261         STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
262         STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
263
264         . = 0xc00
265 /*
266  * r0 - SYSCALL number
267  * r3-... arguments
268  */
269 SystemCall:
270         addis   r11,r0,0                /* get functions table addr */
271         ori     r11,r11,0               /* Note: this code is patched in trap_init */
272         addis   r12,r0,0                /* get number of functions */
273         ori     r12,r12,0
274
275         cmplw   0, r0, r12
276         bge     1f
277
278         rlwinm  r0,r0,2,0,31            /* fn_addr = fn_tbl[r0] */
279         add     r11,r11,r0
280         lwz     r11,0(r11)
281
282         li      r20,0xd00-4             /* Get stack pointer */
283         lwz     r12,0(r20)
284         subi    r12,r12,12              /* Adjust stack pointer */
285         li      r0,0xc00+_end_back-SystemCall
286         cmplw   0, r0, r12              /* Check stack overflow */
287         bgt     1f
288         stw     r12,0(r20)
289
290         mflr    r0
291         stw     r0,0(r12)
292         mfspr   r0,SRR0
293         stw     r0,4(r12)
294         mfspr   r0,SRR1
295         stw     r0,8(r12)
296
297         li      r12,0xc00+_back-SystemCall
298         mtlr    r12
299         mtspr   SRR0,r11
300
301 1:      SYNC
302         rfi
303
304 _back:
305
306         mfmsr   r11                     /* Disable interrupts */
307         li      r12,0
308         ori     r12,r12,MSR_EE
309         andc    r11,r11,r12
310         SYNC                            /* Some chip revs need this... */
311         mtmsr   r11
312         SYNC
313
314         li      r12,0xd00-4             /* restore regs */
315         lwz     r12,0(r12)
316
317         lwz     r11,0(r12)
318         mtlr    r11
319         lwz     r11,4(r12)
320         mtspr   SRR0,r11
321         lwz     r11,8(r12)
322         mtspr   SRR1,r11
323
324         addi    r12,r12,12              /* Adjust stack pointer */
325         li      r20,0xd00-4
326         stw     r12,0(r20)
327
328         SYNC
329         rfi
330 _end_back:
331
332         STD_EXCEPTION(EXC_OFF_TRACE, SingleStep, UnknownException)
333
334         STD_EXCEPTION(EXC_OFF_FPUNASSIST, Trap_0e, UnknownException)
335         STD_EXCEPTION(EXC_OFF_PMI, Trap_0f, UnknownException)
336
337         STD_EXCEPTION(EXC_OFF_ITME, InstructionTransMiss, UnknownException)
338         STD_EXCEPTION(EXC_OFF_DLTME, DataLoadTransMiss, UnknownException)
339         STD_EXCEPTION(EXC_OFF_DSTME, DataStoreTransMiss, UnknownException)
340         STD_EXCEPTION(EXC_OFF_IABE, InstructionBreakpoint, DebugException)
341         STD_EXCEPTION(EXC_OFF_SMIE, SysManageInt, UnknownException)
342         STD_EXCEPTION(0x1500, Reserved5, UnknownException)
343         STD_EXCEPTION(0x1600, Reserved6, UnknownException)
344         STD_EXCEPTION(0x1700, Reserved7, UnknownException)
345         STD_EXCEPTION(0x1800, Reserved8, UnknownException)
346         STD_EXCEPTION(0x1900, Reserved9, UnknownException)
347         STD_EXCEPTION(0x1a00, ReservedA, UnknownException)
348         STD_EXCEPTION(0x1b00, ReservedB, UnknownException)
349         STD_EXCEPTION(0x1c00, ReservedC, UnknownException)
350         STD_EXCEPTION(0x1d00, ReservedD, UnknownException)
351         STD_EXCEPTION(0x1e00, ReservedE, UnknownException)
352         STD_EXCEPTION(0x1f00, ReservedF, UnknownException)
353
354         STD_EXCEPTION(EXC_OFF_RMTE, RunModeTrace, UnknownException)
355
356         .globl  _end_of_vectors
357 _end_of_vectors:
358
359
360         . = 0x3000
361
362 /*
363  * This code finishes saving the registers to the exception frame
364  * and jumps to the appropriate handler for the exception.
365  * Register r21 is pointer into trap frame, r1 has new stack pointer.
366  */
367         .globl  transfer_to_handler
368 transfer_to_handler:
369         stw     r22,_NIP(r21)
370         lis     r22,MSR_POW@h
371         andc    r23,r23,r22
372         stw     r23,_MSR(r21)
373         SAVE_GPR(7, r21)
374         SAVE_4GPRS(8, r21)
375         SAVE_8GPRS(12, r21)
376         SAVE_8GPRS(24, r21)
377 #if 0
378         andi.   r23,r23,MSR_PR
379         mfspr   r23,SPRG3               /* if from user, fix up tss.regs */
380         beq     2f
381         addi    r24,r1,STACK_FRAME_OVERHEAD
382         stw     r24,PT_REGS(r23)
383 2:      addi    r2,r23,-TSS             /* set r2 to current */
384         tovirt(r2,r2,r23)
385 #endif
386         mflr    r23
387         andi.   r24,r23,0x3f00          /* get vector offset */
388         stw     r24,TRAP(r21)
389         li      r22,0
390         stw     r22,RESULT(r21)
391         mtspr   SPRG2,r22               /* r1 is now kernel sp */
392 #if 0
393         addi    r24,r2,TASK_STRUCT_SIZE /* check for kernel stack overflow */
394         cmplw   0,r1,r2
395         cmplw   1,r1,r24
396         crand   1,1,4
397         bgt     stack_ovf               /* if r2 < r1 < r2+TASK_STRUCT_SIZE */
398 #endif
399         lwz     r24,0(r23)              /* virtual address of handler */
400         lwz     r23,4(r23)              /* where to go when done */
401         mtspr   SRR0,r24
402         ori     r20,r20,0x30            /* enable IR, DR */
403         mtspr   SRR1,r20
404         mtlr    r23
405         SYNC
406         rfi                             /* jump to handler, enable MMU */
407
408 int_return:
409         mfmsr   r28             /* Disable interrupts */
410         li      r4,0
411         ori     r4,r4,MSR_EE
412         andc    r28,r28,r4
413         SYNC                    /* Some chip revs need this... */
414         mtmsr   r28
415         SYNC
416         lwz     r2,_CTR(r1)
417         lwz     r0,_LINK(r1)
418         mtctr   r2
419         mtlr    r0
420         lwz     r2,_XER(r1)
421         lwz     r0,_CCR(r1)
422         mtspr   XER,r2
423         mtcrf   0xFF,r0
424         REST_10GPRS(3, r1)
425         REST_10GPRS(13, r1)
426         REST_8GPRS(23, r1)
427         REST_GPR(31, r1)
428         lwz     r2,_NIP(r1)     /* Restore environment */
429         lwz     r0,_MSR(r1)
430         mtspr   SRR0,r2
431         mtspr   SRR1,r0
432         lwz     r0,GPR0(r1)
433         lwz     r2,GPR2(r1)
434         lwz     r1,GPR1(r1)
435         SYNC
436         rfi
437
438 /* Cache functions.
439 */
440         .globl  icache_enable
441 icache_enable:
442         mfspr   r5,HID0         /* turn on the I cache. */
443         ori     r5,r5,0x8800    /* Instruction cache only! */
444         addis   r6,0,0xFFFF
445         ori     r6,r6,0xF7FF
446         and     r6,r5,r6        /* clear the invalidate bit */
447         sync
448         mtspr   HID0,r5
449         mtspr   HID0,r6
450         isync
451         sync
452         blr
453
454         .globl  icache_disable
455 icache_disable:
456         mfspr   r5,HID0
457         addis   r6,0,0xFFFF
458         ori     r6,r6,0x7FFF
459         and     r5,r5,r6
460         sync
461         mtspr   HID0,r5
462         isync
463         sync
464         blr
465
466         .globl  icache_status
467 icache_status:
468         mfspr   r3, HID0
469         srwi    r3, r3, 15      /* >>15 & 1=> select bit 16 */
470         andi.   r3, r3, 1
471         blr
472
473         .globl  dcache_enable
474 dcache_enable:
475         mfspr   r5,HID0         /* turn on the D cache. */
476         ori     r5,r5,0x4400    /* Data cache only! */
477         mfspr   r4, PVR         /* read PVR */
478         srawi   r3, r4, 16      /* shift off the least 16 bits */
479         cmpi    0, 0, r3, 0xC   /* Check for Max pvr */
480         bne     NotMax
481         ori     r5,r5,0x0040    /* setting the DCFA bit, for Max rev 1 errata */
482 NotMax:
483         addis   r6,0,0xFFFF
484         ori     r6,r6,0xFBFF
485         and     r6,r5,r6        /* clear the invalidate bit */
486         sync
487         mtspr   HID0,r5
488         mtspr   HID0,r6
489         isync
490         sync
491         blr
492
493         .globl  dcache_disable
494 dcache_disable:
495         mfspr   r5,HID0
496         addis   r6,0,0xFFFF
497         ori     r6,r6,0xBFFF
498         and     r5,r5,r6
499         sync
500         mtspr   HID0,r5
501         isync
502         sync
503         blr
504
505         .globl  dcache_status
506 dcache_status:
507         mfspr   r3, HID0
508         srwi    r3, r3, 14      /* >>14 & 1=> select bit 17 */
509         andi.   r3, r3, 1
510         blr
511
512         .globl  dc_read
513 dc_read:
514 /*TODO : who uses this, what should it do?
515 */
516         blr
517
518
519         .globl get_pvr
520 get_pvr:
521         mfspr   r3, PVR
522         blr
523
524
525 /*------------------------------------------------------------------------------*/
526
527 /*
528  * void relocate_code (addr_sp, gd, addr_moni)
529  *
530  * This "function" does not return, instead it continues in RAM
531  * after relocating the monitor code.
532  *
533  * r3 = dest
534  * r4 = src
535  * r5 = length in bytes
536  * r6 = cachelinesize
537  */
538         .globl  relocate_code
539 relocate_code:
540
541         mr      r1,  r3         /* Set new stack pointer                */
542         mr      r9,  r4         /* Save copy of Global Data pointer     */
543         mr      r10, r5         /* Save copy of Destination Address     */
544
545         mr      r3,  r5                         /* Destination Address  */
546 #ifdef CFG_RAMBOOT
547         lis     r4, CFG_SDRAM_BASE@h            /* Source      Address  */
548         ori     r4, r4, CFG_SDRAM_BASE@l
549 #else
550         lis     r4, CFG_MONITOR_BASE@h          /* Source      Address  */
551         ori     r4, r4, CFG_MONITOR_BASE@l
552 #endif
553         lwz     r5, GOT(__init_end)
554         sub     r5, r5, r4
555         li      r6, CFG_CACHELINE_SIZE          /* Cache Line Size      */
556
557         /*
558          * Fix GOT pointer:
559          *
560          * New GOT-PTR = (old GOT-PTR - CFG_MONITOR_BASE) + Destination Address
561          *
562          * Offset:
563          */
564         sub     r15, r10, r4
565
566         /* First our own GOT */
567         add     r14, r14, r15
568         /* the the one used by the C code */
569         add     r30, r30, r15
570
571         /*
572          * Now relocate code
573          */
574
575         cmplw   cr1,r3,r4
576         addi    r0,r5,3
577         srwi.   r0,r0,2
578         beq     cr1,4f          /* In place copy is not necessary       */
579         beq     7f              /* Protect against 0 count              */
580         mtctr   r0
581         bge     cr1,2f
582
583         la      r8,-4(r4)
584         la      r7,-4(r3)
585 1:      lwzu    r0,4(r8)
586         stwu    r0,4(r7)
587         bdnz    1b
588         b       4f
589
590 2:      slwi    r0,r0,2
591         add     r8,r4,r0
592         add     r7,r3,r0
593 3:      lwzu    r0,-4(r8)
594         stwu    r0,-4(r7)
595         bdnz    3b
596
597 /*
598  * Now flush the cache: note that we must start from a cache aligned
599  * address. Otherwise we might miss one cache line.
600  */
601 4:      cmpwi   r6,0
602         add     r5,r3,r5
603         beq     7f              /* Always flush prefetch queue in any case */
604         subi    r0,r6,1
605         andc    r3,r3,r0
606         mr      r4,r3
607 5:      dcbst   0,r4
608         add     r4,r4,r6
609         cmplw   r4,r5
610         blt     5b
611         sync                    /* Wait for all dcbst to complete on bus */
612         mr      r4,r3
613 6:      icbi    0,r4
614         add     r4,r4,r6
615         cmplw   r4,r5
616         blt     6b
617 7:      sync                    /* Wait for all icbi to complete on bus */
618         isync
619
620 /*
621  * We are done. Do not return, instead branch to second part of board
622  * initialization, now running from RAM.
623  */
624
625         addi    r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
626         mtlr    r0
627         blr
628
629 in_ram:
630
631         /*
632          * Relocation Function, r14 point to got2+0x8000
633          *
634          * Adjust got2 pointers, no need to check for 0, this code
635          * already puts a few entries in the table.
636          */
637         li      r0,__got2_entries@sectoff@l
638         la      r3,GOT(_GOT2_TABLE_)
639         lwz     r11,GOT(_GOT2_TABLE_)
640         mtctr   r0
641         sub     r11,r3,r11
642         addi    r3,r3,-4
643 1:      lwzu    r0,4(r3)
644         add     r0,r0,r11
645         stw     r0,0(r3)
646         bdnz    1b
647
648         /*
649          * Now adjust the fixups and the pointers to the fixups
650          * in case we need to move ourselves again.
651          */
652 2:      li      r0,__fixup_entries@sectoff@l
653         lwz     r3,GOT(_FIXUP_TABLE_)
654         cmpwi   r0,0
655         mtctr   r0
656         addi    r3,r3,-4
657         beq     4f
658 3:      lwzu    r4,4(r3)
659         lwzux   r0,r4,r11
660         add     r0,r0,r11
661         stw     r10,0(r3)
662         stw     r0,0(r4)
663         bdnz    3b
664 4:
665 clear_bss:
666         /*
667          * Now clear BSS segment
668          */
669         lwz     r3,GOT(__bss_start)
670         lwz     r4,GOT(_end)
671
672         cmplw   0, r3, r4
673         beq     6f
674
675         li      r0, 0
676 5:
677         stw     r0, 0(r3)
678         addi    r3, r3, 4
679         cmplw   0, r3, r4
680         blt     5b
681 6:
682
683         mr      r3, r9          /* Global Data pointer          */
684         mr      r4, r10         /* Destination Address          */
685         bl      board_init_r
686
687         /*
688          * Copy exception vector code to low memory
689          *
690          * r3: dest_addr
691          * r7: source address, r8: end address, r9: target address
692          */
693         .globl  trap_init
694 trap_init:
695         lwz     r7, GOT(_start)
696         lwz     r8, GOT(_end_of_vectors)
697
698         rlwinm  r9, r7, 0, 18, 31       /* _start & 0x3FFF      */
699
700         cmplw   0, r7, r8
701         bgelr                           /* return if r7>=r8 - just in case */
702
703         mflr    r4                      /* save link register           */
704 1:
705         lwz     r0, 0(r7)
706         stw     r0, 0(r9)
707         addi    r7, r7, 4
708         addi    r9, r9, 4
709         cmplw   0, r7, r8
710         bne     1b
711
712         /*
713          * relocate `hdlr' and `int_return' entries
714          */
715         li      r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
716         li      r8, Alignment - _start + EXC_OFF_SYS_RESET
717 2:
718         bl      trap_reloc
719         addi    r7, r7, 0x100           /* next exception vector        */
720         cmplw   0, r7, r8
721         blt     2b
722
723         li      r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
724         bl      trap_reloc
725
726         li      r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
727         bl      trap_reloc
728
729         li      r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
730         li      r8, SystemCall - _start + EXC_OFF_SYS_RESET
731 3:
732         bl      trap_reloc
733         addi    r7, r7, 0x100           /* next exception vector        */
734         cmplw   0, r7, r8
735         blt     3b
736
737         li      r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
738         li      r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
739 4:
740         bl      trap_reloc
741         addi    r7, r7, 0x100           /* next exception vector        */
742         cmplw   0, r7, r8
743         blt     4b
744
745         mtlr    r4                      /* restore link register        */
746         blr
747
748         /*
749          * Function: relocate entries for one exception vector
750          */
751 trap_reloc:
752         lwz     r0, 0(r7)               /* hdlr ...                     */
753         add     r0, r0, r3              /*  ... += dest_addr            */
754         stw     r0, 0(r7)
755
756         lwz     r0, 4(r7)               /* int_return ...               */
757         add     r0, r0, r3              /*  ... += dest_addr            */
758         stw     r0, 4(r7)
759
760         blr
761
762         /* Setup the BAT registers.
763          */
764 setup_bats:
765         lis     r4, CFG_IBAT0L@h
766         ori     r4, r4, CFG_IBAT0L@l
767         lis     r3, CFG_IBAT0U@h
768         ori     r3, r3, CFG_IBAT0U@l
769         mtspr   IBAT0L, r4
770         mtspr   IBAT0U, r3
771         isync
772
773         lis     r4, CFG_DBAT0L@h
774         ori     r4, r4, CFG_DBAT0L@l
775         lis     r3, CFG_DBAT0U@h
776         ori     r3, r3, CFG_DBAT0U@l
777         mtspr   DBAT0L, r4
778         mtspr   DBAT0U, r3
779         isync
780
781         lis     r4, CFG_IBAT1L@h
782         ori     r4, r4, CFG_IBAT1L@l
783         lis     r3, CFG_IBAT1U@h
784         ori     r3, r3, CFG_IBAT1U@l
785         mtspr   IBAT1L, r4
786         mtspr   IBAT1U, r3
787         isync
788
789         lis     r4, CFG_DBAT1L@h
790         ori     r4, r4, CFG_DBAT1L@l
791         lis     r3, CFG_DBAT1U@h
792         ori     r3, r3, CFG_DBAT1U@l
793         mtspr   DBAT1L, r4
794         mtspr   DBAT1U, r3
795         isync
796
797         lis     r4, CFG_IBAT2L@h
798         ori     r4, r4, CFG_IBAT2L@l
799         lis     r3, CFG_IBAT2U@h
800         ori     r3, r3, CFG_IBAT2U@l
801         mtspr   IBAT2L, r4
802         mtspr   IBAT2U, r3
803         isync
804
805         lis     r4, CFG_DBAT2L@h
806         ori     r4, r4, CFG_DBAT2L@l
807         lis     r3, CFG_DBAT2U@h
808         ori     r3, r3, CFG_DBAT2U@l
809         mtspr   DBAT2L, r4
810         mtspr   DBAT2U, r3
811         isync
812
813         lis     r4, CFG_IBAT3L@h
814         ori     r4, r4, CFG_IBAT3L@l
815         lis     r3, CFG_IBAT3U@h
816         ori     r3, r3, CFG_IBAT3U@l
817         mtspr   IBAT3L, r4
818         mtspr   IBAT3U, r3
819         isync
820
821         lis     r4, CFG_DBAT3L@h
822         ori     r4, r4, CFG_DBAT3L@l
823         lis     r3, CFG_DBAT3U@h
824         ori     r3, r3, CFG_DBAT3U@l
825         mtspr   DBAT3L, r4
826         mtspr   DBAT3U, r3
827         isync
828
829         /* Invalidate TLBs.
830          * -> for (val = 0; val < 0x20000; val+=0x1000)
831          * ->   tlbie(val);
832          */
833         lis     r3, 0
834         lis     r5, 2
835
836 1:
837         tlbie   r3
838         addi    r3, r3, 0x1000
839         cmp     0, 0, r3, r5
840         blt     1b
841
842         blr
843
844