///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1997, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
// 
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission. 
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////


//------------------------------------------------------------------------
//
//	Functions to control floating point exceptions.
//
//------------------------------------------------------------------------

#include "IexMathFpu.h"

#include <stdint.h>
#include <IlmBaseConfig.h>
#include <stdio.h>

#if 0
    #include <iostream>
    #define debug(x) (std::cout << x << std::flush)
#else
    #define debug(x)
#endif

#if defined(HAVE_UCONTEXT_H) && (defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86))

#include <ucontext.h>
#include <signal.h>
#include <iostream>
#include <stdint.h>


IEX_INTERNAL_NAMESPACE_SOURCE_ENTER



namespace FpuControl
{

//-------------------------------------------------------------------
//
//    Modern x86 processors and all AMD64 processors have two
//    sets of floating-point control/status registers: cw and sw
//    for legacy x87 stack-based arithmetic, and mxcsr for
//    SIMD arithmetic.  When setting exception masks or checking
//    for exceptions, we must set/check all relevant registers,
//    since applications may contain code that uses either FP
//    model.
//
//    These functions handle both FP models for x86 and AMD64.
//
//-------------------------------------------------------------------

//-------------------------------------------------------------------
//
//    Restore the control register state from a signal handler
//    user context, optionally clearing the exception bits
//    in the restored control register, if applicable.
//
//-------------------------------------------------------------------

void restoreControlRegs (const ucontext_t & ucon,
			 bool clearExceptions = false);


//------------------------------------------------------------
//
//    Set exception mask bits in the control register state.
//    A value of 1 means the exception is masked, a value of
//    0 means the exception is enabled.
//
//    setExceptionMask returns the previous mask value.  If
//    the 'exceptions' pointer is non-null, it returns in 
//    this argument the FPU exception bits.
//
//------------------------------------------------------------

const int INVALID_EXC   = (1<<0);
const int DENORMAL_EXC  = (1<<1);
const int DIVZERO_EXC   = (1<<2);
const int OVERFLOW_EXC  = (1<<3);
const int UNDERFLOW_EXC = (1<<4);
const int INEXACT_EXC   = (1<<5);
const int ALL_EXC       = INVALID_EXC  | DENORMAL_EXC  | DIVZERO_EXC |
                          OVERFLOW_EXC | UNDERFLOW_EXC | INEXACT_EXC;

int setExceptionMask (int mask, int * exceptions = 0);
int getExceptionMask ();


//---------------------------------------------
//
//    Get/clear the exception bits in the FPU.
//
//---------------------------------------------

int  getExceptions ();
void clearExceptions ();


//------------------------------------------------------------------
//
//    Everything below here is implementation.  Do not use these
//    constants or functions in your applications or libraries.
//    This is not the code you're looking for.  Move along.
//
//    Optimization notes -- on a Pentium 4, at least, it appears
//    to be faster to get the mxcsr first and then the cw; and to
//    set the cw first and then the mxcsr.  Also, it seems to
//    be faster to clear the sw exception bits after setting
//    cw and mxcsr.
//
//------------------------------------------------------------------

static inline uint16_t
getSw ()
{
    uint16_t sw;
    asm volatile ("fnstsw %0" : "=m" (sw) : );
    return sw;
}

static inline void
setCw (uint16_t cw)
{
    asm volatile ("fldcw %0" : : "m" (cw) );
}

static inline uint16_t
getCw ()
{
    uint16_t cw;
    asm volatile ("fnstcw %0" : "=m" (cw) : );
    return cw;
}

static inline void
setMxcsr (uint32_t mxcsr, bool clearExceptions)
{
    mxcsr &= clearExceptions ? 0xffffffc0 : 0xffffffff;
    asm volatile ("ldmxcsr %0" : : "m" (mxcsr) );
}

static inline uint32_t
getMxcsr ()
{
    uint32_t mxcsr;
    asm volatile ("stmxcsr %0" : "=m" (mxcsr) : );
    return mxcsr;
}

static inline int
calcMask (uint16_t cw, uint32_t mxcsr)
{
    //
    // Hopefully, if the user has been using FpuControl functions,
    // the masks are the same, but just in case they're not, we
    // AND them together to report the proper subset of the masks.
    //

    return (cw & ALL_EXC) & ((mxcsr >> 7) & ALL_EXC);
}

inline int
setExceptionMask (int mask, int * exceptions)
{
    uint16_t cw = getCw ();
    uint32_t mxcsr = getMxcsr ();
    
    if (exceptions)
	*exceptions = (mxcsr & ALL_EXC) | (getSw () & ALL_EXC);

    int oldmask = calcMask (cw, mxcsr);

    //
    // The exception constants are chosen very carefully so that
    // we can do a simple mask and shift operation to insert
    // them into the control words.  The mask operation is for 
    // safety, in case the user accidentally set some other
    // bits in the exception mask.
    //

    mask &= ALL_EXC;
    cw = (cw & ~ALL_EXC) | mask;
    mxcsr = (mxcsr & ~(ALL_EXC << 7)) | (mask << 7);

    setCw (cw);
    setMxcsr (mxcsr, false);

    return oldmask;
}

inline int
getExceptionMask ()
{
    uint32_t mxcsr = getMxcsr ();
    uint16_t cw = getCw ();
    return calcMask (cw, mxcsr);
}

inline int
getExceptions ()
{
    return (getMxcsr () | getSw ()) & ALL_EXC;
}

void
clearExceptions ()
{
    uint32_t mxcsr = getMxcsr () & 0xffffffc0;
    asm volatile ("ldmxcsr %0\n"
		  "fnclex"
		  : : "m" (mxcsr) );
}

// If the fpe was taken while doing a float-to-int cast using the x87,
// the rounding mode and possibly the precision will be wrong.  So instead
// of restoring to the state as of the fault, we force the rounding mode
// to be 'nearest' and the precision to be double extended.
//
// rounding mode is in bits 10-11, value 00 == round to nearest
// precision is in bits 8-9, value 11 == double extended (80-bit)
//
const uint16_t cwRestoreMask = ~((3 << 10) | (3 << 8));
const uint16_t cwRestoreVal = (0 << 10) | (3 << 8);


#ifdef ILMBASE_HAVE_CONTROL_REGISTER_SUPPORT

inline void
restoreControlRegs (const ucontext_t & ucon, bool clearExceptions)
{
    setCw ((ucon.uc_mcontext.fpregs->cwd & cwRestoreMask) | cwRestoreVal);
    setMxcsr (ucon.uc_mcontext.fpregs->mxcsr, clearExceptions);
}

#else

//
// Ugly, the mxcsr isn't defined in GNU libc ucontext_t, but
// it's passed to the signal handler by the kernel.  Use
// the kernel's version of the ucontext to get it, see
// <asm/sigcontext.h>
//

#include <asm/sigcontext.h>

inline void
restoreControlRegs (const ucontext_t & ucon, bool clearExceptions)
{
    setCw ((ucon.uc_mcontext.fpregs->cw & cwRestoreMask) | cwRestoreVal);
    
    _fpstate * kfp = reinterpret_cast<_fpstate *> (ucon.uc_mcontext.fpregs);
    setMxcsr (kfp->magic == 0 ? kfp->mxcsr : 0, clearExceptions);
}

#endif

} // namespace FpuControl


