/*	$NetBSD: mp_subr.S,v 1.10 2016/05/16 20:03:07 palle Exp $	*/

/*
 * Copyright (c) 2006-2010 Matthew R. Green
 * Copyright (c) 1996-2002 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
 */

#include "opt_ddb.h"
#include "opt_kgdb.h"
#include "opt_multiprocessor.h"
#include "opt_compat_netbsd.h"
#include "opt_compat_netbsd32.h"
#include "opt_lockdebug.h"

#include "assym.h"
#include <machine/param.h>
#include <sparc64/sparc64/intreg.h>
#include <sparc64/sparc64/timerreg.h>
#include <machine/ctlreg.h>
#include <machine/psl.h>
#include <machine/signal.h>
#include <machine/trap.h>
#include <machine/frame.h>
#include <machine/pmap.h>
#include <machine/intr.h>
#include <machine/asm.h>
#include <machine/locore.h>
#include <sys/syscall.h>
#ifdef SUN4V
#include <machine/hypervisor.h>
#endif	
#include "ksyms.h"

	.register	%g2,#scratch
	.register	%g3,#scratch

#define BLOCK_SIZE SPARC64_BLOCK_SIZE
#define BLOCK_ALIGN SPARC64_BLOCK_ALIGN

#if defined(MULTIPROCESSOR)
/*
 * IPI handler to do nothing, but causes rescheduling..
 * void sparc64_ipi_nop(void *);
 */
ENTRY(sparc64_ipi_nop)
	ba,a	ret_from_intr_vector
	 nop

/*
 * IPI handler to halt the CPU.  Just calls the C vector.
 * void sparc64_ipi_halt(void *);
 */
ENTRY(sparc64_ipi_halt)
	call	_C_LABEL(sparc64_ipi_halt_thiscpu)
	 clr	%g4
	sir

/*
 * IPI handler to pause the CPU.  We just trap to the debugger if it
 * is configured, otherwise just return.
 */
ENTRY(sparc64_ipi_pause)
#if defined(DDB)
	.global sparc64_ipi_pause_trap_point
sparc64_ipi_pause_trap_point:
	ta	1
	 nop
#endif
	ba,a	ret_from_intr_vector
	 nop

/*
 * Increment IPI event counter, defined in machine/{cpu,intr}.h.
 */
#define IPIEVC_INC(n,r1,r2)						\
	sethi	%hi(CPUINFO_VA+CI_IPIEVC+EVC_SIZE*n), r2;		\
	ldx	[r2 + %lo(CPUINFO_VA+CI_IPIEVC+EVC_SIZE*n)], r1;	\
	inc	r1;							\
	stx	r1, [r2 + %lo(CPUINFO_VA+CI_IPIEVC+EVC_SIZE*n)]

/*
 * void sparc64_ipi_flush_pte_us(void *);
 * void sparc64_ipi_flush_pte_usiii(void *);
 *
 * IPI handler to flush single pte.  We enter here with %tl already 1
 * and PSTATE_IE already disabled, so there's no need to do it again.
 *
 * On entry:
 *	%g2 = vaddr_t va
 *	%g3 = int ctx
 */
ENTRY(sparc64_ipi_flush_pte_us)
	srlx	%g2, PG_SHIFT4U, %g2		! drop unused va bits
	mov	CTX_SECONDARY, %g5
	sllx	%g2, PG_SHIFT4U, %g2
	ldxa	[%g5] ASI_DMMU, %g6		! Save secondary context
	sethi	%hi(KERNBASE), %g7
	membar	#LoadStore
	stxa	%g3, [%g5] ASI_DMMU		! Insert context to demap
	membar	#Sync
	or	%g2, DEMAP_PAGE_SECONDARY, %g2	! Demap page from secondary context only
	stxa	%g2, [%g2] ASI_DMMU_DEMAP	! Do the demap
	stxa	%g2, [%g2] ASI_IMMU_DEMAP	! to both TLBs
#ifdef TLB_FLUSH_LOWVA
	srl	%g2, 0, %g2			! and make sure it's both 32- and 64-bit entries
	stxa	%g2, [%g2] ASI_DMMU_DEMAP	! Do the demap
	stxa	%g2, [%g2] ASI_IMMU_DEMAP	! Do the demap
