From f2def57b90c8462f1fa2dc0f39efbe8d05f451b0 Mon Sep 17 00:00:00 2001
From: zeromus <zeromus@users.noreply.github.com>
Date: Tue, 23 Jun 2015 19:37:22 -0500
Subject: [PATCH] add new subchannel interleaving code and fix errors in CCD
 subcode loading

---
 BizHawk.Emulation.DiscSystem/CCD_format.cs |  8 +++----
 BizHawk.Emulation.DiscSystem/Subcode.cs    | 28 ++++++++++++++++++++++
 2 files changed, 32 insertions(+), 4 deletions(-)

diff --git a/BizHawk.Emulation.DiscSystem/CCD_format.cs b/BizHawk.Emulation.DiscSystem/CCD_format.cs
index 69d8895ef3..963bc94b30 100644
--- a/BizHawk.Emulation.DiscSystem/CCD_format.cs
+++ b/BizHawk.Emulation.DiscSystem/CCD_format.cs
@@ -457,12 +457,12 @@ namespace BizHawk.Emulation.DiscSystem
 				if ((job.Parts & (ESectorSynthPart.SubcodeAny)) != 0)
 				{
 					ofs = job.LBA * 96;
-					imgBlob.Read(ofs, job.DestBuffer2448, 2352, 96);
+					subBlob.Read(ofs, job.DestBuffer2448, 2352, 96);
 
-					//we may still need to deinterleave it
-					if ((job.Parts & (ESectorSynthPart.SubcodeDeinterleave)) != 0)
+					//sub data comes to us deinterleved; we may still need to interleave it
+					if ((job.Parts & (ESectorSynthPart.SubcodeDeinterleave)) == 0)
 					{
-						SubcodeUtils.DeinterleaveInplace(job.DestBuffer2448, 2352);
+						SubcodeUtils.InterleaveInplace(job.DestBuffer2448, 2352);
 					}
 				}
 			}
diff --git a/BizHawk.Emulation.DiscSystem/Subcode.cs b/BizHawk.Emulation.DiscSystem/Subcode.cs
index dbe8e4ebc5..f8e9047aff 100644
--- a/BizHawk.Emulation.DiscSystem/Subcode.cs
+++ b/BizHawk.Emulation.DiscSystem/Subcode.cs
@@ -72,6 +72,34 @@ namespace BizHawk.Emulation.DiscSystem
 			}
 		}
 
+		/// <summary>
+		/// Converts the useful (but unrealistic) deinterleaved data into the useless (but realistic) interleaved subchannel format.
+		/// </summary>
+		public unsafe static void InterleaveInplace(byte[] buf, int buf_index)
+		{
+			byte* out_buf = stackalloc byte[96];
+
+			for (int i = 0; i < 96; i++)
+				out_buf[i] = 0;
+
+			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 |= ((buf[ch * 12 + d + buf_index] >> (7 - bitpoodle)) & 1) << (7 - ch);
+					}
+					out_buf[(d << 3) + bitpoodle] = (byte)rawb;
+				}
+			}
+
+			for (int i = 0; i < 96; i++)
+				buf[i + buf_index] = out_buf[i];
+		}
+
 		/// <summary>
 		/// Converts the useless (but realistic) interleaved subchannel data into a useful (but unrealistic) deinterleaved format.
 		/// </summary>