namespace {

volatile FpExceptionHandler fpeHandler = 0;

extern "C" void
catchSigFpe (int sig, siginfo_t *info, ucontext_t *ucon)
{
    debug ("catchSigFpe (sig = "<< sig << ", ...)\n");

    FpuControl::restoreControlRegs (*ucon, true);

    if (fpeHandler == 0)
	return;

    if (info->si_code == SI_USER)
    {
	fpeHandler (0, "Floating-point exception, caused by "
		       "a signal sent from another process.");
	return;
    }

    if (sig == SIGFPE)
    {
	switch (info->si_code)
	{
	  //
	  // IEEE 754 floating point exceptions:
	  //

	  case FPE_FLTDIV:
	    fpeHandler (IEEE_DIVZERO, "Floating-point division by zero.");
	    return;

	  case FPE_FLTOVF:
	    fpeHandler (IEEE_OVERFLOW, "Floating-point overflow.");
	    return;

	  case FPE_FLTUND:
	    fpeHandler (IEEE_UNDERFLOW, "Floating-point underflow.");
	    return;

	  case FPE_FLTRES:
	    fpeHandler (IEEE_INEXACT, "Inexact floating-point result.");
	    return;

	  case FPE_FLTINV:
	    fpeHandler (IEEE_INVALID, "Invalid floating-point operation.");
	    return;

	  //
	  // Other arithmetic exceptions which can also
	  // be trapped by the operating system:
	  //

	  case FPE_INTDIV:
	    fpeHandler (0, "Integer division by zero.");
	    break;

	  case FPE_INTOVF:
	    fpeHandler (0, "Integer overflow.");
	    break;

	  case FPE_FLTSUB:
	    fpeHandler (0, "Subscript out of range.");
	    break;
	}
    }

    fpeHandler (0, "Floating-point exception.");
}

} // namespace

