/*	$NetBSD: 4xx_locore.S,v 1.1.4.2 2002/08/31 13:45:42 gehenna Exp $	*/

/*
 * Copyright 2001 Wasabi Systems, Inc.
 * All rights reserved.
 *
 * Written by Eduardo Horvath and Simon Burge for Wasabi Systems, Inc.
 *
 * 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 for the NetBSD Project by
 *      Wasabi Systems, Inc.
 * 4. The name of Wasabi Systems, Inc. may not be used to endorse
 *    or promote products derived from this software without specific prior
 *    written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY WASABI SYSTEMS, INC. ``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 WASABI SYSTEMS, INC
 * 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.
 */

/*
 * Copyright (C) 1995, 1996 Wolfgang Solfrank.
 * Copyright (C) 1995, 1996 TooLs GmbH.
 * 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.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by TooLs GmbH.
 * 4. The name of TooLs GmbH may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``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 TOOLS GMBH 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.
 */

/*
 * This is not a standalone file. To use it, #inlcude it at
 * the end of your port's locore.S
 */

/*
 * Taken straight from Walnut's locore.S, with the following attribution:
 *	$OpenBSD: locore.S,v 1.4 1997/01/26 09:06:38 rahnds Exp $
 */

/*
 * No processes are runnable, so loop waiting for one.
 * Separate label here for accounting purposes.
 * When we get here, interrupts are off (MSR[EE]=0) and sched_lock is held.
 */
ASENTRY(Idle)
	lis	8,_C_LABEL(sched_whichqs)@ha
	lwz	9,_C_LABEL(sched_whichqs)@l(8)

	or.	9,9,9
	bne-	.Lsw1			/* at least one queue non-empty */

	wrteei	1			/* reenable ints again */
	
/* May do some power saving here? */
/* Check if we can use power saving mode */
	lis	8,_C_LABEL(powersave)@ha
	lwz	9,_C_LABEL(powersave)@l(8)

	or.	9,9,9
	beq	1f

	/* TODO:  Enter power saving mode here */
1:

	wrteei	0	/* disable interrupts while manipulating runque */ 

	b	_ASM_LABEL(Idle)

/*
 * switchexit gets called from cpu_exit to complete the exit procedure.
 */
ENTRY(switchexit)
/* First switch to the idle pcb/kernel stack */
	lis	6,idle_u@ha
	lwz	6,idle_u@l(6)
	lis	7,_C_LABEL(curpcb)@ha
	stw	6,_C_LABEL(curpcb)@l(7)
	addi	1,6,USPACE-16		/* 16 bytes are reserved at stack top */
	/*
	 * Schedule the vmspace and stack to be freed (the proc arg is
	 * already in r3).
	 */
	bl	_C_LABEL(exit2)

/* Fall through to cpu_switch to actually select another proc */
	li	3,0			/* indicate exited process */

/*
 * void cpu_switch(struct proc *p)
 * Find a runnable process and switch to it.
 */
/* XXX noprofile?  --thorpej@netbsd.org */
ENTRY(cpu_switch)
	mflr	0			/* save lr */
	stw	0,4(1)
	stwu	1,-16(1)
	stw	31,12(1)
	stw	30,8(1)

	mr	30,3
	lis	3,_C_LABEL(curproc)@ha
	xor	31,31,31
	stw	31,_C_LABEL(curproc)@l(3) /* Zero to not accumulate cpu time */
	lis	3,_C_LABEL(curpcb)@ha
	lwz	31,_C_LABEL(curpcb)@l(3)

	xor	3,3,3
	bl	_C_LABEL(lcsplx)
	stw	3,PCB_SPL(31)		/* save spl */

	wrteei	0	/* disable interrupts while manipulating runque */

/* Find a new process */
	lis	8,_C_LABEL(sched_whichqs)@ha
	lwz	9,_C_LABEL(sched_whichqs)@l(8)

	or.	9,9,9
	beq-	_ASM_LABEL(Idle)	/* all queues empty */
