/*      $OpenBSD: locore.s,v 1.233 2026/04/18 17:17:03 kettenis Exp $   */
/*      $NetBSD: locore.s,v 1.137 2001/08/13 06:10:10 jdolecek Exp $    */

/*
 * Copyright (c) 1996-2001 Eduardo Horvath
 * Copyright (c) 1996 Paul Kranenburg
 * Copyright (c) 1996
 *      The President and Fellows of Harvard College.
 *      All rights reserved.
 * Copyright (c) 1992, 1993
 *      The Regents of the University of California.
 *      All rights reserved.
 *
 * This software was developed by the Computer Systems Engineering group
 * at Lawrence Berkeley Laboratory under DARPA contract BG 91-66 and
 * contributed to Berkeley.
 *
 * All advertising materials mentioning features or use of this software
 * must display the following acknowledgement:
 *      This product includes software developed by the University of
 *      California, Lawrence Berkeley Laboratory.
 *      This product includes software developed by Harvard University.
 *
 * 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.
 * 3. All advertising materials mentioning features or use of this
 *    software must display the following acknowledgement:
 *      This product includes software developed by the University of
 *      California, Berkeley and its contributors.
 *      This product includes software developed by Harvard University.
 *      This product includes software developed by Paul Kranenburg.
 * 4. Neither the name of the University nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS 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.
 *
 *      @(#)locore.s    8.4 (Berkeley) 12/10/93
 */

.register %g2,
.register %g3,

#include "assym.h"
#include "ksyms.h"
#include <machine/param.h>
#include <sparc64/sparc64/intreg.h>
#include <machine/ctlreg.h>
#include <machine/psl.h>
#include <machine/trap.h>
#include <machine/frame.h>
#include <machine/pmap.h>
#include <machine/asm.h>

/* Let us use same syntax as C code */
#define db_enter()      ta      1; nop

/* use as needed to align things on longword boundaries */
#define _ALIGN  .align 8
#define ICACHE_ALIGN    .align  32

/* Give this real authority: reset the machine */
#if 1
#define NOTREACHED      sir
#else   /* 1 */
#define NOTREACHED
#endif  /* 1 */

        .section        .sun4v_patch, "ax"
        .globl sun4v_patch
sun4v_patch:
        .previous

        .section        .sun4v_patch_end, "ax"
        .globl sun4v_patch_end
sun4v_patch_end:
        .previous

        .section        .sun4v_pause_patch, "ax"
        .globl sun4v_pause_patch
sun4v_pause_patch:
        .previous

        .section        .sun4v_pause_patch_end, "ax"
        .globl sun4v_pause_patch_end
sun4v_pause_patch_end:
        .previous

#ifdef MULTIPROCESSOR
        .section        .sun4v_mp_patch, "ax"
        .globl sun4v_mp_patch
sun4v_mp_patch:
        .previous

        .section        .sun4v_mp_patch_end, "ax"
        .globl sun4v_mp_patch_end
sun4v_mp_patch_end:
        .previous

        .section        .sun4u_mtp_patch, "ax"
        .globl sun4u_mtp_patch
sun4u_mtp_patch:
        .previous

        .section        .sun4u_mtp_patch_end, "ax"
        .globl sun4u_mtp_patch_end
sun4u_mtp_patch_end:
        .previous
#endif

/*
 * The UltraSPARC T1 has a "feature" where a LDXA/STXA to ASI_SCRATCHPAD
 * registers may corrupt an unrelated integer register file register.
 * To prevent this, it is required to have a non-store or NOP instruction
 * before any LDXA/STXA to this register.
 */
#define GET_CPUINFO_VA(ci) \
        nop                                     ;\
999:    set     CPUINFO_VA, ci                  ;\
        .section        .sun4v_mp_patch, "ax"   ;\
        .word   999b                            ;\
        ldxa    [%g0] ASI_SCRATCHPAD, ci        ;\
        .previous                               ;\
        .section        .sun4u_mtp_patch, "ax"  ;\
        .word   999b                            ;\
        ldxa    [%g0] ASI_SCRATCH, ci           ;\
        .previous

#ifdef MULTIPROCESSOR
#define GET_INTSTACK(ci) \
998:    set     CPUINFO_VA, ci                  ;\
999:    ldx     [ci + CI_SELF], ci              ;\
        .section        .sun4v_mp_patch, "ax"   ;\
        .word   998b                            ;\
        nop                                     ;\
        .word   999b                            ;\
        ldxa    [%g0] ASI_SCRATCHPAD, ci        ;\
        .previous                               ;\
        .section        .sun4u_mtp_patch, "ax"  ;\
        .word   998b                            ;\
        nop                                     ;\
        .word   999b                            ;\
        ldxa    [%g0] ASI_SCRATCH, ci           ;\
        .previous
#else
#define GET_INTSTACK(ci) \
        GET_CPUINFO_VA(ci)
#endif

#define GET_CPCB(pcb) \
        GET_CPUINFO_VA(pcb)                     ;\
        ldx     [pcb + CI_CPCB], pcb

#define GET_CURPROC(curproc) \
        GET_CPUINFO_VA(curproc)                 ;\
        ldx     [curproc + CI_CURPROC], curproc

#ifdef SUN4V

#define GET_CPUINFO_PA(ci) \
        mov     0x10, ci                        ;\
        ldxa    [ci] ASI_SCRATCHPAD, ci

#define GET_MMFSA(mmfsa) \
        GET_CPUINFO_PA(mmfsa)                   ;\
        add     mmfsa, CI_MMFSA, mmfsa          ;\
        ldxa    [mmfsa] ASI_PHYS_CACHED, mmfsa

#endif

#define GET_MMU_CONTEXTID(ctxid, ctx) \
999:    ldxa    [ctx] ASI_DMMU, ctxid           ;\
        .section        .sun4v_patch, "ax"      ;\
        .word   999b                            ;\
        ldxa    [ctx] ASI_MMU_CONTEXTID, ctxid  ;\
        .previous

#define SET_MMU_CONTEXTID(ctxid, ctx) \
999:    stxa    ctxid, [ctx] ASI_DMMU           ;\
        .section        .sun4v_patch, "ax"      ;\
        .word   999b                            ;\
        stxa    ctxid, [ctx] ASI_MMU_CONTEXTID  ;\
        .previous

#define NORMAL_GLOBALS() \
999:    wrpr    %g0, PSTATE_KERN, %pstate       ;\
        .section        .sun4v_patch, "ax"      ;\
        .word   999b                            ;\
        wrpr    %g0, 0, %gl                     ;\
        .previous

#define ALTERNATE_GLOBALS() \
999:    wrpr    %g0, PSTATE_KERN|PSTATE_AG, %pstate     ;\
        .section        .sun4v_patch, "ax"      ;\
        .word   999b                            ;\
        wrpr    %g0, 1, %gl                     ;\
        .previous


/*
 * A handy macro for maintaining instrumentation counters.
 * Note that this clobbers %o0, %o1 and %o2.  Normal usage is
 * something like:
 *      foointr:
 *              TRAP_SETUP ...          ! makes %o registers safe
 *              INCR uvmexp+V_FOO       ! count a foo
 */
        .macro INCR what
        sethi   %hi(\what), %o0
        or      %o0, %lo(\what), %o0
99:
        lduw    [%o0], %o1
        add     %o1, 1, %o2
        casa    [%o0] ASI_P, %o1, %o2
        cmp     %o1, %o2
        bne,pn  %icc, 99b
         nop
        .endm

/*
 * some macros to load and store a register window
 */
        .macro  SPILL storer,base,size,asi

        .irpc n,01234567
                \storer %l\n, [\base + (\n * \size)] \asi
        .endr
        .irpc n,01234567
                \storer %i\n, [\base + ((8+\n) * \size)] \asi
        .endr
        .endm

        .macro FILL loader, base, size, asi
        .irpc n,01234567
                \loader [\base + (\n * \size)] \asi, %l\n
        .endr

        .irpc n,01234567
                \loader [\base + ((8+\n) * \size)] \asi, %i\n
        .endr
        .endm

/*
 * Reserve the given room on stack.
 */

        .macro STACKFRAME size
        save    %sp, \size, %sp
        .endm


        .data
        .globl  data_start
data_start:                                             ! Start of data segment

/*
 * Process 0's u.
 *
 * This must be aligned on an 8 byte boundary.
 */
        .globl  u0
u0:             .xword  0
estack0:        .xword  0

/*
 * This stack is used for bootstrapping and spinning up CPUs.
 */
        .space  4096
        .align  16
tmpstack:

#ifdef DEBUG
/*
 * This stack is used when we detect kernel stack corruption.
 */
        .space  USPACE
        .align  16
panicstack:
#endif  /* DEBUG */

/*
 * romp is the prom entry pointer
 */
        .globl  romp
romp:   .xword  0

/*
 * cputyp is the current cpu type, used to distinguish between
 * the many variations of different sun4* machines. It contains
 * the value CPU_SUN4U or CPU_SUN4V.
 */
        .globl  cputyp
cputyp:
        .word   CPU_SUN4U

        .globl cold
cold:
        .word 1

        _ALIGN

        .text

/*
 * The v9 trap frame is stored in the special trap registers.  The
 * register window is only modified on window overflow, underflow,
 * and clean window traps, where it points to the register window
 * needing service.  Traps have space for 8 instructions, except for
 * the window overflow, underflow, and clean window traps which are
 * 32 instructions long, large enough to in-line.
 *
 * The spitfire CPU (Ultra I) has 4 different sets of global registers.
 * (blah blah...)
 *
 * I used to generate these numbers by address arithmetic, but gas's
 * expression evaluator has about as much sense as your average slug
 * (oddly enough, the code looks about as slimy too).  Thus, all the
 * trap numbers are given as arguments to the trap macros.  This means
 * there is one line per trap.  Sigh.
 */

/*
 *      TA8 -- trap align for 8 instruction traps
 *      TA32 -- trap align for 32 instruction traps
 */
        .macro TA8
        .align 32
        .endm

        .macro TA32
        .align 128
        .endm

/*
 * v9 trap macros:
 *
 *      We have a problem with v9 traps; we have no registers to put the
 *      trap type into.  But we do have a %tt register which already has
 *      that information.  Trap types in these macros are all dummys.
 */
        /* regular vectored traps */
        .macro VTRAP type, label
        ba,a,pt %icc,\label
        nop
        NOTREACHED
        TA8
        .endm

        /* hardware interrupts */
        .macro HARDINT4U lev
        VTRAP \lev, sparc_interrupt
        .endm

        /* software interrupts */
        .macro SOFTINT4U lev, bit
        HARDINT4U \lev
        .endm

        /* traps that just call trap() */
        .macro TRAP type
        VTRAP \type, slowtrap
        .endm

        /* architecturally undefined traps (cause panic) */
        .macro  UTRAP type
#ifndef DEBUG
        sir
#endif  /* DEBUG */
        VTRAP \type, slowtrap
        .endm

        /* software undefined traps (may be replaced) */
        .macro STRAP type
        VTRAP \type, slowtrap
        .endm

#define SYSCALL         VTRAP 0x100, syscall_setup

/*
 * Macro to clear %tt so we don't get confused with old traps.
 */
        .macro CLRTT n
#ifdef DEBUG
#if 0   /* for henric, but not yet */
        wrpr    %g0, 0x1ff - \n, %tt
#else   /* 0 */
        wrpr    %g0, 0x1ff, %tt
#endif  /* 0 */
#endif  /* DEBUG */
        .endm

        .macro UCLEANWIN
        rdpr %cleanwin, %o7             ! 024-027 = clean window trap
        inc %o7                         !       This handler is in-lined and cannot fault
#ifdef DEBUG
        set     0xbadcafe, %l0          ! DEBUG -- compiler should not rely on zero-ed registers.
#else   /* DEBUG */
        clr     %l0
#endif  /* DEBUG */
        wrpr %g0, %o7, %cleanwin        !       Nucleus (trap&IRQ) code does not need clean windows

        mov %l0,%l1; mov %l0,%l2        !       Clear out %l0-%l8 and %o0-%o8 and inc %cleanwin and done
        mov %l0,%l3; mov %l0,%l4
        mov %l0, %l5
        mov %l0, %l6; mov %l0, %l7; mov %l0, %o0; mov %l0, %o1

        mov %l0, %o2; mov %l0, %o3; mov %l0, %o4; mov %l0, %o5;
        mov %l0, %o6; mov %l0, %o7
        CLRTT 5
        retry; nop; NOTREACHED; TA32
        .endm

        .macro KCLEANWIN
#ifdef DEBUG
        set     0xbadbeef, %l0          ! DEBUG
#else
        clr     %l0
#endif  /* DEBUG */
        mov %l0, %l1; mov %l0, %l2      ! 024-027 = clean window trap
        rdpr %cleanwin, %o7             !       This handler is in-lined and cannot fault
        inc %o7; mov %l0, %l3   !       Nucleus (trap&IRQ) code does not need clean windows
        wrpr %g0, %o7, %cleanwin        !       Clear out %l0-%l8 and %o0-%o8 and inc %cleanwin and done
        mov %l0, %l4; mov %l0, %l5; mov %l0, %l6; mov %l0, %l7
        mov %l0, %o0; mov %l0, %o1; mov %l0, %o2; mov %l0, %o3

        mov %l0, %o4; mov %l0, %o5; mov %l0, %o6; mov %l0, %o7
        CLRTT 8
        retry; nop; TA32
        .endm

        .macro IMMU_MISS n
        ldxa    [%g0] ASI_IMMU_8KPTR, %g2!      Load IMMU 8K TSB pointer
        ldxa    [%g0] ASI_IMMU, %g1     !       Load IMMU tag target register
        ldda    [%g2] ASI_NUCLEUS_QUAD_LDD, %g4!Load TSB tag:data into %g4
        brgez,pn %g5, instr_miss        !       Entry invalid?  Punt
         cmp    %g1, %g4                !       Compare TLB tags
        bne,pn %xcc, instr_miss         !       Got right tag?
         nop
        CLRTT \n
        stxa    %g5, [%g0] ASI_IMMU_DATA_IN!    Enter new mapping
        retry                           !       Try new mapping
1:
        sir
        TA32
        .endm

        .macro DMMU_MISS n
        ldxa    [%g0] ASI_DMMU_8KPTR, %g2!      Load DMMU 8K TSB pointer
        ldxa    [%g0] ASI_DMMU, %g1     !       Load DMMU tag target register
        ldda    [%g2] ASI_NUCLEUS_QUAD_LDD, %g4!Load TSB tag:data into %g4
        brgez,pn %g5, data_miss         !       Entry invalid?  Punt
         xor    %g1, %g4, %g4           !       Compare TLB tags
        brnz,pn %g4, data_miss          !       Got right tag?
         nop
        CLRTT \n
        stxa    %g5, [%g0] ASI_DMMU_DATA_IN!    Enter new mapping
        retry                           !       Try new mapping
1:
        sir
        TA32
        .endm

        .macro DMMU_PROT
        ba,a,pt %xcc, dmmu_write_fault
        nop
        TA32
        .endm
/*
 * Here are some often repeated traps as macros.
 */

        ! spill a 64-bit user register window
        .macro USPILL64 label, as
\label:
        wr      %g0, \as, %asi
        stxa    %l0, [%sp + BIAS + ( 0*8)] %asi
        stxa    %l1, [%sp + BIAS + ( 1*8)] %asi
        stxa    %l2, [%sp + BIAS + ( 2*8)] %asi
        stxa    %l3, [%sp + BIAS + ( 3*8)] %asi
        stxa    %l4, [%sp + BIAS + ( 4*8)] %asi
        stxa    %l5, [%sp + BIAS + ( 5*8)] %asi
        stxa    %l6, [%sp + BIAS + ( 6*8)] %asi
        stxa    %l7, [%sp + BIAS + ( 7*8)] %asi
        stxa    %i0, [%sp + BIAS + ( 8*8)] %asi
        stxa    %i1, [%sp + BIAS + ( 9*8)] %asi
        stxa    %i2, [%sp + BIAS + (10*8)] %asi
        stxa    %i3, [%sp + BIAS + (11*8)] %asi
        stxa    %i4, [%sp + BIAS + (12*8)] %asi
        stxa    %i5, [%sp + BIAS + (13*8)] %asi
        stxa    %i6, [%sp + BIAS + (14*8)] %asi
        GET_CPCB(%g5)
        ldx     [%g5 + PCB_WCOOKIE], %g5
        xor     %g5, %i7, %g5           ! stackghost
        stxa    %g5, [%sp + BIAS + (15*8)] %asi
        saved
        CLRTT 1
        retry
        NOTREACHED
        TA32
        .endm

        ! spill a 64-bit kernel register window
        .macro SPILL64 label, as
\label:
        wr      %g0, \as, %asi
        SPILL   stxa, %sp+BIAS, 8, %asi
        saved
        CLRTT 1
        retry
        NOTREACHED
        TA32
        .endm

        ! spill a 32-bit register window
        .macro SPILL32 label, as
\label:
        wr      %g0, \as, %asi
        srl     %sp, 0, %sp ! fixup 32-bit pointers
        SPILL   stwa, %sp, 4, %asi
        saved
        CLRTT 2
        retry
        NOTREACHED
        TA32
        .endm

        ! Spill either 32-bit or 64-bit register window.
        .macro SPILLBOTH label64,label32, as
        andcc   %sp, 1, %g0
        bnz,pt  %xcc, \label64+4        ! Is it a v9 or v8 stack?
         wr     %g0, \as, %asi
        ba,pt   %xcc, \label32+8
         srl    %sp, 0, %sp ! fixup 32-bit pointers
        NOTREACHED
        TA32
        .endm

        ! fill a 64-bit user register window
        .macro UFILL64 label, as
\label:
        wr      %g0, \as, %asi
        FILL    ldxa, %sp+BIAS, 8, %asi
        GET_CPCB(%g5)
        ldx     [%g5 + PCB_WCOOKIE], %g5
        xor     %g5, %i7, %i7           ! stackghost
        restored
        CLRTT 3
        retry
        NOTREACHED
        TA32
        .endm

        ! fill a 64-bit kernel register window
        .macro FILL64 label, as
\label:
        wr      %g0, \as, %asi
        FILL    ldxa, %sp+BIAS, 8, %asi
        restored
        CLRTT 3
        retry
        NOTREACHED
        TA32
        .endm

        ! fill a 32-bit register window
        .macro FILL32 label, as
\label:
        wr      %g0, \as, %asi
        srl     %sp, 0, %sp ! fixup 32-bit pointers
        FILL    lda, %sp, 4, %asi
        restored
        CLRTT 4
        retry
        NOTREACHED
        TA32
        .endm

        ! fill either 32-bit or 64-bit register window.
        .macro FILLBOTH label64,label32, as
        andcc   %sp, 1, %i0
        bnz     (\label64)+4 ! See if it's a v9 stack or v8
         wr     %g0, \as, %asi
        ba      (\label32)+8
         srl    %sp, 0, %sp ! fixup 32-bit pointers
        NOTREACHED
        TA32
        .endm

#ifdef SUN4V

        .macro  sun4v_tl0_reserved      count
        .rept   \count
        ba,a,pt %xcc, slowtrap
         nop
        .align  32
        .endr
        .endm

#define sun4v_tl0_unused sun4v_tl0_reserved

        .macro  sun4v_tl1_reserved      count
        .rept   \count
        ba,a,pt %xcc, slowtrap
         nop
        .align  32
        .endr
        .endm

#define sun4v_tl1_unused sun4v_tl1_reserved

        .macro sun4v_tl1_kspill_normal
        ba,a,pt %xcc,kspill_normal
         nop
        .align 128
        .endm

        .macro sun4v_tl1_uspill_normal
        ba,a,pt %xcc,pcbspill_normals
         nop
        .align 128
        .endm

        .macro sun4v_tl1_uspill_other
        ba,a,pt %xcc,pcbspill_others
         nop
        .align 128
        .endm

#endif

        .globl  start, kernel_text
        kernel_text = start                     ! for kvm_mkdb(8)
start:
        /* Traps from TL=0 -- traps from user mode */
        .globl  trapbase
trapbase:
        b dostart; nop; TA8     ! 000 = reserved -- Use it to boot
        /* We should not get the next 5 traps */
        UTRAP 0x001             ! 001 = POR Reset -- ROM should get this
        UTRAP 0x002             ! 002 = WDR -- ROM should get this
        UTRAP 0x003             ! 003 = XIR -- ROM should get this
        UTRAP 0x004             ! 004 = SIR -- ROM should get this
        UTRAP 0x005             ! 005 = RED state exception
        UTRAP 0x006; UTRAP 0x007
        VTRAP T_INST_EXCEPT, textfault  ! 008 = instr. access except
        VTRAP T_TEXTFAULT, textfault    ! 009 = instr. access MMU miss
        VTRAP T_INST_ERROR, textfault   ! 00a = instr. access err
        UTRAP 0x00b; UTRAP 0x00c; UTRAP 0x00d; UTRAP 0x00e; UTRAP 0x00f
        TRAP T_ILLINST                  ! 010 = illegal instruction
        TRAP T_PRIVINST         ! 011 = privileged instruction
        UTRAP 0x012                     ! 012 = unimplemented LDD
        UTRAP 0x013                     ! 013 = unimplemented STD
        UTRAP 0x014; UTRAP 0x015; UTRAP 0x016; UTRAP 0x017; UTRAP 0x018
        UTRAP 0x019; UTRAP 0x01a; UTRAP 0x01b; UTRAP 0x01c; UTRAP 0x01d
        UTRAP 0x01e; UTRAP 0x01f
        TRAP T_FPDISABLED               ! 020 = fp instr, but EF bit off in psr
        TRAP T_FP_IEEE_754              ! 021 = ieee 754 exception
        TRAP T_FP_OTHER                 ! 022 = other fp exception
        TRAP T_TAGOF                    ! 023 = tag overflow
        UCLEANWIN                       ! 024-027 = clean window trap
        TRAP T_DIV0                     ! 028 = divide by zero
        UTRAP 0x029                     ! 029 = internal processor error
        UTRAP 0x02a; UTRAP 0x02b; UTRAP 0x02c; UTRAP 0x02d; UTRAP 0x02e; UTRAP 0x02f
        VTRAP T_DATAFAULT, datafault    ! 030 = data fetch fault
        UTRAP 0x031                     ! 031 = data MMU miss -- no MMU
        VTRAP T_DATA_ERROR, datafault   ! 032 = data access error
        VTRAP T_DATA_PROT, datafault    ! 033 = data protection fault
        TRAP T_ALIGN                    ! 034 = address alignment error -- we could fix it inline...
        TRAP T_LDDF_ALIGN               ! 035 = LDDF address alignment error -- we could fix it inline...
        TRAP T_STDF_ALIGN               ! 036 = STDF address alignment error -- we could fix it inline...
        TRAP T_PRIVACT                  ! 037 = privileged action
        TRAP T_LDQF_ALIGN               ! 038 = LDDF address alignment error
        TRAP T_STQF_ALIGN               ! 039 = STQF address alignment error
        UTRAP 0x03a; UTRAP 0x03b; UTRAP 0x03c;
        UTRAP 0x03d; UTRAP 0x03e; UTRAP 0x03f;
        VTRAP T_ASYNC_ERROR, datafault  ! 040 = data fetch fault
        SOFTINT4U 1, IE_L1              ! 041 = level 1 interrupt
        HARDINT4U 2                     ! 042 = level 2 interrupt
        HARDINT4U 3                     ! 043 = level 3 interrupt
        SOFTINT4U 4, IE_L4              ! 044 = level 4 interrupt
        HARDINT4U 5                     ! 045 = level 5 interrupt
        SOFTINT4U 6, IE_L6              ! 046 = level 6 interrupt
        HARDINT4U 7                     ! 047 = level 7 interrupt
        HARDINT4U 8                     ! 048 = level 8 interrupt
        HARDINT4U 9                     ! 049 = level 9 interrupt
        HARDINT4U 10                    ! 04a = level 10 interrupt
        HARDINT4U 11                    ! 04b = level 11 interrupt
        HARDINT4U 12                    ! 04c = level 12 interrupt
        HARDINT4U 13                    ! 04d = level 13 interrupt
        HARDINT4U 14                    ! 04e = level 14 interrupt
        HARDINT4U 15                    ! 04f = nonmaskable interrupt
        UTRAP 0x050; UTRAP 0x051; UTRAP 0x052; UTRAP 0x053; UTRAP 0x054; UTRAP 0x055
        UTRAP 0x056; UTRAP 0x057; UTRAP 0x058; UTRAP 0x059; UTRAP 0x05a; UTRAP 0x05b
        UTRAP 0x05c; UTRAP 0x05d; UTRAP 0x05e; UTRAP 0x05f
        VTRAP 0x060, interrupt_vector;  ! 060 = interrupt vector
        TRAP T_PA_WATCHPT               ! 061 = physical address data watchpoint
        TRAP T_VA_WATCHPT               ! 062 = virtual address data watchpoint
        VTRAP T_ECCERR, cecc_catch      ! 063 = Correctable ECC error
ufast_IMMU_miss:                        ! 064 = fast instr access MMU miss
        IMMU_MISS 6
ufast_DMMU_miss:                        ! 068 = fast data access MMU miss
        DMMU_MISS 7
ufast_DMMU_protection:                  ! 06c = fast data access MMU protection
        DMMU_PROT
        UTRAP 0x070                     ! Implementation dependent traps
        UTRAP 0x071; UTRAP 0x072; UTRAP 0x073; UTRAP 0x074; UTRAP 0x075; UTRAP 0x076
        UTRAP 0x077; UTRAP 0x078; UTRAP 0x079; UTRAP 0x07a; UTRAP 0x07b; UTRAP 0x07c
        UTRAP 0x07d; UTRAP 0x07e; UTRAP 0x07f
user_uspill:
        USPILL64 uspill8,ASI_AIUS       ! 0x080 spill_0_normal -- used to save user windows in user mode
        SPILL32 uspill4,ASI_AIUS        ! 0x084 spill_1_normal
        SPILLBOTH uspill8,uspill4,ASI_AIUS              ! 0x088 spill_2_normal
#ifdef DEBUG
        sir
#endif  /* DEBUG */
        UTRAP 0x08c; TA32       ! 0x08c spill_3_normal
user_kspill:
        UTRAP 0x090; TA32       ! 0x090 spill_4_normal -- used to save supervisor windows
        SPILL64 kspill8,ASI_N   ! 0x094 spill_5_normal
        SPILL32 kspill4,ASI_N   ! 0x098 spill_6_normal
        SPILLBOTH kspill8,kspill4,ASI_N ! 0x09c spill_7_normal
user_uspillk:
        USPILL64 uspillk8,ASI_AIUS      ! 0x0a0 spill_0_other -- used to save user windows in supervisor mode
        SPILL32 uspillk4,ASI_AIUS       ! 0x0a4 spill_1_other
        SPILLBOTH uspillk8,uspillk4,ASI_AIUS    ! 0x0a8 spill_2_other
        UTRAP 0x0ac; TA32       ! 0x0ac spill_3_other
        UTRAP 0x0b0; TA32       ! 0x0b0 spill_4_other
        UTRAP 0x0b4; TA32       ! 0x0b4 spill_5_other
        UTRAP 0x0b8; TA32       ! 0x0b8 spill_6_other
        UTRAP 0x0bc; TA32       ! 0x0bc spill_7_other
user_ufill:
        UFILL64 ufill8,ASI_AIUS ! 0x0c0 fill_0_normal -- used to fill windows when running user mode
        FILL32 ufill4,ASI_AIUS  ! 0x0c4 fill_1_normal
        FILLBOTH ufill8,ufill4,ASI_AIUS ! 0x0c8 fill_2_normal
        UTRAP 0x0cc; TA32       ! 0x0cc fill_3_normal
user_kfill:
        UTRAP 0x0d0; TA32       ! 0x0d0 fill_4_normal -- used to fill windows when running supervisor mode
        FILL64 kfill8,ASI_N     ! 0x0d4 fill_5_normal
        FILL32 kfill4,ASI_N     ! 0x0d8 fill_6_normal
        FILLBOTH kfill8,kfill4,ASI_N    ! 0x0dc fill_7_normal
user_ufillk:
        UFILL64 ufillk8,ASI_AIUS        ! 0x0e0 fill_0_other
        FILL32 ufillk4,ASI_AIUS ! 0x0e4 fill_1_other
        FILLBOTH ufillk8,ufillk4,ASI_AIUS       ! 0x0e8 fill_2_other
        UTRAP 0x0ec; TA32       ! 0x0ec fill_3_other
        UTRAP 0x0f0; TA32       ! 0x0f0 fill_4_other
        UTRAP 0x0f4; TA32       ! 0x0f4 fill_5_other
        UTRAP 0x0f8; TA32       ! 0x0f8 fill_6_other
        UTRAP 0x0fc; TA32       ! 0x0fc fill_7_other
