/*
Copyright 2020 Shenzhen Academy of Aerospace Technology

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

Change Logs:
Date           Author       Notes
2020-10-16     Dystopia     the first version
*/

#define PSR_INIT 0x10C0
#define PREGS 0x80000000
#define IMASK 0x90
#define ICLEAR 0x9c
#define NWINDOWS 8
#define CPU_INTERRUPT_FRAME_SIZE (0x60 + 0x50 + 34 * 4)
#define SPARC_PSR_PIL_MASK 0x00000F00
#define SPARC_PSR_ET_MASK 0x00000020
#define SPARC_PSR_CWP_MASK 0x07

.text

.globl system_vectors
.globl _reset
.globl _context_switch

_reset:
	mov %g0, %asr16
	mov %g0, %asr17
	nop
	nop
	nop
	
	set PSR_INIT, %g1
	mov %g1, %psr
	nop
	nop
	nop
	
	mov %g0, %wim
	nop
	nop
	nop
	
	mov %g0, %g1
	mov %g0, %g2
	mov %g0, %g3
	mov %g0, %g4
	mov %g0, %g5
	mov %g0, %g6
	mov %g0, %g7
	
	mov 0x8, %g1
1:
	mov %g0, %l0
	mov %g0, %l1
	mov %g0, %l2
	mov %g0, %l3
	mov %g0, %l4
	mov %g0, %l5
	mov %g0, %l6
	mov %g0, %l7
	mov %g0, %i0
	mov %g0, %i1
	mov %g0, %i2
	mov %g0, %i3
	mov %g0, %i4
	mov %g0, %i5
	mov %g0, %i6
	mov %g0, %i7
	subcc %g1, 1, %g1
	save
	bne 1b
	nop
	
	set 2, %g1
	mov %g1, %wim
	nop
	nop
	nop
	
	sethi %hi(system_vectors), %g1
	mov %g1, %tbr
	nop
	nop
	nop
	
	set PREGS, %g1
	set 0xffff, %g2
	st %g2, [%g1 + ICLEAR]
	st %g0, [%g1 + IMASK]
	
	set 0x7C47907F, %g2
	st %g2, [%g1 + 4]
	
	set PSR_INIT | 0x20, %g1
	mov %g1, %psr
	nop
	nop
	nop
	
	set _fsrinit, %g1
	ld [%g1], %fsr
	nop
	nop
	nop

	set _fpdata, %g1
	ldd [%g1], %f0
	ldd [%g1], %f2
	ldd [%g1], %f4
	ldd [%g1], %f6
	ldd [%g1], %f8
	ldd [%g1], %f10
	ldd [%g1], %f12
	ldd [%g1], %f14
	ldd [%g1], %f16
	ldd [%g1], %f18
	ldd [%g1], %f20
	ldd [%g1], %f22
	ldd [%g1], %f24
	ldd [%g1], %f26
	ldd [%g1], %f28
	ldd [%g1], %f30

	set __bss_start, %g2
	set __bss_end, %g3
	mov %g0, %g1
bss_loop:
	std %g0, [%g2]
	add %g2, 8, %g2
	cmp %g2, %g3
	bleu,a bss_loop
	nop
	
	set 0x401FFF00, %g1
	mov %g1, %sp

    /* start RT-Thread Kernel */
    call rtthread_startup
    nop
    
/*
l0 = psr
l1 = pc
l2 = npc
l3 = tbr
*/
.globl _ISR_Handler
_ISR_Handler:
	mov %g4, %l4
	mov %g5, %l5
	mov %wim, %g4
	srl %g4, %l0, %g5
	cmp %g5, 1
	bne dont_do_the_window
	nop
	srl %g4, 1, %g5
	sll %g4, NWINDOWS - 1, %g4
	or %g4, %g5, %g4
	save
	mov %g4, %wim
	nop
	nop
	nop
	
	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]
	
	restore
	nop

