#include <stdlib.h>
#include <unistd.h>
// #include <stdio.h>
#include <string.h>
#include <mips/cpu.h>
#include <mips/fpa.h>
#include <mips/hal.h>
#include <mips/uhi_syscalls.h>

#include "hv_chip_Config.h"

/* Defined in .ld file */
static char __use_excpt_boot = 1;
static char __flush_to_zero = 1;


/*
 * Write a string, a formatted number, then a string.
 */
static void putsnds (const char *pre, reg_t value, int digits, const char *post){
    char buf[digits];
    int shift;
    int idx = 0;

    if (pre != NULL){
        Hv_Chip_DebugUartPuts(pre);
    }

    for (shift = ((digits - 1) * 4) ; shift >= 0 ; shift -= 4)
        buf[idx++] = "0123456789ABCDEF"[(value >> shift) & 0xf];
    Hv_Chip_DebugUartWrite (1, buf, digits);

    if (post != NULL){
        Hv_Chip_DebugUartPuts(post);
    }
}



static void putsns (const char *pre, reg_t value, const char *post) {
    putsnds (pre, value, sizeof (reg_t) * 2, post);
}
# define WRITE(MSG) Hv_Chip_DebugUartWrite(1, (MSG), strlen (MSG))
# define PUTSNDS(PRE, VALUE, DIGITS, POST) \
    putsnds ((PRE), (VALUE), (DIGITS), (POST))
# define PUTSNS(PRE, VALUE, POST) \
    putsns ((PRE), (VALUE), (POST))

int32_t _MIPS_HAL_NOMIPS16
__uhi_exception (struct gpctx *ctx, int32_t abi)
{
  register struct gpctx *arg1 asm ("$4") = ctx;
  register int32_t arg2 asm ("$5") = abi;
  register int32_t op asm ("$25") = __MIPS_UHI_EXCEPTION;
  register int32_t ret asm ("$2") = __MIPS_UHI_SYSCALL_NUM;

  __asm__ __volatile__ (" # %0 = __uhi_exception(%1, %2) op=%3\n"
            SYSCALL (__MIPS_UHI_SYSCALL_NUM)
            : "+r" (ret)
            : "r" (arg1), "r" (arg2), "r" (op)
            : "$3");
  return ret;
}


/* Forward a UHI SYSCALL operation to SDBPP interface.  */
int _MIPS_HAL_NOMIPS16
__uhi_indirect (struct gpctx *ctx)
{
  register reg_t arg1 asm ("$4") = ctx->r[C_CTX_REGNO(4)];
  register reg_t arg2 asm ("$5") = ctx->r[C_CTX_REGNO(5)];
  register reg_t arg3 asm ("$6") = ctx->r[C_CTX_REGNO(6)];
  register reg_t arg4 asm ("$7") = ctx->r[C_CTX_REGNO(7)];
  register reg_t op asm ("$25") = ctx->r[C_CTX_REGNO(25)];
  register reg_t ret1 asm ("$2") = 1;
  register reg_t ret2 asm ("$3");

  __asm__ __volatile__(" # UHI indirect\n"
               "\tsdbbp 1"
               : "+r" (ret1), "=r" (ret2), "+r" (arg1), "+r" (arg2)
               : "r" (arg3), "r" (arg4), "r" (op));

  ctx->r[C_CTX_REGNO(2)] = ret1;
  ctx->r[C_CTX_REGNO(3)] = ret2;
  ctx->r[C_CTX_REGNO(4)] = arg1;
  ctx->r[C_CTX_REGNO(5)] = arg2;
  /* Handled, move on.  SYSCALL is 4-bytes in all ISAs.  */
  ctx->epc += 4;
  return 1; /* exception handled */
}

#define BT_ABS(X)   ((X)>=0?(X):(-(X)))
#define FRAME_DEPTH 50