void
setFpExceptions (int when)
{
    int mask = FpuControl::ALL_EXC;

    if (when & IEEE_OVERFLOW)
	mask &= ~FpuControl::OVERFLOW_EXC;
    if (when & IEEE_UNDERFLOW)
	mask &= ~FpuControl::UNDERFLOW_EXC;
    if (when & IEEE_DIVZERO)
	mask &= ~FpuControl::DIVZERO_EXC;
    if (when & IEEE_INEXACT)
	mask &= ~FpuControl::INEXACT_EXC;
    if (when & IEEE_INVALID)
	mask &= ~FpuControl::INVALID_EXC;

    //
    // The Linux kernel apparently sometimes passes
    // incorrect si_info to signal handlers unless
    // the exception flags are cleared.
    //
    // XXX is this still true on 2.4+ kernels?
    //
    
    FpuControl::setExceptionMask (mask);
    FpuControl::clearExceptions ();
}


int
fpExceptions ()
{
    int mask = FpuControl::getExceptionMask ();

    int when = 0;

    if (!(mask & FpuControl::OVERFLOW_EXC))
	when |= IEEE_OVERFLOW;
    if (!(mask & FpuControl::UNDERFLOW_EXC))
	when |= IEEE_UNDERFLOW;
    if (!(mask & FpuControl::DIVZERO_EXC))
	when |= IEEE_DIVZERO;
    if (!(mask & FpuControl::INEXACT_EXC))
	when |= IEEE_INEXACT;
    if (!(mask & FpuControl::INVALID_EXC))
	when |= IEEE_INVALID;

    return when;
}

void
handleExceptionsSetInRegisters()
{
    if (fpeHandler == 0)
	return;

    int mask = FpuControl::getExceptionMask ();

    int exc = FpuControl::getExceptions();

    if (!(mask & FpuControl::DIVZERO_EXC) && (exc & FpuControl::DIVZERO_EXC))
    {
        fpeHandler(IEEE_DIVZERO, "Floating-point division by zero.");
        return;
    }

    if (!(mask & FpuControl::OVERFLOW_EXC) && (exc & FpuControl::OVERFLOW_EXC))
    {
        fpeHandler(IEEE_OVERFLOW, "Floating-point overflow.");
        return;
    }

    if (!(mask & FpuControl::UNDERFLOW_EXC) && (exc & FpuControl::UNDERFLOW_EXC))
    {
        fpeHandler(IEEE_UNDERFLOW, "Floating-point underflow.");
        return;
    }

    if (!(mask & FpuControl::INEXACT_EXC) && (exc & FpuControl::INEXACT_EXC))
    {
        fpeHandler(IEEE_INEXACT, "Inexact floating-point result.");
        return;
    }

    if (!(mask & FpuControl::INVALID_EXC) && (exc & FpuControl::INVALID_EXC))
    {
        fpeHandler(IEEE_INVALID, "Invalid floating-point operation.");
        return;
    }
}


void
setFpExceptionHandler (FpExceptionHandler handler)
{
    if (fpeHandler == 0)
    {
	struct sigaction action;
	sigemptyset (&action.sa_mask);
	action.sa_flags = SA_SIGINFO | SA_NOMASK;
	action.sa_sigaction = (void (*) (int, siginfo_t *, void *)) catchSigFpe;
	action.sa_restorer = 0;

	sigaction (SIGFPE, &action, 0);
    }

    fpeHandler = handler;
}


IEX_INTERNAL_NAMESPACE_SOURCE_EXIT


#else

#include <signal.h>
#include <assert.h>

IEX_INTERNAL_NAMESPACE_SOURCE_ENTER


namespace 
{
	volatile FpExceptionHandler fpeHandler = 0;
	void fpExc_(int x)
	{
	    if (fpeHandler != 0)
	    {
		fpeHandler(x, "");
	    }
	    else
	    {
		assert(0 != "Floating point exception");
	    }
	}
}

void
setFpExceptions( int )
{
}


void
setFpExceptionHandler (FpExceptionHandler handler)
{
    // improve floating point exception handling nanoscopically above "nothing at all"
    fpeHandler = handler;
    signal(SIGFPE, fpExc_);
}

int
fpExceptions()
{
    return 0;
}

void
handleExceptionsSetInRegisters()
{
    // No implementation on this platform
}

IEX_INTERNAL_NAMESPACE_SOURCE_EXIT

#endif