/*      $NetBSD: locore_subr.S,v 1.61 2021/07/15 04:58:33 rin Exp $     */

/*-
 * Copyright (c) 2002 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "opt_compat_netbsd.h"
#include "opt_cputype.h"
#include "opt_ddb.h"
#include "opt_kgdb.h"
#include "opt_modular.h"
#include "assym.h"

#include <sys/syscall.h>        /* SYS___sigreturn14, SYS_exit */
#include <sh3/asm.h>
#include <sh3/locore.h>
#include <sh3/param.h>          /* UPAGES */
#include <sh3/mmu_sh3.h>
#include <sh3/mmu_sh4.h>

__KERNEL_RCSID(0, "$NetBSD: locore_subr.S,v 1.61 2021/07/15 04:58:33 rin Exp $")


/*
 * LINTSTUB: include <sys/types.h>
 * LINTSTUB: include <sys/lwp.h>
 * LINTSTUB: include <sh3/locore.h>
 */


/*
 * Save processor state to pcb->pcb_sf switchframe.
 * Note that offsetof(struct pcb, pcb_sf) is zero.
 */
#define SAVEPCB_AND_JUMP(pcb, jump)                       \
        add     #SF_SIZE, pcb                           ; \
        stc.l   r7_bank, @-pcb                          ; \
        stc.l   r6_bank, @-pcb                          ; \
        mov.l   r15, @-pcb                              ; \
        mov.l   r14, @-pcb                              ; \
        mov.l   r13, @-pcb                              ; \
        mov.l   r12, @-pcb                              ; \
        mov.l   r11, @-pcb                              ; \
        mov.l   r10, @-pcb                              ; \
        mov.l   r9, @-pcb                               ; \
        mov.l   r8, @-pcb                               ; \
        sts.l   pr, @-pcb                               ; \
        stc.l   sr, @-pcb                               ; \
        jump                                            ; \
         stc.l  gbr, @-pcb

/* Hide ugly empty argument if we don't need the jump */
#define SAVEPCB(pcb) \
        SAVEPCB_AND_JUMP(pcb, /* no jump */)


        .text
        .align 5        /* align cache line size (32B) */
/*
 * LINTSTUB: Func: lwp_t *cpu_switchto(lwp_t *olwp, lwp_t *nlwp, bool returning)
 *      Switch from olwp to nlwp.
 *      If returning is true, we do the fast softint dance
 *       and can skip user-space related activities (pmap, ras, etc...)
 *      Return olwp (in the nlwp context).
 */
ENTRY(cpu_switchto)
        !! save old lwp's context to switchframe
        mov.l   @(L_MD_PCB, r4), r1     ! olwp->l_md.md_pcb
        SAVEPCB(r1)

        !! free to use callee-save registers now

        mov.l   .L_curlwp, r2
        mov.l   .L_curpcb, r3
        mov.l   @(L_MD_PCB, r5), r10    ! nlwp->l_md.md_pcb
        !tst    r6, r6                  ! "full" switch?
        mov.l   r5, @r2                 ! curlwp = nlwp;
        !bt/s   .L_prepare_switch
         mov.l  r10, @r3                ! curpcb = nlwp->l_md.md_pcb;

        !mov    r5, r8          ! preserve nlwp
        !bra    .L_restore_nlwp
        ! mov   r4, r9          ! preserve olwp

.L_prepare_switch:
        !! arguments to cpu_switch_prepare are already in the right registers
        mov.l   .L_cpu_switch_prepare, r0
        mov     r5, r8          ! preserve nlwp
        jsr     @r0
         mov    r4, r9          ! preserve olwp

.L_restore_nlwp:
        !! restore new lwp's context from switchframe
        !! r10 is nlwp->l_md.md_pcb == &nlwp->l_md.md_pcb->pcb_sf

        !! setup nlwp's kernel stack first
        mov.l   @(SF_R7_BANK, r10), r0  ! kernel stack bottom
        mov.l   @(SF_R6_BANK, r10), r2  ! current trapframe
        mov.l   @(SF_R15, r10), r3      ! current kernel sp

        !! while switching kernel stack, all exceptions must be disabled
        __EXCEPTION_BLOCK(r1, r11)      ! saves SR in r11
        ldc     r0, r7_bank
        ldc     r2, r6_bank
        mov     r3, r15

