/*      $NetBSD: trap_subr.S,v 1.15 2022/07/05 20:15:40 andvar Exp $    */
/*-
 * Copyright (c) 2010, 2011 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by Raytheon BBN Technologies Corp and Defense Advanced Research Projects
 * Agency and which was developed by Matt Thomas of 3am Software Foundry.
 *
 * This material is based upon work supported by the Defense Advanced Research
 * Projects Agency and Space and Naval Warfare Systems Center, Pacific, under
 * Contract No. N66001-09-C-2073.
 * Approved for Public Release, Distribution Unlimited
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

RCSID("$NetBSD: trap_subr.S,v 1.15 2022/07/05 20:15:40 andvar Exp $")

#ifdef _KERNEL_OPT
#include "opt_altivec.h"
#include "opt_ddb.h"
#include "opt_mpc85xx.h"
#include "opt_multiprocessor.h"
#endif

        .globl  _C_LABEL(sctrapexit), _C_LABEL(trapexit), _C_LABEL(intrcall)

        /*
         * We have a problem with critical (MSR[CE] or machine check (MSR[ME])
         * or debug (MSR[DE]) interrupts/exception in that they could happen
         * in-between the mtsprg1 %r2 and mfsprg1 %r2.  If that happens, %r2
         * will be lost.  Even if we moved to a different sprg, subsequent
         * exceptions would use SPRG1 and its value would be lost.  The only
         * way to be safe for CE/ME/DE faults to save and restore SPRG1.
         *
         * Since CE/ME/DE faults may happen anytime, we need r1 to always
         * contain a valid kernel stack pointer.  Therefore we use r2 as
         * our temporary register.
         *
         * To prevent %r2 being overwritten, each "level" (normal, critical,
         * mchk) uses a unique sprg to save %r2 (sprg1, sprg4, sprg5).
         *
         * Since we can't control how many nested exceptions we might get,
         * we don't use a dedicated save area.  Instead we have an upwards
         * growing "stack" of them; the pointer to which is kept in sprg3.
         *
         * To allocate from the stack, one fetches sprg3, adds the amount
         * needed, saves sprg3, and then refers to the save using a
         * displacement of -amount.
         */
        /*
         * XXXclang
         * At the moment, clang cannot correctly assemble m[ft]sprgN
         * (N >= 4); use m[ft]spr for SPR_SPRGN instead.
         */
#define FRAME_EXC_PROLOGUE(start, sprg, srr)                            \
        mtspr   sprg,%r2;               /* save r2 */                   \
        mfsprg3 %r2;                    /* get save_area pointer */     \
        addi    %r2,%r2,4*(32-start);                                   \
                                        /* allocate save area */        \
        mtsprg3 %r2;                    /* save updated pointer */      \
        stmw    %r##start,-4*(32-start)(%r2);                           \
                                        /* free r24-r31 for use */      \
        mfspr   %r26,sprg;              /* get saved r2 */              \
        mfcr    %r27;                   /* get Condition Register */    \
        mfxer   %r28;                   /* get XER */                   \
        mfspr   %r30, SPR_##srr##0;     /* get SRR0 */                  \
        mfspr   %r31, SPR_##srr##1      /* get SRR1 */

#define PROLOGUE_GET_DEAR       mfspr   %r24, SPR_DEAR
#define PROLOGUE_GET_ESR        mfspr   %r25, SPR_ESR
#define PROLOGUE_GET_SRRS       mfsrr0  %r24; \
                                mfsrr1  %r25
#define PROLOGUE_GET_SPRG1      mfsprg1 %r29
#define PROLOGUE_GET_DBSR       mfspr   %r25, SPR_DBSR
#define SAVE_ESR                stw     %r25, FRAME_ESR(%r1)
#define SAVE_DEAR               stw     %r24, FRAME_DEAR(%r1)
#define SAVE_DEAR_ESR           SAVE_ESR; SAVE_DEAR
#define SAVE_SRRS               SAVE_DEAR_ESR
#define SAVE_SPRG1              stw     %r29, FRAME_SPRG1(%r1)
#define SAVE_DBSR               stw     %r25, FRAME_DBSR(%r1)
#define SAVE_NOTHING            /* nothing */
#define RESTORE_SPRG1(r)        lwz     r, FRAME_SPRG1(%r1); \
                                mtsprg1 r
#define RESTORE_SRR0(r)         lwz     r, FRAME_DEAR(%r1); \
                                mtsrr0  r
#define RESTORE_SRR1(r)         lwz     r, FRAME_ESR(%r1); \
                                mtsrr1  r

#define FRAME_PROLOGUE          \
        FRAME_EXC_PROLOGUE(26, SPR_SPRG1, SRR)

#define FRAME_PROLOGUE_DEAR_ESR \
        FRAME_EXC_PROLOGUE(24, SPR_SPRG1, SRR); \
        PROLOGUE_GET_ESR; \
        PROLOGUE_GET_DEAR

#define FRAME_PROLOGUE_ESR      \
        FRAME_EXC_PROLOGUE(25, SPR_SPRG1, SRR); \
        PROLOGUE_GET_ESR

#define FRAME_TLBPROLOGUE       \
        FRAME_EXC_PROLOGUE(20, SPR_SPRG1, SRR); \
        PROLOGUE_GET_ESR; \
        PROLOGUE_GET_DEAR

#define FRAME_INTR_PROLOGUE     \
        FRAME_EXC_PROLOGUE(26, SPR_SPRG1, SRR)

/*
 * These need to save SRR0/SRR1 as well their SRR0/SRR1 in case normal
 * exceptions happened during their execution.
 */
#define FRAME_CRIT_PROLOGUE     \
        FRAME_EXC_PROLOGUE(24, SPR_SPRG4, CSRR); \
        PROLOGUE_GET_SPRG1; \
        PROLOGUE_GET_SRRS