user_syscall:
        SYSCALL                 ! 0x100 = sun syscall
        TRAP T_BREAKPOINT       ! 0x101 = pseudo breakpoint instruction
        STRAP 0x102; STRAP 0x103; STRAP 0x104; STRAP 0x105; STRAP 0x106; STRAP 0x107
        SYSCALL                 ! 0x108 = svr4 syscall
        SYSCALL                 ! 0x109 = bsd syscall
        TRAP T_KGDB_EXEC        ! 0x10a = enter kernel gdb on kernel startup
        STRAP 0x10b; STRAP 0x10c; STRAP 0x10d; STRAP 0x10e; STRAP 0x10f;
        STRAP 0x110; STRAP 0x111; STRAP 0x112; STRAP 0x113; STRAP 0x114; STRAP 0x115; STRAP 0x116; STRAP 0x117
        STRAP 0x118; STRAP 0x119; STRAP 0x11a; STRAP 0x11b; STRAP 0x11c; STRAP 0x11d; STRAP 0x11e; STRAP 0x11f
        STRAP 0x120; STRAP 0x121; STRAP 0x122; STRAP 0x123; STRAP 0x124; STRAP 0x125; STRAP 0x126; STRAP 0x127
        STRAP 0x128; STRAP 0x129; STRAP 0x12a; STRAP 0x12b; STRAP 0x12c; STRAP 0x12d; STRAP 0x12e; STRAP 0x12f
        STRAP 0x130; STRAP 0x131; STRAP 0x132; STRAP 0x133; STRAP 0x134; STRAP 0x135; STRAP 0x136; STRAP 0x137
        STRAP 0x138; STRAP 0x139; STRAP 0x13a; STRAP 0x13b; STRAP 0x13c; STRAP 0x13d; STRAP 0x13e; STRAP 0x13f
        SYSCALL                 ! 0x140 SVID syscall (Solaris 2.7)
        SYSCALL                 ! 0x141 SPARC International syscall
        SYSCALL                 ! 0x142 OS Vendor syscall
        SYSCALL                 ! 0x143 HW OEM syscall
        STRAP 0x144; STRAP 0x145; STRAP 0x146; STRAP 0x147
        STRAP 0x148; STRAP 0x149; STRAP 0x14a; STRAP 0x14b; STRAP 0x14c; STRAP 0x14d; STRAP 0x14e; STRAP 0x14f
        STRAP 0x150; STRAP 0x151; STRAP 0x152; STRAP 0x153; STRAP 0x154; STRAP 0x155; STRAP 0x156; STRAP 0x157
        STRAP 0x158; STRAP 0x159; STRAP 0x15a; STRAP 0x15b; STRAP 0x15c; STRAP 0x15d; STRAP 0x15e; STRAP 0x15f
        STRAP 0x160; STRAP 0x161; STRAP 0x162; STRAP 0x163; STRAP 0x164; STRAP 0x165; STRAP 0x166; STRAP 0x167
        STRAP 0x168; STRAP 0x169; STRAP 0x16a; STRAP 0x16b; STRAP 0x16c; STRAP 0x16d; STRAP 0x16e; STRAP 0x16f
        STRAP 0x170; STRAP 0x171; STRAP 0x172; STRAP 0x173; STRAP 0x174; STRAP 0x175; STRAP 0x176; STRAP 0x177
        STRAP 0x178; STRAP 0x179; STRAP 0x17a; STRAP 0x17b; STRAP 0x17c; STRAP 0x17d; STRAP 0x17e; STRAP 0x17f
        ! Traps beyond 0x17f are reserved
        UTRAP 0x180; UTRAP 0x181; UTRAP 0x182; UTRAP 0x183; UTRAP 0x184; UTRAP 0x185; UTRAP 0x186; UTRAP 0x187
        UTRAP 0x188; UTRAP 0x189; UTRAP 0x18a; UTRAP 0x18b; UTRAP 0x18c; UTRAP 0x18d; UTRAP 0x18e; UTRAP 0x18f
        UTRAP 0x190; UTRAP 0x191; UTRAP 0x192; UTRAP 0x193; UTRAP 0x194; UTRAP 0x195; UTRAP 0x196; UTRAP 0x197
        UTRAP 0x198; UTRAP 0x199; UTRAP 0x19a; UTRAP 0x19b; UTRAP 0x19c; UTRAP 0x19d; UTRAP 0x19e; UTRAP 0x19f
        UTRAP 0x1a0; UTRAP 0x1a1; UTRAP 0x1a2; UTRAP 0x1a3; UTRAP 0x1a4; UTRAP 0x1a5; UTRAP 0x1a6; UTRAP 0x1a7
        UTRAP 0x1a8; UTRAP 0x1a9; UTRAP 0x1aa; UTRAP 0x1ab; UTRAP 0x1ac; UTRAP 0x1ad; UTRAP 0x1ae; UTRAP 0x1af
        UTRAP 0x1b0; UTRAP 0x1b1; UTRAP 0x1b2; UTRAP 0x1b3; UTRAP 0x1b4; UTRAP 0x1b5; UTRAP 0x1b6; UTRAP 0x1b7
        UTRAP 0x1b8; UTRAP 0x1b9; UTRAP 0x1ba; UTRAP 0x1bb; UTRAP 0x1bc; UTRAP 0x1bd; UTRAP 0x1be; UTRAP 0x1bf
        UTRAP 0x1c0; UTRAP 0x1c1; UTRAP 0x1c2; UTRAP 0x1c3; UTRAP 0x1c4; UTRAP 0x1c5; UTRAP 0x1c6; UTRAP 0x1c7
        UTRAP 0x1c8; UTRAP 0x1c9; UTRAP 0x1ca; UTRAP 0x1cb; UTRAP 0x1cc; UTRAP 0x1cd; UTRAP 0x1ce; UTRAP 0x1cf
        UTRAP 0x1d0; UTRAP 0x1d1; UTRAP 0x1d2; UTRAP 0x1d3; UTRAP 0x1d4; UTRAP 0x1d5; UTRAP 0x1d6; UTRAP 0x1d7
        UTRAP 0x1d8; UTRAP 0x1d9; UTRAP 0x1da; UTRAP 0x1db; UTRAP 0x1dc; UTRAP 0x1dd; UTRAP 0x1de; UTRAP 0x1df
        UTRAP 0x1e0; UTRAP 0x1e1; UTRAP 0x1e2; UTRAP 0x1e3; UTRAP 0x1e4; UTRAP 0x1e5; UTRAP 0x1e6; UTRAP 0x1e7
        UTRAP 0x1e8; UTRAP 0x1e9; UTRAP 0x1ea; UTRAP 0x1eb; UTRAP 0x1ec; UTRAP 0x1ed; UTRAP 0x1ee; UTRAP 0x1ef
        UTRAP 0x1f0; UTRAP 0x1f1; UTRAP 0x1f2; UTRAP 0x1f3; UTRAP 0x1f4; UTRAP 0x1f5; UTRAP 0x1f6; UTRAP 0x1f7
        UTRAP 0x1f8; UTRAP 0x1f9; UTRAP 0x1fa; UTRAP 0x1fb; UTRAP 0x1fc; UTRAP 0x1fd; UTRAP 0x1fe; UTRAP 0x1ff

        /* Traps from TL>0 -- traps from supervisor mode */
trapbase_priv:
        UTRAP 0x000             ! 000 = reserved -- Use it to boot
        /* We should not get the next 5 traps */
        UTRAP 0x001             ! 001 = POR Reset -- ROM should get this
        UTRAP 0x002             ! 002 = WDR Watchdog -- ROM should get this
        UTRAP 0x003             ! 003 = XIR -- ROM should get this
        UTRAP 0x004             ! 004 = SIR -- ROM should get this
        UTRAP 0x005             ! 005 = RED state exception
        UTRAP 0x006; UTRAP 0x007
ktextfault:
        VTRAP T_INST_EXCEPT, textfault  ! 008 = instr. access except
        VTRAP T_TEXTFAULT, textfault    ! 009 = instr. access MMU miss -- no MMU
        VTRAP T_INST_ERROR, textfault   ! 00a = instr. access err
        UTRAP 0x00b; UTRAP 0x00c; UTRAP 0x00d; UTRAP 0x00e; UTRAP 0x00f
        TRAP T_ILLINST                  ! 010 = illegal instruction
        TRAP T_PRIVINST         ! 011 = privileged instruction
        UTRAP 0x012                     ! 012 = unimplemented LDD
        UTRAP 0x013                     ! 013 = unimplemented STD
        UTRAP 0x014; UTRAP 0x015; UTRAP 0x016; UTRAP 0x017; UTRAP 0x018
        UTRAP 0x019; UTRAP 0x01a; UTRAP 0x01b; UTRAP 0x01c; UTRAP 0x01d
        UTRAP 0x01e; UTRAP 0x01f
        TRAP T_FPDISABLED               ! 020 = fp instr, but EF bit off in psr
        TRAP T_FP_IEEE_754              ! 021 = ieee 754 exception
        TRAP T_FP_OTHER                 ! 022 = other fp exception
        TRAP T_TAGOF                    ! 023 = tag overflow
        KCLEANWIN                       ! 024-027 = clean window trap
        TRAP T_DIV0                     ! 028 = divide by zero
        UTRAP 0x029                     ! 029 = internal processor error
        UTRAP 0x02a; UTRAP 0x02b; UTRAP 0x02c; UTRAP 0x02d; UTRAP 0x02e; UTRAP 0x02f
kdatafault:
        VTRAP T_DATAFAULT, winfault     ! 030 = data fetch fault
        UTRAP 0x031                     ! 031 = data MMU miss -- no MMU
        VTRAP T_DATA_ERROR, winfault    ! 032 = data fetch fault
        VTRAP T_DATA_PROT, winfault     ! 033 = data fetch fault
        VTRAP T_ALIGN, checkalign       ! 034 = address alignment error -- we could fix it inline...
!       sir; nop; TA8   ! DEBUG -- trap all kernel alignment errors
        TRAP T_LDDF_ALIGN               ! 035 = LDDF address alignment error -- we could fix it inline...
        TRAP T_STDF_ALIGN               ! 036 = STDF address alignment error -- we could fix it inline...
        TRAP T_PRIVACT                  ! 037 = privileged action
        UTRAP 0x038; UTRAP 0x039; UTRAP 0x03a; UTRAP 0x03b; UTRAP 0x03c;
        UTRAP 0x03d; UTRAP 0x03e; UTRAP 0x03f;
        VTRAP T_ASYNC_ERROR, winfault   ! 040 = data fetch fault
        SOFTINT4U 1, IE_L1              ! 041 = level 1 interrupt
        HARDINT4U 2                     ! 042 = level 2 interrupt
        HARDINT4U 3                     ! 043 = level 3 interrupt
        SOFTINT4U 4, IE_L4              ! 044 = level 4 interrupt
        HARDINT4U 5                     ! 045 = level 5 interrupt
        SOFTINT4U 6, IE_L6              ! 046 = level 6 interrupt
        HARDINT4U 7                     ! 047 = level 7 interrupt
        HARDINT4U 8                     ! 048 = level 8 interrupt
        HARDINT4U 9                     ! 049 = level 9 interrupt
        HARDINT4U 10                    ! 04a = level 10 interrupt
        HARDINT4U 11                    ! 04b = level 11 interrupt
        HARDINT4U 12                    ! 04c = level 12 interrupt
        HARDINT4U 13                    ! 04d = level 13 interrupt
        HARDINT4U 14                    ! 04e = level 14 interrupt
        HARDINT4U 15                    ! 04f = nonmaskable interrupt
        UTRAP 0x050; UTRAP 0x051; UTRAP 0x052; UTRAP 0x053; UTRAP 0x054; UTRAP 0x055
        UTRAP 0x056; UTRAP 0x057; UTRAP 0x058; UTRAP 0x059; UTRAP 0x05a; UTRAP 0x05b
        UTRAP 0x05c; UTRAP 0x05d; UTRAP 0x05e; UTRAP 0x05f
        VTRAP 0x060, interrupt_vector; ! 060 = interrupt vector
        TRAP T_PA_WATCHPT               ! 061 = physical address data watchpoint
        TRAP T_VA_WATCHPT               ! 062 = virtual address data watchpoint
        VTRAP T_ECCERR, cecc_catch      ! 063 = Correctable ECC error
kfast_IMMU_miss:                        ! 064 = fast instr access MMU miss
        IMMU_MISS 9
kfast_DMMU_miss:                        ! 068 = fast data access MMU miss
        DMMU_MISS 10
kfast_DMMU_protection:                  ! 06c = fast data access MMU protection
        DMMU_PROT
        UTRAP 0x070                     ! Implementation dependent traps
        UTRAP 0x071; UTRAP 0x072; UTRAP 0x073; UTRAP 0x074; UTRAP 0x075; UTRAP 0x076
        UTRAP 0x077; UTRAP 0x078; UTRAP 0x079; UTRAP 0x07a; UTRAP 0x07b; UTRAP 0x07c
        UTRAP 0x07d; UTRAP 0x07e; UTRAP 0x07f
nucleus_uspill:
        USPILL64 1,ASI_AIUS     ! 0x080 spill_0_normal -- used to save user windows
        SPILL32 2,ASI_AIUS      ! 0x084 spill_1_normal
        SPILLBOTH 1b,2b,ASI_AIUS        ! 0x088 spill_2_normal
        UTRAP 0x08c; TA32       ! 0x08c spill_3_normal
nucleus_kspill:
        UTRAP 0x090; TA32       ! 0x090 spill_4_normal -- used to save supervisor windows
        SPILL64 1,ASI_N         ! 0x094 spill_5_normal
        SPILL32 2,ASI_N         ! 0x098 spill_6_normal
        SPILLBOTH 1b,2b,ASI_N   ! 0x09c spill_7_normal
nucleus_uspillk:
        USPILL64 1,ASI_AIUS     ! 0x0a0 spill_0_other -- used to save user windows in nucleus mode
        SPILL32 2,ASI_AIUS      ! 0x0a4 spill_1_other
        SPILLBOTH 1b,2b,ASI_AIUS        ! 0x0a8 spill_2_other
        UTRAP 0x0ac; TA32       ! 0x0ac spill_3_other
        UTRAP 0x0b0; TA32       ! 0x0b0 spill_4_other
        UTRAP 0x0b4; TA32       ! 0x0b4 spill_5_other
        UTRAP 0x0b8; TA32       ! 0x0b8 spill_6_other
        UTRAP 0x0bc; TA32       ! 0x0bc spill_7_other
nucleus_ufill:
        UFILL64 1,ASI_AIUS      ! 0x0c0 fill_0_normal -- used to fill windows when running nucleus mode from user
        FILL32 2,ASI_AIUS       ! 0x0c4 fill_1_normal
        FILLBOTH 1b,2b,ASI_AIUS ! 0x0c8 fill_2_normal
        UTRAP 0x0cc; TA32       ! 0x0cc fill_3_normal
nucleus_sfill:
        UTRAP 0x0d0; TA32       ! 0x0d0 fill_4_normal -- used to fill windows when running nucleus mode from supervisor
        FILL64 1,ASI_N          ! 0x0d4 fill_5_normal
        FILL32 2,ASI_N          ! 0x0d8 fill_6_normal
        FILLBOTH 1b,2b,ASI_N    ! 0x0dc fill_7_normal
nucleus_kfill:
        UFILL64 1,ASI_AIUS      ! 0x0e0 fill_0_other -- used to fill user windows when running nucleus mode -- will we ever use this?
        FILL32 2,ASI_AIUS       ! 0x0e4 fill_1_other
        FILLBOTH 1b,2b,ASI_AIUS ! 0x0e8 fill_2_other
        UTRAP 0x0ec; TA32       ! 0x0ec fill_3_other
        UTRAP 0x0f0; TA32       ! 0x0f0 fill_4_other
        UTRAP 0x0f4; TA32       ! 0x0f4 fill_5_other
        UTRAP 0x0f8; TA32       ! 0x0f8 fill_6_other
        UTRAP 0x0fc; TA32       ! 0x0fc fill_7_other
nucleus_syscall:
        SYSCALL                 ! 0x100 = sun syscall
        TRAP T_BREAKPOINT       ! 0x101 = pseudo breakpoint instruction
        STRAP 0x102; STRAP 0x103; STRAP 0x104; STRAP 0x105; STRAP 0x106; STRAP 0x107
        SYSCALL                 ! 0x108 = svr4 syscall
        SYSCALL                 ! 0x109 = bsd syscall
        TRAP T_KGDB_EXEC        ! 0x10a = enter kernel gdb on kernel startup
        STRAP 0x10b; STRAP 0x10c; STRAP 0x10d; STRAP 0x10e; STRAP 0x10f;
        STRAP 0x110; STRAP 0x111; STRAP 0x112; STRAP 0x113; STRAP 0x114; STRAP 0x115; STRAP 0x116; STRAP 0x117
        STRAP 0x118; STRAP 0x119; STRAP 0x11a; STRAP 0x11b; STRAP 0x11c; STRAP 0x11d; STRAP 0x11e; STRAP 0x11f
        STRAP 0x120; STRAP 0x121; STRAP 0x122; STRAP 0x123; STRAP 0x124; STRAP 0x125; STRAP 0x126; STRAP 0x127
        STRAP 0x128; STRAP 0x129; STRAP 0x12a; STRAP 0x12b; STRAP 0x12c; STRAP 0x12d; STRAP 0x12e; STRAP 0x12f
        STRAP 0x130; STRAP 0x131; STRAP 0x132; STRAP 0x133; STRAP 0x134; STRAP 0x135; STRAP 0x136; STRAP 0x137
        STRAP 0x138; STRAP 0x139; STRAP 0x13a; STRAP 0x13b; STRAP 0x13c; STRAP 0x13d; STRAP 0x13e; STRAP 0x13f
        STRAP 0x140; STRAP 0x141; STRAP 0x142; STRAP 0x143; STRAP 0x144; STRAP 0x145; STRAP 0x146; STRAP 0x147
        STRAP 0x148; STRAP 0x149; STRAP 0x14a; STRAP 0x14b; STRAP 0x14c; STRAP 0x14d; STRAP 0x14e; STRAP 0x14f
        STRAP 0x150; STRAP 0x151; STRAP 0x152; STRAP 0x153; STRAP 0x154; STRAP 0x155; STRAP 0x156; STRAP 0x157
        STRAP 0x158; STRAP 0x159; STRAP 0x15a; STRAP 0x15b; STRAP 0x15c; STRAP 0x15d; STRAP 0x15e; STRAP 0x15f
        STRAP 0x160; STRAP 0x161; STRAP 0x162; STRAP 0x163; STRAP 0x164; STRAP 0x165; STRAP 0x166; STRAP 0x167
        STRAP 0x168; STRAP 0x169; STRAP 0x16a; STRAP 0x16b; STRAP 0x16c; STRAP 0x16d; STRAP 0x16e; STRAP 0x16f
        STRAP 0x170; STRAP 0x171; STRAP 0x172; STRAP 0x173; STRAP 0x174; STRAP 0x175; STRAP 0x176; STRAP 0x177
        STRAP 0x178; STRAP 0x179; STRAP 0x17a; STRAP 0x17b; STRAP 0x17c; STRAP 0x17d; STRAP 0x17e; STRAP 0x17f
        ! Traps beyond 0x17f are reserved
        UTRAP 0x180; UTRAP 0x181; UTRAP 0x182; UTRAP 0x183; UTRAP 0x184; UTRAP 0x185; UTRAP 0x186; UTRAP 0x187
        UTRAP 0x188; UTRAP 0x189; UTRAP 0x18a; UTRAP 0x18b; UTRAP 0x18c; UTRAP 0x18d; UTRAP 0x18e; UTRAP 0x18f
        UTRAP 0x190; UTRAP 0x191; UTRAP 0x192; UTRAP 0x193; UTRAP 0x194; UTRAP 0x195; UTRAP 0x196; UTRAP 0x197
        UTRAP 0x198; UTRAP 0x199; UTRAP 0x19a; UTRAP 0x19b; UTRAP 0x19c; UTRAP 0x19d; UTRAP 0x19e; UTRAP 0x19f
        UTRAP 0x1a0; UTRAP 0x1a1; UTRAP 0x1a2; UTRAP 0x1a3; UTRAP 0x1a4; UTRAP 0x1a5; UTRAP 0x1a6; UTRAP 0x1a7
        UTRAP 0x1a8; UTRAP 0x1a9; UTRAP 0x1aa; UTRAP 0x1ab; UTRAP 0x1ac; UTRAP 0x1ad; UTRAP 0x1ae; UTRAP 0x1af
        UTRAP 0x1b0; UTRAP 0x1b1; UTRAP 0x1b2; UTRAP 0x1b3; UTRAP 0x1b4; UTRAP 0x1b5; UTRAP 0x1b6; UTRAP 0x1b7
        UTRAP 0x1b8; UTRAP 0x1b9; UTRAP 0x1ba; UTRAP 0x1bb; UTRAP 0x1bc; UTRAP 0x1bd; UTRAP 0x1be; UTRAP 0x1bf
        UTRAP 0x1c0; UTRAP 0x1c1; UTRAP 0x1c2; UTRAP 0x1c3; UTRAP 0x1c4; UTRAP 0x1c5; UTRAP 0x1c6; UTRAP 0x1c7
        UTRAP 0x1c8; UTRAP 0x1c9; UTRAP 0x1ca; UTRAP 0x1cb; UTRAP 0x1cc; UTRAP 0x1cd; UTRAP 0x1ce; UTRAP 0x1cf
        UTRAP 0x1d0; UTRAP 0x1d1; UTRAP 0x1d2; UTRAP 0x1d3; UTRAP 0x1d4; UTRAP 0x1d5; UTRAP 0x1d6; UTRAP 0x1d7
        UTRAP 0x1d8; UTRAP 0x1d9; UTRAP 0x1da; UTRAP 0x1db; UTRAP 0x1dc; UTRAP 0x1dd; UTRAP 0x1de; UTRAP 0x1df
        UTRAP 0x1e0; UTRAP 0x1e1; UTRAP 0x1e2; UTRAP 0x1e3; UTRAP 0x1e4; UTRAP 0x1e5; UTRAP 0x1e6; UTRAP 0x1e7
        UTRAP 0x1e8; UTRAP 0x1e9; UTRAP 0x1ea; UTRAP 0x1eb; UTRAP 0x1ec; UTRAP 0x1ed; UTRAP 0x1ee; UTRAP 0x1ef
        UTRAP 0x1f0; UTRAP 0x1f1; UTRAP 0x1f2; UTRAP 0x1f3; UTRAP 0x1f4; UTRAP 0x1f5; UTRAP 0x1f6; UTRAP 0x1f7
        UTRAP 0x1f8; UTRAP 0x1f9; UTRAP 0x1fa; UTRAP 0x1fb; UTRAP 0x1fc; UTRAP 0x1fd; UTRAP 0x1fe; UTRAP 0x1ff

#ifdef SUN4V

        .align  0x8000
        .globl  trapbase_sun4v
trapbase_sun4v:
        sun4v_tl0_reserved 8                            ! 0x0-0x7
        VTRAP T_INST_EXCEPT, sun4v_tl0_itsb_miss        ! 0x8
        VTRAP T_TEXTFAULT, sun4v_tl0_itsb_miss          ! 0x9
        sun4v_tl0_reserved 6                            ! 0xa-0xf
        TRAP T_ILLINST                                  ! 0x10
        TRAP T_PRIVINST                                 ! 0x11
        sun4v_tl0_reserved 14                           ! 0x12-0x1f
        TRAP T_FPDISABLED                               ! 0x20
        TRAP T_FP_IEEE_754                              ! 0x21
        TRAP T_FP_OTHER                                 ! 0x22
        TRAP T_TAGOF                                    ! 0x23
        UCLEANWIN                                       ! 0x24-0x27
        TRAP T_DIV0                                     ! 0x28
        sun4v_tl0_reserved 7                            ! 0x29-0x2f
        VTRAP T_DATAFAULT, sun4v_datatrap               ! 0x30
        VTRAP T_DATA_MMU_MISS, sun4v_tl0_dtsb_miss      ! 0x31
        sun4v_tl0_reserved 2                            ! 0x32-0x33
        TRAP T_ALIGN                                    ! 0x34
        TRAP T_LDDF_ALIGN                               ! 0x35
        TRAP T_STDF_ALIGN                               ! 0x36
        TRAP T_PRIVACT                                  ! 0x37
        TRAP T_LDQF_ALIGN                               ! 0x38
        TRAP T_STQF_ALIGN                               ! 0x39
        sun4v_tl0_reserved 7                            ! 0x3a-0x40
        HARDINT4U 1                                     ! 0x41
        HARDINT4U 2                                     ! 0x42
        HARDINT4U 3                                     ! 0x43
        HARDINT4U 4                                     ! 0x44
        HARDINT4U 5                                     ! 0x45
        HARDINT4U 6                                     ! 0x46
        HARDINT4U 7                                     ! 0x47
        HARDINT4U 8                                     ! 0x48
        HARDINT4U 9                                     ! 0x49
        HARDINT4U 10                                    ! 0x4a
        HARDINT4U 11                                    ! 0x4b
        HARDINT4U 12                                    ! 0x4c
        HARDINT4U 13                                    ! 0x4d
        HARDINT4U 14                                    ! 0x4e
        HARDINT4U 15                                    ! 0x4f
        sun4v_tl0_reserved 18                           ! 0x50-0x61
        TRAP T_VA_WATCHPT                               ! 0x62
        sun4v_tl0_reserved 9                            ! 0x63-0x6b
        VTRAP 0x6c, sun4v_tl0_dtsb_prot                 ! 0x6c
        sun4v_tl0_reserved 15                           ! 0x6d-0x7b
        VTRAP 0x7c, sun4v_cpu_mondo                     ! 0x7c
        VTRAP 0x7c, sun4v_dev_mondo                     ! 0x7d
        TRAP 0x7e                                       ! 0x7e
        TRAP 0x7f                                       ! 0x7f
        USPILL64 uspill8v, ASI_AIUS                     ! 0x80
        SPILL32 uspill4v, ASI_AIUS                      ! 0x84
        SPILLBOTH uspill8v,uspill4v, ASI_AIUS           ! 0x88
        sun4v_tl0_unused 4                              ! 0x8c
        sun4v_tl0_unused 4                              ! 0x90
        SPILL64 kspill8v, ASI_N                         ! 0x94
        SPILL32 kspill4v, ASI_N                         ! 0x98
        SPILLBOTH kspill8v, kspill4v, ASI_N             ! 0x9c
        USPILL64 uspillk8v, ASI_AIUS                    ! 0xa0
        SPILL32 uspillk4v, ASI_AIUS                     ! 0xa4
        SPILLBOTH uspillk8v, uspillk4v, ASI_AIUS        ! 0xa8
        sun4v_tl0_unused 4                              ! 0xac
        sun4v_tl0_unused 16                             ! 0xb0-0xbc
        UFILL64 ufill8v, ASI_AIUS                       ! 0xc0
        FILL32 ufill4v, ASI_AIUS                        ! 0xc4
        FILLBOTH ufill8v, ufill4v, ASI_AIUS             ! 0xc8
        sun4v_tl0_unused 4                              ! 0xcc
        sun4v_tl0_unused 4                              ! 0xd0
        FILL64 kfill8v, ASI_N                           ! 0xd4
        FILL32 kfill4v, ASI_N                           ! 0xd8
        FILLBOTH kfill8v, kfill4v, ASI_N                ! 0xdc
        UFILL64 ufillk8v, ASI_AIUS                      ! 0xe0
        FILL32 ufillk4v, ASI_AIUS                       ! 0xe4
        FILLBOTH ufillk8v, ufillk4v, ASI_AIUS           ! 0xe8
        sun4v_tl0_unused 4                              ! 0xef
        sun4v_tl0_unused 16                             ! 0xf0-0xfc
        SYSCALL                                         ! 0x100
        TRAP T_BREAKPOINT                               ! 0x101
        sun4v_tl0_unused 6                              ! 0x102-0x107
        SYSCALL                                         ! 0x108
        SYSCALL                                         ! 0x109
        sun4v_tl0_unused 54                             ! 0x10a-0x13f
        SYSCALL                                         ! 0x140
        SYSCALL                                         ! 0x141
        SYSCALL                                         ! 0x142
        SYSCALL                                         ! 0x143
        sun4v_tl0_unused 60                             ! 0x144-0x17f
        sun4v_tl0_reserved 128                          ! 0x180-0x1ff

        sun4v_tl1_reserved 8                            ! 0x0-0x7
        TRAP T_INST_EXCEPT                              ! 0x8
        TRAP T_TEXTFAULT                                ! 0x9
        sun4v_tl1_reserved 6                            ! 0xa-0xf
        TRAP T_ILLINST                                  ! 0x10
        TRAP T_PRIVINST                                 ! 0x11
        sun4v_tl1_reserved 14                           ! 0x12-0x1f
        TRAP T_FPDISABLED                               ! 0x20
        TRAP T_FP_IEEE_754                              ! 0x21
        TRAP T_FP_OTHER                                 ! 0x22
        TRAP T_TAGOF                                    ! 0x23
        KCLEANWIN                                       ! 0x24-0x27
        TRAP T_DIV0                                     ! 0x28
        sun4v_tl1_reserved 7                            ! 0x29-0x2f
        VTRAP T_DATAFAULT, sun4v_tl1_ptbl_miss          ! 0x30
        VTRAP T_DATA_MMU_MISS, sun4v_tl1_dtsb_miss      ! 0x31
        VTRAP T_DATA_ERROR, sun4v_tl1_ptbl_miss
        VTRAP T_DATA_PROT, sun4v_tl1_ptbl_miss
!       sun4v_tl1_reserved 2                            ! 0x32-0x33
        VTRAP T_ALIGN, sun4v_tl1_ptbl_miss              ! 0x34
        TRAP T_LDDF_ALIGN                               ! 0x35
        TRAP T_STDF_ALIGN                               ! 0x36
        TRAP T_PRIVACT                                  ! 0x37
        TRAP T_LDQF_ALIGN                               ! 0x38
        TRAP T_STQF_ALIGN                               ! 0x39
        sun4v_tl1_reserved 40                           ! 0x3a-0x61
        TRAP T_VA_WATCHPT                               ! 0x62
        sun4v_tl1_reserved 9                            ! 0x63-0x6b
        VTRAP 0x6c, sun4v_tl1_dtsb_prot                 ! 0x6c
        sun4v_tl1_reserved 19                           ! 0x6d-0x7f
        sun4v_tl1_uspill_normal                         ! 0x80
        sun4v_tl1_uspill_normal                         ! 0x84
        sun4v_tl1_uspill_normal                         ! 0x88
        sun4v_tl1_unused 4                              ! 0x8c
        sun4v_tl1_unused 4                              ! 0x90
        sun4v_tl1_kspill_normal                         ! 0x94
        sun4v_tl1_kspill_normal                         ! 0x98
        sun4v_tl1_kspill_normal                         ! 0x9c
        sun4v_tl1_uspill_other                          ! 0xa0
        sun4v_tl1_uspill_other                          ! 0xa4
        sun4v_tl1_uspill_other                          ! 0xa8
        sun4v_tl1_unused 4                              ! 0xac
        sun4v_tl1_unused 16                             ! 0xb0-0xbc
        sun4v_tl1_unused 64                             ! 0xc0-0xfc
        sun4v_tl1_unused 128
        sun4v_tl1_reserved 128

#endif

/*
 * v9 machines do not have a trap window.
 *
 * When we take a trap the trap state is pushed on to the stack of trap
 * registers, interrupts are disabled, then we switch to an alternate set
 * of global registers.
 *
 * The trap handling code needs to allocate a trap frame on the kernel, or
 * for interrupts, the interrupt stack, save the out registers to the trap
 * frame, then switch to the normal globals and save them to the trap frame
 * too.
 *
 * XXX it would be good to save the interrupt stack frame to the kernel
 * stack so we wouldn't have to copy it later if we needed to handle a AST.
 *
 * Since kernel stacks are all on one page and the interrupt stack is entirely
 * within the locked TLB, we can use physical addressing to save out our
 * trap frame so we don't trap during the TRAP_SETUP operation.  There
 * is unfortunately no supportable method for issuing a non-trapping save.
 *
 * However, if we use physical addresses to save our trapframe, we will need
 * to clear out the data cache before continuing much further.
 *
 * In short, what we need to do is:
 *
 *      all preliminary processing is done using the alternate globals
 *
 *      When we allocate our trap windows we must give up our globals because
 *      their state may have changed during the save operation
 *
 *      we need to save our normal globals as soon as we have a stack
 *
 * Finally, we may now call C code.
 *
 * This macro will destroy %g5-%g7.  %g0-%g4 remain unchanged.
 */

 /*
  * Other misc. design criteria:
  *
  * When taking an address fault, fault info is in the sfsr, sfar,
  * TLB_TAG_ACCESS registers.  If we take another address fault
  * while trying to handle the first fault then that information,
  * the only information that tells us what address we trapped on,
  * can potentially be lost.  This trap can be caused when allocating
  * a register window with which to handle the trap because the save
  * may try to store or restore a register window that corresponds
  * to part of the stack that is not mapped.  Preventing this trap,
  * while possible, is much too complicated to do in a trap handler,
  * and then we will need to do just as much work to restore the processor
  * window state.
  *
  * Possible solutions to the problem:
  *
  * Since we have separate AG, MG, and IG, we could have all traps
  * above level-1 preserve AG and use other registers.  This causes
  * a problem for the return from trap code which is coded to use
  * alternate globals only.
  *
  * We could store the trapframe and trap address info to the stack
  * using physical addresses.  Then we need to read it back using
  * physical addressing, or flush the D$.
  *
  * We could identify certain registers to hold address fault info.
  * This means that these registers need to be preserved across all
  * fault handling.  But since we only have 7 useable globals, that
  * really puts a cramp in our style.
  *
  * Finally, there is the issue of returning from kernel mode to user
  * mode.  If we need to issue a restore of a user window in kernel
  * mode, we need the window control registers in a user mode setup.
  * If the trap handlers notice the register windows are in user mode,
  * they will allocate a trapframe at the bottom of the kernel stack,
  * overwriting the frame we were trying to return to.  This means that
  * we must complete the restoration of all registers *before* switching
  * to a user-mode window configuration.
  *
  * Essentially we need to be able to write re-entrant code w/no stack.
  */
        .text

        .macro  TRAP_SETUP stackspace
        GET_CPCB(%g6)
        sethi   %hi((\stackspace)), %g5
        set     USPACE-BIAS, %g7
        or      %g5, %lo((\stackspace)), %g5

        sra     %g5, 0, %g5                     ! Sign extend the damn thing

        add     %g6, %g7, %g6
        rdpr    %wstate, %g7                    ! Find if we're from user mode

        sub     %g7, WSTATE_KERN, %g7           ! Compare & leave in register
        movrz   %g7, %sp, %g6                   ! Select old (kernel) stack or base of kernel stack
        add     %g6, %g5, %g6                   ! Allocate a stack frame
        SPILL stx, %g6 + CC64FSZ + BIAS + TF_L, 8, ! save local + in
        save    %g6, 0, %sp                     ! If we fault we should come right back here
        stx     %i0, [%sp + CC64FSZ + BIAS + TF_O + (0*8)] ! Save out registers to trap frame
        stx     %i1, [%sp + CC64FSZ + BIAS + TF_O + (1*8)]
        stx     %i2, [%sp + CC64FSZ + BIAS + TF_O + (2*8)]
        stx     %i3, [%sp + CC64FSZ + BIAS + TF_O + (3*8)]
        stx     %i4, [%sp + CC64FSZ + BIAS + TF_O + (4*8)]
        stx     %i5, [%sp + CC64FSZ + BIAS + TF_O + (5*8)]

        stx     %i6, [%sp + CC64FSZ + BIAS + TF_O + (6*8)]
        brz,pt  %g7, 1f                 ! If we were in kernel mode start saving globals
         stx    %i7, [%sp + CC64FSZ + BIAS + TF_O + (7*8)]

        ! came from user mode -- switch to kernel mode stack
        rdpr    %canrestore, %g5                ! Fixup register window state registers
        wrpr    %g0, 0, %canrestore
        wrpr    %g0, %g5, %otherwin
        wrpr    %g0, WSTATE_KERN, %wstate       ! Enable kernel mode window traps -- now we can trap again

        mov     CTX_PRIMARY, %g7
        SET_MMU_CONTEXTID(%g0, %g7)             ! Switch MMU to kernel primary context
        sethi   %hi(KERNBASE), %g5
        membar  #Sync                           ! XXXX Should be taken care of by flush
        flush   %g5                             ! Some convenient address that won't trap
