fppalib.s

VxWorks BSP框架源代码包含头文件和驱动

/* fppALib.s - floating-point coprocessor support assembly routines */

/* Copyright 1984-2001 Wind River Systems, Inc. */
#include "copyright_wrs.h"

/*
 * This file has been developed or significantly modified by the
 * MIPS Center of Excellence Dedicated Engineering Staff.
 * This notice is as per the MIPS Center of Excellence Master Partner
 * Agreement, do not remove this notice without checking first with
 * WR/Platforms MIPS Center of Excellence engineering management.
 */

/*
modification history
--------------------
02d,18jan02,agf  add explicit align directive to data section(s)
02c,01aug01,mem  Diab integration.
02b,16jul01,ros  add CofE comment
02a,05jun01,mem  Added hooks for extended save/restore.
01z,09feb01,sru  implement new hazard model
01y,18jan01,pes  Cleanup handling of 32/64-bit floating point
                 init/save/restore.
01x,02jan01,pes  Correct usage of _WRS_VR5400_ERRATA conditionals around nops
01w,20dec00,pes  Update for MIPS32/MIPS64 target combinations.
01v,24aug00,dra  fix fp status reg for vr5074
01u,19jun00,dra  work around 5432 branch bug
01t,10jun99,dra	 Added FP_CONTEXT Expansion support for the NEC VR5464.
		 -Altered fppSave to support both 32 and 16 FP mode.
		 -Altered fppRestore to support both 32 and 16 FP mode.
01s,08may97,mem  added tests for SOFT_FLOAT.
01r,13aug98,kk	 fixed various bugs in fppEmulateBranch() (SPR# 20964, 21119).
		 Made regFetchTable use LW instead of lw to support both 
		 R3XXX & R4XXX.
01q,18oct93,cd   added R4000 support.
01q,18feb94,caf  fixed fppProbe() to just read the status register (SPR 3072).
01p,02sep93,yao  put back FPA emulation code.
02o,15sep92,jdi  ansified declarations for mangenable routines.
02n,12sep92,ajm  changed OPCODE_MASK to GENERAL_OPCODE_MASK, OFFSET_MASK to
		 OFFSET16_MASK
02m,22jul92,ajm  got rid of unused FPA emulation code (for space)
02l,05jun92,ajm  5.0.5 merge, notice mod history changes
02k,26may92,rrr  the tree shuffle
02j,04oct91,rrr  passed through the ansification filter
                  -changed VOID to void
                  -changed ASMLANGUAGE to _ASMLANGUAGE
                  -changed copyright notice
02i,16jan92,jdi  made fppInitialize() and fppClearFLoat() NOMANUAL.
02h,14jan92,jdi  documentation cleanup.
02g,08jan92,ajm  fixed restoration of fpp status reg in fppRestore
02f,16oct91,ajm  documentation
02e,24sep91,wmd  fixed for mangen.
02d,01aug91,ajm  moved in fppProbe.
02c,03mar91,ajm  moved clearing of fpa interrupt from sysALib.s.
02b,28jan91,ajm  read fpa control before write to insure float operations
		  are done.
02a,05jul90,ajm  port to MIPS R3000.
01g,28may88,dnw  fixed bug in fppProbe of only setting byte instead of long
		   flag.
		 made FPP_ASSEM conditionals.
		 cleaned-up.
		 removed extra spaces in .word declarations that may have
		   been screwing up the VMS port.
01f,31mar88,gae  took out save & restore in fppProbeSup() so that
		   privilege violation exceptions wouldn't be generated.
01g,28mar88,gae  oops! forgot hand-assembly for fp codes in 01f.
01f,18mar88,gae  now supports both MC68881 & MC68882.
01e,09mar88,gae  moved host specific _errno definition to hostALib.s.
01d,13feb88,dnw  added .data before .asciz above, for Intermetrics assembler.
01c,04dec87,gae  removed most HOST_TYPE dependencies - fp codes hand assembled.
01b,26aug87,gae  added include of asm.h and special HOST_IRIS version
		   of floating-point instructions.
01a,06aug87,gae  written/extracted from vxALib.s
*/

