vfp.c

ARM9基于WINDOWSCE的BSP源代码

        default:
            break;
        }
        if (fAdd1) {
            FPStatus.iterations = (FPStatus.iterations + 1) & 7;
        }
#endif  // REV0_QUOTE_FEATURE
    }

    FPStatus.Rn = VFPir_DecodeRegister(FPStatus.FPINST, FPStatus.precision, RN_LSB);
    FPStatus.Rm = VFPir_DecodeRegister(FPStatus.FPINST, FPStatus.precision, RM_LSB);
    FPStatus.Rd = VFPir_DecodeRegister(FPStatus.FPINST, FPStatus.precision, RD_LSB);

    // save the interrupt enable bits. Need to restore them after calling the handler
    fpctrl = FPStatus.FPSCR & FPSCR_IE_MASK;

    // clear emulator status first
    _clearfp ();

    // get the software exception enable bits and mask all of them
    // so we don't get exception running emulator's code
    dwIeee = FpscrToIeee (fpctrl);

    __try {
        if (FPStatus.precision == DOUBLE_PRECISION) {
            ret_val = VFPir_DoubleHandler(&FPStatus, pctx);
        } else {
            ret_val = VFPir_SingleHandler(&FPStatus, pctx);
        }
    } __except (EXCEPTION_EXECUTE_HANDLER) {
        ret_val = FALSE;    // unable to handle the exception
    }

    emstat = _statusfp ();   // get the new emulator status status
    
    // updaet emulator fp control to match hardware
    _controlfp (dwIeee, _MCW_EM);

    if (ret_val) {
        DWORD code = CheckException (FPStatus.FPSCR, emstat);
        // exception handled, report exception if we need to
        if (code) {
            pEr->ExceptionCode = code;
            ret_val = FALSE;
        } else {
            // clear control registers
            pctx->FpExtra[FPINST_REG] = 0;
            VFP10RestoreCtrlRegs (pctx->FpExtra, 1);    // update registers

            // restore exception control bits and update thread context
            pctx->Fpscr = (FPStatus.FPSCR & ~FPSCR_IE_MASK) | fpctrl;
        }
    }

    return ret_val;
}