1:
        .endm
        
/*
 * Interrupt setup is almost exactly like trap setup, but we need to
 * go to the interrupt stack if (a) we came from user mode or (b) we
 * came from kernel mode on the kernel stack.
 *
 * We don't guarantee that any registers are preserved during this operation,
 * so we can be more efficient.
 */
        .macro  INTR_SETUP stackspace
        rdpr    %wstate, %g7                    ! Find if we're from user mode

        GET_INTSTACK(%g6)
        sethi   %hi(EINTSTACK-INTSTACK), %g4
        sub     %g6, BIAS, %g6                  ! Base of interrupt stack
        dec     %g4                             ! Make it into a mask

        sub     %g6, %sp, %g1                   ! Offset from interrupt stack
        sethi   %hi((\stackspace)), %g5

        or      %g5, %lo((\stackspace)), %g5

        andn    %g1, %g4, %g4                   ! Are we out of the interrupt stack range?
        xor     %g7, WSTATE_KERN, %g3

        sra     %g5, 0, %g5                     ! Sign extend the damn thing
        or      %g3, %g4, %g4                   ! Definitely not off the interrupt stack

        movrz   %g4, %sp, %g6

        add     %g6, %g5, %g6                   ! Allocate a stack frame
        SPILL stx, %g6 + CC64FSZ + BIAS + TF_L, 8,  ! save local+in to trap frame
        save    %g6, 0, %sp                     ! If we fault we should come right back here
        stx     %i0, [%sp + CC64FSZ + BIAS + TF_O + (0*8)] ! Save out registers to trap frame
        stx     %i1, [%sp + CC64FSZ + BIAS + TF_O + (1*8)]
        stx     %i2, [%sp + CC64FSZ + BIAS + TF_O + (2*8)]
        stx     %i3, [%sp + CC64FSZ + BIAS + TF_O + (3*8)]
        stx     %i4, [%sp + CC64FSZ + BIAS + TF_O + (4*8)]

        stx     %i5, [%sp + CC64FSZ + BIAS + TF_O + (5*8)]
        stx     %i6, [%sp + CC64FSZ + BIAS + TF_O + (6*8)]
        brz,pt  %g3, 1f                         ! If we were in kernel mode start saving globals
         stx    %i7, [%sp + CC64FSZ + BIAS + TF_O + (7*8)]

        ! came from user mode -- switch to kernel mode stack
        rdpr    %canrestore, %g5                ! Fixup register window state registers
        wrpr    %g0, 0, %canrestore
        wrpr    %g0, %g5, %otherwin
        wrpr    %g0, WSTATE_KERN, %wstate       ! Enable kernel mode window traps -- now we can trap again

        mov     CTX_PRIMARY, %g7
        SET_MMU_CONTEXTID(%g0, %g7)             ! Switch MMU to kernel primary context
        sethi   %hi(KERNBASE), %g5
        membar  #Sync                           ! XXXX Should be taken care of by flush
        flush   %g5                             ! Some convenient address that won't trap
1:
        .endm

/*
 * Perform an inline pseg_get(), to retrieve the address of the PTE associated
 * to the given virtual address.
 * On entry: %g3 = va (won't be modified)
 * Registers used: %g4, %g5, %g6
 * Branches to the "failure" label if translation invalid, otherwise ends
 * with the pte address in %g6.
 */
        .macro  PTE_GET failure
        sethi   %hi(0x1fff), %g6                ! 8K context mask
        sethi   %hi(ctxbusy), %g4
        or      %g6, %lo(0x1fff), %g6
        ldx     [%g4 + %lo(ctxbusy)], %g4
        srax    %g3, HOLESHIFT, %g5             ! Check for valid address
        and     %g3, %g6, %g6                   ! Isolate context
        inc     %g5                             ! (0 or -1) -> (1 or 0)
        sllx    %g6, 3, %g6                     ! Make it into an offset into ctxbusy
        ldx     [%g4+%g6], %g4                  ! Load up our page table.
        srlx    %g3, STSHIFT, %g6
        cmp     %g5, 1
        bgu,pn %xcc, \failure                   ! Error!
         srlx   %g3, PDSHIFT, %g5
        and     %g6, STMASK, %g6
        sll     %g6, 3, %g6
        and     %g5, PDMASK, %g5
        sll     %g5, 3, %g5
        add     %g6, %g4, %g4
        ldxa    [%g4] ASI_PHYS_CACHED, %g4
        srlx    %g3, PTSHIFT, %g6               ! Convert to ptab offset
        add     %g5, %g4, %g5
        brz,pn  %g4, \failure                   ! NULL entry? check somewhere else
         and    %g6, PTMASK, %g6
        ldxa    [%g5] ASI_PHYS_CACHED, %g4
        sll     %g6, 3, %g6
        brz,pn  %g4, \failure                   ! NULL entry? check somewhere else
         add    %g6, %g4, %g6
        .endm

/*
 * Make sure the TSB get locked for MULTIPROCESSOR kernels.
 * On entry: %g2 = TSB pointer
 * Registers used: %g5, %g6
 */
        .macro  LOCK_TSB
#ifdef MULTIPROCESSOR
97:
        ld      [%g2], %g6
        btst    (TSB_TAG_LOCKED >> 32), %g6
        bnz,pn  %icc, 97b                       ! Wait until bit is clear
         or     %g6, (TSB_TAG_LOCKED >> 32), %g5
        casa    [%g2] ASI_NUCLEUS, %g6, %g5
        cmp     %g6, %g5
        bne,pn  %icc, 97b                       ! Wait until we can set it
         nop
        membar  #StoreStore
#endif
        .endm

/*
 * This is the MMU protection handler.  It's too big to fit
 * in the trap table so I moved it here.  It's relatively simple.
 * It looks up the page mapping in the page table associated with
 * the trapping context.  It checks to see if the S/W writable bit
 * is set.  If so, it sets the H/W write bit, marks the tte modified,
 * and enters the mapping into the MMU.  Otherwise it does a regular
 * data fault.
 */
        ICACHE_ALIGN
dmmu_write_fault:
        mov     TLB_TAG_ACCESS, %g3
        ldxa    [%g3] ASI_DMMU, %g3                     ! Get fault addr from Tag Target
        PTE_GET winfix
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, winfix                            ! Entry invalid?  Punt
         or     %g4, SUN4U_TLB_MODIFY|SUN4U_TLB_ACCESS|SUN4U_TLB_W, %g7
                ! Update the modified bit
        
        btst    SUN4U_TLB_REAL_W|SUN4U_TLB_W, %g4       ! Is it a ref fault?
        bz,pn   %xcc, winfix                            ! No -- really fault
        /* Need to check for and handle large pages. */
         srlx   %g4, 61, %g5                            ! Isolate the size bits
        ldxa    [%g0] ASI_DMMU_8KPTR, %g2               ! Load DMMU 8K TSB pointer
        andcc   %g5, 0x3, %g5                           ! 8K?
        bnz,pn  %icc, winfix                            ! We punt to the pmap code since we can't handle policy
         ldxa   [%g0] ASI_DMMU, %g1                     ! Hard coded for unified 8K TSB         Load DMMU tag target register
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        
        membar  #StoreLoad
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4U_TLB_MODIFY|SUN4U_TLB_ACCESS|SUN4U_TLB_W, %g4
                ! Update the modified bit

        LOCK_TSB
        stx     %g4, [%g2 + 8]                          ! Update TSB entry data
        mov     SFSR, %g7
        stx     %g1, [%g2]                              ! Update TSB entry tag
        nop
        mov     DEMAP_PAGE_SECONDARY, %g1               ! Secondary flush
        mov     DEMAP_PAGE_NUCLEUS, %g5                 ! Nucleus flush
        stxa    %g0, [%g7] ASI_DMMU                     ! clear out the fault
        membar  #Sync
        sllx    %g3, (64-13), %g7                       ! Need to demap old entry first
        andn    %g3, 0xfff, %g6
        movrz   %g7, %g5, %g1                           ! Pick one
        or      %g6, %g1, %g6
        stxa    %g6, [%g6] ASI_DMMU_DEMAP               ! Do the demap
        membar  #Sync                                   ! No real reason for this XXXX
        
        stxa    %g4, [%g0] ASI_DMMU_DATA_IN             ! Enter new mapping
        membar  #Sync
        retry

/*
 * Each memory data access fault from a fast access miss handler comes here.
 * We will quickly check if this is an original prom mapping before going
 * to the generic fault handler
 *
 * We will assume that %pil is not lost so we won't bother to save it
 * unless we're in an interrupt handler.
 *
 * On entry:
 *      We are on one of the alternate set of globals
 *      %g1 = MMU tag target
 *      %g2 = 8Kptr
 *      %g3 = TLB TAG ACCESS
 *
 * On return:
 *
 */
        ICACHE_ALIGN
data_miss:
        mov     TLB_TAG_ACCESS, %g3                     ! Get real fault page
        ldxa    [%g3] ASI_DMMU, %g3                     ! from tag access register
        PTE_GET data_nfo
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, data_nfo                          ! Entry invalid?  Punt
         nop

        btst    SUN4U_TLB_EXEC_ONLY, %g4                ! no read/write allowed?
        bne,pn  %xcc, data_nfo                          ! bail
         nop
        
        btst    SUN4U_TLB_ACCESS, %g4                   ! Need to update access git?
        bne,pt  %xcc, 1f
         or     %g4, SUN4U_TLB_ACCESS, %g7              ! Update the access bit
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4U_TLB_ACCESS, %g4              ! Update the modified bit
1:
        LOCK_TSB
        stx     %g4, [%g2 + 8]                          ! Update TSB entry data
        stx     %g1, [%g2]                              ! Update TSB entry tag

        stxa    %g4, [%g0] ASI_DMMU_DATA_IN             ! Enter new mapping
        membar  #Sync
        CLRTT
        retry
        NOTREACHED
/*
 * We had a data miss but did not find a mapping.  Insert
 * a NFO mapping to satisfy speculative loads and return.
 * If this had been a real load, it will re-execute and
 * result in a data fault or protection fault rather than
 * a TLB miss.  We insert an 8K TTE with the valid and NFO
 * bits set.  All others should zero.  The TTE looks like this:
 *
 *      0x9000000000000000
 *
 */
data_nfo:
        sethi   %hi(0x90000000), %g4                    ! V(0x8)|NFO(0x1)
        sllx    %g4, 32, %g4
        stxa    %g4, [%g0] ASI_DMMU_DATA_IN             ! Enter new mapping
        membar  #Sync
        CLRTT
        retry   

/*
 * Handler for making the trap window shiny clean.
 *
 * If the store that trapped was to a kernel address, panic.
 *
 * If the store that trapped was to a user address, stick it in the PCB.
 * Since we don't want to force user code to use the standard register
 * convention if we don't have to, we will not assume that %fp points to
 * anything valid.
 *
 * On entry:
 *      We are on one of the alternate set of globals
 *      %g1 = %tl - 1, tstate[tl-1], scratch    - local
 *      %g2 = %tl                               - local
 *      %g3 = MMU tag access                    - in
 *      %g4 = %cwp                              - local
 *      %g5 = scratch                           - local
 *      %g6 = cpcb                              - local
 *      %g7 = scratch                           - local
 *
 * On return:
 *
 * NB:   remove most of this from main codepath & cleanup I$
 */
winfault:
        mov     TLB_TAG_ACCESS, %g3     ! Get real fault page from tag access register
        ldxa    [%g3] ASI_DMMU, %g3     ! And put it into the non-MMU alternate regs
winfix:
        rdpr    %tl, %g2
        subcc   %g2, 1, %g1
        brlez,pt        %g1, datafault  ! Don't go below trap level 1
         nop

        wrpr    %g1, 0, %tl             ! Pop a trap level
        rdpr    %tt, %g7                ! Read type of prev. trap
        rdpr    %tstate, %g4            ! Try to restore prev %cwp if we were executing a restore
        andn    %g7, 0x3f, %g5          !   window fill traps are all 0b 0000 11xx xxxx

#if 1
        cmp     %g7, 0x68               ! If we took a datafault just before this trap
        bne,pt  %icc, winfixfill        ! our stack's probably bad so we need to switch somewhere else
         nop

        !!
        !! Double data fault -- bad stack?
        !!
        wrpr    %g2, %tl        ! Restore trap level.
        sir                     ! Just issue a reset and don't try to recover.
        mov     %fp, %l6                ! Save the frame pointer
        set     EINTSTACK+USPACE+CC64FSZ-BIAS, %fp ! Set the frame pointer to the middle of the idle stack
        add     %fp, -CC64FSZ, %sp      ! Create a stackframe
        wrpr    %g0, 15, %pil           ! Disable interrupts, too
        wrpr    %g0, %g0, %canrestore   ! Our stack is hozed and our PCB
        wrpr    %g0, 7, %cansave        !  probably is too, so blow away
        ba      slowtrap                !  all our register windows.
         wrpr   %g0, 0x101, %tt
#endif  /* 1 */

winfixfill:
        cmp     %g5, 0x0c0              !   so we mask lower bits & compare to 0b 0000 1100 0000
        bne,pt  %icc, winfixspill       ! Dump our trap frame -- we will retry the fill when the page is loaded
         cmp    %g5, 0x080              !   window spill traps are all 0b 0000 10xx xxxx

        !!
        !! This was a fill
        !!
        btst    TSTATE_PRIV, %g4        ! User mode?
        and     %g4, CWP, %g5           ! %g4 = %cwp of trap
        wrpr    %g7, 0, %tt
        bz,a,pt %icc, datafault         ! We were in user mode -- normal fault
         wrpr   %g5, %cwp               ! Restore cwp from before fill trap -- regs should now be consistent

        /*
         * We're in a pickle here.  We were trying to return to user mode
         * and the restore of the user window failed, so now we have one valid
         * kernel window and a user window state.  If we do a TRAP_SETUP now,
         * our kernel window will be considered a user window and cause a
         * fault when we try to save it later due to an invalid user address.
         * If we return to where we faulted, our window state will not be valid
         * and we will fault trying to enter user with our primary context of zero.
         *
         * What we'll do is arrange to have us return to return_from_trap so we will
         * start the whole business over again.  But first, switch to a kernel window
         * setup.  Let's see, canrestore and otherwin are zero.  Set WSTATE_KERN and
         * make sure we're in kernel context and we're done.
         */

        wrpr    %g2, %g0, %tl                           ! Restore trap level
        cmp     %g2, 3
        tne     %icc, 1
        rdpr    %tt, %g5
        wrpr    %g0, 1, %tl                             ! Revert to TL==1 XXX what if this wasn't in rft_user? Oh well.
        wrpr    %g5, %g0, %tt                           ! Set trap type correctly
/*
 * Here we need to implement the beginning of datafault.
 * TRAP_SETUP expects to come from either kernel mode or
 * user mode with at least one valid register window.  It
 * will allocate a trap frame, save the out registers, and
 * fix the window registers to think we have one user
 * register window.
 *
 * However, under these circumstances we don't have any
 * valid register windows, so we need to clean up the window
 * registers to prevent garbage from being saved to either
 * the user stack or the PCB before calling the datafault
 * handler.
 *
 * We could simply jump to datafault if we could somehow
 * make the handler issue a `saved' instruction immediately
 * after creating the trapframe.
 *
 * The following is duplicated from datafault:
 */
        wrpr    %g0, PSTATE_KERN|PSTATE_AG, %pstate     ! We need to save volatile stuff to AG regs
        wr      %g0, ASI_DMMU, %asi                     ! We need to re-load trap info
        ldxa    [%g0 + TLB_TAG_ACCESS] %asi, %g1        ! Get fault address from tag access register
        ldxa    [SFAR] %asi, %g2                        ! sync virt addr; must be read first
        ldxa    [SFSR] %asi, %g3                        ! get sync fault status register
        stxa    %g0, [SFSR] %asi                        ! Clear out fault now
        membar  #Sync                                   ! No real reason for this XXXX

        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF
        saved                                           ! Blow away that one register window we didn't ever use.
        ba,a,pt %icc, Ldatafault_internal               ! Now we should return directly to user mode
         nop

winfixspill:
        bne,a,pt        %xcc, datafault                         ! Was not a spill -- handle it normally
         wrpr   %g2, 0, %tl                             ! Restore trap level for now XXXX

        !!
        !! This was a spill
        !!
#if 1
        btst    TSTATE_PRIV, %g4                        ! From user mode?
!       cmp     %g2, 2                                  ! From normal execution? take a fault.
        wrpr    %g2, 0, %tl                             ! We need to load the fault type so we can
        rdpr    %tt, %g5                                ! overwrite the lower trap and get it to the fault handler
        wrpr    %g1, 0, %tl
        wrpr    %g5, 0, %tt                             ! Copy over trap type for the fault handler
        and     %g4, CWP, %g5                           ! find %cwp from trap

        be,a,pt %xcc, datafault                         ! Let's do a regular datafault.  When we try a save in datafault we'll
         wrpr   %g5, 0, %cwp                            !  return here and write out all dirty windows.
#endif  /* 1 */
        wrpr    %g2, 0, %tl                             ! Restore trap level for now XXXX
        GET_CPUINFO_VA(%g6)
        ldx     [%g6 + CI_CPCBPADDR], %g6
        wr      %g0, ASI_PHYS_CACHED, %asi              ! Use ASI_PHYS_CACHED to prevent possible page faults
        /*
         * Now save all user windows to cpcb.
         */
        rdpr    %otherwin, %g7
        brnz,pt %g7, 1f
         rdpr   %canrestore, %g5
        rdpr    %cansave, %g1
        add     %g5, 1, %g7                             ! add the %cwp window to the list to save
!       movrnz  %g1, %g5, %g7                           ! If we're issuing a save
!       mov     %g5, %g7                                ! DEBUG
        wrpr    %g0, 0, %canrestore
        wrpr    %g7, 0, %otherwin                       ! Still in user mode -- need to switch to kernel mode
1:
        mov     %g7, %g1
        add     %g6, PCB_NSAVED, %g7
        lduba   [%g6 + PCB_NSAVED] %asi, %g7            ! Start incrementing pcb_nsaved

#ifdef DEBUG
        wrpr    %g0, 5, %tl
#endif  /* DEBUG */
        mov     %g6, %g5
        brz,pt  %g7, winfixsave                         ! If it's in use, panic
         saved                                          ! frob window registers

        /* PANIC */
#ifdef DEBUG
        wrpr    %g2, 0, %tl
#endif  /* DEBUG */
        mov     %g7, %o2
        rdpr    %ver, %o1
        sethi   %hi(2f), %o0
        and     %o1, CWP, %o1
        wrpr    %g0, %o1, %cleanwin
        dec     1, %o1
        wrpr    %g0, %o1, %cansave                      ! kludge away any more window problems
        wrpr    %g0, 0, %canrestore
        wrpr    %g0, 0, %otherwin
        or      %lo(2f), %o0, %o0
        wrpr    %g0, WSTATE_KERN, %wstate
#ifdef DEBUG
        set     panicstack-CC64FSZ-BIAS, %sp            ! Use panic stack.
#else   /* DEBUG */
        set     estack0, %sp
        ldx     [%sp], %sp
        add     %sp, -CC64FSZ-BIAS, %sp                 ! Overwrite proc 0's stack.
#endif  /* DEBUG */
        ta      1; nop                                  ! This helps out traptrace.
        call    panic                                   ! This needs to be fixed properly but we should panic here
         mov    %g1, %o1
        NOTREACHED
        .data
2:
        .asciz  "winfault: double invalid window at %p, nsaved=%d"
        _ALIGN
        .text
3:
        saved
        save
winfixsave:
        sllx    %g7, 7, %g5
        add     %g6, %g5, %g5
        SPILL   stxa, %g5 + PCB_RW, 8, %asi     ! Save the window in the pcb

        sllx    %g7, 3, %g5
        add     %g6, %g5, %g5
        stxa    %sp, [%g5 + PCB_RWSP] %asi

!       rdpr    %otherwin, %g1  ! Check to see if we's done
        dec     %g1
        wrpr    %g0, 7, %cleanwin                       ! BUGBUG -- we should not hardcode this, but I have no spare globals
        brnz,pt %g1, 3b
         inc    %g7                                     ! inc pcb_nsaved

        /* fix up pcb fields */
        stba    %g7, [%g6 + PCB_NSAVED] %asi            ! cpcb->pcb_nsaved = n
        /*
         * We just issued a bunch of saves, so %cansave is now 0,
         * probably (if we were doing a flushw then we may have
         * come in with only partially full register windows and
         * it may not be 0).
         *
         * %g7 contains the count of the windows we just finished
         * saving.
         *
         * What we need to do now is move some of the windows from
         * %canrestore to %cansave.  What we should do is take
         * min(%canrestore, %g7) and move that over to %cansave.
         *
         * %g7 is the number of windows we flushed, so we should
         * use that as a base.  Clear out %otherwin, set %cansave
         * to min(%g7, NWINDOWS - 2), set %cleanwin to %canrestore
         * + %cansave and the rest follows:
         *
         * %otherwin = 0
         * %cansave = NWINDOWS - 2 - %canrestore
         */
        wrpr    %g0, 0, %otherwin
        rdpr    %canrestore, %g1
        sub     %g1, %g7, %g1                           ! Calculate %canrestore - %g7
        movrlz  %g1, %g0, %g1                           ! Clamp at zero
        wrpr    %g1, 0, %canrestore                     ! This is the new canrestore
        rdpr    %ver, %g5
        and     %g5, CWP, %g5                           ! NWINDOWS-1
        dec     %g5                                     ! NWINDOWS-2
        wrpr    %g5, 0, %cleanwin                       ! Set cleanwin to max, since we're in-kernel
        sub     %g5, %g1, %g5                           ! NWINDOWS-2-%canrestore
        wrpr    %g5, 0, %cansave

!       rdpr    %tl, %g2                                ! DEBUG DEBUG -- did we trap somewhere?
        sub     %g2, 1, %g1
        rdpr    %tt, %g2
        wrpr    %g1, 0, %tl                             ! We will not attempt to re-execute the spill, so dump our trap frame permanently
        wrpr    %g2, 0, %tt                             ! Move trap type from fault frame here, overwriting spill

        /* Did we save a user or kernel window ? */
!       srax    %g3, 48, %g7                            ! User or kernel store? (TAG TARGET)
        sllx    %g3, (64-13), %g7                       ! User or kernel store? (TAG ACCESS)
        sethi   %hi((2*NBPG)-8), %g7
        brnz,pt %g7, 1f                                 ! User fault -- save windows to pcb
         or     %g7, %lo((2*NBPG)-8), %g7

        and     %g4, CWP, %g4                           ! %g4 = %cwp of trap
        wrpr    %g4, 0, %cwp                            ! Kernel fault -- restore %cwp and force and trap to debugger
        !!
        !! Here we managed to fault trying to access a kernel window
        !! This is a bug.  Switch to the interrupt stack if we aren't
        !! there already and then trap into the debugger or panic.
        !!
        sethi   %hi(EINTSTACK-BIAS), %g6
        btst    1, %sp
        bnz,pt  %icc, 0f
         mov    %sp, %g1
        add     %sp, -BIAS, %g1
0:
        or      %g6, %lo(EINTSTACK-BIAS), %g6
        set     (EINTSTACK-INTSTACK), %g7       ! XXXXXXXXXX This assumes kernel addresses are unique from user addresses
        sub     %g6, %g1, %g2                           ! Determine if we need to switch to intr stack or not
        dec     %g7                                     ! Make it into a mask
        andncc  %g2, %g7, %g0                           ! XXXXXXXXXX This assumes kernel addresses are unique from user addresses */ \
        movz    %xcc, %g1, %g6                          ! Stay on interrupt stack?
        add     %g6, -CC64FSZ, %g6                      ! Allocate a stack frame
        mov     %sp, %l6                                ! XXXXX Save old stack pointer
        mov     %g6, %sp
        ta      1; nop                                  ! Enter debugger
        NOTREACHED
1:
#if 1
        /* Now we need to blast away the D$ to make sure we're in sync */
dlflush1:
        stxa    %g0, [%g7] ASI_DCACHE_TAG
        brnz,pt %g7, 1b
         dec    8, %g7
#endif  /* 1 */

        /*
         * If we had WSTATE_KERN then we had at least one valid kernel window.
         * We should re-execute the trapping save.
         */
        rdpr    %wstate, %g3
        mov     %g3, %g3
        cmp     %g3, WSTATE_KERN
        bne,pt  %icc, 1f
         nop
        retry                                           ! Now we can complete the save
1:
        /*
         * Since we had a WSTATE_USER, we had no valid kernel windows.  This should
         * only happen inside TRAP_SETUP or INTR_SETUP. Emulate
         * the instruction, clean up the register windows, then done.
         */
        rdpr    %cwp, %g1
        inc     %g1
        rdpr    %tstate, %g2
        wrpr    %g1, %cwp
        andn    %g2, CWP, %g2
        wrpr    %g1, %g2, %tstate
        wrpr    %g0, PSTATE_KERN|PSTATE_AG, %pstate
        mov     %g6, %sp
        done

/*
 * Each memory data access fault, from user or kernel mode,
 * comes here.
 *
 * We will assume that %pil is not lost so we won't bother to save it
 * unless we're in an interrupt handler.
 *
 * On entry:
 *      We are on one of the alternate set of globals
 *      %g1 = MMU tag target
 *      %g2 = %tl
 *
 * On return:
 *
 */
datafault:
        wrpr    %g0, PSTATE_KERN|PSTATE_AG, %pstate     ! We need to save volatile stuff to AG regs
        wr      %g0, ASI_DMMU, %asi                     ! We need to re-load trap info
        ldxa    [%g0 + TLB_TAG_ACCESS] %asi, %g1        ! Get fault address from tag access register
        ldxa    [SFAR] %asi, %g2                        ! sync virt addr; must be read first
        ldxa    [SFSR] %asi, %g3                        ! get sync fault status register
        stxa    %g0, [SFSR] %asi                        ! Clear out fault now
        membar  #Sync                                   ! No real reason for this XXXX

        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF
Ldatafault_internal:
        mov     %g1, %o0                                ! Move these to the out regs so we can save the globals
        mov     %g2, %o4
        mov     %g3, %o5

        ldxa    [%g0] ASI_AFAR, %o2                     ! get async fault address
        ldxa    [%g0] ASI_AFSR, %o3                     ! get async fault status
        mov     -1, %g7
        stxa    %g7, [%g0] ASI_AFSR                     ! And clear this out, too
        membar  #Sync                                   ! No real reason for this XXXX

        wrpr    %g0, PSTATE_KERN, %pstate               ! Get back to normal globals

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]      ! save g1
        rdpr    %tt, %o1                                ! find out what trap brought us here
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]      ! save g2
        rdpr    %tstate, %g1
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]      ! (sneak g3 in here)
        rdpr    %tpc, %g2
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]      ! sneak in g4
        rdpr    %tnpc, %g3
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]      ! sneak in g5
        rd      %y, %g4                                 ! save y
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]      ! sneak in g6
        mov     %g2, %o7                                ! Make the fault address look like the return address
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]      ! sneak in g7

        sth     %o1, [%sp + CC64FSZ + BIAS + TF_TT]
        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]         ! set tf.tf_psr, tf.tf_pc
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_PC]             ! set tf.tf_npc
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_NPC]

        rdpr    %pil, %g5
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_PIL]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]

#if 1
        rdpr    %tl, %g7
        dec     %g7
        movrlz  %g7, %g0, %g7
        wrpr    %g0, %g7, %tl           ! Revert to kernel mode
#else   /* 1 */
        wrpr    %g0, 0, %tl             ! Revert to kernel mode
#endif  /* 1 */
        /* Finish stackframe, call C trap handler */
        flushw                                          ! Get this clean so we won't take any more user faults

        GET_CPUINFO_VA(%g7)

        /*
         * Right now the registers have the following values:
         *
         *      %o0 -- MMU_TAG_ACCESS
         *      %o1 -- TT
         *      %o2 -- afar
         *      %o3 -- afsr
         *      %o4 -- sfar
         *      %o5 -- sfsr
         */
        
        cmp     %o1, T_DATA_ERROR
        st      %g4, [%sp + CC64FSZ + BIAS + TF_Y]
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi  ! Restore default ASI
        be,pn   %icc, data_error
         wrpr   %g0, PSTATE_INTR, %pstate       ! reenable interrupts

        mov     %o0, %o3                        ! (argument: trap address)
        mov     %g2, %o2                        ! (argument: trap pc)
        call    data_access_fault               ! data_access_fault(&tf, type, 
                                                !       pc, addr, sfva, sfsr)
         add    %sp, CC64FSZ + BIAS, %o0        ! (argument: &tf)

data_recover:
        wrpr    %g0, PSTATE_KERN, %pstate               ! disable interrupts
        b       return_from_trap                        ! go return
         ldx    [%sp + CC64FSZ + BIAS + TF_TSTATE], %g1         ! Load this for return_from_trap
        NOTREACHED

data_error:
        call    data_access_error               ! data_access_error(&tf, type, 
                                                !       afva, afsr, sfva, sfsr)
         add    %sp, CC64FSZ + BIAS, %o0        ! (argument: &tf)
        ba      data_recover
         nop
        NOTREACHED

/*
 * Each memory instruction access fault from a fast access handler comes here.
 * We will quickly check if this is an original prom mapping before going
 * to the generic fault handler
 *
 * We will assume that %pil is not lost so we won't bother to save it
 * unless we're in an interrupt handler.
 *
 * On entry:
 *      We are on one of the alternate set of globals
 *      %g1 = MMU tag target
 *      %g2 = TSB entry ptr
 *      %g3 = TLB Tag Access
 *
 * On return:
 *
 */

        ICACHE_ALIGN