/*
DESCRIPTION
This library contains routines to be used to support the MIPS R-Series
floating-point coprocessor.
The routines fppSave() and fppRestore() save and restore all the task context
information, the programming model, which includes the 16 double precision
registers and the 1 control register.  The routine fppProbe() checks for the
presence of the MIPS floating point coprocessor.  Higher level access and
initialization routines are found in fppLib.

SEE ALSO:
fppLib
.I "MIPS RISC Architecture"
*/

#define _ASMLANGUAGE
#include "vxWorks.h"
#include "fppLib.h"
#include "esf.h"
#include "dsmLib.h"
#include "private/taskLibP.h"
#include "asm.h"

	/* internals */

	.globl GTEXT(fppSave)
	.globl GTEXT(fppRestore)
	.globl GTEXT(fppInitialize)
	.globl GTEXT(fppClearFloat)	/* clear floating pt interrupt */
	.globl GTEXT(fppArgsToRegs)	/* place passed float args in fpa regs */
	.globl GTEXT(fppEmulateBranch)	/* emulate branch instructions */
	.globl GTEXT(fppProbe)		/* do we have an fpp present */

	.text
	.set	reorder

/******************************************************************************
*
* fppSave - save the floating-point coprocessor context
*
* This routine saves the floating-point coprocessor context.
* The context saved is:
*
*     fpcsr
*     fp0 - fp31
*
* RETURNS: N/A
*
* SEE ALSO:
* fppRestore(),
* .I "MIPS RISC Architecture"

* void fppSave
*     (
*     FP_CONTEXT *  pFpContext	/* where to save context *
*     )

*/

	.ent	fppSave
FUNC_LABEL(fppSave)
#ifndef SOFT_FLOAT
	cfc1	t0, C1_SR
	HAZARD_CP_READ
	sw	t0, FPCSR(a0)
	SWC1	fp0,FP0(a0)
	SWC1	fp1,FP1(a0)
	SWC1	fp2,FP2(a0)
	SWC1	fp3,FP3(a0)
	SWC1	fp4,FP4(a0)
	SWC1	fp5,FP5(a0)
	SWC1	fp6,FP6(a0)
	SWC1	fp7,FP7(a0)
	SWC1	fp8,FP8(a0)
	SWC1	fp9,FP9(a0)
	SWC1	fp10,FP10(a0)
	SWC1	fp11,FP11(a0)
	SWC1	fp12,FP12(a0)
	SWC1	fp13,FP13(a0)
	SWC1	fp14,FP14(a0)
	SWC1	fp15,FP15(a0)
	SWC1	fp16,FP16(a0)
	SWC1	fp17,FP17(a0)
	SWC1	fp18,FP18(a0)
	SWC1	fp19,FP19(a0)
	SWC1	fp20,FP20(a0)
	SWC1	fp21,FP21(a0)
	SWC1	fp22,FP22(a0)
	SWC1	fp23,FP23(a0)
	SWC1	fp24,FP24(a0)
	SWC1	fp25,FP25(a0)
	SWC1	fp26,FP26(a0)
	SWC1	fp27,FP27(a0)
	SWC1	fp28,FP28(a0)
	SWC1	fp29,FP29(a0)
	SWC1	fp30,FP30(a0)
	SWC1	fp31,FP31(a0)
	lw	t0,_func_fppSaveHook
	beqz	t0, 0f
	SETFRAME(fppSave,0)
	subu	sp, FRAMESZ(fppSave)		/* carve stack space */
	SW	ra, FRAMERA(fppSave)(sp)	/* save return address */
	jal	t0
	LW	ra, FRAMERA(fppSave)(sp)	/* get return address */
	addu 	sp, FRAMESZ(fppSave)		/* pop up stack */
0:
#endif	/* !SOFT_FLOAT */
	j	ra
	.end	fppSave

/******************************************************************************
*
* fppRestore - restore the floating-point coprocessor context
*
* This routine restores the coprocessor programming model.
* The context restored is:
*
*     fp0 - fp31
*     fpcsr
*
* RETURNS: N/A
*
* SEE ALSO:
* .I "MIPS RISC Architecture"

* void fppRestore
*    (
*    FP_CONTEXT *  pFpContext	/* from where to restore context *
*    )

*/

	.ent	fppRestore