#if !defined(P1_STACK) && defined(SH4)
        !! wire u-area in TLB
        MOV     (switch_resume, r0)
        jsr     @r0
         mov    r8, r4          ! nlwp
#endif
        !! safe to use nlwp's kernel stack now
        ldc     r11, sr         ! __EXCEPTION_UNBLOCK

        !! finish restoring new lwp's context from switchframe
        !! sf_r15, sf_r6_bank, sf_r7_bank are already restored
        mov     r10, r1         ! &nlwp->l_md.md_pcb->pcb_sf
        mov     r9, r0          ! return olwp (we are about to clobber r9)
        ldc.l   @r1+, gbr

        /*
         * We cannot simply pop SR here; PSL_IMASK field should be
         * inherited to nlwp. Otherwise, IPL is lost during context
         * switch, which allows improper interrupts when, e.g.,
         * spin mutexes are hold.
         */
        mov.l   @r1+, r8        ! r8  = new SR
        mov     #0x78, r9
        shll    r9              ! r9  =  PSL_IMASK
        not     r9, r10         ! r10 = ~PSL_IMASK
        and     r9, r11         ! r11 = old SR & PSL_IMASK
        and     r10, r8
        or      r11, r8
        ldc     r8, sr

        lds.l   @r1+, pr
        mov.l   @r1+, r8
        mov.l   @r1+, r9
        mov.l   @r1+, r10
        mov.l   @r1+, r11
        mov.l   @r1+, r12
        mov.l   @r1+, r13
        rts
         mov.l  @r1+, r14

        .align  2
.L_curlwp:              .long   _C_LABEL(curlwp)
.L_curpcb:              .long   _C_LABEL(curpcb)
.L_cpu_switch_prepare:  .long   _C_LABEL(cpu_switch_prepare)
#ifdef SH4
FUNC_SYMBOL(switch_resume)
#endif
        SET_ENTRY_SIZE(cpu_switchto)


#ifdef SH3
/*
 * LINTSTUB: Func: void sh3_switch_resume(struct lwp *l)
 *     We only need this dummy sh3 version if both SH3 and SH4 are defined.
 */
NENTRY(sh3_switch_resume)
        rts
         nop
        SET_ENTRY_SIZE(sh3_switch_resume)
#endif /* SH3 */


#ifdef SH4
/*
 * LINTSTUB: Func: void sh4_switch_resume(struct lwp *l)
 *      Wire u-area. invalidate TLB entry for kernel stack to prevent
 *      TLB multiple hit.
 */
NENTRY(sh4_switch_resume)
        add     #L_MD_UPTE, r4  ! l->l_md.md_upte
        mov     #UPAGES, r3
        mov.l   @r4, r0         ! if (l->l_md.md_upte[0].addr == 0) return;
        tst     r0, r0
        bt      2f

        /* Save old ASID and set ASID to zero */
        mov     #0, r0
        mov.l   .L_4_PTEH, r2
        mov.l   @r2, r7
        mov.l   r0, @r2

        mov.l   .L_VPN_MASK, r6
        mov.l   .L_4_UTLB_AA_A, r5

        /* TLB address array must be accessed via P2. Setup jump address. */
        mova    1f, r0
        mov.l   .L_P2BASE, r1
        or      r1, r0
        jmp     @r0             ! run on P2
         nop

        /* Probe VPN match TLB entry and invalidate it. */
        .align  2               ! mova target must be 4byte aligned
1:      mov.l   @(4, r4), r0
        and     r6, r0
        mov.l   r0, @r5         ! clear D, V

        /* Wire u-area TLB entry */
        /* Address array */
        mov.l   @r4+, r0        ! addr
        mov.l   @r4+, r1        ! data
        mov.l   r1, @r0         ! *addr = data

        /* Data array */
        mov.l   @r4+, r0        ! addr
        mov.l   @r4+, r1        ! data
        mov.l   r1, @r0         ! *addr = data
        dt      r3
        bf      1b

        /* restore ASID */
        mov.l   r7, @r2

