1 files changed, 80 insertions, 0 deletions
diff --git a/postproc/rgb2rgb_template.c b/postproc/rgb2rgb_template.c
index 3a1a3320f7..46f36d88b9 100644
--- a/postproc/rgb2rgb_template.c
+++ b/postproc/rgb2rgb_template.c
@@ -1197,3 +1197,83 @@ static inline void RENAME(rgb24toyv12)(const uint8_t *src, uint8_t *ydst, uint8_
 		src  += srcStride;
 	}
 }
+
+void RENAME(interleaveBytes)(uint8_t *src1, uint8_t *src2, uint8_t *dest,
+			    int width, int height, int src1Stride, int src2Stride, int dstStride){
+	int h;
+
+	for(h=0; h < height; h++)
+	{
+		int w;
+
+#ifdef HAVE_MMX
+#ifdef HAVE_SSE2
+		asm(
+			"xorl %%eax, %%eax		\n\t"
+			"1:				\n\t"
+			PREFETCH" 64(%1, %%eax)		\n\t"
+			PREFETCH" 64(%2, %%eax)		\n\t"
+			"movdqa (%1, %%eax), %%xmm0	\n\t"
+			"movdqa (%1, %%eax), %%xmm1	\n\t"
+			"movdqa (%2, %%eax), %%xmm2	\n\t"
+			"punpcklbw %%xmm2, %%xmm0	\n\t"
+			"punpckhbw %%xmm2, %%xmm1	\n\t"
+			"movntdq %%xmm0, (%0, %%eax, 2)	\n\t"
+			"movntdq %%xmm1, 16(%0, %%eax, 2)\n\t"
+			"addl $16, %%eax			\n\t"
+			"cmpl %3, %%eax			\n\t"
+			" jb 1b				\n\t"
+			::"r"(dest), "r"(src1), "r"(src2), "r" (width-15)
+			: "memory", "%eax"
+		);
+#else
+		asm(
+			"xorl %%eax, %%eax		\n\t"
+			"1:				\n\t"
+			PREFETCH" 64(%1, %%eax)		\n\t"
+			PREFETCH" 64(%2, %%eax)		\n\t"
+			"movq (%1, %%eax), %%mm0	\n\t"
+			"movq 8(%1, %%eax), %%mm2	\n\t"
+			"movq %%mm0, %%mm1		\n\t"
+			"movq %%mm2, %%mm3		\n\t"
+			"movq (%2, %%eax), %%mm4	\n\t"
+			"movq 8(%2, %%eax), %%mm5	\n\t"
+			"punpcklbw %%mm4, %%mm0		\n\t"
+			"punpckhbw %%mm4, %%mm1		\n\t"
+			"punpcklbw %%mm5, %%mm2		\n\t"
+			"punpckhbw %%mm5, %%mm3		\n\t"
+			MOVNTQ" %%mm0, (%0, %%eax, 2)	\n\t"
+			MOVNTQ" %%mm1, 8(%0, %%eax, 2)	\n\t"
+			MOVNTQ" %%mm2, 16(%0, %%eax, 2)	\n\t"
+			MOVNTQ" %%mm3, 24(%0, %%eax, 2)	\n\t"
+			"addl $16, %%eax			\n\t"
+			"cmpl %3, %%eax			\n\t"
+			" jb 1b				\n\t"
+			::"r"(dest), "r"(src1), "r"(src2), "r" (width-15)
+			: "memory", "%eax"
+		);
+#endif
+		for(w= (width&(~15)); w < width; w++)
+		{
+			dest[2*w+0] = src1[w];
+			dest[2*w+1] = src2[w];
+		}
+#else
+		for(w=0; w < width; w++)
+		{
+			dest[2*w+0] = src1[w];
+			dest[2*w+1] = src2[w];
+		}
+#endif
+		dest += dstStride;
+                src1 += src1Stride;
+                src2 += src2Stride;
+	}
+#ifdef HAVE_MMX
+	asm(
+		EMMS" \n\t"
+		SFENCE" \n\t"
+		::: "memory"
+		);
+#endif
+}