FUNC_LABEL(fppRestore)
#ifndef SOFT_FLOAT
	lw	t0, _func_fppRestoreHook
	beqz	t0, 0f
	SETFRAME(fppSave,0)
	subu	sp, FRAMESZ(fppSave)		/* carve stack space */
	SW	ra, FRAMERA(fppSave)(sp)	/* save return address */
	jal	t0
	LW	ra, FRAMERA(fppSave)(sp)	/* get return address */
	addu 	sp, FRAMESZ(fppSave)		/* pop up stack */
0:
	LWC1	fp0,FP0(a0)
	LWC1	fp1,FP1(a0)
	LWC1	fp2,FP2(a0)
	LWC1	fp3,FP3(a0)
	LWC1	fp4,FP4(a0)
	LWC1	fp5,FP5(a0)
	LWC1	fp6,FP6(a0)
	LWC1	fp7,FP7(a0)
	LWC1	fp8,FP8(a0)
	LWC1	fp9,FP9(a0)
	LWC1	fp10,FP10(a0)
	LWC1	fp11,FP11(a0)
	LWC1	fp12,FP12(a0)
	LWC1	fp13,FP13(a0)
	LWC1	fp14,FP14(a0)
	LWC1	fp15,FP15(a0)
	LWC1	fp16,FP16(a0)
	LWC1	fp17,FP17(a0)
	LWC1	fp18,FP18(a0)
	LWC1	fp19,FP19(a0)
	LWC1	fp20,FP20(a0)
	LWC1	fp21,FP21(a0)
	LWC1	fp22,FP22(a0)
	LWC1	fp23,FP23(a0)
	LWC1	fp24,FP24(a0)
	LWC1	fp25,FP25(a0)
	LWC1	fp26,FP26(a0)
	LWC1	fp27,FP27(a0)
	LWC1	fp28,FP28(a0)
	LWC1	fp29,FP29(a0)
	LWC1	fp30,FP30(a0)
	LWC1	fp31,FP31(a0)
	lw	t0, FPCSR(a0)
	cfc1	t1, C1_SR	/* read to finish float operations */
	ctc1	t0, C1_SR	/* restore fpp status reg */
	HAZARD_CP_WRITE
#endif	/* !SOFT_FLOAT */
	j	ra
	.end	fppRestore

/******************************************************************************
*
* fppInitialize - initialize the floating-point coprocessor
*
* This routine initializes the coprocessor programming model.
* The context initialized is:
*
*     fp0 - fp31
*     fpcsr
*
* RETURNS: N/A
*
* SEE ALSO:
* .I "MIPS RISC Architecture"
*
* NOMANUAL

* void fppInitialize ()

*/

	.ent	fppInitialize
FUNC_LABEL(fppInitialize)
#ifndef SOFT_FLOAT
	MTC1	zero,fp0		/* zero the registers */
	MTC1	zero,fp1
	MTC1	zero,fp2
	MTC1	zero,fp3
	MTC1	zero,fp4
	MTC1	zero,fp5
	MTC1	zero,fp6
	MTC1	zero,fp7
	MTC1	zero,fp8
	MTC1	zero,fp9
	MTC1	zero,fp10
	MTC1	zero,fp11
	MTC1	zero,fp12
	MTC1	zero,fp13
	MTC1	zero,fp14
	MTC1	zero,fp15
	MTC1	zero,fp16
	MTC1	zero,fp17
	MTC1	zero,fp18
	MTC1	zero,fp19
	MTC1	zero,fp20
	MTC1	zero,fp21
	MTC1	zero,fp22
	MTC1	zero,fp23
	MTC1	zero,fp24
	MTC1	zero,fp25
	MTC1	zero,fp26
	MTC1	zero,fp27
	MTC1	zero,fp28
	MTC1	zero,fp29
	MTC1	zero,fp30
	MTC1	zero,fp31
	li	t0, FP_TASK_STATUS	/* see arch.h */
	cfc1	t1, C1_SR		/* read to finish float operations */
	ctc1	t0, C1_SR		/* load new status reg */
	HAZARD_CP_WRITE
#endif	/* !SOFT_FLOAT */
	j	ra
	.end	fppInitialize