dont_do_the_window:
	sub %fp, CPU_INTERRUPT_FRAME_SIZE, %sp
	
	std %l0, [%sp + 0x60]
	st %l2, [%sp + 0x68]
	st %g1, [%sp + 0x6c]
	std %g2, [%sp + 0x70]
	std %l4, [%sp + 0x78]
	std %g6, [%sp + 0x80]
	
	std %i0, [%sp + 0x88]
	std %i2, [%sp + 0x90]
	std %i4, [%sp + 0x98]
	std %i6, [%sp + 0xA0]
	
	mov %y, %g1
	st %g1, [%sp + 0xA8]
	st %l6, [%sp + 0xAc]

	std %f0, [%sp + 0xB0 + 8 * 0x0]
	std %f2, [%sp + 0xB0 + 8 * 0x1]
	std %f4, [%sp + 0xB0 + 8 * 0x2]
	std %f6, [%sp + 0xB0 + 8 * 0x3]
	std %f8, [%sp + 0xB0 + 8 * 0x4]
	std %f10, [%sp + 0xB0 + 8 * 0x5]
	std %f12, [%sp + 0xB0 + 8 * 0x6]
	std %f14, [%sp + 0xB0 + 8 * 0x7]
	std %f16, [%sp + 0xB0 + 8 * 0x8]
	std %f18, [%sp + 0xB0 + 8 * 0x9]
	std %f20, [%sp + 0xB0 + 8 * 0xA]
	std %f22, [%sp + 0xB0 + 8 * 0xB]
	std %f24, [%sp + 0xB0 + 8 * 0xC]
	std %f26, [%sp + 0xB0 + 8 * 0xD]
	std %f28, [%sp + 0xB0 + 8 * 0xE]
	std %f30, [%sp + 0xB0 + 8 * 0xF]
	st %fsr, [%sp + 0xB0 + 8 * 0x10]
	
	mov %l0, %g5
	or %g5, SPARC_PSR_PIL_MASK, %g5
	wr %g5, SPARC_PSR_ET_MASK, %psr
	nop
	nop
	nop
	
	call rt_interrupt_enter
	nop
	
	and %l3, 0x0FF0, %l3
	srl %l3, 4, %o0
	mov %sp, %o1
	
	call rt_hw_trap
	nop
	
	call rt_interrupt_leave
	nop
	
	mov %l0, %psr
	nop
	nop
	nop
	
	ld [%sp + 0xA8], %l5
	mov %l5, %y
	ldd [%sp + 0x60], %l0
	ld [%sp + 0x68], %l2
	
	ld [%sp + 0x6c], %g1
	ldd [%sp + 0x70], %g2
	ldd [%sp + 0x78], %g4
	ldd [%sp + 0x80], %g6

	ldd [%sp + 0x88], %i0
	ldd [%sp + 0x90], %i2
	ldd [%sp + 0x98], %i4
	ldd [%sp + 0xA0], %i6

	ldd [%sp + 0xB0 + 8 * 0x0], %f0
	ldd [%sp + 0xB0 + 8 * 0x1], %f2
	ldd [%sp + 0xB0 + 8 * 0x2], %f4
	ldd [%sp + 0xB0 + 8 * 0x3], %f6
	ldd [%sp + 0xB0 + 8 * 0x4], %f8
	ldd [%sp + 0xB0 + 8 * 0x5], %f10
	ldd [%sp + 0xB0 + 8 * 0x6], %f12
	ldd [%sp + 0xB0 + 8 * 0x7], %f14
	ldd [%sp + 0xB0 + 8 * 0x8], %f16
	ldd [%sp + 0xB0 + 8 * 0x9], %f18
	ldd [%sp + 0xB0 + 8 * 0xA], %f20
	ldd [%sp + 0xB0 + 8 * 0xB], %f22
	ldd [%sp + 0xB0 + 8 * 0xC], %f24
	ldd [%sp + 0xB0 + 8 * 0xD], %f26
	ldd [%sp + 0xB0 + 8 * 0xE], %f28
	ldd [%sp + 0xB0 + 8 * 0xF], %f30
	ld [%sp + 0xB0 + 8 * 0x10], %fsr
	nop
	nop
	nop
	
	mov %wim, %l4
	add %l0, 1, %l6
	and %l6, SPARC_PSR_CWP_MASK, %l6
	srl %l4, %l6, %l5
	cmp %l5, 1
	bne good_task_window
	nop
	sll %l4, 1, %l5
	srl %l4, NWINDOWS - 1, %l4
	or %l4, %l5, %l4
	mov %l4, %wim
	nop
	nop
	nop
	
	restore
	ldd [%sp + 0], %l0             ! Restore window from the stack
	ldd [%sp + 8], %l2
	ldd [%sp + 16], %l4
	ldd [%sp + 24], %l6
	ldd [%sp + 32], %i0
	ldd [%sp + 40], %i2
	ldd [%sp + 48], %i4
	ldd [%sp + 56], %i6
	save
	
good_task_window:
	set rt_thread_switch_interrupt_flag, %l4
	ld [%l4], %l5
	cmp %l5, 1
	be rt_hw_context_switch_interrupt_do
	nop

	mov %l0, %psr
	nop
	nop
	nop
	
	jmp %l1
	rett %l2

rt_hw_context_switch_interrupt_do:
	st %g0, [%l4]
	
	sub %fp, 0x20, %sp
	std %g0, [%sp + 0x00]
	std %g2, [%sp + 0x08]
	std %g4, [%sp + 0x10]
	std %g6, [%sp + 0x18]
	
	mov %sp, %g3
	mov %l1, %g4
	mov %l2, %g5
	mov %l0, %g6
	mov %wim, %g7
	
	mov %g0, %wim
	nop
	nop
	nop
	set 0xFFFFFFF8, %g1
	and %g1, %g6, %g1
	mov %g1, %psr
	nop
	nop
	nop
	
	mov %g0, %g1
