CastReg.h

Go to the documentation of this file.

// CastReg.h:
// ------------
//

#ifndef __IPSDKUTIL_INSTRUCTIONSET_DETAIL_SSE2_CASTREG_H__
#define __IPSDKUTIL_INSTRUCTIONSET_DETAIL_SSE2_CASTREG_H__

#include <IPSDKUtil/IPSDKUtilExports.h>
#include <IPSDKUtil/InstructionSet/detail/CastReg.h>
#include <IPSDKUtil/Tools/ForceInline.h>

#include <boost/mpl/and.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/not.hpp>
#include <boost/mpl/or.hpp>
#include <boost/type_traits/is_signed.hpp>

namespace ipsdk {
namespace simd {
namespace detail {

IPSDK_FORCEINLINE __m128 _custom_mm_cvtepu32_ps(const __m128i v)
{
    const __m128 two16 = _mm_set1_ps(static_cast<float>(1 << 16));

    // Avoid double rounding by doing two exact conversions
    // of high and low 16-bit segments
    const __m128i hi = _mm_srli_epi32(v, 16);
    const __m128i lo = _mm_srli_epi32(_mm_slli_epi32((__m128i)v, 16), 16);
    const __m128 fHi = _mm_mul_ps(_mm_cvtepi32_ps(hi), two16);
    const __m128 fLo = _mm_cvtepi32_ps(lo);

    // do single rounding according to current rounding mode
    return _mm_add_ps(fHi, fLo);
}

IPSDK_FORCEINLINE void _custom_mm_cvtepu32_ps(const __m128i v, __m128& out)
{
    const __m128 two16 = _mm_set1_ps(static_cast<float>(1 << 16));

    // Avoid double rounding by doing two exact conversions
    // of high and low 16-bit segments
    const __m128i hi = _mm_srli_epi32(v, 16);
    const __m128i lo = _mm_srli_epi32(_mm_slli_epi32((__m128i)v, 16), 16);
    const __m128 fHi = _mm_mul_ps(_mm_cvtepi32_ps(hi), two16);
    const __m128 fLo = _mm_cvtepi32_ps(lo);

    // do single rounding according to current rounding mode
    out = _mm_add_ps(fHi, fLo);
}

IPSDK_FORCEINLINE __m128i _custom_mm_cvttps_epu32(__m128 f)
{
    const __m128 two31 = _mm_set1_ps(
        static_cast<float>(static_cast<unsigned int>(1) << 31));
    //const __m128 two32 = _mm_add_ps(two31, two31);
    const __m128 zero = _mm_xor_ps(f, f);

    // check for overflow before conversion to int
    const __m128 overflow = _mm_cmpge_ps(f, two31);
    //const __m128 overflow2 = _mm_cmpge_ps(f, two32);
    const __m128 subval = _mm_and_ps(overflow, two31);
    //const __m128i addval = _mm_slli_epi32((__m128i)overflow, 31);
    __m128i addval = _mm_cvttps_epi32(overflow);
    //addval = _mm_slli_epi32(addval, 31);
    //const __m128i addval = _mm_slli_epi32(_mm_set1_epi32(1), 31);
    __m128i result;

    // bias the value to signed space if it is >= 2**31
    f = _mm_sub_ps(f, subval);

    // clip at zero
    f = _mm_max_ps(f, zero);

    // convert to int with saturation
    result = _mm_cvttps_epi32(f); // rounding mode should be round to nearest

    // unbias
    result = _mm_add_epi32(result, addval);

    // patch up the overflow case
    //result = _mm_or_si128(result, (__m128i)overflow2);

    return result;
}

IPSDK_FORCEINLINE void _custom_mm_cvttps_epu32(__m128 f, __m128i& out)
{
    const __m128 two31 = _mm_set1_ps(
        static_cast<float>(static_cast<unsigned int>(1) << 31));
    //const __m128 two32 = _mm_add_ps(two31, two31);
    const __m128 zero = _mm_xor_ps(f, f);

    // check for overflow before conversion to int
    const __m128 overflow = _mm_cmpge_ps(f, two31);
    //const __m128 overflow2 = _mm_cmpge_ps(f, two32);
    const __m128 subval = _mm_and_ps(overflow, two31);
    //const __m128i addval = _mm_slli_epi32((__m128i)overflow, 31);
    __m128i addval = _mm_cvttps_epi32(overflow);
    //addval = _mm_slli_epi32(addval, 31);
    //const __m128i addval = _mm_slli_epi32(_mm_set1_epi32(1), 31);
    __m128i result;

    // bias the value to signed space if it is >= 2**31
    f = _mm_sub_ps(f, subval);

    // clip at zero
    f = _mm_max_ps(f, zero);

    // convert to int with saturation
    result = _mm_cvttps_epi32(f); // rounding mode should be round to nearest

    // unbias
    out = _mm_add_epi32(result, addval);

    // patch up the overflow case
    //result = _mm_or_si128(result, (__m128i)overflow2);
}

IPSDK_FORCEINLINE __m128d _custom_mm_cvtepu32_pd(const __m128i v)
{
    const __m128d two16 = _mm_set1_pd(static_cast<ipReal64>(1 << 16));

    // Avoid double rounding by doing two exact conversions
    // of high and low 16-bit segments
    const __m128i hi = _mm_srli_epi32(v, 16);
    const __m128i lo = _mm_srli_epi32(_mm_slli_epi32((__m128i)v, 16), 16);
    const __m128d fHi = _mm_mul_pd(_mm_cvtepi32_pd(hi), two16);
    const __m128d fLo = _mm_cvtepi32_pd(lo);

    // do single rounding according to current rounding mode
    return _mm_add_pd(fHi, fLo);
}

IPSDK_FORCEINLINE void _custom_mm_cvtepu32_pd(const __m128i v, __m128d& out)
{
    const __m128d two16 = _mm_set1_pd(static_cast<ipReal64>(1 << 16));

    // Avoid double rounding by doing two exact conversions
    // of high and low 16-bit segments
    const __m128i hi = _mm_srli_epi32(v, 16);
    const __m128i lo = _mm_srli_epi32(_mm_slli_epi32((__m128i)v, 16), 16);
    const __m128d fHi = _mm_mul_pd(_mm_cvtepi32_pd(hi), two16);
    const __m128d fLo = _mm_cvtepi32_pd(lo);

    // do single rounding according to current rounding mode
    out = _mm_add_pd(fHi, fLo);
}

IPSDK_FORCEINLINE __m128i _custom_mm_cvttpd_epu32(__m128d f)
{
    const __m128d two31 = _mm_set1_pd(
        static_cast<ipReal64>(static_cast<unsigned int>(1) << 31));
    //const __m128 two32 = _mm_add_ps(two31, two31);
    const __m128d zero = _mm_xor_pd(f, f);

    // check for overflow before conversion to int
    const __m128d overflow = _mm_cmpge_pd(f, two31);
    //const __m128 overflow2 = _mm_cmpge_ps(f, two32);
    const __m128d subval = _mm_and_pd(overflow, two31);
    //const __m128i addval = _mm_slli_epi32((__m128i)overflow, 31);
    __m128i addval = _mm_cvttpd_epi32(overflow);
    //addval = _mm_slli_epi32(addval, 31);
    //const __m128i addval = _mm_slli_epi32(_mm_set1_epi32(1), 31);
    __m128i result;

    // bias the value to signed space if it is >= 2**31
    f = _mm_sub_pd(f, subval);

    // clip at zero
    f = _mm_max_pd(f, zero);

    // convert to int with saturation
    result = _mm_cvttpd_epi32(f); // rounding mode should be round to nearest

    // unbias
    result = _mm_add_epi32(result, addval);

    // patch up the overflow case
    //result = _mm_or_si128(result, (__m128i)overflow2);

    return result;
}

IPSDK_FORCEINLINE void _custom_mm_cvttpd_epu32(__m128d f, __m128i& out)
{
    const __m128d two31 = _mm_set1_pd(
        static_cast<ipReal64>(static_cast<unsigned int>(1) << 31));
    //const __m128 two32 = _mm_add_ps(two31, two31);
    const __m128d zero = _mm_xor_pd(f, f);

    // check for overflow before conversion to int
    const __m128d overflow = _mm_cmpge_pd(f, two31);
    //const __m128 overflow2 = _mm_cmpge_ps(f, two32);
    const __m128d subval = _mm_and_pd(overflow, two31);
    //const __m128i addval = _mm_slli_epi32((__m128i)overflow, 31);
    __m128i addval = _mm_cvttpd_epi32(overflow);
    //addval = _mm_slli_epi32(addval, 31);
    //const __m128i addval = _mm_slli_epi32(_mm_set1_epi32(1), 31);
    __m128i result;

    // bias the value to signed space if it is >= 2**31
    f = _mm_sub_pd(f, subval);

    // clip at zero
    f = _mm_max_pd(f, zero);

    // convert to int with saturation
    result = _mm_cvttpd_epi32(f); // rounding mode should be round to nearest

    // unbias
    out = _mm_add_epi32(result, addval);

    // patch up the overflow case
    //result = _mm_or_si128(result, (__m128i)overflow2);
}

IPSDK_FORCEINLINE __m128i _custom_mm_packus_epi32(__m128i v0, __m128i v1)
{
    v0 = _mm_slli_epi32 (v0, 16);
    v0 = _mm_srai_epi32 (v0, 16);
    v1 = _mm_slli_epi32 (v1, 16);
    v1 = _mm_srai_epi32 (v1, 16);
    return _mm_packs_epi32 (v0, v1);
}

IPSDK_FORCEINLINE void _custom_mm_packus_epi32(__m128i v0, __m128i v1, __m128i& out)
{
    v0 = _mm_slli_epi32 (v0, 16);
    v0 = _mm_srai_epi32 (v0, 16);
    v1 = _mm_slli_epi32 (v1, 16);
    v1 = _mm_srai_epi32 (v1, 16);
    out = _mm_packs_epi32 (v0, v1);
}


template <typename TIn, typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, TIn, TOut,
    typename boost::enable_if_c<
        boost::is_same<TIn, TOut>::value || (boost::is_integral<TIn>::value
        && boost::is_integral<TOut>::value && sizeof(TIn)==sizeof(TOut))
    >::type
>
{
    static IPSDK_FORCEINLINE
    void act(const typename Sse2Type<TIn>::Type& in, 
             typename Sse2Type<TOut>::Type& out)
    {
        out = in;
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipUInt8, TOut,
    typename boost::enable_if<
        typename boost::mpl::equal_to<
            boost::mpl::int_<sizeof(TOut)>, 
            boost::mpl::int_<2>
        >::type
    >::type
>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipUInt8>::Type& in,
             typename Sse2Type<TOut>::Type& outl,
             typename Sse2Type<TOut>::Type& outh)
    {
        outl = _mm_unpacklo_epi8(in, _mm_set1_epi8(0));
        outh = _mm_unpackhi_epi8(in, _mm_set1_epi8(0));
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipInt8, TOut,
    typename boost::enable_if_c<sizeof(TOut)==2>::type>
{

    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipInt8>::Type& in, 
             typename Sse2Type<TOut>::Type& outl,
             typename Sse2Type<TOut>::Type& outh)
    {
        outl = _mm_unpacklo_epi8(in, in);
        outl = _mm_srai_epi16(outl, 8);
        outh = _mm_unpackhi_epi8(in, in);
        outh = _mm_srai_epi16(outh, 8);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipInt32, ipReal32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipInt32>::Type& in,
             Sse2Type<ipReal32>::Type& out)
    {
        out = _mm_cvtepi32_ps(in);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipUInt32, ipReal32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipUInt32>::Type& in,
             Sse2Type<ipReal32>::Type& out)
    {
        out = _custom_mm_cvtepu32_ps(in);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal32, ipInt32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal32>::Type& in,
             Sse2Type<ipInt32>::Type& out)
    {
        out = _mm_cvttps_epi32(in);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal32, ipUInt32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal32>::Type& in,
             Sse2Type<ipUInt32>::Type& out)
    {
        _custom_mm_cvttps_epu32(in, out);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipInt32, ipReal64>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipInt32>::Type& in,
             Sse2Type<ipReal64>::Type& outl,
             Sse2Type<ipReal64>::Type& outh)
    {
        outl = _mm_cvtepi32_pd(in);
        outh = _mm_cvtepi32_pd(_mm_srli_si128(in, 8));
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipUInt32, ipReal64>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipUInt32>::Type& in,
             Sse2Type<ipReal64>::Type& outl,
             Sse2Type<ipReal64>::Type& outh)
    {
        /*ipUInt32 bufUInt32[4];
        ipReal64 bufReal64[4];

        _mm_storeu_si128(
            reinterpret_cast<Sse2Type<ipUInt32>::Type*>(bufUInt32), in);

        for(ipUInt32 i=0; i<4; ++i)
            bufReal64[i] = bufUInt32[i];
        outl = _mm_loadu_pd(bufReal64);
        outh = _mm_loadu_pd(bufReal64+2);*/
        outl = _custom_mm_cvtepu32_pd(in);
        outh = _custom_mm_cvtepu32_pd(_mm_srli_si128(in, 8));
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal64, ipInt32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal64>::Type& inl,
             const Sse2Type<ipReal64>::Type& inh,
             Sse2Type<ipInt32>::Type& out)
    {
        Sse2Type<ipInt32>::Type outl = _mm_cvttpd_epi32(inl);
        Sse2Type<ipInt32>::Type outh = _mm_cvttpd_epi32(inh);
        out = _mm_or_si128(outl, _mm_slli_si128(outh, 8));
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal64, ipUInt32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal64>::Type& inl,
             const Sse2Type<ipReal64>::Type& inh,
             Sse2Type<ipUInt32>::Type& out)
    {
        Sse2Type<ipInt32>::Type outl, outh;
        _custom_mm_cvttpd_epu32(inl, outl);
        _custom_mm_cvttpd_epu32(inh, outh);
        out = _mm_or_si128(outl, _mm_slli_si128(outh, 8));
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipInt32, TOut,
    typename boost::enable_if_c<sizeof(TOut)==8 &&
    boost::is_integral<TOut>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipInt32>::Type& in,
             typename Sse2Type<TOut>::Type& outl,
             typename Sse2Type<TOut>::Type& outh)
    {
        const Sse2Type<ipInt32>::Type zero = _mm_setzero_si128();
        const Sse2Type<ipInt32>::Type hi =
            _mm_cmplt_epi32(in, zero);
        outl = _mm_unpacklo_epi32(in, hi);
        outh = _mm_unpackhi_epi32(in, hi);
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipUInt32, TOut,
    typename boost::enable_if_c<sizeof(TOut)==8 &&
    boost::is_integral<TOut>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipUInt32>::Type& in,
             typename Sse2Type<TOut>::Type& outl,
             typename Sse2Type<TOut>::Type& outh)
    {
        outl = _mm_unpacklo_epi32(in, _mm_set1_epi32(0));
        outh = _mm_unpackhi_epi32(in, _mm_set1_epi32(0));
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipInt16, TOut,
    typename boost::enable_if_c<sizeof(TOut)==4
    && boost::is_integral<TOut>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipInt16>::Type& in,
             typename Sse2Type<TOut>::Type& outl,
             typename Sse2Type<TOut>::Type& outh)
    {
        outl = _mm_unpacklo_epi16(in, in);
        outh = _mm_unpackhi_epi16(in, in);
        outl = _mm_srai_epi32(outl, 16);
        outh = _mm_srai_epi32(outh, 16);
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipUInt16, TOut,
    typename boost::enable_if_c<sizeof(TOut)==4 &&
    boost::is_integral<TOut>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipUInt16>::Type& in,
             typename Sse2Type<TOut>::Type& outl,
             typename Sse2Type<TOut>::Type& outh)
    {
        outl = _mm_unpacklo_epi16(in, _mm_set1_epi16(0));
        outh = _mm_unpackhi_epi16(in, _mm_set1_epi16(0));
    }
};

template <typename TIn>
struct CastReg<eInstructionSet::eIS_Sse2, TIn, ipReal32,
    typename boost::enable_if_c<sizeof(TIn)==2 && 
    boost::is_integral<TIn>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const typename Sse2Type<TIn>::Type& in,
             Sse2Type<ipReal32>::Type& outl,
             Sse2Type<ipReal32>::Type& outh)
    {
        Sse2Type<ipInt32>::Type in32l, in32h;
        CastReg<eInstructionSet::eIS_Sse2, TIn, ipInt32>::act(in, in32l, in32h);
        CastReg<eInstructionSet::eIS_Sse2, ipInt32, ipReal32>::act(in32l, outl);
        CastReg<eInstructionSet::eIS_Sse2, ipInt32, ipReal32>::act(in32h, outh);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal32, ipReal64>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal32>::Type& in,
             Sse2Type<ipReal64>::Type& outl,
             Sse2Type<ipReal64>::Type& outh)
    {
        outl = _mm_cvtps_pd(in);
        outh = _mm_cvtps_pd(_mm_shuffle_ps(in, in, _MM_SHUFFLE(3, 2, 3, 2)));
    }
};

template <typename TIn>
struct CastReg<eInstructionSet::eIS_Sse2, TIn, ipUInt8,
    typename boost::enable_if_c<sizeof(TIn)==2>::type
>
{
    static IPSDK_FORCEINLINE
    void act(const typename Sse2Type<TIn>::Type& inl, 
             const typename Sse2Type<TIn>::Type& inh,
             Sse2Type<ipUInt8>::Type& out)
    {
        out = _mm_packus_epi16(inl, inh);
    }
};

template <typename TIn>
struct CastReg<eInstructionSet::eIS_Sse2, TIn, ipInt8,
    typename boost::enable_if_c<sizeof(TIn)==2>::type>
{
    static IPSDK_FORCEINLINE
    void act(const typename Sse2Type<TIn>::Type& inl, 
             const typename Sse2Type<TIn>::Type& inh,
             Sse2Type<ipInt8>::Type& out)
    {
        out = _mm_packs_epi16(inl, inh);
    }
};

template <typename TIn>
struct CastReg<eInstructionSet::eIS_Sse2, TIn, ipInt16,
    typename boost::enable_if_c<sizeof(TIn)==4 && boost::is_integral<TIn>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const typename Sse2Type<TIn>::Type& inl,
             const typename Sse2Type<TIn>::Type& inh,
             Sse2Type<ipInt16>::Type& out)
    {
        out = _mm_packs_epi32(inl, inh);
    }
};

