/* fppArchLib.c - Hitachi SH floating-point coprocessor support library *//* Copyright 1984-2002 Wind River Systems, Inc. */#include "copyright_wrs.h"/*modification history--------------------01n,05dec02,hk   restore PC and PR in case of fppExcHandle() error. (SPR#83145)01m,10dec01,zl   removed local typedef of UINT64.01l,11sep01,zl   replaced CPU conditionals with _WRS_HW_FP_SUPPORT.01k,01dec00,zl   endian order in fppExcHandle() to comply with updated uss_xxx.01j,12jul00,hk   added fppExcHandle() and fppExcFixup().01i,28jun00,zl   added fpscrInitValue for SH770001h,05jun00,zl   changed default FPSCR value to support denormalized numbers                 and use rounding to nearest policy.01g,20mar00,zl   added extended floating point registers to the FP context.		 removed fppReset.01f,11aug,99zl   made __fpscr_values external (from gcclib.a) 01e,08jun,99zl   hitachi SH4 architecture port, provided by                   Highlander Engineering. Initialize FPSCR.		  Updated description to be more generic.01d,08mar99,hk   added #if for FPU-less CPU types.01c,15jun98,hk   implemented fppProbe().01b,11may98,hk   paid some efforts for faster operation.01a,26nov97,hms  written based on MC68K's fppArchLib.c*//*DESCRIPTIONThis library provides the low-level interface to Hitachi SH on-chipfloating point facilities.The routines fppTaskRegsSet() and fppTaskRegsGet() inspect and setfloating-point registers on a per task basis.  The routine fppProbe()checks for the presence of the floating-point processor.With the exception of fppProbe(), the higher level facilities indbgLib and usrLib should be used instead of these routines. See fppLib for architecture independent portion.SEE ALSO: fppALib, intConnect(), Hitachi SH Hardware Manuals*/#include "vxWorks.h"#include "objLib.h"#include "taskLib.h"#include "taskArchLib.h"#include "memLib.h"#include "string.h"#include "iv.h"#include "intLib.h"#include "regs.h"#include "fppLib.h"#if (_BYTE_ORDER == _LITTLE_ENDIAN)# define  HREG			1# define  LREG			0#else# define  HREG			0# define  LREG			1#endif#if (CPU==SH7750)#undef FPP_DEBUG#ifdef FPP_DEBUG#include "fioLib.h"				/* to use printExc() */#endif /*FPP_DEBUG*/typedef union    {    UINT64 u64;    UINT32 u32[2];    } UNION64;IMPORT UINT64 uss_dpadd (UINT64 d1, UINT64 d2);		/* d1 + d2 */IMPORT UINT32 uss_fpadd (UINT32 f1, UINT32 f2);		/* f1 + f2 */IMPORT UINT64 uss_dpsub (UINT64 d1, UINT64 d2);		/* d1 - d2 */IMPORT UINT32 uss_fpsub (UINT32 f1, UINT32 f2);		/* f1 - f2 */IMPORT UINT64 uss_dpmul (UINT64 d1, UINT64 d2);		/* d1 * d2 */IMPORT UINT32 uss_fpmul (UINT32 f1, UINT32 f2);		/* f1 * f2 */IMPORT UINT64 uss_dpdiv (UINT64 d1, UINT64 d2);		/* d1 / d2 */IMPORT UINT32 uss_fpdiv (UINT32 f1, UINT32 f2);		/* f1 / f2 */IMPORT UINT64 uss_dpsqrt (UINT64 d1);			/* d1 ^ 0.5 */IMPORT UINT32 uss_fpsqrt (UINT32 f1);			/* f1 ^ 0.5 */IMPORT UINT64 uss_fptodp (UINT32 f1);			/* (double)f1 */IMPORT UINT32 uss_dptofp (UINT64 d1);			/* (float)d1  */LOCAL INSTR *fppExcFixup (ESFSH *pEsf, REG_SET *pRegs);#endif /* CPU==SH7750 *//* globals */REG_INDEX fpRegName [] =    {#ifdef	_WRS_HW_FP_SUPPORT    {"fr0",  FPX_OFFSET(0)},    {"fr1",  FPX_OFFSET(1)},    {"fr2",  FPX_OFFSET(2)},    {"fr3",  FPX_OFFSET(3)},    {"fr4",  FPX_OFFSET(4)},    {"fr5",  FPX_OFFSET(5)},    {"fr6",  FPX_OFFSET(6)},    {"fr7",  FPX_OFFSET(7)},    {"fr8",  FPX_OFFSET(8)},    {"fr9",  FPX_OFFSET(9)},    {"fr10", FPX_OFFSET(10)},    {"fr11", FPX_OFFSET(11)},    {"fr12", FPX_OFFSET(12)},    {"fr13", FPX_OFFSET(13)},    {"fr14", FPX_OFFSET(14)},    {"fr15", FPX_OFFSET(15)},#if (FP_NUM_DREGS == 32)    {"xf0",  FPX_OFFSET(16)},    {"xf1",  FPX_OFFSET(17)},    {"xf2",  FPX_OFFSET(18)},    {"xf3",  FPX_OFFSET(19)},    {"xf4",  FPX_OFFSET(20)},    {"xf5",  FPX_OFFSET(21)},    {"xf6",  FPX_OFFSET(22)},    {"xf7",  FPX_OFFSET(23)},    {"xf8",  FPX_OFFSET(24)},    {"xf9",  FPX_OFFSET(25)},    {"xf10", FPX_OFFSET(26)},    {"xf11", FPX_OFFSET(27)},    {"xf12", FPX_OFFSET(28)},    {"xf13", FPX_OFFSET(29)},    {"xf14", FPX_OFFSET(30)},    {"xf15", FPX_OFFSET(31)},#endif#endif    {NULL,   0},    };REG_INDEX fpCtlRegName [] =    {#ifdef	_WRS_HW_FP_SUPPORT    {"fpul",  FPUL},    {"fpscr", FPSCR},#endif    {NULL, 0},    };#if (CPU==SH7750)IMPORT UINT32 __fpscr_values[2];	/* compiler generated code relies                                            on this to switch between single					   precision and double precision.					   It is in libgcc.a */#endif#ifdef	_WRS_HW_FP_SUPPORTUINT32 fpscrInitValue = FPSCR_INIT;	/* default FPSCR value; can be changed					   at startup if different value is 					   needed */#endif/* locals */#ifdef	_WRS_HW_FP_SUPPORTLOCAL FP_CONTEXT fppInitContext;	/* initialized to initial fp context */#endif/********************************************************************************* fppArchInit - initialize floating-point coprocessor support** This routine must be called before using the floating-point coprocessor.* It is typically called from fppInit().** NOMANUAL*/void fppArchInit (void)    {    fppCreateHookRtn = (FUNCPTR)NULL;#ifdef	_WRS_HW_FP_SUPPORT    bfill ((char *) &fppInitContext, sizeof (FP_CONTEXT),     	    0xff); 					/* fill with NaN */    fppInitContext.fpul = 0;    fppInitContext.fpscr = fpscrInitValue;    fppRestore (&fppInitContext);#if (CPU==SH7750)    __fpscr_values[0] = fpscrInitValue & ~0x80000;	/* clear FPSCR[PR] */    __fpscr_values[1] = fpscrInitValue;#endif#endif    }/********************************************************************************* fppArchTaskCreateInit - initialize floating-point coprocessor support for task** NOMANUAL*/void fppArchTaskCreateInit    (    FP_CONTEXT *pFpContext		/* pointer to FP_CONTEXT */    )    {#ifdef	_WRS_HW_FP_SUPPORT    /* create NULL frame as initial frame */    bcopy ((const char *) &fppInitContext, (char *)pFpContext,	   sizeof (FP_CONTEXT));#endif    }#ifdef	_WRS_HW_FP_SUPPORT/******************************************************************************** fppRegsToCtx - convert FPREG_SET to FP_CONTEXT.*/ void fppRegsToCtx    (    FPREG_SET *  pFpRegSet,		/* input -  fpp reg set */    FP_CONTEXT * pFpContext		/* output - fpp context */    )    {#if 0    *pFpContext = *pFpRegSet;#else    int ix;    /* normal/idle state */    for (ix = 0; ix < FP_NUM_DREGS; ix++)        pFpContext->fpx[ix] = pFpRegSet->fpx [ix];    pFpContext->fpul  = pFpRegSet->fpul;    pFpContext->fpscr = pFpRegSet->fpscr;#endif    }/******************************************************************************** fppCtxToRegs - convert FP_CONTEXT to FPREG_SET.*/ void fppCtxToRegs    (    FP_CONTEXT * pFpContext,		/* input -  fpp context */    FPREG_SET *  pFpRegSet		/* output - fpp register set */    )    {#if 0    *pFpRegSet = *pFpContext;#else    int ix;    /* normal/idle state */    for (ix = 0; ix < FP_NUM_DREGS; ix++)        pFpRegSet->fpx[ix] = pFpContext->fpx[ix];    pFpRegSet->fpul  = pFpContext->fpul;    pFpRegSet->fpscr = pFpContext->fpscr;#endif    }#endif /* _WRS_HW_FP_SUPPORT *//********************************************************************************* fppTaskRegsGet - get the floating-point registers from a task TCB** This routine copies the floating-point registers of a task* (PCR, PSR, and PIAR) to the locations whose pointers are passed as* parameters.  The floating-point registers are copied in* an array containing the 8 registers.** NOTE* This routine only works well if <task> is not the calling task.* If a task tries to discover its own registers, the values will be stale* (i.e., leftover from the last task switch).** RETURNS: OK, or ERROR if there is no floating-point* support or there is an invalid state.** SEE ALSO: fppTaskRegsSet()*/STATUS fppTaskRegsGet    (    int task,           	/* task to get info about */    FPREG_SET *pFpRegSet	/* pointer to floating-point register set */    )    {#ifdef	_WRS_HW_FP_SUPPORT    FAST FP_CONTEXT *pFpContext;    FAST WIND_TCB *pTcb = taskTcb (task);    if (pTcb == NULL)	return (ERROR);    pFpContext = pTcb->pFpContext;    if (pFpContext == (FP_CONTEXT *)NULL)	return (ERROR);			/* no coprocessor support */    fppCtxToRegs (pFpContext, pFpRegSet);    return (OK);#else    return (ERROR);#endif    }/********************************************************************************* fppTaskRegsSet - set the floating-point registers of a task** This routine loads the specified values into the specified task TCB.* The 8 registers f0-f7 are copied to the array <fpregs>.** RETURNS: OK, or ERROR if there is no floating-point* support or there is an invalid state.** SEE ALSO: fppTaskRegsGet()*/STATUS fppTaskRegsSet    (    int task,           	/* task whose registers are to be set */    FPREG_SET *pFpRegSet	/* pointer to floating-point register set */    )    {#ifdef	_WRS_HW_FP_SUPPORT    FAST WIND_TCB *pTcb = taskTcb (task);    FAST FP_CONTEXT *pFpContext;    if (pTcb == NULL)	return (ERROR);    pFpContext = pTcb->pFpContext;    if (pFpContext == (FP_CONTEXT *)NULL)	return (ERROR);			/* no coprocessor support, PUT ERRNO */    fppRegsToCtx (pFpRegSet, pFpContext);    return (OK);#else    return (ERROR);#endif    }/********************************************************************************* fppProbe - probe for the presence of a floating-point coprocessor** This routine determines whether there is an SH7718(SH3e) on-chip* floating-point coprocessor in the system.** IMPLEMENTATION* This routine sets the illegal coprocessor opcode trap vector and executes* a coprocessor instruction.  If the instruction causes an exception,* fppProbe() will return ERROR.  Note that this routine saves and restores* the illegal coprocessor opcode trap vector that was there prior to this* call.** The probe is only performed the first time this routine is called.* The result is stored in a static and returned on subsequent* calls without actually probing.** RETURNS:* OK if the floating-point coprocessor is present, otherwise ERROR.*/STATUS fppProbe (void)    {#ifdef	_WRS_HW_FP_SUPPORT    static int fppProbed = -2;		/* -2 = not done, -1 = ERROR, 0 = OK */    FUNCPTR oldVec;    if (fppProbed == -2)	{	/* save error vector */	oldVec = intVecGet ((FUNCPTR *)INUM_TO_IVEC(INUM_ILLEGAL_INST_GENERAL));	/* replace error vec */	intVecSet ((FUNCPTR *)INUM_TO_IVEC(INUM_ILLEGAL_INST_GENERAL),		(FUNCPTR) fppProbeTrap);	fppProbed = fppProbeSup ();     /* execute coprocessor instruction */	/* replace old err vec*/	intVecSet ((FUNCPTR *)INUM_TO_IVEC(INUM_ILLEGAL_INST_GENERAL),		(FUNCPTR) oldVec);	}    return (fppProbed);#else    return (ERROR);#endif    }#if (CPU==SH7750)/******************************************************************************** fppExcHandle - FPU error exception handler for IEEE754 denormalized number** This routine handles FPU error exception traps caused by inputting IEEE754* denormalized numbers to a specific set of SH7750 FPU instructions.  The goal* is to support denormalized number computation on SH7750, by emulating those* FPU instructions with integer arithmetics.  It first disassembles the FPU* instruction which got the exception, and it picks up the related FPU* registers(s) using fppRegGet().  The actual arithmetic computation is done* by the USS GOFAST FP emulation library, and the result is set to a relevant* FPU register by fppRegSet().  The following FPU instructions are emulated:**	----------------------*		single	double*	----------------------*	fadd	  o	  o*	fsub	  o	  o*	fmul	  o	  o*	fdiv	  o	  o*	fmac	  o	 n/a*	fsqrt	  o	  o*	fcnvsd	 n/a	  o*	fcnvds	 n/a	  o*	----------------------** RETURNS: OK if FP emulation is successful, ERROR otherwise.** NOMANUAL*/STATUS fppExcHandle    (    ESFSH   *pEsf,	/* pointer to exception stack frame */    REG_SET *pRegs	/* pointer to register info on stack */    )    {    INSTR *pcSave = pEsf->pc;	/* save PC before fppExcFixup() call */    INSTR *prSave = pRegs->pr;	/* save PR before fppExcFixup() call */    UINT32 fpscr = pEsf->info;    INSTR *pFpeInsn = fppExcFixup (pEsf, pRegs);    INSTR  insn;    if (pFpeInsn != NULL)	insn = *pFpeInsn;    else	goto FpError;    switch (insn & FPE_MASK_2REG)