2:      rts                     ! to the caller in P1
         nop

        .align  2
.L_4_PTEH:              .long   SH4_PTEH
.L_4_UTLB_AA_A:         .long   (SH4_UTLB_AA | SH4_UTLB_A)
.L_VPN_MASK:            .long   0xfffff000
.L_P2BASE:              .long   0xa0000000
        SET_ENTRY_SIZE(sh4_switch_resume)
#endif /* SH4 */


/*
 * LINTSTUB: Func: int _cpu_intr_raise(int s)
 *      raise SR.IMASK to 's'. if current SR.IMASK is greater equal 's',
 *      nothing to do. returns previous SR.IMASK.
 */
NENTRY(_cpu_intr_raise)
        stc     sr,     r2
        mov     #0x78,  r1
        mov     r2,     r0
        shll    r1              /* r1 = 0xf0 */
        and     r1,     r0      /* r0 = SR & 0xf0 */
        cmp/ge  r4,     r0      /* r0 >= r4 ? T = 1 */
        bt/s    1f
         not    r1,     r1      /* r1 = 0xffffff0f */
        and     r1,     r2      /* r2 = SR & ~0xf0 */
        or      r2,     r4      /* r4 = (SR & ~0xf0) | s */
        ldc     r4,     sr      /* SR = r4 (don't move to delay slot) */
1:      rts
         nop    /* return (SR & 0xf0) */
        SET_ENTRY_SIZE(_cpu_intr_raise)


/*
 * LINTSTUB: Func: int _cpu_intr_suspend(void)
 *      Mask all external interrupt. Returns previous SR.IMASK.
 */
NENTRY(_cpu_intr_suspend)
        stc     sr,     r0      /* r0 = SR */
        mov     #0x78,  r1
        shll    r1              /* r1 = 0x000000f0 */
        mov     r0,     r2      /* r2 = SR */
        or      r1,     r2      /* r2 |= 0x000000f0 */
        ldc     r2,     sr      /* SR = r2 */
        rts
         and    r1,     r0      /* r0 = SR & 0x000000f0 */
        SET_ENTRY_SIZE(_cpu_intr_suspend)



/*
 * LINTSTUB: Func: int _cpu_intr_resume(int s)
 *      Set 's' to SR.IMASK. Returns previous SR.IMASK.
 */
NENTRY(_cpu_intr_resume)
        stc     sr,     r0      /* r0 = SR */
        mov     #0x78,  r2
        shll    r2              /* r2 = 0x000000f0 */
        not     r2,     r1      /* r1 = 0xffffff0f */
        and     r0,     r1      /* r1 = (SR & ~0xf0) */
        or      r1,     r4      /* r4 = (SR & ~0xf0) | level */
        ldc     r4,     sr      /* SR = r0 (don't move to delay slot) */
        rts
         and    r2,     r0      /* return (SR & 0xf0) */
        SET_ENTRY_SIZE(_cpu_intr_resume)


/*
 * LINTSTUB: Func: int _cpu_exception_suspend(void)
 *      Block exception (SR.BL). if external interrupt raise, pending interrupt.
 *      if exception occur, jump to 0xa0000000 (hard reset).
 */
NENTRY(_cpu_exception_suspend)
        stc     sr, r0          /* r0 = SR */
        mov     #0x10, r1       /* bswap32(PSL_BL) - fits immediate */
        swap.b  r1, r1
        mov     r0, r2          /* r2 = r0 */
        swap.w  r1, r1          /* r1 = PSL_BL */
        or      r1, r2          /* r2 |= PSL_BL */
        ldc     r2, sr          /* SR = r2 */
        rts                     /* return old SR */
         nop
        SET_ENTRY_SIZE(_cpu_exception_suspend)


/*
 * LINTSTUB: Func: void _cpu_exception_resume(int s)
 *      restore 's' exception mask. (SR.BL)
 */