#endif
	flush	%g7
	stxa	%g6, [%g5] ASI_DMMU		! Restore secondary context
	membar	#Sync
	IPIEVC_INC(IPI_EVCNT_TLB_PTE,%g2,%g3)
	 
	ba,a	ret_from_intr_vector
	 nop

ENTRY(sparc64_ipi_flush_pte_usiii)
	andn	%g2, 0xfff, %g2			! drop unused va bits
	mov	CTX_PRIMARY, %g5
	ldxa	[%g5] ASI_DMMU, %g6		! Save primary context
	sethi	%hi(KERNBASE), %g7
	membar	#LoadStore
	stxa	%g3, [%g5] ASI_DMMU		! Insert context to demap
	membar	#Sync
	or	%g2, DEMAP_PAGE_PRIMARY, %g2
	stxa	%g2, [%g2] ASI_DMMU_DEMAP	! Do the demap
	stxa	%g2, [%g2] ASI_IMMU_DEMAP	! to both TLBs
#ifdef TLB_FLUSH_LOWVA
	srl	%g2, 0, %g2			! and make sure it's both 32- and 64-bit entries
	stxa	%g2, [%g2] ASI_DMMU_DEMAP	! Do the demap
	stxa	%g2, [%g2] ASI_IMMU_DEMAP	! Do the demap
#endif
	membar	#Sync
	flush	%g7
	stxa	%g6, [%g5] ASI_DMMU		! Restore primary context
	membar	#Sync
	flush	%g7
	IPIEVC_INC(IPI_EVCNT_TLB_PTE,%g2,%g3)

	ba,a	ret_from_intr_vector
	 nop

#ifdef SUN4V
ENTRY(sparc64_ipi_flush_pte_sun4v)
	mov	%o0, %g1		! save input
	mov	%o1, %g2
	mov	%o2, %g4
	mov	%g3, %o0		! vaddr
	mov	%g5, %o1		! ctx
	mov	MAP_DTLB|MAP_ITLB, %o2	! flags
	ta	ST_MMU_UNMAP_ADDR
	mov	%g1, %o0		! restore input
	mov	%g2, %o1
	mov	%g4, %o2

	retry
#endif	

/*
 * Secondary CPU bootstrap code.
 */
	.text
	.align 32
1:	rd	%pc, %l0
	LDULNG	[%l0 + (3f-1b)], %l1	! Load itlb slot count
	LDULNG	[%l0 + (7f-1b)], %g2	! Load cpu_args address.
	add	%l0, (6f-1b), %l2	! tlb slots
	ld	[%g2 + CBA_CPUTYP], %g3 ! Load cputype
	clr	%l3
.Litlb_loop:
	cmp	%l3, %l1
	be	CCCR, .Litlb_done
	 nop
	ldx	[%l2 + TTE_VPN], %l4
	ldx	[%l2 + TTE_DATA], %l5
#ifdef SUN4V	
	cmp	%g3, CPU_SUN4V
	bne,pt	%icc, .Litlb_4u
	 nop
	! sun4v
	mov	%l4, %o0			! vaddr
	clr	%o1				! reserved
	mov	%l5, %o2			! tte
	mov	MAP_DTLB|MAP_ITLB, %o3		! flags
	mov	FT_MMU_MAP_PERM_ADDR, %o5	! hv fast trap function
	ta	ST_FAST_TRAP
	cmp	%o0, 0
	be,pt	%icc, .Litlb_next
	 nop
	sir					! crash if mapping fails
.Litlb_4u:
#endif	
	! sun4u
	wr	%g0, ASI_DMMU, %asi
	stxa	%l4, [%g0 + TLB_TAG_ACCESS] %asi
	stxa	%l5, [%g0] ASI_DMMU_DATA_IN
	wr	%g0, ASI_IMMU, %asi
	stxa	%l4, [%g0 + TLB_TAG_ACCESS] %asi
	stxa	%l5, [%g0] ASI_IMMU_DATA_IN
.Litlb_next:
	membar	#Sync
	flush	%l4
	add	%l2, PTE_SIZE, %l2
	add	%l3, 1, %l3
	ba	%xcc, .Litlb_loop
	 nop
.Litlb_done:
	! continue the same loop (with indices and pointers et al),
	! but load a new upper limit and do not push the entries into
	! the itlb
	LDULNG	[%l0 + (4f-1b)], %l1	! Load dtlb slot count
