libmpcodecs/vf_eq2.c

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