NENTRY(_cpu_exception_resume)
        stc     sr, r0          /* r0 = SR */
        mov     #0x10, r1       /* bswap32(PSL_BL) - fits immediate */
        swap.b  r1, r1
        swap.w  r1, r1
        not     r1, r1          /* r1 = ~PSL_BL */
        and     r1, r0          /* r0 &= ~PSL_BL */
        or      r4, r0          /* r0 |= old SR.BL */
        ldc     r0, sr          /* SR = r0 (don't move to delay slot) */
        rts
         nop
        SET_ENTRY_SIZE(_cpu_exception_resume)


/*
 * LINTSTUB: Func: void _cpu_spin(uint32_t count)
 *      Loop for 'count' * 10 cycles.
 * [...]
 * add    IF ID EX MA WB
 * nop       IF ID EX MA WB
 * cmp/pl       IF ID EX MA WB -  -
 * nop             IF ID EX MA -  -  WB
 * bt                 IF ID EX .  .  MA WB
 * nop                   IF ID -  -  EX MA WB
 * nop                      IF -  -  ID EX MA WB
 * nop                      -  -  -  IF ID EX MA WB
 * add                                  IF ID EX MA WB
 * nop                                     IF ID EX MA WB
 * cmp/pl                                     IF ID EX MA WB -  -
 * nop                                           IF ID EX MA -  - WB
 * bt                                               IF ID EX .  . MA
 * [...]
 */
        .align 5        /* align cache line size (32B) */
NENTRY(_cpu_spin)
1:      nop                     /* 1 */
        nop                     /* 2 */
        nop                     /* 3 */
        add     #-1, r4         /* 4 */
        nop                     /* 5 */
        cmp/pl  r4              /* 6 */
        nop                     /* 7 */
        bt      1b              /* 8, 9, 10 */
        rts
         nop
        SET_ENTRY_SIZE(_cpu_spin)


/*
 * lwp_trampoline:
 *
 * cpu_lwp_fork() arranges for lwp_trampoline() to run when that
 * nascent lwp is selected by cpu_switchto().
 *
 * The switch frame will contain pointer to struct lwp of this lwp in
 * r10, a pointer to the function to call in r12, and an argument to
 * pass to it in r11 (we abuse the callee-saved registers).
 *
 * We enter lwp_trampoline as if we are "returning" from
 * cpu_switchto(), so r0 contains previous lwp (the one we are
 * switching from) that we pass to lwp_startup().
 *
 * After that the trampoline should call the function that is intended
 * to do some additional setup.  When the function returns, the
 * trampoline returns to the user mode.
 */
NENTRY(lwp_trampoline)
        mov.l   .L_lwp_startup, r1
        mov     r0, r4          /* previous lwp returned by cpu_switchto */
        jsr     @r1
         mov    r10, r5         /* my struct lwp */
        jsr     @r12
         mov    r11, r4
        __EXCEPTION_RETURN
        /* NOTREACHED */

        .align  2
.L_lwp_startup:         .long   _C_LABEL(lwp_startup)

        SET_ENTRY_SIZE(lwp_trampoline)


#if defined(COMPAT_16) || defined(MODULAR)
/*
 * LINTSTUB: Var: char sigcode[1]
 *      Signal trampoline. copied to top of user stack.
 *
 *      The kernel arranges for the signal handler to be invoked directly.
 *      This trampoline is used only to perform the return.
 *
 *      On entry, the stack looks like this:
 *
 *      sp->    sigcontext structure
 */
NENTRY(sigcode)
        mov     r15, r4                 /* get pointer to sigcontext */
        mov.l   .L_SYS___sigreturn14, r0
        trapa   #0x80                   /* and call sigreturn() */
        mov.l   .L_SYS_exit, r0
        trapa   #0x80                   /* exit if sigreturn fails */
        /* NOTREACHED */

        .align  2
.L_SYS___sigreturn14:   .long   SYS_compat_16___sigreturn14
.L_SYS_exit:            .long   SYS_exit