save_loop:
	save
	sub %g3, 0x40, %g3
	std %l0, [%g3 + 0x00]
	std %l2, [%g3 + 0x08]
	std %l4, [%g3 + 0x10]
	std %l6, [%g3 + 0x18]
	std %i0, [%g3 + 0x20]
	std %i2, [%g3 + 0x28]
	std %i4, [%g3 + 0x30]
	std %i6, [%g3 + 0x38]
	inc %g1
	cmp %g1, NWINDOWS
	bne save_loop
	nop
	
	sub %g3, 0x88, %g3
	std %f0, [%g3 + 0x00]
	std %f2, [%g3 + 0x08]
	std %f4, [%g3 + 0x10]
	std %f6, [%g3 + 0x18]
	std %f8, [%g3 + 0x20]
	std %f10, [%g3 + 0x28]
	std %f12, [%g3 + 0x30]
	std %f14, [%g3 + 0x38]
	std %f16, [%g3 + 0x40]
	std %f18, [%g3 + 0x48]
	std %f20, [%g3 + 0x50]
	std %f22, [%g3 + 0x58]
	std %f24, [%g3 + 0x60]
	std %f26, [%g3 + 0x68]
	std %f28, [%g3 + 0x70]
	std %f30, [%g3 + 0x78]
	mov %y, %g1
	st %g1, [%g3 + 0x80]
	st %fsr, [%g3 + 0x84]
	
	sub %g3, 0x10, %g3
	std %g4, [%g3 + 0x00]
	std %g6, [%g3 + 0x08]
	
	set rt_interrupt_from_thread, %g1
	ld [%g1], %g2
	st %g3, [%g2]
	
	set rt_interrupt_to_thread, %g1
	ld [%g1], %g1
	ld [%g1], %g3

	ldd [%g3 + 0x00], %g4
	ldd [%g3 + 0x08], %g6
	add %g3, 0x10, %g3
	
	ldd [%g3 + 0x00], %f0
	ldd [%g3 + 0x08], %f2
	ldd [%g3 + 0x10], %f4
	ldd [%g3 + 0x18], %f6
	ldd [%g3 + 0x20], %f8
	ldd [%g3 + 0x28], %f10
	ldd [%g3 + 0x30], %f12
	ldd [%g3 + 0x38], %f14
	ldd [%g3 + 0x40], %f16
	ldd [%g3 + 0x48], %f18
	ldd [%g3 + 0x50], %f20
	ldd [%g3 + 0x58], %f22
	ldd [%g3 + 0x60], %f24
	ldd [%g3 + 0x68], %f26
	ldd [%g3 + 0x70], %f28
	ldd [%g3 + 0x78], %f30
	ld [%g3 + 0x80], %g1
	mov %g1, %y
	ld [%g3 + 0x84], %fsr
	add %g3, 0x88, %g3
	
	set NWINDOWS - 1, %g1
	or %g1, %g6, %g1
	mov %g1, %psr
	nop
	nop
	nop
	
	mov %g0, %g1
restore_loop:
	restore
	ldd [%g3 + 0x00], %l0
	ldd [%g3 + 0x08], %l2
	ldd [%g3 + 0x10], %l4
	ldd [%g3 + 0x18], %l6 
	ldd [%g3 + 0x20], %i0
	ldd [%g3 + 0x28], %i2
	ldd [%g3 + 0x30], %i4
	ldd [%g3 + 0x38], %i6
	add %g3, 0x40, %g3
	inc %g1
	cmp %g1, NWINDOWS
	bne restore_loop
	nop
	
	mov %g6, %psr
	nop
	nop
	nop
	mov %g7, %wim
	nop
	nop
	nop
	
	mov %g4, %l1
	mov %g5, %l2
	mov %g3, %sp
	
	ldd [%sp + 0x00], %g0
	ldd [%sp + 0x08], %g2
	ldd [%sp + 0x10], %g4
	ldd [%sp + 0x18], %g6	
	add %sp, 0x20, %fp
	jmp %l1
	rett %l2

/*
l0 = psr
l1 = pc
l2 = npc
l3 = tbr
*/
_context_switch:
	mov %l2, %l1
	add %l2, 4, %l2
	
	mov %g4, %l4
	mov %g5, %l5
	mov %wim, %g4
	srl %g4, %l0, %g5
	cmp %g5, 1
	bne good_window
	nop
	srl %g4, 1, %g5
	sll %g4, NWINDOWS - 1, %g4
	or %g4, %g5, %g4
	save
	mov %g4, %wim
	nop
	nop
	nop
	
	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]
	
	restore
	nop

