/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License, Version 1.0 only
 * (the "License").  You may not use this file except in compliance
 * with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */
/*
 * Copyright 2005 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

#include <sys/param.h>
#include <sys/errno.h>
#include <sys/asm_linkage.h>
#include <sys/vtrace.h>
#include <sys/machthread.h>
#include <sys/clock.h>
#include <sys/asi.h>
#include <sys/fsr.h>
#include <sys/privregs.h>

#include "assym.h"

#define	FP_USED 1
#define	LOFAULT_SET 2

/*
 * Error barrier:
 * We use membar sync to establish an error barrier for
 * deferred errors. Membar syncs are added before any update
 * to t_lofault to ensure that deferred errors from earlier
 * accesses will not be reported after the membar. This error
 * isolation is important when we try to recover from async
 * errors which tries to distinguish kernel accesses to user
 * data.
 */

/*
 * Zero a block of storage.
 *
 * uzero is used by the kernel to zero a block in user address space.
 */

	ENTRY(uzero)
	!
	! Set a new lo_fault handler only if we came in with one
	! already specified.
	!
	wr	%g0, ASI_USER, %asi
	ldn	[THREAD_REG + T_LOFAULT], %o5
	tst	%o5
	bz,pt	%ncc, .do_zero
	sethi	%hi(.zeroerr), %o2
	or	%o2, %lo(.zeroerr), %o2
	membar	#Sync
	ba,pt	%ncc, .do_zero
	stn	%o2, [THREAD_REG + T_LOFAULT]

	ENTRY(kzero)
	!
	! Always set a lo_fault handler
	!
	wr	%g0, ASI_P, %asi
	ldn	[THREAD_REG + T_LOFAULT], %o5
	sethi	%hi(.zeroerr), %o2
	or	%o5, LOFAULT_SET, %o5
	or	%o2, %lo(.zeroerr), %o2
	membar	#Sync
	ba,pt	%ncc, .do_zero
	stn	%o2, [THREAD_REG + T_LOFAULT]

/*
 * We got here because of a fault during kzero or if
 * uzero or bzero was called with t_lofault non-zero.
 * Otherwise we've already run screaming from the room.
 * Errno value is in %g1. Note that we're here iff
 * we did set t_lofault.
 */
.zeroerr:
	!
	! Undo asi register setting. Just set it to be the
        ! kernel default without checking.
	!
	wr	%g0, ASI_P, %asi
	!
	! If saved t_lofault has FP_USED set, clear the %fprs register
	!
	btst	FP_USED, %o5
	bz,pt	%ncc, 1f		! skip if not used
	nop
	membar #Sync
	wr	%g0, %g0, %fprs		! clear fprs
	andn	%o5, FP_USED, %o5	! turn off flag bit
	!
	! We did set t_lofault. It may well have been zero coming in.
	!
1:
	tst	%o5
	membar #Sync
	bne,pn	%ncc, 3f
	andncc	%o5, LOFAULT_SET, %o5
2:
	!
	! Old handler was zero. Just return the error.
	!
	retl				! return
	mov	%g1, %o0		! error code from %g1
3:
	!
	! We're here because %o5 was non-zero. It was non-zero
	! because either LOFAULT_SET was present, a previous fault
	! handler was present or both. In all cases we need to reset
	! T_LOFAULT to the value of %o5 after clearing LOFAULT_SET
	! before we either simply return the error or we invoke the
	! previously specified handler.
	!
	be	%ncc, 2b
	stn	%o5, [THREAD_REG + T_LOFAULT]
	jmp	%o5			! goto real handler
	  nop
	SET_SIZE(kzero)
	SET_SIZE(uzero)

/*
 * Zero a block of storage.
 */

	ENTRY(bzero)
	wr	%g0, ASI_P, %asi

	ldn	[THREAD_REG + T_LOFAULT], %o5	! save old vector
	tst	%o5
	bz,pt	%ncc, .do_zero
	sethi	%hi(.zeroerr), %o2
	or	%o2, %lo(.zeroerr), %o2
	membar	#Sync				! sync error barrier
	stn	%o2, [THREAD_REG + T_LOFAULT]	! install new vector

.do_zero:
	cmp	%o1, 15			! check for small counts
	blu,pn	%ncc, .byteclr		! just clear bytes
	nop

	cmp	%o1, 192		! check for large counts
	blu	%ncc, .bzero_small
	nop

	sethi	%hi(use_hw_bzero), %o2
	ld	[%o2 + %lo(use_hw_bzero)], %o2
	tst	%o2
	bz	%icc, .bzero_small
	nop

	rd	%fprs, %o2		! check for unused fp
	btst	FPRS_FEF, %o2
	bnz	%icc, .bzero_small
	nop

	ldn	[THREAD_REG + T_LWP], %o2
	tst	%o2
	bz,pn	%ncc, .bzero_small
	nop

	! Check for block alignment
	btst	(64-1), %o0
	bz	%icc, .bzl_block
	nop

	! Check for double-word alignment
	btst	(8-1), %o0
	bz	%icc, .bzl_dword
	nop

	! Check for word alignment
	btst	(4-1), %o0
	bz	%icc, .bzl_word
	nop

	! Clear bytes until word aligned