.Ldtlb_loop:
	cmp	%l3, %l1
	be	CCCR, .Ldtlb_done
	 nop
	ldx	[%l2 + TTE_VPN], %l4
	ldx	[%l2 + TTE_DATA], %l5
#ifdef SUN4V	
	cmp	%g3, CPU_SUN4V
	bne,pt	%icc, .Ldtlb_4u
	 nop
	! sun4v
	mov	%l4, %o0			! vaddr
	clr	%o1				! reserved
	mov	%l5, %o2			! tte
	mov	MAP_DTLB, %o3			! flags
	mov	FT_MMU_MAP_PERM_ADDR, %o5	! hv fast trap function
	ta	ST_FAST_TRAP
	cmp	%o0, 0
	be,pt	%icc, .Ldtlb_next
	 nop
	sir					! crash if mapping fails
.Ldtlb_4u:
#endif	
	! sun4u
	wr	%g0, ASI_DMMU, %asi
	stxa	%l4, [%g0 + TLB_TAG_ACCESS] %asi
	stxa	%l5, [%g0] ASI_DMMU_DATA_IN
.Ldtlb_next:
	membar	#Sync
	flush	%l4
	add	%l2, PTE_SIZE, %l2
	add	%l3, 1, %l3
	ba	%xcc, .Ldtlb_loop
	 nop
.Ldtlb_done:
	LDULNG	[%l0 + (5f-1b)], %l1	! Load function
	jmpl	%l1, %g0
	 nop

	.align PTRSZ
4:	ULONG	0x0
3:	ULONG	0x0
5:	ULONG	0x0
7:	ULONG	0x0
	_ALIGN
6:

#define DATA(name) \
        .data ; \
        .align PTRSZ ; \
        .globl  name ; \
name:

DATA(mp_tramp_code)
	POINTER	1b
DATA(mp_tramp_code_len)
	ULONG	6b-1b
DATA(mp_tramp_dtlb_slots)
	ULONG	4b-1b
DATA(mp_tramp_itlb_slots)
	ULONG	3b-1b
DATA(mp_tramp_func)
	ULONG	5b-1b
DATA(mp_tramp_ci)
	ULONG	7b-1b

	.text
	.align 32


/*
 * IPI handler to store the current FPU state.
 * void sparc64_ipi_save_fpstate(void *);
 *
 * On entry:
 *	%g2 = lwp
 */
ENTRY(sparc64_ipi_save_fpstate)
	sethi	%hi(FPLWP), %g1
	LDPTR	[%g1 + %lo(FPLWP)], %g3
	cmp	%g3, %g2
	bne,pn	CCCR, 7f		! skip if fplwp has changed

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

	LDPTR	[%g3 + L_FPSTATE], %g3
	stx	%fsr, [%g3 + FS_FSR]	! f->fs_fsr = getfsr();
	rd	%gsr, %g2		! Save %gsr
	st	%g2, [%g3 + FS_GSR]
#if FS_REGS > 0
	add	%g3, FS_REGS, %g3
#endif
#ifdef DIAGNOSTIC
	btst	BLOCK_ALIGN, %g3	! Needs to be re-executed
	bnz,pn	%icc, 6f		! Check alignment
#endif
	 st	%g0, [%g3 + FS_QSIZE - FS_REGS]	! f->fs_qsize = 0;
	btst	FPRS_DL|FPRS_DU, %g5	! Both FPU halves clean?
	bz,pt	%icc, 5f		! Then skip it

	 mov	CTX_PRIMARY, %g2
	ldxa	[%g2] ASI_DMMU, %g6
	membar	#LoadStore
	stxa	%g0, [%g2] ASI_DMMU	! Switch MMU to kernel primary context
	membar	#Sync

	btst	FPRS_DL, %g5		! Lower FPU clean?
	bz,a,pt	%icc, 1f		! Then skip it, but upper FPU not clean
	 add	%g3, 2*BLOCK_SIZE, %g3	! Skip a block

	stda	%f0, [%g3] ASI_BLK_P	! f->fs_f0 = etc;
	inc	BLOCK_SIZE, %g3
	stda	%f16, [%g3] ASI_BLK_P

	btst	FPRS_DU, %g5		! Upper FPU clean?
	bz,pt	%icc, 2f		! Then skip it
	 inc	BLOCK_SIZE, %g3
1:
	stda	%f32, [%g3] ASI_BLK_P
	inc	BLOCK_SIZE, %g3
	stda	%f48, [%g3] ASI_BLK_P
