146 lines
3.6 KiB
ArmAsm
146 lines
3.6 KiB
ArmAsm
/*
|
|
* Copyright (c) 2019-2020 Cobham Gaisler AB
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <toolchain.h>
|
|
#include <linker/sections.h>
|
|
#include <offsets_short.h>
|
|
#include <arch/sparc/sparc.h>
|
|
|
|
GTEXT(z_sparc_arch_switch)
|
|
GTEXT(z_sparc_context_switch)
|
|
GTEXT(z_thread_entry_wrapper)
|
|
|
|
/* In this implementation, switch_handle is the thread itself. */
|
|
SECTION_FUNC(TEXT, z_sparc_arch_switch)
|
|
ba z_sparc_context_switch
|
|
sub %o1, ___thread_t_switch_handle_OFFSET, %o1
|
|
|
|
/*
|
|
* This is a leaf function, so only out registers
|
|
* can be used without saving their context first.
|
|
*
|
|
* o0: new thread to restore
|
|
* o1: old thread to save
|
|
*/
|
|
SECTION_FUNC(TEXT, z_sparc_context_switch)
|
|
mov %y, %o4
|
|
st %o4, [%o1 + _thread_offset_to_y]
|
|
std %l0, [%o1 + _thread_offset_to_l0_and_l1]
|
|
std %l2, [%o1 + _thread_offset_to_l2]
|
|
std %l4, [%o1 + _thread_offset_to_l4]
|
|
std %l6, [%o1 + _thread_offset_to_l6]
|
|
std %i0, [%o1 + _thread_offset_to_i0]
|
|
std %i2, [%o1 + _thread_offset_to_i2]
|
|
std %i4, [%o1 + _thread_offset_to_i4]
|
|
std %i6, [%o1 + _thread_offset_to_i6]
|
|
|
|
std %o6, [%o1 + _thread_offset_to_o6]
|
|
|
|
rd %psr, %o4
|
|
st %o4, [%o1 + _thread_offset_to_psr]
|
|
|
|
and %o4, PSR_CWP, %g3 /* %g3 = CWP */
|
|
andn %o4, PSR_ET, %g1 /* %g1 = psr with traps disabled */
|
|
wr %g1, %psr /* disable traps */
|
|
nop
|
|
nop
|
|
nop
|
|
|
|
rd %wim, %g2 /* %g2 = wim */
|
|
mov 1, %g4
|
|
sll %g4, %g3, %g4 /* %g4 = wim mask for CW invalid */
|
|
|
|
.Lsave_frame_loop:
|
|
sll %g4, 1, %g5 /* rotate wim left by 1 */
|
|
srl %g4, (CONFIG_SPARC_NWIN-1), %g4
|
|
or %g4, %g5, %g4 /* %g4 = wim if we do one restore */
|
|
|
|
/* if restore would not underflow, continue */
|
|
andcc %g4, %g2, %g0 /* window to flush? */
|
|
bnz .Ldone_flushing /* continue */
|
|
nop
|
|
restore /* go one window back */
|
|
|
|
/* essentially the same as window overflow */
|
|
/* sp still points to task stack */
|
|
std %l0, [%sp + 0x00]
|
|
std %l2, [%sp + 0x08]
|
|
std %l4, [%sp + 0x10]
|
|
std %l6, [%sp + 0x18]
|
|
std %i0, [%sp + 0x20]
|
|
std %i2, [%sp + 0x28]
|
|
std %i4, [%sp + 0x30]
|
|
std %i6, [%sp + 0x38]
|
|
ba .Lsave_frame_loop
|
|
nop
|
|
|
|
.Ldone_flushing:
|
|
/*
|
|
* "wrpsr" is a delayed write instruction so wait three instructions
|
|
* after the write before using non-global registers or instructions
|
|
* affecting the CWP.
|
|
*/
|
|
wr %g1, %psr /* restore cwp */
|
|
nop
|
|
nop
|
|
nop
|
|
add %g3, 1, %g2 /* calculate desired wim */
|
|
cmp %g2, (CONFIG_SPARC_NWIN-1) /* check if wim is in range */
|
|
bg,a .Lwim_overflow
|
|
mov 0, %g2
|
|
|
|
.Lwim_overflow:
|
|
|
|
mov 1, %g4
|
|
sll %g4, %g2, %g4 /* %g4 = new wim */
|
|
wr %g4, %wim
|
|
nop
|
|
nop
|
|
nop
|
|
|
|
ldd [%o0 + _thread_offset_to_y], %o4
|
|
mov %o4, %y
|
|
|
|
/* restore local registers */
|
|
ldd [%o0 + _thread_offset_to_l0_and_l1], %l0
|
|
ldd [%o0 + _thread_offset_to_l2], %l2
|
|
ldd [%o0 + _thread_offset_to_l4], %l4
|
|
ldd [%o0 + _thread_offset_to_l6], %l6
|
|
|
|
/* restore input registers */
|
|
ldd [%o0 + _thread_offset_to_i0], %i0
|
|
ldd [%o0 + _thread_offset_to_i2], %i2
|
|
ldd [%o0 + _thread_offset_to_i4], %i4
|
|
ldd [%o0 + _thread_offset_to_i6], %i6
|
|
|
|
/* restore output registers */
|
|
ldd [%o0 + _thread_offset_to_o6], %o6
|
|
#ifdef CONFIG_THREAD_LOCAL_STORAGE
|
|
ld [%o0 + _thread_offset_to_tls], %g7
|
|
#endif
|
|
|
|
ld [%o0 + _thread_offset_to_psr], %g1 /* %g1 = new thread psr */
|
|
|
|
andn %g1, PSR_CWP, %g1 /* psr without cwp */
|
|
or %g1, %g3, %g1 /* psr with new cwp */
|
|
wr %g1, %psr /* restore status register and ET */
|
|
nop
|
|
nop
|
|
nop
|
|
|
|
/* jump into thread */
|
|
jmp %o7 + 8
|
|
nop
|
|
|
|
SECTION_FUNC(TEXT, z_thread_entry_wrapper)
|
|
mov %g0, %o7
|
|
ld [%sp + 0x40], %o0
|
|
ld [%sp + 0x44], %o1
|
|
ld [%sp + 0x48], %o2
|
|
ld [%sp + 0x4C], %o3
|
|
call z_thread_entry
|
|
nop
|