#define FRAME_MCHK_PROLOGUE     \
        FRAME_EXC_PROLOGUE(24, SPR_SPRG5, MCSRR); \
        PROLOGUE_GET_SPRG1; \
        PROLOGUE_GET_SRRS

#define FRAME_DEBUG_PROLOGUE    \
        FRAME_EXC_PROLOGUE(24, SPR_SPRG4, CSRR); \
        PROLOGUE_GET_SPRG1; \
        PROLOGUE_GET_SRRS

/*
 * DDB expects to fetch the LR from the previous frame.  But it also
 * expects to be pointing at the instruction after the branch link.  Since
 * we didn't branch, we need to advance it by to fake out DDB.  But there's
 * problem.  If the routine is in either its first or last two instructions
 * (before or after its adjusted its stack pointer), we could possibly
 * overwrite stored return address.  So that stored return address needs to
 * saved and restored.
 */
#if defined(DDB)
#define FRAME_SAVE_SRR0_FOR_DDB                                         \
        lwz     %r29, FRAMELEN+CFRAME_LR(%r1);  /* fetch old return address */\
        stw     %r29, FRAME_CFRAME_LR(%r1);     /* save it */            \
        addi    %r30, %r30, 4;                  /* point to s the next insn */ \
        stw     %r30, FRAMELEN+CFRAME_LR(%r1)   /* appease ddb stacktrace */
#define FRAME_RESTORE_RETURN_ADDRESS                                    \
        lwz     %r3, FRAME_CFRAME_LR(%r1);      /* fetch old return address */ \
        stw     %r3, FRAMELEN+CFRAME_LR(%r1)    /* restore it */
#else
#define FRAME_SAVE_SRR0_FOR_DDB
#define FRAME_RESTORE_RETURN_ADDRESS
#endif

#ifdef PPC_HAVE_SPE
#define FRAME_SAVE_SPEFSCR                                              \
        mfspefscr %r0;                  /* get spefscr */               \
        stw     %r0, FRAME_SPEFSCR(%r1) /* save into trapframe */
#define FRAME_RESTORE_SPEFSCR                                           \
        lwz     %r0, FRAME_SPEFSCR(%r1); /* fetch from trapframe */     \
        mtspefscr %r0                   /* save spefscr */
#else
#define FRAME_SAVE_SPEFSCR
#define FRAME_RESTORE_SPEFSCR
#endif
/*
 * Before the first memory reference, we must have our state inside registers
 * since the first memory access might cause an exception which would cause
 * SRR0/SRR1 and DEAR/ESR to become unrecoverable.  CR and XER also need to be
 * saved early since they will modified by instruction flow.  The saved stack
 * pointer is also critical but LR and CTR can be deferred being saved until
 * we are actually filling a trapframe.
 */
#define FRAME_EXC_ENTER(exc, tf, start, save_prologue)                  \
        mtcr    %r31;                   /* user mode exception? */      \
        mr      %r31, %r1;              /* save SP (SRR1 is safe in CR) */ \
        bf      MSR_PR, 1f;             /*   nope, sp is good */        \
        mfsprg2 %r2;                    /* get curlwp */                \
        lwz     %r2, L_PCB(%r2);        /* get uarea of curlwp */       \
        addi    %r1, %r2, USPACE-CALLFRAMELEN;                          \
                                        /* start stack at top of it */  \
1:                                                                      \
        stwu    %r31, -FRAMELEN(%r1);   /* get space for trapframe */   \
        stw     %r0, FRAME_R0(%r1);     /* save r0 */                   \
        stw     %r31, FRAME_R1(%r1);    /* save (saved) r1 */           \
        stw     %r26, FRAME_R2(%r1);    /* save (saved) r2 */           \
        save_prologue;                  /* save SPRG1/ESR/DEAR */       \
        /* At this point, r26, r29, and r31 have been saved so we */    \
        /* can use them for LR, CTR, and SRR1.                    */    \
        mflr    %r26;                   /* get Link Register */         \
        mfctr   %r29;                   /* get CTR */                   \
        mfcr    %r31;                   /* get SRR1 */                  \
        stmw    %r26, FRAME_LR(%r1);    /* save LR CR XER CTR SRR0/1 */ \
        FRAME_SAVE_SRR0_FOR_DDB;                                        \
        mr      %r0, %r31;              /* save SRR1 for a bit */       \
        mfsprg3 %r2;                    /* get save_area pointer */     \
        addi    %r2,%r2,-4*(32-start);  /* find our save area */        \
        lmw     %r##start,0(%r2);       /* get start-r31 */             \
        mtsprg3 %r2;                    /* save updated pointer */      \
        stmw    %r3, FRAME_R3(%r1);     /* save r2-r31 */               \
        /* Now everything has been saved */                             \
        mr      %r31, %r0;              /* move SRR1 back to r31 */     \
        mfsprg2 %r13;                   /* put curlwp in r13 */         \
        FRAME_SAVE_SPEFSCR;                                             \
        li      %r7, exc;               /* load EXC_* */                \
        stw     %r7, FRAME_EXC(%r1);    /* save into trapframe */       \
        addi    tf, %r1, FRAME_TF       /* get address of trap frame */

#define FRAME_EXC_EXIT(rfi, srr)                                        \
        FRAME_RESTORE_RETURN_ADDRESS;   /* restore return address */    \
        lmw     %r26, FRAME_LR(%r1);    /* get LR CR XER CTR SRR0/1 */  \
        oris    %r31,%r31,PSL_CE@h;                                     \
        mtspr   SPR_##srr##1, %r31;     /* restore SRR1 */              \
        mtspr   SPR_##srr##0, %r30;     /* restore SRR0 */              \
        FRAME_RESTORE_SPEFSCR;                                          \
        mtctr   %r29;                   /* restore CTR */               \
        mtxer   %r28;                   /* restore XER */               \
        mtcr    %r27;                   /* restore CR */                \
        mtlr    %r26;                   /* restore LR */                \
        lmw     %r2, FRAME_R2(%r1);     /* restore r2-r31 */            \
        lwz     %r0, FRAME_R0(%r1);     /* restore r0 */                \
        lwz     %r1, FRAME_R1(%r1);     /* restore r1 */                \
        rfi                             /* return from interrupt */


