CopyFrom to MemoryCopy

Author: benaadams

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

@@ -955,7 +955,7 @@ public void CopyFrom(ArraySegment<byte> buffer)
 
         public void CopyFrom(byte[] data, int offset, int count)
         {
-            if (IsDefault)
+            if (count == 0 || IsDefault)
             {
                 return;
             }
@@ -964,6 +964,19 @@ public void CopyFrom(byte[] data, int offset, int count)
             Debug.Assert(_block.Next == null);
             Debug.Assert(_block.End == _index);
 
+            Debug.Assert(count + offset <= data.Length);
+
+#if NET451
+            BlockCopyFrom(data, offset, count);
+#else
+            MemoryCopyFrom(data, offset, count);
+#endif
+        }
+
+#if NET451
+        private void BlockCopyFrom(byte[] data, int offset, int count)
+        {
+            // Note: Ensure for any changes in this function MemoryCopyFrom is changed to match
             var pool = _block.Pool;
             var block = _block;
             var blockIndex = _index;
@@ -999,6 +1012,49 @@ public void CopyFrom(byte[] data, int offset, int count)
             _block = block;
             _index = blockIndex;
         }
+#else
+        private unsafe void MemoryCopyFrom(byte[] data, int offset, int count)
+        {
+            // Note: Ensure for any changes in this function BlockCopyFrom is changed to match
+            var pool = _block.Pool;
+            var block = _block;
+            var blockIndex = _index;
+
+            var bufferIndex = offset;
+            var remaining = count;
+            var bytesLeftInBlock = block.Data.Offset + block.Data.Count - blockIndex;
+
+            fixed (byte* pData = &data[0])
+            {
+                while (remaining > 0)
+                {
+                    if (bytesLeftInBlock == 0)
+                    {
+                        var nextBlock = pool.Lease();
+                        block.End = blockIndex;
+                        Volatile.Write(ref block.Next, nextBlock);
+                        block = nextBlock;
+
+                        blockIndex = block.Data.Offset;
+                        bytesLeftInBlock = block.Data.Count;
+                    }
+
+                    var bytesToCopy = remaining < bytesLeftInBlock ? remaining : bytesLeftInBlock;
+
+                    Buffer.MemoryCopy(pData + bufferIndex, block.DataFixedPtr + blockIndex, bytesLeftInBlock, bytesToCopy);
+
+                    blockIndex += bytesToCopy;
+                    bufferIndex += bytesToCopy;
+                    remaining -= bytesToCopy;
+                    bytesLeftInBlock -= bytesToCopy;
+                }
+            }
+
+            block.End = blockIndex;
+            _block = block;
+            _index = blockIndex;
+        }
+#endif
 
         public unsafe void CopyFromAscii(string data)
         {