sparc: Move libcpu content to cpukit

This patch is a part of the BSP source reorganization.

Update #3285.

8 files changed, 756 insertions, 4 deletions

diff --git a/cpukit/score/cpu/sparc/Makefile.am b/cpukit/score/cpu/sparc/Makefile.am
index 1a76a2f009..817cc11741 100644
--- a/cpukit/score/cpu/sparc/Makefile.am
+++ b/cpukit/score/cpu/sparc/Makefile.am
@@ -1,12 +1,16 @@
 include $(top_srcdir)/automake/compile.am
 
 noinst_LIBRARIES = libscorecpu.a
-libscorecpu_a_SOURCES = cpu.c
-libscorecpu_a_SOURCES += sparc-context-volatile-clobber.S
+libscorecpu_a_SOURCES =
+libscorecpu_a_SOURCES += access_le.c
+libscorecpu_a_SOURCES += access.S
+libscorecpu_a_SOURCES += cpu.c
 libscorecpu_a_SOURCES += sparc-context-validate.S
-libscorecpu_a_SOURCES += sparc-counter.c
+libscorecpu_a_SOURCES += sparc-context-volatile-clobber.S
 libscorecpu_a_SOURCES += sparc-counter-asm.S
-libscorecpu_a_CPPFLAGS = $(AM_CPPFLAGS)
+libscorecpu_a_SOURCES += sparc-counter.c
+libscorecpu_a_SOURCES += syscall.S
+libscorecpu_a_SOURCES += window.S
 
 include $(top_srcdir)/automake/local.am
 include $(srcdir)/headers.am
