head.S 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641
  1. /* head.S: kernel entry point for FR-V kernel
  2. *
  3. * Copyright (C) 2003, 2004 Red Hat, Inc. All Rights Reserved.
  4. * Written by David Howells (dhowells@redhat.com)
  5. *
  6. * This program is free software; you can redistribute it and/or
  7. * modify it under the terms of the GNU General Public License
  8. * as published by the Free Software Foundation; either version
  9. * 2 of the License, or (at your option) any later version.
  10. */
  11. #include <linux/threads.h>
  12. #include <linux/linkage.h>
  13. #include <asm/ptrace.h>
  14. #include <asm/page.h>
  15. #include <asm/spr-regs.h>
  16. #include <asm/mb86943a.h>
  17. #include <asm/cache.h>
  18. #include "head.inc"
  19. ###############################################################################
  20. #
  21. # void _boot(unsigned long magic, char *command_line) __attribute__((noreturn))
  22. #
  23. # - if magic is 0xdead1eaf, then command_line is assumed to point to the kernel
  24. # command line string
  25. #
  26. ###############################################################################
  27. .section .text.head,"ax"
  28. .balign 4
  29. .globl _boot, __head_reference
  30. .type _boot,@function
  31. _boot:
  32. __head_reference:
  33. sethi.p %hi(LED_ADDR),gr30
  34. setlo %lo(LED_ADDR),gr30
  35. LEDS 0x0000
  36. # calculate reference address for PC-relative stuff
  37. call 0f
  38. 0: movsg lr,gr26
  39. addi gr26,#__head_reference-0b,gr26
  40. # invalidate and disable both of the caches and turn off the memory access checking
  41. dcef @(gr0,gr0),1
  42. bar
  43. sethi.p %hi(~(HSR0_ICE|HSR0_DCE|HSR0_CBM|HSR0_EIMMU|HSR0_EDMMU)),gr4
  44. setlo %lo(~(HSR0_ICE|HSR0_DCE|HSR0_CBM|HSR0_EIMMU|HSR0_EDMMU)),gr4
  45. movsg hsr0,gr5
  46. and gr4,gr5,gr5
  47. movgs gr5,hsr0
  48. movsg hsr0,gr5
  49. LEDS 0x0001
  50. icei @(gr0,gr0),1
  51. dcei @(gr0,gr0),1
  52. bar
  53. # turn the instruction cache back on
  54. sethi.p %hi(HSR0_ICE),gr4
  55. setlo %lo(HSR0_ICE),gr4
  56. movsg hsr0,gr5
  57. or gr4,gr5,gr5
  58. movgs gr5,hsr0
  59. movsg hsr0,gr5
  60. bar
  61. LEDS 0x0002
  62. # retrieve the parameters (including command line) before we overwrite them
  63. sethi.p %hi(0xdead1eaf),gr7
  64. setlo %lo(0xdead1eaf),gr7
  65. subcc gr7,gr8,gr0,icc0
  66. bne icc0,#0,__head_no_parameters
  67. sethi.p %hi(redboot_command_line-1),gr6
  68. setlo %lo(redboot_command_line-1),gr6
  69. sethi.p %hi(__head_reference),gr4
  70. setlo %lo(__head_reference),gr4
  71. sub gr6,gr4,gr6
  72. add.p gr6,gr26,gr6
  73. subi gr9,#1,gr9
  74. setlos.p #511,gr4
  75. setlos #1,gr5
  76. __head_copy_cmdline:
  77. ldubu.p @(gr9,gr5),gr16
  78. subicc gr4,#1,gr4,icc0
  79. stbu.p gr16,@(gr6,gr5)
  80. subicc gr16,#0,gr0,icc1
  81. bls icc0,#0,__head_end_cmdline
  82. bne icc1,#1,__head_copy_cmdline
  83. __head_end_cmdline:
  84. stbu gr0,@(gr6,gr5)
  85. __head_no_parameters:
  86. ###############################################################################
  87. #
  88. # we need to relocate the SDRAM to 0x00000000 (linux) or 0xC0000000 (uClinux)
  89. # - note that we're going to have to run entirely out of the icache whilst
  90. # fiddling with the SDRAM controller registers
  91. #
  92. ###############################################################################
  93. #ifdef CONFIG_MMU
  94. call __head_fr451_describe_sdram
  95. #else
  96. movsg psr,gr5
  97. srli gr5,#28,gr5
  98. subicc gr5,#3,gr0,icc0
  99. beq icc0,#0,__head_fr551_sdram
  100. call __head_fr401_describe_sdram
  101. bra __head_do_sdram
  102. __head_fr551_sdram:
  103. call __head_fr555_describe_sdram
  104. LEDS 0x000d
  105. __head_do_sdram:
  106. #endif
  107. # preload the registers with invalid values in case any DBR/DARS are marked not present
  108. sethi.p %hi(0xfe000000),gr17 ; unused SDRAM DBR value
  109. setlo %lo(0xfe000000),gr17
  110. or.p gr17,gr0,gr20
  111. or gr17,gr0,gr21
  112. or.p gr17,gr0,gr22
  113. or gr17,gr0,gr23
  114. # consult the SDRAM controller CS address registers
  115. cld @(gr14,gr0 ),gr20, cc0,#1 ; DBR0 / DARS0
  116. cld @(gr14,gr11),gr21, cc1,#1 ; DBR1 / DARS1
  117. cld @(gr14,gr12),gr22, cc2,#1 ; DBR2 / DARS2
  118. cld.p @(gr14,gr13),gr23, cc3,#1 ; DBR3 / DARS3
  119. sll gr20,gr15,gr20 ; shift values up for FR551
  120. sll gr21,gr15,gr21
  121. sll gr22,gr15,gr22
  122. sll gr23,gr15,gr23
  123. LEDS 0x0003
  124. # assume the lowest valid CS line to be the SDRAM base and get its address
  125. subcc gr20,gr17,gr0,icc0
  126. subcc.p gr21,gr17,gr0,icc1
  127. subcc gr22,gr17,gr0,icc2
  128. subcc.p gr23,gr17,gr0,icc3
  129. ckne icc0,cc4 ; T if DBR0 != 0xfe000000
  130. ckne icc1,cc5
  131. ckne icc2,cc6
  132. ckne icc3,cc7
  133. cor gr23,gr0,gr24, cc7,#1 ; GR24 = SDRAM base
  134. cor gr22,gr0,gr24, cc6,#1
  135. cor gr21,gr0,gr24, cc5,#1
  136. cor gr20,gr0,gr24, cc4,#1
  137. # calculate the displacement required to get the SDRAM into the right place in memory
  138. sethi.p %hi(__sdram_base),gr16
  139. setlo %lo(__sdram_base),gr16
  140. sub gr16,gr24,gr16 ; delta = __sdram_base - DBRx
  141. # calculate the new values to go in the controller regs
  142. cadd.p gr20,gr16,gr20, cc4,#1 ; DCS#0 (new) = DCS#0 (old) + delta
  143. cadd gr21,gr16,gr21, cc5,#1
  144. cadd.p gr22,gr16,gr22, cc6,#1
  145. cadd gr23,gr16,gr23, cc7,#1
  146. srl gr20,gr15,gr20 ; shift values down for FR551
  147. srl gr21,gr15,gr21
  148. srl gr22,gr15,gr22
  149. srl gr23,gr15,gr23
  150. # work out the address at which the reg updater resides and lock it into icache
  151. # also work out the address the updater will jump to when finished
  152. sethi.p %hi(__head_move_sdram-__head_reference),gr18
  153. setlo %lo(__head_move_sdram-__head_reference),gr18
  154. sethi.p %hi(__head_sdram_moved-__head_reference),gr19
  155. setlo %lo(__head_sdram_moved-__head_reference),gr19
  156. add.p gr18,gr26,gr18
  157. add gr19,gr26,gr19
  158. add.p gr19,gr16,gr19 ; moved = addr + (__sdram_base - DBRx)
  159. add gr18,gr5,gr4 ; two cachelines probably required
  160. icpl gr18,gr0,#1 ; load and lock the cachelines
  161. icpl gr4,gr0,#1
  162. LEDS 0x0004
  163. membar
  164. bar
  165. jmpl @(gr18,gr0)
  166. .balign L1_CACHE_BYTES
  167. __head_move_sdram:
  168. cst gr20,@(gr14,gr0 ), cc4,#1
  169. cst gr21,@(gr14,gr11), cc5,#1
  170. cst gr22,@(gr14,gr12), cc6,#1
  171. cst gr23,@(gr14,gr13), cc7,#1
  172. cld @(gr14,gr0 ),gr20, cc4,#1
  173. cld @(gr14,gr11),gr21, cc5,#1
  174. cld @(gr14,gr12),gr22, cc4,#1
  175. cld @(gr14,gr13),gr23, cc7,#1
  176. bar
  177. membar
  178. jmpl @(gr19,gr0)
  179. .balign L1_CACHE_BYTES
  180. __head_sdram_moved:
  181. icul gr18
  182. add gr18,gr5,gr4
  183. icul gr4
  184. icei @(gr0,gr0),1
  185. dcei @(gr0,gr0),1
  186. LEDS 0x0005
  187. # recalculate reference address
  188. call 0f
  189. 0: movsg lr,gr26
  190. addi gr26,#__head_reference-0b,gr26
  191. ###############################################################################
  192. #
  193. # move the kernel image down to the bottom of the SDRAM
  194. #
  195. ###############################################################################
  196. sethi.p %hi(__kernel_image_size_no_bss+15),gr4
  197. setlo %lo(__kernel_image_size_no_bss+15),gr4
  198. srli.p gr4,#4,gr4 ; count
  199. or gr26,gr26,gr16 ; source
  200. sethi.p %hi(__sdram_base),gr17 ; destination
  201. setlo %lo(__sdram_base),gr17
  202. setlos #8,gr5
  203. sub.p gr16,gr5,gr16 ; adjust src for LDDU
  204. sub gr17,gr5,gr17 ; adjust dst for LDDU
  205. sethi.p %hi(__head_move_kernel-__head_reference),gr18
  206. setlo %lo(__head_move_kernel-__head_reference),gr18
  207. sethi.p %hi(__head_kernel_moved-__head_reference+__sdram_base),gr19
  208. setlo %lo(__head_kernel_moved-__head_reference+__sdram_base),gr19
  209. add gr18,gr26,gr18
  210. icpl gr18,gr0,#1
  211. jmpl @(gr18,gr0)
  212. .balign 32
  213. __head_move_kernel:
  214. lddu @(gr16,gr5),gr10
  215. lddu @(gr16,gr5),gr12
  216. stdu.p gr10,@(gr17,gr5)
  217. subicc gr4,#1,gr4,icc0
  218. stdu.p gr12,@(gr17,gr5)
  219. bhi icc0,#0,__head_move_kernel
  220. jmpl @(gr19,gr0)
  221. .balign 32
  222. __head_kernel_moved:
  223. icul gr18
  224. icei @(gr0,gr0),1
  225. dcei @(gr0,gr0),1
  226. LEDS 0x0006
  227. # recalculate reference address
  228. call 0f
  229. 0: movsg lr,gr26
  230. addi gr26,#__head_reference-0b,gr26
  231. ###############################################################################
  232. #
  233. # rearrange the iomem map and set the protection registers
  234. #
  235. ###############################################################################
  236. #ifdef CONFIG_MMU
  237. LEDS 0x3301
  238. call __head_fr451_set_busctl
  239. LEDS 0x3303
  240. call __head_fr451_survey_sdram
  241. LEDS 0x3305
  242. call __head_fr451_set_protection
  243. #else
  244. movsg psr,gr5
  245. srli gr5,#PSR_IMPLE_SHIFT,gr5
  246. subicc gr5,#PSR_IMPLE_FR551,gr0,icc0
  247. beq icc0,#0,__head_fr555_memmap
  248. subicc gr5,#PSR_IMPLE_FR451,gr0,icc0
  249. beq icc0,#0,__head_fr451_memmap
  250. LEDS 0x3101
  251. call __head_fr401_set_busctl
  252. LEDS 0x3103
  253. call __head_fr401_survey_sdram
  254. LEDS 0x3105
  255. call __head_fr401_set_protection
  256. bra __head_done_memmap
  257. __head_fr451_memmap:
  258. LEDS 0x3301
  259. call __head_fr401_set_busctl
  260. LEDS 0x3303
  261. call __head_fr401_survey_sdram
  262. LEDS 0x3305
  263. call __head_fr451_set_protection
  264. bra __head_done_memmap
  265. __head_fr555_memmap:
  266. LEDS 0x3501
  267. call __head_fr555_set_busctl
  268. LEDS 0x3503
  269. call __head_fr555_survey_sdram
  270. LEDS 0x3505
  271. call __head_fr555_set_protection
  272. __head_done_memmap:
  273. #endif
  274. LEDS 0x0007
  275. ###############################################################################
  276. #
  277. # turn the data cache and MMU on
  278. # - for the FR451 this'll mean that the window through which the kernel is
  279. # viewed will change
  280. #
  281. ###############################################################################
  282. #ifdef CONFIG_MMU
  283. #define MMUMODE HSR0_EIMMU|HSR0_EDMMU|HSR0_EXMMU|HSR0_EDAT|HSR0_XEDAT
  284. #else
  285. #define MMUMODE HSR0_EIMMU|HSR0_EDMMU
  286. #endif
  287. movsg hsr0,gr5
  288. sethi.p %hi(MMUMODE),gr4
  289. setlo %lo(MMUMODE),gr4
  290. or gr4,gr5,gr5
  291. #if defined(CONFIG_FRV_DEFL_CACHE_WTHRU)
  292. sethi.p %hi(HSR0_DCE|HSR0_CBM_WRITE_THRU),gr4
  293. setlo %lo(HSR0_DCE|HSR0_CBM_WRITE_THRU),gr4
  294. #elif defined(CONFIG_FRV_DEFL_CACHE_WBACK)
  295. sethi.p %hi(HSR0_DCE|HSR0_CBM_COPY_BACK),gr4
  296. setlo %lo(HSR0_DCE|HSR0_CBM_COPY_BACK),gr4
  297. #elif defined(CONFIG_FRV_DEFL_CACHE_WBEHIND)
  298. sethi.p %hi(HSR0_DCE|HSR0_CBM_COPY_BACK),gr4
  299. setlo %lo(HSR0_DCE|HSR0_CBM_COPY_BACK),gr4
  300. movsg psr,gr6
  301. srli gr6,#24,gr6
  302. cmpi gr6,#0x50,icc0 // FR451
  303. beq icc0,#0,0f
  304. cmpi gr6,#0x40,icc0 // FR405
  305. bne icc0,#0,1f
  306. 0:
  307. # turn off write-allocate
  308. sethi.p %hi(HSR0_NWA),gr6
  309. setlo %lo(HSR0_NWA),gr6
  310. or gr4,gr6,gr4
  311. 1:
  312. #else
  313. #error No default cache configuration set
  314. #endif
  315. or gr4,gr5,gr5
  316. movgs gr5,hsr0
  317. bar
  318. LEDS 0x0008
  319. sethi.p %hi(__head_mmu_enabled),gr19
  320. setlo %lo(__head_mmu_enabled),gr19
  321. jmpl @(gr19,gr0)
  322. __head_mmu_enabled:
  323. icei @(gr0,gr0),#1
  324. dcei @(gr0,gr0),#1
  325. LEDS 0x0009
  326. #ifdef CONFIG_MMU
  327. call __head_fr451_finalise_protection
  328. #endif
  329. LEDS 0x000a
  330. ###############################################################################
  331. #
  332. # set up the runtime environment
  333. #
  334. ###############################################################################
  335. # clear the BSS area
  336. sethi.p %hi(__bss_start),gr4
  337. setlo %lo(__bss_start),gr4
  338. sethi.p %hi(_end),gr5
  339. setlo %lo(_end),gr5
  340. or.p gr0,gr0,gr18
  341. or gr0,gr0,gr19
  342. 0:
  343. stdi gr18,@(gr4,#0)
  344. stdi gr18,@(gr4,#8)
  345. stdi gr18,@(gr4,#16)
  346. stdi.p gr18,@(gr4,#24)
  347. addi gr4,#24,gr4
  348. subcc gr5,gr4,gr0,icc0
  349. bhi icc0,#2,0b
  350. LEDS 0x000b
  351. # save the SDRAM details
  352. sethi.p %hi(__sdram_old_base),gr4
  353. setlo %lo(__sdram_old_base),gr4
  354. st gr24,@(gr4,gr0)
  355. sethi.p %hi(__sdram_base),gr5
  356. setlo %lo(__sdram_base),gr5
  357. sethi.p %hi(memory_start),gr4
  358. setlo %lo(memory_start),gr4
  359. st gr5,@(gr4,gr0)
  360. add gr25,gr5,gr25
  361. sethi.p %hi(memory_end),gr4
  362. setlo %lo(memory_end),gr4
  363. st gr25,@(gr4,gr0)
  364. # point the TBR at the kernel trap table
  365. sethi.p %hi(__entry_kerneltrap_table),gr4
  366. setlo %lo(__entry_kerneltrap_table),gr4
  367. movgs gr4,tbr
  368. # set up the exception frame for init
  369. sethi.p %hi(__kernel_frame0_ptr),gr28
  370. setlo %lo(__kernel_frame0_ptr),gr28
  371. sethi.p %hi(_gp),gr16
  372. setlo %lo(_gp),gr16
  373. sethi.p %hi(__entry_usertrap_table),gr4
  374. setlo %lo(__entry_usertrap_table),gr4
  375. lddi @(gr28,#0),gr28 ; load __frame & current
  376. ldi.p @(gr29,#4),gr15 ; set current_thread
  377. or gr0,gr0,fp
  378. or gr28,gr0,sp
  379. sti.p gr4,@(gr28,REG_TBR)
  380. setlos #ISR_EDE|ISR_DTT_DIVBYZERO|ISR_EMAM_EXCEPTION,gr5
  381. movgs gr5,isr
  382. # turn on and off various CPU services
  383. movsg psr,gr22
  384. sethi.p %hi(#PSR_EM|PSR_EF|PSR_CM|PSR_NEM),gr4
  385. setlo %lo(#PSR_EM|PSR_EF|PSR_CM|PSR_NEM),gr4
  386. or gr22,gr4,gr22
  387. movgs gr22,psr
  388. andi gr22,#~(PSR_PIL|PSR_PS|PSR_S),gr22
  389. ori gr22,#PSR_ET,gr22
  390. sti gr22,@(gr28,REG_PSR)
  391. ###############################################################################
  392. #
  393. # set up the registers and jump into the kernel
  394. #
  395. ###############################################################################
  396. LEDS 0x000c
  397. # initialise the processor and the peripherals
  398. #call SYMBOL_NAME(processor_init)
  399. #call SYMBOL_NAME(unit_init)
  400. #LEDS 0x0aff
  401. sethi.p #0xe5e5,gr3
  402. setlo #0xe5e5,gr3
  403. or.p gr3,gr0,gr4
  404. or gr3,gr0,gr5
  405. or.p gr3,gr0,gr6
  406. or gr3,gr0,gr7
  407. or.p gr3,gr0,gr8
  408. or gr3,gr0,gr9
  409. or.p gr3,gr0,gr10
  410. or gr3,gr0,gr11
  411. or.p gr3,gr0,gr12
  412. or gr3,gr0,gr13
  413. or.p gr3,gr0,gr14
  414. or gr3,gr0,gr17
  415. or.p gr3,gr0,gr18
  416. or gr3,gr0,gr19
  417. or.p gr3,gr0,gr20
  418. or gr3,gr0,gr21
  419. or.p gr3,gr0,gr23
  420. or gr3,gr0,gr24
  421. or.p gr3,gr0,gr25
  422. or gr3,gr0,gr26
  423. or.p gr3,gr0,gr27
  424. # or gr3,gr0,gr30
  425. or gr3,gr0,gr31
  426. movgs gr0,lr
  427. movgs gr0,lcr
  428. movgs gr0,ccr
  429. movgs gr0,cccr
  430. # initialise the virtual interrupt handling
  431. subcc gr0,gr0,gr0,icc2 /* set Z, clear C */
  432. #ifdef CONFIG_MMU
  433. movgs gr3,scr2
  434. movgs gr3,scr3
  435. #endif
  436. LEDS 0x0fff
  437. # invoke the debugging stub if present
  438. # - arch/frv/kernel/debug-stub.c will shift control directly to init/main.c
  439. # (it will not return here)
  440. break
  441. .globl __debug_stub_init_break
  442. __debug_stub_init_break:
  443. # however, if you need to use an ICE, and don't care about using any userspace
  444. # debugging tools (such as the ptrace syscall), you can just step over the break
  445. # above and get to the kernel this way
  446. # look at arch/frv/kernel/debug-stub.c: debug_stub_init() to see what you've missed
  447. call start_kernel
  448. .globl __head_end
  449. __head_end:
  450. .size _boot, .-_boot
  451. # provide a point for GDB to place a break
  452. .section .text.start,"ax"
  453. .globl _start
  454. .balign 4
  455. _start:
  456. call _boot
  457. .previous
  458. ###############################################################################
  459. #
  460. # split a tile off of the region defined by GR8-GR9
  461. #
  462. # ENTRY: EXIT:
  463. # GR4 - IAMPR value representing tile
  464. # GR5 - DAMPR value representing tile
  465. # GR6 - IAMLR value representing tile
  466. # GR7 - DAMLR value representing tile
  467. # GR8 region base pointer [saved]
  468. # GR9 region top pointer updated to exclude new tile
  469. # GR11 xAMLR mask [saved]
  470. # GR25 SDRAM size [saved]
  471. # GR30 LED address [saved]
  472. #
  473. # - GR8 and GR9 should be rounded up/down to the nearest megabyte before calling
  474. #
  475. ###############################################################################
  476. .globl __head_split_region
  477. .type __head_split_region,@function
  478. __head_split_region:
  479. subcc.p gr9,gr8,gr4,icc0
  480. setlos #31,gr5
  481. scan.p gr4,gr0,gr6
  482. beq icc0,#0,__head_region_empty
  483. sub.p gr5,gr6,gr6 ; bit number of highest set bit (1MB=>20)
  484. setlos #1,gr4
  485. sll.p gr4,gr6,gr4 ; size of region (1 << bitno)
  486. subi gr6,#17,gr6 ; 1MB => 0x03
  487. slli.p gr6,#4,gr6 ; 1MB => 0x30
  488. sub gr9,gr4,gr9 ; move uncovered top down
  489. or gr9,gr6,gr4
  490. ori gr4,#xAMPRx_S_USER|xAMPRx_C_CACHED|xAMPRx_V,gr4
  491. or.p gr4,gr0,gr5
  492. and gr4,gr11,gr6
  493. and.p gr5,gr11,gr7
  494. bralr
  495. __head_region_empty:
  496. or.p gr0,gr0,gr4
  497. or gr0,gr0,gr5
  498. or.p gr0,gr0,gr6
  499. or gr0,gr0,gr7
  500. bralr
  501. .size __head_split_region, .-__head_split_region
  502. ###############################################################################
  503. #
  504. # write the 32-bit hex number in GR8 to ttyS0
  505. #
  506. ###############################################################################
  507. #if 0
  508. .globl __head_write_to_ttyS0
  509. .type __head_write_to_ttyS0,@function
  510. __head_write_to_ttyS0:
  511. sethi.p %hi(0xfeff9c00),gr31
  512. setlo %lo(0xfeff9c00),gr31
  513. setlos #8,gr20
  514. 0: ldubi @(gr31,#5*8),gr21
  515. andi gr21,#0x60,gr21
  516. subicc gr21,#0x60,gr21,icc0
  517. bne icc0,#0,0b
  518. 1: srli gr8,#28,gr21
  519. slli gr8,#4,gr8
  520. addi gr21,#'0',gr21
  521. subicc gr21,#'9',gr0,icc0
  522. bls icc0,#2,2f
  523. addi gr21,#'A'-'0'-10,gr21
  524. 2:
  525. stbi gr21,@(gr31,#0*8)
  526. subicc gr20,#1,gr20,icc0
  527. bhi icc0,#2,1b
  528. setlos #'\r',gr21
  529. stbi gr21,@(gr31,#0*8)
  530. setlos #'\n',gr21
  531. stbi gr21,@(gr31,#0*8)
  532. 3: ldubi @(gr31,#5*8),gr21
  533. andi gr21,#0x60,gr21
  534. subicc gr21,#0x60,gr21,icc0
  535. bne icc0,#0,3b
  536. bralr
  537. .size __head_write_to_ttyS0, .-__head_write_to_ttyS0
  538. #endif