fpparchlib.c

Vxworks 的源代码 C语言编写

    pFpContext->u.o.fptag = pFpRegSet->fptag;
    pFpContext->u.o.op    = pFpRegSet->op;
    pFpContext->u.o.ip    = pFpRegSet->ip;
    pFpContext->u.o.cs    = pFpRegSet->cs;
    pFpContext->u.o.dp    = pFpRegSet->dp;
    pFpContext->u.o.ds    = pFpRegSet->ds;
    }

/******************************************************************************
*
* fppCtxToRegs - convert FPO_CONTEXT to FPREG_SET.
*/ 

void fppCtxToRegs
    (
    FP_CONTEXT * pFpContext,            /* input -  fpp context */
    FPREG_SET *  pFpRegSet              /* output - fpp register set */
    )
    {
    int ix;

    for (ix = 0; ix < FP_NUM_REGS; ix++)
        fppDxtoD ((double *)&pFpRegSet->fpx[ix], 
                  (DOUBLEX *)&pFpContext->u.o.fpx[ix]);

    pFpRegSet->fpcr  = pFpContext->u.o.fpcr;
    pFpRegSet->fpsr  = pFpContext->u.o.fpsr;
    pFpRegSet->fptag = pFpContext->u.o.fptag;
    pFpRegSet->op    = pFpContext->u.o.op;
    pFpRegSet->ip    = pFpContext->u.o.ip;
    pFpRegSet->cs    = pFpContext->u.o.cs;
    pFpRegSet->dp    = pFpContext->u.o.dp;
    pFpRegSet->ds    = pFpContext->u.o.ds;
    }

/******************************************************************************
*
* fppXregsToCtx - convert FPREG_SET to FPX_CONTEXT.
*/ 

void fppXregsToCtx
    (
    FPREG_SET *  pFpRegSet,             /* input -  fpp reg set */
    FP_CONTEXT * pFpContext             /* output - fpp context */
    )
    {
    int ix;

    for (ix = 0; ix < FP_NUM_REGS; ix++)
        fppDtoDx ((DOUBLEX *)&pFpContext->u.x.fpx[ix], 
                  (double *)&pFpRegSet->fpx[ix]);

    pFpContext->u.x.fpcr  = pFpRegSet->fpcr;
    pFpContext->u.x.fpsr  = pFpRegSet->fpsr;
    pFpContext->u.x.fptag = pFpRegSet->fptag;
    pFpContext->u.x.op    = pFpRegSet->op;
    pFpContext->u.x.ip    = pFpRegSet->ip;
    pFpContext->u.x.cs    = pFpRegSet->cs;
    pFpContext->u.x.dp    = pFpRegSet->dp;
    pFpContext->u.x.ds    = pFpRegSet->ds;
    }

/******************************************************************************
*
* fppXctxToRegs - convert FPX_CONTEXT to FPREG_SET.
*/ 

void fppXctxToRegs
    (
    FP_CONTEXT * pFpContext,            /* input -  fpp context */
    FPREG_SET *  pFpRegSet              /* output - fpp register set */
    )
    {
    int ix;

    for (ix = 0; ix < FP_NUM_REGS; ix++)
        fppDxtoD ((double *)&pFpRegSet->fpx[ix], 
                  (DOUBLEX *)&pFpContext->u.x.fpx[ix]);

    pFpRegSet->fpcr  = pFpContext->u.x.fpcr;
    pFpRegSet->fpsr  = pFpContext->u.x.fpsr;
    pFpRegSet->fptag = pFpContext->u.x.fptag;
    pFpRegSet->op    = pFpContext->u.x.op;
    pFpRegSet->ip    = pFpContext->u.x.ip;
    pFpRegSet->cs    = pFpContext->u.x.cs;
    pFpRegSet->dp    = pFpContext->u.x.dp;
    pFpRegSet->ds    = pFpContext->u.x.ds;
    }

/*******************************************************************************
*
* fppTaskRegsGet - get the floating-point registers from a task TCB
*
* This routine copies the floating-point registers of a task
* (control, status, and tag) 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 get its own registers, the values will be stale 
* (i.e., left over 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 */
    )
    {
    WIND_TCB * pTcb = taskTcb (task);
    FP_CONTEXT * pFpContext;

    if ((pTcb == NULL) ||
        ((pFpContext = pTcb->pFpContext) == (FP_CONTEXT *)NULL))
	return (ERROR);				/* no coprocessor support */

    if (sysCpuId.featuresEdx & CPUID_FXSR)
	fppXctxToRegs (pFpContext, pFpRegSet);
    else
	fppCtxToRegs (pFpContext, pFpRegSet);

    return (OK);
    }

