/*
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 TRAPL(H) mov %g0, %l0; sethi %hi(H), %l4; jmp %l4 + %lo(H); nop;
#define TRAP(H) mov %psr, %l0; sethi %hi(H), %l4; jmp %l4 + %lo(H); nop;
#define TRAP_ENTRY(H) mov %psr, %l0; sethi %hi(H), %l4; jmp %l4 + %lo(H); mov %tbr, %l3;
#define BAD_TRAP ta 0; nop; nop; nop;
#define SOFT_TRAP BAD_TRAP
#define NWINDOWS 8

.section .vectors, "ax"

.globl _ISR_Handler
.globl _window_overflow
.globl _window_underflow
.globl _reset
.globl _context_switch

.globl system_vectors
system_vectors:
    TRAPL(_reset);       ! 00 reset trap
    BAD_TRAP;			 ! 01 instruction_access_exception
    BAD_TRAP;			 ! 02 illegal_instruction
    BAD_TRAP;			 ! 03 priveleged_instruction
    BAD_TRAP;
    TRAP(_window_overflow);	            ! 05 window_overflow
    TRAP(_window_underflow);	        ! 06 window_underflow
    BAD_TRAP;			 ! 07 memory_add0ress_not_aligned
    BAD_TRAP;			 ! 08 fp_exception
    BAD_TRAP;			 ! 09 data_access_exception
    BAD_TRAP;			 ! 0A tag_overflow
    BAD_TRAP;			 ! 0B undefined
    BAD_TRAP;			 ! 0C undefined
    BAD_TRAP;			 ! 0D undefined
    BAD_TRAP;			 ! 0E undefined
    BAD_TRAP;			 ! 0F undefined
    BAD_TRAP;			 ! 10 undefined

    /* Interrupt entries */
    TRAP_ENTRY(_ISR_Handler) 	       ! 11 interrupt level 1
    TRAP_ENTRY(_ISR_Handler) 		   ! 12 interrupt level 2
    TRAP_ENTRY(_ISR_Handler) 		   ! 13 interrupt level 3
    TRAP_ENTRY(_ISR_Handler) 		   ! 14 interrupt level 4
    TRAP_ENTRY(_ISR_Handler) 		   ! 15 interrupt level 5
    TRAP_ENTRY(_ISR_Handler) 		   ! 16 interrupt level 6
    TRAP_ENTRY(_ISR_Handler) 		   ! 17 interrupt level 7
    TRAP_ENTRY(_ISR_Handler) 		   ! 18 interrupt level 8
    TRAP_ENTRY(_ISR_Handler) 		   ! 19 interrupt level 9
    TRAP_ENTRY(_ISR_Handler) 		   ! 1A interrupt level 1
    TRAP_ENTRY(_ISR_Handler) 		   ! 1B interrupt level 11
    TRAP_ENTRY(_ISR_Handler) 		   ! 1C interrupt level 12
    TRAP_ENTRY(_ISR_Handler) 		   ! 1D interrupt level 13
    TRAP_ENTRY(_ISR_Handler) 		   ! 1E interrupt level 14
    TRAP_ENTRY(_ISR_Handler) 		   ! 1F interrupt level 15

    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 20 - 23 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;		! 24 - 27 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;     ! 28 - 2B undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 2C - 2F undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 30 - 33 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 34 - 37 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 38 - 3B undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 3C - 3F undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 40 - 43 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 44 - 47 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 48 - 4B undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 4C - 4F undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 50 - 53 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 54 - 57 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 58 - 5B undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 5C - 5F undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 60 - 63 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 64 - 67 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 68 - 6B undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 6C - 6F undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 70 - 73 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 74 - 77 undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 78 - 7B undefined
    BAD_TRAP; BAD_TRAP; BAD_TRAP; BAD_TRAP;	    ! 7C - 7F undefined

    /* Software traps */
    SOFT_TRAP; SOFT_TRAP; TRAP_ENTRY(_context_switch); TRAP_ENTRY(_context_switch) ! 80 - 83
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 84 - 87
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 88 - 8B
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 8C - 8F
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 90 - 93
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 94 - 97
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 98 - 9B
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! 9C - 9F
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! A0 - A3
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! A4 - A7
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! A8 - AB
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! AC - AF
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! B0 - B3
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! B4 - B7
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! B8 - BB
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! BC - BF
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! C0 - C3
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! C4 - C7
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! C8 - CB
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! CC - CF
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! D0 - D3
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! D4 - D7
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! D8 - DB
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! DC - DF
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! E0 - E3
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! E4 - E7
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! E8 - EB
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! EC - EF
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! F0 - F3
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! F4 - F7
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! F8 - FB
    SOFT_TRAP; SOFT_TRAP; SOFT_TRAP; SOFT_TRAP;	! FC - FF
    
_window_overflow:
	mov %wim, %l3                   ! Calculate next WIM
	mov %g1, %l7
	srl %l3, 1, %g1
	sll %l3, NWINDOWS - 1, %l4
	or %l4, %g1, %g1
	save                             ! Get into window to be saved.
	mov %g1, %wim
	nop
	nop
	nop
	std %l0, [%sp + 0]
	std %l2, [%sp + 8]
	std %l4, [%sp + 16]
	std %l6, [%sp + 24]
	std %i0, [%sp + 32]
	std %i2, [%sp + 40]
	std %i4, [%sp + 48]
	std %i6, [%sp + 56]
	restore                          ! Go back to trap window.
	mov %l7, %g1
	jmp %l1                         ! Re-execute save.
	rett %l2
	
_window_underflow:
	mov %wim, %l3                   ! Calculate next WIM
	sll %l3, 1, %l4
	srl %l3, NWINDOWS - 1, %l5
	or %l5, %l4, %l5
	mov %l5, %wim
	nop
	nop
	nop
	restore                          ! Two restores to get into the
	restore                          ! window to 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                             ! Get back to the trap window.
	save
	jmp %l1                         ! Re-execute restore.
	rett %l2