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