#------------------------------------------------------------------------------
#
# LoongArch64 ASM exception handler
#
# Copyright (c) 2024, Loongson Technology Corporation Limited. All rights reserved.<BR>
#
# SPDX-License-Identifier: BSD-2-Clause-Patent
#
#------------------------------------------------------------------------------

#include <Library/BaseLib.h>
#include <Library/CpuLib.h>
#include <Register/LoongArch64/Csr.h>
#include <Protocol/DebugSupport.h>
#include "ExceptionCommon.h"

#define RSIZE                 8           // 64 bit mode register size
#define GP_REG_CONTEXT_SIZE   32 * RSIZE  // General-purpose registers size
#define FP_REG_CONTEXT_SIZE   34 * RSIZE  // Floating-point registers size
#define CSR_REG_CONTEXT_SIZE  9  * RSIZE  // CSR registers size

ASM_GLOBAL ASM_PFX(ExceptionEntry)
ASM_GLOBAL ASM_PFX(ExceptionEntryStart)
ASM_GLOBAL ASM_PFX(ExceptionEntryEnd)

ASM_PFX(ExceptionEntry):
  move    $s0, $a0
  bl      GetExceptionType        // Exception type stored in register a0
  move    $a1, $s0                // SystemContxt
  bl      CommonExceptionHandler

PopContext:
  //
  // Not sure if interrupts are turned on during the exception handler, anyway disable interrupts here.
  // It will be turned on when the instruction 'ertn' is executed.
  //
  bl      DisableInterrupts

  move    $a0, $s0                // Restore a0 parameter through s0(EFI_SYSTEM_CONTEXT)
  bl      GetExceptionType        // Get current exception type, and stored in register a0

  // Check whether the FPE is changed during interrupt handler, if ture restore it.
  ld.d    $t1, $sp, (LOONGARCH_CSR_EUEN * RSIZE + GP_REG_CONTEXT_SIZE)
  csrrd   $t0, LOONGARCH_CSR_EUEN        // Current EUEN
  andi    $t0, $t0, CSR_EUEN_FPEN
  andi    $t1, $t1, CSR_EUEN_FPEN
  li.d    $t2, EXCEPT_LOONGARCH_INT
  bne     $a0, $t2, PopRegs
  beq     $t0, $t1, PopRegs
  beqz    $t1, CloseFP
  bl      EnableFloatingPointUnits
  b       PopRegs

CloseFP:
  bl      DisableFloatingPointUnits

