From 313c0a1c2725be0e339238ae8e6ca47464ec33b5 Mon Sep 17 00:00:00 2001
From: zeromus <zeromus@zeromus.org>
Date: Thu, 25 Dec 2014 19:54:37 +0000
Subject: [PATCH] disc - add subcode interleaving/deinterleaving utilities

---
 BizHawk.Emulation.DiscSystem/Subcode.cs | 38 +++++++++++++++++++++++++
 1 file changed, 38 insertions(+)

diff --git a/BizHawk.Emulation.DiscSystem/Subcode.cs b/BizHawk.Emulation.DiscSystem/Subcode.cs
index 5738902f54..7488b40da8 100644
--- a/BizHawk.Emulation.DiscSystem/Subcode.cs
+++ b/BizHawk.Emulation.DiscSystem/Subcode.cs
@@ -18,6 +18,44 @@ namespace BizHawk.Emulation.DiscSystem
 		void ReadSubcodeChannel(int number, byte[] buffer, int offset);
 	}
 
+	public static class SubcodeUtils
+	{
+		public static void Interleave(byte[] in_buf, int in_buf_index, byte[] out_buf, int out_buf_index)
+		{
+			for (int d = 0; d < 12; d++)
+			{
+				for (int bitpoodle = 0; bitpoodle < 8; bitpoodle++)
+				{
+					int rawb = 0;
+
+					for (int ch = 0; ch < 8; ch++)
+					{
+						rawb |= ((in_buf[ch * 12 + d + in_buf_index] >> (7 - bitpoodle)) & 1) << (7 - ch);
+					}
+					out_buf[(d << 3) + bitpoodle + out_buf_index] = (byte)rawb;
+				}
+			}
+		}
+
+		public static void Deinterleave(byte[] in_buf, int in_buf_index, byte[] out_buf, int out_buf_index)
+		{
+			for (int i = 0; i < 96; i++)
+				out_buf[i] = 0;
+
+			for (int ch = 0; ch < 8; ch++)
+			{
+				for (int i = 0; i < 96; i++)
+				{
+					out_buf[(ch * 12) + (i >> 3) + out_buf_index] |= (byte)(((in_buf[i + in_buf_index] >> (7 - ch)) & 0x1) << (7 - (i & 0x7)));
+				}
+			}
+
+		}
+	}
+
+
+
+
 	/// <summary>
 	/// Reads subcode from an internally-managed buffer
 	/// </summary>