.bzl_byte:
	stba	%g0, [%o0]%asi
	add	%o0, 1, %o0
	btst	(4-1), %o0
	bnz	%icc, .bzl_byte
	sub	%o1, 1, %o1

	! Check for dword-aligned
	btst	(8-1), %o0
	bz	%icc, .bzl_dword
	nop

	! Clear words until double-word aligned
.bzl_word:
	sta	%g0, [%o0]%asi
	add	%o0, 4, %o0
	btst	(8-1), %o0
	bnz	%icc, .bzl_word
	sub	%o1, 4, %o1

.bzl_dword:
	! Clear dwords until block aligned
	stxa	%g0, [%o0]%asi
	add	%o0, 8, %o0
	btst	(64-1), %o0
	bnz	%icc, .bzl_dword
	sub	%o1, 8, %o1

.bzl_block:
	membar	#StoreStore|#StoreLoad|#LoadStore
	wr	%g0, FPRS_FEF, %fprs

	! Set the lower bit in the saved t_lofault to indicate
	! that we need to clear the %fprs register on the way
	! out
	or	%o5, FP_USED, %o5

	! Clear block
	fzero	%d0
	fzero	%d2
	fzero	%d4
	fzero	%d6
	fzero	%d8
	fzero	%d10
	fzero	%d12
	fzero	%d14
	rd	%asi, %o3
	wr	%g0, ASI_BLK_P, %asi
	cmp	%o3, ASI_P
	bne,a	%icc, 1f
	wr	%g0, ASI_BLK_AIUS, %asi
1:
	mov	256, %o3
	ba,pt	%ncc, .bzl_doblock
	nop

.bzl_blkstart:
      ! stda	%d0, [%o0+192]%asi  ! in dly slot of branch that got us here
	stda	%d0, [%o0+128]%asi
	stda	%d0, [%o0+64]%asi
	stda	%d0, [%o0]%asi
.bzl_zinst:
	add	%o0, %o3, %o0
	sub	%o1, %o3, %o1
.bzl_doblock:
	cmp	%o1, 256
	bgeu,a	%ncc, .bzl_blkstart
	stda	%d0, [%o0+192]%asi

	cmp	%o1, 64
	blu	%ncc, .bzl_finish

	andn	%o1, (64-1), %o3
	srl	%o3, 4, %o2		! using blocks, 1 instr / 16 words
	set	.bzl_zinst, %o4
	sub	%o4, %o2, %o4
	jmp	%o4
	nop

.bzl_finish:
	membar	#StoreLoad|#StoreStore
	wr	%g0, %g0, %fprs
	andn	%o5, FP_USED, %o5

	rd	%asi, %o4
	wr	%g0, ASI_P, %asi
	cmp	%o4, ASI_BLK_P
	bne,a	%icc, 1f
	wr	%g0, ASI_USER, %asi
1:

.bzlf_dword:
	! double words
	cmp	%o1, 8
	blu	%ncc, .bzlf_word
	nop
	stxa	%g0, [%o0]%asi
	add	%o0, 8, %o0
	sub	%o1, 8, %o1
	ba,pt	%ncc, .bzlf_dword
	nop

.bzlf_word:
	! words
	cmp	%o1, 4
	blu	%ncc, .bzlf_byte
	nop
	sta	%g0, [%o0]%asi
	add	%o0, 4, %o0
	sub	%o1, 4, %o1
	ba,pt	%ncc, .bzlf_word
	nop

1:
	add	%o0, 1, %o0		! increment address
.bzlf_byte:
	subcc	%o1, 1, %o1		! decrement count
	bgeu,a	%ncc, 1b
	stba	%g0, [%o0]%asi		! zero a byte

	!
	! If we used the FP registers, that bit was turned
	! off after we were finished. We're just concerned with
	! whether t_lofault was set when we came in. We end up
	! here from either kzero() or bzero(). kzero() *always*
	! sets a lofault handler. It ors LOFAULT_SET into %o5
	! to indicate it has done this even if the value of %o5
	! is otherwise zero. bzero() sets a lofault handler *only*
	! if one was previously set. Accordingly we need to examine
	! %o5 and if it is non-zero be sure to clear LOFAULT_SET
	! before resetting the error handler.
	!
	tst	%o5
	bz,pt	%ncc, 1f
	andn	%o5, LOFAULT_SET, %o5
	membar	#Sync				! sync error barrier
	stn	%o5, [THREAD_REG + T_LOFAULT]	! restore old t_lofault