/*******************************************************************************
*
* fppProbe - probe for the presence of a floating-point coprocessor
*
* This routine determines whether there is a
* floating-point coprocessor in the system.
*
* INTERNAL
* This routine simply reads the "coprocessor 1 usable" bit in the R-Series
* status register.  This bit must be correctly initialized in the BSP.  
* A deeper probe would be nice, but the previous attempt didn't work.
*
* RETURNS:
* OK, or ERROR if there is no floating-point coprocessor.
*
* SEE ALSO:
* .I "MIPS RISC Architecture"

* STATUS fppProbe ()

*/

	.ent	fppProbe
FUNC_LABEL(fppProbe)
#ifndef SOFT_FLOAT
	HAZARD_VR5400
	mfc0	t0, C0_SR
	HAZARD_CP_READ
	move	v0, zero		/* OK return value */
	and	t0, t0, SR_CU1		/* coprocessor 1 enabled? */
	bne	t0, zero, probeReturn
#endif	/* !SOFT_FLOAT */
	li	v0, -1			/* ERROR return value */
probeReturn:
	j	ra
	.end	fppProbe

/*******************************************************************************
*
* fppClearFloat -  clear a floating-point exception
*
* This routine zeros the exception bits from the MIPS floating-point
* unit, clearing the exception to the CPU.  It returns the contents
* of the original fpcsr register.
*
* RETURNS: The contents of the original fpcsr register.
*
* NOMANUAL

* ULONG fppClearFloat ()
 
*/
	.ent	fppClearFloat
FUNC_LABEL(fppClearFloat)
#ifndef SOFT_FLOAT
	cfc1	v0, C1_SR		/* read control/status reg	*/
	HAZARD_CP_READ
	and	a0, v0, ~FP_EXC_MASK	/* zero bits		*/
	ctc1	a0, C1_SR		/* clear interrupt		*/
	HAZARD_CP_WRITE
#endif	/* !SOFT_FLOAT */
	j	ra			/* return to caller		*/
	.end	fppClearFloat

/*******************************************************************************
*
* fppArgsToRegs - place floating-point arguments into FPA registers
*
* This routine reads two double-precision floating-point values from memory and
* places them in the parameter passing registers of the MIPS R-Series
* floating-point unit.
*
* RETURNS: N/A
*
* NOMANUAL

* void fppArgsToRegs (dblPtr)
*    double *dblPtr;
 
*/
	.ent	fppArgsToRegs
FUNC_LABEL(fppArgsToRegs)
#ifndef SOFT_FLOAT
#if (_WRS_FP_REGISTER_SIZE == 4)
	/* R3000, or R4000 in 32-bit f.p. compatibility mode */
	LWC1	fp12,(0*_WRS_FP_REGISTER_SIZE)(a0)
	LWC1	fp13,(1*_WRS_FP_REGISTER_SIZE)(a0)
	LWC1	fp14,(2*_WRS_FP_REGISTER_SIZE)(a0)
	LWC1	fp15,(3*_WRS_FP_REGISTER_SIZE)(a0)
#elif (_WRS_FP_REGISTER_SIZE == 8)
	/* R4000 in native 64-bit floating point mode */
	LWC1	fp12,(0*_WRS_FP_REGISTER_SIZE)(a0)
	LWC1	fp14,(1*_WRS_FP_REGISTER_SIZE)(a0)
#else /* _WRS_FP_REGISTER_SIZE */
#error "invalid _WRS_FP_REGISTER_SIZE value"
#endif /* _WRS_FP_REGISTER_SIZE */
#endif	/* !SOFT_FLOAT */
	j	ra			/* return to caller		*/
	.end	fppArgsToRegs

/*******************************************************************************
*
* fppEmulateBranch - calculates the result of a MIPS jump or branch instruction
*
* This routine calculates the resulting EPC when an exception/interrupt
* occurs in the branch delay slot.
*
* RETURNS: The address of the branch target.
*
* SEE ALSO:
* .I "MIPS RISC Architecture"
*
* NOMANUAL

* STATUS fppEmulateBranch (pEsf,brInstr,fpcsr)
*     ESFMIPS *pEsf;     /* pointer to exception stack frame *
*     ULONG   brInstr,  /* branch instruction to emulate    *
* 	      fpcsr;    /* fp control/status register       *

*/

	.ent	fppEmulateBranch