good_window:
	and %l3, 0x0FF0, %l3
	srl %l3, 4, %l4
	cmp %l4, 0x82
	bne switch_to
	nop
	
	sub %fp, 0x20, %sp
	std %g0, [%sp + 0x00]
	std %g2, [%sp + 0x08]
	std %g4, [%sp + 0x10]
	std %g6, [%sp + 0x18]
	
	mov %sp, %g3
	mov %l1, %g4
	mov %l2, %g5
	mov %l0, %g6
	mov %wim, %g7
	
	mov %g0, %wim
	nop
	nop
	nop
	set 0xFFFFFFF8, %g1
	and %g1, %g6, %g1
	mov %g1, %psr
	nop
	nop
	nop
	
	mov %g0, %g1
save_window:
	save
	sub %g3, 0x40, %g3
	std %l0, [%g3 + 0x00]
	std %l2, [%g3 + 0x08]
	std %l4, [%g3 + 0x10]
	std %l6, [%g3 + 0x18]
	std %i0, [%g3 + 0x20]
	std %i2, [%g3 + 0x28]
	std %i4, [%g3 + 0x30]
	std %i6, [%g3 + 0x38]
	inc %g1
	cmp %g1, NWINDOWS
	bne save_window
	nop
	
	sub %g3, 0x88, %g3
	std %f0, [%g3 + 0x00]
	std %f2, [%g3 + 0x08]
	std %f4, [%g3 + 0x10]
	std %f6, [%g3 + 0x18]
	std %f8, [%g3 + 0x20]
	std %f10, [%g3 + 0x28]
	std %f12, [%g3 + 0x30]
	std %f14, [%g3 + 0x38]
	std %f16, [%g3 + 0x40]
	std %f18, [%g3 + 0x48]
	std %f20, [%g3 + 0x50]
	std %f22, [%g3 + 0x58]
	std %f24, [%g3 + 0x60]
	std %f26, [%g3 + 0x68]
	std %f28, [%g3 + 0x70]
	std %f30, [%g3 + 0x78]
	mov %y, %g1
	st %g1, [%g3 + 0x80]
	st %fsr, [%g3 + 0x84]
	
	sub %g3, 0x10, %g3
	std %g4, [%g3 + 0x00]
	std %g6, [%g3 + 0x08]
	
	mov %g6, %psr
	nop
	nop
	nop
	
	st %g3, [%i0]
switch_to:
	mov %g0, %wim
	nop
	nop
	nop
	ld [%i1], %g3

	ldd [%g3 + 0x00], %g4
	ldd [%g3 + 0x08], %g6
	add %g3, 0x10, %g3
	
	ldd [%g3 + 0x00], %f0
	ldd [%g3 + 0x08], %f2
	ldd [%g3 + 0x10], %f4
	ldd [%g3 + 0x18], %f6
	ldd [%g3 + 0x20], %f8
	ldd [%g3 + 0x28], %f10
	ldd [%g3 + 0x30], %f12
	ldd [%g3 + 0x38], %f14
	ldd [%g3 + 0x40], %f16
	ldd [%g3 + 0x48], %f18
	ldd [%g3 + 0x50], %f20
	ldd [%g3 + 0x58], %f22
	ldd [%g3 + 0x60], %f24
	ldd [%g3 + 0x68], %f26
	ldd [%g3 + 0x70], %f28
	ldd [%g3 + 0x78], %f30
	ld [%g3 + 0x80], %g1
	mov %g1, %y
	ld [%g3 + 0x84], %fsr
	add %g3, 0x88, %g3
	
	set NWINDOWS - 1, %g1
	or %g1, %g6, %g1
	mov %g1, %psr
	nop
	nop
	nop
	
	mov %g0, %g1
restore_window:
	restore
	ldd [%g3 + 0x00], %l0
	ldd [%g3 + 0x08], %l2
	ldd [%g3 + 0x10], %l4
	ldd [%g3 + 0x18], %l6 
	ldd [%g3 + 0x20], %i0
	ldd [%g3 + 0x28], %i2
	ldd [%g3 + 0x30], %i4
	ldd [%g3 + 0x38], %i6
	add %g3, 0x40, %g3
	inc %g1
	cmp %g1, NWINDOWS
	bne restore_window
	nop
	
	mov %g6, %psr
	nop
	nop
	nop
	mov %g7, %wim
	nop
	nop
	nop
	
	mov %g4, %l1
	mov %g5, %l2
	mov %g3, %sp
	
	ldd [%sp + 0x00], %g0
	ldd [%sp + 0x08], %g2
	ldd [%sp + 0x10], %g4
	ldd [%sp + 0x18], %g6	
	add %sp, 0x20, %fp
	jmp %l1
	rett %l2
	
.data 
.align 8
_fpdata:
	.word 0, 0
_fsrinit:
	.word 0