Merge commit 'b42f49e42f8cde25a788b2d13d03e99ca2956647'
[ffmpeg.git] / libavfilter / libmpcodecs / vf_eq2.c
1 /*
2  * Software equalizer (brightness, contrast, gamma, saturation)
3  *
4  * Hampa Hug <hampa@hampa.ch> (original LUT gamma/contrast/brightness filter)
5  * Daniel Moreno <comac@comac.darktech.org> (saturation, R/G/B gamma support)
6  * Richard Felker (original MMX contrast/brightness code (vf_eq.c))
7  * Michael Niedermayer <michalni@gmx.at> (LUT16)
8  *
9  * This file is part of MPlayer.
10  *
11  * MPlayer is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * MPlayer is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License along
22  * with MPlayer; if not, write to the Free Software Foundation, Inc.,
23  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
24  */
25
26 #include <stdio.h>
27 #include <stdlib.h>
28 #include <string.h>
29 #include <math.h>
30 #include <inttypes.h>
31
32 #include "config.h"
33 #include "mp_msg.h"
34 #include "cpudetect.h"
35
36 #include "img_format.h"
37 #include "mp_image.h"
38 #include "vf.h"
39
40 #define LUT16
41
42 /* Per channel parameters */
43 typedef struct eq2_param_t {
44   unsigned char lut[256];
45 #ifdef LUT16
46   uint16_t lut16[256*256];
47 #endif
48   int           lut_clean;
49
50   void (*adjust) (struct eq2_param_t *par, unsigned char *dst, unsigned char *src,
51     unsigned w, unsigned h, unsigned dstride, unsigned sstride);
52
53   double        c;
54   double        b;
55   double        g;
56   double        w;
57 } eq2_param_t;
58
59 typedef struct vf_priv_s {
60   eq2_param_t param[3];
61
62   double        contrast;
63   double        brightness;
64   double        saturation;
65
66   double        gamma;
67   double        gamma_weight;
68   double        rgamma;
69   double        ggamma;
70   double        bgamma;
71
72   unsigned      buf_w[3];
73   unsigned      buf_h[3];
74   unsigned char *buf[3];
75 } vf_eq2_t;
76
77
78 static
79 void create_lut (eq2_param_t *par)
80 {
81   unsigned i;
82   double   g, v;
83   double   lw, gw;
84
85   g = par->g;
86   gw = par->w;
87   lw = 1.0 - gw;
88
89   if ((g < 0.001) || (g > 1000.0)) {
90     g = 1.0;
91   }
92
93   g = 1.0 / g;
94
95   for (i = 0; i < 256; i++) {
96     v = (double) i / 255.0;
97     v = par->c * (v - 0.5) + 0.5 + par->b;
98
99     if (v <= 0.0) {
100       par->lut[i] = 0;
101     }
102     else {
103       v = v*lw + pow(v, g)*gw;
104
105       if (v >= 1.0) {
106         par->lut[i] = 255;
107       }
108       else {
109         par->lut[i] = (unsigned char) (256.0 * v);
110       }
111     }
112   }
113
114 #ifdef LUT16
115   for(i=0; i<256*256; i++){
116     par->lut16[i]= par->lut[i&0xFF] + (par->lut[i>>8]<<8);
117   }
118 #endif
119
120   par->lut_clean = 1;
121 }
122
123 #if HAVE_MMX
124 static
125 void affine_1d_MMX (eq2_param_t *par, unsigned char *dst, unsigned char *src,
126   unsigned w, unsigned h, unsigned dstride, unsigned sstride)
127 {
128   unsigned i;
129   int      contrast, brightness;
130   unsigned dstep, sstep;
131   int      pel;
132   short    brvec[4];
133   short    contvec[4];
134
135 //  printf("\nmmx: src=%p dst=%p w=%d h=%d ds=%d ss=%d\n",src,dst,w,h,dstride,sstride);
136
137   contrast = (int) (par->c * 256 * 16);
138   brightness = ((int) (100.0 * par->b + 100.0) * 511) / 200 - 128 - contrast / 32;
139
140   brvec[0] = brvec[1] = brvec[2] = brvec[3] = brightness;
141   contvec[0] = contvec[1] = contvec[2] = contvec[3] = contrast;
142
143   sstep = sstride - w;
144   dstep = dstride - w;
145
146   while (h-- > 0) {
147     __asm__ volatile (
148       "movq (%5), %%mm3 \n\t"
149       "movq (%6), %%mm4 \n\t"
150       "pxor %%mm0, %%mm0 \n\t"
151       "movl %4, %%eax\n\t"
152       ASMALIGN(4)
153       "1: \n\t"
154       "movq (%0), %%mm1 \n\t"
155       "movq (%0), %%mm2 \n\t"
156       "punpcklbw %%mm0, %%mm1 \n\t"
157       "punpckhbw %%mm0, %%mm2 \n\t"
158       "psllw $4, %%mm1 \n\t"
159       "psllw $4, %%mm2 \n\t"
160       "pmulhw %%mm4, %%mm1 \n\t"
161       "pmulhw %%mm4, %%mm2 \n\t"
162       "paddw %%mm3, %%mm1 \n\t"
163       "paddw %%mm3, %%mm2 \n\t"
164       "packuswb %%mm2, %%mm1 \n\t"
165       "add $8, %0 \n\t"
166       "movq %%mm1, (%1) \n\t"
167       "add $8, %1 \n\t"
168       "decl %%eax \n\t"
169       "jnz 1b \n\t"
170       : "=r" (src), "=r" (dst)
171       : "0" (src), "1" (dst), "r" (w >> 3), "r" (brvec), "r" (contvec)
172       : "%eax"
173     );
174
175     for (i = w & 7; i > 0; i--) {
176       pel = ((*src++ * contrast) >> 12) + brightness;
177       if (pel & 768) {
178         pel = (-pel) >> 31;
179       }
180       *dst++ = pel;
181     }
182
183     src += sstep;
184     dst += dstep;
185   }
186
187   __asm__ volatile ( "emms \n\t" ::: "memory" );
188 }
189 #endif
190
191 static
192 void apply_lut (eq2_param_t *par, unsigned char *dst, unsigned char *src,
193   unsigned w, unsigned h, unsigned dstride, unsigned sstride)
194 {
195   unsigned      i, j, w2;
196   unsigned char *lut;
197   uint16_t *lut16;
198
199   if (!par->lut_clean) {
200     create_lut (par);
201   }
202
203   lut = par->lut;
204 #ifdef LUT16
205   lut16 = par->lut16;
206   w2= (w>>3)<<2;
207   for (j = 0; j < h; j++) {
208     uint16_t *src16= (uint16_t*)src;
209     uint16_t *dst16= (uint16_t*)dst;
210     for (i = 0; i < w2; i+=4) {
211       dst16[i+0] = lut16[src16[i+0]];
212       dst16[i+1] = lut16[src16[i+1]];
213       dst16[i+2] = lut16[src16[i+2]];
214       dst16[i+3] = lut16[src16[i+3]];
215     }
216     i <<= 1;
217 #else
218   w2= (w>>3)<<3;
219   for (j = 0; j < h; j++) {
220     for (i = 0; i < w2; i+=8) {
221       dst[i+0] = lut[src[i+0]];
222       dst[i+1] = lut[src[i+1]];
223       dst[i+2] = lut[src[i+2]];
224       dst[i+3] = lut[src[i+3]];
225       dst[i+4] = lut[src[i+4]];
226       dst[i+5] = lut[src[i+5]];
227       dst[i+6] = lut[src[i+6]];
228       dst[i+7] = lut[src[i+7]];
229     }
230 #endif
231     for (; i < w; i++) {
232       dst[i] = lut[src[i]];
233     }
234
235     src += sstride;
236     dst += dstride;
237   }
238 }
239
240 static
241 int put_image (vf_instance_t *vf, mp_image_t *src, double pts)
242 {
243   unsigned      i;
244   vf_eq2_t      *eq2;
245   mp_image_t    *dst;
246   unsigned long img_n,img_c;
247
248   eq2 = vf->priv;
249
250   if ((eq2->buf_w[0] != src->w) || (eq2->buf_h[0] != src->h)) {
251     eq2->buf_w[0] = src->w;
252     eq2->buf_h[0] = src->h;
253       eq2->buf_w[1] = eq2->buf_w[2] = src->w >> src->chroma_x_shift;
254       eq2->buf_h[1] = eq2->buf_h[2] = src->h >> src->chroma_y_shift;
255     img_n = eq2->buf_w[0]*eq2->buf_h[0];
256     if(src->num_planes>1){
257       img_c = eq2->buf_w[1]*eq2->buf_h[1];
258       eq2->buf[0] = realloc (eq2->buf[0], img_n + 2*img_c);
259       eq2->buf[1] = eq2->buf[0] + img_n;
260       eq2->buf[2] = eq2->buf[1] + img_c;
261     } else
262       eq2->buf[0] = realloc (eq2->buf[0], img_n);
263   }
264
265   dst = ff_vf_get_image (vf->next, src->imgfmt, MP_IMGTYPE_EXPORT, 0, src->w, src->h);
266
267   for (i = 0; i < ((src->num_planes>1)?3:1); i++) {
268     if (eq2->param[i].adjust != NULL) {
269       dst->planes[i] = eq2->buf[i];
270       dst->stride[i] = eq2->buf_w[i];
271
272       eq2->param[i].adjust (&eq2->param[i], dst->planes[i], src->planes[i],
273         eq2->buf_w[i], eq2->buf_h[i], dst->stride[i], src->stride[i]);
274     }
275     else {
276       dst->planes[i] = src->planes[i];
277       dst->stride[i] = src->stride[i];
278     }
279   }
280
281   return ff_vf_next_put_image (vf, dst, pts);
282 }
283
284 static
285 void check_values (eq2_param_t *par)
286 {
287   /* yuck! floating point comparisons... */
288
289   if ((par->c == 1.0) && (par->b == 0.0) && (par->g == 1.0)) {
290     par->adjust = NULL;
291   }
292 #if HAVE_MMX
293   else if (par->g == 1.0 && ff_gCpuCaps.hasMMX) {
294     par->adjust = &affine_1d_MMX;
295   }
296 #endif
297   else {
298     par->adjust = &apply_lut;
299   }
300 }
301
302 static
303 void print_values (vf_eq2_t *eq2)
304 {
305   ff_mp_msg (MSGT_VFILTER, MSGL_V, "vf_eq2: c=%.2f b=%.2f g=%.4f s=%.2f \n",
306     eq2->contrast, eq2->brightness, eq2->gamma, eq2->saturation
307   );
308 }
309
310 static
311 void set_contrast (vf_eq2_t *eq2, double c)
312 {
313   eq2->contrast = c;
314   eq2->param[0].c = c;
315   eq2->param[0].lut_clean = 0;
316   check_values (&eq2->param[0]);
317   print_values (eq2);
318 }
319
320 static
321 void set_brightness (vf_eq2_t *eq2, double b)
322 {
323   eq2->brightness = b;
324   eq2->param[0].b = b;
325   eq2->param[0].lut_clean = 0;
326   check_values (&eq2->param[0]);
327   print_values (eq2);
328 }
329
330 static
331 void set_gamma (vf_eq2_t *eq2, double g)
332 {
333   eq2->gamma = g;
334
335   eq2->param[0].g = eq2->gamma * eq2->ggamma;
336   eq2->param[1].g = sqrt (eq2->bgamma / eq2->ggamma);
337   eq2->param[2].g = sqrt (eq2->rgamma / eq2->ggamma);
338   eq2->param[0].w = eq2->param[1].w = eq2->param[2].w = eq2->gamma_weight;
339
340   eq2->param[0].lut_clean = 0;
341   eq2->param[1].lut_clean = 0;
342   eq2->param[2].lut_clean = 0;
343
344   check_values (&eq2->param[0]);
345   check_values (&eq2->param[1]);
346   check_values (&eq2->param[2]);
347
348   print_values (eq2);
349 }
350
351 static
352 void set_saturation (vf_eq2_t *eq2, double s)
353 {
354   eq2->saturation = s;
355
356   eq2->param[1].c = s;
357   eq2->param[2].c = s;
358
359   eq2->param[1].lut_clean = 0;
360   eq2->param[2].lut_clean = 0;
361
362   check_values (&eq2->param[1]);
363   check_values (&eq2->param[2]);
364
365   print_values (eq2);
366 }
367
368 static
369 int control (vf_instance_t *vf, int request, void *data)
370 {
371   vf_equalizer_t *eq;
372
373   switch (request) {
374     case VFCTRL_SET_EQUALIZER:
375       eq = (vf_equalizer_t *) data;
376
377       if (strcmp (eq->item, "gamma") == 0) {
378         set_gamma (vf->priv, exp (log (8.0) * eq->value / 100.0));
379         return CONTROL_TRUE;
380       }
381       else if (strcmp (eq->item, "contrast") == 0) {
382         set_contrast (vf->priv, (1.0 / 100.0) * (eq->value + 100));
383         return CONTROL_TRUE;
384       }
385       else if (strcmp (eq->item, "brightness") == 0) {
386         set_brightness (vf->priv, (1.0 / 100.0) * eq->value);
387         return CONTROL_TRUE;
388       }
389       else if (strcmp (eq->item, "saturation") == 0) {
390         set_saturation (vf->priv, (double) (eq->value + 100) / 100.0);
391         return CONTROL_TRUE;
392       }
393       break;
394
395     case VFCTRL_GET_EQUALIZER:
396       eq = (vf_equalizer_t *) data;
397       if (strcmp (eq->item, "gamma") == 0) {
398         eq->value = (int) (100.0 * log (vf->priv->gamma) / log (8.0));
399         return CONTROL_TRUE;
400       }
401       else if (strcmp (eq->item, "contrast") == 0) {
402         eq->value = (int) (100.0 * vf->priv->contrast) - 100;
403         return CONTROL_TRUE;
404       }
405       else if (strcmp (eq->item, "brightness") == 0) {
406         eq->value = (int) (100.0 * vf->priv->brightness);
407         return CONTROL_TRUE;
408       }
409       else if (strcmp (eq->item, "saturation") == 0) {
410         eq->value = (int) (100.0 * vf->priv->saturation) - 100;
411         return CONTROL_TRUE;
412       }
413       break;
414   }
415
416   return ff_vf_next_control (vf, request, data);
417 }
418
419 static
420 int query_format (vf_instance_t *vf, unsigned fmt)
421 {
422   switch (fmt) {
423     case IMGFMT_YVU9:
424     case IMGFMT_IF09:
425     case IMGFMT_YV12:
426     case IMGFMT_I420:
427     case IMGFMT_IYUV:
428     case IMGFMT_Y800:
429     case IMGFMT_Y8:
430     case IMGFMT_444P:
431     case IMGFMT_422P:
432     case IMGFMT_411P:
433       return ff_vf_next_query_format (vf, fmt);
434   }
435
436   return 0;
437 }
438
439 static
440 void uninit (vf_instance_t *vf)
441 {
442   if (vf->priv != NULL) {
443     free (vf->priv->buf[0]);
444     free (vf->priv);
445   }
446 }
447
448 static
449 int vf_open(vf_instance_t *vf, char *args)
450 {
451   unsigned i;
452   vf_eq2_t *eq2;
453   double   par[8];
454
455   vf->control = control;
456   vf->query_format = query_format;
457   vf->put_image = put_image;
458   vf->uninit = uninit;
459
460   vf->priv = malloc (sizeof (vf_eq2_t));
461   eq2 = vf->priv;
462
463   for (i = 0; i < 3; i++) {
464     eq2->buf[i] = NULL;
465     eq2->buf_w[i] = 0;
466     eq2->buf_h[i] = 0;
467
468     eq2->param[i].adjust = NULL;
469     eq2->param[i].c = 1.0;
470     eq2->param[i].b = 0.0;
471     eq2->param[i].g = 1.0;
472     eq2->param[i].lut_clean = 0;
473   }
474
475   eq2->contrast = 1.0;
476   eq2->brightness = 0.0;
477   eq2->saturation = 1.0;
478
479   eq2->gamma = 1.0;
480   eq2->gamma_weight = 1.0;
481   eq2->rgamma = 1.0;
482   eq2->ggamma = 1.0;
483   eq2->bgamma = 1.0;
484
485   if (args != NULL) {
486     par[0] = 1.0;
487     par[1] = 1.0;
488     par[2] = 0.0;
489     par[3] = 1.0;
490     par[4] = 1.0;
491     par[5] = 1.0;
492     par[6] = 1.0;
493     par[7] = 1.0;
494     sscanf (args, "%lf:%lf:%lf:%lf:%lf:%lf:%lf:%lf",
495       par, par + 1, par + 2, par + 3, par + 4, par + 5, par + 6, par + 7
496     );
497
498     eq2->rgamma = par[4];
499     eq2->ggamma = par[5];
500     eq2->bgamma = par[6];
501     eq2->gamma_weight = par[7];
502
503     set_gamma (eq2, par[0]);
504     set_contrast (eq2, par[1]);
505     set_brightness (eq2, par[2]);
506     set_saturation (eq2, par[3]);
507   }
508
509   return 1;
510 }
511
512 const vf_info_t ff_vf_info_eq2 = {
513   "Software equalizer",
514   "eq2",
515   "Hampa Hug, Daniel Moreno, Richard Felker",
516   "",
517   &vf_open,
518   NULL
519 };