kernel/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         sethi.p         #0xe5e5,gr3
 483         setlo           #0xe5e5,gr3
 484         or.p            gr3,gr0,gr4
 485         or              gr3,gr0,gr5
 486         or.p            gr3,gr0,gr6
 487         or              gr3,gr0,gr7
 488         or.p            gr3,gr0,gr8
 489         or              gr3,gr0,gr9
 490         or.p            gr3,gr0,gr10
 491         or              gr3,gr0,gr11
 492         or.p            gr3,gr0,gr12
 493         or              gr3,gr0,gr13
 494         or.p            gr3,gr0,gr14
 495         or              gr3,gr0,gr17
 496         or.p            gr3,gr0,gr18
 497         or              gr3,gr0,gr19
 498         or.p            gr3,gr0,gr20
 499         or              gr3,gr0,gr21
 500         or.p            gr3,gr0,gr23
 501         or              gr3,gr0,gr24
 502         or.p            gr3,gr0,gr25
 503         or              gr3,gr0,gr26
 504         or.p            gr3,gr0,gr27
 505 #       or              gr3,gr0,gr30
 506         or              gr3,gr0,gr31
 507         movgs           gr0,lr
 508         movgs           gr0,lcr
 509         movgs           gr0,ccr
 510         movgs           gr0,cccr
 511
 512         # initialise the virtual interrupt handling
 513         subcc           gr0,gr0,gr0,icc2                /* set Z, clear C */
 514
 515 #ifdef CONFIG_MMU
 516         movgs           gr3,scr2
 517         movgs           gr3,scr3
 518 #endif
 519
 520         LEDS            0x0fff
 521
 522         # invoke the debugging stub if present
 523         # - arch/frv/kernel/debug-stub.c will shift control directly to init/main.c
 524         #   (it will not return here)
 525         break
 526         .globl          __debug_stub_init_break
 527 __debug_stub_init_break:
 528
 529         # however, if you need to use an ICE, and don't care about using any userspace
 530         # debugging tools (such as the ptrace syscall), you can just step over the break
 531         # above and get to the kernel this way
 532         # look at arch/frv/kernel/debug-stub.c: debug_stub_init() to see what you've missed
 533         call            start_kernel
 534
 535         .globl          __head_end
 536 __head_end:
 537         .size           _boot, .-_boot
 538
 539         # provide a point for GDB to place a break
 540         .section        .text..start,"ax"
 541         .globl          _start
 542         .balign         4
 543 _start:
 544         call            _boot
 545
 546         .previous
 547 ###############################################################################
 548 #
 549 # split a tile off of the region defined by GR8-GR9
 550 #
 551 #       ENTRY:                  EXIT:
 552 # GR4   -                       IAMPR value representing tile
 553 # GR5   -                       DAMPR value representing tile
 554 # GR6   -                       IAMLR value representing tile
 555 # GR7   -                       DAMLR value representing tile
 556 # GR8   region base pointer     [saved]
 557 # GR9   region top pointer      updated to exclude new tile
 558 # GR11  xAMLR mask              [saved]
 559 # GR25  SDRAM size              [saved]
 560 # GR30  LED address             [saved]
 561 #
 562 # - GR8 and GR9 should be rounded up/down to the nearest megabyte before calling
 563 #
 564 ###############################################################################
 565         .globl          __head_split_region
 566         .type           __head_split_region,@function
 567 __head_split_region:
 568         subcc.p         gr9,gr8,gr4,icc0
 569         setlos          #31,gr5
 570         scan.p          gr4,gr0,gr6
 571         beq             icc0,#0,__head_region_empty
 572         sub.p           gr5,gr6,gr6                     ; bit number of highest set bit (1MB=>20)
 573         setlos          #1,gr4
 574         sll.p           gr4,gr6,gr4                     ; size of region (1 << bitno)
 575         subi            gr6,#17,gr6                     ; 1MB => 0x03
 576         slli.p          gr6,#4,gr6                      ; 1MB => 0x30
 577         sub             gr9,gr4,gr9                     ; move uncovered top down
 578
 579         or              gr9,gr6,gr4
 580         ori             gr4,#xAMPRx_S_USER|xAMPRx_C_CACHED|xAMPRx_V,gr4
 581         or.p            gr4,gr0,gr5
 582
 583         and             gr4,gr11,gr6
 584         and.p           gr5,gr11,gr7
 585         bralr
 586
 587 __head_region_empty:
 588         or.p            gr0,gr0,gr4
 589         or              gr0,gr0,gr5
 590         or.p            gr0,gr0,gr6
 591         or              gr0,gr0,gr7
 592         bralr
 593         .size           __head_split_region, .-__head_split_region
 594
 595 ###############################################################################
 596 #
 597 # write the 32-bit hex number in GR8 to ttyS0
 598 #
 599 ###############################################################################
 600 #if 0
 601         .globl          __head_write_to_ttyS0
 602         .type           __head_write_to_ttyS0,@function
 603 __head_write_to_ttyS0:
 604         sethi.p         %hi(0xfeff9c00),gr31
 605         setlo           %lo(0xfeff9c00),gr31
 606         setlos          #8,gr20
 607
 608 0:      ldubi           @(gr31,#5*8),gr21
 609         andi            gr21,#0x60,gr21
 610         subicc          gr21,#0x60,gr21,icc0
 611         bne             icc0,#0,0b
 612
 613 1:      srli            gr8,#28,gr21
 614         slli            gr8,#4,gr8
 615
 616         addi            gr21,#'0',gr21
 617         subicc          gr21,#'9',gr0,icc0
 618         bls             icc0,#2,2f
 619         addi            gr21,#'A'-'0'-10,gr21
 620 2:
 621         stbi            gr21,@(gr31,#0*8)
 622         subicc          gr20,#1,gr20,icc0
 623         bhi             icc0,#2,1b
 624
 625         setlos          #'\r',gr21
 626         stbi            gr21,@(gr31,#0*8)
 627
 628         setlos          #'\n',gr21
 629         stbi            gr21,@(gr31,#0*8)
 630
 631 3:      ldubi           @(gr31,#5*8),gr21
 632         andi            gr21,#0x60,gr21
 633         subicc          gr21,#0x60,gr21,icc0
 634         bne             icc0,#0,3b
 635         bralr
 636
 637         .size           __head_write_to_ttyS0, .-__head_write_to_ttyS0
 638 #endif