int __backtrace_mips32(struct gpctx *ctx, int depth)
{
    volatile unsigned int *addr;
    volatile unsigned int *ra;
    volatile unsigned int *sp;

    volatile unsigned int ra_offset;
    volatile unsigned int stack_size;

    volatile unsigned int bt = 0;
    unsigned int bt_done = 0;

    if(depth < 0)
    {
        return -1;
    }

    ra = (volatile unsigned int *)ctx->ra;
    sp = (volatile unsigned int *)ctx->sp;

    if(((unsigned int)ra >= 0xC0000000) || ((unsigned int)ra < 0x80000000)
        || ((unsigned int)sp >= 0xC0000000) || ((unsigned int)sp < 0x80000000)){
        return -1;
    }

    for(bt=0;bt<depth&&ra;bt++)
    {
        ra_offset = 0;
        stack_size = 0;

        for(addr=ra; ra_offset==0||stack_size==0; addr--)
        {
            switch(*addr&0xffff0000)
            {
                case 0x27bd0000:
                    stack_size = BT_ABS((short)(*addr&0xffff));
                    if(stack_size != 0){
                        PUTSNS("[BackTrace]-[", (reg_t)(bt), NULL);
                        PUTSNS("]-[0x", (reg_t)addr, NULL);
                        WRITE ("]\n");
                    }
                    if(bt_done){
                        return bt;
                    }
                    break;
                case 0xafbf0000:
                    ra_offset = (short)(*addr&0xffff);
                    break;
                default:
                    break;
            }
            if(*addr == 0x0c0002b4){
                bt_done = 1;
            }
        }

        ra =(unsigned int *)*((unsigned int *)((unsigned int)sp + ra_offset));
        sp =(unsigned int *)((unsigned int)sp + stack_size);

        if(((unsigned int)ra >= 0xC0000000) || ((unsigned int)ra < 0x80000000)
            || ((unsigned int)sp >= 0xC0000000) || ((unsigned int)sp < 0x80000000)){
            return -1;
        }
    }

    return bt;
}
#ifdef SW_DUMMY_DEBUG
int sw_dummy_debug = 0;
void dumpGPR2R(struct gpctx *ctx)
{
    if (sw_dummy_debug)
    {
        sw_dummy_debug = 1;
        return;
    }
    /* Dump registers */
    HV_WT32 (SW_DUMMY_AT, ctx->at);

    HV_WT32 (SW_DUMMY_V0, ctx->v[0]);
    HV_WT32 (SW_DUMMY_V1, ctx->v[1]);

    HV_WT32 (SW_DUMMY_A0, ctx->a[0]);
    HV_WT32 (SW_DUMMY_A1, ctx->a[1]);
    HV_WT32 (SW_DUMMY_A2, ctx->a[2]);
    HV_WT32 (SW_DUMMY_A3, ctx->a[3]);

    HV_WT32 (SW_DUMMY_T0, ctx->t[0]);
    HV_WT32 (SW_DUMMY_T1, ctx->t[1]);
    HV_WT32 (SW_DUMMY_T2, ctx->t[2]);
    HV_WT32 (SW_DUMMY_T3, ctx->t[3]);

    HV_WT32 (SW_DUMMY_T4, ctx->t[4]);
    HV_WT32 (SW_DUMMY_T5, ctx->t[5]);
    HV_WT32 (SW_DUMMY_T6, ctx->t[6]);
    HV_WT32 (SW_DUMMY_T7, ctx->t[7]);

    HV_WT32 (SW_DUMMY_S0, ctx->s[0]);
    HV_WT32 (SW_DUMMY_S1, ctx->s[1]);
    HV_WT32 (SW_DUMMY_S2, ctx->s[2]);
    HV_WT32 (SW_DUMMY_S3, ctx->s[3]);

    HV_WT32 (SW_DUMMY_S4, ctx->s[4]);
    HV_WT32 (SW_DUMMY_S5, ctx->s[5]);
    HV_WT32 (SW_DUMMY_S6, ctx->s[6]);
    HV_WT32 (SW_DUMMY_S7, ctx->s[7]);

    HV_WT32 (SW_DUMMY_T8, ctx->t2[0]);
    HV_WT32 (SW_DUMMY_T9, ctx->t2[1]);
    HV_WT32 (SW_DUMMY_K0, ctx->k[0]);
    HV_WT32 (SW_DUMMY_K1, ctx->k[1]);

    HV_WT32 (SW_DUMMY_GP, ctx->gp);
    HV_WT32 (SW_DUMMY_SP, ctx->sp);
    HV_WT32 (SW_DUMMY_FP, ctx->fp);
    HV_WT32 (SW_DUMMY_RA, ctx->ra);

    HV_WT32 (SW_DUMMY_EPC,       ctx->epc);
    HV_WT32 (SW_DUMMY_BADVADDR,  ctx->badvaddr);

    HV_WT32 (SW_DUMMY_STATUS,    ctx->status);
    HV_WT32 (SW_DUMMY_CAUSE,     ctx->cause);
    HV_WT32 (SW_DUMMY_BADPISTR,  ctx->badinstr);
    HV_WT32 (SW_DUMMY_BADPISTR,  ctx->badpinstr);
#ifdef HV_SCALER_DEBUG_VERSION
    while(1);
#endif
}
#endif