instr_miss:
        mov     TLB_TAG_ACCESS, %g3                     ! Get real fault page
        ldxa    [%g3] ASI_IMMU, %g3                     ! from tag access register
        PTE_GET textfault
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, textfault
         nop

        /* Check if it's an executable mapping. */
        andcc   %g4, SUN4U_TLB_EXEC, %g0
        bz,pn   %xcc, textfault
         nop

        or      %g4, SUN4U_TLB_ACCESS, %g7              ! Update accessed bit
        btst    SUN4U_TLB_ACCESS, %g4                   ! Need to update access bit?
        bne,pt  %xcc, 1f
         nop
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and store it
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4U_TLB_ACCESS, %g4              ! Update accessed bit
1:
        LOCK_TSB
        stx     %g4, [%g2 + 8]                          ! Update TSB entry data
        stx     %g1, [%g2]                              ! Update TSB entry tag
        stxa    %g4, [%g0] ASI_IMMU_DATA_IN             ! Enter new mapping
        membar  #Sync
        CLRTT
        retry
        NOTREACHED
        !!
        !!  Check our prom mappings -- temporary
        !!

/*
 * Each memory text access fault, from user or kernel mode,
 * comes here.
 *
 * We will assume that %pil is not lost so we won't bother to save it
 * unless we're in an interrupt handler.
 *
 * On entry:
 *      We are on one of the alternate set of globals
 *      %g1 = MMU tag target
 *      %g2 = %tl
 *      %g3 = %tl - 1
 *
 * On return:
 *
 */


textfault:
        wrpr    %g0, PSTATE_KERN|PSTATE_AG, %pstate     ! We need to save volatile stuff to AG regs
        wr      %g0, ASI_IMMU, %asi
        ldxa    [%g0 + TLB_TAG_ACCESS] %asi, %g1        ! Get fault address from tag access register
        ldxa    [SFSR] %asi, %g3                        ! get sync fault status register
        membar  #LoadStore
        stxa    %g0, [SFSR] %asi                        ! Clear out old info
        membar  #Sync                                   ! No real reason for this XXXX

        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF
        mov     %g3, %o3

        wrpr    %g0, PSTATE_KERN, %pstate               ! Switch to normal globals
        ldxa    [%g0] ASI_AFSR, %o4                     ! get async fault status
        ldxa    [%g0] ASI_AFAR, %o5                     ! get async fault address
        mov     -1, %o0
        stxa    %o0, [%g0] ASI_AFSR                     ! Clear this out
        membar  #Sync                                   ! No real reason for this XXXX
        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]      ! save g1
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]      ! save g2
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]      ! (sneak g3 in here)
        rdpr    %tt, %o1                                ! Find out what caused this trap
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]      ! sneak in g4
        rdpr    %tstate, %g1
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]      ! sneak in g5
        rdpr    %tpc, %o2                               ! sync virt addr; must be read first
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]      ! sneak in g6
        rdpr    %tnpc, %g3
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]      ! sneak in g7
        rd      %y, %g4                                 ! save y

        /* Finish stackframe, call C trap handler */
        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]         ! set tf.tf_psr, tf.tf_pc
        sth     %o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug

        stx     %o2, [%sp + CC64FSZ + BIAS + TF_PC]
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_NPC]            ! set tf.tf_npc

        rdpr    %pil, %g5
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_PIL]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]

        rdpr    %tl, %g7
        dec     %g7
        movrlz  %g7, %g0, %g7
        wrpr    %g0, %g7, %tl           ! Revert to kernel mode

        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore default ASI
        flushw                                          ! Get rid of any user windows so we don't deadlock

        GET_CPUINFO_VA(%g7)

        /* Use trap type to see what handler to call */
        cmp     %o1, T_INST_ERROR
        be,pn   %xcc, text_error
         st     %g4, [%sp + CC64FSZ + BIAS + TF_Y]              ! set tf.tf_y

        wrpr    %g0, PSTATE_INTR, %pstate       ! reenable interrupts
        call    text_access_fault               ! mem_access_fault(&tf, type, pc, sfsr)
         add    %sp, CC64FSZ + BIAS, %o0        ! (argument: &tf)
text_recover:
        wrpr    %g0, PSTATE_KERN, %pstate       ! disable interrupts
        b       return_from_trap                ! go return
         ldx    [%sp + CC64FSZ + BIAS + TF_TSTATE], %g1 ! Load this for return_from_trap
        NOTREACHED

text_error:
        wrpr    %g0, PSTATE_INTR, %pstate       ! reenable interrupts
        call    text_access_error               ! mem_access_fault(&tfm type, sfva [pc], sfsr,
                                                !               afva, afsr);
         add    %sp, CC64FSZ + BIAS, %o0        ! (argument: &tf)
        ba      text_recover
         nop
        NOTREACHED

#ifdef SUN4V

/*
 * Perform an inline pseg_get(), to retrieve the address of the PTE associated
 * to the given virtual address.
 * On entry: %g3 = va (won't be modified), %g6 = context
 * Registers used: %g4,%g5, %g6
 * Branches to the "failure" label if translation invalid, otherwise ends
 * with the pte address in %g6.
 */
        .macro  PTE_GET_SUN4V   failure
        sethi   %hi(ctxbusy), %g4
        ldx     [%g4 + %lo(ctxbusy)], %g4
        sllx    %g6, 3, %g6                     ! Make it into an offset into ctxbusy
        ldx     [%g4+%g6], %g4                  ! Load up our page table.

        srax    %g3, HOLESHIFT, %g5             ! Check for valid address
        brz,pt  %g5, 0f                         ! Should be zero or -1
         inc    %g5                             ! (0 or -1) -> (1 or 0)
        brnz,pn %g5, \failure                   ! Error! In hole!
0:
        srlx    %g3, STSHIFT, %g6
        and     %g6, STMASK, %g6                ! Index into pm_segs
        sll     %g6, 3, %g6
        add     %g4, %g6, %g4
        ldxa    [%g4] ASI_PHYS_CACHED, %g4      ! Load page directory pointer
        srlx    %g3, PDSHIFT, %g6
        and     %g6, PDMASK, %g6
        sll     %g6, 3, %g6
        brz,pn  %g4, \failure                   ! NULL entry? check somewhere else
         add    %g4, %g6, %g4
        ldxa    [%g4] ASI_PHYS_CACHED, %g4      ! Load page table pointer
        srlx    %g3, PTSHIFT, %g6               ! Convert to ptab offset
        and     %g6, PTMASK, %g6
        sll     %g6, 3, %g6
        brz,pn  %g4, \failure                   ! NULL entry? check somewhere else
         add    %g4, %g6, %g6
        .endm

/*
 * Traps for sun4v.
 */

sun4v_tl1_dtsb_miss:
        GET_MMFSA(%g1)
        add     %g1, 0x48, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        PTE_GET_SUN4V sun4v_tl1_ptbl_miss
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, sun4v_tl1_ptbl_miss               ! Entry invalid?  Punt
         or     %g4, SUN4V_TLB_ACCESS, %g7              ! Update the access bit

        btst    SUN4V_TLB_ACCESS, %g4                   ! Need to update access git?
        bne,pt  %xcc, 2f
         nop
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4V_TLB_ACCESS, %g4              ! Update the modified bit
2:
        sethi   %hi(tsb_dmmu), %g2
        ldx     [%g2 + %lo(tsb_dmmu)], %g2

        mov     %g1, %g7
        /* Construct TSB tag word. */
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        mov     %g3, %g1
        srlx    %g1, 22, %g1
        sllx    %g6, 48, %g6
        or      %g1, %g6, %g1

        srlx    %g3, PTSHIFT, %g3
        sethi   %hi(tsbsize), %g5
        mov     512, %g6
        ld      [%g5 + %lo(tsbsize)], %g5
        sllx    %g6, %g5, %g5
        sub     %g5, 1, %g5
        and     %g3, %g5, %g3
        sllx    %g3, 4, %g3
        add     %g2, %g3, %g2

        LOCK_TSB
        stx     %g4, [%g2 + 8]
        stx     %g1, [%g2]              ! unlock

        retry
        NOTREACHED

sun4v_tl1_dtsb_prot:
        GET_MMFSA(%g1)
        add     %g1, 0x48, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        PTE_GET_SUN4V sun4v_tl1_ptbl_miss
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, sun4v_tl1_ptbl_miss               ! Entry invalid?  Punt
         or     %g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g7
                ! Update the modified bit

#       btst    SUN4V_TLB_REAL_W|SUN4V_TLB_W, %g4       ! Is it a ref fault?
        mov     1, %g2
        sllx    %g2, 61, %g2
        or      %g2, SUN4V_TLB_W, %g2
        btst    %g2, %g4
        bz,pn   %xcc, sun4v_tl1_ptbl_miss               ! No -- really fault
         nop
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g4
                ! Update the modified bit
2:
        sethi   %hi(tsb_dmmu), %g2
        ldx     [%g2 + %lo(tsb_dmmu)], %g2

        mov     %g1, %g7
        /* Construct TSB tag word. */
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        mov     %g3, %g1
        srlx    %g1, 22, %g1
        sllx    %g6, 48, %g6
        or      %g1, %g6, %g1

        srlx    %g3, PTSHIFT, %g3
        sethi   %hi(tsbsize), %g5
        mov     512, %g6
        ld      [%g5 + %lo(tsbsize)], %g5
        sllx    %g6, %g5, %g5
        sub     %g5, 1, %g5
        and     %g3, %g5, %g3
        sllx    %g3, 4, %g3
        add     %g2, %g3, %g2

        LOCK_TSB
        stx     %g4, [%g2 + 8]
        stx     %g1, [%g2]              ! unlock

        mov     %o0, %g1
        mov     %o1, %g2
        mov     %o2, %g3

#define MAP_DTLB        0x1
#define MAP_ITLB        0x2
#define MMU_UNMAP_ADDR  0x84
        add     %g7, 0x48, %o0
        ldxa    [%o0] ASI_PHYS_CACHED, %o0
        add     %g7, 0x50, %o1
        ldxa    [%o1] ASI_PHYS_CACHED, %o1
        mov     MAP_DTLB, %o2
        ta      MMU_UNMAP_ADDR

        mov     %g1, %o0
        mov     %g2, %o1
        mov     %g3, %o2

        retry
        NOTREACHED


sun4v_tl1_ptbl_miss:
        rdpr    %tpc, %g1

        set     rft_user_fault_start, %g2
        cmp     %g1, %g2
        blu,pt  %xcc, 1f
         set    rft_user_fault_end, %g2
        cmp     %g1, %g2
        bgeu,pt %xcc, 1f
         nop

        /* Fixup %cwp. */
        rdpr    %cwp, %g1
        inc     %g1
        wrpr    %g1, %cwp

        rdpr    %tt, %g1
        wrpr    1, %tl
        wrpr    %g1, %tt
        rdpr    %cwp, %g1
        set     TSTATE_KERN, %g2
        wrpr    %g1, %g2, %tstate
        set     return_from_trap, %g1
        wrpr    %g1, %tpc
        add     %g1, 4, %g1
        wrpr    %g1, %tnpc
        wrpr    %g0, 1, %gl

        ba,pt %xcc, sun4v_datatrap
         wrpr   WSTATE_KERN, %wstate

1:
        rdpr    %tstate, %g3
        rdpr    %tt, %g4

        rdpr    %tl, %g1
        dec     %g1
        wrpr    %g1, %tl
        rdpr    %tt, %g2
        inc     %g1
        wrpr    %g1, %tl

        wrpr    %g0, %g3, %tstate
        wrpr    %g0, %g4, %tt

        andn    %g2, 0x00f, %g3
        cmp     %g3, 0x080
        be,pn   %icc, flush_normals
         nop
        cmp     %g3, 0x0a0
        be,pn   %icc, flush_others
         nop
        cmp     %g3, 0x0c0
        be,pn   %icc, ufill_trap
         nop

        db_enter()
        NOTREACHED

flush_others:
        set     pcbspill_others, %g1
        wrpr    %g1, %tnpc
        done
        NOTREACHED

flush_normals:
ufill_trap:
        /*
         * Rearrange our trap state such that it appears as if we got
         * this trap directly from user mode.  Then process it at TL = 1.
         * We'll take the spill/fill trap again once we return to user mode.
         */
        rdpr    %tt, %g1
        rdpr    %tstate, %g3
        wrpr    %g0, 1, %tl
        wrpr    %g0, %g1, %tt
        rdpr    %tstate, %g2
        wrpr    %g0, 2, %tl
        and     %g2, TSTATE_CWP, %g2
        andn    %g3, TSTATE_CWP, %g3
        wrpr    %g2, %g3, %tstate
        set     sun4v_datatrap, %g4
        wrpr    %g0, %g4, %tnpc
        done

sun4v_tl0_dtsb_miss:
        GET_MMFSA(%g1)
        add     %g1, 0x48, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        PTE_GET_SUN4V sun4v_datatrap
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, sun4v_datatrap                    ! Entry invalid?  Punt
         or     %g4, SUN4V_TLB_ACCESS, %g7              ! Update the access bit

        btst    SUN4V_TLB_ACCESS, %g4                   ! Need to update access git?
        bne,pt  %xcc, 2f
         nop
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4V_TLB_ACCESS, %g4              ! Update the modified bit
2:
        sethi   %hi(tsb_dmmu), %g2
        ldx     [%g2 + %lo(tsb_dmmu)], %g2

        mov     %g1, %g7
        /* Construct TSB tag word. */
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        mov     %g3, %g1
        srlx    %g1, 22, %g1
        sllx    %g6, 48, %g6
        or      %g1, %g6, %g1

        srlx    %g3, PTSHIFT, %g3
        sethi   %hi(tsbsize), %g5
        mov     512, %g6
        ld      [%g5 + %lo(tsbsize)], %g5
        sllx    %g6, %g5, %g5
        sub     %g5, 1, %g5
        and     %g3, %g5, %g3
        sllx    %g3, 4, %g3
        add     %g2, %g3, %g2

        LOCK_TSB
        stx     %g4, [%g2 + 8]
        stx     %g1, [%g2]              ! unlock

        retry
        NOTREACHED

sun4v_tl0_dtsb_prot:
        GET_MMFSA(%g1)
        add     %g1, 0x48, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        PTE_GET_SUN4V sun4v_datatrap
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, sun4v_datatrap                    ! Entry invalid?  Punt
         or     %g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g7
                ! Update the modified bit

#       btst    SUN4V_TLB_REAL_W|SUN4V_TLB_W, %g4       ! Is it a ref fault?
        mov     1, %g2
        sllx    %g2, 61, %g2
        or      %g2, SUN4V_TLB_W, %g2
        btst    %g2, %g4
        bz,pn   %xcc, sun4v_datatrap                    ! No -- really fault
         nop
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g4
                ! Update the modified bit
2:
        sethi   %hi(tsb_dmmu), %g2
        ldx     [%g2 + %lo(tsb_dmmu)], %g2

        mov     %g1, %g7
        /* Construct TSB tag word. */
        add     %g1, 0x50, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        mov     %g3, %g1
        srlx    %g1, 22, %g1
        sllx    %g6, 48, %g6
        or      %g1, %g6, %g1

        srlx    %g3, PTSHIFT, %g3
        sethi   %hi(tsbsize), %g5
        mov     512, %g6
        ld      [%g5 + %lo(tsbsize)], %g5
        sllx    %g6, %g5, %g5
        sub     %g5, 1, %g5
        and     %g3, %g5, %g3
        sllx    %g3, 4, %g3
        add     %g2, %g3, %g2

        LOCK_TSB
        stx     %g4, [%g2 + 8]
        stx     %g1, [%g2]              ! unlock

        mov     %o0, %g1
        mov     %o1, %g2
        mov     %o2, %g3

#define MAP_DTLB        0x1
#define MMU_UNMAP_ADDR  0x84
        add     %g7, 0x48, %o0
        ldxa    [%o0] ASI_PHYS_CACHED, %o0
        add     %g7, 0x50, %o1
        ldxa    [%o1] ASI_PHYS_CACHED, %o1
        mov     MAP_DTLB, %o2
        ta      MMU_UNMAP_ADDR

        mov     %g1, %o0
        mov     %g2, %o1
        mov     %g3, %o2

        retry
        NOTREACHED

sun4v_tl0_itsb_miss:
        GET_MMFSA(%g1)
        add     %g1, 0x8, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        add     %g1, 0x10, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        PTE_GET_SUN4V sun4v_texttrap
1:
        ldxa    [%g6] ASI_PHYS_CACHED, %g4
        brgez,pn %g4, sun4v_texttrap                    ! Entry invalid?  Punt
         or     %g4, SUN4V_TLB_ACCESS, %g7              ! Update the access bit

        btst    SUN4V_TLB_EXEC, %g4
        bz,pn   %xcc, sun4v_texttrap
         nop
        btst    SUN4V_TLB_ACCESS, %g4                   ! Need to update access git?
        bne,pt  %xcc, 2f
         nop
        casxa   [%g6] ASI_PHYS_CACHED, %g4, %g7         !  and write it out
        cmp     %g4, %g7
        bne,pn  %xcc, 1b
         or     %g4, SUN4V_TLB_ACCESS, %g4              ! Update the modified bit
2:
        sethi   %hi(tsb_dmmu), %g2
        ldx     [%g2 + %lo(tsb_dmmu)], %g2

        mov     %g1, %g7
        /* Construct TSB tag word. */
        add     %g1, 0x10, %g6
        ldxa    [%g6] ASI_PHYS_CACHED, %g6
        mov     %g3, %g1
        srlx    %g1, 22, %g1
        sllx    %g6, 48, %g6
        or      %g1, %g6, %g1

        srlx    %g3, PTSHIFT, %g3
        sethi   %hi(tsbsize), %g5
        mov     512, %g6
        ld      [%g5 + %lo(tsbsize)], %g5
        sllx    %g6, %g5, %g5
        sub     %g5, 1, %g5
        and     %g3, %g5, %g3
        sllx    %g3, 4, %g3
        add     %g2, %g3, %g2

        LOCK_TSB
        stx     %g4, [%g2 + 8]
        stx     %g1, [%g2]              ! unlock

        retry
        NOTREACHED

kspill_normal:
        wrpr    0x90, %tt

        GET_CPUINFO_PA(%g1)
        wr      %g0, ASI_PHYS_CACHED, %asi

        SPILL   stxa, %g1 + CI_RW, 8, %asi
        saved

        stxa    %sp, [%g1 + CI_RWSP] %asi

        retry
        NOTREACHED

/*
 * Spill user windows into the PCB.
 */
pcbspill_normals:
        ba,pt   %xcc, pcbspill
         wrpr   0x80, %tt

pcbspill_others:
        wrpr    0xa0, %tt

pcbspill:
        GET_CPUINFO_PA(%g6)
        wr      %g0, ASI_PHYS_CACHED, %asi              ! Use ASI_PHYS_CACHED to prevent possible page faults
        ldxa    [%g6 + CI_CPCBPADDR] %asi, %g6
        
        lduba   [%g6 + PCB_NSAVED] %asi, %g7
        sllx    %g7, 7, %g5
        add     %g6, %g5, %g5
        SPILL   stxa, %g5 + PCB_RW, 8, %asi
        saved

        sllx    %g7, 3, %g5
        add     %g6, %g5, %g5
        stxa    %sp, [%g5 + PCB_RWSP] %asi

        inc     %g7
        stba    %g7, [%g6 + PCB_NSAVED] %asi

        retry
        NOTREACHED

        /*
         * Copy the trap-time register window to the stack, if needed,
         * and reset ci_rwsp afterwards.
         * On entry: %g7 is curcpu
         * Registers used: %g2 and all locals
         */
        .macro  SUN4V_PUSH_RWINDOW
        ldx     [%g7 + CI_RWSP], %g2
        brz,pt  %g2, 98f
         nop

        ldx     [%g7 + CI_RW + (0*8)], %l0
        ldx     [%g7 + CI_RW + (1*8)], %l1
        ldx     [%g7 + CI_RW + (2*8)], %l2
        ldx     [%g7 + CI_RW + (3*8)], %l3
        ldx     [%g7 + CI_RW + (4*8)], %l4
        ldx     [%g7 + CI_RW + (5*8)], %l5
        ldx     [%g7 + CI_RW + (6*8)], %l6
        ldx     [%g7 + CI_RW + (7*8)], %l7
        stx     %l0, [%g2 + BIAS + (0*8)]
        stx     %l1, [%g2 + BIAS + (1*8)]
        stx     %l2, [%g2 + BIAS + (2*8)]
        stx     %l3, [%g2 + BIAS + (3*8)]
        stx     %l4, [%g2 + BIAS + (4*8)]
        stx     %l5, [%g2 + BIAS + (5*8)]
        stx     %l6, [%g2 + BIAS + (6*8)]
        stx     %l7, [%g2 + BIAS + (7*8)]
        ldx     [%g7 + CI_RW + (8*8)], %l0
        ldx     [%g7 + CI_RW + (9*8)], %l1
        ldx     [%g7 + CI_RW + (10*8)], %l2
        ldx     [%g7 + CI_RW + (11*8)], %l3
        ldx     [%g7 + CI_RW + (12*8)], %l4
        ldx     [%g7 + CI_RW + (13*8)], %l5
        ldx     [%g7 + CI_RW + (14*8)], %l6
        ldx     [%g7 + CI_RW + (15*8)], %l7
        stx     %l0, [%g2 + BIAS + (8*8)]
        stx     %l1, [%g2 + BIAS + (9*8)]
        stx     %l2, [%g2 + BIAS + (10*8)]
        stx     %l3, [%g2 + BIAS + (11*8)]
        stx     %l4, [%g2 + BIAS + (12*8)]
        stx     %l5, [%g2 + BIAS + (13*8)]
        stx     %l6, [%g2 + BIAS + (14*8)]
        stx     %l7, [%g2 + BIAS + (15*8)]

        stx     %g0, [%g7 + CI_RWSP]
98:
        .endm

sun4v_datatrap:
        GET_MMFSA(%g3)
        add     %g3, 0x48, %g1
        ldxa    [%g1] ASI_PHYS_CACHED, %g1
        add     %g3, 0x50, %g2
        ldxa    [%g2] ASI_PHYS_CACHED, %g2

        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF
        or      %g1, %g2, %o3
        mov     %g1, %o4

        rdpr    %tt, %g4
        rdpr    %tstate, %g1
        rdpr    %tpc, %g2
        rdpr    %tnpc, %g3

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
        mov     %g4, %o1                ! (type)
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_PC]
        rd      %y, %g5
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_NPC]
        st      %g5, [%sp + CC64FSZ + BIAS + TF_Y]
        mov     %g2, %o2                ! (pc)
        sth     %o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug

        cmp     %o1, T_FDMMU_PROT
        bne,pn  %icc, 1f
         mov    SFSR_FV, %o5
        or      %o5, SFSR_W, %o5

1:
        wrpr    %g0, PSTATE_KERN, %pstate               ! Get back to normal globals
        wrpr    %g0, 0, %gl

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]
        add     %sp, CC64FSZ + BIAS, %o0                ! (&tf)
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]
        rdpr    %pil, %g5
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_PIL]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]

        /*
         * Phew, ready to enable traps and call C code.
         */
        wrpr    %g0, 0, %tl

        GET_CPUINFO_VA(%g7)
        SUN4V_PUSH_RWINDOW

        wr      %g0, ASI_PRIMARY_NOFAULT, %asi  ! Restore default ASI
        wrpr    %g0, PSTATE_INTR, %pstate       ! traps on again
        call    data_access_fault               ! data_acces_fault(tf, type, ...)
         nop

        ba,a,pt %icc, return_from_trap
         nop
        NOTREACHED

sun4v_texttrap:
        GET_MMFSA(%g3)
        add     %g3, 0x08, %g1
        ldxa    [%g1] ASI_PHYS_CACHED, %g1
        add     %g3, 0x10, %g2
        ldxa    [%g2] ASI_PHYS_CACHED, %g2

        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF

        or      %g1, %g2, %o2
        clr     %o3

        rdpr    %tt, %g4
        rdpr    %tstate, %g1
        rdpr    %tpc, %g2
        rdpr    %tnpc, %g3

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
        mov     %g4, %o1                ! (type)
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_PC]
        rd      %y, %g5
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_NPC]
        st      %g5, [%sp + CC64FSZ + BIAS + TF_Y]
        sth     %o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug

        wrpr    %g0, PSTATE_KERN, %pstate               ! Get back to normal globals
        wrpr    %g0, 0, %gl

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]
        add     %sp, CC64FSZ + BIAS, %o0                ! (&tf)
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]
        rdpr    %pil, %g5
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_PIL]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]

        /*
         * Phew, ready to enable traps and call C code.
         */
        wrpr    %g0, 0, %tl

        GET_CPUINFO_VA(%g7)
        SUN4V_PUSH_RWINDOW

        wr      %g0, ASI_PRIMARY_NOFAULT, %asi  ! Restore default ASI
        wrpr    %g0, PSTATE_INTR, %pstate       ! traps on again
        call    text_access_fault               ! text_access_fault(tf, type, ...)
         nop

        ba,a,pt %icc, return_from_trap
         nop
        NOTREACHED

        .align 8
NENTRY(sun4v_tlb_flush_pte)
        ba      hv_mmu_demap_page
         mov    MAP_ITLB|MAP_DTLB, %o2
END(sun4v_tlb_flush_pte)

        .align 8
NENTRY(sun4v_tlb_flush_ctx)
        ba      hv_mmu_demap_ctx
         mov    MAP_ITLB|MAP_DTLB, %o1
END(sun4v_tlb_flush_ctx)

#endif  /* SUN4V */

/*
 * We're here because we took an alignment fault in NUCLEUS context.
 * This could be a kernel bug or it could be due to saving or restoring
 * a user window to/from an invalid stack pointer.
 * 
 * If the latter is the case, we could try to emulate unaligned accesses, 
 * but we really don't know where to store the registers since we can't 
 * determine if there's a stack bias.  Or we could store all the regs 
 * into the PCB and punt, until the user program uses up all the CPU's
 * register windows and we run out of places to store them.  So for
 * simplicity we'll just blow them away and enter the trap code which
 * will generate a bus error.  Debugging the problem will be a bit
 * complicated since lots of register windows will be lost, but what
 * can we do?
 */
checkalign:
        rdpr    %tl, %g2
        subcc   %g2, 1, %g1
        bneg,pn %icc, slowtrap          ! Huh?
         nop

        wrpr    %g1, 0, %tl
        rdpr    %tt, %g7
        rdpr    %tstate, %g4
        andn    %g7, 0x07f, %g5         ! Window spill traps are all 0b 0000 10xx xxxx
        cmp     %g5, 0x080              ! Window fill traps are all 0b 0000 11xx xxxx
        bne,a,pn %icc, slowtrap
         nop

        /*
         * %g1 -- current tl
         * %g2 -- original tl
         * %g4 -- tstate
         * %g7 -- tt
         */

        and     %g4, CWP, %g5
        wrpr    %g5, %cwp               ! Go back to the original register window

        rdpr    %otherwin, %g6
        rdpr    %cansave, %g5
        add     %g5, %g6, %g5
        wrpr    %g0, 0, %otherwin       ! Just blow away all user windows
        wrpr    %g5, 0, %cansave
        rdpr    %canrestore, %g5
        wrpr    %g5, 0, %cleanwin

        wrpr    %g0, T_ALIGN, %tt       ! This was an alignment fault 
        /*
         * Now we need to determine if this was a userland store/load or not.
         * Userland stores occur in anything other than the kernel spill/fill
         * handlers (trap type 0x9x/0xdx).
         */
        and     %g7, 0xff0, %g5
        cmp     %g5, 0x90
        bz,pn   %icc, slowtrap
         nop
        cmp     %g5, 0xd0
        bz,pn   %icc, slowtrap
         nop
        and     %g7, 0xfc0, %g5
        wrpr    %g5, 0, %tt
        ba,a,pt %icc, slowtrap
         nop

/*
 * slowtrap() builds a trap frame and calls trap().
 * This is called `slowtrap' because it *is*....
 * We have to build a full frame for ptrace(), for instance.
 *
 * Registers:
 *
 */
slowtrap:
        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF

        rdpr    %tt, %g4
        rdpr    %tstate, %g1
        rdpr    %tpc, %g2
        rdpr    %tnpc, %g3

Lslowtrap_reenter:
        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
        mov     %g4, %o1                ! (type)
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_PC]
        rd      %y, %g5
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_NPC]
        mov     %g1, %o3                ! (pstate)
        st      %g5, [%sp + CC64FSZ + BIAS + TF_Y]
        mov     %g2, %o2                ! (pc)
        sth     %o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug

        NORMAL_GLOBALS()

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]
        add     %sp, CC64FSZ + BIAS, %o0                ! (&tf)
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]
        rdpr    %pil, %g5
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_PIL]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]

        /*
         * Phew, ready to enable traps and call C code.
         */
        wrpr    %g0, 0, %tl

        GET_CPUINFO_VA(%g7)
#ifdef SUN4V
        SUN4V_PUSH_RWINDOW
#endif

        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore default ASI
        wrpr    %g0, PSTATE_INTR, %pstate       ! traps on again
        call    trap                            ! trap(tf, type, pc, pstate)
         nop

        ba,a,pt %icc, return_from_trap
         nop
        NOTREACHED

/*
 * Do a `software' trap by re-entering the trap code, possibly first
 * switching from interrupt stack to kernel stack.  This is used for
 * scheduling and signal ASTs (which generally occur from softclock or
 * tty or net interrupts).
 *
 * We enter with the trap type in %g1.  All we have to do is jump to
 * Lslowtrap_reenter above, but maybe after switching stacks....
 *
 * We should be running alternate globals.  The normal globals and
 * out registers were just loaded from the old trap frame.
 *
 *      Input Params:
 *      %g1 = tstate
 *      %g2 = tpc
 *      %g3 = tnpc
 *      %g4 = tt == T_AST
 */
softtrap:
        GET_INTSTACK(%g5)
        sethi   %hi(EINTSTACK-INTSTACK), %g7
        sub     %g5, BIAS, %g5
        dec     %g7

        sub     %g5, %sp, %g5
        andncc  %g5, %g7, %g0
        bnz,pt  %xcc, Lslowtrap_reenter
         nop
        GET_CPCB(%g7)
        set     USPACE-CC64FSZ-TRAPFRAME_SIZEOF-BIAS, %g5
        add     %g7, %g5, %g6
        stx     %i0, [%g6 + CC64FSZ + BIAS + TF_O + (0*8)]      ! Generate a new trapframe
        stx     %i1, [%g6 + CC64FSZ + BIAS + TF_O + (1*8)]      !       but don't bother with
        stx     %i2, [%g6 + CC64FSZ + BIAS + TF_O + (2*8)]      !       locals and ins
        stx     %i3, [%g6 + CC64FSZ + BIAS + TF_O + (3*8)]
        stx     %i4, [%g6 + CC64FSZ + BIAS + TF_O + (4*8)]
        stx     %i5, [%g6 + CC64FSZ + BIAS + TF_O + (5*8)]
        stx     %i6, [%g6 + CC64FSZ + BIAS + TF_O + (6*8)]
        stx     %i7, [%g6 + CC64FSZ + BIAS + TF_O + (7*8)]
        ba,pt   %xcc, Lslowtrap_reenter
         mov    %g6, %sp

/*
 * syscall_setup() builds a trap frame and calls syscall().
 * XXX  should not have to save&reload ALL the registers just for
 *      ptrace...
 */
syscall_setup:
        TRAP_SETUP -CC64FSZ-TRAPFRAME_SIZEOF

#ifdef DEBUG
        rdpr    %tt, %o1        ! debug
        sth     %o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug
