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