.Lsw1:
	cntlzw	10,9
	lis	4,_C_LABEL(sched_qs)@ha
	addi	4,4,_C_LABEL(sched_qs)@l
	slwi	3,10,3
	add	3,3,4			/* select queue */

	lwz	31,P_FORW(3)		/* unlink first proc from queue */
	lwz	4,P_FORW(31)
	stw	4,P_FORW(3)
	stw	3,P_BACK(4)

	cmpl	0,3,4			/* queue empty? */
	bne	1f

	lis	3,0x80000000@h
	srw	3,3,10
	andc	9,9,3
	stw	9,_C_LABEL(sched_whichqs)@l(8) /* mark it empty */

1:
	/* just did this resched thing */
	xor	3,3,3
	lis	4,_C_LABEL(want_resched)@ha
	stw	3,_C_LABEL(want_resched)@l(4)

	stw	3,P_BACK(31)		/* probably superfluous */

	/* Process now running on a processor. */
	li	3,SONPROC		/* p->p_stat = SONPROC */
	stb	3,P_STAT(31)

	/* record new process */
	lis	4,_C_LABEL(curproc)@ha
	stw	31,_C_LABEL(curproc)@l(4)

	wrteei	1			/* Now we can interrupt again */

	cmpl	0,31,30			/* is it the same process? */
	beq	switch_return

	or.	30,30,30		/* old process was exiting? */
	beq	switch_exited

#ifndef PPC_IBM4XX
	mfsr	10,USER_SR		/* save USER_SR for copyin/copyout */
#else
	li	10,0			/* no SR for 4xx CPUs */
#endif
	mfcr	11			/* save cr */
	mr	12,2			/* save r2 */
	stwu	1,-SFRAMELEN(1)		/* still running on old stack */
	stmw	10,8(1)
	lwz	3,P_ADDR(30)
	stw	1,PCB_SP(3)		/* save SP */

switch_exited:
	wrteei	0			/* disable interrupts while actually switching */

	/* indicate new pcb */
	lwz	4,P_ADDR(31)
	lis	5,_C_LABEL(curpcb)@ha
	stw	4,_C_LABEL(curpcb)@l(5)

	/* save real pmap pointer for spill fill */
	lwz	5,PCB_PMR(4)
	lis	6,_C_LABEL(curpm)@ha
	stwu	5,_C_LABEL(curpm)@l(6)
	stwcx.	5,0,6			/* clear possible reservation */

	/* Switch to the new virtual space */
	/* Do not have to do anything here. TLB PID gets updated on return from trap. */

	lwz	1,PCB_SP(4)		/* get new procs SP */

	wrteei	1			/* interrupts are okay again */

	lmw	10,8(1)			/* get other regs */
	lwz	1,0(1)			/* get saved SP */
	mr	2,12			/* get saved r2 */
	mtcr	11			/* get saved cr */
	isync

switch_return:
	mr	30,7			/* save proc pointer */

	lwz	3,PCB_SPL(4)
	bl	_C_LABEL(lcsplx)

0:
	lis	3,_C_LABEL(curpm)@ha
	addi	3,3,_C_LABEL(curpm)@l	/* Do we need a context? */
	lwz	4,PM_CTX(3)
	cmpwi	4,0
#	mtspr	SPR_SPR0,4		/* Always keep the current ctx here */
	mr	3,30
	bne	1f
	bl	_C_LABEL(ctx_alloc)
	mr	3,30			/* get curproc for special fork
					   returns */
	b	0b			/* Reload */
1:
	lwz	31,12(1)
	lwz	30,8(1)
	addi	1,1,16
	lwz	0,4(1)
	mtlr	0
	blr

/*
 * Child comes here at the end of a fork.
 * Return to userspace via the trap return path.
 */
	.globl	_C_LABEL(fork_trampoline)
_C_LABEL(fork_trampoline):
	xor	3,3,3
	bl	_C_LABEL(lcsplx)
	mtlr	31
	mr	3,30
	blrl				/* jump indirect to r31 */
	b	trapexit

/*
 * Pull in common trap vector code.
 */