#endif  /* DEBUG */

        NORMAL_GLOBALS()

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + ( 1*8)]
        mov     %g1, %o1                        ! code
        rdpr    %tpc, %o2                       ! (pc)
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + ( 2*8)]
        rdpr    %tstate, %g1
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + ( 3*8)]
        rdpr    %tnpc, %o3
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + ( 4*8)]
        rd      %y, %o4
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + ( 5*8)]
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + ( 6*8)]
        wrpr    %g0, 0, %tl                     ! return to tl=0
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + ( 7*8)]
        add     %sp, CC64FSZ + BIAS, %o0        ! (&tf)

        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
        stx     %o2, [%sp + CC64FSZ + BIAS + TF_PC]
        stx     %o3, [%sp + CC64FSZ + BIAS + TF_NPC]
        st      %o4, [%sp + CC64FSZ + BIAS + TF_Y]

        rdpr    %pil, %g5
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_PIL]
        stb     %g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]

        wr      %g0, ASI_PRIMARY_NOFAULT, %asi  ! Restore default ASI

        GET_CPUINFO_VA(%g7)
        call    syscall                         ! syscall(&tf, code, pc)
         wrpr   %g0, PSTATE_INTR, %pstate       ! turn on interrupts

        wrpr    %g0, PSTATE_KERN, %pstate       ! Disable interrupts
        wrpr    %g0, 0, %tl                     ! Return to tl==0
        ba,a,pt %icc, return_from_trap
         nop
        NOTREACHED

/*
 * interrupt_vector:
 *
 * Spitfire chips never get level interrupts directly from H/W.
 * Instead, all interrupts come in as interrupt_vector traps.
 * The interrupt number or handler address is an 11 bit number
 * encoded in the first interrupt data word.  Additional words
 * are application specific and used primarily for cross-calls.
 *
 * The interrupt vector handler then needs to identify the
 * interrupt source from the interrupt number and arrange to
 * invoke the interrupt handler.  This can either be done directly
 * from here, or a softint at a particular level can be issued.
 *
 * To call an interrupt directly and not overflow the trap stack,
 * the trap registers should be saved on the stack, registers
 * cleaned, trap-level decremented, the handler called, and then
 * the process must be reversed.
 *
 * To simplify life all we do here is issue an appropriate softint.
 *
 * Note:        It is impossible to identify or change a device's
 *              interrupt number until it is probed.  That's the
 *              purpose for all the funny interrupt acknowledge
 *              code.
 *
 */

/*
 * Vectored interrupts:
 *
 * When an interrupt comes in, interrupt_vector uses the interrupt
 * vector number to lookup the appropriate intrhand from the intrlev
 * array.  It then looks up the interrupt level from the intrhand
 * structure.  It uses the level to index the per-cpu intrpending array,
 * and inserts the intrhand at the head of the proper intrpending entry.
 *
 * Then interrupt_vector uses the interrupt level in the intrhand
 * to issue a softint of the appropriate level.  The softint handler
 * figures out what level interrupt it's handling and pulls the
 * intrhand pointer at thead of the intrpending list for that interrupt
 * level, removes it from the list, clears the interrupt generator,
 * and invokes the interrupt handler.
 */

        .text
interrupt_vector:
        ldxa    [%g0] ASI_IRSR, %g1
        mov     IRDR_0H, %g2
        ldxa    [%g2] ASI_IRDR, %g2     ! Get interrupt number
        membar  #Sync

        sethi   %hi(KERNBASE), %g3
        btst    IRSR_BUSY, %g1
        bz,pn   %icc, 3f                ! Spurious interrupt
         cmp    %g2, %g3
#ifdef MULTIPROCESSOR
        blu,pt  %xcc, Lsoftint_regular
         and    %g2, MAXINTNUM-1, %g5   ! XXX make sun4us work
        mov     IRDR_1H, %g3
        ldxa    [%g3] ASI_IRDR, %g3     ! Get IPI handler arg0
        mov     IRDR_2H, %g5
        ldxa    [%g5] ASI_IRDR, %g5     ! Get IPI handler arg1

        stxa    %g0, [%g0] ASI_IRSR     ! Ack IRQ
        membar  #Sync                   ! Should not be needed due to retry

        jmpl    %g2, %g0
         nop
        db_enter()
        NOTREACHED
#else
        bgeu,pn %xcc, 3f
         and    %g2, MAXINTNUM-1, %g5   ! XXX make sun4us work
#endif

Lsoftint_regular:
        stxa    %g0, [%g0] ASI_IRSR     ! Ack IRQ
        membar  #Sync                   ! Should not be needed due to retry

        sethi   %hi(intrlev), %g3
        or      %g3, %lo(intrlev), %g3
        sllx    %g5, 3, %g5             ! Calculate entry number
        ldx     [%g3 + %g5], %g5        ! We have a pointer to the handler
        
        brz,pn  %g5, 3f                 ! NULL means it isn't registered yet.  Skip it.
         nop

setup_sparcintr:
        ldx     [%g5+IH_PEND], %g6      ! Check if already in use
        brnz,pn %g6, ret_from_intr_vector ! Skip it if it's running
         ldub   [%g5+IH_PIL], %g6       ! Read interrupt mask
        GET_CPUINFO_VA(%g1)
        sll     %g6, 3, %g3             ! Find start of list for this IPL
        add     %g1, CI_INTRPENDING, %g1
        add     %g1, %g3, %g1
1:
        ldx     [%g1], %g3              ! Load list head
        add     %g5, IH_PEND, %g7
        casxa   [%g7] ASI_N, %g0, %g3
        brnz,pn %g3, ret_from_intr_vector
         nop
        stx     %g5, [%g1]

        mov     1, %g7
        sll     %g7, %g6, %g6
        wr      %g6, 0, SET_SOFTINT     ! Invoke a softint

ret_from_intr_vector:
        CLRTT
        retry
        NOTREACHED

3:
        ba,a    ret_from_intr_vector
         nop                            ! XXX spitfire bug?

#ifdef SUN4V

sun4v_cpu_mondo:
        mov     0x3c0, %g1
        ldxa    [%g1] ASI_QUEUE, %g2

        GET_CPUINFO_PA(%g3)
        add     %g3, CI_CPUMQ, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        ldxa    [%g3 + %g2] ASI_PHYS_CACHED, %g4
        add     %g2, 8, %g5
        ldxa    [%g3 + %g5] ASI_PHYS_CACHED, %g5
        add     %g2, 16, %g6
        ldxa    [%g3 + %g6] ASI_PHYS_CACHED, %g6
        add     %g2, 64, %g2
        and     %g2, 0x7ff, %g2
        stxa    %g2, [%g1] ASI_QUEUE
        membar  #Sync

        mov     %g4, %g2
        mov     %g5, %g3
        mov     %g6, %g5
        jmpl    %g2, %g0
         nop                    ! No store here!
        retry
        NOTREACHED

sun4v_dev_mondo:
        mov     0x3d0, %g1
        ldxa    [%g1] ASI_QUEUE, %g2

        GET_CPUINFO_PA(%g3)
        add     %g3, CI_DEVMQ, %g3
        ldxa    [%g3] ASI_PHYS_CACHED, %g3
        ldxa    [%g3 + %g2] ASI_PHYS_CACHED, %g5
        add     %g2, 64, %g2
        and     %g2, 0x7ff, %g2
        stxa    %g2, [%g1] ASI_QUEUE
        membar  #Sync

        cmp     %g5, MAXINTNUM
        bgeu,pt %xcc, 1f
         nop

        sethi   %hi(intrlev), %g3
        or      %g3, %lo(intrlev), %g3
        sllx    %g5, 3, %g5             ! Calculate entry number
        ldx     [%g3 + %g5], %g5        ! We have a pointer to the handler
1:
        brnz,pt %g5, setup_sparcintr
         nop

        ba,a    3b
         nop

#endif

#ifdef MULTIPROCESSOR
NENTRY(sun4u_ipi_tlb_page_demap)
        rdpr    %pstate, %g1
        andn    %g1, PSTATE_IE, %g2
        wrpr    %g2, %pstate                            ! disable interrupts

        rdpr    %tl, %g2
        brnz    %g2, 1f
         add    %g2, 1, %g4
        wrpr    %g0, %g4, %tl                           ! Switch to traplevel > 0
1:
        mov     CTX_PRIMARY, %g4
        andn    %g3, 0xfff, %g3                         ! drop unused va bits
        ldxa    [%g4] ASI_DMMU, %g6                     ! Save primary context
        sethi   %hi(KERNBASE), %g7
        membar  #LoadStore
        stxa    %g5, [%g4] ASI_DMMU                     ! Insert context to demap
        membar  #Sync
        or      %g3, DEMAP_PAGE_PRIMARY, %g3
        stxa    %g0, [%g3] ASI_DMMU_DEMAP
        stxa    %g0, [%g3] ASI_IMMU_DEMAP
        membar  #Sync
        flush   %g7
        stxa    %g6, [%g4] ASI_DMMU
        membar  #Sync
        flush   %g7

        wrpr    %g2, %tl
        wrpr    %g1, %pstate
        ba,a    ret_from_intr_vector
         nop
END(sun4u_ipi_tlb_page_demap)

NENTRY(sun4u_ipi_tlb_context_demap)
        rdpr    %pstate, %g1
        andn    %g1, PSTATE_IE, %g2
        wrpr    %g2, %pstate                            ! disable interrupts

        rdpr    %tl, %g2
        brnz    %g2, 1f
         add    %g2, 1, %g4
        wrpr    %g0, %g4, %tl                           ! Switch to traplevel > 0
1:
        mov     CTX_PRIMARY, %g4
        sethi   %hi(KERNBASE), %g7
        ldxa    [%g4] ASI_DMMU, %g6                     ! Save primary context
        membar  #LoadStore
        stxa    %g3, [%g4] ASI_DMMU                     ! Insert context to demap
        membar  #Sync
        set     DEMAP_CTX_PRIMARY, %g3
        stxa    %g0, [%g3] ASI_DMMU_DEMAP
        stxa    %g0, [%g3] ASI_IMMU_DEMAP
        membar  #Sync
        flush   %g7
        stxa    %g6, [%g4] ASI_DMMU
        membar  #Sync
        flush   %g7

        wrpr    %g2, %tl
        wrpr    %g1, %pstate
        ba,a    ret_from_intr_vector
         nop
END(sun4u_ipi_tlb_context_demap)

#ifdef SUN4V
NENTRY(sun4v_ipi_tlb_page_demap)
        mov     %o0, %g1
        mov     %o1, %g2
        mov     %o2, %g4
        mov     %g3, %o0
        mov     %g5, %o1
        mov     MAP_DTLB|MAP_ITLB, %o2
        ta      MMU_UNMAP_ADDR
        mov     %g1, %o0
        mov     %g2, %o1
        mov     %g4, %o2

        retry
END(sun4v_ipi_tlb_page_demap)

NENTRY(sun4v_ipi_tlb_context_demap)
        NOTREACHED
END(sun4v_ipi_tlb_context_demap)
#endif

NENTRY(ipi_save_fpstate)
        GET_CPUINFO_VA(%g1)
        ldx     [%g1 + CI_FPPROC], %g2
        cmp     %g2, %g3
        bne,pn  %xcc, 3f

         mov    CTX_SECONDARY, %g2
        GET_MMU_CONTEXTID(%g6, %g2)
        membar  #LoadStore
        SET_MMU_CONTEXTID(%g0, %g2)
        membar  #Sync

        ldx     [%g3 + P_FPSTATE], %g3

        rdpr    %pstate, %g2            ! enable FP before we begin
        rd      %fprs, %g4
        wr      %g0, FPRS_FEF, %fprs
        or      %g2, PSTATE_PEF, %g2
        wrpr    %g2, 0, %pstate

        stx     %fsr, [%g3 + FS_FSR]    ! f->fs_fsr = getfsr();

        rd      %gsr, %g2               ! Save %gsr
        st      %g2, [%g3 + FS_GSR]

        add     %g3, FS_REGS, %g3       ! This is zero...
        btst    FPRS_DL, %g4            ! Lower FPU clean?
        bz,a,pt %icc, 1f                ! Then skip it
         add    %g3, 128, %g3           ! Skip a block

        membar  #Sync
        stda    %f0, [%g3] ASI_BLK_S    ! f->fs_f0 = etc;
        inc     BLOCK_SIZE, %g3
        stda    %f16, [%g3] ASI_BLK_S
        inc     BLOCK_SIZE, %g3
1:
        btst    FPRS_DU, %g4            ! Upper FPU clean?
        bz,pt   %icc, 2f                ! Then skip it
         nop

        membar  #Sync
        stda    %f32, [%g3] ASI_BLK_S
        inc     BLOCK_SIZE, %g3
        stda    %f48, [%g3] ASI_BLK_S
2:
        membar  #Sync                   ! Finish operation so we can
        wr      %g0, FPRS_FEF, %fprs    ! Mark FPU clean

        stx     %g0, [%g1 + CI_FPPROC]  ! fpproc = NULL
        mov     CTX_SECONDARY, %g2
        SET_MMU_CONTEXTID(%g6, %g2)
        membar  #Sync
3:
        ba      ret_from_intr_vector
         nop
END(ipi_save_fpstate)

NENTRY(ipi_drop_fpstate)
        GET_CPUINFO_VA(%g1)
        ldx     [%g1 + CI_FPPROC], %g2
        cmp     %g2, %g3
        bne,pn  %xcc, 1f
         nop
        wr      %g0, FPRS_FEF, %fprs
        stx     %g0, [%g1 + CI_FPPROC]          ! fpproc = NULL
1:
        ba      ret_from_intr_vector
         nop
END(ipi_drop_fpstate)

NENTRY(ipi_softint)
        ba      ret_from_intr_vector
         wr     %g3, 0, SET_SOFTINT
END(ipi_softint)

NENTRY(ipi_db)
        ba      slowtrap
         wrpr   %g0, T_BREAKPOINT, %tt
END(ipi_db)
#endif

/*
 * Ultra1 and Ultra2 CPUs use soft interrupts for everything.  What we do
 * on a soft interrupt, is we should check which bits in ASR_SOFTINT(0x16)
 * are set, handle those interrupts, then clear them by setting the
 * appropriate bits in ASR_CLEAR_SOFTINT(0x15).
 *
 * We have a list of interrupt handlers for each of 15 interrupt levels.
 * If a vectored interrupt can be dispatched, the dispatch routine will put
 * the intrhand at the head of the appropriate list. The interrupt handler
 * will go through the list to look for an interrupt to dispatch.  If it
 * finds one it will pull it off the list, and call the handler.
 *
 * After preliminary setup work, the interrupt is passed to each
 * registered handler in turn.  These are expected to return nonzero if
 * they took care of the interrupt.  If a handler claims the interrupt,
 * we exit (hardware interrupts are latched in the requestor so we'll
 * just take another interrupt in the unlikely event of simultaneous
 * interrupts from two different devices at the same level).
 *
 * Inputs:
 *      %l0 = %tstate
 *      %l1 = return pc
 *      %l2 = return npc
 *      %l3 = interrupt level
 *
 * Internal:
 *      %l4, %l5: local variables
 *      %l6 = %y
 *      %l7 = %g1
 *      %g2..%g7 go to stack
 *
 * An interrupt frame is built in the space for a full trapframe;
 * this contains the psr, pc, npc, and interrupt level.
 *
 * The level of this interrupt is determined by:
 *
 *       IRQ# = %tt - 0x40
 */

        .globl sparc_interrupt                  ! This is for interrupt debugging
sparc_interrupt:
        /*
         * If this is a %tick softint, clear it then call interrupt_vector.
         */
        rd      SOFTINT, %g1
        set     (TICK_INT|STICK_INT), %g2
        andcc   %g2, %g1, %g2
        bz,pt   %icc, 0f
         GET_CPUINFO_VA(%g7)
        wr      %g2, 0, CLEAR_SOFTINT
        ba,pt   %icc, setup_sparcintr
         add    %g7, CI_TICKINTR, %g5
0:
        ! We don't use TRAPFRAME_SIZEOF here because it might be a clock
        ! interrupt, which uses a larger frame. 
        INTR_SETUP -CC64FSZ-CLOCKFRAME_SIZEOF

        NORMAL_GLOBALS()

        /* Save normal globals */
        stx     %g1, [%sp + CC64FSZ + BIAS + TF_G + ( 1*8)]
        stx     %g2, [%sp + CC64FSZ + BIAS + TF_G + ( 2*8)]
        stx     %g3, [%sp + CC64FSZ + BIAS + TF_G + ( 3*8)]
        stx     %g4, [%sp + CC64FSZ + BIAS + TF_G + ( 4*8)]
        stx     %g5, [%sp + CC64FSZ + BIAS + TF_G + ( 5*8)]
        stx     %g6, [%sp + CC64FSZ + BIAS + TF_G + ( 6*8)]
        stx     %g7, [%sp + CC64FSZ + BIAS + TF_G + ( 7*8)]

999:    flushw                  ! Do not remove this instruction -- causes interrupt loss
        .section        .sun4v_patch, "ax"
        .word   999b
        nop
        .previous

        GET_CPUINFO_VA(%g7)
#ifdef SUN4V
        SUN4V_PUSH_RWINDOW
#endif

        rd      %y, %l6
        INCR uvmexp+V_INTR              ! uvmexp.intrs++; (clobbers %o0,%o1,%o2)
        rdpr    %tt, %l5                ! Find out our current IPL
        rdpr    %tstate, %l0
        rdpr    %tpc, %l1
        rdpr    %tnpc, %l2
        wrpr    %g0, 0, %tl

        ! Dump our trap frame now we have taken the IRQ
        stw     %l6, [%sp + CC64FSZ + BIAS + TF_Y]      ! Silly, but we need to save this for rft
        sth     %l5, [%sp + CC64FSZ + BIAS + TF_TT]! debug
        stx     %l0, [%sp + CC64FSZ + BIAS + TF_TSTATE] ! set up intrframe/clockframe
        stx     %l1, [%sp + CC64FSZ + BIAS + TF_PC]
        stx     %l2, [%sp + CC64FSZ + BIAS + TF_NPC]
        
        sub     %l5, 0x40, %l6                  ! Convert to interrupt level
        stb     %l6, [%sp + CC64FSZ + BIAS + TF_PIL]    ! set up intrframe/clockframe
        rdpr    %pil, %o1
        stb     %o1, [%sp + CC64FSZ + BIAS + TF_OLDPIL] ! old %pil
        clr     %l5                     ! Zero handled count
        mov     1, %l3                  ! Ack softint
        sll     %l3, %l6, %l3           ! Generate IRQ mask

        wrpr    %l6, %pil

        /*
         * Set handled_intr_level and save the old one so we can restore it
         * later.
         */
        ld      [%g7 + CI_HANDLED_INTR_LEVEL], %l0
        st      %l6, [%g7 + CI_HANDLED_INTR_LEVEL]
        st      %l0, [%sp + CC64FSZ + BIAS + SAVED_INTR_LEVEL]

sparc_intr_retry:
        wr      %l3, 0, CLEAR_SOFTINT   ! (don't clear possible %tick IRQ)
        wrpr    %g0, PSTATE_INTR, %pstate       ! Reenable interrupts
        sll     %l6, 3, %l2
        add     %g7, CI_INTRPENDING, %l4
        add     %l2, %l4, %l4

1:
        membar  #StoreLoad              ! Make sure any failed casxa instructions complete
        ldx     [%l4], %l2              ! Check the head of the list
        brz,pn  %l2, intrcmplt          ! Empty list?

         clr    %l7
        membar  #LoadStore
        casxa   [%l4] ASI_N, %l2, %l7   ! Grab the entire list
        cmp     %l7, %l2
        bne,pn  %icc, 1b
         nop

2:
        ldx     [%l2 + IH_PEND], %l7    ! Load next pending
        add     %l2, IH_PEND, %l3
        clr     %l4
        casxa   [%l3] ASI_N, %l7, %l4   ! Unlink from list
        cmp     %l7, %l4
        bne,pn  %xcc, 2b                ! Retry?
         add    %sp, CC64FSZ+BIAS, %o0  ! tf = %sp + CC64FSZ + BIAS

        ldx     [%l2 + IH_ACK], %l1     ! ih->ih_ack

        ! At this point, the current ih could already be added
        ! back to the pending list.

        call    intr_handler
         mov    %l2, %o1

        brz,pn  %l1, 0f
         add    %l5, %o0, %l5           ! Add handler return value
        ldx     [%l2 + IH_COUNT], %o0   ! ih->ih_count.ec_count++;
        inc     %o0
        stx     %o0, [%l2 + IH_COUNT]

        jmpl    %l1, %o7                ! (*ih->ih_ack)(ih)
         mov    %l2, %o0
0:
        brnz,pn %l7, 2b                 ! Another?
         mov    %l7, %l2

intrcmplt:
        /*
         * Re-read SOFTINT to see if there are any new pending interrupts
         * at this level.
         */
        mov     1, %l3                  ! Ack softint
        rd      SOFTINT, %l7
        sll     %l3, %l6, %l3           ! Generate IRQ mask
        btst    %l3, %l7                ! leave mask in %l3 for retry code
        bnz,pn  %icc, sparc_intr_retry
         mov    1, %l5                  ! initialize intr count for next run

        /* Restore old handled_intr_level */
        st      %l0, [%g7 + CI_HANDLED_INTR_LEVEL]

        ldub    [%sp + CC64FSZ + BIAS + TF_OLDPIL], %l3 ! restore old %pil
        wrpr    %g0, PSTATE_KERN, %pstate       ! Disable interrupts
        wrpr    %l3, 0, %pil

        ba,a,pt %icc, return_from_trap
         nop

        .globl  return_from_trap, rft_kernel, rft_user
        .globl  softtrap, slowtrap
        .globl  syscall

/*
 * Various return-from-trap routines.
 */

/*
 * Return from trap.
 * registers are:
 *
 *      [%sp + CC64FSZ + BIAS] => trap frame
 *
 * We must load all global, out, and trap registers from the trap frame.
 *
 * If returning to kernel, we should be at the proper trap level because
 * we don't touch %tl.
 *
 * When returning to user mode, the trap level does not matter, as it
 * will be set explicitly.
 *
 * If we are returning to user code, we must check for register windows in
 * the pcb that belong on the stack, and reload them, if there are any.
 */
return_from_trap:
        ldx     [%sp + CC64FSZ + BIAS + TF_TSTATE], %g1
        btst    TSTATE_PRIV, %g1
        !!
        !! Let all pending interrupts drain before returning to userland
        !!
        bnz,pn  %icc, 1f                                ! Returning to userland?
         nop
        wrpr    %g0, PSTATE_INTR, %pstate
        wrpr    %g0, %g0, %pil                          ! Lower IPL
1:
        wrpr    %g0, PSTATE_KERN, %pstate               ! Disable IRQs

        /* Restore normal globals */
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (1*8)], %g1
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (2*8)], %g2
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (3*8)], %g3
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (4*8)], %g4
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (5*8)], %g5
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (6*8)], %g6
        bnz,pn  %icc, 2f
         nop
        ldx     [%sp + CC64FSZ + BIAS + TF_G + (7*8)], %g7
2:
        ALTERNATE_GLOBALS()

        /* Restore outs */
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (0*8)], %i0
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (1*8)], %i1
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (2*8)], %i2
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (3*8)], %i3
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (4*8)], %i4
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (5*8)], %i5
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (6*8)], %i6
        ldx     [%sp + CC64FSZ + BIAS + TF_O + (7*8)], %i7
        /* Now load trap registers into alternate globals */
        ld      [%sp + CC64FSZ + BIAS + TF_Y], %g4
        ldx     [%sp + CC64FSZ + BIAS + TF_TSTATE], %g1         ! load new values
        wr      %g4, 0, %y
        ldx     [%sp + CC64FSZ + BIAS + TF_PC], %g2
        ldx     [%sp + CC64FSZ + BIAS + TF_NPC], %g3

        /* Returning to user mode or kernel mode? */
        btst    TSTATE_PRIV, %g1                ! returning to userland?
        bz,pt   %icc, rft_user
         nop

/*
 * Return from trap, to kernel.
 *
 * We will assume, for the moment, that all kernel traps are properly stacked
 * in the trap registers, so all we have to do is insert the (possibly modified)
 * register values into the trap registers then do a retry.
 *
 */
rft_kernel:
        rdpr    %tl, %g4                        ! Grab a set of trap registers
        inc     %g4
        wrpr    %g4, %g0, %tl
        wrpr    %g3, 0, %tnpc
        wrpr    %g2, 0, %tpc
        wrpr    %g1, 0, %tstate

        rdpr    %canrestore, %g2
        brnz    %g2, 1f
         nop

        wr      %g0, ASI_NUCLEUS, %asi
        rdpr    %cwp, %g1
        dec     %g1
        wrpr    %g1, %cwp
        FILL    ldxa, %sp+BIAS, 8, %asi
        restored
        inc     %g1
        wrpr    %g1, %cwp
1:
        restore

        rdpr    %tstate, %g1                    ! Since we may have trapped our regs may be toast
        rdpr    %cwp, %g2
        andn    %g1, CWP, %g1
        wrpr    %g1, %g2, %tstate               ! Put %cwp in %tstate
        retry                                   ! We should allow some way to distinguish retry/done
        NOTREACHED

/*
 * Return from trap, to user.  Checks for scheduling trap (`ast') first;
 * will re-enter trap() if set.  Note that we may have to switch from
 * the interrupt stack to the kernel stack in this case.
 *      %g1 = %tstate
 *      %g2 = return %pc
 *      %g3 = return %npc
 * If returning to a valid window, just set psr and return.
 */
rft_user:
        GET_CURPROC(%g7)
        lduw    [%g7 + P_MD_ASTPENDING], %g7    ! want AST trap?
        brnz,pn %g7, softtrap                   ! yes, re-enter trap with type T_AST
         mov    T_AST, %g4

        /*
         * NB: only need to do this after a cache miss
         */
        /*
         * Now check to see if any regs are saved in the pcb and restore them.
         *
         * Here we need to undo the damage caused by switching to a kernel 
         * stack.
         *
         * We will use alternate globals %g4..%g7 because %g1..%g3 are used
         * by the data fault trap handlers and we don't want possible conflict.
         */

        GET_CPCB(%g6)
        ldub    [%g6 + PCB_NSAVED], %g7         ! Any saved reg windows?
        brnz,pn %g7, softtrap
         mov    T_RWRET, %g4

        /*
         * Set up our return trapframe so we can recover if we trap from here
         * on in.
         */
        wrpr    %g0, 1, %tl                     ! Set up the trap state
        wrpr    %g2, 0, %tpc
        wrpr    %g3, 0, %tnpc
        rdpr    %cwp, %g7
        andn    %g1, CWP, %g1
        wrpr    %g1, %g7, %tstate

        rdpr    %otherwin, %g7
        brnz    %g7, 1f
         nop

        /* XXX Rewrite sun4u code to handle faults like sun4v. */
        sethi   %hi(cputyp), %g2
        ld      [%g2 + %lo(cputyp)], %g2
        cmp     %g2, CPU_SUN4V
        bne,pt  %icc, 1f
         nop

        wr      %g0, ASI_AIUS, %asi
        rdpr    %cwp, %g1
        dec     %g1
        wrpr    %g1, 0, %cwp
rft_user_fault_start:
        FILL    ldxa, %sp+BIAS, 8, %asi
        ldx     [%g6 + PCB_WCOOKIE], %g7
        xor     %g7, %i7, %i7           ! stackghost
rft_user_fault_end:
        restored
        inc     %g1
        wrpr    %g1, 0, %cwp

        rdpr    %canrestore, %g7
        wrpr    %g7, 0, %otherwin
        wrpr    %g0, 0, %canrestore
        rdpr    %otherwin, %g7
1:
        wrpr    %g7, 0, %canrestore
        wrpr    %g0, 0, %otherwin
        wrpr    WSTATE_USER, %wstate            ! Need to know where our sp points
        wrpr    %g7, 0, %cleanwin               ! Force cleanup of kernel windows

        restore

        rdpr    %tstate, %g1
        rdpr    %cwp, %g7                       ! Find our cur window
        andn    %g1, CWP, %g1                   ! Clear it from %tstate
        wrpr    %g1, %g7, %tstate               ! Set %tstate with %cwp

        mov     CTX_SECONDARY, %g1              ! Restore the user context
        GET_MMU_CONTEXTID(%g4, %g1)
        mov     CTX_PRIMARY, %g2
        SET_MMU_CONTEXTID(%g4, %g2)
        sethi   %hi(KERNBASE), %g7              ! Should not be needed due to retry
        membar  #Sync                           ! Should not be needed due to retry
        flush   %g7                             ! Should not be needed due to retry

        wrpr    %g0, 0, %pil                    ! Enable all interrupts
        retry

! exported end marker for kernel gdb
        .globl  endtrapcode
endtrapcode:

#ifdef DDB
!!!
!!! Dump the DTLB to phys address in %o0 and print it
!!!
!!! Only toast a few %o registers
!!!
        .globl  dump_dtlb
dump_dtlb:
        clr     %o1
        add     %o1, (64*8), %o3
1:
        ldxa    [%o1] ASI_DMMU_TLB_TAG, %o2
        membar  #Sync
        stx     %o2, [%o0]
        membar  #Sync
        inc     8, %o0
        ldxa    [%o1] ASI_DMMU_TLB_DATA, %o4
        membar  #Sync
        inc     8, %o1
        stx     %o4, [%o0]
        cmp     %o1, %o3
        membar  #Sync
        bl      1b
         inc    8, %o0

        retl
         nop

        .globl  print_dtlb
print_dtlb:
        save    %sp, -CC64FSZ, %sp
        clr     %l1
        add     %l1, (64*8), %l3
        clr     %l2
1:
        ldxa    [%l1] ASI_DMMU_TLB_TAG, %o2
        membar  #Sync
        mov     %l2, %o1
        ldxa    [%l1] ASI_DMMU_TLB_DATA, %o3
        membar  #Sync
        inc     %l2
        set     2f, %o0
        call    db_printf
         inc    8, %l1

        ldxa    [%l1] ASI_DMMU_TLB_TAG, %o2
        membar  #Sync
        mov     %l2, %o1
        ldxa    [%l1] ASI_DMMU_TLB_DATA, %o3
        membar  #Sync
        inc     %l2
        set     3f, %o0
        call    db_printf
         inc    8, %l1

        cmp     %l1, %l3
        bl      1b
         inc    8, %l0

        ret
         restore
        .data
2:
        .asciz  "%2d:%016lx %016lx "
3:
        .asciz  "%2d:%016lx %016lx\r\n"
        .text
#endif  /* DDB */

        .align  8
dostart:
        /*
         * Startup.
         *
         * The Sun FCODE bootloader is nice and loads us where we want
         * to be.  We have a full set of mappings already set up for us.
         *
         * I think we end up having an entire 16M allocated to us.
         *
         * We enter with the prom entry vector in %o0, dvec in %o1,
         * and the bootops vector in %o2.
         *
         * All we need to do is:
         *      1:      Save the prom vector
         *      2:      Create a decent stack for ourselves
         *      3:      Install the permanent 4MB kernel mapping
         *      4:      Call the C language initialization code
         */

        /*
         * Set the psr into a known state:
         * Set supervisor mode, interrupt level >= 13, traps enabled
         */
        wrpr    %g0, 13, %pil
        wrpr    %g0, PSTATE_INTR|PSTATE_PEF, %pstate
        wr      %g0, FPRS_FEF, %fprs            ! Turn on FPU

#if defined(DDB) || NKSYMS > 0
        /*
         * First, check for DDB arguments.  A pointer to an argument
         * is passed in %o1 who's length is passed in %o2.  Our
         * bootloader passes in a magic number as the first argument,
         * followed by esym as argument 2, and ssym as argument 3,
         * so check that %o3 >= 12.
         */
        cmp     %o2, 12
        blt     1f                      ! Not enough args
         nop
        
        set     0x44444230, %l3
        ldx     [%o1], %l4
        cmp     %l3, %l4                ! chk magic
        bne     %xcc, 1f
         nop

        ldx     [%o1+8], %l4
        sethi   %hi(esym), %l3                  ! store esym
        stx     %l4, [%l3 + %lo(esym)]

        ldx     [%o1+16], %l4
        sethi   %hi(ssym), %l3                  ! store ssym
        stx     %l4, [%l3 + %lo(ssym)]
