avcodec: copy correct chroma plane in CopyFromYv12.
[vlc.git] / modules / video_chroma / copy.c
1 /*****************************************************************************
2  * copy.c: Fast YV12/NV12 copy
3  *****************************************************************************
4  * Copyright (C) 2010 Laurent Aimar
5  * $Id$
6  *
7  * Authors: Laurent Aimar <fenrir _AT_ videolan _DOT_ org>
8  *
9  * This program is free software; you can redistribute it and/or modify it
10  * under the terms of the GNU Lesser General Public License as published by
11  * the Free Software Foundation; either version 2.1 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with this program; if not, write to the Free Software Foundation,
21  * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
22  *****************************************************************************/
23
24 #ifdef HAVE_CONFIG_H
25 # include "config.h"
26 #endif
27
28 #include <vlc_common.h>
29 #include <vlc_picture.h>
30 #include <vlc_cpu.h>
31 #include <assert.h>
32
33 #include "copy.h"
34
35 int CopyInitCache(copy_cache_t *cache, unsigned width)
36 {
37 #ifdef CAN_COMPILE_SSE2
38     cache->size = __MAX((width + 0x0f) & ~ 0x0f, 4096);
39     cache->buffer = vlc_memalign(16, cache->size);
40     if (!cache->buffer)
41         return VLC_EGENERIC;
42 #else
43     (void) cache; (void) width;
44 #endif
45     return VLC_SUCCESS;
46 }
47
48 void CopyCleanCache(copy_cache_t *cache)
49 {
50 #ifdef CAN_COMPILE_SSE2
51     vlc_free(cache->buffer);
52     cache->buffer = NULL;
53     cache->size   = 0;
54 #else
55     (void) cache;
56 #endif
57 }
58
59 #ifdef CAN_COMPILE_SSE2
60 /* Copy 64 bytes from srcp to dstp loading data with the SSE>=2 instruction
61  * load and storing data with the SSE>=2 instruction store.
62  */
63 #define COPY64(dstp, srcp, load, store) \
64     asm volatile (                      \
65         load "  0(%[src]), %%xmm1\n"    \
66         load " 16(%[src]), %%xmm2\n"    \
67         load " 32(%[src]), %%xmm3\n"    \
68         load " 48(%[src]), %%xmm4\n"    \
69         store " %%xmm1,    0(%[dst])\n" \
70         store " %%xmm2,   16(%[dst])\n" \
71         store " %%xmm3,   32(%[dst])\n" \
72         store " %%xmm4,   48(%[dst])\n" \
73         : : [dst]"r"(dstp), [src]"r"(srcp) : "memory", "xmm1", "xmm2", "xmm3", "xmm4")
74
75 #ifndef __SSE4_1__
76 # undef vlc_CPU_SSE4_1
77 # define vlc_CPU_SSE4_1() ((cpu & VLC_CPU_SSE4_1) != 0)
78 #endif
79
80 #ifndef __SSSE3__
81 # undef vlc_CPU_SSSE3
82 # define vlc_CPU_SSSE3() ((cpu & VLC_CPU_SSSE3) != 0)
83 #endif
84
85 #ifndef __SSE2__
86 # undef vlc_CPU_SSE2
87 # define vlc_CPU_SSE2() ((cpu & VLC_CPU_SSE2) != 0)
88 #endif
89
90 /* Optimized copy from "Uncacheable Speculative Write Combining" memory
91  * as used by some video surface.
92  * XXX It is really efficient only when SSE4.1 is available.
93  */
94 VLC_SSE
95 static void CopyFromUswc(uint8_t *dst, size_t dst_pitch,
96                          const uint8_t *src, size_t src_pitch,
97                          unsigned width, unsigned height,
98                          unsigned cpu)
99 {
100     assert(((intptr_t)dst & 0x0f) == 0 && (dst_pitch & 0x0f) == 0);
101
102     asm volatile ("mfence");
103
104     for (unsigned y = 0; y < height; y++) {
105         const unsigned unaligned = (-(uintptr_t)src) & 0x0f;
106         unsigned x = 0;
107
108         for (; x < unaligned; x++)
109             dst[x] = src[x];
110
111 #ifdef CAN_COMPILE_SSE4_1
112         if (vlc_CPU_SSE4_1()) {
113             if (!unaligned) {
114                 for (; x+63 < width; x += 64)
115                     COPY64(&dst[x], &src[x], "movntdqa", "movdqa");
116             } else {
117                 for (; x+63 < width; x += 64)
118                     COPY64(&dst[x], &src[x], "movntdqa", "movdqu");
119             }
120         } else
121 #endif
122         {
123             if (!unaligned) {
124                 for (; x+63 < width; x += 64)
125                     COPY64(&dst[x], &src[x], "movdqa", "movdqa");
126             } else {
127                 for (; x+63 < width; x += 64)
128                     COPY64(&dst[x], &src[x], "movdqa", "movdqu");
129             }
130         }
131
132         for (; x < width; x++)
133             dst[x] = src[x];
134
135         src += src_pitch;
136         dst += dst_pitch;
137     }
138 }
139
140 VLC_SSE
141 static void Copy2d(uint8_t *dst, size_t dst_pitch,
142                    const uint8_t *src, size_t src_pitch,
143                    unsigned width, unsigned height)
144 {
145     assert(((intptr_t)src & 0x0f) == 0 && (src_pitch & 0x0f) == 0);
146
147     asm volatile ("mfence");
148
149     for (unsigned y = 0; y < height; y++) {
150         unsigned x = 0;
151
152         bool unaligned = ((intptr_t)dst & 0x0f) != 0;
153         if (!unaligned) {
154             for (; x+63 < width; x += 64)
155                 COPY64(&dst[x], &src[x], "movdqa", "movntdq");
156         } else {
157             for (; x+63 < width; x += 64)
158                 COPY64(&dst[x], &src[x], "movdqa", "movdqu");
159         }
160
161         for (; x < width; x++)
162             dst[x] = src[x];
163
164         src += src_pitch;
165         dst += dst_pitch;
166     }
167 }
168
169 VLC_SSE
170 static void SSE_SplitUV(uint8_t *dstu, size_t dstu_pitch,
171                         uint8_t *dstv, size_t dstv_pitch,
172                         const uint8_t *src, size_t src_pitch,
173                         unsigned width, unsigned height, unsigned cpu)
174 {
175     const uint8_t shuffle[] = { 0, 2, 4, 6, 8, 10, 12, 14,
176                                 1, 3, 5, 7, 9, 11, 13, 15 };
177     const uint8_t mask[] = { 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00,
178                              0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00 };
179
180     assert(((intptr_t)src & 0x0f) == 0 && (src_pitch & 0x0f) == 0);
181
182     asm volatile ("mfence");
183
184     for (unsigned y = 0; y < height; y++) {
185         unsigned x = 0;
186
187 #define LOAD64 \
188     "movdqa  0(%[src]), %%xmm0\n" \
189     "movdqa 16(%[src]), %%xmm1\n" \
190     "movdqa 32(%[src]), %%xmm2\n" \
191     "movdqa 48(%[src]), %%xmm3\n"
192
193 #define STORE2X32 \
194     "movq   %%xmm0,   0(%[dst1])\n" \
195     "movq   %%xmm1,   8(%[dst1])\n" \
196     "movhpd %%xmm0,   0(%[dst2])\n" \
197     "movhpd %%xmm1,   8(%[dst2])\n" \
198     "movq   %%xmm2,  16(%[dst1])\n" \
199     "movq   %%xmm3,  24(%[dst1])\n" \
200     "movhpd %%xmm2,  16(%[dst2])\n" \
201     "movhpd %%xmm3,  24(%[dst2])\n"
202
203 #ifdef CAN_COMPILE_SSSE3
204         if (vlc_CPU_SSSE3())
205         {
206             for (x = 0; x < (width & ~31); x += 32) {
207                 asm volatile (
208                     "movdqu (%[shuffle]), %%xmm7\n"
209                     LOAD64
210                     "pshufb  %%xmm7, %%xmm0\n"
211                     "pshufb  %%xmm7, %%xmm1\n"
212                     "pshufb  %%xmm7, %%xmm2\n"
213                     "pshufb  %%xmm7, %%xmm3\n"
214                     STORE2X32
215                     : : [dst1]"r"(&dstu[x]), [dst2]"r"(&dstv[x]), [src]"r"(&src[2*x]), [shuffle]"r"(shuffle) : "memory", "xmm0", "xmm1", "xmm2", "xmm3", "xmm7");
216             }
217         } else
218 #endif
219         {
220             for (x = 0; x < (width & ~31); x += 32) {
221                 asm volatile (
222                     "movdqu (%[mask]), %%xmm7\n"
223                     LOAD64
224                     "movdqa   %%xmm0, %%xmm4\n"
225                     "movdqa   %%xmm1, %%xmm5\n"
226                     "movdqa   %%xmm2, %%xmm6\n"
227                     "psrlw    $8,     %%xmm0\n"
228                     "psrlw    $8,     %%xmm1\n"
229                     "pand     %%xmm7, %%xmm4\n"
230                     "pand     %%xmm7, %%xmm5\n"
231                     "pand     %%xmm7, %%xmm6\n"
232                     "packuswb %%xmm4, %%xmm0\n"
233                     "packuswb %%xmm5, %%xmm1\n"
234                     "pand     %%xmm3, %%xmm7\n"
235                     "psrlw    $8,     %%xmm2\n"
236                     "psrlw    $8,     %%xmm3\n"
237                     "packuswb %%xmm6, %%xmm2\n"
238                     "packuswb %%xmm7, %%xmm3\n"
239                     STORE2X32
240                     : : [dst2]"r"(&dstu[x]), [dst1]"r"(&dstv[x]), [src]"r"(&src[2*x]), [mask]"r"(mask) : "memory", "xmm0", "xmm1", "xmm2", "xmm3", "xmm4", "xmm5", "xmm6", "xmm7");
241             }
242         }
243 #undef STORE2X32
244 #undef LOAD64
245
246         for (; x < width; x++) {
247             dstu[x] = src[2*x+0];
248             dstv[x] = src[2*x+1];
249         }
250         src  += src_pitch;
251         dstu += dstu_pitch;
252         dstv += dstv_pitch;
253     }
254 }
255
256 static void SSE_CopyPlane(uint8_t *dst, size_t dst_pitch,
257                           const uint8_t *src, size_t src_pitch,
258                           uint8_t *cache, size_t cache_size,
259                           unsigned width, unsigned height, unsigned cpu)
260 {
261     const unsigned w16 = (width+15) & ~15;
262     const unsigned hstep = cache_size / w16;
263     assert(hstep > 0);
264
265     for (unsigned y = 0; y < height; y += hstep) {
266         const unsigned hblock =  __MIN(hstep, height - y);
267
268         /* Copy a bunch of line into our cache */
269         CopyFromUswc(cache, w16,
270                      src, src_pitch,
271                      width, hblock, cpu);
272
273         /* Copy from our cache to the destination */
274         Copy2d(dst, dst_pitch,
275                cache, w16,
276                width, hblock);
277
278         /* */
279         src += src_pitch * hblock;
280         dst += dst_pitch * hblock;
281     }
282     asm volatile ("mfence");
283 }
284
285 static void SSE_SplitPlanes(uint8_t *dstu, size_t dstu_pitch,
286                             uint8_t *dstv, size_t dstv_pitch,
287                             const uint8_t *src, size_t src_pitch,
288                             uint8_t *cache, size_t cache_size,
289                             unsigned width, unsigned height, unsigned cpu)
290 {
291     const unsigned w2_16 = (2*width+15) & ~15;
292     const unsigned hstep = cache_size / w2_16;
293     assert(hstep > 0);
294
295     for (unsigned y = 0; y < height; y += hstep) {
296         const unsigned hblock =  __MIN(hstep, height - y);
297
298         /* Copy a bunch of line into our cache */
299         CopyFromUswc(cache, w2_16, src, src_pitch,
300                      2*width, hblock, cpu);
301
302         /* Copy from our cache to the destination */
303         SSE_SplitUV(dstu, dstu_pitch, dstv, dstv_pitch,
304                     cache, w2_16, width, hblock, cpu);
305
306         /* */
307         src  += src_pitch  * hblock;
308         dstu += dstu_pitch * hblock;
309         dstv += dstv_pitch * hblock;
310     }
311     asm volatile ("mfence");
312 }
313
314 static void SSE_CopyFromNv12(picture_t *dst,
315                              uint8_t *src[2], size_t src_pitch[2],
316                              unsigned width, unsigned height,
317                              copy_cache_t *cache, unsigned cpu)
318 {
319     SSE_CopyPlane(dst->p[0].p_pixels, dst->p[0].i_pitch,
320                   src[0], src_pitch[0],
321                   cache->buffer, cache->size,
322                   width, height, cpu);
323     SSE_SplitPlanes(dst->p[2].p_pixels, dst->p[2].i_pitch,
324                     dst->p[1].p_pixels, dst->p[1].i_pitch,
325                     src[1], src_pitch[1],
326                     cache->buffer, cache->size,
327                     width/2, height/2, cpu);
328     asm volatile ("emms");
329 }
330
331 static void SSE_CopyFromYv12(picture_t *dst,
332                              uint8_t *src[3], size_t src_pitch[3],
333                              unsigned width, unsigned height,
334                              copy_cache_t *cache, unsigned cpu)
335 {
336     for (unsigned n = 0; n < 3; n++) {
337         const unsigned d = n > 0 ? 2 : 1;
338         SSE_CopyPlane(dst->p[n].p_pixels, dst->p[n].i_pitch,
339                       src[n], src_pitch[n],
340                       cache->buffer, cache->size,
341                       width/d, height/d, cpu);
342     }
343     asm volatile ("emms");
344 }
345 #undef COPY64
346 #endif /* CAN_COMPILE_SSE2 */
347
348 static void CopyPlane(uint8_t *dst, size_t dst_pitch,
349                       const uint8_t *src, size_t src_pitch,
350                       unsigned width, unsigned height)
351 {
352     for (unsigned y = 0; y < height; y++) {
353         memcpy(dst, src, width);
354         src += src_pitch;
355         dst += dst_pitch;
356     }
357 }
358
359 static void SplitPlanes(uint8_t *dstu, size_t dstu_pitch,
360                         uint8_t *dstv, size_t dstv_pitch,
361                         const uint8_t *src, size_t src_pitch,
362                         unsigned width, unsigned height)
363 {
364     for (unsigned y = 0; y < height; y++) {
365         for (unsigned x = 0; x < width; x++) {
366             dstu[x] = src[2*x+0];
367             dstv[x] = src[2*x+1];
368         }
369         src  += src_pitch;
370         dstu += dstu_pitch;
371         dstv += dstv_pitch;
372     }
373 }
374
375 void CopyFromNv12(picture_t *dst, uint8_t *src[2], size_t src_pitch[2],
376                   unsigned width, unsigned height,
377                   copy_cache_t *cache)
378 {
379 #ifdef CAN_COMPILE_SSE2
380     unsigned cpu = vlc_CPU();
381     if (vlc_CPU_SSE2())
382         return SSE_CopyFromNv12(dst, src, src_pitch, width, height,
383                                 cache, cpu);
384 #else
385     (void) cache;
386 #endif
387
388     CopyPlane(dst->p[0].p_pixels, dst->p[0].i_pitch,
389               src[0], src_pitch[0],
390               width, height);
391     SplitPlanes(dst->p[2].p_pixels, dst->p[2].i_pitch,
392                 dst->p[1].p_pixels, dst->p[1].i_pitch,
393                 src[1], src_pitch[1],
394                 width/2, height/2);
395 }
396
397 void CopyFromYv12(picture_t *dst, uint8_t *src[3], size_t src_pitch[3],
398                   unsigned width, unsigned height,
399                   copy_cache_t *cache)
400 {
401 #ifdef CAN_COMPILE_SSE2
402     unsigned cpu = vlc_CPU();
403     if (vlc_CPU_SSE2())
404         return SSE_CopyFromYv12(dst, src, src_pitch, width, height,
405                                 cache, cpu);
406 #else
407     (void) cache;
408 #endif
409
410      CopyPlane(dst->p[0].p_pixels, dst->p[0].i_pitch,
411                src[0], src_pitch[0], width, height);
412      CopyPlane(dst->p[1].p_pixels, dst->p[1].i_pitch,
413                src[1], src_pitch[1], width / 2, height / 2);
414      CopyPlane(dst->p[2].p_pixels, dst->p[2].i_pitch,
415                src[2], src_pitch[2], width / 2, height / 2);
416 }