vfp.c

ARM9基于WINDOWSCE的BSP源代码

/*   The content of this file or document is CONFIDENTIAL and PROPRIETARY
*   to Jade Technologies Co., Ltd.  It is subjected to the terms of a
*   License Agreement between Licensee and Jade Technologies Co., Ltd.
*   restricting among other things, the use, reproduction, distribution
*   and transfer.  Each of the embodiments, including this information 
*   and any derivative work shall retain this copyright notice.
* 
*   Copyright (c) 2004 - 2005 Jade Technologies Co., Ltd. 
*   All rights reserved.
*/
// ----------------------------------------------------------------
// File:     vfp.c,v
// Revision: 1.0
// ----------------------------------------------------------------
// $
/*****************************************************************************
 *  
 *  This file contains the VFP support code to handle any VFP exceptions
 *
******************************************************************************/
#include "windows.h"
#include "vfp.h"

#define FPINST_REG      0

float VFP_GetRegSingle (int Sn, PCONTEXT pctx)
{
    SingleIntUnion f;
    f.as_unsignedint = pctx->S[Sn];
    return f.as_float;
}

void VFP_SetRegSingle (int Sn, float fval, PCONTEXT pctx)
{
    SingleIntUnion f;
    f.as_float = fval;
    pctx->S[Sn] = f.as_unsignedint;
}

double VFP_GetRegDouble (int Dn, PCONTEXT pctx)
{
    DoubleIntUnion d;
    d.as_msw_lsw.lowAddrWord  = pctx->S[Dn * 2];
    d.as_msw_lsw.highAddrWord = pctx->S[Dn * 2 + 1];
    return d.as_double;
}

void VFP_SetRegDouble (int Dn, double dval, PCONTEXT pctx)
{
    DoubleIntUnion d;
    d.as_double = dval;
    pctx->S[Dn * 2]     = d.as_msw_lsw.lowAddrWord;
    pctx->S[Dn * 2 + 1] = d.as_msw_lsw.highAddrWord;
}

/*****************************************************************************
*
* void VFPir_UpdateRegisters (_FPStruct *ptrFPStatus)
*
* Updates the Rd, Rn, Rm registers in the case of the exceptional instruction
* being a vector instruction.
*
*       ptrFPStatus :   Pointer to the floating point status structure.
*
******************************************************************************/
void VFPir_UpdateRegisters (_FPStruct *ptrFPStatus)
{               
    unsigned int wrap = 4; // Assume double precision.
    unsigned int wrap_limit = 3;
    
    if (ptrFPStatus->stride)
    {
        wrap_limit -= 1;
    }
    
    if (ptrFPStatus->precision == SINGLE_PRECISION)
    {
        wrap = 8;
        wrap_limit += 4;
    }
        
    if (ptrFPStatus->iterations >= 1)
    {
        if (ptrFPStatus->Rd < wrap)
        {   // Scaler only
            ptrFPStatus->iterations = 0;
        }
        else
        {
            if (ptrFPStatus->stride)
            {
                ptrFPStatus->Rm += 2;
                // Check for mixed scaler / vector operations (d bank and m bank)
                if ((ptrFPStatus->Rm >= wrap) && (!(ptrFPStatus->Rm & wrap_limit)))
                {   // Vector only operations
                    ptrFPStatus->Rm -= wrap;
                }
                ptrFPStatus->Rn += 2;
                if (!(ptrFPStatus->Rn & wrap_limit))
                    ptrFPStatus->Rn -= wrap;
                ptrFPStatus->Rd += 2;
                if (!(ptrFPStatus->Rd & wrap_limit))
                    ptrFPStatus->Rd -= wrap;
            }
            else
            {
                ptrFPStatus->Rm += 1;
                // Check for mixed scaler / vector operations (d bank and m bank)
                if ((ptrFPStatus->Rm >= wrap) && (!(ptrFPStatus->Rm & wrap_limit)))
                {   // Vector only operations
                    ptrFPStatus->Rm -= wrap;
                }
                ptrFPStatus->Rn += 1;
                if (!(ptrFPStatus->Rn & wrap_limit))
                    ptrFPStatus->Rn -= wrap;
                ptrFPStatus->Rd += 1;
                if (!(ptrFPStatus->Rd & wrap_limit))
                    ptrFPStatus->Rd -= wrap;
            }
        }   
    }
}   


// Defined within vfp_support.c
BOOL VFPir_SingleHandler (_FPStruct *ptrFPStatus, PCONTEXT pctx);
BOOL VFPir_DoubleHandler (_FPStruct *ptrFPStatus, PCONTEXT pctx);