diff --git a/cpukit/score/cpu/sparc/access.S b/cpukit/score/cpu/sparc/access.S
new file mode 100644
index 0000000000..9397cb815b
--- /dev/null
+++ b/cpukit/score/cpu/sparc/access.S
@@ -0,0 +1,109 @@
+/*
+ *  Optimized access routines for SPARC.
+ *
+ *  Note the difference between byteorder.h (inlined functions) and access.S
+ *  where the functions will be declared in the library archive librtemscpu.a.
+ *  Function names starting with _ are in library and can be referenced by
+ *  function pointers.
+ *
+ *  _ldN, _stN          standard machine endianess access (SPARC: big-endian)
+ *  _ld_beN, _st_beN    forced big-endian
+ *  _ld_leN, _st_leN    forced little-endian (defined in access_le.C)
+ *
+ *  This file is written in assembly because the big-endian functions maps to
+ *  machine dependant access methods, i.e. same function has two names.
+ *
+ *  COPYRIGHT (c) 2011
+ *  Aeroflex Gaisler.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <rtems/asm.h>
+
+	.align 4
+	.section ".text"
+	PUBLIC(_ld8)
+	PUBLIC(_ld16)
+	PUBLIC(_ld32)
+	PUBLIC(_ld64)
+	PUBLIC(_st8)
+	PUBLIC(_st16)
+	PUBLIC(_st32)
+	PUBLIC(_st64)
+	PUBLIC(_ld_be16)
+	PUBLIC(_ld_be32)
+	PUBLIC(_ld_be64)
+	PUBLIC(_st_be16)
+	PUBLIC(_st_be32)
+	PUBLIC(_st_be64)
+
+SYM(_ld8):
+	retl
+	 ldub [%o0], %o0
+
+SYM(_ld_be16):
+SYM(_ld16):
+	retl
+	 lduh [%o0], %o0
+
+SYM(_ld_be32):
+SYM(_ld32):
+	retl
+	 ld [%o0], %o0
+
+SYM(_ld_be64):
+SYM(_ld64):
+	retl
+	 ldd [%o0], %o0
+
+#if defined(__FIX_LEON3FT_B2BST)
+
+SYM(_st8):
+	stub %o1, [%o0]
+	retl
+	 nop
+
+SYM(_st_be16):
+SYM(_st16):
+	stuh %o1, [%o0]
+	retl
+	 nop
+
+SYM(_st_be32):
+SYM(_st32):
+	st %o1, [%o0]
+	retl
+	 nop
+
+SYM(_st_be64):
+SYM(_st64):
+	std %o1, [%o0]
+	retl
+	 nop
+
+#else
+
+SYM(_st8):
+	retl
+	 stb %o1, [%o0]
+
+SYM(_st_be16):
+SYM(_st16):
+	retl
+	 sth %o1, [%o0]
+
+SYM(_st_be32):
+SYM(_st32):
+	retl
+	 st %o1, [%o0]
+
+SYM(_st_be64):
+SYM(_st64):
+	mov %o2, %o3
+	mov %o1, %o2
+	retl
+	 std %o2, [%o0]
+#endif
diff --git a/cpukit/score/cpu/sparc/access_le.c b/cpukit/score/cpu/sparc/access_le.c
new file mode 100644
index 0000000000..d3a0e93adb
--- /dev/null
+++ b/cpukit/score/cpu/sparc/access_le.c
@@ -0,0 +1,33 @@
+/*
+ *  Little-endian access routines for SPARC
+ *
+ *  COPYRIGHT (c) 2011
+ *  Aeroflex Gaisler.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#include <libcpu/byteorder.h>
+#include <libcpu/access.h>
+
+uint16_t _ld_le16(uint16_t *addr)
+{
+	return ld_le16(addr);
+}
+
+void _st_le16(uint16_t *addr, uint16_t val)
+{
+	st_le16(addr, val);
+}
+
+uint32_t _ld_le32(uint32_t *addr)
+{
+	return ld_le32(addr);
+}
+
+void _st_le32(uint32_t *addr, uint32_t val)
+{
+	st_le32(addr, val);
+}
diff --git a/cpukit/score/cpu/sparc/headers.am b/cpukit/score/cpu/sparc/headers.am
index 0e1aea959b..f0c179c827 100644
--- a/cpukit/score/cpu/sparc/headers.am
+++ b/cpukit/score/cpu/sparc/headers.am
@@ -2,6 +2,7 @@
 
 include_libcpudir = $(includedir)/libcpu
 include_libcpu_HEADERS =
+include_libcpu_HEADERS += include/libcpu/access.h
 include_libcpu_HEADERS += include/libcpu/byteorder.h
 
 include_machinedir = $(includedir)/machine
diff --git a/cpukit/score/cpu/sparc/include/libcpu/access.h b/cpukit/score/cpu/sparc/include/libcpu/access.h
new file mode 100644
index 0000000000..cdf6b77122
--- /dev/null
+++ b/cpukit/score/cpu/sparc/include/libcpu/access.h
@@ -0,0 +1,50 @@
+/*
+ *  access.h  - access routines for SPARC. SPARC is big endian only.
+ *
+ *  COPYRIGHT (c) 2011
+ *  Aeroflex Gaisler.
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.org/license/LICENSE.
+ */
+
+#ifndef _LIBCPU_ACCESS_H
+#define _LIBCPU_ACCESS_H
+
+#include <rtems/system.h>
+#include <rtems/score/cpu.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* "Raw" access */
+extern uint8_t _ld8(uint8_t *addr);
+extern void _st8(uint8_t *addr, uint8_t val);
+extern uint16_t _ld16(uint16_t *addr);
+extern void _st16(uint16_t *addr, uint16_t val);
+extern uint32_t _ld32(uint32_t *addr);
+extern void _st32(uint32_t *addr, uint32_t val);
+extern uint64_t _ld64(uint64_t *addr);
+extern void _st64(uint64_t *addr, uint64_t val);
+
+/* Aliases for Big Endian */
+extern uint16_t _ld_be16(uint16_t *addr);
+extern void _st_be16(uint16_t *addr, uint16_t val);
+extern uint32_t _ld_be32(uint32_t *addr);
+extern void _st_be32(uint32_t *addr, uint32_t val);
+extern uint64_t _ld_be64(uint64_t *addr);
+extern void _st_be64(uint64_t *addr, uint64_t val);
+
+/* Little endian */
+extern uint16_t _ld_le16(uint16_t *addr);
+extern void _st_le16(uint16_t *addr, uint16_t val);
+extern uint32_t _ld_le32(uint32_t *addr);
+extern void _st_le32(uint32_t *addr, uint32_t val);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/cpukit/score/cpu/sparc/syscall.S b/cpukit/score/cpu/sparc/syscall.S
new file mode 100644
index 0000000000..da0ee43889
--- /dev/null
+++ b/cpukit/score/cpu/sparc/syscall.S
@@ -0,0 +1,295 @@
+/*
+ *  systrap.S
+ *
+ *  This file contains emulated system calls using software trap 0.
+ *  The following calls are supported:
+ *
+ *    + SYS_exit	(halt)
+ *    + SYS_irqdis	(disable interrupts)
+ *    + SYS_irqset	(set interrupt level)
+ *
+ *  COPYRIGHT:
+ *
+ *  COPYRIGHT (c) 1995. European Space Agency.
+ *  Copyright (c) 2016, 2017 embedded brains GmbH
+ *
+ *  This terms of the RTEMS license apply to this file.
+ *
+ */
+
+#include <rtems/asm.h>
+#include <rtems/score/cpuimpl.h>
+#include <rtems/score/percpu.h>
+#include "syscall.h"
+
+	.section    ".text"
+	/*
+	 *  system call - halt
+	 *
+	 *  On entry:
+	 *
+	 *    l0 = psr (from trap table)
+	 *    l1 = pc
+	 *    l2 = npc
+	 *    g1 = system call id (1)
+	 *
+	 *  System Call 1 (exit):
+	 *    g2 = additional exit code 1
+	 *    g3 = additional exit code 2
+	 */
+
+	PUBLIC(syscall)
+
+SYM(syscall):
+	ta	0			! syscall 1, halt with %g1,%g2,%g3 info
+
+	PUBLIC(sparc_syscall_exit)
+
+SYM(sparc_syscall_exit):
+
+	mov	SYS_exit, %g1
+	mov	%o0, %g2	! Additional exit code 1
+	mov	%o1, %g3	! Additional exit code 2
+	ta	SPARC_SWTRAP_SYSCALL
+
+	/*
+	 *  system call - Interrupt Disable
+	 *
+	 *  On entry:
+	 *
+	 *    l0 = psr (from trap table)
+	 *    l1 = pc
+	 *    l2 = npc
+	 *    l3 = psr | SPARC_PSR_PIL_MASK
+	 *
+	 *  On exit:
+	 *    g1 = old psr (to user)
+	 */
+
+.align 32				! Align to 32-byte cache-line
+	PUBLIC(syscall_irqdis)
+
+SYM(syscall_irqdis):
+	mov	%l3, %psr			! Set PSR. Write delay 3 instr
+	or	%l0, SPARC_PSR_ET_MASK, %g1	! return old PSR with ET=1
+	nop					! PSR write delay
+	jmp	%l2				! Return to after TA 9.
+	 rett	%l2 + 4
+
+	/*
+	 *  system call - Interrupt Enable
+	 *
+	 *  On entry:
+	 *
+	 *    l0 = psr (from trap table)
+	 *    l1 = pc
+	 *    l2 = npc
+	 *    l3 = psr & ~0x0f00
+	 *    g1 = new PIL to write (from user)
+	 */
+
+.align 32				! Align to 32-byte cache-line
+	PUBLIC(syscall_irqen)
+
+SYM(syscall_irqen):
+	and	%g1, SPARC_PSR_PIL_MASK, %l4	! %l4 = (%g1 & 0xf00)
+	wr	%l3, %l4, %psr			! PSR = (PSR & ~0xf00) ^ %l4
+	nop; nop				! PSR write delay;
+	jmp	%l2				! Return to after TA 10.
+	 rett	%l2 + 4
+
+#if defined(SPARC_USE_SYNCHRONOUS_FP_SWITCH)
+	/*
+	 *  system call - Interrupt disable and set PSR[EF] according to caller
+	 *                specified %g1
+	 *
+	 *  On entry:
+	 *
+	 *    g1 = the desired PSR[EF] value (from caller)
+	 *    l0 = psr (from trap table)
+	 *    l1 = pc
+	 *    l2 = npc
+	 *    l3 = psr | SPARC_PSR_PIL_MASK
+	 *
+	 *  On exit:
+	 *    g1 = old psr (to user)
+	 */
+
+.align 32				! Align to 32-byte cache-line
+	PUBLIC(syscall_irqdis_fp)
+
+SYM(syscall_irqdis_fp):
+	/*
+	 * We cannot use an intermediate value for operations with the PSR[EF]
+	 * bit since they use a 13-bit sign extension and PSR[EF] is bit 12.
+	 */
+	sethi	%hi(SPARC_PSR_EF_MASK), %l4
+
+	andn	%l3, %l4, %l3			! Clear PSR[EF]
+	and	%g1, %l4, %g1			! Select PSR[EF] only from %g1
+	or	%l3, %g1, %l3			! Set PSR[EF] according to %g1
+	mov	%l3, %psr			! Set PSR. Write delay 3 instr
+	or	%l0, SPARC_PSR_ET_MASK, %g1	! return old PSR with ET=1
+	nop					! PSR write delay
+	jmp	%l2				! Return to after TA 9.
+	 rett	%l2 + 4
+#endif
+
+#if defined(SPARC_USE_LAZY_FP_SWITCH)
+
+	/*
+	 *  system call - Perform a lazy floating point switch
+	 *
+	 *  On entry:
+	 *
+	 *    l0 = psr (from trap table)
+	 *    l1 = pc
+	 *    l2 = npc
+	 *    l3 = SPARC_PSR_EF_MASK
+	 */
+
+.align 32				! Align to 32-byte cache-line
+	PUBLIC(syscall_lazy_fp_switch)
+
+SYM(syscall_lazy_fp_switch):
+	ld	[%g6 + PER_CPU_OFFSET_EXECUTING], %l4
+	ld	[%g6 + PER_CPU_ISR_NEST_LEVEL], %l5
+	ld	[%l4 + %lo(SPARC_THREAD_CONTROL_FP_CONTEXT_OFFSET)], %l6
+	ld	[%g6 + SPARC_PER_CPU_FP_OWNER_OFFSET], %l7
+
+	/* Ensure that we are not in interrupt context */
+	cmp	%l5, 0
+	bne	.Lillegal_use_of_floating_point_unit
+	 or	%l0, %l3, %l0
+
+	/* Ensure that we are a proper floating point thread */
+	cmp	%l6, 0
+	be	.Lillegal_use_of_floating_point_unit
+	 ld	[%l4 + %lo(SPARC_THREAD_CONTROL_REGISTERS_FP_CONTEXT_OFFSET)], %l6
+
+	/* Set PSR[EF] to 1, PSR write delay 3 instructions! */
+	mov	%l0, %psr
+
+	/*
+	 * Check if there is a floating point owner.  We have to check this
+	 * here, since the floating point owner may have been deleted in the
+	 * meantime.  Save the floating point context if necessary.
+	 */
+	cmp	%l7, 0
+	be	.Lfp_save_done
+	 nop
+	ld	[%l7 + %lo(SPARC_THREAD_CONTROL_FP_CONTEXT_OFFSET)], %l5
+	std	%f0, [%l5 + SPARC_FP_CONTEXT_OFFSET_F0_F1]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f2, [%l5 + SPARC_FP_CONTEXT_OFFSET_F2_F3]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f4, [%l5 + SPARC_FP_CONTEXT_OFFSET_F4_F5]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f6, [%l5 + SPARC_FP_CONTEXT_OFFSET_F6_F7]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f8, [%l5 + SPARC_FP_CONTEXT_OFFSET_F8_F9]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f10, [%l5 + SPARC_FP_CONTEXT_OFFSET_F10_F11]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f12, [%l5 + SPARC_FP_CONTEXT_OFFSET_F12_F13]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f14, [%l5 + SPARC_FP_CONTEXT_OFFSET_F14_F15]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f16, [%l5 + SPARC_FP_CONTEXT_OFFSET_F16_F17]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f18, [%l5 + SPARC_FP_CONTEXT_OFFSET_F18_F19]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f20, [%l5 + SPARC_FP_CONTEXT_OFFSET_F20_F21]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f22, [%l5 + SPARC_FP_CONTEXT_OFFSET_F22_F23]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f24, [%l5 + SPARC_FP_CONTEXT_OFFSET_F24_F25]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f26, [%l5 + SPARC_FP_CONTEXT_OFFSET_F26_F27]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f28, [%l5 + SPARC_FP_CONTEXT_OFFSET_F28_F29]
+	SPARC_LEON3FT_B2BST_NOP
+	std	%f30, [%l5 + SPARC_FP_CONTEXT_OFFSET_F30_F31]
+	SPARC_LEON3FT_B2BST_NOP
+	st	%fsr, [%l5 + SPARC_FP_CONTEXT_OFFSET_FSR]
+	SPARC_LEON3FT_B2BST_NOP
+	st	%g0, [%g6 + SPARC_PER_CPU_FP_OWNER_OFFSET]
+	SPARC_LEON3FT_B2BST_NOP
+	st	%l5, [%l7 + %lo(SPARC_THREAD_CONTROL_REGISTERS_FP_CONTEXT_OFFSET)]
+
+.Lfp_save_done:
+
+	/* Restore the floating point context if necessary */
+	cmp	%l6, 0
+	be	.Lfp_restore_done
+	 st	%g0, [%l4 + %lo(SPARC_THREAD_CONTROL_REGISTERS_FP_CONTEXT_OFFSET)]
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F0_F1], %f0
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F2_F3], %f2
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F4_F5], %f4
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F6_F7], %f6
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F8_F9], %f8
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F10_F11], %f10
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F12_F13], %f12
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F14_F15], %f14
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F16_F17], %f16
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F18_F19], %f18
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F20_F21], %f20
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F22_F23], %f22
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F24_F25], %f24
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F26_F27], %f26
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F28_F29], %f28
+	ldd	[%l6 + SPARC_FP_CONTEXT_OFFSET_F30_F31], %f30
+	ld	[%l6 + SPARC_FP_CONTEXT_OFFSET_FSR], %fsr
+
+.Lfp_restore_done:
+
+	/* Now, retry the floating point instruction with PSR[EF] == 1 */
+	jmp	%l1
+	 rett	%l1 + 4
+
+.Lillegal_use_of_floating_point_unit:
+
+	sethi	%hi(_Internal_error), %l1
+	or	%l1, %lo(_Internal_error), %l1
+	mov	38, %i0
+	jmp	%l1
+	 rett	%l1 + 4
+#endif
+
+#if defined(RTEMS_PARAVIRT)
+
+        PUBLIC(_SPARC_Get_PSR)
+
+SYM(_SPARC_Get_PSR):
+
+	retl
+	 rd     %psr, %o0
+
+        PUBLIC(_SPARC_Set_PSR)
+
+SYM(_SPARC_Set_PSR):
+
+	mov     %o0, %psr
+	nop
+	nop
+	nop
+	retl
+	 nop
+
+        PUBLIC(_SPARC_Get_TBR)
+
+SYM(_SPARC_Get_TBR):
+
+	retl
+	 rd    %tbr, %o0
+
+        PUBLIC(_SPARC_Set_TBR)
+
+SYM(_SPARC_Set_TBR):
+
+	retl
+	 wr    %o0, 0, %tbr
+
+#endif /* defined(RTEMS_PARAVIRT) */
+
+/* end of file */
diff --git a/cpukit/score/cpu/sparc/syscall.h b/cpukit/score/cpu/sparc/syscall.h
new file mode 100644
index 0000000000..2f20886840
--- /dev/null
+++ b/cpukit/score/cpu/sparc/syscall.h
@@ -0,0 +1 @@
+#define SYS_exit        1
diff --git a/cpukit/score/cpu/sparc/window.S b/cpukit/score/cpu/sparc/window.S
new file mode 100644
index 0000000000..5a36fd65be
--- /dev/null
+++ b/cpukit/score/cpu/sparc/window.S
@@ -0,0 +1,259 @@
+/*
+ *  window.s
+ *
+ *  This file contains the register window management routines for the
+ *  SPARC architecture.  Trap handlers for the following capabilities
+ *  are included in this file:
+ *
+ *    + Window Overflow
+ *    + Window Underflow
+ *    + Flushing All Windows
+ *
+ *  COPYRIGHT:
+ *
+ *  This file includes the window overflow and underflow handlers from
+ *  the file srt0.s provided with the binary distribution of the SPARC
+ *  Instruction Simulator (SIS) found at
+ *  ftp://ftp.estec.esa.nl/pub/ws/wsd/erc32.
+ *
+ *  COPYRIGHT (c) 1995. European Space Agency.
+ *
+ *  This terms of the RTEMS license apply to this file.
+ */
+
+#include <rtems/asm.h>
+
+        .section    ".text"
+        /*
+         *  Window overflow trap handler.
+         *
+         *  On entry:
+         *
+         *    prev regwin l1 = pc
+         *    prev regwin l2 = npc
+         */
+
+        PUBLIC(window_overflow_trap_handler)
+
+SYM(window_overflow_trap_handler):
+
+        /*
+         *  Calculate new WIM by "rotating" the valid bits in the WIM right
+         *  by one position.  The following shows how the bits move for a SPARC
+         *  cpu implementation where SPARC_NUMBER_OF_REGISTER_WINDOWS is 8.
+         *
+         *    OLD WIM = 76543210
+         *    NEW WIM = 07654321
+         *
+         *  NOTE: New WIM must be stored in a global register since the
+         *        "save" instruction just prior to the load of the wim
+         *        register will result in the local register set changing.
+         */
+
+        std  %l0, [%sp + 0x00]           ! save local register set
+        SPARC_LEON3FT_B2BST_NOP
+        std  %l2, [%sp + 0x08]
+        mov  %wim, %l3
+        sll  %l3, SPARC_NUMBER_OF_REGISTER_WINDOWS-1 , %l2
+                                         ! l2  = WIM << (Number Windows - 1)
+        std  %l4, [%sp + 0x10]
+        SPARC_LEON3FT_B2BST_NOP
+        std  %l6, [%sp + 0x18]
+        srl  %l3, 1, %l3                 ! l3  = WIM >> 1
+        wr   %l3, %l2, %wim              ! WIM = (WIM >> 1) ^
+                                         !       (WIM << (Number Windows - 1))
+                                         ! 3 instruction delay not needed here
+        std  %i0, [%sp + 0x20]           ! save input register set
+        SPARC_LEON3FT_B2BST_NOP
+        std  %i2, [%sp + 0x28]
+        SPARC_LEON3FT_B2BST_NOP
+        std  %i4, [%sp + 0x30]
+        SPARC_LEON3FT_B2BST_NOP
+        std  %i6, [%sp + 0x38]
+        restore                          ! Go back to trap window.
+        jmp  %l1                         ! Re-execute save.
+         rett %l2
+
+        /*
+         *  Window underflow trap handler.
+         *
+         *  On entry:
+         *
+         *    l1 = pc
+         *    l2 = npc
+         *    l3 = wim (from trap vector)
+         *    l4 = wim << 1 (from trap vector)
+         */
+
+        PUBLIC(window_underflow_trap_handler)
+
+SYM(window_underflow_trap_handler):
+
+        /*
+         *  Calculate new WIM by "rotating" the valid bits in the WIM left
+         *  by one position.  The following shows how the bits move for a SPARC
+         *  cpu implementation where SPARC_NUMBER_OF_REGISTER_WINDOWS is 8.
+         *
+         *    OLD WIM = 76543210
+         *    NEW WIM = 07654321
+         *
+         *  NOTE: New WIM must be stored in a global register since the
+         *        "save" instruction just prior to the load of the wim
+         *        register will result in the local register set changing.
+         */
+
+        srl  %l3, SPARC_NUMBER_OF_REGISTER_WINDOWS-1, %l5
+        or   %l5, %l4, %l5              ! l5 = (WIM << 1) |
+                                        !      (WIM >> (Number Windows-1))
+        mov  %l5, %wim                  ! load the new WIM
+        nop; nop; nop                   ! 3 slot delay
+        restore                         ! Two restores to get into the
+        restore                         ! window to restore
+        ldd  [%sp + 0x00], %l0          ! First the local register set
+        ldd  [%sp + 0x08], %l2
+        ldd  [%sp + 0x10], %l4
+        ldd  [%sp + 0x18], %l6
+        ldd  [%sp + 0x20], %i0          ! Then the input registers
+        ldd  [%sp + 0x28], %i2
+        ldd  [%sp + 0x30], %i4
+        ldd  [%sp + 0x38], %i6
+        save                            ! Get back to the trap window.
+        save
+        jmp  %l1                        ! Re-execute restore.
+         rett  %l2
+
+        /*
+         *  Flush All Windows trap handler.
+         *
+         *  Flush all windows with valid contents except the current one
+         *  and the one we will be returning to.
+         *
+         *  In examining the set register windows, one may logically divide
+         *  the windows into sets (some of which may be empty) based on their
+         *  current status:
+         *
+         *    + current (i.e. in use),
+         *    + used (i.e. a restore would not trap)
+         *    + invalid (i.e. 1 in corresponding bit in WIM)
+         *    + unused
+         *
+         *  Either the used or unused set of windows may be empty.
+         *
+         *  NOTE: We assume only one bit is set in the WIM at a time.
+         *
+         *  Given a CWP of 5 and a WIM of 0x1, the registers are divided
+         *  into sets as follows:
+         *
+         *    + 0   - invalid
+         *    + 1-4 - unused
+         *    + 5   - current
+         *    + 6-7 - used
+         *
+         *  In this case, we only would save the used windows which we
+         *  will not be returning to -- 6.
+         *
+         *    Register Usage while saving the windows:
+         *      g1 = current PSR
+         *      g2 = current wim
+         *      g3 = CWP
+         *      g4 = wim scratch
+         *      g5 = scratch
+         *
+         *  On entry:
+         *
+         *    l0 = psr (from trap table)
+         *    l1 = pc
+         *    l2 = npc
+         */
+
+        PUBLIC(window_flush_trap_handler)
+
+SYM(window_flush_trap_handler):
+        /*
+         *  Save the global registers we will be using
+         */
+
+        mov     %g1, %l3
+        mov     %g2, %l4
+        mov     %g3, %l5
+        mov     %g4, %l6
+        mov     %g5, %l7
+
+        mov     %l0, %g1                      ! g1 = psr
+        mov     %wim, %g2                     ! g2 = wim
+        and     %l0, SPARC_PSR_CWP_MASK, %g3  ! g3 = CWP
+
+        add     %g3, 1, %g5                   ! g5 = CWP + 1
+        and     %g5, SPARC_NUMBER_OF_REGISTER_WINDOWS - 1, %g5
+
+        mov     1, %g4
+        sll     %g4, %g5, %g4                 ! g4 = WIM mask for CWP+1 invalid
+
+        restore                               ! go back one register window
+
+save_frame_loop:
+        sll     %g4, 1, %g5                   ! rotate the "wim" left 1
+        srl     %g4, SPARC_NUMBER_OF_REGISTER_WINDOWS - 1, %g4
+        or      %g4, %g5, %g4                 ! g4 = wim if we do one restore
+
+        /*
+         *  If a restore would not underflow, then continue.
+         */
+
+        andcc   %g4, %g2, %g0                 ! Any windows to flush?
+        bnz     done_flushing                 ! No, then continue
+        nop
+
+        restore                               ! back one window
+
+        /*
+         *  Now save the window just as if we overflowed to it.
+         */
+
+        std     %l0, [%sp + CPU_STACK_FRAME_L0_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+        std     %l2, [%sp + CPU_STACK_FRAME_L2_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+        std     %l4, [%sp + CPU_STACK_FRAME_L4_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+        std     %l6, [%sp + CPU_STACK_FRAME_L6_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+
+        std     %i0, [%sp + CPU_STACK_FRAME_I0_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+        std     %i2, [%sp + CPU_STACK_FRAME_I2_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+        std     %i4, [%sp + CPU_STACK_FRAME_I4_OFFSET]
+        SPARC_LEON3FT_B2BST_NOP
+        std     %i6, [%sp + CPU_STACK_FRAME_I6_FP_OFFSET]
+
+        ba      save_frame_loop
+        nop
+
+done_flushing:
+
+        add     %g3, 2, %g3                   ! calculate desired WIM
+        and     %g3, SPARC_NUMBER_OF_REGISTER_WINDOWS - 1, %g3
+        mov     1, %g4
+        sll     %g4, %g3, %g4                 ! g4 = new WIM
+        mov     %g4, %wim
+
+        mov     %g1, %psr                     ! restore PSR
+        nop
+        nop
+        nop
+
+        /*
+         *  Restore the global registers we used
+         */
+
+        mov     %l3, %g1
+        mov     %l4, %g2
+        mov     %l5, %g3
+        mov     %l6, %g4
+        mov     %l7, %g5
+
+        jmpl    %l2, %g0
+        rett    %l2 + 4
+
+/* end of file */