noise.c

linux下的MPEG1

/*
 * Copyright (C) 2000-2006 the xine project
 * 
 * This file is part of xine, a free video player.
 * 
 * xine is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * xine is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA
 *
 * $Id: noise.c,v 1.4 2007/03/22 23:32:05 dgp85 Exp $
 *
 * mplayer's noise filter, ported by Jason Tackaberry.  Original filter
 * is copyright 2002 Michael Niedermayer <[e-mail]>
 */

#include "xine_internal.h"
#include "post.h"
#include "xineutils.h"
#include <math.h>
#include <pthread.h>


#ifdef ARCH_X86_64
#  define REG_a  "rax"
#else
#  define REG_a  "eax"
#endif

#define MAX_NOISE 4096
#define MAX_SHIFT 1024
#define MAX_RES (MAX_NOISE-MAX_SHIFT)

static inline void lineNoise_C(uint8_t *dst, uint8_t *src, int8_t *noise, int len, int shift);
static inline void lineNoiseAvg_C(uint8_t *dst, uint8_t *src, int len, int8_t **shift);

static void (*lineNoise)(uint8_t *dst, uint8_t *src, int8_t *noise, int len, int shift) = lineNoise_C;
static void (*lineNoiseAvg)(uint8_t *dst, uint8_t *src, int len, int8_t **shift) = lineNoiseAvg_C;


typedef struct noise_param_t {
    int strength,
        uniform,
        temporal,
        quality,
        averaged,
        pattern,
        shiftptr;
    int8_t *noise,
           *prev_shift[MAX_RES][3];
    void *base;
} noise_param_t;

static int nonTempRandShift[MAX_RES]= {-1};

static const int patt[4] = {
    -1,0,1,0
};

#define RAND_N(range) ((int) ((double)range*rand()/(RAND_MAX+1.0)))
static int8_t *initNoise(noise_param_t *fp){
    int strength= fp->strength;
    int uniform= fp->uniform;
    int averaged= fp->averaged;
    int pattern= fp->pattern;
    int8_t *noise;
    int i, j;
    void *base;

    noise = xine_xmalloc_aligned(16, MAX_NOISE*sizeof(int8_t), &base);
    srand(123457);

    for(i=0,j=0; i<MAX_NOISE; i++,j++)
    {
        if(uniform) {
            if (averaged) {
                    if (pattern) {
                    noise[i]= (RAND_N(strength) - strength/2)/6
                        +patt[j%4]*strength*0.25/3;
                } else {
                    noise[i]= (RAND_N(strength) - strength/2)/3;
                    }
            } else {
                    if (pattern) {
                    noise[i]= (RAND_N(strength) - strength/2)/2
                        + patt[j%4]*strength*0.25;
                } else {
                    noise[i]= RAND_N(strength) - strength/2;
                    }
            }
            } else {
            double x1, x2, w, y1;
            do {
                x1 = 2.0 * rand()/(float)RAND_MAX - 1.0;
                x2 = 2.0 * rand()/(float)RAND_MAX - 1.0;
                w = x1 * x1 + x2 * x2;
            } while ( w >= 1.0 );

            w = sqrt( (-2.0 * log( w ) ) / w );
            y1= x1 * w;
            y1*= strength / sqrt(3.0);
            if (pattern) {
                y1 /= 2;
                y1 += patt[j%4]*strength*0.35;
            }
            if     (y1<-128) y1=-128;
            else if(y1> 127) y1= 127;
            if (averaged) y1 /= 3.0;
            noise[i]= (int)y1;
        }
        if (RAND_N(6) == 0) j--;
    }


    for (i = 0; i < MAX_RES; i++)
        for (j = 0; j < 3; j++)
        fp->prev_shift[i][j] = noise + (rand()&(MAX_SHIFT-1));

    if(nonTempRandShift[0]==-1){
        for(i=0; i<MAX_RES; i++){
            nonTempRandShift[i]= rand()&(MAX_SHIFT-1);
        }
    }

    fp->noise= noise;
    fp->base = base;
    fp->shiftptr= 0;
    return noise;
}

static inline void lineNoise_C(uint8_t *dst, uint8_t *src, int8_t *noise, int len, int shift){
    int i;
    noise+= shift;
    for(i=0; i<len; i++)
    {
        int v= src[i]+ noise[i];
        if(v>255)   dst[i]=255; //FIXME optimize
        else if(v<0)    dst[i]=0;
        else        dst[i]=v;
    }
}

#ifdef ARCH_X86
static inline void lineNoise_MMX(uint8_t *dst, uint8_t *src, int8_t *noise, int len, int shift){
    long mmx_len= len&(~7);
    noise+=shift;

    asm volatile(
        "mov %3, %%"REG_a"      \n\t"
        "pcmpeqb %%mm7, %%mm7       \n\t"
        "psllw $15, %%mm7       \n\t"
        "packsswb %%mm7, %%mm7      \n\t"
	ASMALIGN(4)
        "1:             \n\t"
        "movq (%0, %%"REG_a"), %%mm0    \n\t"
        "movq (%1, %%"REG_a"), %%mm1    \n\t"
        "pxor %%mm7, %%mm0      \n\t"
        "paddsb %%mm1, %%mm0        \n\t"
        "pxor %%mm7, %%mm0      \n\t"
        "movq %%mm0, (%2, %%"REG_a")    \n\t"
        "add $8, %%"REG_a"      \n\t"
        " js 1b             \n\t"
        :: "r" (src+mmx_len), "r" (noise+mmx_len), "r" (dst+mmx_len), "g" (-mmx_len)
        : "%"REG_a
    );
    if(mmx_len!=len)
        lineNoise_C(dst+mmx_len, src+mmx_len, noise+mmx_len, len-mmx_len, 0);
}

