start.S 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711
  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-2009 Wolfgang Denk <wd@denx.de>
  5. * Copyright Freescale Semiconductor, Inc. 2004, 2006.
  6. *
  7. * See file CREDITS for list of people who contributed to this
  8. * project.
  9. *
  10. * This program is free software; you can redistribute it and/or
  11. * modify it under the terms of the GNU General Public License as
  12. * published by the Free Software Foundation; either version 2 of
  13. * the License, or (at your option) any later version.
  14. *
  15. * This program is distributed in the hope that it will be useful,
  16. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  18. * GNU General Public License for more details.
  19. *
  20. * You should have received a copy of the GNU General Public License
  21. * along with this program; if not, write to the Free Software
  22. * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  23. * MA 02111-1307 USA
  24. *
  25. * Based on the MPC83xx code.
  26. */
  27. /*
  28. * U-Boot - Startup Code for MPC512x based Embedded Boards
  29. */
  30. #include <config.h>
  31. #include <timestamp.h>
  32. #include <version.h>
  33. #define CONFIG_521X 1 /* needed for Linux kernel header files*/
  34. #include <asm/immap_512x.h>
  35. #include "asm-offsets.h"
  36. #include <ppc_asm.tmpl>
  37. #include <ppc_defs.h>
  38. #include <asm/cache.h>
  39. #include <asm/mmu.h>
  40. #ifndef CONFIG_IDENT_STRING
  41. #define CONFIG_IDENT_STRING "MPC512X"
  42. #endif
  43. /*
  44. * Floating Point enable, Machine Check and Recoverable Interr.
  45. */
  46. #undef MSR_KERNEL
  47. #ifdef DEBUG
  48. #define MSR_KERNEL (MSR_FP|MSR_RI)
  49. #else
  50. #define MSR_KERNEL (MSR_FP|MSR_ME|MSR_RI)
  51. #endif
  52. /* Macros for manipulating CSx_START/STOP */
  53. #define START_REG(start) ((start) >> 16)
  54. #define STOP_REG(start, size) (((start) + (size) - 1) >> 16)
  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. GOT_ENTRY(_start)
  64. GOT_ENTRY(_start_of_vectors)
  65. GOT_ENTRY(_end_of_vectors)
  66. GOT_ENTRY(transfer_to_handler)
  67. GOT_ENTRY(__init_end)
  68. GOT_ENTRY(_end)
  69. GOT_ENTRY(__bss_start)
  70. END_GOT
  71. /*
  72. * Magic number and version string
  73. */
  74. .long 0x27051956 /* U-Boot Magic Number */
  75. .globl version_string
  76. version_string:
  77. .ascii U_BOOT_VERSION
  78. .ascii " (", U_BOOT_DATE, " - ", U_BOOT_TIME, ")"
  79. .ascii " ", CONFIG_IDENT_STRING, "\0"
  80. /*
  81. * Vector Table
  82. */
  83. .text
  84. . = EXC_OFF_SYS_RESET
  85. .globl _start
  86. /* Start from here after reset/power on */
  87. _start:
  88. li r21, BOOTFLAG_COLD /* Normal Power-On: Boot from FLASH */
  89. b boot_cold
  90. .globl _start_of_vectors
  91. _start_of_vectors:
  92. /* Machine check */
  93. STD_EXCEPTION(0x200, MachineCheck, MachineCheckException)
  94. /* Data Storage exception. */
  95. STD_EXCEPTION(0x300, DataStorage, UnknownException)
  96. /* Instruction Storage exception. */
  97. STD_EXCEPTION(0x400, InstStorage, UnknownException)
  98. /* External Interrupt exception. */
  99. STD_EXCEPTION(0x500, ExtInterrupt, UnknownException)
  100. /* Alignment exception. */
  101. . = 0x600
  102. Alignment:
  103. EXCEPTION_PROLOG(SRR0, SRR1)
  104. mfspr r4,DAR
  105. stw r4,_DAR(r21)
  106. mfspr r5,DSISR
  107. stw r5,_DSISR(r21)
  108. addi r3,r1,STACK_FRAME_OVERHEAD
  109. EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
  110. /* Program check exception */
  111. . = 0x700
  112. ProgramCheck:
  113. EXCEPTION_PROLOG(SRR0, SRR1)
  114. addi r3,r1,STACK_FRAME_OVERHEAD
  115. EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
  116. MSR_KERNEL, COPY_EE)
  117. /* Floating Point Unit unavailable exception */
  118. STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
  119. /* Decrementer */
  120. STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
  121. /* Critical interrupt */
  122. STD_EXCEPTION(0xa00, Critical, UnknownException)
  123. /* System Call */
  124. STD_EXCEPTION(0xc00, SystemCall, UnknownException)
  125. /* Trace interrupt */
  126. STD_EXCEPTION(0xd00, Trace, UnknownException)
  127. /* Performance Monitor interrupt */
  128. STD_EXCEPTION(0xf00, PerfMon, UnknownException)
  129. /* Intruction Translation Miss */
  130. STD_EXCEPTION(0x1000, InstructionTLBMiss, UnknownException)
  131. /* Data Load Translation Miss */
  132. STD_EXCEPTION(0x1100, DataLoadTLBMiss, UnknownException)
  133. /* Data Store Translation Miss */
  134. STD_EXCEPTION(0x1200, DataStoreTLBMiss, UnknownException)
  135. /* Instruction Address Breakpoint */
  136. STD_EXCEPTION(0x1300, InstructionAddrBreakpoint, DebugException)
  137. /* System Management interrupt */
  138. STD_EXCEPTION(0x1400, SystemMgmtInterrupt, UnknownException)
  139. .globl _end_of_vectors
  140. _end_of_vectors:
  141. . = 0x3000
  142. boot_cold:
  143. /* Save msr contents */
  144. mfmsr r5
  145. /* Set IMMR area to our preferred location */
  146. lis r4, CONFIG_DEFAULT_IMMR@h
  147. lis r3, CONFIG_SYS_IMMR@h
  148. ori r3, r3, CONFIG_SYS_IMMR@l
  149. stw r3, IMMRBAR(r4)
  150. mtspr MBAR, r3 /* IMMRBAR is mirrored into the MBAR SPR (311) */
  151. /* Initialise the machine */
  152. bl cpu_early_init
  153. /*
  154. * Set up Local Access Windows:
  155. *
  156. * 1) Boot/CS0 (boot FLASH)
  157. * 2) On-chip SRAM (initial stack purposes)
  158. */
  159. /* Boot CS/CS0 window range */
  160. lis r3, CONFIG_SYS_IMMR@h
  161. ori r3, r3, CONFIG_SYS_IMMR@l
  162. lis r4, START_REG(CONFIG_SYS_FLASH_BASE)
  163. ori r4, r4, STOP_REG(CONFIG_SYS_FLASH_BASE, CONFIG_SYS_FLASH_SIZE)
  164. stw r4, LPCS0AW(r3)
  165. /*
  166. * The SRAM window has a fixed size (256K), so only the start address
  167. * is necessary
  168. */
  169. lis r4, START_REG(CONFIG_SYS_SRAM_BASE) & 0xff00
  170. stw r4, SRAMBAR(r3)
  171. /*
  172. * According to MPC5121e RM, configuring local access windows should
  173. * be followed by a dummy read of the config register that was
  174. * modified last and an isync
  175. */
  176. lwz r4, SRAMBAR(r3)
  177. isync
  178. /*
  179. * Set configuration of the Boot/CS0, the SRAM window does not have a
  180. * config register so no params can be set for it
  181. */
  182. lis r3, (CONFIG_SYS_IMMR + LPC_OFFSET)@h
  183. ori r3, r3, (CONFIG_SYS_IMMR + LPC_OFFSET)@l
  184. lis r4, CONFIG_SYS_CS0_CFG@h
  185. ori r4, r4, CONFIG_SYS_CS0_CFG@l
  186. stw r4, CS0_CONFIG(r3)
  187. /* Master enable all CS's */
  188. lis r4, CS_CTRL_ME@h
  189. ori r4, r4, CS_CTRL_ME@l
  190. stw r4, CS_CTRL(r3)
  191. lis r4, (CONFIG_SYS_MONITOR_BASE)@h
  192. ori r4, r4, (CONFIG_SYS_MONITOR_BASE)@l
  193. addi r5, r4, in_flash - _start + EXC_OFF_SYS_RESET
  194. mtlr r5
  195. blr
  196. in_flash:
  197. lis r1, (CONFIG_SYS_INIT_RAM_ADDR + CONFIG_SYS_GBL_DATA_OFFSET)@h
  198. ori r1, r1, (CONFIG_SYS_INIT_RAM_ADDR + CONFIG_SYS_GBL_DATA_OFFSET)@l
  199. li r0, 0 /* Make room for stack frame header and */
  200. stwu r0, -4(r1) /* clear final stack frame so that */
  201. stwu r0, -4(r1) /* stack backtraces terminate cleanly */
  202. /* let the C-code set up the rest */
  203. /* */
  204. /* Be careful to keep code relocatable & stack humble */
  205. /*------------------------------------------------------*/
  206. GET_GOT /* initialize GOT access */
  207. /* r3: IMMR */
  208. lis r3, CONFIG_SYS_IMMR@h
  209. /* run low-level CPU init code (in Flash) */
  210. bl cpu_init_f
  211. /* r3: BOOTFLAG */
  212. mr r3, r21
  213. /* run 1st part of board init code (in Flash) */
  214. bl board_init_f
  215. /* NOTREACHED - board_init_f() does not return */
  216. /*
  217. * This code finishes saving the registers to the exception frame
  218. * and jumps to the appropriate handler for the exception.
  219. * Register r21 is pointer into trap frame, r1 has new stack pointer.
  220. */
  221. .globl transfer_to_handler
  222. transfer_to_handler:
  223. stw r22,_NIP(r21)
  224. lis r22,MSR_POW@h
  225. andc r23,r23,r22
  226. stw r23,_MSR(r21)
  227. SAVE_GPR(7, r21)
  228. SAVE_4GPRS(8, r21)
  229. SAVE_8GPRS(12, r21)
  230. SAVE_8GPRS(24, r21)
  231. mflr r23
  232. andi. r24,r23,0x3f00 /* get vector offset */
  233. stw r24,TRAP(r21)
  234. li r22,0
  235. stw r22,RESULT(r21)
  236. lwz r24,0(r23) /* virtual address of handler */
  237. lwz r23,4(r23) /* where to go when done */
  238. mtspr SRR0,r24
  239. mtspr SRR1,r20
  240. mtlr r23
  241. SYNC
  242. rfi /* jump to handler, enable MMU */
  243. int_return:
  244. mfmsr r28 /* Disable interrupts */
  245. li r4,0
  246. ori r4,r4,MSR_EE
  247. andc r28,r28,r4
  248. SYNC /* Some chip revs need this... */
  249. mtmsr r28
  250. SYNC
  251. lwz r2,_CTR(r1)
  252. lwz r0,_LINK(r1)
  253. mtctr r2
  254. mtlr r0
  255. lwz r2,_XER(r1)
  256. lwz r0,_CCR(r1)
  257. mtspr XER,r2
  258. mtcrf 0xFF,r0
  259. REST_10GPRS(3, r1)
  260. REST_10GPRS(13, r1)
  261. REST_8GPRS(23, r1)
  262. REST_GPR(31, r1)
  263. lwz r2,_NIP(r1) /* Restore environment */
  264. lwz r0,_MSR(r1)
  265. mtspr SRR0,r2
  266. mtspr SRR1,r0
  267. lwz r0,GPR0(r1)
  268. lwz r2,GPR2(r1)
  269. lwz r1,GPR1(r1)
  270. SYNC
  271. rfi
  272. /*
  273. * This code initialises the machine, it expects original MSR contents to be in r5.
  274. */
  275. cpu_early_init:
  276. /* Initialize machine status; enable machine check interrupt */
  277. /*-----------------------------------------------------------*/
  278. li r3, MSR_KERNEL /* Set ME and RI flags */
  279. rlwimi r3, r5, 0, 25, 25 /* preserve IP bit */
  280. #ifdef DEBUG
  281. rlwimi r3, r5, 0, 21, 22 /* debugger might set SE, BE bits */
  282. #endif
  283. mtmsr r3
  284. SYNC
  285. mtspr SRR1, r3 /* Mirror current MSR state in SRR1 */
  286. lis r3, CONFIG_SYS_IMMR@h
  287. #if defined(CONFIG_WATCHDOG)
  288. /* Initialise the watchdog and reset it */
  289. /*--------------------------------------*/
  290. lis r4, CONFIG_SYS_WATCHDOG_VALUE
  291. ori r4, r4, (SWCRR_SWEN | SWCRR_SWRI | SWCRR_SWPR)
  292. stw r4, SWCRR(r3)
  293. /* reset */
  294. li r4, 0x556C
  295. sth r4, SWSRR@l(r3)
  296. li r4, 0x0
  297. ori r4, r4, 0xAA39
  298. sth r4, SWSRR@l(r3)
  299. #else
  300. /* Disable the watchdog */
  301. /*----------------------*/
  302. lwz r4, SWCRR(r3)
  303. /*
  304. * Check to see if it's enabled for disabling: once disabled by s/w
  305. * it's not possible to re-enable it
  306. */
  307. andi. r4, r4, 0x4
  308. beq 1f
  309. xor r4, r4, r4
  310. stw r4, SWCRR(r3)
  311. 1:
  312. #endif /* CONFIG_WATCHDOG */
  313. /* Initialize the Hardware Implementation-dependent Registers */
  314. /* HID0 also contains cache control */
  315. /*------------------------------------------------------*/
  316. lis r3, CONFIG_SYS_HID0_INIT@h
  317. ori r3, r3, CONFIG_SYS_HID0_INIT@l
  318. SYNC
  319. mtspr HID0, r3
  320. lis r3, CONFIG_SYS_HID0_FINAL@h
  321. ori r3, r3, CONFIG_SYS_HID0_FINAL@l
  322. SYNC
  323. mtspr HID0, r3
  324. lis r3, CONFIG_SYS_HID2@h
  325. ori r3, r3, CONFIG_SYS_HID2@l
  326. SYNC
  327. mtspr HID2, r3
  328. sync
  329. blr
  330. /* Cache functions.
  331. *
  332. * Note: requires that all cache bits in
  333. * HID0 are in the low half word.
  334. */
  335. .globl icache_enable
  336. icache_enable:
  337. mfspr r3, HID0
  338. ori r3, r3, HID0_ICE
  339. lis r4, 0
  340. ori r4, r4, HID0_ILOCK
  341. andc r3, r3, r4
  342. ori r4, r3, HID0_ICFI
  343. isync
  344. mtspr HID0, r4 /* sets enable and invalidate, clears lock */
  345. isync
  346. mtspr HID0, r3 /* clears invalidate */
  347. blr
  348. .globl icache_disable
  349. icache_disable:
  350. mfspr r3, HID0
  351. lis r4, 0
  352. ori r4, r4, HID0_ICE|HID0_ILOCK
  353. andc r3, r3, r4
  354. ori r4, r3, HID0_ICFI
  355. isync
  356. mtspr HID0, r4 /* sets invalidate, clears enable and lock*/
  357. isync
  358. mtspr HID0, r3 /* clears invalidate */
  359. blr
  360. .globl icache_status
  361. icache_status:
  362. mfspr r3, HID0
  363. rlwinm r3, r3, (31 - HID0_ICE_SHIFT + 1), 31, 31
  364. blr
  365. .globl dcache_enable
  366. dcache_enable:
  367. mfspr r3, HID0
  368. li r5, HID0_DCFI|HID0_DLOCK
  369. andc r3, r3, r5
  370. mtspr HID0, r3 /* no invalidate, unlock */
  371. ori r3, r3, HID0_DCE
  372. ori r5, r3, HID0_DCFI
  373. mtspr HID0, r5 /* enable + invalidate */
  374. mtspr HID0, r3 /* enable */
  375. sync
  376. blr
  377. .globl dcache_disable
  378. dcache_disable:
  379. mfspr r3, HID0
  380. lis r4, 0
  381. ori r4, r4, HID0_DCE|HID0_DLOCK
  382. andc r3, r3, r4
  383. ori r4, r3, HID0_DCI
  384. sync
  385. mtspr HID0, r4 /* sets invalidate, clears enable and lock */
  386. sync
  387. mtspr HID0, r3 /* clears invalidate */
  388. blr
  389. .globl dcache_status
  390. dcache_status:
  391. mfspr r3, HID0
  392. rlwinm r3, r3, (31 - HID0_DCE_SHIFT + 1), 31, 31
  393. blr
  394. .globl get_pvr
  395. get_pvr:
  396. mfspr r3, PVR
  397. blr
  398. /*-------------------------------------------------------------------*/
  399. /*
  400. * void relocate_code (addr_sp, gd, addr_moni)
  401. *
  402. * This "function" does not return, instead it continues in RAM
  403. * after relocating the monitor code.
  404. *
  405. * r3 = dest
  406. * r4 = src
  407. * r5 = length in bytes
  408. * r6 = cachelinesize
  409. */
  410. .globl relocate_code
  411. relocate_code:
  412. mr r1, r3 /* Set new stack pointer */
  413. mr r9, r4 /* Save copy of Global Data pointer */
  414. mr r10, r5 /* Save copy of Destination Address */
  415. GET_GOT
  416. mr r3, r5 /* Destination Address */
  417. lis r4, CONFIG_SYS_MONITOR_BASE@h /* Source Address */
  418. ori r4, r4, CONFIG_SYS_MONITOR_BASE@l
  419. lwz r5, GOT(__init_end)
  420. sub r5, r5, r4
  421. li r6, CONFIG_SYS_CACHELINE_SIZE /* Cache Line Size */
  422. /*
  423. * Fix GOT pointer:
  424. *
  425. * New GOT-PTR = (old GOT-PTR - CONFIG_SYS_MONITOR_BASE)
  426. * + Destination Address
  427. *
  428. * Offset:
  429. */
  430. sub r15, r10, r4
  431. /* First our own GOT */
  432. add r12, r12, r15
  433. /* then the one used by the C code */
  434. add r30, r30, r15
  435. /*
  436. * Now relocate code
  437. */
  438. cmplw cr1,r3,r4
  439. addi r0,r5,3
  440. srwi. r0,r0,2
  441. beq cr1,4f /* In place copy is not necessary */
  442. beq 7f /* Protect against 0 count */
  443. mtctr r0
  444. bge cr1,2f
  445. la r8,-4(r4)
  446. la r7,-4(r3)
  447. /* copy */
  448. 1: lwzu r0,4(r8)
  449. stwu r0,4(r7)
  450. bdnz 1b
  451. addi r0,r5,3
  452. srwi. r0,r0,2
  453. mtctr r0
  454. la r8,-4(r4)
  455. la r7,-4(r3)
  456. /* and compare */
  457. 20: lwzu r20,4(r8)
  458. lwzu r21,4(r7)
  459. xor. r22, r20, r21
  460. bne 30f
  461. bdnz 20b
  462. b 4f
  463. /* compare failed */
  464. 30: li r3, 0
  465. blr
  466. 2: slwi r0,r0,2 /* re copy in reverse order ... y do we needed it? */
  467. add r8,r4,r0
  468. add r7,r3,r0
  469. 3: lwzu r0,-4(r8)
  470. stwu r0,-4(r7)
  471. bdnz 3b
  472. /*
  473. * Now flush the cache: note that we must start from a cache aligned
  474. * address. Otherwise we might miss one cache line.
  475. */
  476. 4: cmpwi r6,0
  477. add r5,r3,r5
  478. beq 7f /* Always flush prefetch queue in any case */
  479. subi r0,r6,1
  480. andc r3,r3,r0
  481. mr r4,r3
  482. 5: dcbst 0,r4
  483. add r4,r4,r6
  484. cmplw r4,r5
  485. blt 5b
  486. sync /* Wait for all dcbst to complete on bus */
  487. mr r4,r3
  488. 6: icbi 0,r4
  489. add r4,r4,r6
  490. cmplw r4,r5
  491. blt 6b
  492. 7: sync /* Wait for all icbi to complete on bus */
  493. isync
  494. /*
  495. * We are done. Do not return, instead branch to second part of board
  496. * initialization, now running from RAM.
  497. */
  498. addi r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
  499. mtlr r0
  500. blr
  501. in_ram:
  502. /*
  503. * Relocation Function, r12 point to got2+0x8000
  504. *
  505. * Adjust got2 pointers, no need to check for 0, this code
  506. * already puts a few entries in the table.
  507. */
  508. li r0,__got2_entries@sectoff@l
  509. la r3,GOT(_GOT2_TABLE_)
  510. lwz r11,GOT(_GOT2_TABLE_)
  511. mtctr r0
  512. sub r11,r3,r11
  513. addi r3,r3,-4
  514. 1: lwzu r0,4(r3)
  515. cmpwi r0,0
  516. beq- 2f
  517. add r0,r0,r11
  518. stw r0,0(r3)
  519. 2: bdnz 1b
  520. /*
  521. * Now adjust the fixups and the pointers to the fixups
  522. * in case we need to move ourselves again.
  523. */
  524. li r0,__fixup_entries@sectoff@l
  525. lwz r3,GOT(_FIXUP_TABLE_)
  526. cmpwi r0,0
  527. mtctr r0
  528. addi r3,r3,-4
  529. beq 4f
  530. 3: lwzu r4,4(r3)
  531. lwzux r0,r4,r11
  532. add r0,r0,r11
  533. stw r10,0(r3)
  534. stw r0,0(r4)
  535. bdnz 3b
  536. 4:
  537. clear_bss:
  538. /*
  539. * Now clear BSS segment
  540. */
  541. lwz r3,GOT(__bss_start)
  542. lwz r4,GOT(_end)
  543. cmplw 0, r3, r4
  544. beq 6f
  545. li r0, 0
  546. 5:
  547. stw r0, 0(r3)
  548. addi r3, r3, 4
  549. cmplw 0, r3, r4
  550. bne 5b
  551. 6:
  552. mr r3, r9 /* Global Data pointer */
  553. mr r4, r10 /* Destination Address */
  554. bl board_init_r
  555. /*
  556. * Copy exception vector code to low memory
  557. *
  558. * r3: dest_addr
  559. * r7: source address, r8: end address, r9: target address
  560. */
  561. .globl trap_init
  562. trap_init:
  563. mflr r4 /* save link register */
  564. GET_GOT
  565. lwz r7, GOT(_start)
  566. lwz r8, GOT(_end_of_vectors)
  567. li r9, 0x100 /* reset vector at 0x100 */
  568. cmplw 0, r7, r8
  569. bgelr /* return if r7>=r8 - just in case */
  570. 1:
  571. lwz r0, 0(r7)
  572. stw r0, 0(r9)
  573. addi r7, r7, 4
  574. addi r9, r9, 4
  575. cmplw 0, r7, r8
  576. bne 1b
  577. /*
  578. * relocate `hdlr' and `int_return' entries
  579. */
  580. li r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
  581. li r8, Alignment - _start + EXC_OFF_SYS_RESET
  582. 2:
  583. bl trap_reloc
  584. addi r7, r7, 0x100 /* next exception vector */
  585. cmplw 0, r7, r8
  586. blt 2b
  587. li r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
  588. bl trap_reloc
  589. li r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
  590. bl trap_reloc
  591. li r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
  592. li r8, SystemCall - _start + EXC_OFF_SYS_RESET
  593. 3:
  594. bl trap_reloc
  595. addi r7, r7, 0x100 /* next exception vector */
  596. cmplw 0, r7, r8
  597. blt 3b
  598. li r7, .L_Trace - _start + EXC_OFF_SYS_RESET
  599. li r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
  600. 4:
  601. bl trap_reloc
  602. addi r7, r7, 0x100 /* next exception vector */
  603. cmplw 0, r7, r8
  604. blt 4b
  605. mfmsr r3 /* now that the vectors have */
  606. lis r7, MSR_IP@h /* relocated into low memory */
  607. ori r7, r7, MSR_IP@l /* MSR[IP] can be turned off */
  608. andc r3, r3, r7 /* (if it was on) */
  609. SYNC /* Some chip revs need this... */
  610. mtmsr r3
  611. SYNC
  612. mtlr r4 /* restore link register */
  613. blr