#define FRAME_ENTER(exc, tf)            \
        FRAME_EXC_ENTER(exc, tf, 26, SAVE_NOTHING)

#define FRAME_ENTER_ESR(exc, tf)        \
        FRAME_EXC_ENTER(exc, tf, 25, SAVE_ESR)

#define FRAME_ENTER_DEAR_ESR(exc, tf)   \
        FRAME_EXC_ENTER(exc, tf, 24, SAVE_DEAR_ESR)

#define FRAME_EXIT              FRAME_EXC_EXIT(rfi, SRR)

#define FRAME_TLBENTER(exc)     \
        FRAME_EXC_ENTER(exc, %r4, 20, SAVE_DEAR_ESR)
#define FRAME_TLBEXIT           FRAME_EXC_EXIT(rfi, SRR)

#define FRAME_MCHK_ENTER(exc)   \
        FRAME_EXC_ENTER(exc, %r3, 26, SAVE_SPRG1; SAVE_SRRS)
#define FRAME_MCHK_EXIT         \
        RESTORE_SRR0(%r28);     \
        RESTORE_SRR1(%r27);     \
        RESTORE_SPRG1(%r26);    \
        FRAME_EXC_EXIT(rfmci, MCSRR)

#define FRAME_DEBUG_ENTER(exc)  \
        FRAME_EXC_ENTER(exc, %r4, 26, SAVE_SPRG1; SAVE_SRRS)
#define FRAME_DEBUG_EXIT        \
        RESTORE_SPRG1(%r26); FRAME_EXC_EXIT(rfci, CSRR)

#define FRAME_INTR_SP                                                   \
        bf      MSR_PR, 1f;             /*    nope, sp is good */       \
        mfsprg2 %r2;                    /* get curlwp */                \
        lwz     %r2, L_PCB(%r2);        /* get uarea of curlwp */       \
        addi    %r1, %r2, USPACE-CALLFRAMELEN;                          \
                                        /* start stack at top of it */  \
1:

#define FRAME_INTR_SP_NEW(sym)                                          \
        lis     %r2,(sym)@ha;                                           \
        addi    %r1,%r2,(sym)@l

#define FRAME_INTR_XENTER(exc, start, get_intr_sp, save_prologue)       \
        mtcr    %r31;                   /* user mode exception? */      \
        mr      %r31, %r1;              /* save SP (SRR1 is safe in CR) */ \
        get_intr_sp;                    /* get kernel stack pointer */  \
        stwu    %r31, -FRAMELEN(%r1);   /* get space for trapframe */   \
        stw     %r0, FRAME_R0(%r1);     /* save r0 */                   \
        stw     %r31, FRAME_R1(%r1);    /* save (saved) r1 */           \
        stw     %r26, FRAME_R2(%r1);    /* save (saved) r2 */           \
        save_prologue;                  /* save SPRG1 (maybe) */        \
        mflr    %r26;                   /* get LR */                    \
        mfctr   %r29;                   /* get CTR */                   \
        mfcr    %r31;                   /* get SRR1 */                  \
        stmw    %r26, FRAME_LR(%r1);    /* save LR CR XER CTR SRR0/1 */ \
        FRAME_SAVE_SRR0_FOR_DDB;                                        \
        stw     %r3, FRAME_R3(%r1);     /* save r3 */                   \
        stw     %r4, FRAME_R4(%r1);     /* save r4 */                   \
        stw     %r5, FRAME_R5(%r1);     /* save r5 */                   \
        stw     %r6, FRAME_R6(%r1);     /* save r6 */                   \
        stw     %r7, FRAME_R7(%r1);     /* save r7 */                   \
        stw     %r8, FRAME_R8(%r1);     /* save r8 */                   \
        stw     %r9, FRAME_R9(%r1);     /* save r9 */                   \
        stw     %r10, FRAME_R10(%r1);   /* save r10 */                  \
        stw     %r11, FRAME_R11(%r1);   /* save r11 */                  \
        stw     %r12, FRAME_R12(%r1);   /* save r12 */                  \
        stw     %r13, FRAME_R13(%r1);   /* save r13 */                  \
        mfsprg3 %r2;                    /* get save_area pointer */     \
        addi    %r2,%r2,-4*(32-start);  /* find our save area */        \
        lmw     %r##start,0(%r2);       /* get start-r31 */             \
        mtsprg3 %r2;                    /* save updated pointer */      \
        mfsprg2 %r13;                   /* put curlwp into r13 */       \
        li      %r7, exc;               /* load EXC_* */                \
        stw     %r7, FRAME_EXC(%r1);    /* save into trapframe */       \
        addi    %r3, %r1, FRAME_TF      /* only argument is trapframe */

