Merge pull request #1811 from SixLabors/bp/quantizeblocksse

Add SSE41 version of Quantize block

Author: brianpopow

src/ImageSharp/Formats/Webp/Lossy/QuantEnc.cs

@@ -3,20 +3,37 @@
 
 using System;
 using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+#if SUPPORTS_RUNTIME_INTRINSICS
+using System.Runtime.Intrinsics;
+using System.Runtime.Intrinsics.X86;
+#endif
 
 namespace SixLabors.ImageSharp.Formats.Webp.Lossy
 {
     /// <summary>
     /// Quantization methods.
     /// </summary>
-    internal static class QuantEnc
+    internal static unsafe class QuantEnc
     {
         private static readonly byte[] Zigzag = { 0, 1, 4, 8, 5, 2, 3, 6, 9, 12, 13, 10, 7, 11, 14, 15 };
 
         private static readonly ushort[] WeightY = { 38, 32, 20, 9, 32, 28, 17, 7, 20, 17, 10, 4, 9, 7, 4, 2 };
 
         private const int MaxLevel = 2047;
 
+#if SUPPORTS_RUNTIME_INTRINSICS
+        private static readonly Vector128<short> MaxCoeff2047 = Vector128.Create((short)MaxLevel);
+
+        private static readonly Vector128<byte> CstLo = Vector128.Create(0, 1, 2, 3, 8, 9, 254, 255, 10, 11, 4, 5, 6, 7, 12, 13);
+
+        private static readonly Vector128<byte> Cst7 = Vector128.Create(254, 255, 254, 255, 254, 255, 254, 255, 14, 15, 254, 255, 254, 255, 254, 255);
+
+        private static readonly Vector128<byte> CstHi = Vector128.Create(2, 3, 8, 9, 10, 11, 4, 5, 254, 255, 6, 7, 12, 13, 14, 15);
+
+        private static readonly Vector128<byte> Cst8 = Vector128.Create(254, 255, 254, 255, 254, 255, 0, 1, 254, 255, 254, 255, 254, 255, 254, 255);
+#endif
+
         // Diffusion weights. We under-correct a bit (15/16th of the error is actually
         // diffused) to avoid 'rainbow' chessboard pattern of blocks at q~=0.
         private const int C1 = 7;    // fraction of error sent to the 4x4 block below
@@ -298,14 +315,14 @@ public static int ReconstructIntra16(Vp8EncIterator it, Vp8SegmentInfo dqm, Vp8M
             }
 
             Vp8Encoding.FTransformWht(tmp, dcTmp, scratch);
-            nz |= QuantizeBlock(dcTmp, rd.YDcLevels, dqm.Y2) << 24;
+            nz |= QuantizeBlock(dcTmp, rd.YDcLevels, ref dqm.Y2) << 24;
 
             for (n = 0; n < 16; n += 2)
             {
                 // Zero-out the first coeff, so that: a) nz is correct below, and
                 // b) finding 'last' non-zero coeffs in SetResidualCoeffs() is simplified.
                 tmp[n * 16] = tmp[(n + 1) * 16] = 0;
-                nz |= Quantize2Blocks(tmp.Slice(n * 16, 32), rd.YAcLevels.AsSpan(n * 16, 32), dqm.Y1) << n;
+                nz |= Quantize2Blocks(tmp.Slice(n * 16, 32), rd.YAcLevels.AsSpan(n * 16, 32), ref dqm.Y1) << n;
             }
 
             // Transform back.
@@ -326,7 +343,7 @@ public static int ReconstructIntra4(Vp8EncIterator it, Vp8SegmentInfo dqm, Span<
             tmp.Clear();
             scratch.Clear();
             Vp8Encoding.FTransform(src, reference, tmp, scratch);
-            int nz = QuantizeBlock(tmp, levels, dqm.Y1);
+            int nz = QuantizeBlock(tmp, levels, ref dqm.Y1);
             Vp8Encoding.ITransform(reference, tmp, yuvOut, false, scratch);
 
             return nz;
@@ -353,11 +370,11 @@ public static int ReconstructUv(Vp8EncIterator it, Vp8SegmentInfo dqm, Vp8ModeSc
                     scratch);
             }
 
-            CorrectDcValues(it, dqm.Uv, tmp, rd);
+            CorrectDcValues(it, ref dqm.Uv, tmp, rd);
 
             for (n = 0; n < 8; n += 2)
             {
-                nz |= Quantize2Blocks(tmp.Slice(n * 16, 32), rd.UvLevels.AsSpan(n * 16, 32), dqm.Uv) << n;
+                nz |= Quantize2Blocks(tmp.Slice(n * 16, 32), rd.UvLevels.AsSpan(n * 16, 32), ref dqm.Uv) << n;
             }
 
             for (n = 0; n < 8; n += 2)
@@ -508,58 +525,155 @@ public static void RefineUsingDistortion(Vp8EncIterator it, Vp8SegmentInfo[] seg
         }
 
         [MethodImpl(InliningOptions.ShortMethod)]
-        public static int Quantize2Blocks(Span<short> input, Span<short> output, Vp8Matrix mtx)
+        public static int Quantize2Blocks(Span<short> input, Span<short> output, ref Vp8Matrix mtx)
         {
-            int nz = QuantizeBlock(input, output, mtx) << 0;
-            nz |= QuantizeBlock(input.Slice(1 * 16), output.Slice(1 * 16), mtx) << 1;
+            int nz = QuantizeBlock(input.Slice(0, 16), output.Slice(0, 16), ref mtx) << 0;
+            nz |= QuantizeBlock(input.Slice(1 * 16, 16), output.Slice(1 * 16, 16), ref mtx) << 1;
             return nz;
         }
 
-        public static int QuantizeBlock(Span<short> input, Span<short> output, Vp8Matrix mtx)
+        public static int QuantizeBlock(Span<short> input, Span<short> output, ref Vp8Matrix mtx)
         {
-            int last = -1;
-            int n;
-            for (n = 0; n < 16; ++n)
+#if SUPPORTS_RUNTIME_INTRINSICS
+            if (Sse41.IsSupported)
             {
-                int j = Zigzag[n];
-                bool sign = input[j] < 0;
-                uint coeff = (uint)((sign ? -input[j] : input[j]) + mtx.Sharpen[j]);
-                if (coeff > mtx.ZThresh[j])
+                // Load all inputs.
+                Vector128<short> input0 = Unsafe.As<short, Vector128<short>>(ref MemoryMarshal.GetReference(input));
+                Vector128<short> input8 = Unsafe.As<short, Vector128<short>>(ref MemoryMarshal.GetReference(input.Slice(8, 8)));
+                Vector128<ushort> iq0 = Unsafe.As<ushort, Vector128<ushort>>(ref mtx.IQ[0]);
+                Vector128<ushort> iq8 = Unsafe.As<ushort, Vector128<ushort>>(ref mtx.IQ[8]);
+                Vector128<ushort> q0 = Unsafe.As<ushort, Vector128<ushort>>(ref mtx.Q[0]);
+                Vector128<ushort> q8 = Unsafe.As<ushort, Vector128<ushort>>(ref mtx.Q[8]);
+
+                // coeff = abs(in)
+                Vector128<ushort> coeff0 = Ssse3.Abs(input0);
+                Vector128<ushort> coeff8 = Ssse3.Abs(input8);
+
+                // coeff = abs(in) + sharpen
+                Vector128<short> sharpen0 = Unsafe.As<short, Vector128<short>>(ref mtx.Sharpen[0]);
+                Vector128<short> sharpen8 = Unsafe.As<short, Vector128<short>>(ref mtx.Sharpen[8]);
+                Sse2.Add(coeff0.AsInt16(), sharpen0);
+                Sse2.Add(coeff8.AsInt16(), sharpen8);
+
+                // out = (coeff * iQ + B) >> QFIX
+                // doing calculations with 32b precision (QFIX=17)
+                // out = (coeff * iQ)
+                Vector128<ushort> coeffiQ0H = Sse2.MultiplyHigh(coeff0, iq0);
+                Vector128<ushort> coeffiQ0L = Sse2.MultiplyLow(coeff0, iq0);
+                Vector128<ushort> coeffiQ8H = Sse2.MultiplyHigh(coeff8, iq8);
+                Vector128<ushort> coeffiQ8L = Sse2.MultiplyLow(coeff8, iq8);
+                Vector128<ushort> out00 = Sse2.UnpackLow(coeffiQ0L, coeffiQ0H);
+                Vector128<ushort> out04 = Sse2.UnpackHigh(coeffiQ0L, coeffiQ0H);
+                Vector128<ushort> out08 = Sse2.UnpackLow(coeffiQ8L, coeffiQ8H);
+                Vector128<ushort> out12 = Sse2.UnpackHigh(coeffiQ8L, coeffiQ8H);
+
+                // out = (coeff * iQ + B)
+                Vector128<uint> bias00 = Unsafe.As<uint, Vector128<uint>>(ref mtx.Bias[0]);
+                Vector128<uint> bias04 = Unsafe.As<uint, Vector128<uint>>(ref mtx.Bias[4]);
+                Vector128<uint> bias08 = Unsafe.As<uint, Vector128<uint>>(ref mtx.Bias[8]);
+                Vector128<uint> bias12 = Unsafe.As<uint, Vector128<uint>>(ref mtx.Bias[12]);
+                out00 = Sse2.Add(out00.AsInt32(), bias00.AsInt32()).AsUInt16();
+                out04 = Sse2.Add(out04.AsInt32(), bias04.AsInt32()).AsUInt16();
+                out08 = Sse2.Add(out08.AsInt32(), bias08.AsInt32()).AsUInt16();
+                out12 = Sse2.Add(out12.AsInt32(), bias12.AsInt32()).AsUInt16();
+
+                // out = QUANTDIV(coeff, iQ, B, QFIX)
+                out00 = Sse2.ShiftRightArithmetic(out00.AsInt32(), WebpConstants.QFix).AsUInt16();
+                out04 = Sse2.ShiftRightArithmetic(out04.AsInt32(), WebpConstants.QFix).AsUInt16();
+                out08 = Sse2.ShiftRightArithmetic(out08.AsInt32(), WebpConstants.QFix).AsUInt16();
+                out12 = Sse2.ShiftRightArithmetic(out12.AsInt32(), WebpConstants.QFix).AsUInt16();
+
+                // pack result as 16b
+                Vector128<short> out0 = Sse2.PackSignedSaturate(out00.AsInt32(), out04.AsInt32());
+                Vector128<short> out8 = Sse2.PackSignedSaturate(out08.AsInt32(), out12.AsInt32());
+
+                // if (coeff > 2047) coeff = 2047
+                out0 = Sse2.Min(out0, MaxCoeff2047);
+                out8 = Sse2.Min(out8, MaxCoeff2047);
+
+                // put sign back
+                out0 = Ssse3.Sign(out0, input0);
+                out8 = Ssse3.Sign(out8, input8);
+
+                // in = out * Q
+                input0 = Sse2.MultiplyLow(out0, q0.AsInt16());
+                input8 = Sse2.MultiplyLow(out8, q8.AsInt16());
+
+                // in = out * Q
+                ref short inputRef = ref MemoryMarshal.GetReference(input);
+                Unsafe.As<short, Vector128<short>>(ref inputRef) = input0;
+                Unsafe.As<short, Vector128<short>>(ref Unsafe.Add(ref inputRef, 8)) = input8;
+
+                // zigzag the output before storing it. The re-ordering is:
+                //    0 1 2 3 4 5 6 7 | 8  9 10 11 12 13 14 15
+                // -> 0 1 4[8]5 2 3 6 | 9 12 13 10 [7]11 14 15
+                // There's only two misplaced entries ([8] and [7]) that are crossing the
+                // reg's boundaries.
+                // We use pshufb instead of pshuflo/pshufhi.
+                Vector128<byte> tmpLo = Ssse3.Shuffle(out0.AsByte(), CstLo);
+                Vector128<byte> tmp7 = Ssse3.Shuffle(out0.AsByte(), Cst7);  // extract #7
+                Vector128<byte> tmpHi = Ssse3.Shuffle(out8.AsByte(), CstHi);
+                Vector128<byte> tmp8 = Ssse3.Shuffle(out8.AsByte(), Cst8);  // extract #8
+                Vector128<byte> outZ0 = Sse2.Or(tmpLo, tmp8);
+                Vector128<byte> outZ8 = Sse2.Or(tmpHi, tmp7);
+
+                ref short outputRef = ref MemoryMarshal.GetReference(output);
+                Unsafe.As<short, Vector128<short>>(ref outputRef) = outZ0.AsInt16();
+                Unsafe.As<short, Vector128<short>>(ref Unsafe.Add(ref outputRef, 8)) = outZ8.AsInt16();
+
+                Vector128<sbyte> packedOutput = Sse2.PackSignedSaturate(outZ0.AsInt16(), outZ8.AsInt16());
+
+                // Detect if all 'out' values are zeros or not.
+                Vector128<sbyte> cmpeq = Sse2.CompareEqual(packedOutput, Vector128<sbyte>.Zero);
+                return Sse2.MoveMask(cmpeq) != 0xffff ? 1 : 0;
+            }
+            else
+#endif
+            {
+                int last = -1;
+                int n;
+                for (n = 0; n < 16; ++n)
                 {
-                    uint q = mtx.Q[j];
-                    uint iQ = mtx.IQ[j];
-                    uint b = mtx.Bias[j];
-                    int level = QuantDiv(coeff, iQ, b);
-                    if (level > MaxLevel)
+                    int j = Zigzag[n];
+                    bool sign = input[j] < 0;
+                    uint coeff = (uint)((sign ? -input[j] : input[j]) + mtx.Sharpen[j]);
+                    if (coeff > mtx.ZThresh[j])
                     {
-                        level = MaxLevel;
-                    }
+                        uint q = mtx.Q[j];
+                        uint iQ = mtx.IQ[j];
+                        uint b = mtx.Bias[j];
+                        int level = QuantDiv(coeff, iQ, b);
+                        if (level > MaxLevel)
+                        {
+                            level = MaxLevel;
+                        }
 
-                    if (sign)
-                    {
-                        level = -level;
-                    }
+                        if (sign)
+                        {
+                            level = -level;
+                        }
 
-                    input[j] = (short)(level * (int)q);
-                    output[n] = (short)level;
-                    if (level != 0)
+                        input[j] = (short)(level * (int)q);
+                        output[n] = (short)level;
+                        if (level != 0)
+                        {
+                            last = n;
+                        }
+                    }
+                    else
                     {
-                        last = n;
+                        output[n] = 0;
+                        input[j] = 0;
                     }
                 }
-                else
-                {
-                    output[n] = 0;
-                    input[j] = 0;
-                }
-            }
 
-            return last >= 0 ? 1 : 0;
+                return last >= 0 ? 1 : 0;
+            }
         }
 
         // Quantize as usual, but also compute and return the quantization error.
         // Error is already divided by DSHIFT.