void dumpGPR(struct gpctx *ctx)
{
    /* Dump registers */
    PUTSNS (" 0:\t", 0, "\t");
    PUTSNS ("at:\t", ctx->at,   "\t");
    PUTSNS ("v0:\t", ctx->v[0], "\t");
    PUTSNS ("v1:\t", ctx->v[1], "\n");

    PUTSNS ("a0:\t", ctx->a[0], "\t");
    PUTSNS ("a1:\t", ctx->a[1], "\t");
    PUTSNS ("a2:\t", ctx->a[2], "\t");
    PUTSNS ("a3:\t", ctx->a[3], "\n");

    PUTSNS ("t0:\t", ctx->t[0], "\t");
    PUTSNS ("t1:\t", ctx->t[1], "\t");
    PUTSNS ("t2:\t", ctx->t[2], "\t");
    PUTSNS ("t3:\t", ctx->t[3], "\n");

    PUTSNS ("t4:\t", ctx->t[4], "\t");
    PUTSNS ("t5:\t", ctx->t[5], "\t");
    PUTSNS ("t6:\t", ctx->t[6], "\t");
    PUTSNS ("t7:\t", ctx->t[7], "\n");

    PUTSNS ("s0:\t", ctx->s[0], "\t");
    PUTSNS ("s1:\t", ctx->s[1], "\t");
    PUTSNS ("s2:\t", ctx->s[2], "\t");
    PUTSNS ("s3:\t", ctx->s[3], "\n");

    PUTSNS ("s4:\t", ctx->s[4], "\t");
    PUTSNS ("s5:\t", ctx->s[5], "\t");
    PUTSNS ("s6:\t", ctx->s[6], "\t");
    PUTSNS ("s7:\t", ctx->s[7], "\n");

    PUTSNS ("t8:\t", ctx->t2[0],"\t");
    PUTSNS ("t9:\t", ctx->t2[1],"\t");
    PUTSNS ("k0:\t", ctx->k[0], "\t");
    PUTSNS ("k1:\t", ctx->k[1], "\n");

    PUTSNS ("gp:\t", ctx->gp,   "\t");
    PUTSNS ("sp:\t", ctx->sp,   "\t");
    PUTSNS ("fp:\t", ctx->fp,   "\t");
    PUTSNS ("ra:\t", ctx->ra,   "\n");

    #if __mips_isa_rev < 6
        PUTSNS ("hi:\t", ctx->hi, "\t");
        PUTSNS ("lo:\t", ctx->lo, "\n");
    #endif

    PUTSNS ("epc:       \t", ctx->epc,         "\n");
    PUTSNS ("BadVAddr:  \t", ctx->badvaddr,    "\n");

    PUTSNDS ("Status:   \t", ctx->status,   8, "\n");
    PUTSNDS ("Cause:    \t", ctx->cause,    8, "\n");
    PUTSNDS ("BadInstr: \t", ctx->badinstr, 8, "\n");
    PUTSNDS ("BadPInstr:\t", ctx->badpinstr,8, "\n");
      __backtrace_mips32(ctx, FRAME_DEPTH);

#ifdef SW_DUMMY_DEBUG
    dumpGPR2R(ctx);
#endif
}