#define FRAME_INTR_XEXIT(rfi, srr)                                      \
        FRAME_RESTORE_RETURN_ADDRESS;   /* restore return address */    \
        lwz     %r8, FRAME_LR(%r1);     /* get LR */                    \
        lwz     %r9, FRAME_CR(%r1);     /* get CR */                    \
        lwz     %r10, FRAME_XER(%r1);   /* get XER */                   \
        lwz     %r11, FRAME_CTR(%r1);   /* get CTR */                   \
        lwz     %r12, FRAME_SRR0(%r1);  /* get SRR0 */                  \
        lwz     %r13, FRAME_SRR1(%r1);  /* get SRR1 */                  \
        mtspr   SPR_##srr##1, %r13;     /* restore SRR1 */              \
        mtspr   SPR_##srr##0, %r12;     /* restore SRR0 */              \
        mtctr   %r11;                   /* restore CTR */               \
        mtxer   %r10;                   /* restore XER */               \
        mtcr    %r9;                    /* restore CR */                \
        mtlr    %r8;                    /* restore LR */                \
        lwz     %r13, FRAME_R13(%r1);   /* restore r13 */               \
        lwz     %r12, FRAME_R12(%r1);   /* restore r12 */               \
        lwz     %r11, FRAME_R11(%r1);   /* restore r11 */               \
        lwz     %r10, FRAME_R10(%r1);   /* restore r10 */               \
        lwz     %r9, FRAME_R9(%r1);     /* restore r9 */                \
        lwz     %r8, FRAME_R8(%r1);     /* restore r8 */                \
        lwz     %r7, FRAME_R7(%r1);     /* restore r7 */                \
        lwz     %r6, FRAME_R6(%r1);     /* restore r6 */                \
        lwz     %r5, FRAME_R5(%r1);     /* restore r5 */                \
        lwz     %r4, FRAME_R4(%r1);     /* restore r4 */                \
        lwz     %r3, FRAME_R3(%r1);     /* restore r3 */                \
        lwz     %r2, FRAME_R2(%r1);     /* restore r2 */                \
        lwz     %r0, FRAME_R0(%r1);     /* restore r0 */                \
        lwz     %r1, FRAME_R1(%r1);     /* restore r1 */                \
        rfi                             /* return from interrupt */

#define FRAME_INTR_ENTER(exc)   \
        FRAME_INTR_XENTER(exc, 26, FRAME_INTR_SP, SAVE_NOTHING)
#define FRAME_INTR_EXIT         \
        FRAME_INTR_XEXIT(rfi, SRR)
#define FRAME_CRIT_ENTER(exc)   \
        FRAME_INTR_XENTER(exc, 24, FRAME_INTR_SP, SAVE_SPRG1)
#define FRAME_WDOG_ENTER(exc, sym)      \
        FRAME_INTR_XENTER(exc, 24, FRAME_INTR_SP_NEW(sym), SAVE_SPRG1)
#define FRAME_CRIT_EXIT         \
        RESTORE_SRR0(%r4);      \
        RESTORE_SRR1(%r5);      \
        RESTORE_SPRG1(%r6);     \
        FRAME_INTR_XEXIT(rfci, CSRR)

#if defined(MULTIPROCESSOR)
#define FRAME_TLBMISSLOCK                                               \
        GET_CPUINFO(%r23);                                              \
        ldint   %r22, CI_MTX_COUNT(%r23);                               \
        subi    %r22, %r22, 1;                                          \
        stint   %r22, CI_MTX_COUNT(%r23);                               \
        isync;                                                          \
        cmpwi   %r22, 0;                                                \
        bne     1f;                                                     \
        ldint   %r22, CI_CPL(%r23);                                     \
        stint   %r22, CI_MTX_OLDSPL(%r23);                              \
1:      lis     %r23, _C_LABEL(pmap_tlb_miss_lock)@h;                   \
        ori     %r23, %r23, _C_LABEL(pmap_tlb_miss_lock)@l;             \
        li      %r20, MTX_LOCK;                                         \
2:      lwarx   %r22, %r20, %r23;                                       \
        cmpwi   %r22, __SIMPLELOCK_UNLOCKED;                            \
        beq+    4f;                                                     \
3:      lwzx    %r22, %r20, %r23;                                       \
        cmpwi   %r22, __SIMPLELOCK_UNLOCKED;                            \
        beq+    2b;                                                     \
        b       3b;                                                     \
4:      li      %r21, __SIMPLELOCK_LOCKED;                              \
        stwcx.  %r21, %r20, %r23;                                       \
        bne-    2b;                                                     \
        isync;                                                          \
        msync;
#define FRAME_TLBMISSUNLOCK                                             \
        sync;                                                           \
        lis     %r23, _C_LABEL(pmap_tlb_miss_lock)@h;                   \
        ori     %r23, %r23, _C_LABEL(pmap_tlb_miss_lock)@l;             \
        li      %r22, __SIMPLELOCK_UNLOCKED;                            \
        stw     %r22, MTX_LOCK(%r23);                                   \
        isync;                                                          \
        msync;                                                          \
        GET_CPUINFO(%r23);                                              \
        ldint   %r22, CI_MTX_COUNT(%r23);                               \
        addi    %r22, %r22, 1;                                          \
        stint   %r22, CI_MTX_COUNT(%r23);                               \
        isync;
#else   /* !MULTIPROCESSOR */
#define FRAME_TLBMISSLOCK
#define FRAME_TLBMISSUNLOCK
#endif  /* MULTIPROCESSOR */

        .text
        .p2align 4
_C_LABEL(critical_input_vector):
        /* MSR[ME] is unchanged, all others cleared */
        FRAME_CRIT_PROLOGUE                     /* save SP r26-31 CR LR XER */
        FRAME_CRIT_ENTER(EXC_CII)
        bl      _C_LABEL(intr_critintr)         /* critintr(tf) */
        FRAME_CRIT_EXIT

        .p2align 4
_C_LABEL(machine_check_vector):
        /* all MSR bits are cleared */
        FRAME_MCHK_PROLOGUE                     /* save SP r25-31 CR LR XER */
        FRAME_MCHK_ENTER(EXC_MCHK)
        /*
         * MCAR/MCSR don't need to be saved early since MSR[ME] is cleared
         * on entry.
         */
        mfspr   %r7, SPR_MCAR
        mfspr   %r6, SPR_MCSR
        stw     %r6, FRAME_MCSR(%r1)
        stw     %r7, FRAME_MCAR(%r1)
        li      %r3, T_MACHINE_CHECK
        bl      _C_LABEL(trap)                  /* trap(T_MACHINE_CHECK, tf) */
        FRAME_MCHK_EXIT

        .p2align 4