PopRegs:
  //
  // Pop CSR reigsters
  //
  addi.d  $sp, $sp, GP_REG_CONTEXT_SIZE

  ld.d    $t0, $sp, LOONGARCH_CSR_CRMD * RSIZE
  csrwr   $t0, LOONGARCH_CSR_CRMD
  ld.d    $t0, $sp, LOONGARCH_CSR_PRMD * RSIZE
  csrwr   $t0, LOONGARCH_CSR_PRMD
  ld.d    $t0, $sp, LOONGARCH_CSR_ECFG * RSIZE
  csrwr   $t0, LOONGARCH_CSR_ECFG
  ld.d    $t0, $sp, LOONGARCH_CSR_ERA * RSIZE
  csrwr   $t0, LOONGARCH_CSR_ERA

  addi.d  $sp, $sp, CSR_REG_CONTEXT_SIZE  // Fource change the stack pointer befor pop the FP registers.

  beqz    $t1, PopGP                      // If the FPE not set, only pop the GP registers.

  //
  // Pop FP registers
  //
  fld.d  $fa0, $sp, 0 * RSIZE
  fld.d  $fa1, $sp, 1 * RSIZE
  fld.d  $fa2, $sp, 2 * RSIZE
  fld.d  $fa3, $sp, 3 * RSIZE
  fld.d  $fa4, $sp, 4 * RSIZE
  fld.d  $fa5, $sp, 5 * RSIZE
  fld.d  $fa6, $sp, 6 * RSIZE
  fld.d  $fa7, $sp, 7 * RSIZE
  fld.d  $ft0, $sp, 8 * RSIZE
  fld.d  $ft1, $sp, 9 * RSIZE
  fld.d  $ft2, $sp, 10 * RSIZE
  fld.d  $ft3, $sp, 11 * RSIZE
  fld.d  $ft4, $sp, 12 * RSIZE
  fld.d  $ft5, $sp, 13 * RSIZE
  fld.d  $ft6, $sp, 14 * RSIZE
  fld.d  $ft7, $sp, 15 * RSIZE
  fld.d  $ft8, $sp, 16 * RSIZE
  fld.d  $ft9, $sp, 17 * RSIZE
  fld.d  $ft10, $sp, 18 * RSIZE
  fld.d  $ft11, $sp, 19 * RSIZE
  fld.d  $ft12, $sp, 20 * RSIZE
  fld.d  $ft13, $sp, 21 * RSIZE
  fld.d  $ft14, $sp, 22 * RSIZE
  fld.d  $ft15, $sp, 23 * RSIZE
  fld.d  $fs0, $sp, 24 * RSIZE
  fld.d  $fs1, $sp, 25 * RSIZE
  fld.d  $fs2, $sp, 26 * RSIZE
  fld.d  $fs3, $sp, 27 * RSIZE
  fld.d  $fs4, $sp, 28 * RSIZE
  fld.d  $fs5, $sp, 29 * RSIZE
  fld.d  $fs6, $sp, 30 * RSIZE
  fld.d  $fs7, $sp, 31 * RSIZE

  ld.d        $t0, $sp, 32 * RSIZE
  movgr2fcsr  $fcsr0, $t0          // Pop the fcsr0 register.

  //
  // Pop the fcc0-fcc7 registers.
  //
  ld.d        $t0, $sp, 33 * RSIZE
  bstrpick.d  $t1, $t0, 7, 0
  movgr2cf    $fcc0, $t1
  bstrpick.d  $t1, $t0, 15, 8
  movgr2cf    $fcc1, $t1
  bstrpick.d  $t1, $t0, 23, 16
  movgr2cf    $fcc2, $t1
  bstrpick.d  $t1, $t0, 31, 24
  movgr2cf    $fcc3, $t1
  bstrpick.d  $t1, $t0, 39, 32
  movgr2cf    $fcc4, $t1
  bstrpick.d  $t1, $t0, 47, 40
  movgr2cf    $fcc5, $t1
  bstrpick.d  $t1, $t0, 55, 48
  movgr2cf    $fcc6, $t1
  bstrpick.d  $t1, $t0, 63, 56
  movgr2cf    $fcc7, $t1

PopGP:
  //
  // Pop GP registers
  //
  addi.d  $sp, $sp, -(GP_REG_CONTEXT_SIZE + CSR_REG_CONTEXT_SIZE)
  ld.d    $ra, $sp, 1 * RSIZE
  ld.d    $tp, $sp, 2 * RSIZE
  ld.d    $a0, $sp, 4 * RSIZE
  ld.d    $a1, $sp, 5 * RSIZE
  ld.d    $a2, $sp, 6 * RSIZE
  ld.d    $a3, $sp, 7 * RSIZE
  ld.d    $a4, $sp, 8 * RSIZE
  ld.d    $a5, $sp, 9 * RSIZE
  ld.d    $a6, $sp, 10 * RSIZE
  ld.d    $a7, $sp, 11 * RSIZE
  ld.d    $t0, $sp, 12 * RSIZE
  ld.d    $t1, $sp, 13 * RSIZE
  ld.d    $t2, $sp, 14 * RSIZE
  ld.d    $t3, $sp, 15 * RSIZE
  ld.d    $t4, $sp, 16 * RSIZE
  ld.d    $t5, $sp, 17 * RSIZE
  ld.d    $t6, $sp, 18 * RSIZE
  ld.d    $t7, $sp, 19 * RSIZE
  ld.d    $t8, $sp, 20 * RSIZE
  ld.d    $r21, $sp, 21 * RSIZE
  ld.d    $fp, $sp, 22 * RSIZE
  ld.d    $s0, $sp, 23 * RSIZE
  ld.d    $s1, $sp, 24 * RSIZE
  ld.d    $s2, $sp, 25 * RSIZE
  ld.d    $s3, $sp, 26 * RSIZE
  ld.d    $s4, $sp, 27 * RSIZE
  ld.d    $s5, $sp, 28 * RSIZE
  ld.d    $s6, $sp, 29 * RSIZE
  ld.d    $s7, $sp, 30 * RSIZE
  ld.d    $s8, $sp, 31 * RSIZE
  ld.d    $sp, $sp, 3 * RSIZE

  ertn // Returen from exception.
