floatdsp: move vector_fmul_add from dsputil to avfloatdsp.

9 files changed, 122 insertions, 0 deletions

diff --git a/libavutil/arm/float_dsp_init_neon.c b/libavutil/arm/float_dsp_init_neon.c
index 16ea47154a..41e513fcdc 100644
--- a/libavutil/arm/float_dsp_init_neon.c
+++ b/libavutil/arm/float_dsp_init_neon.c
@@ -35,10 +35,14 @@ void ff_vector_fmul_scalar_neon(float *dst, const float *src, float mul,
 void ff_vector_fmul_window_neon(float *dst, const float *src0,
                                 const float *src1, const float *win, int len);
 
+void ff_vector_fmul_add_neon(float *dst, const float *src0, const float *src1,
+                             const float *src2, int len);
+
 void ff_float_dsp_init_neon(AVFloatDSPContext *fdsp)
 {
     fdsp->vector_fmul = ff_vector_fmul_neon;
     fdsp->vector_fmac_scalar = ff_vector_fmac_scalar_neon;
     fdsp->vector_fmul_scalar = ff_vector_fmul_scalar_neon;
     fdsp->vector_fmul_window = ff_vector_fmul_window_neon;
+    fdsp->vector_fmul_add    = ff_vector_fmul_add_neon;
 }
diff --git a/libavutil/arm/float_dsp_neon.S b/libavutil/arm/float_dsp_neon.S
index 540cfc6907..100eb02455 100644
--- a/libavutil/arm/float_dsp_neon.S
+++ b/libavutil/arm/float_dsp_neon.S
@@ -193,3 +193,30 @@ function ff_vector_fmul_window_neon, export=1
         vst1.32         {d22,d23},[ip,:128], r5
         pop             {r4,r5,pc}
 endfunc
+
+function ff_vector_fmul_add_neon, export=1
+        ldr             r12, [sp]
+        vld1.32         {q0-q1},  [r1,:128]!
+        vld1.32         {q8-q9},  [r2,:128]!
+        vld1.32         {q2-q3},  [r3,:128]!
+        vmul.f32        q10, q0,  q8
+        vmul.f32        q11, q1,  q9
+1:      vadd.f32        q12, q2,  q10
+        vadd.f32        q13, q3,  q11
+        pld             [r1, #16]
+        pld             [r2, #16]
+        pld             [r3, #16]
+        subs            r12, r12, #8
+        beq             2f
+        vld1.32         {q0},     [r1,:128]!
+        vld1.32         {q8},     [r2,:128]!
+        vmul.f32        q10, q0,  q8
+        vld1.32         {q1},     [r1,:128]!
+        vld1.32         {q9},     [r2,:128]!
+        vmul.f32        q11, q1,  q9
+        vld1.32         {q2-q3},  [r3,:128]!
+        vst1.32         {q12-q13},[r0,:128]!
+        b               1b
+2:      vst1.32         {q12-q13},[r0,:128]!
+        bx              lr
+endfunc
diff --git a/libavutil/float_dsp.c b/libavutil/float_dsp.c
index cf33df303c..456f83b656 100644
--- a/libavutil/float_dsp.c
+++ b/libavutil/float_dsp.c
@@ -71,6 +71,14 @@ static void vector_fmul_window_c(float *dst, const float *src0,
     }
 }
 
+static void vector_fmul_add_c(float *dst, const float *src0, const float *src1,
+                              const float *src2, int len){
+    int i;
+
+    for (i = 0; i < len; i++)
+        dst[i] = src0[i] * src1[i] + src2[i];
+}
+
 void avpriv_float_dsp_init(AVFloatDSPContext *fdsp, int bit_exact)
 {
     fdsp->vector_fmul = vector_fmul_c;
@@ -78,6 +86,7 @@ void avpriv_float_dsp_init(AVFloatDSPContext *fdsp, int bit_exact)
     fdsp->vector_fmul_scalar = vector_fmul_scalar_c;
     fdsp->vector_dmul_scalar = vector_dmul_scalar_c;
     fdsp->vector_fmul_window = vector_fmul_window_c;
+    fdsp->vector_fmul_add = vector_fmul_add_c;
 
 #if ARCH_ARM
     ff_float_dsp_init_arm(fdsp);
diff --git a/libavutil/float_dsp.h b/libavutil/float_dsp.h
index d0ceaaf5e1..b45c4560ff 100644
--- a/libavutil/float_dsp.h
+++ b/libavutil/float_dsp.h
@@ -100,6 +100,24 @@ typedef struct AVFloatDSPContext {
      */
     void (*vector_fmul_window)(float *dst, const float *src0,
                                const float *src1, const float *win, int len);
+
+    /**
+     * Calculate the product of two vectors of floats, add a third vector of
+     * floats and store the result in a vector of floats.
+     *
+     * @param dst  output vector
+     *             constraints: 32-byte aligned
+     * @param src0 first input vector
+     *             constraints: 32-byte aligned
+     * @param src1 second input vector
+     *             constraints: 32-byte aligned
+     * @param src1 third input vector
+     *             constraints: 32-byte aligned
+     * @param len  number of elements in the input
+     *             constraints: multiple of 16
+     */
+    void (*vector_fmul_add)(float *dst, const float *src0, const float *src1,
+                            const float *src2, int len);
 } AVFloatDSPContext;
 
 /**
diff --git a/libavutil/ppc/float_dsp_altivec.c b/libavutil/ppc/float_dsp_altivec.c
index e5fd9aba33..87379e92f8 100644
--- a/libavutil/ppc/float_dsp_altivec.c
+++ b/libavutil/ppc/float_dsp_altivec.c
@@ -69,3 +69,27 @@ void ff_vector_fmul_window_altivec(float *dst, const float *src0,
         vec_st(t1, j, dst);
     }
 }
+
+void ff_vector_fmul_add_altivec(float *dst, const float *src0,
+                                const float *src1, const float *src2,
+                                int len)
+{
+    int i;
+    vector float d, s0, s1, s2, t0, t1, edges;
+    vector unsigned char align = vec_lvsr(0,dst),
+                         mask = vec_lvsl(0, dst);
+
+    for (i = 0; i < len - 3; i += 4) {
+        t0 = vec_ld(0, dst + i);
+        t1 = vec_ld(15, dst + i);
+        s0 = vec_ld(0, src0 + i);
+        s1 = vec_ld(0, src1 + i);
+        s2 = vec_ld(0, src2 + i);
+        edges = vec_perm(t1, t0, mask);
+        d = vec_madd(s0, s1, s2);
+        t1 = vec_perm(d, edges, align);
+        t0 = vec_perm(edges, d, align);
+        vec_st(t1, 15, dst + i);
+        vec_st(t0, 0, dst + i);
+    }
+}
diff --git a/libavutil/ppc/float_dsp_altivec.h b/libavutil/ppc/float_dsp_altivec.h
index 4d46edf61a..38a9e15434 100644
--- a/libavutil/ppc/float_dsp_altivec.h
+++ b/libavutil/ppc/float_dsp_altivec.h
@@ -28,4 +28,8 @@ extern void ff_vector_fmul_window_altivec(float *dst, const float *src0,
                                           const float *src1, const float *win,
                                           int len);
 
+extern void ff_vector_fmul_add_altivec(float *dst, const float *src0,
+                                       const float *src1, const float *src2,
+                                       int len);
+
 #endif /* AVUTIL_PPC_FLOAT_DSP_ALTIVEC_H */
diff --git a/libavutil/ppc/float_dsp_init.c b/libavutil/ppc/float_dsp_init.c
index 1134b56926..f3d8a42f53 100644
--- a/libavutil/ppc/float_dsp_init.c
+++ b/libavutil/ppc/float_dsp_init.c
@@ -32,6 +32,7 @@ void ff_float_dsp_init_ppc(AVFloatDSPContext *fdsp, int bit_exact)
         return;
 
     fdsp->vector_fmul = ff_vector_fmul_altivec;
+    fdsp->vector_fmul_add = ff_vector_fmul_add_altivec;
 
     if (!bit_exact) {
         fdsp->vector_fmul_window = ff_vector_fmul_window_altivec;
diff --git a/libavutil/x86/float_dsp.asm b/libavutil/x86/float_dsp.asm
index 4113fd91e4..70fc1d0310 100644
--- a/libavutil/x86/float_dsp.asm
+++ b/libavutil/x86/float_dsp.asm
@@ -162,3 +162,31 @@ VECTOR_DMUL_SCALAR
 INIT_YMM avx
 VECTOR_DMUL_SCALAR
 %endif
+
+;-----------------------------------------------------------------------------
+; vector_fmul_add(float *dst, const float *src0, const float *src1,
+;                 const float *src2, int len)
+;-----------------------------------------------------------------------------
+%macro VECTOR_FMUL_ADD 0
+cglobal vector_fmul_add, 5,5,2, dst, src0, src1, src2, len
+    lea       lenq, [lend*4 - 2*mmsize]
+ALIGN 16
+.loop:
+    mova    m0,   [src0q + lenq]
+    mova    m1,   [src0q + lenq + mmsize]
+    mulps   m0, m0, [src1q + lenq]
+    mulps   m1, m1, [src1q + lenq + mmsize]
+    addps   m0, m0, [src2q + lenq]
+    addps   m1, m1, [src2q + lenq + mmsize]
+    mova    [dstq + lenq], m0
+    mova    [dstq + lenq + mmsize], m1
+
+    sub     lenq,   2*mmsize
+    jge     .loop
+    REP_RET
+%endmacro
+
+INIT_XMM sse
+VECTOR_FMUL_ADD
+INIT_YMM avx
+VECTOR_FMUL_ADD
diff --git a/libavutil/x86/float_dsp_init.c b/libavutil/x86/float_dsp_init.c
index 1c678cb40c..0a2ad5ba0e 100644
--- a/libavutil/x86/float_dsp_init.c
+++ b/libavutil/x86/float_dsp_init.c
@@ -41,6 +41,11 @@ extern void ff_vector_dmul_scalar_sse2(double *dst, const double *src,
 extern void ff_vector_dmul_scalar_avx(double *dst, const double *src,
                                       double mul, int len);
 
+void ff_vector_fmul_add_sse(float *dst, const float *src0, const float *src1,
+                            const float *src2, int len);
+void ff_vector_fmul_add_avx(float *dst, const float *src0, const float *src1,
+                            const float *src2, int len);
+
 #if HAVE_6REGS && HAVE_INLINE_ASM
 static void vector_fmul_window_3dnowext(float *dst, const float *src0,
                                         const float *src1, const float *win,
@@ -123,6 +128,7 @@ void ff_float_dsp_init_x86(AVFloatDSPContext *fdsp)
         fdsp->vector_fmul = ff_vector_fmul_sse;
         fdsp->vector_fmac_scalar = ff_vector_fmac_scalar_sse;
         fdsp->vector_fmul_scalar = ff_vector_fmul_scalar_sse;
+        fdsp->vector_fmul_add    = ff_vector_fmul_add_sse;
     }
     if (EXTERNAL_SSE2(mm_flags)) {
         fdsp->vector_dmul_scalar = ff_vector_dmul_scalar_sse2;
@@ -131,5 +137,6 @@ void ff_float_dsp_init_x86(AVFloatDSPContext *fdsp)
         fdsp->vector_fmul = ff_vector_fmul_avx;
         fdsp->vector_fmac_scalar = ff_vector_fmac_scalar_avx;
         fdsp->vector_dmul_scalar = ff_vector_dmul_scalar_avx;
+        fdsp->vector_fmul_add    = ff_vector_fmul_add_avx;
     }
 }