_C_LABEL(data_storage_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE_DEAR_ESR         /* save r2 DEAR ESR r24-31 CR XER SRR */
        FRAME_ENTER_DEAR_ESR(EXC_DSI, %r4)
        li      %r3, T_DSI
        /* FRAME_ENTER leaves SRR1 in %r31 */
trapenter:
trapagain:
        wrtee   %r31                            /* restore MSR[EE] */

        bl      _C_LABEL(trap)                  /* trap(trapcode, tf) */
_C_LABEL(trapexit):
        wrteei  0                               /* disable interrupts */
#       andis.  %r0, %r31, PSL_CE@h
#       tweqi   %r0, 0
        andi.   %r4, %r31, PSL_PR               /* lets look at PSL_PR */
        beq     trapdone                        /* if clear, skip to exit  */
        lwz     %r4, L_MD_ASTPENDING(%r13)      /* get ast pending */
        cmplwi  %r4, 0                          /* is there an ast pending */
        beq+    trapdone                        /*    nope, proceed to exit */
        li      %r6, EXC_AST                    /* yes. */
        stw     %r6, FRAME_EXC(%r1)             /* pretend this is an AST */
        addi    %r4, %r1, FRAME_TF              /* get address of trap frame */
        li      %r3, T_AST
        b       trapagain                       /* and deal with it */
trapdone:
        FRAME_EXIT

        .p2align 4
_C_LABEL(instruction_storage_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE_ESR              /* save ESR r2 r25-31 CR XER SRR0/1 */
        FRAME_ENTER_ESR(EXC_ISI, %r4)
        li      %r3, T_ISI
        b       trapenter

        .p2align 4
_ENTRY(external_input_vector)
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_INTR_PROLOGUE                     /* save SP r25-31 CR LR XER */
        FRAME_INTR_ENTER(EXC_EXI)

        bl      _C_LABEL(intr_extintr)
_C_LABEL(intrcall):
        GET_CPUINFO(%r6)                        /* get curcpu() */
        lwz     %r5, FRAME_SRR1(%r1)            /* get saved SRR1 */
#       andis.  %r0, %r5, PSL_CE@h
#       tweqi   %r0, 0
        andi.   %r4, %r5, PSL_PR                /* lets look at PSL_PR */
        beq     intrexit                        /* if clear, skip to exit  */
        lwz     %r4, L_MD_ASTPENDING(%r13)      /* get ast pending */
        cmplwi  %r4, 0                          /* is there an ast pending */
        beq+    intrexit                        /*    nope, proceed to exit */
        stmw    %r14, FRAME_R14(%r1)            /* save rest of registers */
        FRAME_SAVE_SPEFSCR
        mr      %r31, %r5                       /* needed for trapagain */
        li      %r4, EXC_AST                    /* */
        stw     %r4, FRAME_EXC(%r1)             /* pretend this is an AST */
        addi    %r4, %r1, FRAME_TF              /* get address of trap frame */
        li      %r3, T_AST
        b       trapagain                       /* and deal with it */
intrexit:
        FRAME_INTR_EXIT

        .p2align 4
_C_LABEL(alignment_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE_DEAR_ESR                 /* save SP r25-31 CR LR XER */
        FRAME_ENTER_DEAR_ESR(EXC_ALI, %r4)
        li      %r3, T_ALIGNMENT
        b       trapenter

        .p2align 4
_C_LABEL(program_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE_ESR                      /* save SP r25-31 CR LR XER */
        FRAME_ENTER_ESR(EXC_PGM, %r4)
        li      %r3, T_PROGRAM
        b       trapenter

#ifdef SPR_IVOR7
        .p2align 4
_C_LABEL(fp_unavailable_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE_ESR                      /* save SP r25-31 CR LR XER */
        FRAME_ENTER_ESR(EXC_FPU, %r4)
        li      %r3, T_FP_UNAVAILABLE
        b       trapenter
#endif

        .p2align 4
_C_LABEL(system_call_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE                          /* save SP r26-31 CR LR XER */
        FRAME_ENTER(EXC_SC, %r3)

        wrteei  1                               /* enable interrupts */
        lwz     %r7, L_PROC(%r13)               /* get proc for lwp */
        lwz     %r8, P_MD_SYSCALL(%r7)          /* get syscall */
        mtlr    %r8                             /* need to call indirect */
        blrl                                    /* syscall(tf) */
_C_LABEL(sctrapexit):
        wrteei  0                               /* disable interrupts */
        lwz     %r4, L_MD_ASTPENDING(%r13)      /* get ast pending */
        cmplwi  %r4, 0                          /* is there an ast pending */
        beq+    trapdone                        /*    nope, proceed to exit */
        li      %r0, EXC_AST                    /* yes. */
        stw     %r0, FRAME_EXC(%r1)             /* pretend this is an AST */
        addi    %r4, %r1, FRAME_TF              /* get address of trap frame */
        li      %r3, T_AST
        b       trapenter                       /* and deal with it */

#ifdef SPR_IVOR9
        .p2align 4
_C_LABEL(ap_unavailable_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE                          /* save SP r25-31 CR LR XER */
        FRAME_ENTER(EXC_PGM, %r4)
        li      %r3, T_AP_UNAVAILABLE
        b       trapenter
#endif

        .p2align 4
_C_LABEL(decrementer_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_INTR_PROLOGUE                     /* save SP r25-31 CR LR XER */
        FRAME_INTR_ENTER(EXC_DECR)

        bl      _C_LABEL(intr_decrintr)
        b       intrexit

        .p2align 4
_C_LABEL(fixed_interval_timer_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_PROLOGUE                          /* save SP r25-31 CR LR XER */
        FRAME_INTR_ENTER(EXC_FIT)

        bl      _C_LABEL(intr_fitintr)
        b       intrexit

#ifdef E500_WDOG_STACK
        .data
        .lcomm wdogstk,4096
#endif
        .text
        .p2align 4
_C_LABEL(watchdog_timer_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_CRIT_PROLOGUE                     /* save SP r25-31 CR LR XER */
#ifdef E500_WDOG_STACK
        FRAME_WDOG_ENTER(EXC_WDOG, wdogstk+4096-CALLFRAMELEN)
#else
        FRAME_CRIT_ENTER(EXC_WDOG);
#endif

        bl      _C_LABEL(intr_wdogintr)
        FRAME_CRIT_EXIT

        .p2align 4
_C_LABEL(data_tlb_error_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_TLBPROLOGUE
        FRAME_TLBMISSLOCK
        /*
         * Registers as this point:
         *
         * r2  = cpu_info
         * r20 = scratch
         * r21 = scratch
         * r22 = scratch
         * r23 = scratch
         * r24 = DEAR
         * r25 = ESR
         * r26 = saved r2
         * r27 = CR
         * r28 = XER
         * r29 = scratch
         * r30 = SRR0
         * r31 = SRR1
         *
         * Except for r29, these values must be retained.  However we must
         * be cognizant of nesting.  There are two cases here, both related.
         *
         * We get a critical input or machine check exception and the kernel
         * stack doesn't have a TLB entry so we take an exception.  The other
         * nesting path is some page used by the exception handler will cause
         * a TLB data error.
         *
         * The second case (more probable) is that the PTE loading will fail
         * so we will have to do a hard trap to resolve it.  But in doing so
         * we need to save a trapframe which could result in another DTLB
         * fault.
         *
         * In all cases, the save area stack shall protect us.
         */
        /*
         * Attempt to update the TLB from the page table.
         */
        mflr    %r29                            /* save LR */
        mr      %r23, %r24                      /* address of exception */
        rlwinm  %r22, %r31,                     /* index into ci_pmap_segtab */\
                MSR_DS+PTR_SCALESHIFT+1, \
                31-PTR_SCALESHIFT, \
                31-PTR_SCALESHIFT               /* move PSL_DS[27] to bit 29 */
        bl      pte_load
        FRAME_TLBMISSUNLOCK
        mtlr    %r29                            /* restore LR */
        /*
         * If we returned, pte load failed so let trap deal with it but
         * has kept the contents of r24-r31 (expect r29) intact.
         */
        FRAME_TLBENTER(EXC_DSI)
        li      %r3, T_DATA_TLB_ERROR
        b       trapenter

        .p2align 4
_C_LABEL(instruction_tlb_error_vector):
        /* MSR[CE], MSR[ME], MSR[DE] are unchanged, all others cleared */
        FRAME_TLBPROLOGUE
        FRAME_TLBMISSLOCK
        /*
         * Attempt to update the TLB from the page table.
         */
        mflr    %r29                            /* save LR */
        mr      %r23, %r30                      /* PC of exception */
        rlwinm  %r22, %r31,                     /* index into ci_pmap_segtab */\
                MSR_IS+PTR_SCALESHIFT+1, \
                31-PTR_SCALESHIFT, \
                31-PTR_SCALESHIFT               /* move PSL_IS[26] to bit 29 */
        bl      pte_load
        FRAME_TLBMISSUNLOCK
        mtlr    %r29                            /* restore LR */
        /*
         * If we returned, pte load failed so let trap deal with it but
         * has kept the contents of r24-r31 (expect r29) intact.
         */
        FRAME_TLBENTER(EXC_ISI)
        li      %r3, T_INSTRUCTION_TLB_ERROR
        b       trapenter

        .p2align 4
_C_LABEL(debug_vector):
        FRAME_CRIT_PROLOGUE                     /* save SP r25-31 CR LR XER */
        FRAME_CRIT_ENTER(EXC_DEBUG)
        mfspr   %r6, SPR_DBSR
        stw     %r6, FRAME_ESR(%r1)
        li      %r3, T_DEBUG
        bl      _C_LABEL(trap)
        FRAME_CRIT_EXIT

        .p2align 4
_C_LABEL(spv_unavailable_vector):
        FRAME_PROLOGUE_ESR                      /* save SP r25-31 CR LR XER */
        FRAME_ENTER_ESR(EXC_VEC, %r4)
        li      %r3, T_SPE_UNAVAILABLE
        b       trapenter

        .p2align 4
_C_LABEL(fpdata_vector):
        FRAME_PROLOGUE_ESR                      /* save SP r25-31 CR LR XER */
        FRAME_ENTER_ESR(EXC_FPA, %r4)
        li      %r3, T_EMBEDDED_FP_DATA
        b       trapenter

        .p2align 4
_C_LABEL(fpround_vector):
        FRAME_PROLOGUE_ESR                      /* save SP r25-31 CR LR XER */
        FRAME_ENTER_ESR(EXC_FPA, %r4)
        li      %r3, T_EMBEDDED_FP_ROUND
        b       trapenter

        .p2align 4
_C_LABEL(perfmon_vector):
        FRAME_PROLOGUE_ESR                      /* save SP r25-31 CR LR XER */
        FRAME_ENTER_ESR(EXC_PERF, %r4)
        li      %r3, T_EMBEDDED_PERF_MONITOR
        b       trapenter

        .p2align 4
pte_load:
        /*
         * r2 = scratch
         * r20 = scratch
         * r21 = scratch
         * r22 = index into ci_pmap_{kern,user}_segtab
         * r23 = faulting address
         * The rest are for reference and aren't modifiable.  If the load
         * fails, they will be used by FRAME_TLBENTER to create the trapframe.
         * r24 = DEAR
         * r25 = ESR
         * r26 = saved r2
         * r27 = CR
         * r28 = XER
         * r29 = LR
         * r30 = SRR0
         * r31 = SRR1
         */
        cmplwi  %cr2, %r22, 0           /* remember address space */
        GET_CPUINFO(%r2)
        addi    %r22, %r22, CI_PMAP_SEGTAB /* index into segtab(s) */
        lwzx    %r20, %r22, %r2         /* load kern/user L1 PT addr */
        cmplwi  %r20, 0                 /* is segtab null? */
        beqlr   %cr0                    /* yes, return to fallback to trap */

        rlwinm  %r22, %r23, NSEGPG_SCALESHIFT + PTR_SCALESHIFT, \
                31-(NSEGPG_SCALESHIFT + PTR_SCALESHIFT - 1), \
                31-PTR_SCALESHIFT       /* extract addr bits [0:9] to [20:29] */
        lwzx    %r20, %r22, %r20        /* load address of page table page */
        cmplwi  %r20, 0                 /* is page null? */
        beqlr   %cr0                    /* yes, return to fallback to trap */

        rlwinm  %r22, %r23, \
                NSEGPG_SCALESHIFT + NPTEPG_SCALESHIFT + PTE_SCALESHIFT, \
                31-(NPTEPG_SCALESHIFT + PTE_SCALESHIFT - 1), \
                31-PTE_SCALESHIFT       /* extract addr bits [10:19] to [20:29] */
        lwzx    %r20, %r22, %r20        /* load PTE from page table page */
        cmplwi  %r20, 0                 /* is there a valid PTE? */
        beqlr   %cr0                    /* no, return to fallback to trap */

#if (PTE_UNSYNCED << 1) != PTE_xX
#error PTE_UNSYNCED definition error
#endif
#if (PTE_UNMODIFIED << 1) != PTE_xW
#error PTE_UNMODIFIED definition error
#endif
        andi.   %r22, %r20, (PTE_UNSYNCED|PTE_UNMODIFIED)
                                        /* Does the PTE need to be changed? */
        rotlwi  %r22, %r22, 1           /* if so, clear the right PTE bits */
        andc    %r20, %r20, %r22        /* pte &= ~((pte & (PTE_UNSYNCED|PTE_UNMODIFIED)) << 1)*/

        /*
         * r24-r32 = (no touch)
         * r23 = scratch (was fault addr)
         * r22 = scratch
         * r21 = scratch
         * r20 = pte
         * cr2 = AS 0=eq/!0=ne
         */

        /*
         * This is all E500 specific.  We should have a patchable branch
         * to support other BookE (440) implementations.
         */
e500_pte_load:
        bne+    %cr2, 1f                /* user access? MAS1 is ok. */
        mfspr   %r22, SPR_MAS1          /* get MAS1 */
        lis     %r21, MAS1_TID@h        /* get TID mask */
        andc    %r22, %r22, %r21        /* clear TID */
        mtspr   SPR_MAS1, %r22          /* save MAS1 */
1:
        andi.   %r21, %r20, PTE_WIMGE_MASK /* extract WIMGE from PTE */
        cmplwi  %r21, PTE_M             /* if just PTE_M is set, */
        beq+    %cr0, 2f                /*    skip munging mas2 */
        mfspr   %r22, SPR_MAS2          /* get MAS2 (updated by error) */
        clrrwi  %r22, %r22, PTE_RWX_SHIFT /* clear WIMGE bits */
        or      %r22, %r22, %r21        /* combine with MAS2 contents */
        mtspr   SPR_MAS2, %r22          /* put back into MAS2 */
2:
        /*
         * r23 = fault addr
         * r22 = scratch
         * r21 = scratch
         * r20 = pte
         */

        /*
         * In MAS3, the protection bits are in the low 6 bits:
         *    UX SX UW SW UR SR
         * The User bits are 1 bit left of their Supervisor counterparts.
         * Rotate the PTE protection bits left until they wrap around to become
         * the least significant bits, where the Supervisor protection bits
         * are located.  Increase the rotate amount by 1 to place them where
         * the User protection bits are located.  We get that 1 by extracting
         * the MAS1[TS] (set for User access) and moving it to bit 31 (LSB).
         */
        mfspr   %r21, SPR_MAS1          /* get MAS1 which has TS bit */
        extrwi  %r21, %r21, 1, 31-MAS1_TS_SHIFT
                                        /* extract MAS1_TS to LSB */
        clrrwi  %r23, %r20, PAGE_SHIFT  /* clear non-RPN bits from PTE */
        andi.   %r20, %r20, PTE_RWX_MASK /* isolate protection bits */
        rotrwi  %r20, %r20, PTE_RWX_SHIFT
        andi.   %r22, %r20, (MAS3_SW|MAS3_SR) /* user pages need to be R/W by kernel */
        rotlw   %r20, %r20, %r21        /* rotate protection to correct loc */
        or      %r20, %r20, %r22        /* combine system protection bits */
        or      %r23, %r23, %r20        /* combine RPN and protection bits */
        mtspr   SPR_MAS3, %r23          /* put into MAS3 */
        isync                           /* because ECORE500RM tells us too */
        tlbwe                           /* write the TLB entry */
        /*
         * Increment a counter to show how many tlb misses we've handled here.
         */
        lmw     %r30, CI_EV_TLBMISS_SOFT(%r2)
        addic   %r31, %r31, 1
        addze   %r30, %r30
        stmw    %r30, CI_EV_TLBMISS_SOFT(%r2)

        FRAME_TLBMISSUNLOCK

        /*
         * Cleanup and leave.  We know any higher priority exception will
         * save and restore SPRG1 and %r2 thereby preserving their values.
         *
         * r24 = DEAR (don't care)
         * r25 = ESR (don't care)
         * r26 = saved r2
         * r27 = CR
         * r28 = XER
         * r29 = LR
         * r30 = LSW of counter
         * r31 = MSW of counter
         */
        mtlr    %r29                    /* restore Link Register */
        mtxer   %r28                    /* restore XER */
        mtcr    %r27                    /* restore Condition Register */
        mtsprg1 %r26                    /* save saved r2 across load multiple */
        mfsprg3 %r2                     /* get end of save area */
        addi    %r2,%r2,-4*(32-20)      /* adjust save area down */
        lmw     %r20,0(%r2)             /* restore r20-r31 */
        mtsprg3 %r2                     /* save new end of save area */
        mfsprg1 %r2                     /* restore r2 */
        rfi

        .p2align 4
        .globl _C_LABEL(exception_init)
_C_LABEL(exception_init):
        lis     %r6,_C_LABEL(critical_input_vector)@h
        mtspr   SPR_IVPR, %r6

        ori     %r5,%r6,_C_LABEL(critical_input_vector)@l
        mtspr   SPR_IVOR0, %r5

        ori     %r5,%r6,_C_LABEL(machine_check_vector)@l
        mtspr   SPR_IVOR1, %r5

        ori     %r5,%r6,_C_LABEL(data_storage_vector)@l
        mtspr   SPR_IVOR2, %r5

        ori     %r5,%r6,_C_LABEL(instruction_storage_vector)@l
        mtspr   SPR_IVOR3, %r5

        ori     %r5,%r6,_C_LABEL(external_input_vector)@l
        mtspr   SPR_IVOR4, %r5

        ori     %r5,%r6,_C_LABEL(alignment_vector)@l
        mtspr   SPR_IVOR5, %r5

        ori     %r5,%r6,_C_LABEL(program_vector)@l
        mtspr   SPR_IVOR6, %r5

#ifdef SPR_IVOR7
        ori     %r5,%r6,_C_LABEL(fp_unavailable_vector)@l
        mtspr   SPR_IVOR7, %r5
#endif

        ori     %r5,%r6,_C_LABEL(system_call_vector)@l
        mtspr   SPR_IVOR8, %r5

#ifdef SPR_IVOR9
        ori     %r5,%r6,_C_LABEL(ap_unavailable_vector)@l
        mtspr   SPR_IVOR9, %r5
#endif

        ori     %r5,%r6,_C_LABEL(decrementer_vector)@l
        mtspr   SPR_IVOR10, %r5

        ori     %r5,%r6,_C_LABEL(fixed_interval_timer_vector)@l
        mtspr   SPR_IVOR11, %r5

        ori     %r5,%r6,_C_LABEL(watchdog_timer_vector)@l
        mtspr   SPR_IVOR12, %r5

        ori     %r5,%r6,_C_LABEL(data_tlb_error_vector)@l
        mtspr   SPR_IVOR13, %r5

        ori     %r5,%r6,_C_LABEL(instruction_tlb_error_vector)@l
        mtspr   SPR_IVOR14, %r5

        ori     %r5,%r6,_C_LABEL(debug_vector)@l
        mtspr   SPR_IVOR15, %r5

        ori     %r5,%r6,_C_LABEL(spv_unavailable_vector)@l
        mtspr   SPR_IVOR32, %r5

        ori     %r5,%r6,_C_LABEL(fpdata_vector)@l
        mtspr   SPR_IVOR33, %r5

        ori     %r5,%r6,_C_LABEL(fpround_vector)@l
        mtspr   SPR_IVOR34, %r5

        ori     %r5,%r6,_C_LABEL(perfmon_vector)@l
        mtspr   SPR_IVOR35, %r5

        mfspr   %r5, SPR_PIR    /* get Processor ID register */
        cmplwi  %r5,0
        bnelr                   /* return if non-0 (non-primary) */

        lis     %r5,_C_LABEL(powerpc_intrsw)@ha
        stw     %r3,_C_LABEL(powerpc_intrsw)@l(%r5)

        blr

#ifdef notyet
        .data
        .lcomm  ddbstk,4096
        .text

_ENTRY(cpu_Debugger)
        mflr    %r0
        stw     %r0, CFRAME_LR(%r1)
        
        mfmsr   %r3
        wrteei  0
        mr      %r4,%r1
        lis     %r10,ddbstk@ha
        addi    %r10,%r10,ddbstk@l
        sub     %r5,%r1,%r10
        cmplwi  %r5,4096
        blt     %cr0, 1f
        addi    %r1,%r10,4096-CALLFRAMELEN
1:
        stwu    %r4,-FRAMELEN(%r1)
        stw     %r4,FRAME_R1(%r1)
        stmw    %r13,FRAME_R13(%r1)
        mr      %r26,%r0
        mfcr    %r27
        mfxer   %r28
        mfctr   %r29
        mr      %r30,%r0
        mr      %r31,%r3
        stmw    %r26,FRAME_LR(%r1)
        mr      %r31,%r1
        mr      %r1,%r10
        addi    %r4,%r1,FRAME_TF
        li      %r3,EXC_PGM
        stw     %r3,FRAME_EXC(%r1)
        li      %r3,T_PROGRAM
        bl      _C_LABEL(trap)
        lmw     %r26,FRAME_LR(%r1)
        mtlr    %r26
        mtcr    %r27
        mtxer   %r28
        mtctr   %r29
        mr      %r0,%r31
        lmw     %r13,FRAME_R13(%r1)
        lwz     %r1,FRAME_R1(%r1)
        wrtee   %r0
        blr
#endif /* notyet */