//
// End of ExceptionEntry
//

ASM_PFX(ExceptionEntryStart):
  //
  // Store the old stack pointer in preparation for pushing the exception context onto the new stack.
  //
  csrwr   $sp, LOONGARCH_CSR_KS0

  csrrd   $sp, LOONGARCH_CSR_KS0

  //
  // Push GP registers
  //
  addi.d  $sp, $sp, -(GP_REG_CONTEXT_SIZE + FP_REG_CONTEXT_SIZE + CSR_REG_CONTEXT_SIZE)
  st.d    $zero, $sp, 0 * RSIZE
  st.d    $ra, $sp, 1 * RSIZE
  st.d    $tp, $sp, 2 * RSIZE
  st.d    $a0, $sp, 4 * RSIZE
  st.d    $a1, $sp, 5 * RSIZE
  st.d    $a2, $sp, 6 * RSIZE
  st.d    $a3, $sp, 7 * RSIZE
  st.d    $a4, $sp, 8 * RSIZE
  st.d    $a5, $sp, 9 * RSIZE
  st.d    $a6, $sp, 10 * RSIZE
  st.d    $a7, $sp, 11 * RSIZE
  st.d    $t0, $sp, 12 * RSIZE
  st.d    $t1, $sp, 13 * RSIZE
  st.d    $t2, $sp, 14 * RSIZE
  st.d    $t3, $sp, 15 * RSIZE
  st.d    $t4, $sp, 16 * RSIZE
  st.d    $t5, $sp, 17 * RSIZE
  st.d    $t6, $sp, 18 * RSIZE
  st.d    $t7, $sp, 19 * RSIZE
  st.d    $t8, $sp, 20 * RSIZE
  st.d    $r21, $sp, 21 * RSIZE
  st.d    $fp, $sp, 22 * RSIZE
  st.d    $s0, $sp, 23 * RSIZE
  st.d    $s1, $sp, 24 * RSIZE
  st.d    $s2, $sp, 25 * RSIZE
  st.d    $s3, $sp, 26 * RSIZE
  st.d    $s4, $sp, 27 * RSIZE
  st.d    $s5, $sp, 28 * RSIZE
  st.d    $s6, $sp, 29 * RSIZE
  st.d    $s7, $sp, 30 * RSIZE
  st.d    $s8, $sp, 31 * RSIZE
  csrrd   $t0, LOONGARCH_CSR_KS0  // Read the old stack pointer.
  st.d    $t0, $sp, 3 * RSIZE

  //
  // Push CSR registers
  //
  addi.d  $sp, $sp, GP_REG_CONTEXT_SIZE

  csrrd   $t0, LOONGARCH_CSR_CRMD
  st.d    $t0, $sp, LOONGARCH_CSR_CRMD * RSIZE
  csrrd   $t0, LOONGARCH_CSR_PRMD
  st.d    $t0, $sp, LOONGARCH_CSR_PRMD * RSIZE
  csrrd   $t0, LOONGARCH_CSR_EUEN
  st.d    $t0, $sp, LOONGARCH_CSR_EUEN * RSIZE
  csrrd   $t0, LOONGARCH_CSR_MISC
  st.d    $t0, $sp, LOONGARCH_CSR_MISC * RSIZE
  csrrd   $t0, LOONGARCH_CSR_ECFG
  st.d    $t0, $sp, LOONGARCH_CSR_ECFG * RSIZE
  csrrd   $t0, LOONGARCH_CSR_ESTAT
  st.d    $t0, $sp, LOONGARCH_CSR_ESTAT * RSIZE
  csrrd   $t0, LOONGARCH_CSR_ERA
  st.d    $t0, $sp, LOONGARCH_CSR_ERA * RSIZE
  csrrd   $t0, LOONGARCH_CSR_BADV
  st.d    $t0, $sp, LOONGARCH_CSR_BADV * RSIZE
  csrrd   $t0, LOONGARCH_CSR_BADI
  st.d    $t0, $sp, LOONGARCH_CSR_BADI * RSIZE

  //
  // Push FP registers
  //
  addi.d  $sp, $sp, CSR_REG_CONTEXT_SIZE

  csrrd   $t0, LOONGARCH_CSR_EUEN
  andi    $t0, $t0, CSR_EUEN_FPEN
  beqz    $t0, ExcVectorDispatcher

  fst.d  $fa0, $sp, 0 * RSIZE
  fst.d  $fa1, $sp, 1 * RSIZE
  fst.d  $fa2, $sp, 2 * RSIZE
  fst.d  $fa3, $sp, 3 * RSIZE
  fst.d  $fa4, $sp, 4 * RSIZE
  fst.d  $fa5, $sp, 5 * RSIZE
  fst.d  $fa6, $sp, 6 * RSIZE
  fst.d  $fa7, $sp, 7 * RSIZE
  fst.d  $ft0, $sp, 8 * RSIZE
  fst.d  $ft1, $sp, 9 * RSIZE
  fst.d  $ft2, $sp, 10 * RSIZE
  fst.d  $ft3, $sp, 11 * RSIZE
  fst.d  $ft4, $sp, 12 * RSIZE
  fst.d  $ft5, $sp, 13 * RSIZE
  fst.d  $ft6, $sp, 14 * RSIZE
  fst.d  $ft7, $sp, 15 * RSIZE
  fst.d  $ft8, $sp, 16 * RSIZE
  fst.d  $ft9, $sp, 17 * RSIZE
  fst.d  $ft10, $sp, 18 * RSIZE
  fst.d  $ft11, $sp, 19 * RSIZE
  fst.d  $ft12, $sp, 20 * RSIZE
  fst.d  $ft13, $sp, 21 * RSIZE
  fst.d  $ft14, $sp, 22 * RSIZE
  fst.d  $ft15, $sp, 23 * RSIZE
  fst.d  $fs0, $sp, 24 * RSIZE
  fst.d  $fs1, $sp, 25 * RSIZE
  fst.d  $fs2, $sp, 26 * RSIZE
  fst.d  $fs3, $sp, 27 * RSIZE
  fst.d  $fs4, $sp, 28 * RSIZE
  fst.d  $fs5, $sp, 29 * RSIZE
  fst.d  $fs6, $sp, 30 * RSIZE
  fst.d  $fs7, $sp, 31 * RSIZE

  movfcsr2gr  $t3, $fcsr0
  st.d        $t3, $sp, 32 * RSIZE  // Push the FCSR0 register.

  //
  // Push the fcc0-fcc7 registers.
  //
  movcf2gr    $t3, $fcc0
  or          $t2, $t3, $zero
  movcf2gr    $t3, $fcc1
  bstrins.d   $t2, $t3, 0xf, 0x8
  movcf2gr    $t3, $fcc2
  bstrins.d   $t2, $t3, 0x17, 0x10
  movcf2gr    $t3, $fcc3
  bstrins.d   $t2, $t3, 0x1f, 0x18
  movcf2gr    $t3, $fcc4
  bstrins.d   $t2, $t3, 0x27, 0x20
  movcf2gr    $t3, $fcc5
  bstrins.d   $t2, $t3, 0x2f, 0x28
  movcf2gr    $t3, $fcc6
  bstrins.d   $t2, $t3, 0x37, 0x30
  movcf2gr    $t3, $fcc7
  bstrins.d   $t2, $t3, 0x3f, 0x38
  st.d        $t2, $sp, 33 * RSIZE
  //
  // Push exception context down
  //