#include <powerpc/ibm4xx/trap_subr.S>
#include <powerpc/ibm4xx/4xx_trap_subr.S>

/*
 * int setfault()
 *
 * Similar to setjmp to setup for handling faults on accesses to user memory.
 * Any routine using this may only call bcopy, either the form below,
 * or the (currently used) C code optimized, so it doesn't use any non-volatile
 * registers.
 */
	.globl	_C_LABEL(setfault)
_C_LABEL(setfault):
	mflr	0
	mfcr	12
	lis	4,_C_LABEL(curpcb)@ha
	lwz	4,_C_LABEL(curpcb)@l(4)
	stw	3,PCB_FAULT(4)
	stw	0,0(3)
	stw	1,4(3)
	stw	2,8(3)
	stmw	12,12(3)
	xor	3,3,3
	blr

	.globl	_C_LABEL(ppc4xx_reset)
_C_LABEL(ppc4xx_reset):
	mfspr	3,SPR_DBCR0
	oris	3,r13,DBCR0_RST_SYSTEM@h
	mtspr	SPR_DBCR0,3
	ba	0

#if 0
/*
 * XXXX the following doesn't quite work right yet.
 */
/*
 * void bcopy(const void *src, void *dst, size_t len);
 *
 * swap r3 and r4 and fall through to memcopy.
 */
	.globl _C_LABEL(bcopy)
_C_LABEL(bcopy):
	mr	r0,r3
	mr	r3,r4
	mr	r4,r0
	/* FALLTHROUGH */

/*
 * void * memcpy(void *dst (r3), const void *src (r4), size_t len (r5));
 *
 * Copy memory (obviously)
 *
 * We will try to do data cache block aligned stores so we
 * can use block allocate and not have to read from the
 * destination.
 *
 * Register use:
 *
 *	r1		stack (of course)
 *	r3		dst
 *	r4		src
 *	r5		len
 *	r6		tmp
 *	r7		holds 32
 *	r8		holds dst
 *	r24-r31		block move regs
 *
 */

ENTRY(memcpy)
	stwu	r1,-(10*4)(r1)	/* Allocate some RAM to save 8 regs to. */
	cmpwi	r5, 32		/* Less than 32 bytes ? */
	stmw	r24,8(r1)	/* Save ALL regs (could be optimized) */

	mr	r8,r3		/* save dst */
	li	r7,32

	dcbt	0,r4		/* Start bringing in cache line. */
	blt	1f		/* Finish up */

	neg	r6,r3		/* Find how far unaligned we are... */
	andi.	r6,r6,31	/* Cache-align dest. */
	mtxer	r6
	sub	r5,r5,r6	/* subtract count */
	lswx	r24,0,r4	/* Load some. */
	add	r4,r4,r6
	dcbt	0,r4		/* Fetch next line */
	stswx	r24,0,r3	/* Store some */
	add	r3,r3,r6
	addic.	r6,r5,-32	/* Pre-decrement next line */
	ble	1f		/* Less than 32-bytes? finishup */

	/* Dest should not be cache line aligned. */
	/* XXX need gas 2.11 to grok dcba insn */
#ifdef GAS_2_11
	dcba	0,r3		/* Allocate a line */
#else
	.long	0x7c001dec	/* dcba	0,r3 */
#endif
0:
	dcbt	r7,r4		/* Bring in the next line, too */

	lswi	r24,r4,32
	addi	r4,r4,32	/* Inc src */
	mr	r5,r6

	addic.	r6,r5,-32
	stswi	r24,r3,32
	addi	r3,r3,32	/* Inc dst */
#ifdef GAS_2_11
	dcba	0,r3		/* Allocate another line */
#else
	.long	0x7c071dec	/* dcba	r7,r3 */
#endif	
	bgt	0b
1:
	mtxer	r5		/* Store byte count */
	lswx	r24,0,r4	/* Load up to 32 bytes */
	stswx	r24,0,r3	/* Store up to 32 bytes */

	mr	r3,r8		/* Return dst */

	lmw	r24,8(r1)
	addi	r1,r1,(10*4)
	blr
#endif
