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 MPC8220 CPUs
27 */
28#include <config.h>
29#include <mpc8220.h>
30#include <timestamp.h>
31#include <version.h>
32
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 r12 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
79version_string:
80	.ascii U_BOOT_VERSION
81	.ascii " (", U_BOOT_DATE, " - ", U_BOOT_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
102boot_cold:
103boot_warm:
104	mfmsr	r5		    /* save msr contents    */
105
106	/* replace default MBAR base address from 0x80000000
107	    to 0xf0000000 */
108
109#if defined(CONFIG_SYS_DEFAULT_MBAR) && !defined(CONFIG_SYS_RAMBOOT)
110	lis	r3, CONFIG_SYS_MBAR@h
111	ori	r3, r3, CONFIG_SYS_MBAR@l
112
113	/* MBAR is mirrored into the MBAR SPR */
114	mtspr	MBAR,r3
115	mtspr	SPRN_SPRG7W,r3
116	lis	r4, CONFIG_SYS_DEFAULT_MBAR@h
117	stw	r3, 0(r4)
118#endif /* CONFIG_SYS_DEFAULT_MBAR */
119
120	/* Initialise the MPC8220 processor core			*/
121	/*--------------------------------------------------------------*/
122
123	bl  init_8220_core
124
125	/* initialize some things that are hard to access from C	*/
126	/*--------------------------------------------------------------*/
127
128	/* set up stack in on-chip SRAM */
129	lis	r3, CONFIG_SYS_INIT_RAM_ADDR@h
130	ori	r3, r3, CONFIG_SYS_INIT_RAM_ADDR@l
131	ori	r1, r3, CONFIG_SYS_INIT_SP_OFFSET
132
133	li	r0, 0		/* Make room for stack frame header and */
134	stwu	r0, -4(r1)	/* clear final stack frame so that	*/
135	stwu	r0, -4(r1)	/* stack backtraces terminate cleanly	*/
136
137	/* let the C-code set up the rest				*/
138	/*								*/
139	/* Be careful to keep code relocatable !			*/
140	/*--------------------------------------------------------------*/
141
142	GET_GOT			/* initialize GOT access		*/
143
144	/* r3: IMMR */
145	bl	cpu_init_f	/* run low-level CPU init code (in Flash)*/
146
147	mr	r3, r21
148	/* r3: BOOTFLAG */
149	bl	board_init_f	/* run 1st part of board init code (in Flash)*/
150
151/*
152 * Vector Table
153 */
154
155	.globl	_start_of_vectors
156_start_of_vectors:
157
158/* Machine check */
159	STD_EXCEPTION(0x200, MachineCheck, MachineCheckException)
160
161/* Data Storage exception. */
162	STD_EXCEPTION(0x300, DataStorage, UnknownException)
163
164/* Instruction Storage exception. */
165	STD_EXCEPTION(0x400, InstStorage, UnknownException)
166
167/* External Interrupt exception. */
168	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
169
170/* Alignment exception. */
171	. = 0x600
172Alignment:
173	EXCEPTION_PROLOG(SRR0, SRR1)
174	mfspr	r4,DAR
175	stw	r4,_DAR(r21)
176	mfspr	r5,DSISR
177	stw	r5,_DSISR(r21)
178	addi	r3,r1,STACK_FRAME_OVERHEAD
179	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
180
181/* Program check exception */
182	. = 0x700
183ProgramCheck:
184	EXCEPTION_PROLOG(SRR0, SRR1)
185	addi	r3,r1,STACK_FRAME_OVERHEAD
186	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
187		MSR_KERNEL, COPY_EE)
188
189	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
190
191	/* I guess we could implement decrementer, and may have
192	 * to someday for timekeeping.
193	 */
194	STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
195
196	STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
197	STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
198	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
199	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
200
201	STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
202	STD_EXCEPTION(0xf00, Trap_0f, UnknownException)
203
204	STD_EXCEPTION(0x1000, InstructionTLBMiss, UnknownException)
205	STD_EXCEPTION(0x1100, DataLoadTLBMiss, UnknownException)
206	STD_EXCEPTION(0x1200, DataStoreTLBMiss, UnknownException)
207#ifdef DEBUG
208	. = 0x1300
209	/*
210	 * This exception occurs when the program counter matches the
211	 * Instruction Address Breakpoint Register (IABR).
212	 *
213	 * I want the cpu to halt if this occurs so I can hunt around
214	 * with the debugger and look at things.
215	 *
216	 * When DEBUG is defined, both machine check enable (in the MSR)
217	 * and checkstop reset enable (in the reset mode register) are
218	 * turned off and so a checkstop condition will result in the cpu
219	 * halting.
220	 *
221	 * I force the cpu into a checkstop condition by putting an illegal
222	 * instruction here (at least this is the theory).
223	 *
224	 * well - that didnt work, so just do an infinite loop!
225	 */
2261:	b	1b
227#else
228	STD_EXCEPTION(0x1300, InstructionBreakpoint, DebugException)
229#endif
230	STD_EXCEPTION(0x1400, SMI, UnknownException)
231
232	STD_EXCEPTION(0x1500, Trap_15, UnknownException)
233	STD_EXCEPTION(0x1600, Trap_16, UnknownException)
234	STD_EXCEPTION(0x1700, Trap_17, UnknownException)
235	STD_EXCEPTION(0x1800, Trap_18, UnknownException)
236	STD_EXCEPTION(0x1900, Trap_19, UnknownException)
237	STD_EXCEPTION(0x1a00, Trap_1a, UnknownException)
238	STD_EXCEPTION(0x1b00, Trap_1b, UnknownException)
239	STD_EXCEPTION(0x1c00, Trap_1c, UnknownException)
240	STD_EXCEPTION(0x1d00, Trap_1d, UnknownException)
241	STD_EXCEPTION(0x1e00, Trap_1e, UnknownException)
242	STD_EXCEPTION(0x1f00, Trap_1f, UnknownException)
243	STD_EXCEPTION(0x2000, Trap_20, UnknownException)
244	STD_EXCEPTION(0x2100, Trap_21, UnknownException)
245	STD_EXCEPTION(0x2200, Trap_22, UnknownException)
246	STD_EXCEPTION(0x2300, Trap_23, UnknownException)
247	STD_EXCEPTION(0x2400, Trap_24, UnknownException)
248	STD_EXCEPTION(0x2500, Trap_25, UnknownException)
249	STD_EXCEPTION(0x2600, Trap_26, UnknownException)
250	STD_EXCEPTION(0x2700, Trap_27, UnknownException)
251	STD_EXCEPTION(0x2800, Trap_28, UnknownException)
252	STD_EXCEPTION(0x2900, Trap_29, UnknownException)
253	STD_EXCEPTION(0x2a00, Trap_2a, UnknownException)
254	STD_EXCEPTION(0x2b00, Trap_2b, UnknownException)
255	STD_EXCEPTION(0x2c00, Trap_2c, UnknownException)
256	STD_EXCEPTION(0x2d00, Trap_2d, UnknownException)
257	STD_EXCEPTION(0x2e00, Trap_2e, UnknownException)
258	STD_EXCEPTION(0x2f00, Trap_2f, UnknownException)
259
260
261	.globl	_end_of_vectors
262_end_of_vectors:
263
264	. = 0x3000
265
266/*
267 * This code finishes saving the registers to the exception frame
268 * and jumps to the appropriate handler for the exception.
269 * Register r21 is pointer into trap frame, r1 has new stack pointer.
270 */
271	.globl	transfer_to_handler
272transfer_to_handler:
273	stw	r22,_NIP(r21)
274	lis	r22,MSR_POW@h
275	andc	r23,r23,r22
276	stw	r23,_MSR(r21)
277	SAVE_GPR(7, r21)
278	SAVE_4GPRS(8, r21)
279	SAVE_8GPRS(12, r21)
280	SAVE_8GPRS(24, r21)
281	mflr	r23
282	andi.	r24,r23,0x3f00	    /* get vector offset */
283	stw	r24,TRAP(r21)
284	li	r22,0
285	stw	r22,RESULT(r21)
286	lwz	r24,0(r23)		/* virtual address of handler */
287	lwz	r23,4(r23)		/* where to go when done */
288	mtspr	SRR0,r24
289	mtspr	SRR1,r20
290	mtlr	r23
291	SYNC
292	rfi			    /* jump to handler, enable MMU */
293
294int_return:
295	mfmsr	r28		    /* Disable interrupts */
296	li	r4,0
297	ori	r4,r4,MSR_EE
298	andc	r28,r28,r4
299	SYNC			    /* Some chip revs need this... */
300	mtmsr	r28
301	SYNC
302	lwz	r2,_CTR(r1)
303	lwz	r0,_LINK(r1)
304	mtctr	r2
305	mtlr	r0
306	lwz	r2,_XER(r1)
307	lwz	r0,_CCR(r1)
308	mtspr	XER,r2
309	mtcrf	0xFF,r0
310	REST_10GPRS(3, r1)
311	REST_10GPRS(13, r1)
312	REST_8GPRS(23, r1)
313	REST_GPR(31, r1)
314	lwz	r2,_NIP(r1)	    /* Restore environment */
315	lwz	r0,_MSR(r1)
316	mtspr	SRR0,r2
317	mtspr	SRR1,r0
318	lwz	r0,GPR0(r1)
319	lwz	r2,GPR2(r1)
320	lwz	r1,GPR1(r1)
321	SYNC
322	rfi
323
324/*
325 * This code initialises the MPC8220 processor core
326 * (conforms to PowerPC 603e spec)
327 * Note: expects original MSR contents to be in r5.
328 */
329
330	.globl	init_8220_core
331init_8220_core:
332
333	/* Initialize machine status; enable machine check interrupt	*/
334	/*--------------------------------------------------------------*/
335
336	li	r3, MSR_KERNEL	    /* Set ME and RI flags		*/
337	rlwimi	r3, r5, 0, 25, 25   /* preserve IP bit set by HRCW	*/
338#ifdef DEBUG
339	rlwimi	r3, r5, 0, 21, 22   /* debugger might set SE & BE bits	*/
340#endif
341	SYNC			    /* Some chip revs need this...	*/
342	mtmsr	r3
343	SYNC
344	mtspr	SRR1, r3	    /* Make SRR1 match MSR		*/
345
346	/* Initialize the Hardware Implementation-dependent Registers	*/
347	/* HID0 also contains cache control				*/
348	/*--------------------------------------------------------------*/
349
350	lis	r3, CONFIG_SYS_HID0_INIT@h
351	ori	r3, r3, CONFIG_SYS_HID0_INIT@l
352	SYNC
353	mtspr	HID0, r3
354
355	lis	r3, CONFIG_SYS_HID0_FINAL@h
356	ori	r3, r3, CONFIG_SYS_HID0_FINAL@l
357	SYNC
358	mtspr	HID0, r3
359
360	/* Enable Extra BATs */
361	mfspr	r3, 1011    /* HID2 */
362	lis	r4, 0x0004
363	ori	r4, r4, 0x0000
364	or	r4, r4, r3
365	mtspr	1011, r4
366	sync
367
368	/* clear all BAT's						*/
369	/*--------------------------------------------------------------*/
370
371	li	r0, 0
372	mtspr	DBAT0U, r0
373	mtspr	DBAT0L, r0
374	mtspr	DBAT1U, r0
375	mtspr	DBAT1L, r0
376	mtspr	DBAT2U, r0
377	mtspr	DBAT2L, r0
378	mtspr	DBAT3U, r0
379	mtspr	DBAT3L, r0
380	mtspr	DBAT4U, r0
381	mtspr	DBAT4L, r0
382	mtspr	DBAT5U, r0
383	mtspr	DBAT5L, r0
384	mtspr	DBAT6U, r0
385	mtspr	DBAT6L, r0
386	mtspr	DBAT7U, r0
387	mtspr	DBAT7L, r0
388	mtspr	IBAT0U, r0
389	mtspr	IBAT0L, r0
390	mtspr	IBAT1U, r0
391	mtspr	IBAT1L, r0
392	mtspr	IBAT2U, r0
393	mtspr	IBAT2L, r0
394	mtspr	IBAT3U, r0
395	mtspr	IBAT3L, r0
396	mtspr	IBAT4U, r0
397	mtspr	IBAT4L, r0
398	mtspr	IBAT5U, r0
399	mtspr	IBAT5L, r0
400	mtspr	IBAT6U, r0
401	mtspr	IBAT6L, r0
402	mtspr	IBAT7U, r0
403	mtspr	IBAT7L, r0
404	SYNC
405
406	/* invalidate all tlb's						*/
407	/*								*/
408	/* From the 603e User Manual: "The 603e provides the ability to */
409	/* invalidate a TLB entry. The TLB Invalidate Entry (tlbie)	*/
410	/* instruction invalidates the TLB entry indexed by the EA, and */
411	/* operates on both the instruction and data TLBs simultaneously*/
412	/* invalidating four TLB entries (both sets in each TLB). The	*/
413	/* index corresponds to bits 15-19 of the EA. To invalidate all */
414	/* entries within both TLBs, 32 tlbie instructions should be	*/
415	/* issued, incrementing this field by one each time."		*/
416	/*								*/
417	/* "Note that the tlbia instruction is not implemented on the	*/
418	/* 603e."							*/
419	/*								*/
420	/* bits 15-19 correspond to addresses 0x00000000 to 0x0001F000	*/
421	/* incrementing by 0x1000 each time. The code below is sort of	*/
422	/* based on code in "flush_tlbs" from arch/powerpc/kernel/head.S	*/
423	/*								*/
424	/*--------------------------------------------------------------*/
425
426	li	r3, 32
427	mtctr	r3
428	li	r3, 0
4291:	tlbie	r3
430	addi	r3, r3, 0x1000
431	bdnz	1b
432	SYNC
433
434	/* Done!							*/
435	/*--------------------------------------------------------------*/
436
437	blr
438
439/* Cache functions.
440 *
441 * Note: requires that all cache bits in
442 * HID0 are in the low half word.
443 */
444	.globl	icache_enable
445icache_enable:
446	lis	r4, 0
447	ori	r4, r4, CONFIG_SYS_HID0_INIT /* set ICE & ICFI bit		*/
448	rlwinm	r3, r4, 0, 21, 19     /* clear the ICFI bit		*/
449
450	/*
451	 * The setting of the instruction cache enable (ICE) bit must be
452	 * preceded by an isync instruction to prevent the cache from being
453	 * enabled or disabled while an instruction access is in progress.
454	 */
455	isync
456	mtspr	HID0, r4	      /* Enable Instr Cache & Inval cache */
457	mtspr	HID0, r3	      /* using 2 consec instructions	*/
458	isync
459	blr
460
461	.globl	icache_disable
462icache_disable:
463	mfspr	r3, HID0
464	rlwinm	r3, r3, 0, 17, 15     /* clear the ICE bit		*/
465	mtspr	HID0, r3
466	isync
467	blr
468
469	.globl	icache_status
470icache_status:
471	mfspr	r3, HID0
472	rlwinm	r3, r3, HID0_ICE_BITPOS + 1, 31, 31
473	blr
474
475	.globl	dcache_enable
476dcache_enable:
477	lis	r4, 0
478	ori	r4, r4, HID0_DCE|HID0_DCFI /* set DCE & DCFI bit	*/
479	rlwinm	r3, r4, 0, 22, 20     /* clear the DCFI bit		*/
480
481	/* Enable address translation in MSR bit */
482	mfmsr	r5
483	ori	r5, r5, 0x
484
485
486	/*
487	 * The setting of the instruction cache enable (ICE) bit must be
488	 * preceded by an isync instruction to prevent the cache from being
489	 * enabled or disabled while an instruction access is in progress.
490	 */
491	isync
492	mtspr	HID0, r4	      /* Enable Data Cache & Inval cache*/
493	mtspr	HID0, r3	      /* using 2 consec instructions	*/
494	isync
495	blr
496
497	.globl	dcache_disable
498dcache_disable:
499	mfspr	r3, HID0
500	rlwinm	r3, r3, 0, 18, 16     /* clear the DCE bit */
501	mtspr	HID0, r3
502	isync
503	blr
504
505	.globl	dcache_status
506dcache_status:
507	mfspr	r3, HID0
508	rlwinm	r3, r3, HID0_DCE_BITPOS + 1, 31, 31
509	blr
510
511	.globl	get_pvr
512get_pvr:
513	mfspr	r3, PVR
514	blr
515
516/*------------------------------------------------------------------------------*/
517
518/*
519 * void relocate_code (addr_sp, gd, addr_moni)
520 *
521 * This "function" does not return, instead it continues in RAM
522 * after relocating the monitor code.
523 *
524 * r3 = dest
525 * r4 = src
526 * r5 = length in bytes
527 * r6 = cachelinesize
528 */
529	.globl	relocate_code
530relocate_code:
531	mr	r1,  r3	    /* Set new stack pointer		*/
532	mr	r9,  r4	    /* Save copy of Global Data pointer */
533	mr	r10, r5	    /* Save copy of Destination Address */
534
535	GET_GOT
536	mr	r3,  r5	    /* Destination Address		*/
537	lis	r4, CONFIG_SYS_MONITOR_BASE@h	/* Source Address	*/
538	ori	r4, r4, CONFIG_SYS_MONITOR_BASE@l
539	lwz	r5, GOT(__init_end)
540	sub	r5, r5, r4
541	li	r6, CONFIG_SYS_CACHELINE_SIZE	/* Cache Line Size	*/
542
543	/*
544	 * Fix GOT pointer:
545	 *
546	 * New GOT-PTR = (old GOT-PTR - CONFIG_SYS_MONITOR_BASE) + Destination Address
547	 *
548	 * Offset:
549	 */
550	sub	r15, r10, r4
551
552	/* First our own GOT */
553	add	r12, r12, r15
554	/* then the one used by the C code */
555	add	r30, r30, r15
556
557	/*
558	 * Now relocate code
559	 */
560
561	cmplw	cr1,r3,r4
562	addi	r0,r5,3
563	srwi.	r0,r0,2
564	beq	cr1,4f	    /* In place copy is not necessary	*/
565	beq	7f	    /* Protect against 0 count		*/
566	mtctr	r0
567	bge	cr1,2f
568
569	la	r8,-4(r4)
570	la	r7,-4(r3)
5711:	lwzu	r0,4(r8)
572	stwu	r0,4(r7)
573	bdnz	1b
574	b	4f
575
5762:	slwi	r0,r0,2
577	add	r8,r4,r0
578	add	r7,r3,r0
5793:	lwzu	r0,-4(r8)
580	stwu	r0,-4(r7)
581	bdnz	3b
582
583/*
584 * Now flush the cache: note that we must start from a cache aligned
585 * address. Otherwise we might miss one cache line.
586 */
5874:	cmpwi	r6,0
588	add	r5,r3,r5
589	beq	7f	    /* Always flush prefetch queue in any case	*/
590	subi	r0,r6,1
591	andc	r3,r3,r0
592	mfspr	r7,HID0	    /* don't do dcbst if dcache is disabled	*/
593	rlwinm	r7,r7,HID0_DCE_BITPOS+1,31,31
594	cmpwi	r7,0
595	beq	9f
596	mr	r4,r3
5975:	dcbst	0,r4
598	add	r4,r4,r6
599	cmplw	r4,r5
600	blt	5b
601	sync		    /* Wait for all dcbst to complete on bus	*/
6029:	mfspr	r7,HID0	    /* don't do icbi if icache is disabled	*/
603	rlwinm	r7,r7,HID0_ICE_BITPOS+1,31,31
604	cmpwi	r7,0
605	beq	7f
606	mr	r4,r3
6076:	icbi	0,r4
608	add	r4,r4,r6
609	cmplw	r4,r5
610	blt	6b
6117:	sync		    /* Wait for all icbi to complete on bus	*/
612	isync
613
614/*
615 * We are done. Do not return, instead branch to second part of board
616 * initialization, now running from RAM.
617 */
618
619	addi	r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
620	mtlr	r0
621	blr
622
623in_ram:
624
625	/*
626	 * Relocation Function, r12 point to got2+0x8000
627	 *
628	 * Adjust got2 pointers, no need to check for 0, this code
629	 * already puts a few entries in the table.
630	 */
631	li	r0,__got2_entries@sectoff@l
632	la	r3,GOT(_GOT2_TABLE_)
633	lwz	r11,GOT(_GOT2_TABLE_)
634	mtctr	r0
635	sub	r11,r3,r11
636	addi	r3,r3,-4
6371:	lwzu	r0,4(r3)
638	cmpwi	r0,0
639	beq-	2f
640	add	r0,r0,r11
641	stw	r0,0(r3)
6422:	bdnz	1b
643
644	/*
645	 * Now adjust the fixups and the pointers to the fixups
646	 * in case we need to move ourselves again.
647	 */
648	li	r0,__fixup_entries@sectoff@l
649	lwz	r3,GOT(_FIXUP_TABLE_)
650	cmpwi	r0,0
651	mtctr	r0
652	addi	r3,r3,-4
653	beq	4f
6543:	lwzu	r4,4(r3)
655	lwzux	r0,r4,r11
656	add	r0,r0,r11
657	stw	r10,0(r3)
658	stw	r0,0(r4)
659	bdnz	3b
6604:
661clear_bss:
662	/*
663	 * Now clear BSS segment
664	 */
665	lwz	r3,GOT(__bss_start)
666	lwz	r4,GOT(_end)
667
668	cmplw	0, r3, r4
669	beq	6f
670
671	li	r0, 0
6725:
673	stw	r0, 0(r3)
674	addi	r3, r3, 4
675	cmplw	0, r3, r4
676	bne	5b
6776:
678
679	mr	r3, r9	    /* Global Data pointer	*/
680	mr	r4, r10	    /* Destination Address	*/
681	bl	board_init_r
682
683	/*
684	 * Copy exception vector code to low memory
685	 *
686	 * r3: dest_addr
687	 * r7: source address, r8: end address, r9: target address
688	 */
689	.globl	trap_init
690trap_init:
691	mflr	r4	    /* save link register		*/
692	GET_GOT
693	lwz	r7, GOT(_start)
694	lwz	r8, GOT(_end_of_vectors)
695
696	li	r9, 0x100   /* reset vector always at 0x100	*/
697
698	cmplw	0, r7, r8
699	bgelr		    /* return if r7>=r8 - just in case	*/
7001:
701	lwz	r0, 0(r7)
702	stw	r0, 0(r9)
703	addi	r7, r7, 4
704	addi	r9, r9, 4
705	cmplw	0, r7, r8
706	bne	1b
707
708	/*
709	 * relocate `hdlr' and `int_return' entries
710	 */
711	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
712	li	r8, Alignment - _start + EXC_OFF_SYS_RESET
7132:
714	bl	trap_reloc
715	addi	r7, r7, 0x100	    /* next exception vector	    */
716	cmplw	0, r7, r8
717	blt	2b
718
719	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
720	bl	trap_reloc
721
722	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
723	bl	trap_reloc
724
725	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
726	li	r8, SystemCall - _start + EXC_OFF_SYS_RESET
7273:
728	bl	trap_reloc
729	addi	r7, r7, 0x100	    /* next exception vector	    */
730	cmplw	0, r7, r8
731	blt	3b
732
733	li	r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
734	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
7354:
736	bl	trap_reloc
737	addi	r7, r7, 0x100	    /* next exception vector	    */
738	cmplw	0, r7, r8
739	blt	4b
740
741	mfmsr	r3		    /* now that the vectors have    */
742	lis	r7, MSR_IP@h	    /* relocated into low memory    */
743	ori	r7, r7, MSR_IP@l    /* MSR[IP] can be turned off    */
744	andc	r3, r3, r7	    /* (if it was on)		    */
745	SYNC			    /* Some chip revs need this...  */
746	mtmsr	r3
747	SYNC
748
749	mtlr	r4		    /* restore link register	    */
750	blr
751