ExcVectorDispatcher:
  csrrd     $t0, LOONGARCH_CSR_ESTAT
  li.d      $t1, CSR_ESTAT_EXC
  and       $t1, $t0, $t1
  srli.d    $t1, $t1, CSR_ESTAT_EXC_SHIFT
  beqz      $t1, IntVectorDispatcher // If Ecode is 0, it indicates that an interrupt has occurred.
  b         EntryCommonHandler       // Currently, other exception should be handled using C code.

IntVectorDispatcher:
  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_TIMER
  andi      $t1, $t1, BIT0
  bnez      $t1, TimerVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IPI
  andi      $t1, $t1, BIT0
  bnez      $t1, IpiVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_PMC
  andi      $t1, $t1, BIT0
  bnez      $t1, PmcVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP7
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP6
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP5
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP4
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP3
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP2
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP1
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_IP0
  andi      $t1, $t1, BIT0
  bnez      $t1, HwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_SIP1
  andi      $t1, $t1, BIT0
  bnez      $t1, SwIpVector

  srli.d    $t1, $t0, EXCEPT_LOONGARCH_INT_SIP0
  andi      $t1, $t1, BIT0
  bnez      $t1, SwIpVector

  b         EntryCommonHandler // Never shouldn't be here, go to the C code to dump the context.

