/*
 * hal_kentry.S - Interrupt / Exception / Syscall kernel entry point for MIPS32
 * 
 * AUthors   Ghassan Almaless (2007,2008,2009,2010,2011,2012)
 *           Mohamed Lamine Karaoui (2015)
 *           Alain Greiner (2016,2017,2018,2019)
 *
 * Copyright (c) UPMC Sorbonne Universites
 * 
 * This file is part of ALMOS-MKH.
 *
 * ALMOS-MKH is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.0 of the License.
 *
 * ALMOS-MKH is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ALMOS-MKH; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#define      UZ_MODE_DEXT    0              
#define      UZ_AT           1
#define      UZ_V0           2
#define      UZ_V1           3
#define      UZ_A0           4 
#define      UZ_A1           5
#define      UZ_A2           6
#define      UZ_A3           7
#define      UZ_T0           8
#define      UZ_T1           9
#define      UZ_T2           10
#define      UZ_T3           11
#define      UZ_T4           12
#define      UZ_T5           13
#define      UZ_T6           14
#define      UZ_T7           15
#define      UZ_S0           16
#define      UZ_S1           17
#define      UZ_S2           18
#define      UZ_S3           19
#define      UZ_S4           20
#define      UZ_S5           21
#define      UZ_S6           22
#define      UZ_S7           23
#define      UZ_T8           24
#define      UZ_T9           25

#define      UZ_LO           26
#define      UZ_HI           27

#define      UZ_GP           28
#define      UZ_SP           29
#define      UZ_S8           30
#define      UZ_RA           31
#define      UZ_PTPR         32
#define      UZ_EPC          33
#define      UZ_SR           34
#define      UZ_TH           35
#define      UZ_CR           36

#define      UZ_REGS         37

#include <kernel_config.h>

	.section   .kentry, "ax", @progbits

	.extern    hal_do_interrupt
	.extern    hal_do_exception
	.extern    hal_do_syscall
    .extern    puts
    .extern    putx
    .extern    putl

	.org       0x180

	.global    hal_kentry_enter
    .global    hal_kentry_eret

	.set       noat
	.set       noreorder

#------------------------------------------------------------------------------------
#       Kernel Entry point for Interrupt / Exception / Syscall
#  At kernel entry, the relevant CPU, CP0, & CP2 registers are saved
#  in a "UZONE" implemented in  the calling thread kernel stack.
#  All these registers are restored from the UZONE after the kernel
#  completes the requested service.
#
#  Implementation note: As this code can be executed when the core is already
#  in kernel mode (after a syscall, the core must handle interrupts, or 
#  non-fatal MMU exceptions), this code implement a two slots stack of UZONE(s).
#------------------------------------------------------------------------------------

hal_kentry_enter:

#------------------------------------------------------------------------------------
# This code made the following actions:
# - save CP2_MMU_MODE & CP2_MMU_DEXT in $26, 
# - set local_cxy into CP2_MMU_DEXT,
# - desactivate DATA MMU in CP2_MMU_MODE,
# - test if the core is already in kernel mode.

    mfc2    $26,    $1                  # $26 <= CP2_MMU_MODE
	andi    $27,    $26,  0xB           # $27 <= code data MMU OFF
	mtc2    $27,    $1			        # set data MMU OFF

    mfc2    $27,    $24                 # $27 <= CP2_MMU_DEXT
    sll     $27,    $27,  4             # $27 <= CP2_MMU_DEXT << 4
    or      $26,    $26,  $27           # $26 <= CP2_MMU_DEXT | CP2_MMU_MODE

    mfc0    $27,    $15,  1             # $27 <= core CP0_EBASE         
    andi    $27,    $27,  0xFFF         # $27 <= gid
    srl     $27,    $27,  2             # $27 <= local_cxy
    mtc2    $27,    $24                 # CP2_MMU_DEXT <= local_cxy

	mfc0    $27,    $12                 # $27 <= CP0_SR 
	andi    $27,    $27,  0x10          # test User Mode bit
	beq     $27,    $0,	  kernel_mode   # jump if core already in kernel
    nop
	
#------------------------------------------------------------------------------------
# This code is executed when the core is in user mode,
# to handle a syscall, an interrupt, or an exception.
# - copy user stack pointer in $27 to be saved in uzone.
# - set kernel stack pointer in $29 (kernel stack empty at firts entry).

user_mode:

    move    $27,    $29                 # $27 <= user stack pointer
	mfc0    $29,    $4,   2             # get pointer on thread descriptor from c0_th
    addi    $29,    $29,  CONFIG_THREAD_DESC_SIZE
    addi    $29,    $29,  -8            # $29 <= kernel stack pointer
    j       unified_mode
    nop

#------------------------------------------------------------------------------------
# This code is executed when the core is already in kernel mode,
# (after a syscall), to handle an interrupt, or a non-fatal exception.
# - copy current kernel stack pointer in $27.

kernel_mode:

    move    $27,    $29                 # $27 <= current kernel stack pointer
    j       unified_mode
    nop

#------------------------------------------------------------------------------------	
# This code is executed in both modes (user or kernel).
# It executes the following actions:
# - decrement $29 to allocate an uzone in kernel stack
# - save GPR, CP0 and CP2 registers to uzone.
# - set the SR in kernel mode: IRQ disabled, clear EXL.
# The assumptions are:
# - c2_mode contains the data MMU OFF value.
# - $26 contains the previous c2_mode and c2_dext values.
# - $27 contains the previous sp value (can be usp or ksp).
# - $29 contains the current kernel stack pointer.

unified_mode:

	addiu   $29,    $29,  -(UZ_REGS*4)	   # allocate uzone in kernel stack 

    sw      $26,    (UZ_MODE_DEXT*4)($29)  # save previous c2_mode and c2_dext values
	sw      $1,	    (UZ_AT*4)($29)
	sw      $2,     (UZ_V0*4)($29)
	sw      $3,     (UZ_V1*4)($29)
	sw      $4,     (UZ_A0*4)($29)
	sw      $5,     (UZ_A1*4)($29)
	sw      $6,     (UZ_A2*4)($29)
	sw      $7,     (UZ_A3*4)($29)
	sw      $8,     (UZ_T0*4)($29)
	sw      $9,     (UZ_T1*4)($29)
	sw      $10,	(UZ_T2*4)($29)
	sw      $11,	(UZ_T3*4)($29)
	sw      $12,	(UZ_T4*4)($29)
	sw      $13,	(UZ_T5*4)($29)
	sw      $14,	(UZ_T6*4)($29)
	sw      $15,	(UZ_T7*4)($29)
	sw      $16,	(UZ_S0*4)($29)
	sw      $17,	(UZ_S1*4)($29)
	sw	    $18,	(UZ_S2*4)($29)
	sw	    $19,	(UZ_S3*4)($29)
	sw	    $20,	(UZ_S4*4)($29)
	sw	    $21,	(UZ_S5*4)($29)
	sw	    $22,	(UZ_S6*4)($29)
	sw	    $23,	(UZ_S7*4)($29)
	sw      $24,	(UZ_T8*4)($29)
	sw      $25,	(UZ_T9*4)($29)

	mflo	$1
	sw      $1,     (UZ_LO*4)($29)         # save lo
	mflo	$1
	sw      $1, 	(UZ_HI*4)($29)	       # save hi

	sw	    $28,	(UZ_GP*4)($29)         # save gp
	sw	    $27,	(UZ_SP*4)($29)         # save previous sp (can be usp or ksp)
	sw	    $30,	(UZ_S8*4)($29)         # save s8
	sw	    $31,	(UZ_RA*4)($29)         # save ra

	mfc0	$1,     $14
	sw      $1,     (UZ_EPC*4)($29)	       # save c0_epc
	mfc0	$1,	    $12 
	sw	    $1,	    (UZ_SR*4)($29)		   # save c0_sr
	mfc0    $1,     $4,  2
	sw      $1,	    (UZ_TH*4)($29)		   # save c0_th
	mfc0    $1,     $13   
	sw      $1,	    (UZ_CR*4)($29)		   # save c0_cr
	mfc2    $1,     $0
	sw      $1, 	(UZ_PTPR*4)($29)	   # save c2_ptpr


    mfc0    $3,     $12                    # $3 <= c0_sr
	srl	    $3,     $3,   5
	sll     $3,	    $3,   5	               # reset 5 LSB bits
	mtc0	$3,	    $12			           # set new c0_sr 

#--------------------
#if DEBUG_HAL_KENTRY 

    # display "enter" message
    la      $4,     msg_enter
    jal     puts
    nop
    move    $4,     $29
    jal     putx
    nop
    la      $4,     msg_cycle
    jal     puts
    nop
    jal     hal_time_stamp
    nop
    move    $4,     $2
    jal     putd
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved CR value
    la      $4,     msg_cr
    jal     puts
    nop
    lw	    $4,  	(UZ_CR*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved SP value
    la      $4,     msg_sp
    jal     puts
    nop
    lw	    $4,  	(UZ_SP*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved RA value
    la      $4,     msg_ra
    jal     puts
    nop
    lw	    $4,  	(UZ_RA*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved TH value
    la      $4,     msg_th
    jal     puts
    nop
    lw	    $4,  	(UZ_TH*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved EPC value
    la      $4,     msg_epc
    jal     puts
    nop
    lw	    $4,  	(UZ_EPC*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved MODE & DEXT values
    la      $4,     msg_mode
    jal     puts
    nop
    lw	    $4,  	(UZ_MODE_DEXT*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved V0 value
    la      $4,     msg_v0
    jal     puts
    nop
    lw	    $4,  	(UZ_V0*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    

#endif
#-----
    
#------------------------------------------------------------------------------------
# This code handle the two-slots uzone pointers stack, and calls the relevant 
# Interrupt / Exception / Syscall handler, depending on XCODE in CP0_CR.
# Both the hal_do_syscall() and the hal_do_exception() functions use
# the values saved in the "uzone", but a syscall can be interrupted 
# by an interrupt, or by a non-fatal exception. Therefore, we need 
# to handle a two-slots "stack of uzones", implemented in the kernel stack,
# using the two "current_uzone" and "previous_uzone" pointers in thread descriptor. 
# - at kernel_entry, we copy the "current_uzone" pointer to the "previous_uzone"
#   slot, and copy the "$29" stack pointer to the "current_uzone" slot.
# - at kernel_exit, we simply restore the "previous_uzone" value to the
#   "current_uzone" slot.
# For a syscall, the hal_do_syscall() function increment the uzone[EPC]
# slot and set the return value in the uzone[V0] slot before returning.

    # update "current_uzone" and "previous_uzone" pointers
	mfc0    $4,     $4,   2             # $4 <= pointer on thread desc
    lw      $5,     8($4)               # $5 <= current uzone pointer trom thread
    sw      $29,    8($4)               # current uzone pointer <= $29
    sw      $5,    12($4)               # previous uzone pointer <= current

    # analyse XCODE to call relevant handler
	mfc0    $17,    $13                 # $17 <= CR
	andi    $17,    $17,  0x3F          # $17 <= XCODE
	ori	    $8,     $0,   0x20
    beq	    $8,	    $17,  cause_sys     # go to syscall handler
    nop
	beq     $17,   	$0,	  cause_int     # go to interrupt handler
    nop

cause_excp:

	jal     hal_do_exception            # call exception handler
	nop
	j       kentry_exit                 # jump to kentry_exit
    nop

cause_sys:

	jal 	hal_do_syscall              # call syscall handler                 
    nop
	j	    kentry_exit                 # jump to kentry_exit
	nop 
	
cause_int:

	jal     hal_do_interrupt            # call interrupt handler
    nop

# -----------------------------------------------------------------------------------
# Kernel exit
# - All registers saved in the uzone are restored, using the pointer on uzone,
#   that is contained in $29.
# - The "current_uzone" pointer in thread descriptor, that has beeen modified at
#   kernel entry is restored from value contained in the uzone[UZ_SP] slot. 
# -----------------------------------------------------------------------------------

kentry_exit:

    # restore "current_uzone" pointer
	mfc0    $4,      $4,   2            # $4 <= pointer on thread desc
    lw      $5,   12($4)                # $5 <= previous uzone pointer from thread
    sw      $5,    8($4)                # current uzone pointer <= previous

#-------------------
#if DEBUG_HAL_KENTRY

    # display "exit" message
    la      $4,     msg_exit
    jal     puts
    nop
    move    $4,     $29
    jal     putx
    nop
    la      $4,     msg_cycle
    jal     puts
    nop
    jal     hal_time_stamp
    nop
    move    $4,     $2
    jal     putd
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved CR value
    la      $4,     msg_cr
    jal     puts
    nop
    lw	    $4,  	(UZ_CR*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved SP value
    la      $4,     msg_sp
    jal     puts
    nop
    lw	    $4,  	(UZ_SP*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved RA value
    la      $4,     msg_ra
    jal     puts
    nop
    lw	    $4,  	(UZ_RA*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved TH value
    la      $4,     msg_th
    jal     puts
    nop
    lw	    $4,  	(UZ_TH*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved EPC value
    la      $4,     msg_epc
    jal     puts
    nop
    lw	    $4,  	(UZ_EPC*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved MODE_DEXT value
    la      $4,     msg_mode
    jal     puts
    nop
    lw	    $4,  	(UZ_MODE_DEXT*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
    # display saved V0 value
    la      $4,     msg_v0
    jal     puts
    nop
    lw	    $4,  	(UZ_V0*4)($29)
    jal     putx
    nop
    la      $4,     msg_crlf
    jal     puts
    nop    
	
#endif
#-----
    
	# restore registers from uzone
	or	    $27,    $0,	$29             # $27 <= ksp (contains &uzone)

	lw	    $1,	    (UZ_EPC*4)($27)	       
	mtc0	$1,	    $14			        # restore c0_epc from uzone
	lw	    $1,	    (UZ_SR*4)($27)
	mtc0	$1,     $12			        # restore c0_sr from uzone

	lw	    $26,    (UZ_HI*4)($27)
	mthi	$26				            # restore hi from uzone
	lw	    $26,    (UZ_LO*4)($27)
	mtlo	$26				            # restore lo from uzone

	lw	    $1,     (UZ_AT*4)($27)		
    lw	    $2,	    (UZ_V0*4)($27)
    lw	    $3,	    (UZ_V1*4)($27)
	lw	    $4,	    (UZ_A0*4)($27)
	lw	    $5,     (UZ_A1*4)($27)
	lw	    $6,     (UZ_A2*4)($27)
	lw	    $7,	    (UZ_A3*4)($27)
	lw	    $8,     (UZ_T0*4)($27)
	lw	    $9,	    (UZ_T1*4)($27)
	lw	    $10,    (UZ_T2*4)($27)
	lw	    $11,    (UZ_T3*4)($27)
	lw	    $12,    (UZ_T4*4)($27)
	lw	    $13,    (UZ_T5*4)($27)
	lw	    $14,    (UZ_T6*4)($27)
	lw	    $15,    (UZ_T7*4)($27)
	lw	    $16,    (UZ_S0*4)($27)
	lw	    $17,    (UZ_S1*4)($27)
	lw	    $18,	(UZ_S2*4)($27)
	lw	    $19,    (UZ_S3*4)($27)
	lw	    $20,	(UZ_S4*4)($27)
	lw	    $21,	(UZ_S5*4)($27)
	lw	    $22,	(UZ_S6*4)($27)
	lw	    $23,	(UZ_S7*4)($27)
	lw	    $24,    (UZ_T8*4)($27)
	lw	    $25,    (UZ_T9*4)($27)	

	lw	    $28,	(UZ_GP*4)($27)      # restore gp_28 from uzone
	lw	    $29,	(UZ_SP*4)($27)		# restore sp_29 from uzone
	lw	    $30,	(UZ_S8*4)($27)      # restore s8_30 from uzone
	lw	    $31,	(UZ_RA*4)($27)      # restore ra_31 from uzone

	lw	    $26,    (UZ_MODE_DEXT*4)($27)    
    srl     $27,    $26,  4             # $27 <= CP2_DEXT
    mtc2    $27,    $24                 # restore c2_dest from uzone
    andi    $27,    $26,  0xF           # $27 <= CP2_MODE
    mtc2    $27,    $1                  # restore c2_mode from uzone

# -----------------------------------------------------------------------------------
# eret function
# -----------------------------------------------------------------------------------

hal_kentry_eret:
    eret                                # jump to EPC, reset EXL bit

    .set reorder
    .set at

#------------------------------------------------------------------------------------
    .section .kdata

msg_cr:
    .align 2
    .asciiz "- UZ_CR   = " 
msg_sp:
    .align 2
    .asciiz "- UZ_SP   = " 
msg_ra:
    .align 2
    .asciiz "- UZ_RA   = " 
msg_epc:
    .align 2
    .asciiz "- UZ_EPC  = " 
msg_th:
    .align 2
    .asciiz "- UZ_TH   = " 
msg_mode:
    .align 2
    .asciiz "- UZ_MODE = " 
msg_v0:
    .align 2
    .asciiz "- UZ_V0   = " 
msg_crlf:
    .align 2
    .asciiz "\n" 
msg_enter:
    .align 2
    .asciiz "\nenter kernel : &uzone = " 
msg_exit:
    .align 2
    .asciiz "\nexit kernel : &uzone = " 
msg_cycle:
    .align 2
    .asciiz " / cycle = "