1:
#endif  /* defined(DDB) || NKSYMS > 0 */
        /*
         * Step 1: Save rom entry pointer
         */

        mov     %o4, %g7        ! save prom vector pointer
        set     romp, %o5
        stx     %o4, [%o5]      ! It's initialized data, I hope

        /*
         * Switch to temporary stack.
         */
        set     tmpstack-CC64FSZ-BIAS, %sp

        /*
         * Ready to run C code; finish bootstrap.
         */
1:
        set     0x2000, %o0                     ! fixed: 8192 contexts
        call    bootstrap
         clr    %g4                             ! Clear data segment pointer

        /*
         * pmap_bootstrap should have allocated a stack for proc 0 and
         * stored the start and end in u0 and estack0.  Switch to that
         * stack now.
         */

        sethi   %hi(cpus), %g2
        ldx     [%g2 + %lo(cpus)], %g2
        ldx     [%g2 + CI_PADDR], %g2           ! Load the interrupt stack's PA

/*
 * Initialize a CPU.  This is used both for bootstrapping the first CPU
 * and spinning up each subsequent CPU.  Basically:
 *
 *      Install trap table.
 *      Switch to the initial stack.
 *      Call the routine passed in in cpu_info->ci_spinup.
 */

cpu_initialize:

        wrpr    %g0, 0, %tl                     ! Make sure we're not in NUCLEUS mode
        flushw

        /* Change the trap base register */
        set     trapbase, %l1
#ifdef SUN4V
        sethi   %hi(cputyp), %l0
        ld      [%l0 + %lo(cputyp)], %l0
        cmp     %l0, CPU_SUN4V
        bne,pt  %icc, 1f
         nop
        set     trapbase_sun4v, %l1
        GET_MMFSA(%o1)
1:
#endif
        call    prom_set_trap_table             ! Now we should be running 100% from our handlers
         mov    %l1, %o0
        wrpr    %l1, 0, %tba                    ! Make sure the PROM didn't foul up.
        wrpr    %g0, WSTATE_KERN, %wstate

        /*
         * Switch to our initial stack.
         */

        GET_CPUINFO_VA(%g7)
        ldx     [%g7 + CI_INITSTACK], %l0
        add     %l0, -BIAS-CC64FSZ, %sp

        /*
         * Call our startup routine.
         */

        ldx     [%g7 + CI_SPINUP], %o1
        call    %o1                             ! Call routine
         nop
        NOTREACHED

        set     1f, %o0                         ! Main should never come back here
        call    panic
         nop
        .data
1:
        .asciz  "main() returned\r\n"
        _ALIGN
        .text

ENTRY(sun4u_set_tsbs)

        /* Set the dmmu tsb */
        sethi   %hi(0x1fff), %o2
        set     tsb_dmmu, %o0
        ldx     [%o0], %o0
        set     tsbsize, %o1
        or      %o2, %lo(0x1fff), %o2
        ld      [%o1], %o1
        andn    %o0, %o2, %o0                   ! Mask off size and split bits
        or      %o0, %o1, %o0                   ! Make a TSB pointer
        set     TSB, %o2
        stxa    %o0, [%o2] ASI_DMMU             ! Install data TSB pointer
        membar  #Sync

        /* Set the immu tsb */
        sethi   %hi(0x1fff), %o2
        set     tsb_immu, %o0
        ldx     [%o0], %o0
        set     tsbsize, %o1
        or      %o2, %lo(0x1fff), %o2
        ld      [%o1], %o1
        andn    %o0, %o2, %o0                   ! Mask off size and split bits
        or      %o0, %o1, %o0                   ! Make a TSB pointer
        set     TSB, %o2
        stxa    %o0, [%o2] ASI_IMMU             ! Install insn TSB pointer
        membar  #Sync

        retl
         nop
END(sun4u_set_tsbs)


#ifdef MULTIPROCESSOR
ENTRY(cpu_mp_startup)
        mov     %o0, %g2

        wrpr    %g0, 13, %pil
        wrpr    %g0, PSTATE_INTR|PSTATE_PEF, %pstate
        wr      %g0, FPRS_FEF, %fprs            ! Turn on FPU

        set     tmpstack-CC64FSZ-BIAS, %sp

        call    pmap_bootstrap_cpu
         nop

        ba,a,pt %xcc, cpu_initialize
         nop
END(cpu_mp_startup)
#endif

/*
 * openfirmware(cell* param);
 *
 * OpenFirmware entry point
 */
        .align 8
NENTRY(openfirmware)
        sethi   %hi(romp), %o4
        ldx     [%o4+%lo(romp)], %o4
        save    %sp, -CC64FSZ, %sp
        rdpr    %pil, %i2
        mov     PIL_HIGH, %i3
        cmp     %i3, %i2
        movle   %icc, %i2, %i3
        wrpr    %g0, %i3, %pil
        mov     %i0, %o0
        mov     %g1, %l1
        mov     %g2, %l2
        mov     %g3, %l3
        mov     %g4, %l4
        mov     %g5, %l5
        mov     %g6, %l6
        mov     %g7, %l7
        rdpr    %pstate, %l0
        jmpl    %i4, %o7
         wrpr   %g0, PSTATE_PROM|PSTATE_IE, %pstate
        wrpr    %l0, %g0, %pstate
        mov     %l1, %g1
        mov     %l2, %g2
        mov     %l3, %g3
        mov     %l4, %g4
        mov     %l5, %g5
        mov     %l6, %g6
        mov     %l7, %g7
        wrpr    %i2, 0, %pil
        ret
         restore        %o0, %g0, %o0
END(openfirmware)

/*
 * tlb_flush_pte(vaddr_t va, int ctx)
 *
 * Flush tte from both IMMU and DMMU.
 *
 */
        .align 8
NENTRY(us_tlb_flush_pte)
        mov     CTX_SECONDARY, %o2
        andn    %o0, 0xfff, %g2                         ! drop unused va bits
        ldxa    [%o2] ASI_DMMU, %g1                     ! Save secondary context
        sethi   %hi(KERNBASE), %o4
        membar  #LoadStore
        stxa    %o1, [%o2] ASI_DMMU                     ! Insert context to demap
        membar  #Sync
        or      %g2, DEMAP_PAGE_SECONDARY, %g2          ! Demap page from secondary context only
        stxa    %g0, [%g2] ASI_DMMU_DEMAP               ! Do the demap
        stxa    %g0, [%g2] ASI_IMMU_DEMAP               ! to both TLBs
        membar  #Sync                                   ! No real reason for this XXXX
        flush   %o4
        stxa    %g1, [%o2] ASI_DMMU                     ! Restore asi
        membar  #Sync                                   ! No real reason for this XXXX
        flush   %o4
        retl
         nop
END(us_tlb_flush_pte)

        .align 8
NENTRY(us3_tlb_flush_pte)
        rdpr    %pstate, %o5
        andn    %o5, PSTATE_IE, %o4
        wrpr    %o4, %pstate                            ! disable interrupts

        rdpr    %tl, %o3
        brnz    %o3, 1f
         add    %o3, 1, %g2
        wrpr    %g0, %g2, %tl                           ! Switch to traplevel > 0
1:      
        mov     CTX_PRIMARY, %o2
        andn    %o0, 0xfff, %g2                         ! drop unused va bits
        ldxa    [%o2] ASI_DMMU, %g1                     ! Save primary context
        sethi   %hi(KERNBASE), %o4
        membar  #LoadStore
        stxa    %o1, [%o2] ASI_DMMU                     ! Insert context to demap
        membar  #Sync
        or      %g2, DEMAP_PAGE_PRIMARY, %g2            ! Demap page from primary context only
        stxa    %g0, [%g2] ASI_DMMU_DEMAP               ! Do the demap
        stxa    %g0, [%g2] ASI_IMMU_DEMAP               ! to both TLBs
        membar  #Sync                                   ! No real reason for this XXXX
        flush   %o4
        stxa    %g1, [%o2] ASI_DMMU                     ! Restore asi
        membar  #Sync                                   ! No real reason for this XXXX
        flush   %o4
        wrpr    %g0, %o3, %tl                           ! Restore traplevel
        wrpr    %o5, %pstate                            ! Restore interrupts
        retl
         nop
END(us_tlb_flush_pte)

/*
 * tlb_flush_ctx(int ctx)
 *
 * Flush entire context from both IMMU and DMMU.
 *
 */
        .align 8
NENTRY(us_tlb_flush_ctx)
        mov     CTX_SECONDARY, %o2
        sethi   %hi(KERNBASE), %o4
        ldxa    [%o2] ASI_DMMU, %g1             ! Save secondary context
        membar  #LoadStore
        stxa    %o0, [%o2] ASI_DMMU             ! Insert context to demap
        membar  #Sync
        set     DEMAP_CTX_SECONDARY, %g2        ! Demap context from secondary context only
        stxa    %g0, [%g2] ASI_DMMU_DEMAP               ! Do the demap
        stxa    %g0, [%g2] ASI_IMMU_DEMAP               ! Do the demap
        membar  #Sync
        stxa    %g1, [%o2] ASI_DMMU             ! Restore secondary asi
        membar  #Sync                                   ! No real reason for this XXXX
        flush   %o4
        retl
         nop
END(us_tlb_flush_ctx)

        .align 8
NENTRY(us3_tlb_flush_ctx)
        rdpr    %pstate, %o5
        andn    %o5, PSTATE_IE, %o4
        wrpr    %o4, %pstate                            ! disable interrupts

        rdpr    %tl, %o3
        brnz    %o3, 1f
         add    %o3, 1, %g2
        wrpr    %g0, %g2, %tl                           ! Switch to traplevel > 0
1:      
        mov     CTX_PRIMARY, %o2
        sethi   %hi(KERNBASE), %o4
        ldxa    [%o2] ASI_DMMU, %g1             ! Save primary context
        membar  #LoadStore
        stxa    %o0, [%o2] ASI_DMMU             ! Insert context to demap
        membar  #Sync
        set     DEMAP_CTX_PRIMARY, %g2          ! Demap context from primary context only
        stxa    %g0, [%g2] ASI_DMMU_DEMAP               ! Do the demap
        stxa    %g0, [%g2] ASI_IMMU_DEMAP               ! Do the demap
        membar  #Sync
        stxa    %g1, [%o2] ASI_DMMU             ! Restore secondary asi
        membar  #Sync                                   ! No real reason for this XXXX
        flush   %o4
        wrpr    %g0, %o3, %tl                           ! Restore traplevel
        wrpr    %o5, %pstate                            ! Restore interrupts
        retl
         nop
END(us3_tlb_flush_ctx)

/*
 * dcache_flush_page(paddr_t pa)
 *
 * Clear one page from D$.
 *
 */
        .align 8
NENTRY(us_dcache_flush_page)
        mov     -1, %o1         ! Generate mask for tag: bits [29..2]
        srlx    %o0, 13-2, %o2  ! Tag is VA bits <40:13> in bits <29:2>
        clr     %o4
        srl     %o1, 2, %o1     ! Now we have bits <29:0> set
        set     (2*NBPG), %o5
        ba,pt   %icc, 1f
         andn   %o1, 3, %o1     ! Now we have bits <29:2> set
        
        .align 8
1:
        ldxa    [%o4] ASI_DCACHE_TAG, %o3
        mov     %o4, %o0
        deccc   16, %o5
        bl,pn   %icc, 2f
        
         inc    16, %o4
        xor     %o3, %o2, %o3
        andcc   %o3, %o1, %g0
        bne,pt  %xcc, 1b
         membar #LoadStore
        
dlflush2:
        stxa    %g0, [%o0] ASI_DCACHE_TAG
        ba,pt   %icc, 1b
         membar #StoreLoad
2:

        wr      %g0, ASI_PRIMARY_NOFAULT, %asi
        sethi   %hi(KERNBASE), %o5
        flush   %o5
        retl
         membar #Sync
END(us_dcache_flush_page)

        .align 8
NENTRY(us3_dcache_flush_page)
        ldxa    [%g0] ASI_MCCR, %o1
        btst    MCCR_DCACHE_EN, %o1
        bz,pn   %icc, 1f
         nop
        sethi   %hi(PAGE_SIZE), %o4
        or      %g0, (PAGE_SIZE - 1), %o3
        andn    %o0, %o3, %o0
2:
        subcc   %o4, 32, %o4
        stxa    %g0, [%o0 + %o4] ASI_DCACHE_INVALIDATE
        membar  #Sync
        bne,pt  %icc, 2b
         nop
1:
        retl
         nop
END(us3_dcache_flush_page)

        .globl no_dcache_flush_page
ENTRY(no_dcache_flush_page)
        retl
         nop
END(no_dcache_flush_page)

/*
 * cache_flush_virt(va, len)
 *
 * Clear everything in that va range from D$.
 *
 */
        .align 8
NENTRY(cache_flush_virt)
        brz,pn  %o1, 2f         ! What? nothing to clear?
         add    %o0, %o1, %o2
        mov     0x1ff, %o3
        sllx    %o3, 5, %o3     ! Generate mask for VA bits
        and     %o0, %o3, %o0
        and     %o2, %o3, %o2
        sub     %o2, %o1, %o4   ! End < start? need to split flushes.
        brlz,pn %o4, 1f
         movrz  %o4, %o3, %o4   ! If start == end we need to wrap

        !! Clear from start to end
1:
dlflush3:
        stxa    %g0, [%o0] ASI_DCACHE_TAG
        dec     16, %o4
        brgz,pt %o4, 1b
         inc    16, %o0
2:
        sethi   %hi(KERNBASE), %o5
        flush   %o5
        membar  #Sync
        retl
         nop
END(cache_flush_virt)

/*
 * The following code is copied to a dedicated page,
 * and signals are `trampolined' off it.
 *
 * When this code is run, the stack looks like:
 *      [%sp]                   128 bytes to which registers can be dumped
 *      [%sp + 128]             signal number (goes in %o0)
 *      [%sp + 128 + 4]         signal code (ignored)
 *      [%sp + 128 + 8]         siginfo pointer(goes in %o1)
 *      [%sp + 128 + 16]        first word of saved state (sigcontext)
 *          .
 *          .
 *          .
 *      [%sp + NNN]             last word of saved state
 *      [%sp + ...]             siginfo structure
 * (followed by previous stack contents or top of signal stack).
 * The address of the function to call is in %g1; the old %g1 and %o0
 * have already been saved in the sigcontext.  We are running in a clean
 * window, all previous windows now being saved to the stack.
 */
        .section .rodata
        .globl  sigcode
sigcode:
        /*
         * XXX  the `save' and `restore' below are unnecessary: should
         *      replace with simple arithmetic on %sp
         *
         * Make room on the stack for 64 %f registers + %fsr.  This comes
         * out to 64*4+8 or 264 bytes, but this must be aligned to a multiple
         * of 64, or 320 bytes.
         */
        save    %sp, -CC64FSZ - 320, %sp
        mov     %g2, %l2                ! save globals in %l registers
        mov     %g3, %l3
        mov     %g4, %l4
        mov     %g5, %l5
        mov     %g6, %l6
        mov     %g7, %l7
        /*
         * Saving the fpu registers is expensive, so do it iff it is
         * enabled and dirty.
         */
        rd      %fprs, %l0
        btst    FPRS_DL|FPRS_DU, %l0    ! All clean?
        bz,pt   %icc, 2f
         btst   FPRS_DL, %l0            ! test dl
        bz,pt   %icc, 1f
         btst   FPRS_DU, %l0            ! test du

        ! fpu is enabled, oh well
        stx     %fsr, [%sp + CC64FSZ + BIAS + 0]
        add     %sp, BIAS+CC64FSZ+BLOCK_SIZE, %l0       ! Generate a pointer so we can
        andn    %l0, BLOCK_ALIGN, %l0   ! do a block store
        stda    %f0, [%l0] ASI_BLK_P
        inc     BLOCK_SIZE, %l0
        stda    %f16, [%l0] ASI_BLK_P
1:
        bz,pt   %icc, 2f
         add    %sp, BIAS+CC64FSZ+BLOCK_SIZE, %l0       ! Generate a pointer so we can
        andn    %l0, BLOCK_ALIGN, %l0   ! do a block store
        add     %l0, 2*BLOCK_SIZE, %l0  ! and skip what we already stored
        stda    %f32, [%l0] ASI_BLK_P
        inc     BLOCK_SIZE, %l0
        stda    %f48, [%l0] ASI_BLK_P
2:
        membar  #Sync
        rd      %fprs, %l0              ! reload fprs copy, for checking after
        rd      %y, %l1                 ! in any case, save %y
        lduw    [%fp + BIAS + 128], %o0 ! sig
        ldx     [%fp + BIAS + 128 + 8], %o1     ! siginfo
        call    %g1                     ! (*sa->sa_handler)(sig, sip, scp)
         add    %fp, BIAS + 128 + 16, %o2       ! scp
        wr      %l1, %g0, %y            ! in any case, restore %y

        /*
         * Now that the handler has returned, re-establish all the state
         * we just saved above, then do a sigreturn.
         */
        btst    FPRS_DL|FPRS_DU, %l0    ! All clean?
        bz,pt   %icc, 2f
         btst   FPRS_DL, %l0            ! test dl
        bz,pt   %icc, 1f
         btst   FPRS_DU, %l0            ! test du

        ldx     [%sp + CC64FSZ + BIAS + 0], %fsr
        add     %sp, BIAS+CC64FSZ+BLOCK_SIZE, %l0       ! Generate a pointer so we can
        andn    %l0, BLOCK_ALIGN, %l0   ! do a block load
        ldda    [%l0] ASI_BLK_P, %f0
        inc     BLOCK_SIZE, %l0
        ldda    [%l0] ASI_BLK_P, %f16
1:
        bz,pt   %icc, 2f
         nop
        add     %sp, BIAS+CC64FSZ+BLOCK_SIZE, %l0       ! Generate a pointer so we can
        andn    %l0, BLOCK_ALIGN, %l0   ! do a block load
        inc     2*BLOCK_SIZE, %l0       ! and skip what we already loaded
        ldda    [%l0] ASI_BLK_P, %f32
        inc     BLOCK_SIZE, %l0
        ldda    [%l0] ASI_BLK_P, %f48
2:
        mov     %l2, %g2
        mov     %l3, %g3
        mov     %l4, %g4
        mov     %l5, %g5
        mov     %l6, %g6
        mov     %l7, %g7
        membar  #Sync

        restore %g0, SYS_sigreturn, %g1 ! get registers back & set syscall #
        add     %sp, BIAS + 128 + 16, %o0       ! compute scp
        .globl  sigcoderet
sigcoderet:
        .globl  sigcodecall
sigcodecall:
        t       ST_SYSCALL              ! sigreturn(scp)
        ! sigreturn does not return unless it fails
        .globl  esigcode
esigcode:
        /* FALLTHROUGH */
        .globl  sigfill
sigfill:
        unimp
esigfill:

        .globl  sigfillsiz
sigfillsiz:
        .word   esigfill - sigfill

        .text

/*
 * Primitives
 */

/*
 * getfp() - get unbiased stack frame pointer
 */
ENTRY(getfp)
        mov %fp, %o0
        retl
         add    %o0, BIAS, %o0

/*
 * _copyinstr(fromaddr, toaddr, maxlength, &lencopied)
 *
 * Copy a null terminated string from the user address space into
 * the kernel address space.
 */
ENTRY(_copyinstr)
        ! %o0 = fromaddr, %o1 = toaddr, %o2 = maxlen, %o3 = &lencopied
        brgz,pt %o2, 1f                                 ! Make sure len is valid
         nop
        retl
         mov    ENAMETOOLONG, %o0
1:
        GET_CPCB(%o4)                   ! catch faults
        set     Lcsfault, %o5
        membar  #Sync
        stx     %o5, [%o4 + PCB_ONFAULT]

        mov     %o1, %o5                !       save = toaddr;
! XXX should do this in bigger chunks when possible
0:                                      ! loop:
        ldsba   [%o0] ASI_AIUS, %g1     !       c = *fromaddr;
        stb     %g1, [%o1]              !       *toaddr++ = c;
        inc     %o1
        brz,a,pn        %g1, Lcsdone    !       if (c == NULL)
         clr    %o0                     !               { error = 0; done; }
        deccc   %o2                     !       if (--len > 0) {
        bg,pt   %icc, 0b                !               fromaddr++;
         inc    %o0                     !               goto loop;
        ba,pt   %xcc, Lcsdone           !       }
         mov    ENAMETOOLONG, %o0       !       error = ENAMETOOLONG;
        NOTREACHED

/*
 * copyoutstr(fromaddr, toaddr, maxlength, &lencopied)
 *
 * Copy a null terminated string from the kernel
 * address space to the user address space.
 */
ENTRY(copyoutstr)
        ! %o0 = fromaddr, %o1 = toaddr, %o2 = maxlen, %o3 = &lencopied
        brgz,pt %o2, 1f                                 ! Make sure len is valid
         nop
        retl
         mov    ENAMETOOLONG, %o0
1:
        GET_CPCB(%o4)                   ! catch faults
        set     Lcsfault, %o5
        membar  #Sync
        stx     %o5, [%o4 + PCB_ONFAULT]

        mov     %o1, %o5                !       save = toaddr;
! XXX should do this in bigger chunks when possible
0:                                      ! loop:
        ldsb    [%o0], %g1              !       c = *fromaddr;
        stba    %g1, [%o1] ASI_AIUS     !       *toaddr++ = c;
        inc     %o1
        brz,a,pn        %g1, Lcsdone    !       if (c == NULL)
         clr    %o0                     !               { error = 0; done; }
        deccc   %o2                     !       if (--len > 0) {
        bg,pt   %icc, 0b                !               fromaddr++;
         inc    %o0                     !               goto loop;
                                        !       }
        mov     ENAMETOOLONG, %o0       !       error = ENAMETOOLONG;
Lcsdone:                                ! done:
        sub     %o1, %o5, %o1           !       len = to - save;
        brnz,a  %o3, 1f                 !       if (lencopied)
         stx    %o1, [%o3]              !               *lencopied = len;
1:
        retl                            ! cpcb->pcb_onfault = 0;
         stx    %g0, [%o4 + PCB_ONFAULT]! return (error);

Lcsfault:
        b       Lcsdone                 ! error = EFAULT;
         mov    EFAULT, %o0             ! goto ret;

/*
 * copyin(src, dst, len)
 *
 * Copy specified amount of data from user space into the kernel.
 *
 * This is a modified version of bcopy that uses ASI_AIUS.  When
 * bcopy is optimized to use block copy ASIs, this should be also.
 */

#define BCOPY_SMALL     32      /* if < 32, copy by bytes */

ENTRY(_copyin)
!       flushw                  ! Make sure we don't have stack probs & lose hibits of %o
        GET_CPCB(%o3)
        wr      %g0, ASI_AIUS, %asi
        set     Lcopyfault, %o4
        membar  #Sync
        stx     %o4, [%o3 + PCB_ONFAULT]
        cmp     %o2, BCOPY_SMALL
Lcopyin_start:
        bge,a   Lcopyin_fancy   ! if >= this many, go be fancy.
         btst   7, %o0          ! (part of being fancy)

        /*
         * Not much to copy, just do it a byte at a time.
         */
        deccc   %o2             ! while (--len >= 0)
        bl      1f
0:
         inc    %o0
        ldsba   [%o0 - 1] %asi, %o4!    *dst++ = (++src)[-1];
        stb     %o4, [%o1]
        deccc   %o2
        bge     0b
         inc    %o1
1:
        ba      Lcopyin_done
         clr    %o0
        NOTREACHED

        /*
         * Plenty of data to copy, so try to do it optimally.
         */
Lcopyin_fancy:
        ! check for common case first: everything lines up.
!       btst    7, %o0          ! done already
        bne     1f
!        XXX check no delay slot
        btst    7, %o1
        be,a    Lcopyin_doubles
         dec    8, %o2          ! if all lined up, len -= 8, goto copyin_doubes

        ! If the low bits match, we can make these line up.
1:
        xor     %o0, %o1, %o3   ! t = src ^ dst;
        btst    1, %o3          ! if (t & 1) {
        be,a    1f
         btst   1, %o0          ! [delay slot: if (src & 1)]

        ! low bits do not match, must copy by bytes.
0:
        ldsba   [%o0] %asi, %o4 !       do {
        inc     %o0             !               (++dst)[-1] = *src++;
        inc     %o1
        deccc   %o2
        bnz     0b              !       } while (--len != 0);
         stb    %o4, [%o1 - 1]
        ba      Lcopyin_done
         clr    %o0
        NOTREACHED

        ! lowest bit matches, so we can copy by words, if nothing else
1:
        be,a    1f              ! if (src & 1) {
         btst   2, %o3          ! [delay slot: if (t & 2)]

        ! although low bits match, both are 1: must copy 1 byte to align
        ldsba   [%o0] %asi, %o4 !       *dst++ = *src++;
        stb     %o4, [%o1]
        inc     %o0
        inc     %o1
        dec     %o2             !       len--;
        btst    2, %o3          ! } [if (t & 2)]
1:
        be,a    1f              ! if (t & 2) {
         btst   2, %o0          ! [delay slot: if (src & 2)]
        dec     2, %o2          !       len -= 2;
0:
        ldsha   [%o0] %asi, %o4 !       do {
        sth     %o4, [%o1]      !               *(short *)dst = *(short *)src;
        inc     2, %o0          !               dst += 2, src += 2;
        deccc   2, %o2          !       } while ((len -= 2) >= 0);
        bge     0b
         inc    2, %o1
        b       Lcopyin_mopb    !       goto mop_up_byte;
         btst   1, %o2          ! } [delay slot: if (len & 1)]
        NOTREACHED

        ! low two bits match, so we can copy by longwords
1:
        be,a    1f              ! if (src & 2) {
         btst   4, %o3          ! [delay slot: if (t & 4)]

        ! although low 2 bits match, they are 10: must copy one short to align
        ldsha   [%o0] %asi, %o4 !       (*short *)dst = *(short *)src;
        sth     %o4, [%o1]
        inc     2, %o0          !       dst += 2;
        inc     2, %o1          !       src += 2;
        dec     2, %o2          !       len -= 2;
        btst    4, %o3          ! } [if (t & 4)]
1:
        be,a    1f              ! if (t & 4) {
         btst   4, %o0          ! [delay slot: if (src & 4)]
        dec     4, %o2          !       len -= 4;
0:
        lduwa   [%o0] %asi, %o4 !       do {
        st      %o4, [%o1]      !               *(int *)dst = *(int *)src;
        inc     4, %o0          !               dst += 4, src += 4;
        deccc   4, %o2          !       } while ((len -= 4) >= 0);
        bge     0b
         inc    4, %o1
        b       Lcopyin_mopw    !       goto mop_up_word_and_byte;
         btst   2, %o2          ! } [delay slot: if (len & 2)]
        NOTREACHED

        ! low three bits match, so we can copy by doublewords
1:
        be      1f              ! if (src & 4) {
         dec    8, %o2          ! [delay slot: len -= 8]
        lduwa   [%o0] %asi, %o4 !       *(int *)dst = *(int *)src;
        st      %o4, [%o1]
        inc     4, %o0          !       dst += 4, src += 4, len -= 4;
        inc     4, %o1
        dec     4, %o2          ! }
1:
Lcopyin_doubles:
        ldxa    [%o0] %asi, %g1 ! do {
        stx     %g1, [%o1]      !       *(double *)dst = *(double *)src;
        inc     8, %o0          !       dst += 8, src += 8;
        deccc   8, %o2          ! } while ((len -= 8) >= 0);
        bge     Lcopyin_doubles
         inc    8, %o1

        ! check for a usual case again (save work)
        btst    7, %o2          ! if ((len & 7) == 0)
        be      Lcopyin_done    !       goto copyin_done;

         btst   4, %o2          ! if ((len & 4) == 0)
        be,a    Lcopyin_mopw    !       goto mop_up_word_and_byte;
         btst   2, %o2          ! [delay slot: if (len & 2)]
        lduwa   [%o0] %asi, %o4 !       *(int *)dst = *(int *)src;
        st      %o4, [%o1]
        inc     4, %o0          !       dst += 4;
        inc     4, %o1          !       src += 4;
        btst    2, %o2          ! } [if (len & 2)]

1:
        ! mop up trailing word (if present) and byte (if present).
Lcopyin_mopw:
        be      Lcopyin_mopb    ! no word, go mop up byte
         btst   1, %o2          ! [delay slot: if (len & 1)]
        ldsha   [%o0] %asi, %o4 ! *(short *)dst = *(short *)src;
        be      Lcopyin_done    ! if ((len & 1) == 0) goto done;
         sth    %o4, [%o1]
        ldsba   [%o0 + 2] %asi, %o4     ! dst[2] = src[2];
        stb     %o4, [%o1 + 2]
        ba      Lcopyin_done
         clr    %o0
        NOTREACHED

        ! mop up trailing byte (if present).
Lcopyin_mopb:
        be,a    Lcopyin_done
         nop
        ldsba   [%o0] %asi, %o4
        stb     %o4, [%o1]

Lcopyin_done:
        GET_CPCB(%o3)
        membar  #Sync
        stx     %g0, [%o3 + PCB_ONFAULT]
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore ASI
        retl
         clr    %o0                     ! return 0
END(_copyin)

/*
 * copyout(src, dst, len)
 *
 * Copy specified amount of data from kernel to user space.
 * Just like copyin, except that the `dst' addresses are user space
 * rather than the `src' addresses.
 *
 * This is a modified version of bcopy that uses ASI_AIUS.  When
 * bcopy is optimized to use block copy ASIs, this should be also.
 */
ENTRY(copyout)
Ldocopy:
        GET_CPCB(%o3)
        wr      %g0, ASI_AIUS, %asi
        set     Lcopyfault, %o4
        membar  #Sync
        stx     %o4, [%o3 + PCB_ONFAULT]
        cmp     %o2, BCOPY_SMALL
Lcopyout_start:
        membar  #StoreStore
        bge,a   Lcopyout_fancy  ! if >= this many, go be fancy.
         btst   7, %o0          ! (part of being fancy)

        /*
         * Not much to copy, just do it a byte at a time.
         */
        deccc   %o2             ! while (--len >= 0)
        bl      1f
!        XXX check no delay slot
0:
        inc     %o0
        ldsb    [%o0 - 1], %o4! (++dst)[-1] = *src++;
        stba    %o4, [%o1] %asi
        deccc   %o2
        bge     0b
         inc    %o1
1:
        ba      Lcopyout_done
         clr    %o0
        NOTREACHED

        /*
         * Plenty of data to copy, so try to do it optimally.
         */
Lcopyout_fancy:
        ! check for common case first: everything lines up.
!       btst    7, %o0          ! done already
        bne     1f
!        XXX check no delay slot
        btst    7, %o1
        be,a    Lcopyout_doubles
         dec    8, %o2          ! if all lined up, len -= 8, goto copyout_doubes

        ! If the low bits match, we can make these line up.