/* LINTSTUB: Var: char esigcode[1] */
.globl  _C_LABEL(esigcode)
_C_LABEL(esigcode):
        SET_ENTRY_SIZE(sigcode)
#endif /* COMPAT_16 || MODULAR */


/*
 * LINTSTUB: Func: void savectx(struct pcb *pcb)
 *      Save CPU state in pcb->pcb_sf
 */
ENTRY(savectx)
        SAVEPCB_AND_JUMP(r4, rts)
        SET_ENTRY_SIZE(savectx)


/*
 * LINTSTUB: Func: int copyout(const void *ksrc, void *udst, size_t len)
 *      Copy len bytes into the user address space.
 */
ENTRY(copyout)
        mov.l   r14,    @-r15
        sts.l   pr,     @-r15
        mov     r15,    r14

        mov     r4,     r3
        mov     r5,     r2
        mov     r5,     r4
        add     r6,     r2
        cmp/hs  r5,     r2              /* bomb if uaddr+len wraps */
        bf      3f
        mov.l   .L_copyout_VM_MAXUSER_ADDRESS, r1
        cmp/hi  r1,     r2              /* bomb if uaddr isn't in user space */
        bt      3f

        mov.l   .L_copyout_curpcb, r1   /* set fault handler */
        mov.l   @r1,    r2
        mov.l   .L_copyout_onfault, r1
        mov.l   r1,     @(PCB_ONFAULT,r2)
        mov.l   .L_copyout_memcpy, r1
        jsr     @r1                     /* memcpy(uaddr, kaddr, len) */
         mov    r3,     r5

        mov     #0,     r0
1:
        mov.l   .L_copyout_curpcb, r1   /* clear fault handler */
        mov.l   @r1,    r2
        mov     #0,     r1
        mov.l   r1,     @(PCB_ONFAULT,r2)
2:
        mov     r14,    r15
        lds.l   @r15+,  pr
        rts
         mov.l  @r15+,  r14

3:
        bra     2b
         mov    #EFAULT, r0

        .align 2
.L_copyout_onfault:
        .long   1b
.L_copyout_VM_MAXUSER_ADDRESS:
        .long   VM_MAXUSER_ADDRESS
.L_copyout_curpcb:
        .long   _C_LABEL(curpcb)
.L_copyout_memcpy:
        .long   _C_LABEL(memcpy)
        SET_ENTRY_SIZE(copyout)


/*
 * LINTSTUB: Func: int copyin(const void *usrc, void *kdst, size_t len)
 *      Copy len bytes from the user address space.
 */
ENTRY(copyin)
        mov.l   r14,    @-r15
        sts.l   pr,     @-r15
        mov     r15,    r14

        mov     r4,     r3
        mov     r5,     r4
        mov     r3,     r2
        add     r6,     r2
        cmp/hs  r3,     r2              /* bomb if uaddr+len wraps */
        bf      3f
        mov.l   .L_copyin_VM_MAXUSER_ADDRESS, r1
        cmp/hi  r1,     r2              /* bomb if uaddr isn't in user space */
        bt      3f

        mov.l   .L_copyin_curpcb, r1    /* set fault handler */
        mov.l   @r1,    r2
        mov.l   .L_copyin_onfault, r1
        mov.l   r1,     @(PCB_ONFAULT,r2)
        mov.l   .L_copyin_memcpy, r1
        jsr     @r1                     /* memcpy(kaddr, uaddr, len) */
         mov    r3,     r5

        mov     #0,     r0
1:
        mov.l   .L_copyin_curpcb, r1    /* clear fault handler */
        mov.l   @r1,    r2
        mov     #0,     r1
        mov.l   r1,     @(PCB_ONFAULT,r2)
2:
        mov     r14,    r15
        lds.l   @r15+,  pr
        rts
         mov.l  @r15+,  r14

3:
        bra     2b
         mov    #EFAULT, r0

        .align 2
.L_copyin_onfault:
        .long   1b
.L_copyin_VM_MAXUSER_ADDRESS:
        .long   VM_MAXUSER_ADDRESS
