CopyTo to Buffer.MemoryCopy

Author: benaadams

src/Microsoft.AspNetCore.Server.Kestrel/Internal/Infrastructure/MemoryPoolIterator.cs

@@ -870,17 +870,30 @@ public int GetLength(MemoryPoolIterator end)
 
         public MemoryPoolIterator CopyTo(byte[] array, int offset, int count, out int actual)
         {
-            // Note: Ensure for any changes in this function Consume is changed to match
             if (count == 0 || IsDefault)
             {
                 actual = 0;
                 return this;
             }
+
             if (array == null)
             {
                 return Consume(count, out actual);
             }
 
+            Debug.Assert(count + offset <= array.Length);
+
+#if NET451
+            return BlockCopyTo(array, ref offset, count, out actual);
+#else
+            return MemoryCopyTo(array, ref offset, count, out actual);
+#endif
+        }
+
+#if NET451
+        private MemoryPoolIterator BlockCopyTo(byte[] array, ref int offset, int count, out int actual)
+        {
+            // Note: Ensure for any changes in this function MemoryCopyTo & Consume are changed to match
             var block = _block;
             var index = _index;
             var remaining = count;
@@ -914,9 +927,51 @@ public MemoryPoolIterator CopyTo(byte[] array, int offset, int count, out int ac
                 }
             }
         }
+#else
+        private unsafe MemoryPoolIterator MemoryCopyTo(byte[] array, ref int offset, int count, out int actual)
+        {
+            // Note: Ensure for any changes in this function BlockCopyTo & Consume are changed to match
+            var block = _block;
+            var index = _index;
+            var remaining = count;
+            fixed (byte* pArray = &array[0])
+            {
+                while (true)
+                {
+                    // Determine if we might attempt to copy data from block.Next before
+                    // calculating "following" so we don't risk skipping data that could
+                    // be added after block.End when we decide to copy from block.Next.
+                    // block.End will always be advanced before block.Next is set.
+                    var wasLastBlock = block.Next == null;
+                    var following = block.End - index;
+                    if (remaining <= following)
+                    {
+                        actual = count;
+                        Buffer.MemoryCopy(block.DataFixedPtr + index, pArray + offset, remaining, remaining);
+                        return new MemoryPoolIterator(block, index + remaining);
+                    }
+                    else if (wasLastBlock)
+                    {
+                        actual = count - remaining + following;
+                        Buffer.MemoryCopy(block.DataFixedPtr + index, pArray + offset, following, following);
+                        return new MemoryPoolIterator(block, index + following);
+                    }
+                    else
+                    {
+                        Buffer.MemoryCopy(block.DataFixedPtr + index, pArray + offset, following, following);
+                        offset += following;
+                        remaining -= following;
+                        block = block.Next;
+                        index = block.Start;
+                    }
+                }
+            }
+        }
+#endif
 
         private MemoryPoolIterator Consume(int count, out int actual)
         {
+            // Note: Ensure for any changes in this function CopyToBlockCopy & CopyToMemoryCopy are changed to match
             var block = _block;
             var index = _index;
             var remaining = count;