start.S 17 KB

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