arch/sparc/cpu/leon3/start.S

   1 /* This is where the SPARC/LEON3 starts
   2  * Copyright (C) 2007,
   3  * Daniel Hellstrom, daniel@gaisler.com
   4  *
   5  * SPDX-License-Identifier:     GPL-2.0+
   6  */
   7
   8 #include <asm-offsets.h>
   9 #include <config.h>
  10 #include <asm/asmmacro.h>
  11 #include <asm/winmacro.h>
  12 #include <asm/psr.h>
  13 #include <asm/stack.h>
  14 #include <asm/leon.h>
  15 #include <version.h>
  16 #include <ambapp.h>
  17
  18 /* Default Plug&Play I/O area */
  19 #ifndef CONFIG_AMBAPP_IOAREA
  20 #define CONFIG_AMBAPP_IOAREA AMBA_DEFAULT_IOAREA
  21 #endif
  22
  23 /* Entry for traps which jump to a programmer-specified trap handler.  */
  24 #define TRAPR(H)  \
  25         wr      %g0, 0xfe0, %psr; \
  26         mov     %g0, %tbr; \
  27         ba      (H); \
  28         mov     %g0, %wim;
  29
  30 #define TRAP(H) \
  31         mov     %psr, %l0; \
  32         ba      (H); \
  33         nop; nop;
  34
  35 #define TRAPI(ilevel) \
  36         mov     ilevel, %l7; \
  37         mov     %psr, %l0; \
  38         b       _irq_entry; \
  39         mov     %wim, %l3
  40
  41 /* Unexcpected trap will halt the processor by forcing it to error state */
  42 #undef BAD_TRAP
  43 #define BAD_TRAP ta 0; nop; nop; nop;
  44
  45 /* Software trap. Treat as BAD_TRAP for the time being... */
  46 #define SOFT_TRAP TRAP(_hwerr)
  47
  48 #define PSR_INIT   0x1FC0       /* Disable traps, set s and ps */
  49 #define WIM_INIT   2
  50
  51 /* All traps low-level code here must end with this macro. */
  52 #define RESTORE_ALL b ret_trap_entry; clr %l6;
  53
  54 #define WRITE_PAUSE nop;nop;nop
  55
  56 WINDOWSIZE = (16 * 4)
  57 ARGPUSHSIZE = (6 * 4)
  58 ARGPUSH = (WINDOWSIZE + 4)
  59 MINFRAME = (WINDOWSIZE + ARGPUSHSIZE + 4)
  60
  61 /* Number of register windows */
  62 #ifndef CONFIG_SYS_SPARC_NWINDOWS
  63 #error Must define number of SPARC register windows, default is 8
  64 #endif
  65
  66 /* Macros to load address into a register. Uses GOT table for PIC */
  67 #ifdef __PIC__
  68
  69 #define SPARC_PIC_THUNK_CALL(reg) \
  70         sethi   %pc22(_GLOBAL_OFFSET_TABLE_-4), %##reg; \
  71         call    __sparc_get_pc_thunk.reg; \
  72          add    %##reg, %pc10(_GLOBAL_OFFSET_TABLE_+4), %##reg;
  73
  74 #define SPARC_LOAD_ADDRESS(sym, got, reg) \
  75         sethi   %gdop_hix22(sym), %##reg; \
  76         xor     %##reg, %gdop_lox10(sym), %##reg; \
  77         ld      [%##got + %##reg], %##reg, %gdop(sym);
  78
  79 #else
  80
  81 #define SPARC_PIC_THUNK_CALL(reg)
  82 #define SPARC_LOAD_ADDRESS(sym, got, tmp) \
  83         set     sym, %##reg;
  84
  85 #endif
  86
  87 #define STACK_ALIGN     8
  88 #define SA(X)   (((X)+(STACK_ALIGN-1)) & ~(STACK_ALIGN-1))
  89
  90         .section ".start", "ax"
  91         .globl  _start, start, _trap_table
  92         .globl  _irq_entry, nmi_trap
  93         .globl  _reset_reloc
  94
  95 /* at address 0
  96  * Hardware traps
  97  */
  98 start:
  99 _start:
 100 _trap_table:
 101         TRAPR(_hardreset);              ! 00 reset trap
 102         BAD_TRAP;                       ! 01 instruction_access_exception
 103         BAD_TRAP;                       ! 02 illegal_instruction
 104         BAD_TRAP;                       ! 03 priveleged_instruction
 105         BAD_TRAP;                       ! 04 fp_disabled
 106         TRAP(_window_overflow);         ! 05 window_overflow
 107         TRAP(_window_underflow);        ! 06 window_underflow
 108         BAD_TRAP;                       ! 07 Memory Address Not Aligned
 109         BAD_TRAP;                       ! 08 Floating Point Exception
 110         BAD_TRAP;                       ! 09 Data Miss Exception
 111         BAD_TRAP;                       ! 0a Tagged Instruction Ovrflw
 112         BAD_TRAP;                       ! 0b Watchpoint Detected
 113         BAD_TRAP;                       ! 0c
 114         BAD_TRAP;                       ! 0d
 115         BAD_TRAP;                       ! 0e
 116         BAD_TRAP;                       ! 0f
 117         BAD_TRAP;                       ! 10
 118         TRAPI(1);                       ! 11 IRQ level 1
 119         TRAPI(2);                       ! 12 IRQ level 2
 120         TRAPI(3);                       ! 13 IRQ level 3
 121         TRAPI(4);                       ! 14 IRQ level 4
 122         TRAPI(5);                       ! 15 IRQ level 5
 123         TRAPI(6);                       ! 16 IRQ level 6
 124         TRAPI(7);                       ! 17 IRQ level 7
 125         TRAPI(8);                       ! 18 IRQ level 8
 126         TRAPI(9);                       ! 19 IRQ level 9
 127         TRAPI(10);                      ! 1a IRQ level 10
 128         TRAPI(11);                      ! 1b IRQ level 11
 129         TRAPI(12);                      ! 1c IRQ level 12
 130         TRAPI(13);                      ! 1d IRQ level 13
 131         TRAPI(14);                      ! 1e IRQ level 14
 132         TRAP(_nmi_trap);                ! 1f IRQ level 15 /
 133                                         ! NMI (non maskable interrupt)
 134         BAD_TRAP;                       ! 20 r_register_access_error
 135         BAD_TRAP;                       ! 21 instruction access error
 136         BAD_TRAP;                       ! 22
 137         BAD_TRAP;                       ! 23
 138         BAD_TRAP;                       ! 24 co-processor disabled
 139         BAD_TRAP;                       ! 25 uniplemented FLUSH
 140         BAD_TRAP;                       ! 26
 141         BAD_TRAP;                       ! 27
 142         BAD_TRAP;                       ! 28 co-processor exception
 143         BAD_TRAP;                       ! 29 data access error
 144         BAD_TRAP;                       ! 2a division by zero
 145         BAD_TRAP;                       ! 2b data store error
 146         BAD_TRAP;                       ! 2c data access MMU miss
 147         BAD_TRAP;                       ! 2d
 148         BAD_TRAP;                       ! 2e
 149         BAD_TRAP;                       ! 2f
 150         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 30-33
 151         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 34-37
 152         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 38-3b
 153         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 3c-3f
 154         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 40-43
 155         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 44-47
 156         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 48-4b
 157         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 4c-4f
 158         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 50-53
 159         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 54-57
 160         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 58-5b
 161         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 5c-5f
 162
 163         /* implementaion dependent */
 164         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 60-63
 165         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 64-67
 166         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 68-6b
 167         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 6c-6f
 168         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 70-73
 169         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 74-77
 170         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 78-7b
 171         BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP; ! 7c-7f
 172
 173         /* Software traps, not handled */
 174         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 80-83
 175         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 84-87
 176         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 88-8b
 177         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 8c-8f
 178         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 90-93
 179         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 94-97
 180         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 98-9b
 181         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! 9c-9f
 182         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! a0-a3
 183         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! a4-a7
 184         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! a8-ab
 185         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! ac-af
 186         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! b0-b3
 187         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! b4-b7
 188         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! b8-bb
 189         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! bc-bf
 190         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! c0-c3
 191         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! c4-c7
 192         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! c8-cb
 193         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! cc-cf
 194         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! d0-d3
 195         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! d4-d7
 196         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! d8-db
 197         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! dc-df
 198         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! e0-e3
 199         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! e4-e7
 200         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! e8-eb
 201         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! ec-ef
 202         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! f0-f3
 203         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! f4-f7
 204         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! f8-fb
 205         SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;     ! fc-ff
 206 /*
 207  * Version string
 208  */
 209
 210         .data
 211         .extern leon3_snooping_avail
 212         .globl  version_string
 213 version_string:
 214         .ascii U_BOOT_VERSION_STRING, "\0"
 215
 216         .section        ".text"
 217         .extern _nomem_amba_init, _nomem_memory_ctrl_init
 218         .align 4
 219
 220 _hardreset:
 221 1000:
 222         flush
 223
 224         /* Enable I/D-Cache and Snooping */
 225         set     0x0081000f, %g2
 226         sta     %g2, [%g0] 2
 227
 228         mov     %g0, %y
 229         clr     %g1
 230         clr     %g2
 231         clr     %g3
 232         clr     %g4
 233         clr     %g5
 234         clr     %g6
 235         clr     %g7
 236
 237         mov     %asr17, %g3
 238         and     %g3, 0x1f, %g3
 239 clear_window:
 240         mov     %g0, %l0
 241         mov     %g0, %l1
 242         mov     %g0, %l2
 243         mov     %g0, %l3
 244         mov     %g0, %l4
 245         mov     %g0, %l5
 246         mov     %g0, %l6
 247         mov     %g0, %l7
 248         mov     %g0, %o0
 249         mov     %g0, %o1
 250         mov     %g0, %o2
 251         mov     %g0, %o3
 252         mov     %g0, %o4
 253         mov     %g0, %o5
 254         mov     %g0, %o6
 255         mov     %g0, %o7
 256         subcc   %g3, 1, %g3
 257         bge     clear_window
 258         save
 259
 260 wiminit:
 261         set     WIM_INIT, %g3
 262         mov     %g3, %wim
 263
 264 stackp:
 265         set     CONFIG_SYS_INIT_SP_OFFSET, %fp
 266         andn    %fp, 0x0f, %fp
 267         sub     %fp, 64, %sp
 268
 269 /* Obtain the address of _GLOBAL_OFFSET_TABLE_ */
 270         SPARC_PIC_THUNK_CALL(l7)
 271
 272 /* Scan AMBA Bus for AMBA buses using PnP information. All found
 273  * AMBA buses I/O area will be located in i0-i5 upon return.
 274  * The i0-i5 registers are later used by _nomem_amba_init2
 275  */
 276 ambainit:
 277         call    _nomem_amba_init
 278          sethi  %hi(CONFIG_AMBAPP_IOAREA), %o0
 279
 280 /* Scan AMBA Buses for memory controllers, then initialize the
 281  * memory controllers. Note that before setting up the memory controller
 282  * the stack can not be used.
 283  */
 284 memory_ctrl_init:
 285         SPARC_LOAD_ADDRESS(grlib_mctrl_handlers, l7, o0)
 286
 287         call    _nomem_memory_ctrl_init
 288          nop
 289
 290 /* The return valu indicate how many memory controllers where found and
 291  * initialized, if no memory controller was initialized, we can not continue
 292  * because from here on we expect memory to be working.
 293  */
 294         cmp     %o0, 0
 295 memory_ctrl_init_failed:
 296         beq     memory_ctrl_init_failed
 297          nop
 298
 299 /*** From now on the stack can be used. ***/
 300
 301 cpu_init_unreloc:
 302         call    cpu_init_f
 303          nop
 304
 305 /* un relocated start address of monitor */
 306 #define TEXT_START _text
 307
 308 /* un relocated end address of monitor */
 309 #define DATA_END __init_end
 310
 311 reloc:
 312         SPARC_LOAD_ADDRESS(TEXT_START, l7, g2)
 313         SPARC_LOAD_ADDRESS(DATA_END, l7, g3)
 314         set     CONFIG_SYS_RELOC_MONITOR_BASE,%g4
 315 reloc_loop:
 316         ldd     [%g2],%l0
 317         ldd     [%g2+8],%l2
 318         std     %l0,[%g4]
 319         std     %l2,[%g4+8]
 320         inc     16,%g2
 321         subcc   %g3,%g2,%g0
 322         bne     reloc_loop
 323          inc    16,%g4
 324
 325         clr     %l0
 326         clr     %l1
 327         clr     %l2
 328         clr     %l3
 329         clr     %g2
 330
 331 /* register g4 contain address to start
 332  * This means that BSS must be directly after data and code segments
 333  *
 334  * g3 is length of bss = (__bss_end-__bss_start)
 335  *
 336  */
 337
 338 clr_bss:
 339 /* clear bss area (the relocated) */
 340         SPARC_LOAD_ADDRESS(__bss_start, l7, g2)
 341         SPARC_LOAD_ADDRESS(__bss_end, l7, g3)
 342         sub     %g3,%g2,%g3
 343         add     %g3,%g4,%g3
 344         clr     %g1     /* std %g0 uses g0 and g1 */
 345 /* clearing 16byte a time ==> linker script need to align to 16 byte offset */
 346 clr_bss_16:
 347         std     %g0,[%g4]
 348         std     %g0,[%g4+8]
 349         inc     16,%g4
 350         cmp     %g3,%g4
 351         bne     clr_bss_16
 352          nop
 353
 354 /* add offsets to GOT table */
 355 fixup_got:
 356         SPARC_LOAD_ADDRESS(__got_start, l7, g4)
 357         SPARC_LOAD_ADDRESS(__got_end, l7, g3)
 358 /*
 359  * new got offset = (old GOT-PTR (read with ld) -
 360  *   CONFIG_SYS_RELOC_MONITOR_BASE(from define) ) +
 361  *   Destination Address (from define)
 362  */
 363         set     CONFIG_SYS_RELOC_MONITOR_BASE,%g2
 364         SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
 365         add     %g4,%g2,%g4
 366         sub     %g4,%g1,%g4
 367         add     %g3,%g2,%g3
 368         sub     %g3,%g1,%g3
 369         sub     %g2,%g1,%g2     ! prepare register with (new base address) -
 370                                 !  (old base address)
 371 got_loop:
 372         ld      [%g4],%l0       ! load old GOT-PTR
 373         add     %l0,%g2,%l0     ! increase with (new base address) -
 374                                 !  (old base)
 375         st      %l0,[%g4]
 376         inc     4,%g4
 377         cmp     %g3,%g4
 378         bne     got_loop
 379          nop
 380
 381 prom_relocate:
 382         SPARC_LOAD_ADDRESS(__prom_start, l7, g2)
 383         SPARC_LOAD_ADDRESS(__prom_end, l7, g3)
 384         set     CONFIG_SYS_PROM_OFFSET, %g4
 385
 386 prom_relocate_loop:
 387         ldd     [%g2],%l0
 388         ldd     [%g2+8],%l2
 389         std     %l0,[%g4]
 390         std     %l2,[%g4+8]
 391         inc     16,%g2
 392         subcc   %g3,%g2,%g0
 393         bne     prom_relocate_loop
 394          inc    16,%g4
 395
 396 /* Trap table has been moved, lets tell CPU about
 397  * the new trap table address
 398  */
 399
 400         set     CONFIG_SYS_RELOC_MONITOR_BASE, %g2
 401         wr      %g0, %g2, %tbr
 402         nop
 403         nop
 404         nop
 405
 406 /* If CACHE snooping is available in hardware the
 407  * variable leon3_snooping_avail will be set to
 408  * 0x800000 else 0.
 409  */
 410 snoop_detect:
 411         sethi   %hi(0x00800000), %o0
 412         lda     [%g0] 2, %o1
 413         and     %o0, %o1, %o0
 414         sethi   %hi(leon3_snooping_avail+CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE), %o1
 415         st      %o0, [%lo(leon3_snooping_avail+CONFIG_SYS_RELOC_MONITOR_BASE-CONFIG_SYS_TEXT_BASE)+%o1]
 416
 417 /*      call    relocate*/
 418         nop
 419 /* Call relocated init functions */
 420 jump:
 421         SPARC_LOAD_ADDRESS(cpu_init_f2, l7, o1)
 422         set     CONFIG_SYS_RELOC_MONITOR_BASE,%o2
 423         add     %o1,%o2,%o1
 424         sub     %o1,%g1,%o1
 425         call    %o1
 426          clr    %o0
 427
 428         SPARC_LOAD_ADDRESS(board_init_f, l7, o1)
 429         set     CONFIG_SYS_RELOC_MONITOR_BASE,%o2
 430         SPARC_LOAD_ADDRESS(TEXT_START, l7, g1)
 431         add     %o1,%o2,%o1
 432         sub     %o1,%g1,%o1
 433         call    %o1
 434          clr    %o0
 435
 436 dead:   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
 563 trap_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
 597 1:
 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
 607 ret_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          */
 643 1:
 644         PT_LOAD_ALL(sp, t_psr, t_pc, t_npc, g1)
 645 2:
 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