/*****************************************************************************
*
* int VFPir_SingleHandler (_FPStruct *ptrFPStatus, PCONTEXT pctx)
*
* Handles the single precision floating point exceptions (or possible 
* exceptions). After excution of each operation the number of exceptions is 
* tested to determine if an exception occurred. If an exception did occur and 
* the exception was enabled by the user the floating point status is saved and 
* passed onto the users handler on exit from the VFP exception handling code.
*
*       ptrFPStatus :   Pointer to the floating point status structure.
*
******************************************************************************/
BOOL VFPir_SingleHandler (_FPStruct *ptrFPStatus, PCONTEXT pctx)
{
    SingleIntUnion tmp_dest, tmp_rhs;
    float dest;
    float rhs, lhs;
    double dest_double;
    int i;
    DWORD temp_emctrl;

    for (i = 0; i <= (int)ptrFPStatus->iterations; i++)
    {
        lhs = VFP_GetRegSingle (ptrFPStatus->Rn, pctx);   
        rhs = VFP_GetRegSingle (ptrFPStatus->Rm, pctx);   
        dest = VFP_GetRegSingle (ptrFPStatus->Rd, pctx);

        // dest = Fd    lhs = Fn    rhs = Fm
        switch (ptrFPStatus->opcode)
        {
            case FMAC:      // Fd = Fd + (Fn * Fm)
                rhs = lhs * rhs;
                dest = dest + rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FNMAC:     // Fd = Fd - (Fn * Fm)
                rhs = lhs * rhs;
                dest = dest - rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FMSC:      // Fd = -Fd + (Fn * Fm)
                rhs = lhs * rhs;
                lhs = -dest;  
                dest = lhs + rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FNMSC:     // Fd = -Fd - (Fn * Fm)
                rhs = lhs * rhs;
                lhs = -dest;
                dest = dest - rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FMUL:      // Fd = Fn * Fm
                dest = lhs * rhs;
                break;
            case FNMUL:     // Fd = -(Fn * Fm)
                rhs = lhs * rhs;
                dest = -rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FADD:      // Fd = Fn + Fm
                dest = lhs + rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FSUB:      // Fd = Fn - Fm
                dest = lhs - rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case FDIV:      // Fd = Fn / Fm
                dest = lhs / rhs;
                VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                break;
            case EXTEND:    // Extended operations
                switch (ptrFPStatus->opcode_ext)
                {
                    case FCPY:      // Fd = Fm
                        VFP_SetRegSingle (ptrFPStatus->Rd, rhs, pctx);
                        break;
                    case FABS:      // Fd = abs(Fm)
                        dest = (float) fabs(rhs);
                        VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FNEG:      // Fd = -(Fm)
                        dest = -rhs;
                        VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FSQRT:     // Fd = sqrt(Fm)
                        dest = (float) sqrt (rhs);
                        VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FCMP:      // Flags := Fd <> Fm
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_float = dest;
                        tmp_rhs.as_float = rhs;
                        
                        if (dest == rhs)
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < rhs)
                            ptrFPStatus->FPSCR |= LESS_THAN;
                        else if (dest > rhs)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            // Only signal if there is a S-NAN
                            ptrFPStatus->FPSCR |= UNORDERED;
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception
                            if (!SIGNAL_NaN_S(tmp_dest.as_unsignedint) && !SIGNAL_NaN_S(tmp_rhs.as_unsignedint)) {
                                // signal invalid
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }
                        break;
                    case FCMPE:     // Flags := Fd <> Fm with exception reporting
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_float = dest;
                        tmp_rhs.as_float = rhs;
                        if (dest == rhs)
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < rhs)
                            ptrFPStatus->FPSCR |= LESS_THAN;
                        else if (dest > rhs)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            // signal as long as there's a NAN.
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception
                            ptrFPStatus->FPSCR |= UNORDERED;
                            if (SIGNAL_NaN_S(tmp_dest.as_unsignedint)
                                || QUIET_NaN_S(tmp_dest.as_unsignedint)
                                || SIGNAL_NaN_S(tmp_rhs.as_unsignedint)
                                || QUIET_NaN_S(tmp_rhs.as_unsignedint)) {
                                // signal invalid
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }
                        
                        break;
                    case FCMPZ:     // Flags := Fd <> Zero
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_float = dest;

                        if (dest == (float) 0.0)                        
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < (float) 0.0)
                            ptrFPStatus->FPSCR |= LESS_THAN;                            
                        else if (dest > (float) 0.0)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            // Only signal if there is a S-NAN
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception
                            ptrFPStatus->FPSCR |= UNORDERED;
                            if (!SIGNAL_NaN_S(tmp_dest.as_unsignedint)) {
                                // signal invalid
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }
                        break;
                    case FCMPEZ:    // Flags := Fd <> Zero with exception reporting
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_float = dest;

                        if (dest == (float) 0.0)                        
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < (float) 0.0)
                            ptrFPStatus->FPSCR |= LESS_THAN;                            
                        else if (dest > (float) 0.0)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception
                            ptrFPStatus->FPSCR |= UNORDERED;
                            if (SIGNAL_NaN_S(tmp_dest.as_unsignedint) || QUIET_NaN_S(tmp_dest.as_unsignedint)) {
                                // signal invalid
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }
                        break;
                    case FCVTDS:    // Fd(double reg encoding) = Fm(single reg encoding)
                        ptrFPStatus->iterations = 0;
                        dest_double = rhs;
                        VFP_SetRegDouble (VFPir_DecodeRegister(ptrFPStatus->FPINST, DOUBLE_PRECISION, RD_LSB),
                                dest_double, pctx);
                        break;                      
                    case FUITOS:    // Fd = ConvertUnsignedIntToSingle Fm (cp 10)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs.as_float = rhs;
                        dest = (float) tmp_rhs.as_unsignedint;
                        VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                        break;                                              
                    case FSITOS:    // Fd = ConvertSignedIntToSingle Fm (cp 10)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs.as_float = rhs;
                        dest = (float) tmp_rhs.as_signedint;
                        VFP_SetRegSingle (ptrFPStatus->Rd, dest, pctx);
                        break;                                              
                    case FTOUIS:    // Fd = ConvertToUnsignedInt Fm (cp 10 single Current RMODE)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs.as_unsignedint = (unsigned int) rhs;
                        VFP_SetRegSingle (ptrFPStatus->Rd, tmp_rhs.as_float, pctx);
                        break;
                    case FTOUIZS:   // Fd = ConvertToUnsignedInt Fm <cp 10 single RZ Mode)
                        ptrFPStatus->iterations = 0;
                        // Set the software status to Round Towards Zero
                        temp_emctrl = _controlfp (_RC_DOWN, _MCW_RC);
                        tmp_rhs.as_unsignedint = (unsigned int) rhs;
                        VFP_SetRegSingle (ptrFPStatus->Rd, tmp_rhs.as_float, pctx);
                        // Return the software status
                        _controlfp (temp_emctrl, _MCW_RC);
                        break;                                              
                    case FTOSIS:    // Fd = ConvertToSignedInt Fm (cp 10 single Current RMODE)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs.as_signedint = (int) rhs;
                        VFP_SetRegSingle (ptrFPStatus->Rd, tmp_rhs.as_float, pctx);
                        break;                                          
                    case FTOSIZS:   // Fd = ConvertToSignedInt Fm (cp 10 single RZ mode)
                        ptrFPStatus->iterations = 0;
                        // Set the software status to Round Towards Zero
                        temp_emctrl = _controlfp (_RC_DOWN, _MCW_RC);
                        tmp_rhs.as_signedint = (int) rhs;
                        VFP_SetRegSingle (ptrFPStatus->Rd, tmp_rhs.as_float, pctx);