2:
	membar	#Sync			! Finish operation so we can
	brz,pn	%g6, 5f			! Skip if context 0
	 nop
	stxa	%g6, [%g2] ASI_DMMU	! Restore primary context
	membar	#Sync
5:
	wr	%g0, FPRS_FEF, %fprs	! Mark FPU clean
	STPTR	%g0, [%g1 + %lo(FPLWP)]	! fplwp = NULL
7:
	IPIEVC_INC(IPI_EVCNT_FPU_SYNCH,%g2,%g3)
	ba,a	ret_from_intr_vector
	 nop

#ifdef DIAGNOSTIC
	!!
	!! Damn thing is *NOT* aligned on a 64-byte boundary
	!! 
6:
	wr	%g0, FPRS_FEF, %fprs
	! XXX -- we should panic instead of silently entering debugger
	ta	1
	 nop
	ba,a	ret_from_intr_vector
	 nop
#endif

/*
 * IPI handler to drop the current FPU state.
 * void sparc64_ipi_drop_fpstate(void *);
 *
 * On entry:
 *	%g2 = lwp
 */
ENTRY(sparc64_ipi_drop_fpstate)
	rdpr	%pstate, %g1
	wr	%g0, FPRS_FEF, %fprs
	or	%g1, PSTATE_PEF, %g1
	wrpr	%g1, 0, %pstate
	set	FPLWP, %g1
	CASPTRA	[%g1] ASI_N, %g2, %g0	! fplwp = NULL if fplwp == %g2
	membar	#Sync			! Should not be needed due to retry
	IPIEVC_INC(IPI_EVCNT_FPU_FLUSH,%g2,%g3)
	ba,a	ret_from_intr_vector
	 nop

/*
 * Flush data cache page.
 * void sparc64_ipi_dcache_flush_page_usiii(paddr_t pa, int line_size)
 * void sparc64_ipi_dcache_flush_page_us(paddr_t pa, int line_size)
 * void sparc64_ipi_dcache_flush_page_sun4v(paddr_t pa, int line_size)
 *
 * On entry:
 *	%g2 = pa
 *	%g3 = line_size
 */
ENTRY(sparc64_ipi_dcache_flush_page_usiii)
	set	NBPG, %g1
	add	%g2, %g1, %g1	! end address

1:
	stxa	%g0, [%g2] ASI_DCACHE_INVALIDATE
	add	%g2, %g3, %g2
	cmp	%g2, %g1
	bl,pt	%xcc, 1b
	 nop

	sethi	%hi(KERNBASE), %g5
	flush	%g5
	membar	#Sync
	ba,a	ret_from_intr_vector
	 nop

ENTRY(sparc64_ipi_dcache_flush_page_us)
	mov	-1, %g1		! Generate mask for tag: bits [29..2]
	srlx	%g2, 13-2, %g5	! Tag is PA bits <40:13> in bits <29:2>
	clr	%g4
	srl	%g1, 2, %g1	! Now we have bits <29:0> set
	set	(2*NBPG), %g7
	ba,pt	%icc, 1f
	 andn	%g1, 3, %g1	! Now we have bits <29:2> set

	.align 8
1:
	ldxa	[%g4] ASI_DCACHE_TAG, %g6
	mov	%g4, %g2
	deccc	32, %g7
	bl,pn	%icc, 2f
	 inc	32, %g4

	xor	%g6, %g5, %g6
	andcc	%g6, %g1, %g0
	bne,pt	%xcc, 1b
	 membar	#LoadStore

	stxa	%g0, [%g2] ASI_DCACHE_TAG
	ba,pt	%icc, 1b
	 membar	#StoreLoad
2:

	sethi	%hi(KERNBASE), %g5
	flush	%g5
	membar	#Sync
	ba,a	ret_from_intr_vector
	 nop

#ifdef SUN4V
ENTRY(sparc64_ipi_dcache_flush_page_sun4v)
	set	NBPG, %o1
	call	hv_mem_sync
	 mov	%g2, %o0
	cmp	%o0, 0
	be,pt	%icc, 1f
	 nop
	sir				! crash if hv-call fails
1:
	sethi	%hi(KERNBASE), %g5
	flush	%g5
	membar	#Sync
	ba,a	ret_from_intr_vector
	 nop
#endif
		
#endif