/*******************************************************************************
*
* fppTaskRegsSet - set the floating-point registers of a task 
*
* This routine loads the specified values into the specified task TCB.
* The 8 registers st/mm0 - st/mm7 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 */
    )
    {
    WIND_TCB * pTcb = taskTcb (task);
    FP_CONTEXT * pFpContext;

    if ((pTcb == NULL) ||
        ((pFpContext = pTcb->pFpContext) == (FP_CONTEXT *)NULL))
	return (ERROR);				/* no coprocessor support */

    if (sysCpuId.featuresEdx & CPUID_FXSR)
	fppXregsToCtx (pFpRegSet, pFpContext);
    else
	fppRegsToCtx (pFpRegSet, pFpContext);

    return (OK);
    }

/*******************************************************************************
*
* fppProbe - probe for the presence of a floating-point coprocessor
*
* This routine determines whether there is an Intel Architecture 
* Floating-Point Unit (FPU) in the system.
*
* IMPLEMENTATION
* This routine sets the illegal coprocessor op-code 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 op-code trap vector that was present 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)
    {
    static int fppProbed = -2;		/* -2 = not done, -1 = ERROR, 0 = OK */

    if (fppProbed == -2)
	{
	fppProbed = fppProbeSup ();	/* check ET bit in %cr0 */
        if (fppProbed == ERROR)
            {
            if ((emuInitFunc != NULL) && (emu387Func != NULL))
                {
                intVecSet2 ((FUNCPTR *)IV_NO_DEVICE, (FUNCPTR)emu387Func,
			    IDT_TRAP_GATE, sysCsExc);
                (* emuInitFunc) ();             /* initialize the emulator */
                fppProbed = OK;                 /* pretend as we have 80387 */
                }
            }
	}

    return (fppProbed);
    }

/*******************************************************************************
*
* fppArchSwitchHook - switch hook routine to set or clear the TS flag in CR0
*
* This routine is a switch hook routine to set or clear the TS flag in CR0.
* The purpose of toggling the TS bit is to detect an illegal FPU/MMX 
* instruction usage without the task option VX_FP_TASK.  The TS flag is set 
* when the new task's VX_FP_TASK bit is set, and cleared otherwise.
* Setting the TS flag generates the device-not-available exception prior 
* to the execution of a FPU/MMX instruction.  Since the MP flag in CR0 is 
* cleared in the default, the execution of WAIT/FWAIT instruction does not 
* raise the exception.  The swap hook routines or switch hook routines that 
* are executed prior to this routine may need to clear the TS bit by itself 
* before using the FPU/MMX instructions.
*
* RETURNS: N/A
*/

void fppArchSwitchHook
    (
    WIND_TCB * pOldTcb,		/* pointer to old task's WIND_TCB */
    WIND_TCB * pNewTcb		/* pointer to new task's WIND_TCB */
    )
    {
    INT32 tempReg; 

    /* enable(clear the TS flag) or disbale(set the TS flag) FPU */

    if (pNewTcb->options & VX_FP_TASK)
	{
	WRS_ASM ("clts");				/* clear TS */
	}
    else
	{
	WRS_ASM ("movl %%cr0,%0; orl $0x8,%0; movl %0,%%cr0;" \
		 : "=r" (tempReg) : : "memory");	/* set TS */
	}
    }

/*******************************************************************************
*
* fppArchSwitchHookEnable - enable or disable the switch hook routine 
*
* This routine is enables or disables the architecture specific FPU switch
* hook routine that detect the illegal FPU/MMX usage.  
* Ths usage of this mechanism is optional, and disabled in the default.
*
* RETURNS: OK, or ERROR if the associated Add/Delete routine failed.
*/

STATUS fppArchSwitchHookEnable
    (
    BOOL enable			/* TRUE to enable, FALSE to disable */
    )
    {
    static INT32 oldCr0 = 0;	/* default: TS flag cleared */
    INT32 tempReg; 

    if (enable)
	{
	if (taskSwitchHookAdd ((FUNCPTR) fppArchSwitchHook) != OK)
	    return (ERROR);

	/* save the TS flag */

        oldCr0 = vxCr0Get ();	

	/* enable(clear the TS flag) or disbale(set the TS flag) FPU */

	if (taskIdCurrent->options & VX_FP_TASK)
	    {
	    WRS_ASM ("clts");				/* clear TS */
	    }
	else
	    {
	    WRS_ASM ("movl %%cr0,%0; orl $0x8,%0; movl %0,%%cr0;" \
		     : "=r" (tempReg) : : "memory");	/* set TS */
	    }
	}
    else
	{
	if (taskSwitchHookDelete ((FUNCPTR) fppArchSwitchHook) != OK)
	    return (ERROR);

	/* restore the TS flag */

	if (oldCr0 & CR0_TS)
	    {
	    WRS_ASM ("movl %%cr0,%0; orl $0x8,%0; movl %0,%%cr0;" \
		     : "=r" (tempReg) : : "memory");	/* set TS */
	    }
	else
	    {
	    WRS_ASM ("clts");				/* clear TS */
	    }
	}

    return (OK);
    }