/*****************************************************************************
*
* unsigned int VFPir_DecodeRegister (   unsigned int instruction, unsigned int precision, 
*                                       unsigned int Register_LSB)
*
* Decodes the instruction registers, rd, rm, rn 
*
*       instruction     :   The VFP instruction being executed.
*       precision       :   Single or Double (saves decoding it again).                         
*       Register_LSB    :   The least significant bit of the register number.
*
******************************************************************************/
unsigned int VFPir_DecodeRegister ( unsigned int instruction, unsigned int precision, 
                                    unsigned int Register_LSB)
{
    unsigned int reg_decoded;
    unsigned int bit;
    
    switch (Register_LSB)
    {
        case RD_LSB:
            reg_decoded = EXTRACT_BITS(instruction, 15, 12);
            bit = 22;
            break;
        case RN_LSB:
            reg_decoded = EXTRACT_BITS(instruction, 19, 16);
            bit = 7;
            break;
        case RM_LSB:
            reg_decoded = EXTRACT_BITS(instruction, 3, 0);
            bit = 5;
            break;
        default:
            bit = 0;
            break;
    }
    
    if (precision == SINGLE_PRECISION)
    {   // Additional bit to decode the single precision registers
        reg_decoded = (reg_decoded << 1) | EXTRACT_BITS(instruction, bit, bit);
    }

    return reg_decoded; 
}

DWORD FpscrToIeee (DWORD fpscr)
{
    DWORD dwIeee = _controlfp (0, 0);    // get current status

    _controlfp (_MCW_EM, _MCW_EM);      // mask all 

    if (fpscr & FPSCR_IXE) {
        dwIeee &= ~_EM_INEXACT;
    }
    if (fpscr & FPSCR_IOE) {
        dwIeee &= ~_EM_INVALID;
    }
    if (fpscr & FPSCR_DZE) {
        dwIeee &= ~_EM_ZERODIVIDE;
    }
    if (fpscr & FPSCR_OFE) {
        dwIeee &= ~_EM_OVERFLOW;
    }
    if (fpscr & FPSCR_UFE) {
        dwIeee &= ~_EM_UNDERFLOW;
    }

    return dwIeee;
}

DWORD CheckException (DWORD fpscr, DWORD emstat)
{
    if ((emstat & _SW_ZERODIVIDE) && (fpscr & FPSCR_DZE)) {
        return STATUS_FLOAT_DIVIDE_BY_ZERO;
    }
    if ((emstat & _SW_INVALID) && (fpscr & FPSCR_IOE)) {
        return STATUS_FLOAT_INVALID_OPERATION;
    }
    if ((emstat & _SW_UNDERFLOW) && (fpscr & FPSCR_UFE)) {
        return STATUS_FLOAT_UNDERFLOW;
    }
    if ((emstat & _SW_OVERFLOW) && (fpscr & FPSCR_OFE)) {
        return STATUS_FLOAT_OVERFLOW;
    }
    if ((emstat & _SW_INEXACT) && (fpscr & FPSCR_IXE)) {
        return STATUS_FLOAT_INEXACT_RESULT;
    }
    return 0;
}




/*****************************************************************************
*
* int VFP10HandleVFPException (EXCEPTION_RECORD *pEr, PCONTEXT pctx)
*
* Deals with the Exception by passing control onto the appropriate handler
* returns an code indicating whether the exception has been handled or not.
*
******************************************************************************/
BOOL VFP10HandleVFPException (EXCEPTION_RECORD *pEr, PCONTEXT pctx)
{
    _FPStruct FPStatus;
    BOOL ret_val;
    DWORD emstat;           // the emulator fpstat
    DWORD fpctrl, dwIeee;

    FPStatus.FPEXC = pctx->FpExc; // Get VFP exception status   
    FPStatus.FPSCR = pctx->Fpscr;
    FPStatus.FPINST = pctx->FpExtra[FPINST_REG];

    // Obtain precision, either double or single and setup function pointer
    FPStatus.precision = EXTRACT_BITS(FPStatus.FPINST, 8, 8);
    FPStatus.stride = EXTRACT_BITS(FPStatus.FPSCR, 21, 20);
    FPStatus.len = EXTRACT_BITS(FPStatus.FPSCR, 18, 16);

    FPStatus.opcode = EXTRACT_BITS(FPStatus.FPINST, 6, 6);
    FPStatus.opcode |= (EXTRACT_BITS(FPStatus.FPINST, 21, 20) << 1);
    FPStatus.opcode |= (EXTRACT_BITS(FPStatus.FPINST, 23, 23) << 3);
    
    FPStatus.opcode_ext = 0;
    if (FPStatus.opcode == EXTEND) {
        FPStatus.opcode_ext = EXTRACT_BITS(FPStatus.FPINST, 19, 16) << 1;
        FPStatus.opcode_ext |= EXTRACT_BITS(FPStatus.FPINST, 7, 7);
    }
    
    // Determine remaining iterations, FMULD, FDIV and FSQRT have one
    // less iteration than specified in FPEXC
    if (FPStatus.len == 0) {
       FPStatus.iterations = 0;
    } else {
        BOOL fAdd1 = FALSE;     // Rev0 "Feature", should remove from Rev1
        FPStatus.iterations = EXTRACT_BITS(FPStatus.FPEXC, 10, 8);

#define REV0_QUOTE_FEATURE
#ifdef REV0_QUOTE_FEATURE        
        switch (FPStatus.opcode) {
        case FMUL:
            if (FPStatus.precision == DOUBLE_PRECISION) {
                break;
            }
            // fall through
        case FDIV:
            fAdd1 = TRUE;
            break;
        case EXTEND:
            if (FPStatus.opcode_ext == FSQRT) {
                fAdd1 = TRUE;
            }
            break;