/* Handle an exception */
void __exception_handle(struct gpctx *ctx, int exception)
{

    switch (exception)
    {
        case EXC_MOD:
        {
            Hv_Chip_DebugUartPuts ("TLB modification exception\n");
            break;
        }
        case EXC_TLBL:
        {
            Hv_Chip_DebugUartPuts ("TLB error on load from ");
            dumpGPR(ctx);
            //PUTSNS (" @0x", ctx->epc, "\n");
            break;
        }
        case EXC_TLBS:
        {
            Hv_Chip_DebugUartPuts ("TLB error on store to ");
            dumpGPR(ctx);
            //PUTSNS (" @0x", ctx->epc, "\n");
            break;
        }
        case EXC_ADEL:
        {
            Hv_Chip_DebugUartPuts ("Address error on load from \n");
            dumpGPR(ctx);
            //PUTSNS (" @0x", ctx->epc, "\n");
            break;
        }
        case EXC_ADES:
        {
            Hv_Chip_DebugUartPuts ("Address error on store to \n");
            dumpGPR(ctx);
            //PUTSNS (" @0x", ctx->epc, "\n");
            break;
        }
        case EXC_IBE:
        {
            Hv_Chip_DebugUartPuts ("Instruction bus error\n");
            break;
        }
        case EXC_DBE:
        {
            Hv_Chip_DebugUartPuts ("Data bus error\n");
            dumpGPR(ctx);
            break;
        }
        case EXC_SYS:
        {
              /* Process a UHI SYSCALL, all other SYSCALLs should have been processed
             by our caller.  __use_excpt_boot has following values:
             0 = Do not use exception handler present in boot.
             1 = Use exception handler present in boot if BEV
                 is 0 at startup.
             2 = Always use exception handler present in boot.   */

              /* Special handling for boot/low level failures.  */
            if (ctx->t2[1] == __MIPS_UHI_BOOTFAIL)
            {
                switch (ctx->a[0])
                {
                    case __MIPS_UHI_BF_CACHE:
                    {
                        Hv_Chip_DebugUartPuts ("L2 cache configuration error\n");
                        break;
                    }
                    default:
                    {
                        Hv_Chip_DebugUartPuts ("Unknown boot failure error\n");
                        break;
                    }
                }

                /* These are unrecoverable.  Abort.  */
                ctx->epc = (sreg_t)(long)&__exit;
                /* Exit code of 255 */
                ctx->a[0] = 0xff;
                return;
            }
            if (((long) __use_excpt_boot == 2
                || ((long) __use_excpt_boot == 1
                && __get_startup_BEV
                && __get_startup_BEV () == 0))
                && __chain_uhi_excpt)
            {
                /* This will not return.  */
                __chain_uhi_excpt (ctx);
            }
            else
            {
                __uhi_indirect (ctx);
            }
            return;
        }
        case EXC_BP:
        {
            /* Return from exception handler if breakpoint is handled.  */
            if (__uhi_break && __uhi_break (ctx))
                return;
            Hv_Chip_DebugUartPuts ("Breakpoint \n");
            break;
        }
        case EXC_RI:
        {
            Hv_Chip_DebugUartPuts ("Illegal instruction \n");
            break;
        }
        case EXC_CPU:
        {
            Hv_Chip_DebugUartPuts ("Coprocessor unusable\n");
            break;
        }
        case EXC_OVF:
        {
            Hv_Chip_DebugUartPuts ("Overflow\n");
            break;
        }
        case EXC_TRAP:
        {
            dumpGPR(ctx);
            Hv_Chip_DebugUartPuts ("Trap\n");
            break;
        }
        case EXC_MSAFPE:
        {
            Hv_Chip_DebugUartPuts ("MSA Floating point error\n");
            break;
        }
        case EXC_FPE:
        {
            /* Turn on flush to zero the first time we hit an unimplemented
            operation.  If we hit it again then stop.  */
            if (__flush_to_zero
            && (fpa_getsr () & FPA_CSR_UNI_X)
            && (fpa_getsr () & FPA_CSR_FS) == 0)
            {
                unsigned int sr = fpa_getsr ();
                sr &= ~FPA_CSR_UNI_X;
                sr |= FPA_CSR_FS;
                fpa_setsr (sr);

                return;
            }
            //Hv_Chip_DebugUartPuts ("Floating point error\n");
            break;
        }
        case EXC_IS1:
        {
            //Hv_Chip_DebugUartPuts ("Implementation specific exception (16)\n");
            break;
        }
        case EXC_IS2:
        {
            //Hv_Chip_DebugUartPuts ("Implementation specific exception (17)\n");
            break;
        }
        case EXC_C2E:
        {
            Hv_Chip_DebugUartPuts ("Precise Coprocessor 2 exception\n");
            break;
        }
        case EXC_TLBRI:
        {
            Hv_Chip_DebugUartPuts ("TLB read inhibit exception\n");
            break;
        }
        case EXC_TLBXI:
        {
            Hv_Chip_DebugUartPuts ("TLB execute inhibit exception\n");
            break;
        }
        case EXC_MSAU:
        {
            //Hv_Chip_DebugUartPuts ("MSA unusable \n");
            break;
        }
        case EXC_MDMX:
        {
            //Hv_Chip_DebugUartPuts ("MDMX exception \n");
            break;
        }
        case EXC_WATCH:
        {
            //Hv_Chip_DebugUartPuts ("Watchpoint\n");
            break;
        }
        case EXC_MCHECK:
        {
            //Hv_Chip_DebugUartPuts ("Machine check error\n");
            break;
        }
        case EXC_THREAD:
        {
            Hv_Chip_DebugUartPuts ("Thread exception\n");
            break;
        }
        case EXC_DSPU:
        {
            //Hv_Chip_DebugUartPuts ("DSP unusable\n");
            break;
        }
        case EXC_RES30:
        {
            //Hv_Chip_DebugUartPuts ("Cache error\n");
            break;
        }
        default:
        {
            //Hv_Chip_DebugUartPuts ("Unhandled exception " "\n");
            break;
        }
    }

    /* Raise UHI exception which may or may not return.  */
    if (__uhi_exception (ctx, UHI_ABI) != 0)
    {
        /* The exception was acknowledged but not handled.  Abort.  */
        ctx->epc = (sreg_t)(long)&__exit;
        /* Exit code of 255 */
        ctx->a[0] = 0xff;
    }
    return;
}