Shaker/Calc/SIMDFxpConvert/SIMDFxpConverter.cs

using FxpConvert.Common;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;

namespace SIMDFxpConvert
{
    public unsafe sealed class SIMDFxpConverter : IFxpConvert
    {
        public bool IsSupport => true;

        public void DoubleConvertToDxp(ref double value, NiFpga_FxpTypeInfo dxpinfo, ref ulong fxp, uint count)
        {
            if (count == 0) return;
            double pow = (1ul << (dxpinfo.wordLength - dxpinfo.integerWordLength));
            uint onecount = (uint)(512 / (Unsafe.SizeOf<double>() * 8));
            uint c = count / onecount;
            ref var source = ref Unsafe.As<double, System.Runtime.Intrinsics.Vector512<double>>(ref value);
            ulong* fxpptr = (ulong*)Unsafe.AsPointer(ref fxp);
            for (int i = 0; i < c; i++)
            {
                var temp = Unsafe.Add(ref source, i) * pow;
                for (int j = 0; j < onecount; j++)
                {
                    fxpptr[i * onecount + j] = (ulong)temp[j];
                }
            }
            uint c1 = count % onecount;
            if (c1 != 0)
            {
                uint start = c * onecount;
                for (uint i = 0; i < c1; i++)
                {
                    ref double temp = ref Unsafe.Add(ref value, start + i);

                    ref ulong tempu = ref Unsafe.Add(ref fxp, start + i);
                    tempu = (ulong)(temp * pow);
                }
            }
        }

        public void FloatConvertToDxp(ref float value, NiFpga_FxpTypeInfo dxpinfo, ref ulong fxp, uint count)
        {
            if (count == 0) return;
            float pow = (1ul << (dxpinfo.wordLength - dxpinfo.integerWordLength));
            uint onecount = (uint)(512 / (Unsafe.SizeOf<float>() * 8));
            uint c = count / onecount;
            ref var source = ref Unsafe.As<float, System.Runtime.Intrinsics.Vector512<float>>(ref value);
            ulong* fxpptr = (ulong*)Unsafe.AsPointer(ref fxp);
            for (int i = 0; i < c; i++)
            {
                var temp = Unsafe.Add(ref source, i) * pow;
                for (int j = 0; j < onecount; j++)
                {
                    fxpptr[i * onecount + j] = (ulong)temp[j];
                }
            }
            uint c1 = count % onecount;
            if (c1 != 0)
            {
                uint start = c * onecount;
                for (uint i = 0; i < c1; i++)
                {
                    ref float temp = ref Unsafe.Add(ref value, start + i);

                    ref ulong tempu = ref Unsafe.Add(ref fxp, start + i);
                    tempu = (ulong)(temp * pow);
                }
            }
        }

        public void FxpConvertToDouble(ref ulong fxp, NiFpga_FxpTypeInfo dxpinfo, ref double value, uint count)
        {
            if (count == 0) return;
            double delta = Math.Pow(2, dxpinfo.integerWordLength - dxpinfo.wordLength);
            ulong mask = dxpinfo.wordLength == 64 ? ulong.MaxValue : ((1ul << dxpinfo.wordLength) - 1);
            if (dxpinfo.isSigned)
            {
                ulong signedmask = 1ul << (dxpinfo.wordLength - 1);
                for (int i = 0; i < count; i++)
                {
                    ref double resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = tempfxpref & mask;
                    if ((tempdata & signedmask) != 0)
                    {
                        long signeddata = (long)(tempdata ^ mask);
                        signeddata = (signeddata + 1) * -1;
                        resultref = signeddata * delta;
                    }
                    else resultref = delta * tempdata;
                }
            }
            else
            {
                for (int i = 0; i < count; i++)
                {
                    ref double resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = tempfxpref & mask;
                    resultref = delta * tempdata;
                }
            }
        }

        public void FxpConvertToFloat(ref ulong fxp, NiFpga_FxpTypeInfo dxpinfo, ref float value, uint count)
        {
            if (count == 0) return;
            float delta = MathF.Pow(2, dxpinfo.integerWordLength - dxpinfo.wordLength);
            ulong mask = dxpinfo.wordLength == 64 ? ulong.MaxValue : ((1ul << dxpinfo.wordLength) - 1);
            if (dxpinfo.isSigned)
            {
                ulong signedmask = 1ul << (dxpinfo.wordLength - 1);
                for (int i = 0; i < count; i++)
                {
                    ref float resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = tempfxpref & mask;
                    if ((tempdata & signedmask) != 0)
                    {
                        long signeddata = (long)(tempdata ^ mask);
                        signeddata = (signeddata + 1) * -1;
                        resultref = signeddata * delta;
                    }
                    else resultref = delta * tempdata;
                }
            }
            else
            {
                for (int i = 0; i < count; i++)
                {
                    ref float resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = tempfxpref & mask;
                    resultref = delta * tempdata;
                }
            }
        }
    }
}