1:
        xor     %o0, %o1, %o3   ! t = src ^ dst;
        btst    1, %o3          ! if (t & 1) {
        be,a    1f
         btst   1, %o0          ! [delay slot: if (src & 1)]

        ! low bits do not match, must copy by bytes.
0:
        ldsb    [%o0], %o4      !       do {
        inc     %o0             !               (++dst)[-1] = *src++;
        inc     %o1
        deccc   %o2
        bnz     0b              !       } while (--len != 0);
         stba   %o4, [%o1 - 1] %asi
        ba      Lcopyout_done
         clr    %o0
        NOTREACHED

        ! lowest bit matches, so we can copy by words, if nothing else
1:
        be,a    1f              ! if (src & 1) {
         btst   2, %o3          ! [delay slot: if (t & 2)]

        ! although low bits match, both are 1: must copy 1 byte to align
        ldsb    [%o0], %o4      !       *dst++ = *src++;
        stba    %o4, [%o1] %asi
        inc     %o0
        inc     %o1
        dec     %o2             !       len--;
        btst    2, %o3          ! } [if (t & 2)]
1:
        be,a    1f              ! if (t & 2) {
         btst   2, %o0          ! [delay slot: if (src & 2)]
        dec     2, %o2          !       len -= 2;
0:
        ldsh    [%o0], %o4      !       do {
        stha    %o4, [%o1] %asi !               *(short *)dst = *(short *)src;
        inc     2, %o0          !               dst += 2, src += 2;
        deccc   2, %o2          !       } while ((len -= 2) >= 0);
        bge     0b
         inc    2, %o1
        b       Lcopyout_mopb   !       goto mop_up_byte;
         btst   1, %o2          ! } [delay slot: if (len & 1)]
        NOTREACHED

        ! low two bits match, so we can copy by longwords
1:
        be,a    1f              ! if (src & 2) {
         btst   4, %o3          ! [delay slot: if (t & 4)]

        ! although low 2 bits match, they are 10: must copy one short to align
        ldsh    [%o0], %o4      !       (*short *)dst = *(short *)src;
        stha    %o4, [%o1] %asi
        inc     2, %o0          !       dst += 2;
        inc     2, %o1          !       src += 2;
        dec     2, %o2          !       len -= 2;
        btst    4, %o3          ! } [if (t & 4)]
1:
        be,a    1f              ! if (t & 4) {
         btst   4, %o0          ! [delay slot: if (src & 4)]
        dec     4, %o2          !       len -= 4;
0:
        lduw    [%o0], %o4      !       do {
        sta     %o4, [%o1] %asi !               *(int *)dst = *(int *)src;
        inc     4, %o0          !               dst += 4, src += 4;
        deccc   4, %o2          !       } while ((len -= 4) >= 0);
        bge     0b
         inc    4, %o1
        b       Lcopyout_mopw   !       goto mop_up_word_and_byte;
         btst   2, %o2          ! } [delay slot: if (len & 2)]
        NOTREACHED

        ! low three bits match, so we can copy by doublewords
1:
        be      1f              ! if (src & 4) {
         dec    8, %o2          ! [delay slot: len -= 8]
        lduw    [%o0], %o4      !       *(int *)dst = *(int *)src;
        sta     %o4, [%o1] %asi
        inc     4, %o0          !       dst += 4, src += 4, len -= 4;
        inc     4, %o1
        dec     4, %o2          ! }
1:
Lcopyout_doubles:
        ldx     [%o0], %g1      ! do {
        stxa    %g1, [%o1] %asi !       *(double *)dst = *(double *)src;
        inc     8, %o0          !       dst += 8, src += 8;
        deccc   8, %o2          ! } while ((len -= 8) >= 0);
        bge     Lcopyout_doubles
         inc    8, %o1

        ! check for a usual case again (save work)
        btst    7, %o2          ! if ((len & 7) == 0)
        be      Lcopyout_done   !       goto copyout_done;

         btst   4, %o2          ! if ((len & 4) == 0)
        be,a    Lcopyout_mopw   !       goto mop_up_word_and_byte;
         btst   2, %o2          ! [delay slot: if (len & 2)]
        lduw    [%o0], %o4      !       *(int *)dst = *(int *)src;
        sta     %o4, [%o1] %asi
        inc     4, %o0          !       dst += 4;
        inc     4, %o1          !       src += 4;
        btst    2, %o2          ! } [if (len & 2)]

1:
        ! mop up trailing word (if present) and byte (if present).
Lcopyout_mopw:
        be      Lcopyout_mopb   ! no word, go mop up byte
         btst   1, %o2          ! [delay slot: if (len & 1)]
        ldsh    [%o0], %o4      ! *(short *)dst = *(short *)src;
        be      Lcopyout_done   ! if ((len & 1) == 0) goto done;
         stha   %o4, [%o1] %asi
        ldsb    [%o0 + 2], %o4  ! dst[2] = src[2];
        stba    %o4, [%o1 + 2] %asi
        ba      Lcopyout_done
         clr    %o0
        NOTREACHED

        ! mop up trailing byte (if present).
Lcopyout_mopb:
        be,a    Lcopyout_done
         nop
        ldsb    [%o0], %o4
        stba    %o4, [%o1] %asi

Lcopyout_done:
        GET_CPCB(%o3)
        membar  #Sync
        stx     %g0, [%o3 + PCB_ONFAULT]
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore ASI
        membar  #StoreStore|#StoreLoad
        retl                    ! New instr
         clr    %o0                     ! return 0
END(copyout)

ENTRY(copyin32)
        andcc   %o0, 0x3, %g0
        bnz,pn  %xcc, Lcopyfault
         nop
        GET_CPCB(%o3)
        set     Lcopyfault, %o4
        membar  #Sync
        stx     %o4, [%o3 + PCB_ONFAULT]
        lduwa   [%o0] ASI_AIUS, %o2
        stw     %o2, [%o1]
        membar  #Sync
        stx     %g0, [%o3 + PCB_ONFAULT]
        retl
         clr    %o0
END(copyin32)

! Copyin or copyout fault.  Clear cpcb->pcb_onfault and return EFAULT.
Lcopyfault:
        GET_CPCB(%o3)
        stx     %g0, [%o3 + PCB_ONFAULT]
        membar  #StoreStore|#StoreLoad
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore ASI
        retl
         mov    EFAULT, %o0

/*
 * cpu_switchto(struct proc *old, struct proc *new)
 *
 * Save the context of "old" and switch to "new".
 */
ENTRY(cpu_switchto)
        save    %sp, -CC64FSZ, %sp
        rdpr    %pstate, %o1            ! oldpstate = %pstate;
        wrpr    %g0, PSTATE_INTR, %pstate ! make sure we're on normal globals

        ldx     [%g7 + CI_CPCB], %l5

        /*
         * Register usage:
         *
         *      %i0 = oldproc
         *      %i1 = newproc
         *      %l1 = newpcb
         *      %l2 = newpstate
         *      %l5 = cpcb
         *      %o0 = tmp 1
         *      %o1 = oldpstate
         *      %o2 = tmp 2
         *      %o3 = vm
         *      %o4 = sswap
         *      %o5 = <free>
         */

        /*
         * Committed to running process p.
         */
#if defined(MULTIPROCESSOR)
        /*
         * p->p_cpu = curcpu();
         */
        ldx     [%g7 + CI_SELF], %o0
        stx     %o0, [%i1 + P_CPU]
#endif  /* defined(MULTIPROCESSOR) */
        mov     SONPROC, %o0                    ! newproc->p_stat = SONPROC
        stb     %o0, [%i1 + P_STAT]
        ldx     [%i1 + P_ADDR], %l1             ! newpcb = newproc->p_addr;

        flushw                          ! save all register windows except this one

        /*
         * Save the old process, if any; then load p.
         */
        brz,pn  %i0, Lsw_load           ! if no old process, go load
         wrpr   %g0, PSTATE_KERN, %pstate

        stx     %i6, [%l5 + PCB_SP]     ! cpcb->pcb_sp = sp;
        stx     %i7, [%l5 + PCB_PC]     ! cpcb->pcb_pc = pc;
        sth     %o1, [%l5 + PCB_PSTATE] ! cpcb->pcb_pstate = oldpstate;
        rdpr    %cwp, %o2               ! Useless
        stb     %o2, [%l5 + PCB_CWP]

        /*
         * Load the new process.  To load, we must change stacks and
         * alter cpcb and the window control registers, hence we must
         * disable interrupts.
         *
         * We also must load up the `in' and `local' registers.
         */
Lsw_load:
        /* set new cpcb and cpcbpaddr */
        stx     %i1, [%g7 + CI_CURPROC]         ! curproc = newproc;
        ldx     [%i1 + P_MD_PCBPADDR], %o2
        stx     %l1, [%g7 + CI_CPCB]            ! cpcb = newpcb;
        stx     %o2, [%g7 + CI_CPCBPADDR]

        ldx     [%l1 + PCB_SP], %i6
        ldx     [%l1 + PCB_PC], %i7

        /* finally, enable traps */
        wrpr    %g0, PSTATE_INTR, %pstate

        /*
         * Now running p.  Make sure it has a context so that it
         * can talk about user space stuff.  (Its pcb_uw is currently
         * zero so it is safe to have interrupts going here.)
         */
        ldx     [%i1 + P_VMSPACE], %o3          ! vm = newproc->p_vmspace;
        sethi   %hi(kernel_pmap_), %o1
        mov     CTX_SECONDARY, %l5              ! Recycle %l5
        ldx     [%o3 + VM_PMAP], %o2            ! if (vm->vm_pmap != kernel_pmap_)
        or      %o1, %lo(kernel_pmap_), %o1
        cmp     %o2, %o1
        bz,pn   %xcc, Lsw_havectx               ! Don't replace kernel context!
         ld     [%o2 + PM_CTX], %o0
        brnz,pt %o0, Lsw_havectx                !       goto havecontext;
         nop
        
        /* p does not have a context: call ctx_alloc to get one */
        call    ctx_alloc                       ! ctx_alloc(&vm->vm_pmap);
         mov    %o2, %o0

        set     DEMAP_CTX_SECONDARY, %o1        ! This context has been recycled
        stxa    %o0, [%l5] ASI_DMMU             ! so we need to invalidate
        membar  #Sync
        stxa    %o1, [%o1] ASI_DMMU_DEMAP       ! whatever bits of it may
        stxa    %o1, [%o1] ASI_IMMU_DEMAP       ! be left in the TLB
        membar  #Sync
        /* p does have a context: just switch to it */
Lsw_havectx:
        ! context is in %o0
        /*
         * We probably need to flush the cache here.
         */
        SET_MMU_CONTEXTID(%o0, %l5)             ! Maybe we should invalidate the old context?
        membar  #Sync                           ! Maybe we should use flush here?
        flush   %sp

!       wrpr    %g0, 0, %cleanwin       ! DEBUG
        clr     %g4             ! This needs to point to the base of the data segment
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore default ASI
        wrpr    %g0, PSTATE_INTR, %pstate
        ret
         restore
END(cpu_switchto)

/*
 * Snapshot the current process so that stack frames are up to date.
 * Only used just before a crash dump.
 */
ENTRY(snapshot)
        rdpr    %pstate, %o1            ! save psr
        stx     %o6, [%o0 + PCB_SP]     ! save sp
        rdpr    %pil, %o2
        sth     %o1, [%o0 + PCB_PSTATE]
        rdpr    %cwp, %o3
        stb     %o2, [%o0 + PCB_PIL]
        stb     %o3, [%o0 + PCB_CWP]

        flushw
        save    %sp, -CC64FSZ, %sp
        flushw
        ret
         restore
END(snapshot)

/*
 * cpu_fork() arrange for proc_trampoline() to run
 * after a process gets chosen in mi_switch(). The stack frame will
 * contain a function pointer in %l0, and an argument to pass to it in %l1.
 *
 * If the function *(%l0) returns, we arrange for an immediate return
 * to user mode. This happens in two known cases: after execve(2) of init,
 * and when returning a child to user mode after a fork(2).
 */
ENTRY(proc_trampoline)
        save    %sp, -CC64FSZ, %sp
        call    proc_trampoline_mi
         nop
        restore
        call    %l0                     ! re-use current frame
         mov    %l1, %o0

        /*
         * Here we finish up as in syscall, but simplified.
         */
!       save    %sp, -CC64FSZ, %sp              ! Save a kernel frame to emulate a syscall
        mov     PSTATE_USER, %g1                ! XXXX user pstate (no need to load it)
        sllx    %g1, TSTATE_PSTATE_SHIFT, %g1   ! Shift it into place
        rdpr    %cwp, %g5                       ! Fixup %cwp in %tstate
        or      %g1, %g5, %g1
        stx     %g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
        ba,a,pt %icc, return_from_trap
         nop
END(proc_trampoline)

#ifdef DDB

/*
 * The following probably need to be changed, but to what I don't know.
 */

/*
 * u_int64_t
 * probeget(addr, asi, size)
 *      paddr_t addr;
 *      int asi;
 *      int size;
 *
 * Read a (byte,short,int,long) from the given address.
 * Like copyin but our caller is supposed to know what he is doing...
 * the address can be anywhere.
 *
 * We optimize for space, rather than time, here.
 */
ENTRY(probeget)
        mov     %o2, %o4
        ! %o0 = addr, %o1 = asi, %o4 = (1,2,4)
        GET_CPCB(%o2)                   ! cpcb->pcb_onfault = Lfsprobe;
        set     Lfsprobe, %o5
        stx     %o5, [%o2 + PCB_ONFAULT]
        or      %o0, 0x9, %o3           ! if (PHYS_ASI(asi)) {
        sub     %o3, 0x1d, %o3
        brz,a   %o3, 0f
         mov    %g0, %o5
                                        ! }
0:
        btst    1, %o4
        wr      %o1, 0, %asi
        membar  #Sync
        bz      0f                      ! if (len & 1)
         btst   2, %o4
        ba,pt   %icc, 1f
         lduba  [%o0] %asi, %o0         !       value = *(char *)addr;
0:
        bz      0f                      ! if (len & 2)
         btst   4, %o4
        ba,pt   %icc, 1f
         lduha  [%o0] %asi, %o0         !       value = *(short *)addr;
0:
        bz      0f                      ! if (len & 4)
         btst   8, %o4
        ba,pt   %icc, 1f
         lda    [%o0] %asi, %o0         !       value = *(int *)addr;
0:
        ldxa    [%o0] %asi, %o0         !       value = *(long *)addr;
1:      
        membar  #Sync
        brz     %o5, 1f                 ! if (cache flush addr != 0)
         nop
1:
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore default ASI   
        stx     %g0, [%o2 + PCB_ONFAULT]
        retl                            ! made it, clear onfault and return
         membar #StoreStore|#StoreLoad
END(probeget)

        /*
         * Fault handler for probeget
         */
        .globl  Lfsprobe
Lfsprobe:
        stx     %g0, [%o2 + PCB_ONFAULT]! error in r/w, clear pcb_onfault
        mov     -1, %o1
        wr      %g0, ASI_PRIMARY_NOFAULT, %asi          ! Restore default ASI   
        membar  #StoreStore|#StoreLoad
        retl                            ! and return error indicator
         mov    -1, %o0
END(Lfsprobe)
#endif  /* DDB */

/*
 * pmap_zero_phys(pa)
 *
 * Zero one page physically addressed
 *
 * Block load/store ASIs do not exist for physical addresses,
 * so we won't use them.
 *
 * While we do the zero operation, we also need to blast away
 * the contents of the D$.  We will execute a flush at the end
 * to sync the I$.
 */
        .text
ENTRY(pmap_zero_phys)
        set     NBPG, %o2               ! Loop count
        clr     %o1
1:
        dec     8, %o2
        stxa    %g0, [%o0] ASI_PHYS_CACHED
        inc     8, %o0
dlflush4:
        stxa    %g0, [%o1] ASI_DCACHE_TAG
        brgz    %o2, 1b
         inc    16, %o1

        sethi   %hi(KERNBASE), %o3
        flush   %o3
        retl
         nop
 END(pmap_zero_phys)

/*
 * pmap_copy_phys(src, dst)
 *
 * Copy one page physically addressed
 */
ENTRY(pmap_copy_phys)
        set     NBPG, %o3
        add     %o3, %o0, %o3
1:
        ldxa    [%o0] ASI_PHYS_CACHED, %o4
        inc     8, %o0
        cmp     %o0, %o3
        stxa    %o4, [%o1] ASI_PHYS_CACHED
        blu,pt  %xcc, 1b
         inc    8, %o1
        retl
         nop
END(pmap_copy_phys)

/*
 * extern int64_t pseg_get(struct pmap* %o0, vaddr_t addr %o1);
 *
 * Return TTE at addr in pmap.  Uses physical addressing only.
 * pmap->pm_physaddr must by the physical address of pm_segs
 *
 */
ENTRY(pseg_get)
        ldx     [%o0 + PM_PHYS], %o2                    ! pmap->pm_segs

        srax    %o1, HOLESHIFT, %o3                     ! Check for valid address
        brz,pt  %o3, 0f                                 ! Should be zero or -1
         inc    %o3                                     ! Make -1 -> 0
        brnz,pn %o3, 1f                                 ! Error! In hole!
0:
        srlx    %o1, STSHIFT, %o3
        and     %o3, STMASK, %o3                        ! Index into pm_segs
        sll     %o3, 3, %o3
        add     %o2, %o3, %o2
        ldxa    [%o2] ASI_PHYS_CACHED, %o2              ! Load page directory pointer

        srlx    %o1, PDSHIFT, %o3
        and     %o3, PDMASK, %o3
        sll     %o3, 3, %o3
        brz,pn  %o2, 1f                                 ! NULL entry? check somewhere else
         add    %o2, %o3, %o2
        ldxa    [%o2] ASI_PHYS_CACHED, %o2              ! Load page table pointer

        srlx    %o1, PTSHIFT, %o3                       ! Convert to ptab offset
        and     %o3, PTMASK, %o3
        sll     %o3, 3, %o3
        brz,pn  %o2, 1f                                 ! NULL entry? check somewhere else
         add    %o2, %o3, %o2
        ldxa    [%o2] ASI_PHYS_CACHED, %o0
        brgez,pn %o0, 1f                                ! Entry invalid?  Punt
         nop
        retl
         nop
1:
        retl
         clr    %o0
END(pseg_get)

/*
 * extern int pseg_set(struct pmap* %o0, vaddr_t addr %o1, int64_t tte %o2,
 *                      paddr_t spare %o3);
 *
 * Set a pseg entry to a particular TTE value.  Returns 0 on success,
 * 1 if it needs to fill a pseg, 2 if it succeeded but didn't need the
 * spare page, and -1 if the address is in the virtual hole.
 * (NB: nobody in pmap checks for the virtual hole, so the system will hang.)
 * Allocate a page, pass the phys addr in as the spare, and try again.
 * If spare is not NULL it is assumed to be the address of a zeroed physical
 * page that can be used to generate a directory table or page table if needed.
 *
 */
ENTRY(pseg_set)
        !!
        !! However we managed to get here we now have:
        !!
        !! %o0 = *pmap
        !! %o1 = addr
        !! %o2 = tte
        !! %o3 = spare
        !!
        srax    %o1, HOLESHIFT, %o4                     ! Check for valid address
        brz,pt  %o4, 0f                                 ! Should be zero or -1
         inc    %o4                                     ! Make -1 -> 0
        brz,pt  %o4, 0f
         nop
        mov     -1, %o0                                 ! Error -- in hole!
        retl
         mov    -1, %o1
0:
        ldx     [%o0 + PM_PHYS], %o4                    ! pmap->pm_segs
        srlx    %o1, STSHIFT, %o5
        and     %o5, STMASK, %o5
        sll     %o5, 3, %o5
        add     %o4, %o5, %o4
2:
        ldxa    [%o4] ASI_PHYS_CACHED, %o5              ! Load page directory pointer

        brnz,a,pt       %o5, 0f                         ! Null pointer?
         mov    %o5, %o4
        brz,pn  %o3, 1f                                 ! Have a spare?
         mov    %o3, %o5
        casxa   [%o4] ASI_PHYS_CACHED, %g0, %o5
        brnz,pn %o5, 2b                                 ! Something changed?
        mov     %o3, %o4
        clr     %o3                                     ! Mark spare as used
0:
        srlx    %o1, PDSHIFT, %o5
        and     %o5, PDMASK, %o5
        sll     %o5, 3, %o5
        add     %o4, %o5, %o4
2:
        ldxa    [%o4] ASI_PHYS_CACHED, %o5              ! Load table directory pointer

        brnz,a,pt       %o5, 0f                         ! Null pointer?
         mov    %o5, %o4
        brz,pn  %o3, 1f                                 ! Have a spare?
         mov    %o3, %o5
        casxa   [%o4] ASI_PHYS_CACHED, %g0, %o5
        brnz,pn %o5, 2b                                 ! Something changed?
        mov     %o3, %o4
        clr     %o3                                     ! Mark spare as used
0:
        srlx    %o1, PTSHIFT, %o5                       ! Convert to ptab offset
        and     %o5, PTMASK, %o5
        sll     %o5, 3, %o5
        add     %o5, %o4, %o4
        stxa    %o2, [%o4] ASI_PHYS_CACHED              ! Easier than shift+or
        mov     2, %o0                                  ! spare unused?
        retl
         movrz  %o3, %g0, %o0                           ! No. return 0
1:
        retl
         mov    1, %o0
END(pseg_set)


/*
 * memcpy(dst, src, len) - always copies forward.
 */
ENTRY(memcpy) /* dest, src, size */
        cmp     %o2, BCOPY_SMALL! (check length for doublecopy first)
Lmemcpy_start:
        bge,pt  %xcc, 2f        ! if >= this many, go be fancy.
         nop

        mov     %o0, %o5        ! Save memcpy return value
        /*
         * Not much to copy, just do it a byte at a time.
         */
        deccc   %o2             ! while (--len >= 0)
        bl      1f
!        XXX check no delay slot
0:
        inc     %o1
        ldsb    [%o1 - 1], %o4  !       (++dst)[-1] = *src++;
        stb     %o4, [%o0]
        deccc   %o2
        bge     0b
         inc    %o0
1:
        retl
         mov    %o5, %o0
        NOTREACHED

        /*
         * Plenty of data to copy, so try to do it optimally.
         */
2:
Lbcopy_fancy:

        !!
        !! First align the output to a 8-byte entity
        !! 

        save    %sp, -CC64FSZ, %sp
        
        mov     %i1, %l0
        mov     %i0, %l1
        
        mov     %i2, %l2
        btst    1, %l1
        
        bz,pt   %icc, 4f
         btst   2, %l1
        ldub    [%l0], %l4                              ! Load 1st byte
        
        deccc   1, %l2
        ble,pn  %xcc, Lbcopy_finish                     ! XXXX
         inc    1, %l0
        
        stb     %l4, [%l1]                              ! Store 1st byte
        inc     1, %l1                                  ! Update address
        btst    2, %l1
4:      
        bz,pt   %icc, 4f
        
         btst   1, %l0
        bz,a    1f
         lduh   [%l0], %l4                              ! Load short

        ldub    [%l0], %l4                              ! Load bytes
        
        ldub    [%l0+1], %l3
        sllx    %l4, 8, %l4
        or      %l3, %l4, %l4
        
1:      
        deccc   2, %l2
        ble,pn  %xcc, Lbcopy_finish                     ! XXXX
         inc    2, %l0
        sth     %l4, [%l1]                              ! Store 1st short
        
        inc     2, %l1
4:
        btst    4, %l1
        bz,pt   %xcc, 4f
        
         btst   3, %l0
        bz,a,pt %xcc, 1f
         lduw   [%l0], %l4                              ! Load word -1

        btst    1, %l0
        bz,a,pt %icc, 2f
         lduh   [%l0], %l4
        
        ldub    [%l0], %l4
        
        lduh    [%l0+1], %l3
        sllx    %l4, 16, %l4
        or      %l4, %l3, %l4
        
        ldub    [%l0+3], %l3
        sllx    %l4, 8, %l4
        ba,pt   %icc, 1f
         or     %l4, %l3, %l4
        
2:
        lduh    [%l0+2], %l3
        sllx    %l4, 16, %l4
        or      %l4, %l3, %l4
        
1:      
        deccc   4, %l2
        ble,pn  %xcc, Lbcopy_finish             ! XXXX
         inc    4, %l0
        
        st      %l4, [%l1]                              ! Store word
        inc     4, %l1
4:
        !!
        !! We are now 32-bit aligned in the dest.
        !!
Lbcopy_common:  

        and     %l0, 7, %l4                             ! Shift amount
        andn    %l0, 7, %l0                             ! Source addr
        
        brz,pt  %l4, Lbcopy_noshift8                    ! No shift version...

         sllx   %l4, 3, %l4                             ! In bits
        mov     8<<3, %l3
        
        ldx     [%l0], %o0                              ! Load word -1
        sub     %l3, %l4, %l3                           ! Reverse shift
        deccc   12*8, %l2                               ! Have enough room?
        
        sllx    %o0, %l4, %o0
        bl,pn   %xcc, 2f
         and    %l3, 0x38, %l3
Lbcopy_unrolled8:

        /*
         * This is about as close to optimal as you can get, since
         * the shifts require EU0 and cannot be paired, and you have
         * 3 dependent operations on the data.
         */ 

!       ldx     [%l0+0*8], %o0                          ! Already done
!       sllx    %o0, %l4, %o0                           ! Already done
        ldx     [%l0+1*8], %o1
        ldx     [%l0+2*8], %o2
        ldx     [%l0+3*8], %o3
        ldx     [%l0+4*8], %o4
        ba,pt   %icc, 1f
         ldx    [%l0+5*8], %o5
        .align  8
1:
        srlx    %o1, %l3, %g1
        inc     6*8, %l0
        
        sllx    %o1, %l4, %o1
        or      %g1, %o0, %g6
        ldx     [%l0+0*8], %o0
        
        stx     %g6, [%l1+0*8]
        srlx    %o2, %l3, %g1

        sllx    %o2, %l4, %o2
        or      %g1, %o1, %g6
        ldx     [%l0+1*8], %o1
        
        stx     %g6, [%l1+1*8]
        srlx    %o3, %l3, %g1
        
        sllx    %o3, %l4, %o3
        or      %g1, %o2, %g6
        ldx     [%l0+2*8], %o2
        
        stx     %g6, [%l1+2*8]
        srlx    %o4, %l3, %g1
        
        sllx    %o4, %l4, %o4   
        or      %g1, %o3, %g6
        ldx     [%l0+3*8], %o3
        
        stx     %g6, [%l1+3*8]
        srlx    %o5, %l3, %g1
        
        sllx    %o5, %l4, %o5
        or      %g1, %o4, %g6
        ldx     [%l0+4*8], %o4

        stx     %g6, [%l1+4*8]
        srlx    %o0, %l3, %g1
        deccc   6*8, %l2                                ! Have enough room?

        sllx    %o0, %l4, %o0                           ! Next loop
        or      %g1, %o5, %g6
        ldx     [%l0+5*8], %o5
        
        stx     %g6, [%l1+5*8]
        bge,pt  %xcc, 1b
         inc    6*8, %l1

Lbcopy_unrolled8_cleanup:       
        !!
        !! Finished 8 byte block, unload the regs.
        !! 
        srlx    %o1, %l3, %g1
        inc     5*8, %l0
        
        sllx    %o1, %l4, %o1
        or      %g1, %o0, %g6
                
        stx     %g6, [%l1+0*8]
        srlx    %o2, %l3, %g1
        
        sllx    %o2, %l4, %o2
        or      %g1, %o1, %g6
                
        stx     %g6, [%l1+1*8]
        srlx    %o3, %l3, %g1
        
        sllx    %o3, %l4, %o3
        or      %g1, %o2, %g6
                
        stx     %g6, [%l1+2*8]
        srlx    %o4, %l3, %g1
        
        sllx    %o4, %l4, %o4   
        or      %g1, %o3, %g6
                
        stx     %g6, [%l1+3*8]
        srlx    %o5, %l3, %g1
        
        sllx    %o5, %l4, %o5
        or      %g1, %o4, %g6
                
        stx     %g6, [%l1+4*8]
        inc     5*8, %l1
        
        mov     %o5, %o0                                ! Save our unused data
        dec     5*8, %l2
2:
        inccc   12*8, %l2
        bz,pn   %icc, Lbcopy_complete
        
        !! Unrolled 8 times
Lbcopy_aligned8:        
!       ldx     [%l0], %o0                              ! Already done
!       sllx    %o0, %l4, %o0                           ! Shift high word
        
         deccc  8, %l2                                  ! Pre-decrement
        bl,pn   %xcc, Lbcopy_finish
1:
        ldx     [%l0+8], %o1                            ! Load word 0
        inc     8, %l0
        
        srlx    %o1, %l3, %g6
        or      %g6, %o0, %g6                           ! Combine
        
        stx     %g6, [%l1]                              ! Store result
         inc    8, %l1
        
        deccc   8, %l2
        bge,pn  %xcc, 1b
         sllx   %o1, %l4, %o0   

        btst    7, %l2                                  ! Done?
        bz,pt   %xcc, Lbcopy_complete

        !!
        !! Loadup the last dregs into %o0 and shift it into place
        !! 
         srlx   %l3, 3, %g6                             ! # bytes in %o0
        dec     8, %g6                                  !  - 8
        !! n-8 - (by - 8) -> n - by
        subcc   %l2, %g6, %g0                           ! # bytes we need
        ble,pt  %icc, Lbcopy_finish
         nop
        ldx     [%l0+8], %o1                            ! Need another word
        srlx    %o1, %l3, %o1
        ba,pt   %icc, Lbcopy_finish
         or     %o0, %o1, %o0                           ! All loaded up.
        
Lbcopy_noshift8:
        deccc   6*8, %l2                                ! Have enough room?
        bl,pn   %xcc, 2f
         nop
        ba,pt   %icc, 1f
         nop
        .align  32
1:      
        ldx     [%l0+0*8], %o0
        ldx     [%l0+1*8], %o1
        ldx     [%l0+2*8], %o2
        stx     %o0, [%l1+0*8]
        stx     %o1, [%l1+1*8]
        stx     %o2, [%l1+2*8]

        
        ldx     [%l0+3*8], %o3
        ldx     [%l0+4*8], %o4
        ldx     [%l0+5*8], %o5
        inc     6*8, %l0
        stx     %o3, [%l1+3*8]
        deccc   6*8, %l2
        stx     %o4, [%l1+4*8]
        stx     %o5, [%l1+5*8]
        bge,pt  %xcc, 1b
         inc    6*8, %l1
2:
        inc     6*8, %l2
1:      
        deccc   8, %l2
        bl,pn   %icc, 1f                                ! < 0 --> sub word
         nop
        ldx     [%l0], %g6
        inc     8, %l0
        stx     %g6, [%l1]
        bg,pt   %icc, 1b                                ! Exactly 0 --> done
         inc    8, %l1
1:
        btst    7, %l2                                  ! Done?
        bz,pt   %xcc, Lbcopy_complete
         clr    %l4
        ldx     [%l0], %o0
Lbcopy_finish:
        
        brz,pn  %l2, 2f                                 ! 100% complete?
         cmp    %l2, 8                                  ! Exactly 8 bytes?
        bz,a,pn %xcc, 2f
         stx    %o0, [%l1]

        btst    4, %l2                                  ! Word store?
        bz      %xcc, 1f
         srlx   %o0, 32, %g6                            ! Shift high word down
        stw     %g6, [%l1]
        inc     4, %l1
        mov     %o0, %g6                                ! Operate on the low bits
1:
        btst    2, %l2
        mov     %g6, %o0
        bz      1f
         srlx   %o0, 16, %g6
        
        sth     %g6, [%l1]                              ! Store short
        inc     2, %l1
        mov     %o0, %g6                                ! Operate on low bytes
1:
        mov     %g6, %o0
        btst    1, %l2                                  ! Byte aligned?
        bz      2f
         srlx   %o0, 8, %g6

        stb     %g6, [%l1]                              ! Store last byte
        inc     1, %l1                                  ! Update address
2:      
Lbcopy_complete:
        ret
         restore %i0, %g0, %o0
END(memcpy)
        
/*
 * bzero(addr, len)
 *
 * XXXXX To produce more efficient code, we do not allow lengths
 * greater than 0x80000000000000000, which are negative numbers.
 * This should not really be an issue since the VA hole should
 * cause any such ranges to fail anyway.
 */
ENTRY(bzero)
        ! %o0 = addr, %o1 = len
        mov     %o1, %o2
        clr     %o1                     ! Initialize our pattern
/*
 * memset(addr, c, len)
 */
ENTRY(memset)
        ! %o0 = addr, %o1 = pattern, %o2 = len
        mov     %o0, %o4                ! Save original pointer

Lbzero_internal:
        btst    7, %o0                  ! Word aligned?
        bz,pn   %xcc, 0f
         nop
        inc     %o0
        deccc   %o2                     ! Store up to 7 bytes
        bge,a,pt        %xcc, Lbzero_internal
         stb    %o1, [%o0 - 1]

        retl                            ! Duplicate Lbzero_done
         mov    %o4, %o0