-        public static int QuantizeSingle(Span<short> v, Vp8Matrix mtx)
+        public static int QuantizeSingle(Span<short> v, ref Vp8Matrix mtx)
         {
             int v0 = v[0];
             bool sign = v0 < 0;
@@ -580,7 +694,7 @@ public static int QuantizeSingle(Span<short> v, Vp8Matrix mtx)
             return (sign ? -v0 : v0) >> DSCALE;
         }
 
-        public static void CorrectDcValues(Vp8EncIterator it, Vp8Matrix mtx, Span<short> tmp, Vp8ModeScore rd)
+        public static void CorrectDcValues(Vp8EncIterator it, ref Vp8Matrix mtx, Span<short> tmp, Vp8ModeScore rd)
         {
 #pragma warning disable SA1005 // Single line comments should begin with single space
             //         | top[0] | top[1]
@@ -597,13 +711,13 @@ public static void CorrectDcValues(Vp8EncIterator it, Vp8Matrix mtx, Span<short>
                 Span<sbyte> left = it.LeftDerr.AsSpan(ch, 2);
                 Span<short> c = tmp.Slice(ch * 4 * 16, 4 * 16);
                 c[0] += (short)(((C1 * top[0]) + (C2 * left[0])) >> (DSHIFT - DSCALE));
-                int err0 = QuantizeSingle(c, mtx);
+                int err0 = QuantizeSingle(c, ref mtx);
                 c[1 * 16] += (short)(((C1 * top[1]) + (C2 * err0)) >> (DSHIFT - DSCALE));
-                int err1 = QuantizeSingle(c.Slice(1 * 16), mtx);
+                int err1 = QuantizeSingle(c.Slice(1 * 16), ref mtx);
                 c[2 * 16] += (short)(((C1 * err0) + (C2 * left[1])) >> (DSHIFT - DSCALE));
-                int err2 = QuantizeSingle(c.Slice(2 * 16), mtx);
+                int err2 = QuantizeSingle(c.Slice(2 * 16), ref mtx);
                 c[3 * 16] += (short)(((C1 * err1) + (C2 * err2)) >> (DSHIFT - DSCALE));
-                int err3 = QuantizeSingle(c.Slice(3 * 16), mtx);
+                int err3 = QuantizeSingle(c.Slice(3 * 16), ref mtx);
 
                 rd.Derr[ch, 0] = err1;
                 rd.Derr[ch, 1] = err2;

src/ImageSharp/Formats/Webp/Lossy/Vp8Encoder.cs

@@ -502,7 +502,7 @@ private void SetLoopParams(float q)
             this.ResetStats();
         }
 
-        private void AdjustFilterStrength()
+        private unsafe void AdjustFilterStrength()
         {
             if (this.filterStrength > 0)
             {
@@ -806,18 +806,14 @@ private void ResetStats()
             proba.NbSkip = 0;
         }
 
-        private void SetupMatrices(Vp8SegmentInfo[] dqm)
+        private unsafe void SetupMatrices(Vp8SegmentInfo[] dqm)
         {
             int tlambdaScale = this.method >= WebpEncodingMethod.Default ? this.spatialNoiseShaping : 0;
             for (int i = 0; i < dqm.Length; i++)
             {
                 Vp8SegmentInfo m = dqm[i];
                 int q = m.Quant;
 
-                m.Y1 = new Vp8Matrix();
-                m.Y2 = new Vp8Matrix();
-                m.Uv = new Vp8Matrix();
-
                 m.Y1.Q[0] = WebpLookupTables.DcTable[Numerics.Clamp(q + this.DqY1Dc, 0, 127)];
                 m.Y1.Q[1] = WebpLookupTables.AcTable[Numerics.Clamp(q, 0, 127)];