FUNC_LABEL(fppEmulateBranch)
#ifndef SOFT_FLOAT

	/* unconditional immediates are easy, 
	   so they go first (jal, j) */

	and	t0, a1, GENERAL_OPCODE_MASK	/* look at opcode field */

	li	t3, J_INSTR			/* load temp j value */
	bne	t0, t3, 1f			/* am I jump always */
	and	a1, TARGET_MASK			/* grab 26 bit target */
	sll	a1, 2				/* shift to word bounds */
	lw	t1, E_STK_EPC(a0)		/* grab epc	*/
	

	addiu	t1, t1, 4			/* calculate BD slot addr */
	and	t2, t1, ~TARGET_MASK		/* grab upper 4 pc bits */
	or	v0, a1, t2			/* combine TARGET with pc top 
						   4 bits, return value */
	j	ra				/* return	*/

1:	and	t0, a1, GENERAL_OPCODE_MASK	/* look at opcode field */
	li	t3, JAL_INSTR			/* load temp jal value */
	bne	t0, t3, 2f			/* am I jump and link */

	lw	t1, E_STK_EPC(a0)		/* grab epc	*/
	addu	t2, t1, 8			/* increment epc for ra */
	sw	t2, E_STK_RA(a0)		/* replace ra	*/
	and	a1, TARGET_MASK			/* grab 26 bit target */
	sll	a1, 2				/* shift to word bounds */
	addiu	t1, t1, 4			/* calculate BD slot addr */
	and	t2, t1, ~TARGET_MASK		/* grab upper 4 pc bits */
	or	v0, a1, t2			/* combine TARGET with pc top 
						   4 bits, return value */
	j	ra				/* return	*/

2:	bne	t0, zero, 4f			/* are we special opcode */

	/* now for unconditional registers (jr, jalr) */

	and	t0, a1, SPECIAL_MASK		/* look at special field */
	li	t3, JR_INSTR			/* load temp jr value */
	bne	t0, t3, 3f			/* am I jump register */
	srl	a1, (RS_POS - 2)		/* get table register
						   entry, notice word
						   offset (-2)	*/
	lw	t2, regFetchIndexTbl(a1)	/* grab correct instructions */
        move    t7, ra                          /* set return address */
	j	t2				/* excecute the instructions, 
						   return taken care of	*/

3:	and	t0, a1, SPECIAL_MASK		/* look at special field */
	li	t3, JALR_INSTR			/* load temp jalr value */
	bne	t0, t3 , 4f			/* am I jump and 
						   link register */
	lw	t1, E_STK_EPC(a0)		/* grab epc	*/
	addu	t2, t1, 8			/* increment epc for ra */
	sw	t2, E_STK_RA(a0)		/* replace ra	*/
	srl	a1, RS_POS - 2			/* get table register
						   entry, notice word
						   offset (-2)	*/
	lw	t2, regFetchIndexTbl(a1)	/* grab correct instructions */
        move    t7, ra                          /* set return address */
	j	t2				/* excecute the instructions,
						   return taken care of	*/
4:
	/* now for the hard part, branches.  Order of importance seems
	   to be as follows :   beq, bne, blez, bgez, bltz, bgtz,
				bcf, bct, bltzal, bgtzal
           we also check for branch likely instructions
	   exceptions occuring in the branch delay slot of a 
	   branch likely should mean that the branch will be taken
           (otherwise  the instruction shopuld have been nullified)
	   we go through the comparison routines just to be sure
	*/

	and	t0, a1, (OFFSET16_MASK << 16)	/* mask high 16 bits	*/
	and	t3, t0, GENERAL_OPCODE_MASK	/* get branch opcode	*/

	beq	t3, BEQ_INSTR, 1f
	bne	t3, BEQL_INSTR, 5f		/* are we beq/beql instr */

1:	and	t2, t0, RS_MASK			/* look at RS only	*/
	srl	t2, (RS_POS - 2)
	lw	t2, regFetchIndexTbl(t2)	/* get routine address	*/
	jalr	t7, t2				/* read RS register	*/
	move	t4, v0				/* store value in t4	*/

	and	t2, t0, RT_MASK			/* look at RT only	*/
	srl	t2, (RT_POS - 2)
	lw	t2, regFetchIndexTbl(t2)	/* get routine address	*/
	jalr	t7, t2				/* read RT register	*/
	move	t5, v0				/* store value in t5	*/

	bne	t4, t5, 15f 			/* don't take branch	*/