//duplicate of previous except movntq
static inline void lineNoise_MMX2(uint8_t *dst, uint8_t *src, int8_t *noise, int len, int shift){
    long mmx_len= len&(~7);
    noise+=shift;

    asm volatile(
        "mov %3, %%"REG_a"      \n\t"
        "pcmpeqb %%mm7, %%mm7       \n\t"
        "psllw $15, %%mm7       \n\t"
        "packsswb %%mm7, %%mm7      \n\t"
	ASMALIGN(4)
        "1:             \n\t"
        "movq (%0, %%"REG_a"), %%mm0    \n\t"
        "movq (%1, %%"REG_a"), %%mm1    \n\t"
        "pxor %%mm7, %%mm0      \n\t"
        "paddsb %%mm1, %%mm0        \n\t"
        "pxor %%mm7, %%mm0      \n\t"
        "movntq %%mm0, (%2, %%"REG_a")  \n\t"
        "add $8, %%"REG_a"      \n\t"
        " js 1b             \n\t"
        :: "r" (src+mmx_len), "r" (noise+mmx_len), "r" (dst+mmx_len), "g" (-mmx_len)
        : "%"REG_a
    );
    if(mmx_len!=len)
        lineNoise_C(dst+mmx_len, src+mmx_len, noise+mmx_len, len-mmx_len, 0);
}

#endif

/***************************************************************************/

static inline void lineNoiseAvg_C(uint8_t *dst, uint8_t *src, int len, int8_t **shift){
    int i;
    int8_t *src2= (int8_t*)src;

    for(i=0; i<len; i++)
    {
        const int n= shift[0][i] + shift[1][i] + shift[2][i];
        dst[i]= src2[i]+((n*src2[i])>>7);
    }
}

#ifdef ARCH_X86

static inline void lineNoiseAvg_MMX(uint8_t *dst, uint8_t *src, int len, int8_t **shift){
    long mmx_len= len&(~7);

    asm volatile(
        "mov %5, %%"REG_a"      \n\t"
	ASMALIGN(4)
        "1:             \n\t"
        "movq (%1, %%"REG_a"), %%mm1    \n\t"
        "movq (%0, %%"REG_a"), %%mm0    \n\t"
        "paddb (%2, %%"REG_a"), %%mm1   \n\t"
        "paddb (%3, %%"REG_a"), %%mm1   \n\t"
        "movq %%mm0, %%mm2      \n\t"
        "movq %%mm1, %%mm3      \n\t"
        "punpcklbw %%mm0, %%mm0     \n\t"
        "punpckhbw %%mm2, %%mm2     \n\t"
        "punpcklbw %%mm1, %%mm1     \n\t"
        "punpckhbw %%mm3, %%mm3     \n\t"
        "pmulhw %%mm0, %%mm1        \n\t"
        "pmulhw %%mm2, %%mm3        \n\t"
        "paddw %%mm1, %%mm1     \n\t"
        "paddw %%mm3, %%mm3     \n\t"
        "paddw %%mm0, %%mm1     \n\t"
        "paddw %%mm2, %%mm3     \n\t"
        "psrlw $8, %%mm1        \n\t"
        "psrlw $8, %%mm3        \n\t"
                "packuswb %%mm3, %%mm1      \n\t"
        "movq %%mm1, (%4, %%"REG_a")    \n\t"
        "add $8, %%"REG_a"      \n\t"
        " js 1b             \n\t"
        :: "r" (src+mmx_len), "r" (shift[0]+mmx_len), "r" (shift[1]+mmx_len), "r" (shift[2]+mmx_len),
                   "r" (dst+mmx_len), "g" (-mmx_len)
        : "%"REG_a
    );

    if(mmx_len!=len){
        int8_t *shift2[3]={shift[0]+mmx_len, shift[1]+mmx_len, shift[2]+mmx_len};
        lineNoiseAvg_C(dst+mmx_len, src+mmx_len, len-mmx_len, shift2);
    }
}
#endif

/***************************************************************************/

static void noise(uint8_t *dst, uint8_t *src, int dstStride, int srcStride, int width, int height, noise_param_t *fp)
{
    int8_t *noise= fp->noise;
    int y;
    int shift=0;

    if(!noise)
    {
        if(src==dst) return;

        if(dstStride==srcStride) memcpy(dst, src, srcStride*height);
        else
        {
            for(y=0; y<height; y++)
            {
                memcpy(dst, src, width);
                dst+= dstStride;
                src+= srcStride;
            }
        }
        return;
    }

    for(y=0; y<height; y++)
    {
        if(fp->temporal)    shift=  rand()&(MAX_SHIFT  -1);
        else                shift= nonTempRandShift[y];

        if(fp->quality==0) shift&= ~7;
        if (fp->averaged) {
            lineNoiseAvg(dst, src, width, fp->prev_shift[y]);
            fp->prev_shift[y][fp->shiftptr] = noise + shift;
        } else {
            lineNoise(dst, src, noise, width, shift);
        }
        dst+= dstStride;
        src+= srcStride;
    }
    fp->shiftptr++;