LXR linux/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
  450:      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
 2250:      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
 2740:      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
 3650:
 366        # turn off write-allocate
 367        sethi.p         %hi(HSR0_NWA),gr6
 368        setlo           %lo(HSR0_NWA),gr6
 369        or              gr4,gr6,gr4
 3701:
 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
 4120:
 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
 6080:      ldubi           @(gr31,#5*8),gr21
 609        andi            gr21,#0x60,gr21
 610        subicc          gr21,#0x60,gr21,icc0
 611        bne             icc0,#0,0b
 612
 6131:      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
 6202:
 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
 6313:      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
 639