From a8f2e8fe358473801c36d00aface596df2c303e9 Mon Sep 17 00:00:00 2001
From: Anton Firszov <anton.firszov@sapagroup.com>
Date: Tue, 12 Sep 2017 19:34:23 +0200
Subject: [PATCH] Optimized Block8x8F.CopyTo() for the 4:2:0 case

---
 .../Formats/Jpeg/Common/Block8x8F.cs          | 67 ++++++++++++++++---
 src/ImageSharp/Memory/BufferArea{T}.cs        |  1 +
 2 files changed, 59 insertions(+), 9 deletions(-)

diff --git a/src/ImageSharp/Formats/Jpeg/Common/Block8x8F.cs b/src/ImageSharp/Formats/Jpeg/Common/Block8x8F.cs
index 4d0ec3393..474c75adf 100644
--- a/src/ImageSharp/Formats/Jpeg/Common/Block8x8F.cs
+++ b/src/ImageSharp/Formats/Jpeg/Common/Block8x8F.cs
@@ -305,14 +305,7 @@ namespace SixLabors.ImageSharp.Formats.Jpeg.Common
             }
         }
 
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        private static void CopyRowImpl(ref byte selfBase, ref byte destBase, int destStride, int row)
-        {
-            ref byte s = ref Unsafe.Add(ref selfBase, row * 8 * sizeof(float));
-            ref byte d = ref Unsafe.Add(ref destBase, row * destStride);
-            Unsafe.CopyBlock(ref d, ref s, 8 * sizeof(float));
-        }
-
+        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public void CopyTo(BufferArea<float> area)
         {
             ref byte selfBase = ref Unsafe.As<Block8x8F, byte>(ref this);
@@ -336,17 +329,23 @@ namespace SixLabors.ImageSharp.Formats.Jpeg.Common
                 this.CopyTo(area);
                 return;
             }
+            else if (horizontalScale == 2 && verticalScale == 2)
+            {
+                this.CopyTo2x2(area);
+                return;
+            }
 
             // TODO: Optimize: implement all the cases with loopless special code! (T4?)
             for (int y = 0; y < 8; y++)
             {
                 int yy = y * verticalScale;
+                int y8 = y * 8;
 
                 for (int x = 0; x < 8; x++)
                 {
                     int xx = x * horizontalScale;
 
-                    float value = this[(y * 8) + x];
+                    float value = this[y8 + x];
 
                     for (int i = 0; i < verticalScale; i++)
                     {
@@ -359,6 +358,56 @@ namespace SixLabors.ImageSharp.Formats.Jpeg.Common
             }
         }
 
+        private void CopyTo2x2(BufferArea<float> area)
+        {
+            ref float destBase = ref area.GetReferenceToOrigo();
+            int destStride = area.Stride;
+
+            this.CopyRow2x2Impl(ref destBase, 0, destStride);
+            this.CopyRow2x2Impl(ref destBase, 1, destStride);
+            this.CopyRow2x2Impl(ref destBase, 2, destStride);
+            this.CopyRow2x2Impl(ref destBase, 3, destStride);
+            this.CopyRow2x2Impl(ref destBase, 4, destStride);
+            this.CopyRow2x2Impl(ref destBase, 5, destStride);
+            this.CopyRow2x2Impl(ref destBase, 6, destStride);
+            this.CopyRow2x2Impl(ref destBase, 7, destStride);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void CopyRowImpl(ref byte selfBase, ref byte destBase, int destStride, int row)
+        {
+            ref byte s = ref Unsafe.Add(ref selfBase, row * 8 * sizeof(float));
+            ref byte d = ref Unsafe.Add(ref destBase, row * destStride);
+            Unsafe.CopyBlock(ref d, ref s, 8 * sizeof(float));
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private void CopyRow2x2Impl(ref float destBase, int row, int destStride)
+        {
+            ref Vector4 selfLeft = ref Unsafe.Add(ref this.V0L, 2 * row);
+            ref Vector4 selfRight = ref Unsafe.Add(ref selfLeft, 1);
+            ref float destLocalOrigo = ref Unsafe.Add(ref destBase, row * 2 * destStride);
+
+            Stride2VectorCopyImpl(ref selfLeft, ref destLocalOrigo);
+            Stride2VectorCopyImpl(ref selfRight, ref Unsafe.Add(ref destLocalOrigo, 8));
+
+            Stride2VectorCopyImpl(ref selfLeft, ref Unsafe.Add(ref destLocalOrigo, destStride));
+            Stride2VectorCopyImpl(ref selfRight, ref Unsafe.Add(ref destLocalOrigo, destStride + 8));
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        private static void Stride2VectorCopyImpl(ref Vector4 s, ref float destBase)
+        {
+            Unsafe.Add(ref destBase, 0) = s.X;
+            Unsafe.Add(ref destBase, 1) = s.X;
+            Unsafe.Add(ref destBase, 2) = s.Y;
+            Unsafe.Add(ref destBase, 3) = s.Y;
+            Unsafe.Add(ref destBase, 4) = s.Z;
+            Unsafe.Add(ref destBase, 5) = s.Z;
+            Unsafe.Add(ref destBase, 6) = s.W;
+            Unsafe.Add(ref destBase, 7) = s.W;
+        }
+
         public float[] ToArray()
         {
             float[] result = new float[Size];
diff --git a/src/ImageSharp/Memory/BufferArea{T}.cs b/src/ImageSharp/Memory/BufferArea{T}.cs
index 92e78e9c0..8ead22680 100644
--- a/src/ImageSharp/Memory/BufferArea{T}.cs
+++ b/src/ImageSharp/Memory/BufferArea{T}.cs
@@ -60,6 +60,7 @@ namespace SixLabors.ImageSharp.Memory
         /// Gets a reference to the [0,0] element.
         /// </summary>
         /// <returns>The reference to the [0,0] element</returns>
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref T GetReferenceToOrigo() =>
             ref this.DestinationBuffer.Span[(this.Rectangle.Y * this.DestinationBuffer.Width) + this.Rectangle.X];