PNG: Add SSE/AVX version of Sub, Up, Average and Paethfilters

src/ImageSharp/Formats/Png/Filters/AverageFilter.cs

@@ -20,21 +20,76 @@ namespace SixLabors.ImageSharp.Formats.Png.Filters
     internal static class AverageFilter
     {
         /// <summary>
-        /// Decodes the scanline
+        /// Decodes a scanline, which was filtered with the average filter.
         /// </summary>
-        /// <param name="scanline">The scanline to decode</param>
+        /// <param name="scanline">The scanline to decode.</param>
         /// <param name="previousScanline">The previous scanline.</param>
         /// <param name="bytesPerPixel">The bytes per pixel.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Decode(Span<byte> scanline, Span<byte> previousScanline, int bytesPerPixel)
         {
             DebugGuard.MustBeSameSized<byte>(scanline, previousScanline, nameof(scanline));
 
+            // The Avg filter predicts each pixel as the (truncated) average of a and b:
+            // Average(x) + floor((Raw(x-bpp)+Prior(x))/2)
+            // With pixels positioned like this:
+            //  prev:  c b
+            //  row:   a d
+#if SUPPORTS_RUNTIME_INTRINSICS
+            if (Sse2.IsSupported && bytesPerPixel is 4)
+            {
+                DecodeSse2(scanline, previousScanline);
+            }
+            else
+#endif
+            {
+                DecodeScalar(scanline, previousScanline, bytesPerPixel);
+            }
+        }
+
+#if SUPPORTS_RUNTIME_INTRINSICS
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void DecodeSse2(Span<byte> scanline, Span<byte> previousScanline)
+        {
             ref byte scanBaseRef = ref MemoryMarshal.GetReference(scanline);
             ref byte prevBaseRef = ref MemoryMarshal.GetReference(previousScanline);
 
-            // Average(x) + floor((Raw(x-bpp)+Prior(x))/2)
-            int x = 1;
+            Vector128<byte> d = Vector128<byte>.Zero;
+            var ones = Vector128.Create((byte)1);
+
+            int rb = scanline.Length;
+            nint offset = 1;
+            while (rb >= 4)
+            {
+                ref byte scanRef = ref Unsafe.Add(ref scanBaseRef, offset);
+                Vector128<byte> a = d;
+                Vector128<byte> b = Sse2.ConvertScalarToVector128Int32(Unsafe.As<byte, int>(ref Unsafe.Add(ref prevBaseRef, offset))).AsByte();
+                d = Sse2.ConvertScalarToVector128Int32(Unsafe.As<byte, int>(ref scanRef)).AsByte();
+
+                // PNG requires a truncating average, so we can't just use _mm_avg_epu8,
+                // but we can fix it up by subtracting off 1 if it rounded up.
+                Vector128<byte> avg = Sse2.Average(a, b);
+                Vector128<byte> xor = Sse2.Xor(a, b);
+                Vector128<byte> and = Sse2.And(xor, ones);
+                avg = Sse2.Subtract(avg, and);
+                d = Sse2.Add(d, avg);
+
+                // Store the result.
+                Unsafe.As<byte, int>(ref scanRef) = Sse2.ConvertToInt32(d.AsInt32());
+
+                rb -= 4;
+                offset += 4;
+            }
+        }
+#endif
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void DecodeScalar(Span<byte> scanline, Span<byte> previousScanline, int bytesPerPixel)
+        {
+            ref byte scanBaseRef = ref MemoryMarshal.GetReference(scanline);
+            ref byte prevBaseRef = ref MemoryMarshal.GetReference(previousScanline);
+
+            nint x = 1;
             for (; x <= bytesPerPixel /* Note the <= because x starts at 1 */; ++x)
             {
                 ref byte scan = ref Unsafe.Add(ref scanBaseRef, x);
@@ -52,13 +107,13 @@ public static void Decode(Span<byte> scanline, Span<byte> previousScanline, int
         }
 
         /// <summary>
-        /// Encodes the scanline
+        /// Encodes a scanline with the average filter applied.
         /// </summary>
-        /// <param name="scanline">The scanline to encode</param>
+        /// <param name="scanline">The scanline to encode.</param>
         /// <param name="previousScanline">The previous scanline.</param>
         /// <param name="result">The filtered scanline result.</param>
         /// <param name="bytesPerPixel">The bytes per pixel.</param>
-        /// <param name="sum">The sum of the total variance of the filtered row</param>
+        /// <param name="sum">The sum of the total variance of the filtered row.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Encode(ReadOnlySpan<byte> scanline, ReadOnlySpan<byte> previousScanline, Span<byte> result, int bytesPerPixel, out int sum)
         {

src/ImageSharp/Formats/Png/Filters/PaethFilter.cs

@@ -22,16 +22,96 @@ namespace SixLabors.ImageSharp.Formats.Png.Filters
     internal static class PaethFilter
     {
         /// <summary>
-        /// Decodes the scanline
+        /// Decodes a scanline, which was filtered with the paeth filter.
         /// </summary>
-        /// <param name="scanline">The scanline to decode</param>
+        /// <param name="scanline">The scanline to decode.</param>
         /// <param name="previousScanline">The previous scanline.</param>
         /// <param name="bytesPerPixel">The bytes per pixel.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Decode(Span<byte> scanline, Span<byte> previousScanline, int bytesPerPixel)
         {
             DebugGuard.MustBeSameSized<byte>(scanline, previousScanline, nameof(scanline));
 
+            // Paeth tries to predict pixel d using the pixel to the left of it, a,
+            // and two pixels from the previous row, b and c:
+            // prev: c b
+            // row:  a d
+            // The Paeth function predicts d to be whichever of a, b, or c is nearest to
+            // p = a + b - c.
+#if SUPPORTS_RUNTIME_INTRINSICS
+            if (Sse41.IsSupported && bytesPerPixel is 4)
+            {
+                DecodeSse41(scanline, previousScanline);
+            }
+            else
+#endif
+            {
+                DecodeScalar(scanline, previousScanline, bytesPerPixel);
+            }
+        }
+
+#if SUPPORTS_RUNTIME_INTRINSICS
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void DecodeSse41(Span<byte> scanline, Span<byte> previousScanline)
+        {
+            ref byte scanBaseRef = ref MemoryMarshal.GetReference(scanline);
+            ref byte prevBaseRef = ref MemoryMarshal.GetReference(previousScanline);
+
+            Vector128<byte> b = Vector128<byte>.Zero;
+            Vector128<byte> d = Vector128<byte>.Zero;
+
+            int rb = scanline.Length;
+            nint offset = 1;
+            while (rb >= 4)
+            {
+                ref byte scanRef = ref Unsafe.Add(ref scanBaseRef, offset);
+
+                // It's easiest to do this math (particularly, deal with pc) with 16-bit intermediates.
+                Vector128<byte> c = b;
+                Vector128<byte> a = d;
+                b = Sse2.UnpackLow(
+                    Sse2.ConvertScalarToVector128Int32(Unsafe.As<byte, int>(ref Unsafe.Add(ref prevBaseRef, offset))).AsByte(),
+                    Vector128<byte>.Zero);
+                d = Sse2.UnpackLow(
+                    Sse2.ConvertScalarToVector128Int32(Unsafe.As<byte, int>(ref scanRef)).AsByte(),
+                    Vector128<byte>.Zero);
+
+                // (p-a) == (a+b-c - a) == (b-c)
+                Vector128<short> pa = Sse2.Subtract(b.AsInt16(), c.AsInt16());
+
+                // (p-b) == (a+b-c - b) == (a-c)
+                Vector128<short> pb = Sse2.Subtract(a.AsInt16(), c.AsInt16());
+
+                // (p-c) == (a+b-c - c) == (a+b-c-c) == (b-c)+(a-c)
+                Vector128<short> pc = Sse2.Add(pa.AsInt16(), pb.AsInt16());
+
+                pa = Ssse3.Abs(pa.AsInt16()).AsInt16(); /* |p-a| */
+                pb = Ssse3.Abs(pb.AsInt16()).AsInt16(); /* |p-b| */
+                pc = Ssse3.Abs(pc.AsInt16()).AsInt16(); /* |p-c| */
+
+                Vector128<short> smallest = Sse2.Min(pc, Sse2.Min(pa, pb));
+
+                // Paeth breaks ties favoring a over b over c.
+                Vector128<byte> mask = Sse41.BlendVariable(c, b, Sse2.CompareEqual(smallest, pb).AsByte());
+                Vector128<byte> nearest = Sse41.BlendVariable(mask, a, Sse2.CompareEqual(smallest, pa).AsByte());
+
+                // Note `_epi8`: we need addition to wrap modulo 255.
+                d = Sse2.Add(d, nearest);
+
+                // Store the result.
+                Unsafe.As<byte, int>(ref scanRef) = Sse2.ConvertToInt32(Sse2.PackUnsignedSaturate(d.AsInt16(), d.AsInt16()).AsInt32());
+
+                rb -= 4;
+                offset += 4;
+            }
+        }
+
+#endif
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void DecodeScalar(Span<byte> scanline, Span<byte> previousScanline, int bytesPerPixel)
+        {
             ref byte scanBaseRef = ref MemoryMarshal.GetReference(scanline);
             ref byte prevBaseRef = ref MemoryMarshal.GetReference(previousScanline);
 
@@ -56,13 +136,13 @@ public static void Decode(Span<byte> scanline, Span<byte> previousScanline, int
         }
 
         /// <summary>
-        /// Encodes the scanline
+        /// Encodes a scanline and applies the paeth filter.
         /// </summary>
         /// <param name="scanline">The scanline to encode</param>
         /// <param name="previousScanline">The previous scanline.</param>
         /// <param name="result">The filtered scanline result.</param>
         /// <param name="bytesPerPixel">The bytes per pixel.</param>
-        /// <param name="sum">The sum of the total variance of the filtered row</param>
+        /// <param name="sum">The sum of the total variance of the filtered row.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Encode(ReadOnlySpan<byte> scanline, ReadOnlySpan<byte> previousScanline, Span<byte> result, int bytesPerPixel, out int sum)
         {

src/ImageSharp/Formats/Png/Filters/SubFilter.cs

@@ -21,17 +21,57 @@ namespace SixLabors.ImageSharp.Formats.Png.Filters
     internal static class SubFilter
     {
         /// <summary>
-        /// Decodes the scanline
+        /// Decodes a scanline, which was filtered with the sub filter.
         /// </summary>
-        /// <param name="scanline">The scanline to decode</param>
+        /// <param name="scanline">The scanline to decode.</param>
         /// <param name="bytesPerPixel">The bytes per pixel.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Decode(Span<byte> scanline, int bytesPerPixel)
+        {
+            // The Sub filter predicts each pixel as the previous pixel.
+#if SUPPORTS_RUNTIME_INTRINSICS
+            if (Sse2.IsSupported && bytesPerPixel is 4)
+            {
+                DecodeSse2(scanline);
+            }
+            else
+#endif
+            {
+                DecodeScalar(scanline, bytesPerPixel);
+            }
+        }
+
+#if SUPPORTS_RUNTIME_INTRINSICS
+        private static void DecodeSse2(Span<byte> scanline)
+        {
+            ref byte scanBaseRef = ref MemoryMarshal.GetReference(scanline);
+
+            Vector128<byte> d = Vector128<byte>.Zero;
+
+            int rb = scanline.Length;
+            nint offset = 1;
+            while (rb >= 4)
+            {
+                ref byte scanRef = ref Unsafe.Add(ref scanBaseRef, offset);
+                Vector128<byte> a = d;
+                d = Sse2.ConvertScalarToVector128Int32(Unsafe.As<byte, int>(ref scanRef)).AsByte();
+
+                d = Sse2.Add(d, a);
+
+                Unsafe.As<byte, int>(ref scanRef) = Sse2.ConvertToInt32(d.AsInt32());
+
+                rb -= 4;
+                offset += 4;
+            }
+        }
+#endif
+
+        private static void DecodeScalar(Span<byte> scanline, int bytesPerPixel)
         {
             ref byte scanBaseRef = ref MemoryMarshal.GetReference(scanline);
 
             // Sub(x) + Raw(x-bpp)
-            int x = bytesPerPixel + 1;
+            nint x = bytesPerPixel + 1;
             Unsafe.Add(ref scanBaseRef, x);
             for (; x < scanline.Length; ++x)
             {
@@ -42,12 +82,12 @@ public static void Decode(Span<byte> scanline, int bytesPerPixel)
         }
 
         /// <summary>
-        /// Encodes the scanline
+        /// Encodes a scanline with the sup filter applied.
         /// </summary>
-        /// <param name="scanline">The scanline to encode</param>
+        /// <param name="scanline">The scanline to encode.</param>
         /// <param name="result">The filtered scanline result.</param>
         /// <param name="bytesPerPixel">The bytes per pixel.</param>
-        /// <param name="sum">The sum of the total variance of the filtered row</param>
+        /// <param name="sum">The sum of the total variance of the filtered row.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public static void Encode(ReadOnlySpan<byte> scanline, ReadOnlySpan<byte> result, int bytesPerPixel, out int sum)
         {