template <typename TIn>
struct CastReg<eInstructionSet::eIS_Sse2, TIn, ipUInt16,
    typename boost::enable_if_c<sizeof(TIn)==4 && boost::is_integral<TIn>::value>::type>
{
    static IPSDK_FORCEINLINE
    void act(const typename Sse2Type<TIn>::Type& inl,
             const typename Sse2Type<TIn>::Type& inh,
             Sse2Type<ipUInt16>::Type& out)
    {
        out = _custom_mm_packus_epi32(inl, inh);
    }
};

template <typename TOut>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal32, TOut,
    typename boost::enable_if_c<sizeof(TOut)==2>::type>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal32>::Type& inl,
             const Sse2Type<ipReal32>::Type& inh,
             typename Sse2Type<TOut>::Type& out)
    {
        Sse2Type<ipInt32>::Type inlInt32, inhInt32;
        CastReg<eInstructionSet::eIS_Sse2, ipReal32, ipInt32>::act(
            inl, inlInt32);
        CastReg<eInstructionSet::eIS_Sse2, ipReal32, ipInt32>::act(
            inh, inhInt32);
        CastReg<eInstructionSet::eIS_Sse2, ipInt32, TOut>::act(
            inlInt32, inhInt32, out);
    }
};

template <>
struct CastReg<eInstructionSet::eIS_Sse2, ipReal64, ipReal32>
{
    static IPSDK_FORCEINLINE
    void act(const Sse2Type<ipReal64>::Type& inl,
             const Sse2Type<ipReal64>::Type& inh,
             Sse2Type<ipReal32>::Type& out)
    {
        out = _mm_movelh_ps(_mm_cvtpd_ps(inl), _mm_cvtpd_ps(inh));
    }
};


} // end of namespace detail
} // end of namespace simd
} // end of namespace ipsdk

#endif // __IPSDKUTIL_INSTRUCTIONSET_DETAIL_SSE2_CASTREG_H__