0:
        /*
         * Duplicate the pattern so it fills 64-bits.
         */
        andcc   %o1, 0x0ff, %o1         ! No need to extend zero
        bz,pt   %icc, 1f
         sllx   %o1, 8, %o3             ! sigh.  all dependent instructions.
        or      %o1, %o3, %o1
        sllx    %o1, 16, %o3
        or      %o1, %o3, %o1
        sllx    %o1, 32, %o3
         or     %o1, %o3, %o1
1:      
         deccc  8, %o2
Lbzero_longs:
        bl,pn   %xcc, Lbzero_cleanup    ! Less than 8 bytes left
         nop
3:      
        inc     8, %o0
        deccc   8, %o2
        bge,pt  %xcc, 3b
         stx    %o1, [%o0 - 8]          ! Do 1 longword at a time

        /*
         * Len is in [-8..-1] where -8 => done, -7 => 1 byte to zero,
         * -6 => two bytes, etc.  Mop up this remainder, if any.
         */
Lbzero_cleanup: 
        btst    4, %o2
        bz,pt   %xcc, 5f                ! if (len & 4) {
         nop
        stw     %o1, [%o0]              !       *(int *)addr = 0;
        inc     4, %o0                  !       addr += 4;
5:      
        btst    2, %o2
        bz,pt   %xcc, 7f                ! if (len & 2) {
         nop
        sth     %o1, [%o0]              !       *(short *)addr = 0;
        inc     2, %o0                  !       addr += 2;
7:      
        btst    1, %o2
        bnz,a   %icc, Lbzero_done       ! if (len & 1)
         stb    %o1, [%o0]              !       *addr = 0;
Lbzero_done:
        retl
         mov    %o4, %o0                ! Restore pointer for memset (ugh)
END(memset)

/*
 * kcopy() is exactly like bcopy except that it sets pcb_onfault such that
 * when a fault occurs, it is able to return EFAULT to indicate this to the
 * caller.
 */
ENTRY(kcopy)
        GET_CPCB(%o5)                   ! cpcb->pcb_onfault = Lkcerr;
        set     Lkcerr, %o3
        ldx     [%o5 + PCB_ONFAULT], %g1! save current onfault handler
        membar  #LoadStore
        stx     %o3, [%o5 + PCB_ONFAULT]
        membar  #StoreStore|#StoreLoad

        cmp     %o2, BCOPY_SMALL
Lkcopy_start:
        bge,a   Lkcopy_fancy    ! if >= this many, go be fancy.
         btst   7, %o0          ! (part of being fancy)

        /*
         * Not much to copy, just do it a byte at a time.
         */
        deccc   %o2             ! while (--len >= 0)
        bl      1f
!        XXX check no delay slot
0:
        ldsb    [%o0], %o4      !       *dst++ = *src++;
        inc     %o0
        stb     %o4, [%o1]
        deccc   %o2
        bge     0b
         inc    %o1
1:
        membar  #Sync           ! Make sure all faults are processed
        stx     %g1, [%o5 + PCB_ONFAULT]! restore fault handler
        membar  #StoreStore|#StoreLoad
        retl
         clr    %o0
        NOTREACHED

        /*
         * Plenty of data to copy, so try to do it optimally.
         */
Lkcopy_fancy:
        ! check for common case first: everything lines up.
!       btst    7, %o0          ! done already
        bne     1f
!        XXX check no delay slot
        btst    7, %o1
        be,a    Lkcopy_doubles
         dec    8, %o2          ! if all lined up, len -= 8, goto kcopy_doubes

        ! If the low bits match, we can make these line up.
1:
        xor     %o0, %o1, %o3   ! t = src ^ dst;
        btst    1, %o3          ! if (t & 1) {
        be,a    1f
         btst   1, %o0          ! [delay slot: if (src & 1)]

        ! low bits do not match, must copy by bytes.
0:
        ldsb    [%o0], %o4      !       do {
        inc     %o0             !               *dst++ = *src++;
        stb     %o4, [%o1]
        deccc   %o2
        bnz     0b              !       } while (--len != 0);
         inc    %o1
        membar  #Sync           ! Make sure all traps are taken
        stx     %g1, [%o5 + PCB_ONFAULT]! restore fault handler
        membar  #StoreStore|#StoreLoad
        retl
         clr    %o0
        NOTREACHED

        ! lowest bit matches, so we can copy by words, if nothing else
1:
        be,a    1f              ! if (src & 1) {
         btst   2, %o3          ! [delay slot: if (t & 2)]

        ! although low bits match, both are 1: must copy 1 byte to align
        ldsb    [%o0], %o4      !       *dst++ = *src++;
        inc     %o0
        stb     %o4, [%o1]
        dec     %o2             !       len--;
        inc     %o1
        btst    2, %o3          ! } [if (t & 2)]
1:
        be,a    1f              ! if (t & 2) {
         btst   2, %o0          ! [delay slot: if (src & 2)]
        dec     2, %o2          !       len -= 2;
0:
        ldsh    [%o0], %o4      !       do {
        inc     2, %o0          !               dst += 2, src += 2;
        sth     %o4, [%o1]      !               *(short *)dst = *(short *)src;
        deccc   2, %o2          !       } while ((len -= 2) >= 0);
        bge     0b
         inc    2, %o1
        b       Lkcopy_mopb     !       goto mop_up_byte;
         btst   1, %o2          ! } [delay slot: if (len & 1)]
        NOTREACHED

        ! low two bits match, so we can copy by longwords
1:
        be,a    1f              ! if (src & 2) {
         btst   4, %o3          ! [delay slot: if (t & 4)]

        ! although low 2 bits match, they are 10: must copy one short to align
        ldsh    [%o0], %o4      !       (*short *)dst = *(short *)src;
        inc     2, %o0          !       dst += 2;
        sth     %o4, [%o1]
        dec     2, %o2          !       len -= 2;
        inc     2, %o1          !       src += 2;
        btst    4, %o3          ! } [if (t & 4)]
1:
        be,a    1f              ! if (t & 4) {
         btst   4, %o0          ! [delay slot: if (src & 4)]
        dec     4, %o2          !       len -= 4;
0:
        ld      [%o0], %o4      !       do {
        inc     4, %o0          !               dst += 4, src += 4;
        st      %o4, [%o1]      !               *(int *)dst = *(int *)src;
        deccc   4, %o2          !       } while ((len -= 4) >= 0);
        bge     0b
         inc    4, %o1
        b       Lkcopy_mopw     !       goto mop_up_word_and_byte;
         btst   2, %o2          ! } [delay slot: if (len & 2)]
        NOTREACHED

        ! low three bits match, so we can copy by doublewords
1:
        be      1f              ! if (src & 4) {
         dec    8, %o2          ! [delay slot: len -= 8]
        ld      [%o0], %o4      !       *(int *)dst = *(int *)src;
        inc     4, %o0          !       dst += 4, src += 4, len -= 4;
        st      %o4, [%o1]
        dec     4, %o2          ! }
        inc     4, %o1
1:
Lkcopy_doubles:
        ldx     [%o0], %g5      ! do {
        inc     8, %o0          !       dst += 8, src += 8;
        stx     %g5, [%o1]      !       *(double *)dst = *(double *)src;
        deccc   8, %o2          ! } while ((len -= 8) >= 0);
        bge     Lkcopy_doubles
         inc    8, %o1

        ! check for a usual case again (save work)
        btst    7, %o2          ! if ((len & 7) == 0)
        be      Lkcopy_done     !       goto kcopy_done;

         btst   4, %o2          ! if ((len & 4) == 0)
        be,a    Lkcopy_mopw     !       goto mop_up_word_and_byte;
         btst   2, %o2          ! [delay slot: if (len & 2)]
        ld      [%o0], %o4      !       *(int *)dst = *(int *)src;
        inc     4, %o0          !       dst += 4;
        st      %o4, [%o1]
        inc     4, %o1          !       src += 4;
        btst    2, %o2          ! } [if (len & 2)]

1:
        ! mop up trailing word (if present) and byte (if present).
Lkcopy_mopw:
        be      Lkcopy_mopb     ! no word, go mop up byte
         btst   1, %o2          ! [delay slot: if (len & 1)]
        ldsh    [%o0], %o4      ! *(short *)dst = *(short *)src;
        be      Lkcopy_done     ! if ((len & 1) == 0) goto done;
         sth    %o4, [%o1]
        ldsb    [%o0 + 2], %o4  ! dst[2] = src[2];
        stb     %o4, [%o1 + 2]
        membar  #Sync           ! Make sure all traps are taken
        stx     %g1, [%o5 + PCB_ONFAULT]! restore fault handler
        membar  #StoreStore|#StoreLoad
        retl
         clr    %o0
        NOTREACHED

        ! mop up trailing byte (if present).
Lkcopy_mopb:
        bne,a   1f
         ldsb   [%o0], %o4

Lkcopy_done:
        membar  #Sync           ! Make sure all traps are taken
        stx     %g1, [%o5 + PCB_ONFAULT]! restore fault handler
        membar  #StoreStore|#StoreLoad
        retl
         clr    %o0
        NOTREACHED

1:
        stb     %o4, [%o1]
        membar  #Sync           ! Make sure all traps are taken
        stx     %g1, [%o5 + PCB_ONFAULT]! restore fault handler
        membar  #StoreStore|#StoreLoad
        retl
         clr    %o0
        NOTREACHED

Lkcerr:
        stx     %g1, [%o5 + PCB_ONFAULT]! restore fault handler
        membar  #StoreStore|#StoreLoad
        retl                            ! and return error indicator
         mov    EFAULT, %o0
        NOTREACHED
END(kcopy)

/*
 * bcopy(src, dest, size) - overlaps detected and copied in reverse
 */
ENTRY(bcopy)
        /*
         * Swap args and continue to memmove.
         */
        mov     %o0, %o3
        mov     %o1, %o0
        mov     %o3, %o1
/*
 * memmove(dst, src, len) - overlaps detected and copied in reverse
 */
ENTRY(memmove)
        mov     %o0, %o5        ! Save memcpy return value

        cmp     %o1, %o0        ! src < dst?
        bgeu    Lmemcpy_start   ! no, go copy forwards as via memcpy
         cmp    %o2, BCOPY_SMALL! (check length for doublecopy first)

        /*
         * Since src comes before dst, and the regions might overlap,
         * we have to do the copy starting at the end and working backwards.
         */
        add     %o2, %o1, %o1   ! src += len
        add     %o2, %o0, %o0   ! dst += len
        bge,a   Lback_fancy     ! if len >= BCOPY_SMALL, go be fancy
         btst   3, %o1

        /*
         * Not much to copy, just do it a byte at a time.
         */
        deccc   %o2             ! while (--len >= 0)
        bl      1f
!        XXX check no delay slot
0:
        dec     %o1             !       *--dst = *--src;
        ldsb    [%o1], %o4
        dec     %o0
        deccc   %o2
        bge     0b
         stb    %o4, [%o0]
1:
        retl
         mov    %o5, %o0
        NOTREACHED

        /*
         * Plenty to copy, try to be optimal.
         * We only bother with word/halfword/byte copies here.
         */
Lback_fancy:
!       btst    3, %o1          ! done already
        bnz     1f              ! if ((src & 3) == 0 &&
         btst   3, %o0          !     (dst & 3) == 0)
        bz,a    Lback_words     !       goto words;
         dec    4, %o2          ! (done early for word copy)

1:
        /*
         * See if the low bits match.
         */
        xor     %o1, %o0, %o3   ! t = src ^ dst;
        btst    1, %o3
        bz,a    3f              ! if (t & 1) == 0, can do better
         btst   1, %o1

        /*
         * Nope; gotta do byte copy.
         */
2:
        dec     %o1             ! do {
        ldsb    [%o1], %o4      !       *--dst = *--src;
        dec     %o0
        deccc   %o2             ! } while (--len != 0);
        bnz     2b
         stb    %o4, [%o0]
        retl
         nop

3:
        /*
         * Can do halfword or word copy, but might have to copy 1 byte first.
         */
!       btst    1, %o1          ! done earlier
        bz,a    4f              ! if (src & 1) {        /* copy 1 byte */
         btst   2, %o3          ! (done early)
        dec     %o1             !       *--dst = *--src;
        ldsb    [%o1], %o4
        dec     %o0
        stb     %o4, [%o0]
        dec     %o2             !       len--;
        btst    2, %o3          ! }

4:
        /*
         * See if we can do a word copy ((t&2) == 0).
         */
!       btst    2, %o3          ! done earlier
        bz,a    6f              ! if (t & 2) == 0, can do word copy
         btst   2, %o1          ! (src&2, done early)

        /*
         * Gotta do halfword copy.
         */
        dec     2, %o2          ! len -= 2;
5:
        dec     2, %o1          ! do {
        ldsh    [%o1], %o4      !       src -= 2;
        dec     2, %o0          !       dst -= 2;
        deccc   2, %o2          !       *(short *)dst = *(short *)src;
        bge     5b              ! } while ((len -= 2) >= 0);
         sth    %o4, [%o0]
        b       Lback_mopb      ! goto mop_up_byte;
         btst   1, %o2          ! (len&1, done early)

6:
        /*
         * We can do word copies, but we might have to copy
         * one halfword first.
         */
!       btst    2, %o1          ! done already
        bz      7f              ! if (src & 2) {
         dec    4, %o2          ! (len -= 4, done early)
        dec     2, %o1          !       src -= 2, dst -= 2;
        ldsh    [%o1], %o4      !       *(short *)dst = *(short *)src;
        dec     2, %o0
        sth     %o4, [%o0]
        dec     2, %o2          !       len -= 2;
                                ! }

7:
Lback_words:
        /*
         * Do word copies (backwards), then mop up trailing halfword
         * and byte if any.
         */
!       dec     4, %o2          ! len -= 4, done already
0:                              ! do {
        dec     4, %o1          !       src -= 4;
        dec     4, %o0          !       src -= 4;
        ld      [%o1], %o4      !       *(int *)dst = *(int *)src;
        deccc   4, %o2          ! } while ((len -= 4) >= 0);
        bge     0b
         st     %o4, [%o0]

        /*
         * Check for trailing shortword.
         */
        btst    2, %o2          ! if (len & 2) {
        bz,a    1f
         btst   1, %o2          ! (len&1, done early)
        dec     2, %o1          !       src -= 2, dst -= 2;
        ldsh    [%o1], %o4      !       *(short *)dst = *(short *)src;
        dec     2, %o0
        sth     %o4, [%o0]      ! }
        btst    1, %o2

        /*
         * Check for trailing byte.
         */
1:
Lback_mopb:
!       btst    1, %o2          ! (done already)
        bnz,a   1f              ! if (len & 1) {
         ldsb   [%o1 - 1], %o4  !       b = src[-1];
        retl
         mov    %o5, %o0
        NOTREACHED

1:
        stb     %o4, [%o0 - 1]  ! }
        retl                    !       dst[-1] = b;
         mov    %o5, %o0
        NOTREACHED
END(memmove)

/*
 * clearfpstate()
 *
 * Drops the current fpu state, without saving it.
 */
ENTRY(clearfpstate)
        retl
         wr     %g0, FPRS_FEF, %fprs
END(clearfpstate)

/*
 * savefpstate(struct fpstate *f)
 *
 * Store the current FPU state.
 *
 * Since the kernel may need to use the FPU and we have problems atomically
 * testing and enabling the FPU, we leave here with the FPRS_FEF bit set.
 * Normally this should be turned on in loadfpstate().
 */
 /* XXXXXXXXXX  Assume caller created a proper stack frame */
ENTRY(savefpstate)
        rdpr    %pstate, %o1            ! enable FP before we begin
        rd      %fprs, %o5
        wr      %g0, FPRS_FEF, %fprs
        or      %o1, PSTATE_PEF, %o1
        wrpr    %o1, 0, %pstate

        stx     %fsr, [%o0 + FS_FSR]    ! f->fs_fsr = getfsr();

        rd      %gsr, %o4               ! Save %gsr
        st      %o4, [%o0 + FS_GSR]

        add     %o0, FS_REGS, %o2       ! This is zero...
        btst    FPRS_DL, %o5            ! Lower FPU clean?
        bz,a,pt %icc, 1f                ! Then skip it
         add    %o2, 128, %o2           ! Skip a block

        membar  #Sync
        stda    %f0, [%o2] ASI_BLK_P    ! f->fs_f0 = etc;
        inc     BLOCK_SIZE, %o2
        stda    %f16, [%o2] ASI_BLK_P
        inc     BLOCK_SIZE, %o2
1:
        btst    FPRS_DU, %o5            ! Upper FPU clean?
        bz,pt   %icc, 2f                ! Then skip it
         nop

        membar  #Sync
        stda    %f32, [%o2] ASI_BLK_P
        inc     BLOCK_SIZE, %o2
        stda    %f48, [%o2] ASI_BLK_P
2:
        membar  #Sync                   ! Finish operation so we can
        retl
         wr     %g0, FPRS_FEF, %fprs    ! Mark FPU clean
END(savefpstate)

/*
 * Load FPU state.
 */
 /* XXXXXXXXXX  Should test to see if we only need to do a partial restore */
ENTRY(loadfpstate)
        rdpr    %pstate, %o1            ! enable FP before we begin
        ld      [%o0 + FS_GSR], %o4     ! Restore %gsr
        set     PSTATE_PEF, %o2
        wr      %g0, FPRS_FEF, %fprs
        or      %o1, %o2, %o1
        wrpr    %o1, 0, %pstate
        ldx     [%o0 + FS_FSR], %fsr    ! setfsr(f->fs_fsr);
        add     %o0, FS_REGS, %o3       ! This is zero...
        wr      %o4, %g0, %gsr
        membar  #Sync
        ldda    [%o3] ASI_BLK_P, %f0
        inc     BLOCK_SIZE, %o3
        ldda    [%o3] ASI_BLK_P, %f16
        inc     BLOCK_SIZE, %o3
        ldda    [%o3] ASI_BLK_P, %f32
        inc     BLOCK_SIZE, %o3
        ldda    [%o3] ASI_BLK_P, %f48
        membar  #Sync                   ! Make sure loads are complete
        retl
         wr     %g0, FPRS_FEF, %fprs    ! Clear dirty bits
END(loadfpstate)

/* XXX belongs elsewhere (ctlreg.h?) */
#define AFSR_CECC_ERROR         0x100000        /* AFSR Correctable ECC err */
#define DATAPATH_CE             0x100           /* Datapath Correctable Err */

        .data
        _ALIGN
        .globl  cecclast, ceccerrs
cecclast:
        .xword 0
ceccerrs:
        .word 0
        _ALIGN
        .text

/*
 * ECC Correctable Error handler - this doesn't do much except intercept
 * the error and reset the status bits.
 */
ENTRY(cecc_catch)
        ldxa    [%g0] ASI_AFSR, %g1                     ! g1 = AFSR
        ldxa    [%g0] ASI_AFAR, %g2                     ! g2 = AFAR

        sethi   %hi(cecclast), %g1                      ! cecclast = AFAR
        or      %g1, %lo(cecclast), %g1
        stx     %g2, [%g1]

        sethi   %hi(ceccerrs), %g1                      ! get current count
        or      %g1, %lo(ceccerrs), %g1
        lduw    [%g1], %g2                              ! g2 = ceccerrs

        ldxa    [%g0] ASI_DATAPATH_ERR_REG_READ, %g3    ! Read UDB-Low status
        andcc   %g3, DATAPATH_CE, %g4                   ! Check CE bit
        be,pn   %xcc, 1f                                ! Don't clear unless
         nop                                            !  necessary
        stxa    %g4, [%g0] ASI_DATAPATH_ERR_REG_WRITE   ! Clear CE bit in UDBL
        membar  #Sync                                   ! sync store
        inc     %g2                                     ! ceccerrs++
1:      mov     0x18, %g5
        ldxa    [%g5] ASI_DATAPATH_ERR_REG_READ, %g3    ! Read UDB-High status
        andcc   %g3, DATAPATH_CE, %g4                   ! Check CE bit
        be,pn   %xcc, 1f                                ! Don't clear unless
         nop                                            !  necessary
        stxa    %g4, [%g5] ASI_DATAPATH_ERR_REG_WRITE   ! Clear CE bit in UDBH
        membar  #Sync                                   ! sync store
        inc     %g2                                     ! ceccerrs++
1:      set     AFSR_CECC_ERROR, %g3
        stxa    %g3, [%g0] ASI_AFSR                     ! Clear CE in AFSR
        stw     %g2, [%g1]                              ! set ceccerrs
        membar  #Sync                                   ! sync store
        CLRTT
        retry
        NOTREACHED
END(cecc_catch)

/*
 * send_softint(level, intrhand)
 *
 * Send a softint with an intrhand pointer so we can cause a vectored
 * interrupt instead of a polled interrupt.  This does pretty much the
 * same as interrupt_vector.  If intrhand is NULL then it just sends
 * a polled interrupt.
 */
ENTRY(send_softint)
        rdpr    %pstate, %g1
        andn    %g1, PSTATE_IE, %o3
        wrpr    %o3, 0, %pstate

        brz,pn  %o1, 1f
         add    %g7, CI_INTRPENDING, %o3

        ldx     [%o1 + IH_PEND], %o5
        brnz,pn %o5, 1f
         sll    %o0, 3, %o5             ! Find start of list for this IPL
        add     %o3, %o5, %o3

        ldx     [%o3], %o5              ! Load list head
        add     %o1, IH_PEND, %o4
        casxa   [%o4] ASI_N, %g0, %o5
        brnz,pn %o5, 1f
         nop
        stx     %o1, [%o3]

        mov     1, %o3                  ! Change from level to bitmask
        sllx    %o3, %o0, %o3
        wr      %o3, 0, SET_SOFTINT     ! SET_SOFTINT
1:
        retl
         wrpr   %g1, 0, %pstate         ! restore interrupts
END(send_softint)

/*
 * Flush user windows to memory.
 */
ENTRY(write_user_windows)
        rdpr    %otherwin, %g1
        brz     %g1, 3f
        clr     %g2
1:
        save    %sp, -CC64FSZ, %sp
        rdpr    %otherwin, %g1
        brnz    %g1, 1b
         inc    %g2
2:
        dec     %g2
        brnz    %g2, 2b
         restore
3:
        retl
         nop
END(write_user_windows)

/*
 * Clear the Nonprivileged Trap (NPT) bit of %tick such that it can be
 * read from userland.  This requires us to read the current value and
 * write it back with the bit cleared.  As a result we will lose a
 * couple of ticks.  In order to limit the number of lost ticks, we
 * block interrupts and make sure the instructions to read and write
 * %tick live in the same cache line.  We tag on an extra read to work
 * around a Blackbird (UltraSPARC-II) errata (see below).
 */
ENTRY(tick_enable)
        rdpr    %pstate, %o0
        andn    %o0, PSTATE_IE, %o1
        wrpr    %o1, 0, %pstate         ! disable interrupts
        rdpr    %tick, %o2
        brgez,pn %o2, 1f
         clr    %o1
        mov     1, %o1
        sllx    %o1, 63, %o1
        ba,pt   %xcc, 1f
         nop
        .align  64
1:      rdpr    %tick, %o2
        wrpr    %o2, %o1, %tick
        rdpr    %tick, %g0

        retl
         wrpr   %o0, 0, %pstate         ! restore interrupts
END(tick_enable)

/*
 * On Blackbird (UltraSPARC-II) CPUs, writes to %tick_cmpr may fail.
 * The workaround is to do a read immediately after the write and make
 * sure those two instructions are in the same cache line.
 */
ENTRY(tickcmpr_set)
        .align  64
        wr      %o0, 0, %tick_cmpr
        rd      %tick_cmpr, %g0
        retl
         nop
END(tickcmpr_set)

ENTRY(sys_tick_enable)
        rdpr    %pstate, %o0
        andn    %o0, PSTATE_IE, %o1
        wrpr    %o1, 0, %pstate         ! disable interrupts
        rd      %sys_tick, %o2
        brgez,pn %o2, 1f
         clr    %o1
        mov     1, %o1
        sllx    %o1, 63, %o1
        ba,pt   %xcc, 1f
         nop
        .align  64
1:      rd      %sys_tick, %o2
        wr      %o2, %o1, %sys_tick
        rd      %sys_tick, %g0

        retl
         wrpr   %o0, 0, %pstate         ! restore interrupts
END(sys_tick_enable)

ENTRY(sys_tickcmpr_set)
        .align  64
        wr      %o0, 0, %sys_tick_cmpr
        rd      %sys_tick_cmpr, %g0
        retl
         nop
END(sys_tickcmpr_set)

/*
 * Support for the STICK logic found on the integrated PCI host bridge
 * of Hummingbird (UltraSPARC-IIe).  The chip designers made the
 * brilliant decision to split the 64-bit counters into two 64-bit
 * aligned 32-bit registers, making atomic access impossible.  This
 * means we have to check for wraparound in various places.  Sigh.
 */

#define STICK_CMP_LOW   0x1fe0000f060
#define STICK_CMP_HIGH  0x1fe0000f068
#define STICK_REG_LOW   0x1fe0000f070
#define STICK_REG_HIGH  0x1fe0000f078

ENTRY(stick)
        setx    STICK_REG_LOW, %o1, %o3
0:
        ldxa    [%o3] ASI_PHYS_NON_CACHED, %o0
        add     %o3, (STICK_REG_HIGH - STICK_REG_LOW), %o4
        ldxa    [%o4] ASI_PHYS_NON_CACHED, %o1
        ldxa    [%o3] ASI_PHYS_NON_CACHED, %o2
        cmp     %o2, %o0                ! Check for wraparound
        blu,pn  %icc, 0b
         sllx   %o1, 33, %o1            ! Clear the MSB
        srlx    %o1, 1, %o1
        retl
         or     %o2, %o1, %o0
END(stick)

ENTRY(stickcmpr_set)
        setx    STICK_CMP_HIGH, %o1, %o3
        srlx    %o0, 32, %o1
        stxa    %o1, [%o3] ASI_PHYS_NON_CACHED
        add     %o3, (STICK_CMP_LOW - STICK_CMP_HIGH), %o4
        stxa    %o0, [%o4] ASI_PHYS_NON_CACHED
        retl
         nop
END(stickcmpr_set)

#define MICROPERSEC     (1000000)
        .data
        .align  16
        .globl  cpu_clockrate
cpu_clockrate:
        !! Pretend we have a 200MHz clock -- cpu_attach will fix this
        .xword  200000000
        !! Here we'll store cpu_clockrate/1000000 so we can calculate usecs
        .xword  0
        .text

/*
 * delay function
 *
 * void delay(N)  -- delay N microseconds
 *
 * Register usage: %o0 = "N" number of usecs to go (counts down to zero)
 *                 %o2 = counter for 1 usec (counts down from %o1 to zero)
 *
 *
 *      cpu_clockrate should be tuned during CPU probe to the CPU clockrate in Hz
 *
 */
ENTRY(delay)                    ! %o0 = n
        rdpr    %tick, %o1                                      ! Take timer snapshot
        sethi   %hi(cpu_clockrate), %o2
        sethi   %hi(MICROPERSEC), %o3
        ldx     [%o2 + %lo(cpu_clockrate + 8)], %o4             ! Get scale factor
        brnz,pt %o4, 0f
         or     %o3, %lo(MICROPERSEC), %o3

        !! Calculate ticks/usec
        ldx     [%o2 + %lo(cpu_clockrate)], %o4                 ! No, we need to calculate it
        udivx   %o4, %o3, %o4
        stx     %o4, [%o2 + %lo(cpu_clockrate + 8)]             ! Save it so we don't need to divide again
0:

        mulx    %o0, %o4, %o0                                   ! Convert usec -> ticks
        rdpr    %tick, %o2                                      ! Top of next itr
1:
        sub     %o2, %o1, %o3                                   ! How many ticks have gone by?
        sub     %o0, %o3, %o4                                   ! Decrement count by that much
        movrgz  %o3, %o4, %o0                                   ! But only if we're decrementing
        mov     %o2, %o1                                        ! Remember last tick
        brgz,pt %o0, 1b                                         ! Done?
         rdpr   %tick, %o2                                      ! Get new tick

        retl
         nop
END(delay)

#ifdef DDB
ENTRY(setjmp)
        save    %sp, -CC64FSZ, %sp      ! Need a frame to return to.
        flushw
        stx     %fp, [%i0+0]    ! 64-bit stack pointer
        stx     %i7, [%i0+8]    ! 64-bit return pc
        ret
         restore        %g0, 0, %o0
END(setjmp)

ENTRY(longjmp)
        save    %sp, -CC64FSZ, %sp      ! prepare to restore to (old) frame
        flushw
        mov     1, %i2
        ldx     [%i0+0], %fp    ! get return stack
        ldx     [%i0+8], %i7    ! get rpc
        ret
         restore        %i2, 0, %o0
END(longjmp)

        /*
         * Debug stuff.  Dump the trap registers into buffer & set tl=0.
         *
         *  %o0 = *ts
         */
ENTRY(savetstate)
        mov     %o0, %o1
        rdpr    %tl, %o0
        brz     %o0, 2f
         mov    %o0, %o2
1:
        rdpr    %tstate, %o3
        stx     %o3, [%o1]
        deccc   %o2
        inc     8, %o1
        rdpr    %tpc, %o4
        stx     %o4, [%o1]
        inc     8, %o1
        rdpr    %tnpc, %o5
        stx     %o5, [%o1]
        inc     8, %o1
        rdpr    %tt, %o4
        stx     %o4, [%o1]
        inc     8, %o1
        bnz     1b
         wrpr   %o2, 0, %tl
2:
        retl
         nop
END(savetstate)

        /*
         * Debug stuff.  Restore trap registers from buffer.
         *
         *  %o0 = %tl
         *  %o1 = *ts
         *
         * Maybe this should be re-written to increment tl instead of decrementing.
         */
ENTRY(restoretstate)
        flushw                  ! Make sure we don't have stack probs & lose hibits of %o
        brz,pn  %o0, 2f
         mov    %o0, %o2
        wrpr    %o0, 0, %tl
1:
        ldx     [%o1], %o3
        deccc   %o2
        inc     8, %o1
        wrpr    %o3, 0, %tstate
        ldx     [%o1], %o4
        inc     8, %o1
        wrpr    %o4, 0, %tpc
        ldx     [%o1], %o5
        inc     8, %o1
        wrpr    %o5, 0, %tnpc
        ldx     [%o1], %o4
        inc     8, %o1
        wrpr    %o4, 0, %tt
        bnz     1b
         wrpr   %o2, 0, %tl
2:
        retl
         wrpr   %o0, 0, %tl
END(restoretstate)

        /*
         * Switch to context in %o0
         */
ENTRY(switchtoctx)
        set     DEMAP_CTX_SECONDARY, %o3
        stxa    %o3, [%o3] ASI_DMMU_DEMAP
        membar  #Sync
        mov     CTX_SECONDARY, %o4
        stxa    %o3, [%o3] ASI_IMMU_DEMAP
        membar  #Sync
        stxa    %o0, [%o4] ASI_DMMU             ! Maybe we should invalidate the old context?
        membar  #Sync                           ! No real reason for this XXXX
        sethi   %hi(KERNBASE), %o2
        flush   %o2
        retl
         nop
END(switchtoctx)

#endif /* DDB */        /* DDB */

        .data
        _ALIGN
#if defined(DDB) || NKSYMS > 0
        .globl  esym
esym:
        .xword  0
        .globl  ssym
ssym:
        .xword  0
#endif  /* defined(DDB) || NKSYMS > 0 */
        .globl  proc0paddr
proc0paddr:
        .xword  u0                      ! KVA of proc0 uarea

#ifdef DEBUG
        .comm   pmapdebug, 4
#endif  /* DEBUG */

        .globl  dlflush_start
dlflush_start:
        .xword  dlflush1
        .xword  dlflush2
        .xword  dlflush3
        .xword  dlflush4
        .xword  0