.L_copyin_curpcb:
        .long   _C_LABEL(curpcb)
.L_copyin_memcpy:
        .long   _C_LABEL(memcpy)
        SET_ENTRY_SIZE(copyin)


/*
 * LINTSTUB: Func: int copyoutstr(const void *ksrc, void *udst, size_t maxlen, size_t *lencopied)
 *      Copy a NUL-terminated string, at most maxlen characters long,
 *      into the user address space.  Return the number of characters
 *      copied (including the NUL) in *lencopied.  If the string is
 *      too long, return ENAMETOOLONG; else return 0 or EFAULT.
 */
ENTRY(copyoutstr)
        mov.l   r8,     @-r15

        mov     r4,     r8
        mov.l   .L_copyoutstr_curpcb, r1        /* set fault handler */
        mov.l   @r1,    r2
        mov.l   .L_copyoutstr_onfault, r1
        mov.l   r1,     @(PCB_ONFAULT,r2)
        mov.l   .L_copyoutstr_VM_MAXUSER_ADDRESS, r3
        cmp/hi  r3,     r5              /* bomb if udst isn't in user space */
        bt      5f
        mov     r3,     r0
        sub     r5,     r0
        cmp/hi  r6,     r0              /* don't beyond user space */
        bf      2f
        bra     2f
         mov    r6,     r0

        .align 2
1:
        mov.b   @r4+,   r1              /* copy str */
        mov.b   r1,     @r5
        extu.b  r1,     r1
        add     #1,     r5
        tst     r1,     r1
        bf      2f
        bra     3f
         mov    #0,     r0

        .align 2
2:
        add     #-1,    r0
        cmp/eq  #-1,    r0
        bf      1b
        cmp/hi  r3,     r5
        bf      6f
        mov     #0,     r0

3:
        tst     r7,     r7              /* set lencopied if needed */
        bt      4f
        mov     r4,     r1
        sub     r8,     r1
        mov.l   r1,     @r7

4:
        mov.l   .L_copyoutstr_curpcb, r1        /* clear fault handler */
        mov.l   @r1,    r2
        mov     #0,     r1
        mov.l   r1,     @(PCB_ONFAULT,r2)

        rts
         mov.l  @r15+,  r8

5:
        bra     4b
         mov    #EFAULT, r0

6:      bra     3b
         mov    #ENAMETOOLONG, r0

        .align 2
.L_copyoutstr_onfault:
        .long   4b
.L_copyoutstr_VM_MAXUSER_ADDRESS:
        .long   VM_MAXUSER_ADDRESS
.L_copyoutstr_curpcb:
        .long   _C_LABEL(curpcb)
        SET_ENTRY_SIZE(copyoutstr)


/*
 * LINTSTUB: Func: int copyinstr(const void *usrc, void *kdst, size_t maxlen, size_t *lencopied)
 *      Copy a NUL-terminated string, at most maxlen characters long,
 *      from the user address space.  Return the number of characters
 *      copied (including the NUL) in *lencopied.  If the string is
 *      too long, return ENAMETOOLONG; else return 0 or EFAULT.
 */
ENTRY(copyinstr)
        mov.l   r8,     @-r15

        mov     r4,     r8
        mov.l   .L_copyinstr_curpcb, r1 /* set fault handler */
        mov.l   @r1,    r2
        mov.l   .L_copyinstr_onfault, r1
        mov.l   r1,     @(PCB_ONFAULT,r2)

        mov.l   .L_copyinstr_VM_MAXUSER_ADDRESS, r3
        cmp/hi  r3,     r4              /* bomb if usrc isn't in user space */
        bt      5f
        mov     r3,     r0
        sub     r4,     r0
        cmp/hi  r6,     r0              /* don't beyond user space */
        bf      2f
        bra     2f
         mov    r6,     r0

        .align 2
1:
        mov.b   @r4+,   r1              /* copy str */
        mov.b   r1,     @r5
        extu.b  r1,     r1
        add     #1,     r5
        tst     r1,     r1
        bf      2f
        bra     3f
         mov    #0,     r0

        .align 2