TimerVector:
PmcVector:
HwIpVector:
SwIpVector:
  b         EntryCommonHandler // Timer, PMC, HWINT, SWINT should be handled using C code.

IpiVector:
  //
  // Get the IPI vector.
  //
  li.d      $t0, LOONGARCH_IOCSR_IPI_STATUS
  iocsrrd.w $t1, $t0

  //
  // SMP_BOOT_CPU vector, fast path.
  //
  andi      $t1, $t1, SMP_BOOT_CPU
  beqz      $t1, EntryCommonHandler

  //
  // Clear current IPI interrupt bit.
  //
  li.d      $t0, LOONGARCH_IOCSR_IPI_CLEAR
  iocsrwr.w $t1, $t0

  //
  // Get the resume vector.
  //
TryReadResumeVector:
  li.d      $t0, LOONGARCH_IOCSR_MBUF0
  iocsrrd.w $a0, $t0
  bnez      $a0, ReadRealResumeVector
  b         TryReadResumeVector
ReadRealResumeVector:
  iocsrrd.d $a0, $t0

  //
  // Clean up current processor mailbox 0.
  //
  li.d      $t0, LOONGARCH_IOCSR_MBUF0
  iocsrwr.d $zero, $t0

  //
  // The SMP_BOOT_CPU vector should never be returned. Clean up the PIE and ensure that
  // global interrupts are turned off for the current processor for handover.
  //
  csrwr     $a0, LOONGARCH_CSR_ERA         // Update ERA
  li.w      $t0, BIT2                      // IE
  csrxchg   $zero, $t0, LOONGARCH_CSR_PRMD // Clean PIE

  //
  // Use ERA to jump to next stage.
  //
  ertn

  //
  // End of IpiVector fast path.
  //

EntryCommonHandler:
  addi.d  $sp, $sp, -(GP_REG_CONTEXT_SIZE + CSR_REG_CONTEXT_SIZE)
  move    $a0, $sp
  la.abs  $ra, ExceptionEntry
  jirl    $zero, $ra, 0
ASM_PFX(ExceptionEntryEnd):
.end
