arch/sparc/cpu/leon2/start.S

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