arch/frv/kernel/head.S

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