2:
        add     #-1,    r0
        cmp/eq  #-1,    r0
        bf      1b
        cmp/hi  r3,     r4
        bf      6f
        mov     #0,     r0

3:
        tst     r7,     r7              /* set lencopied if needed */
        bt      4f
        mov     r4,     r1
        sub     r8,     r1
        mov.l   r1,     @r7

4:
        mov.l   .L_copyinstr_curpcb, r1 /* clear fault handler */
        mov.l   @r1,    r2
        mov     #0,     r1
        mov.l   r1,     @(PCB_ONFAULT,r2)

        rts
         mov.l  @r15+,  r8

5:
        bra     4b
         mov    #EFAULT, r0

6:
        bra     3b
         mov    #ENAMETOOLONG, r0

        .align 2
.L_copyinstr_onfault:
        .long   4b
.L_copyinstr_VM_MAXUSER_ADDRESS:
        .long   VM_MAXUSER_ADDRESS
.L_copyinstr_curpcb:
        .long   _C_LABEL(curpcb)
        SET_ENTRY_SIZE(copyinstr)

/**************************************************************************/

#define UFETCHSTORE_PROLOGUE(func)                                       \
        mov.l   .L ## func ## _VM_MAXUSER_ADDRESS, r1                   ;\
        cmp/hi  r1,     r4      /* bomb if uaddr isn't in user space */ ;\
        bt/s    2f                                                      ;\
         mov    #EFAULT,r0                                              ;\
        mov.l   .L ## func ## _curpcb, r1                               ;\
        mov.l   @r1,    r2      /* r2 = curpcb */                       ;\
        mov.l   .L ## func ## _onfault, r1                              ;\
        mov.l   r1,     @(PCB_ONFAULT,r2)       /* set pcb_onfault */

#define UFETCHSTORE_EPILOGUE(func)                                       \
        mov     #0,     r0      /* return "success!" */                 ;\
1:                                                                      ;\
        mov.l   .L ## func ## _curpcb, r1                               ;\
        mov.l   @r1,    r2      /* r2 = curpcb */                       ;\
        mov     #0,     r1                                              ;\
        mov.l   r1,     @(PCB_ONFAULT,r2)                               ;\
2:                                                                      ;\
        rts                                                             ;\
         nop                                                            ;\
                                                                         \
        .align 2                                                        ;\
.L ## func ## _onfault:                                                 ;\
        .long   1b                                                      ;\
.L ## func ## _VM_MAXUSER_ADDRESS:                                      ;\
        .long   VM_MAXUSER_ADDRESS - 4  /* sizeof(long) */              ;\
.L ## func ## _curpcb:                                                  ;\
        .long   _C_LABEL(curpcb)

/* LINTSTUB: int _ufetch_8(const uint8_t *uaddr, uint8_t *valp); */
ENTRY(_ufetch_8)
        UFETCHSTORE_PROLOGUE(_ufetch_8)
        mov.b   @r4,    r1      /* r1 = *uaddr */
        mov.b   r1,     @r5     /* *valp = r1 */
        UFETCHSTORE_EPILOGUE(_ufetch_8)
        SET_ENTRY_SIZE(_ufetch_8)

/* LINTSTUB: int _ufetch_16(const uint16_t *uaddr, uint16_t *valp); */
ENTRY(_ufetch_16)
        UFETCHSTORE_PROLOGUE(_ufetch_16)
        mov.w   @r4,    r1      /* r1 = *uaddr */
        mov.w   r1      @r5     /* *valp = r1 */
        UFETCHSTORE_EPILOGUE(_ufetch_16)
        SET_ENTRY_SIZE(_ufetch_16)

/* LINTSTUB: int _ufetch_32(const uint32_t *uaddr, uint32_t *valp); */
ENTRY(_ufetch_32)
        UFETCHSTORE_PROLOGUE(_ufetch_32)
        mov.l   @r4,    r1      /* r1 = *uaddr */
        mov.l   r1,     @r5     /* *valp = r1 */
        UFETCHSTORE_EPILOGUE(_ufetch_32)
        SET_ENTRY_SIZE(_ufetch_32)

