LXR uboot/arch/sparc/cpu/leon2/start.S

   1/* This is where the SPARC/LEON3 starts
   2 * Copyright (C) 2007,
   3 * Daniel Hellstrom, daniel@gaisler.com
   4 *
   5 * See file CREDITS for list of people who contributed to this
   6 * project.
   7 *
   8 * This program is free software; you can redistribute it and/or
   9 * modify it under the terms of the GNU General Public License as
  10 * published by the Free Software Foundation; either version 2 of
  11 * the License, or (at your option) any later version.
  12 *
  13 * This program is distributed in the hope that it will be useful,
  14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16 * GNU General Public License for more details.
  17 *
  18 * You should have received a copy of the GNU General Public License
  19 * along with this program; if not, write to the Free Software
  20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  21 * MA 02111-1307 USA
  22 */
  23
  24#include <asm-offsets.h>
  25#include <config.h>
  26#include <asm/asmmacro.h>
  27#include <asm/winmacro.h>
  28#include <asm/psr.h>
  29#include <asm/stack.h>
  30#include <asm/leon.h>
  31#include <version.h>
  32
  33/* Entry for traps which jump to a programmer-specified trap handler.  */
  34#define TRAPR(H)  \
  35        wr      %g0, 0xfe0, %psr; \
  36        mov     %g0, %tbr; \
  37        ba      (H); \
  38        mov     %g0, %wim;
  39
  40#define TRAP(H) \
  41        mov     %psr, %l0; \
  42        ba      (H); \
  43        nop; nop;
  44
  45#define TRAPI(ilevel) \
  46        mov     ilevel, %l7; \
  47        mov     %psr, %l0; \
  48        b       _irq_entry; \
  49        mov     %wim, %l3
  50
  51/* Unexcpected trap will halt the processor by forcing it to error state */
  52#undef BAD_TRAP
  53#define BAD_TRAP ta 0; nop; nop; nop;
  54
  55/* Software trap. Treat as BAD_TRAP for the time being... */
  56#define SOFT_TRAP TRAP(_hwerr)
  57
  58#define PSR_INIT   0x1FC0       /* Disable traps, set s and ps */
  59#define WIM_INIT   2
  60
  61/* All traps low-level code here must end with this macro. */
  62#define RESTORE_ALL b ret_trap_entry; clr %l6;
  63
  64#define WRITE_PAUSE nop;nop;nop
  65
  66WINDOWSIZE = (16 * 4)
  67ARGPUSHSIZE = (6 * 4)
  68ARGPUSH = (WINDOWSIZE + 4)
  69MINFRAME = (WINDOWSIZE + ARGPUSHSIZE + 4)
  70
  71/* Number of register windows */
  72#ifndef CONFIG_SYS_SPARC_NWINDOWS
  73#error Must define number of SPARC register windows, default is 8
  74#endif
  75
  76#define STACK_ALIGN     8
  77#define SA(X)   (((X)+(STACK_ALIGN-1)) & ~(STACK_ALIGN-1))
  78
  79        .section ".start", "ax"
  80        .globl  _start, start, _trap_table
  81        .globl  _irq_entry, nmi_trap
  82        .globl  _reset_reloc
  83
  84/* at address 0
  85 * Hardware traps
  86 */
  87start:
  88_start:
  89_trap_table:
  90        TRAPR(_hardreset);              ! 00 reset trap
  91        BAD_TRAP;                       ! 01 instruction_access_exception
  92        BAD_TRAP;                       ! 02 illegal_instruction
  93        BAD_TRAP;                       ! 03 priveleged_instruction
  94        BAD_TRAP;                       ! 04 fp_disabled
  95        TRAP(_window_overflow);         ! 05 window_overflow
  96        TRAP(_window_underflow);        ! 06 window_underflow
  97        BAD_TRAP;                       ! 07 Memory Address Not Aligned
  98        BAD_TRAP;                       ! 08 Floating Point Exception
  99        BAD_TRAP;                       ! 09 Data Miss Exception
 100        BAD_TRAP;                       ! 0a Tagged Instruction Ovrflw
 101        BAD_TRAP;                       ! 0b Watchpoint Detected
 102        BAD_TRAP;                       ! 0c
 103        BAD_TRAP;                       ! 0d
 104        BAD_TRAP;                       ! 0e
 105        BAD_TRAP;                       ! 0f
 106        BAD_TRAP;                       ! 10
 107        TRAPI(1);                       ! 11 IRQ level 1
 108        TRAPI(2);                       ! 12 IRQ level 2
 109        TRAPI(3);                       ! 13 IRQ level 3
 110        TRAPI(4);                       ! 14 IRQ level 4
 111        TRAPI(5);                       ! 15 IRQ level 5
 112        TRAPI(6);                       ! 16 IRQ level 6
 113        TRAPI(7);                       ! 17 IRQ level 7
 114        TRAPI(8);                       ! 18 IRQ level 8
 115        TRAPI(9);                       ! 19 IRQ level 9
 116        TRAPI(10);                      ! 1a IRQ level 10
 117        TRAPI(11);                      ! 1b IRQ level 11
 118        TRAPI(12);                      ! 1c IRQ level 12
 119        TRAPI(13);                      ! 1d IRQ level 13
 120        TRAPI(14);                      ! 1e IRQ level 14
 121        TRAP(_nmi_trap);                ! 1f IRQ level 15 /
 122                                        ! NMI (non maskable interrupt)
 123        BAD_TRAP;                       ! 20 r_register_access_error
 124        BAD_TRAP;                       ! 21 instruction access error
 125        BAD_TRAP;                       ! 22
 126        BAD_TRAP;                       ! 23
 127        BAD_TRAP;                       ! 24 co-processor disabled
 128        BAD_TRAP;                       ! 25 uniplemented FLUSH
 129        BAD_TRAP;                       ! 26
 130        BAD_TRAP;                       ! 27
 131        BAD_TRAP;                       ! 28 co-processor exception
 132        BAD_TRAP;                       ! 29 data access error
 133        BAD_TRAP;                       ! 2a division by zero
 134        BAD_TRAP;                       ! 2b data store error
 135        BAD_TRAP;                       ! 2c data access MMU miss
 136        BAD_TRAP;                       ! 2d
 137        BAD_TRAP;                       ! 2e
 138        BAD_TRAP;                       ! 2f
 139        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 30-33
 140        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 34-37
 141        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 38-3b
 142        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 3c-3f
 143        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 40-43
 144        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 44-47
 145        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 48-4b
 146        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 4c-4f
 147        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 50-53
 148        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 54-57
 149        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 58-5b
 150        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 5c-5f
 151
 152        /* implementaion dependent */
 153        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 60-63
 154        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 64-67
 155        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 68-6b
 156        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 6c-6f
 157        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 70-73
 158        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 74-77
 159        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 78-7b
 160        BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 7c-7f
 161
 162        /* Software traps, not handled */
 163        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 80-83
 164        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 84-87
 165        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 88-8b
 166        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 8c-8f
 167        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 90-93
 168        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 94-97
 169        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 98-9b
 170        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 9c-9f
 171        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! a0-a3
 172        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! a4-a7
 173        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! a8-ab
 174        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! ac-af
 175        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! b0-b3
 176        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! b4-b7
 177        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! b8-bb
 178        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! bc-bf
 179        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! c0-c3
 180        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! c4-c7
 181        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! c8-cb
 182        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! cc-cf
 183        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! d0-d3
 184        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! d4-d7
 185        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! d8-db
 186        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! dc-df
 187        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! e0-e3
 188        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! e4-e7
 189        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! e8-eb
 190        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! ec-ef
 191        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! f0-f3
 192        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! f4-f7
 193        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! f8-fb
 194        SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! fc-ff
 195/*
 196 * Version string
 197 */
 198
 199        .data
 200        .globl  version_string
 201version_string:
 202        .ascii U_BOOT_VERSION_STRING, "\0"
 203
 204        .section        ".text"
 205        .align 4
 206
 207_hardreset:
 2081000:
 209        flush
 210        nop
 211        nop
 212        nop
 213
 214        /* Init Cache */
 215        set     (LEON2_PREGS+LEON_REG_CACHECTRL_OFFSET), %g1
 216        set     0x0081000f, %g2
 217        st      %g2, [%g1]
 218
 219        mov     %g0, %y
 220        clr     %g1
 221        clr     %g2
 222        clr     %g3
 223        clr     %g4
 224        clr     %g5
 225        clr     %g6
 226        clr     %g7
 227
 228        mov     %asr17, %g3
 229        and     %g3, 0x1f, %g3
 230clear_window:
 231        mov     %g0, %l0
 232        mov     %g0, %l1
 233        mov     %g0, %l2
 234        mov     %g0, %l3
 235        mov     %g0, %l4
 236        mov     %g0, %l5
 237        mov     %g0, %l6
 238        mov     %g0, %l7
 239        mov     %g0, %o0
 240        mov     %g0, %o1
 241        mov     %g0, %o2
 242        mov     %g0, %o3
 243        mov     %g0, %o4
 244        mov     %g0, %o5
 245        mov     %g0, %o6
 246        mov     %g0, %o7
 247        subcc   %g3, 1, %g3
 248        bge     clear_window
 249        save
 250
 251leon2_init:
 252        /* LEON2 Register Base in g1 */
 253        set     LEON2_PREGS, %g1
 254
 255leon2_init_cache:
 256        /* Set Cache control register */
 257        set     0x1000f, %g2
 258        st      %g2, [%g1 + 0x14]
 259
 260leon2_init_clear:
 261
 262        /* Clear LEON2 registers */
 263        st      %g0, [%g1 + LEON2_ECTRL]
 264        st      %g0, [%g1 + LEON2_IMASK]
 265        st      %g0, [%g1 + LEON2_IPEND]
 266        st      %g0, [%g1 + LEON2_IFORCE]
 267        st      %g0, [%g1 + LEON2_ICLEAR]
 268        st      %g0, [%g1 + LEON2_IOREG]
 269        st      %g0, [%g1 + LEON2_IODIR]
 270        st      %g0, [%g1 + LEON2_IOICONF]
 271        st      %g0, [%g1 + LEON2_UCTRL0]
 272        st      %g0, [%g1 + LEON2_UCTRL1]
 273
 274leon2_init_ioport:
 275        /* I/O port initialization */
 276        set     0xaa00, %g2
 277        st      %g2, [%g1 + LEON2_IOREG]
 278
 279leon2_init_mctrl:
 280
 281        /* memory config register 1 */
 282        set     CONFIG_SYS_GRLIB_MEMCFG1, %g2
 283        ld      [%g1], %g3              !
 284        and     %g3, 0x300, %g3
 285        or      %g2, %g3, %g2
 286        st      %g2, [%g1 + LEON2_MCFG1]
 287        set     CONFIG_SYS_GRLIB_MEMCFG2, %g2           ! Load memory config register 2
 288#if !( defined(TSIM) || !defined(BZIMAGE))
 289        st      %g2, [%g1 + LEON2_MCFG2]        ! only for prom version, else done by "dumon -i"
 290#endif
 291        set     CONFIG_SYS_GRLIB_MEMCFG3, %g2           ! Init FT register
 292        st      %g2, [%g1 + LEON2_ECTRL]
 293        ld      [%g1 + LEON2_ECTRL], %g2
 294        srl     %g2, 30, %g2
 295        andcc   %g2, 3, %g6
 296        bne,a   leon2_init_wim
 297        mov     %g0, %asr16             ! clear err_reg
 298
 299leon2_init_wim:
 300        set     WIM_INIT, %g3
 301        mov     %g3, %wim
 302
 303leon2_init_psr:
 304        set     0x1000, %g3
 305        mov     %psr, %g2
 306        wr      %g2, %g3, %psr
 307        nop
 308        nop
 309        nop
 310
 311leon2_init_stackp:
 312        set     CONFIG_SYS_INIT_SP_OFFSET, %fp
 313        andn    %fp, 0x0f, %fp
 314        sub     %fp, 64, %sp
 315
 316cpu_init_unreloc:
 317        call    cpu_init_f
 318        nop
 319
 320/* un relocated start address of monitor */
 321#define TEXT_START _text
 322
 323/* un relocated end address of monitor */
 324#define DATA_END __init_end
 325
 326reloc:
 327        set     TEXT_START,%g2
 328        set     DATA_END,%g3
 329        set     CONFIG_SYS_RELOC_MONITOR_BASE,%g4
 330reloc_loop:
 331        ldd     [%g2],%l0
 332        ldd     [%g2+8],%l2
 333        std     %l0,[%g4]
 334        std     %l2,[%g4+8]
 335        inc     16,%g2
 336        subcc   %g3,%g2,%g0
 337        bne     reloc_loop
 338        inc     16,%g4
 339
 340        clr     %l0
 341        clr     %l1
 342        clr     %l2
 343        clr     %l3
 344        clr     %g2
 345
 346/* register g4 contain address to start
 347 * This means that BSS must be directly after data and code segments
 348 *
 349 * g3 is length of bss = (__bss_end-__bss_start)
 350 *
 351 */
 352
 353clr_bss:
 354/* clear bss area (the relocated) */
 355        set     __bss_start,%g2
 356        set     __bss_end,%g3
 357        sub     %g3,%g2,%g3
 358        add     %g3,%g4,%g3
 359        clr     %g1     /* std %g0 uses g0 and g1 */
 360/* clearing 16byte a time ==> linker script need to align to 16 byte offset */
 361clr_bss_16:
 362        std     %g0,[%g4]
 363        std     %g0,[%g4+8]
 364        inc     16,%g4
 365        cmp     %g3,%g4
 366        bne     clr_bss_16
 367        nop
 368
 369/* add offsets to GOT table */
 370fixup_got:
 371        set     __got_start,%g4
 372        set     __got_end,%g3
 373/*
 374 * new got offset = (old GOT-PTR (read with ld) -
 375 *   CONFIG_SYS_RELOC_MONITOR_BASE(from define) ) +
 376 *   Destination Address (from define)
 377 */
 378        set     CONFIG_SYS_RELOC_MONITOR_BASE,%g2
 379        set     TEXT_START, %g1
 380        add     %g4,%g2,%g4
 381        sub     %g4,%g1,%g4
 382        add     %g3,%g2,%g3
 383        sub     %g3,%g1,%g3
 384        sub     %g2,%g1,%g2     ! prepare register with (new base address) -
 385                                !  (old base address)
 386got_loop:
 387        ld      [%g4],%l0       ! load old GOT-PTR
 388        add     %l0,%g2,%l0     ! increase with (new base address) -
 389                                !  (old base)
 390        st      %l0,[%g4]
 391        inc     4,%g4
 392        cmp     %g3,%g4
 393        bne     got_loop
 394        nop
 395
 396prom_relocate:
 397        set     __prom_start, %g2
 398        set     __prom_end, %g3
 399        set     CONFIG_SYS_PROM_OFFSET, %g4
 400
 401prom_relocate_loop:
 402        ldd     [%g2],%l0
 403        ldd     [%g2+8],%l2
 404        std     %l0,[%g4]
 405        std     %l2,[%g4+8]
 406        inc     16,%g2
 407        subcc   %g3,%g2,%g0
 408        bne     prom_relocate_loop
 409        inc     16,%g4
 410
 411/* Trap table has been moved, lets tell CPU about
 412 * the new trap table address
 413 */
 414
 415        set     CONFIG_SYS_RELOC_MONITOR_BASE, %g2
 416        wr      %g0, %g2, %tbr
 417
 418/*      call    relocate*/
 419        nop
 420/* Call relocated init functions */
 421jump:
 422        set     cpu_init_f2,%o1
 423        set     CONFIG_SYS_RELOC_MONITOR_BASE,%o2
 424        add     %o1,%o2,%o1
 425        sub     %o1,%g1,%o1
 426        call    %o1
 427        clr     %o0
 428
 429        set     board_init_f,%o1
 430        set     CONFIG_SYS_RELOC_MONITOR_BASE,%o2
 431        add     %o1,%o2,%o1
 432        sub     %o1,%g1,%o1
 433        call    %o1
 434        clr     %o0
 435
 436dead:   ta 0                            ! if call returns...
 437        nop
 438
 439/* Interrupt handler caller,
 440 * reg L7: interrupt number
 441 * reg L0: psr after interrupt
 442 * reg L1: PC
 443 * reg L2: next PC
 444 * reg L3: wim
 445 */
 446_irq_entry:
 447        SAVE_ALL
 448
 449        or      %l0, PSR_PIL, %g2
 450        wr      %g2, 0x0, %psr
 451        WRITE_PAUSE
 452        wr      %g2, PSR_ET, %psr
 453        WRITE_PAUSE
 454        mov     %l7, %o0                ! irq level
 455        set     handler_irq, %o1
 456        set     (CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %o2
 457        add     %o1, %o2, %o1
 458        call    %o1
 459        add     %sp, SF_REGS_SZ, %o1    ! pt_regs ptr
 460        or      %l0, PSR_PIL, %g2       ! restore PIL after handler_irq
 461        wr      %g2, PSR_ET, %psr       ! keep ET up
 462        WRITE_PAUSE
 463
 464        RESTORE_ALL
 465
 466!Window overflow trap handler.
 467        .global _window_overflow
 468
 469_window_overflow:
 470
 471        mov     %wim, %l3               ! Calculate next WIM
 472        mov     %g1, %l7
 473        srl     %l3, 1, %g1
 474        sll     %l3, (CONFIG_SYS_SPARC_NWINDOWS-1) , %l4
 475        or      %l4, %g1, %g1
 476
 477        save                            ! Get into window to be saved.
 478        mov     %g1, %wim
 479        nop;
 480        nop;
 481        nop
 482        st      %l0, [%sp + 0];
 483        st      %l1, [%sp + 4];
 484        st      %l2, [%sp + 8];
 485        st      %l3, [%sp + 12];
 486        st      %l4, [%sp + 16];
 487        st      %l5, [%sp + 20];
 488        st      %l6, [%sp + 24];
 489        st      %l7, [%sp + 28];
 490        st      %i0, [%sp + 32];
 491        st      %i1, [%sp + 36];
 492        st      %i2, [%sp + 40];
 493        st      %i3, [%sp + 44];
 494        st      %i4, [%sp + 48];
 495        st      %i5, [%sp + 52];
 496        st      %i6, [%sp + 56];
 497        st      %i7, [%sp + 60];
 498        restore                         ! Go back to trap window.
 499        mov     %l7, %g1
 500        jmp     %l1                     ! Re-execute save.
 501        rett    %l2
 502
 503/* Window underflow trap handler.  */
 504
 505        .global  _window_underflow
 506
 507_window_underflow:
 508
 509        mov  %wim, %l3                  ! Calculate next WIM
 510        sll  %l3, 1, %l4
 511        srl  %l3, (CONFIG_SYS_SPARC_NWINDOWS-1), %l5
 512        or   %l5, %l4, %l5
 513        mov  %l5, %wim
 514        nop; nop; nop
 515        restore                         ! Two restores to get into the
 516        restore                         ! window to restore
 517        ld      [%sp + 0], %l0;         ! Restore window from the stack
 518        ld      [%sp + 4], %l1;
 519        ld      [%sp + 8], %l2;
 520        ld      [%sp + 12], %l3;
 521        ld      [%sp + 16], %l4;
 522        ld      [%sp + 20], %l5;
 523        ld      [%sp + 24], %l6;
 524        ld      [%sp + 28], %l7;
 525        ld      [%sp + 32], %i0;
 526        ld      [%sp + 36], %i1;
 527        ld      [%sp + 40], %i2;
 528        ld      [%sp + 44], %i3;
 529        ld      [%sp + 48], %i4;
 530        ld      [%sp + 52], %i5;
 531        ld      [%sp + 56], %i6;
 532        ld      [%sp + 60], %i7;
 533        save                            ! Get back to the trap window.
 534        save
 535        jmp     %l1                     ! Re-execute restore.
 536        rett    %l2
 537
 538        retl
 539
 540_nmi_trap:
 541        nop
 542        jmp %l1
 543        rett %l2
 544
 545_hwerr:
 546        ta 0
 547        nop
 548        nop
 549        b _hwerr                        ! loop infinite
 550        nop
 551
 552/* Registers to not touch at all. */
 553#define t_psr      l0 /* Set by caller */
 554#define t_pc       l1 /* Set by caller */
 555#define t_npc      l2 /* Set by caller */
 556#define t_wim      l3 /* Set by caller */
 557#define t_twinmask l4 /* Set at beginning of this entry routine. */
 558#define t_kstack   l5 /* Set right before pt_regs frame is built */
 559#define t_retpc    l6 /* If you change this, change winmacro.h header file */
 560#define t_systable l7 /* Never touch this, could be the syscall table ptr. */
 561#define curptr     g6 /* Set after pt_regs frame is built */
 562
 563trap_setup:
 564/* build a pt_regs trap frame. */
 565        sub     %fp, (SF_REGS_SZ + PT_REGS_SZ), %t_kstack
 566        PT_STORE_ALL(t_kstack, t_psr, t_pc, t_npc, g2)
 567
 568        /* See if we are in the trap window. */
 569        mov     1, %t_twinmask
 570        sll     %t_twinmask, %t_psr, %t_twinmask ! t_twinmask = (1 << psr)
 571        andcc   %t_twinmask, %t_wim, %g0
 572        beq     1f              ! in trap window, clean up
 573        nop
 574
 575        /*-------------------------------------------------
 576         * Spill , adjust %wim and go.
 577         */
 578        srl     %t_wim, 0x1, %g2                ! begin computation of new %wim
 579
 580        set     (CONFIG_SYS_SPARC_NWINDOWS-1), %g3      !NWINDOWS-1
 581
 582        sll     %t_wim, %g3, %t_wim     ! NWINDOWS-1
 583        or      %t_wim, %g2, %g2
 584        and     %g2, 0xff, %g2
 585
 586        save    %g0, %g0, %g0           ! get in window to be saved
 587
 588        /* Set new %wim value */
 589        wr      %g2, 0x0, %wim
 590
 591        /* Save the kernel window onto the corresponding stack. */
 592        RW_STORE(sp)
 593
 594        restore %g0, %g0, %g0
 595        /*-------------------------------------------------*/
 596
 5971:
 598        /* Trap from kernel with a window available.
 599         * Just do it...
 600         */
 601        jmpl    %t_retpc + 0x8, %g0     ! return to caller
 602         mov    %t_kstack, %sp          ! jump onto new stack
 603
 604#define twin_tmp1 l4
 605#define glob_tmp  g4
 606#define curptr    g6
 607ret_trap_entry:
 608        wr      %t_psr, 0x0, %psr       ! enable nesting again, clear ET
 609
 610        /* Will the rett land us in the invalid window? */
 611        mov     2, %g1
 612        sll     %g1, %t_psr, %g1
 613
 614        set     CONFIG_SYS_SPARC_NWINDOWS, %g2  !NWINDOWS
 615
 616        srl     %g1, %g2, %g2
 617        or      %g1, %g2, %g1
 618        rd      %wim, %g2
 619        andcc   %g2, %g1, %g0
 620        be      1f              ! Nope, just return from the trap
 621         sll    %g2, 0x1, %g1
 622
 623        /* We have to grab a window before returning. */
 624        set     (CONFIG_SYS_SPARC_NWINDOWS-1), %g3      !NWINDOWS-1
 625
 626        srl     %g2, %g3,  %g2
 627        or      %g1, %g2, %g1
 628        and     %g1, 0xff, %g1
 629
 630        wr      %g1, 0x0, %wim
 631
 632        /* Grrr, make sure we load from the right %sp... */
 633        PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)
 634
 635        restore %g0, %g0, %g0
 636        RW_LOAD(sp)
 637        b       2f
 638        save    %g0, %g0, %g0
 639
 640        /* Reload the entire frame in case this is from a
 641         * kernel system call or whatever...
 642         */
 6431:
 644        PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)
 6452:
 646        wr      %t_psr, 0x0, %psr
 647        nop;
 648        nop;
 649        nop
 650
 651        jmp     %t_pc
 652        rett    %t_npc
 653
 654/* This is called from relocated C-code.
 655 * It resets the system by jumping to _start
 656 */
 657_reset_reloc:
 658        set     start, %l0
 659        call    %l0
 660        nop
 661