1:
	retl
	clr	%o0			! return (0)

.bzero_small:

	!
	! Check for word alignment.
	!
	btst	3, %o0
	bz	.bzero_probe
	mov	0x100, %o3		! constant size of main loop
	!
	!
	! clear bytes until word aligned
	!
1:	stba	%g0,[%o0]%asi
	add	%o0, 1, %o0
	btst	3, %o0
	bnz	1b
	sub	%o1, 1, %o1
.bzero_probe:

	!
	! if needed move a word to become double-word aligned.
	!
	btst	7, %o0			! is double aligned?
	bz	%icc, .bzero_nobuf
	nop
	sta	%g0, [%o0]%asi		! clr to double boundry
	sub	%o1, 4, %o1
	ba,pt	%ncc, .bzero_nobuf
	add	%o0, 4, %o0

	!stxa	%g0, [%o0+0xf8]%asi
.bzero_blk:
	stxa	%g0, [%o0+0xf0]%asi
	stxa	%g0, [%o0+0xe8]%asi
	stxa	%g0, [%o0+0xe0]%asi
	stxa	%g0, [%o0+0xd8]%asi
	stxa	%g0, [%o0+0xd0]%asi
	stxa	%g0, [%o0+0xc8]%asi
	stxa	%g0, [%o0+0xc0]%asi
	stxa	%g0, [%o0+0xb8]%asi
	stxa	%g0, [%o0+0xb0]%asi
	stxa	%g0, [%o0+0xa8]%asi
	stxa	%g0, [%o0+0xa0]%asi
	stxa	%g0, [%o0+0x98]%asi
	stxa	%g0, [%o0+0x90]%asi
	stxa	%g0, [%o0+0x88]%asi
	stxa	%g0, [%o0+0x80]%asi
	stxa	%g0, [%o0+0x78]%asi
	stxa	%g0, [%o0+0x70]%asi
	stxa	%g0, [%o0+0x68]%asi
	stxa	%g0, [%o0+0x60]%asi
	stxa	%g0, [%o0+0x58]%asi
	stxa	%g0, [%o0+0x50]%asi
	stxa	%g0, [%o0+0x48]%asi
	stxa	%g0, [%o0+0x40]%asi
	stxa	%g0, [%o0+0x38]%asi
	stxa	%g0, [%o0+0x30]%asi
	stxa	%g0, [%o0+0x28]%asi
	stxa	%g0, [%o0+0x20]%asi
	stxa	%g0, [%o0+0x18]%asi
	stxa	%g0, [%o0+0x10]%asi
	stxa	%g0, [%o0+0x08]%asi
	stxa	%g0, [%o0]%asi
.zinst:
	add	%o0, %o3, %o0		! increment source address
	sub	%o1, %o3, %o1		! decrement count
.bzero_nobuf:
	cmp	%o1, 0x100		! can we do whole chunk?
	bgeu,a	%ncc, .bzero_blk
	stxa	%g0, [%o0+0xf8]%asi	! do first double of chunk

	cmp	%o1, 7			! can we zero any more double words
	bleu	%ncc, .byteclr		! too small go zero bytes

	andn	%o1, 7, %o3		! %o3 bytes left, double-word aligned
	srl	%o3, 1, %o2		! using doubles, need 1 instr / 2 words
	set	.zinst, %o4		! address of clr instructions
	sub	%o4, %o2, %o4		! jmp address relative to instr
	jmp	%o4
	nop
	!
	! do leftover bytes
	!
3:
	add	%o0, 1, %o0		! increment address
.byteclr:
	subcc	%o1, 1, %o1		! decrement count
	bgeu,a	%ncc, 3b
	stba	%g0, [%o0]%asi		! zero a byte

.bzero_finished:
	!
	! We're just concerned with whether t_lofault was set
	! when we came in. We end up here from either kzero()
	! or bzero(). kzero() *always* sets a lofault handler.
	! It ors LOFAULT_SET into %o5 to indicate it has done
	! this even if the value of %o5 is otherwise zero.
	! bzero() sets a lofault handler *only* if one was
	! previously set. Accordingly we need to examine
	! %o5 and if it is non-zero be sure to clear LOFAULT_SET
	! before resetting the error handler.
	!
	tst	%o5
	bz	%ncc, 1f
	andn	%o5, LOFAULT_SET, %o5
	membar	#Sync				! sync error barrier
	stn	%o5, [THREAD_REG + T_LOFAULT]	! restore old t_lofault
1:
	retl
	clr	%o0			! return (0)

	SET_SIZE(bzero)