/* LINTSTUB: int _ustore_8(uint8_t *uaddr, uint8_t val); */
ENTRY(_ustore_8)
        UFETCHSTORE_PROLOGUE(_ustore_8)
        mov.b   r5,     @r4     /* *uaddr = val */
        UFETCHSTORE_EPILOGUE(_ustore_8)
        SET_ENTRY_SIZE(_ustore_8)

/* LINTSTUB: int _ustore_16(uint16_t *uaddr, uint16_t val); */
ENTRY(_ustore_16)
        UFETCHSTORE_PROLOGUE(_ustore_16)
        mov.w   r5,     @r4     /* *uaddr = val */
        UFETCHSTORE_EPILOGUE(_ustore_16)
        SET_ENTRY_SIZE(_ustore_16)

/* LINTSTUB: int _ustore_32(uint32_t *uaddr, uint32_t val); */
ENTRY(_ustore_32)
        UFETCHSTORE_PROLOGUE(_ustore_32)
        mov.l   r5,     @r4     /* *uaddr = val */
        UFETCHSTORE_EPILOGUE(_ustore_32)
        SET_ENTRY_SIZE(_ustore_32)

/**************************************************************************/

/*
 * LINTSTUB: Func: int kcopy(const void *src, void *dst, size_t len)
 */
ENTRY(kcopy)
        mov.l   r8,     @-r15
        mov.l   r14,    @-r15
        sts.l   pr,     @-r15
        mov     r15,    r14

        mov     r4,     r3
        mov.l   .L_kcopy_curpcb, r1
        mov.l   @r1,    r2
        mov.l   @(PCB_ONFAULT,r2) ,r8   /* save old fault handler */
        mov.l   .L_kcopy_onfault, r1
        mov.l   r1,     @(PCB_ONFAULT,r2) /* set fault handler */
        mov.l   .L_kcopy_memcpy, r1
        mov     r5,     r4
        jsr     @r1                     /* memcpy(dst, src, len) */
         mov    r3,     r5
        mov     #0,     r0
1:
        mov.l   .L_kcopy_curpcb, r1     /* restore fault handler */
        mov.l   @r1,    r2
        mov.l   r8,     @(PCB_ONFAULT,r2)

        mov     r14,    r15
        lds.l   @r15+,  pr
        mov.l   @r15+,  r14
        rts
         mov.l  @r15+,  r8

        .align 2
.L_kcopy_onfault:
        .long   1b
.L_kcopy_curpcb:
        .long   _C_LABEL(curpcb)
.L_kcopy_memcpy:
        .long   _C_LABEL(memcpy)
        SET_ENTRY_SIZE(kcopy)


#if defined(DDB) || defined(KGDB)

/*
 * LINTSTUB: Func: int setjmp(label_t *jmpbuf)
 */
ENTRY(setjmp)
        add     #4*9,   r4
        mov.l   r8,     @-r4
        mov.l   r9,     @-r4
        mov.l   r10,    @-r4
        mov.l   r11,    @-r4
        mov.l   r12,    @-r4
        mov.l   r13,    @-r4
        mov.l   r14,    @-r4
        mov.l   r15,    @-r4
        sts.l   pr,     @-r4
        rts
         xor    r0, r0
        SET_ENTRY_SIZE(setjmp)

/*
 * LINTSTUB: Func: void longjmp(label_t *jmpbuf)
 */
ENTRY(longjmp)
        lds.l   @r4+,   pr
        mov.l   @r4+,   r15
        mov.l   @r4+,   r14
        mov.l   @r4+,   r13
        mov.l   @r4+,   r12
        mov.l   @r4+,   r11
        mov.l   @r4+,   r10
        mov.l   @r4+,   r9
        mov.l   @r4+,   r8
        rts
         mov    #1, r0          /* return 1 from setjmp */
        SET_ENTRY_SIZE(longjmp)

#endif /* DDB || KGDB */