/*

 * Copyright (c) 2007

 *	Josep Torra <josep@fluendo.com>.  All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 * 1. Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 * 2. Redistributions in binary form must reproduce the above copyright

 *    notice, this list of conditions and the following disclaimer in the

 *    documentation and/or other materials provided with the distribution.

 *

 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND

 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE

 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL

 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS

 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)

 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY

 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF

 * SUCH DAMAGE.

 */

#if __VFP_FP__

/* 

** compile with -mcpu=arm1136j-s -mfpu=vfp -mfloat-abi=softfp

**

** void vfp_add_f32 (float *d, const float *s1, const float *s2, int n);

** void vfp_add_f64 (double *d, const double *s1, const double *s2, int n);

** void vfp_divide_f32 (float *d, const float *s1, const float *s2, int n);

** void vfp_divide_f64 (double *d, const double *s1, const double *s2, int n);

** void vfp_multiply_f32 (float *d, const float *s1, const float *s2, int n);

** void vfp_multiply_f64 (double *d, const double *s1, const double *s2, int n);

** void vfp_subtract_f32 (float *d, const float *s1, const float *s2, int n);

** void vfp_subtract_f64 (double *d, const double *s1, const double *s2, int n);

**

** d:   $r0     |   s1: $r1     | s2:  $r2     |   n:  $r3     |

**

*/

#define UNROLL_F32_TEMPLATE(fname,finst) \

  .global vfp_ ## fname ## ;                                                  \

  vfp_ ## fname ## :                                                          \

    stmdb         sp!, {fp, lr};            /* save registers to stack */     \

    ands          ip, r3, #7;               /* ip = n % 8 */                  \

    beq           vfp_ ## fname ## _unroll; /* if ip == 0 goto prep_loop2 */  \

  vfp_ ## fname ## _loop1:                                                    \

    fldmias       r1!, {s0};                                                  \

    fldmias       r2!, {s1};                                                  \

    ## finst ##s  s2, s0, s1;                                                 \

    fstmias       r0!, {s2};                                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop1;                                    \

  vfp_ ## fname ## _unroll:                 /* unroll by 8 */                 \

    movs          ip, r3, lsr #3;           /* ip = n / 8 */                  \

    beq           vfp_ ## fname ## _end;    /* if ip == 0 goto finish */      \

    fmrx          lr, fpscr;                /* read fpscr register into arm */\

    mov           fp, #7;                                                     \

    orr           fp, lr, fp, lsl #16;      /* set vector lenght to 8 */      \

    fmxr          fpscr, fp;                                                  \

  vfp_ ## fname ## _loop2:                                                    \

    fldmias       r1!, {s8, s9, s10, s11, s12, s13, s14, s15};                \

    fldmias       r2!, {s16, s17, s18, s19, s20, s21, s22, s23};              \

    ## finst ##s  s24, s8, s16;                                               \

    fstmias       r0!, {s24, s25, s26, s27, s28, s29, s30, s31};              \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop2;                                    \

    fmxr          fpscr, lr;                /* restore original fpscr */      \

  vfp_ ## fname ## _end:                                                      \

    ldmia         sp!, {fp, pc};        /* recovering from stack and return */   

#define UNROLL_F64_TEMPLATE(fname,finst) \

  .global vfp_ ## fname ## ;                                                  \

  vfp_ ## fname ## :                                                          \

    stmdb         sp!, {fp, lr};            /* save registers to stack */     \

    ands          ip, r3, #3;               /* ip = n % 3 */                  \

    beq           vfp_ ## fname ## _unroll; /* if ip == 0 goto prep_loop2 */  \

  vfp_ ## fname ## _loop1:                                                    \

    fldmiad       r1!, {d0};                                                  \

    fldmiad       r2!, {d1};                                                  \

    ## finst ##d  d2, d0, d1;                                                 \

    fstmiad       r0!, {d2};                                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop1;                                    \

  vfp_ ## fname ## _unroll:                 /* unroll by 4 */                 \

    movs          ip, r3, lsr #2;           /* ip = n / 4 */                  \

    beq           vfp_ ## fname ## _end;    /* if ip == 0 goto finish */      \

    fmrx          lr, fpscr;                /* read fpscr register into arm */\

    mov           fp, #3;                                                     \

    orr           fp, lr, fp, lsl #16;      /* set vector lenght to 8 */      \

    fmxr          fpscr, fp;                                                  \

  vfp_ ## fname ## _loop2:                                                    \

    fldmiad       r1!, {d4, d5, d6, d7};                                      \

    fldmiad       r2!, {d8, d9, d10, d11};                                    \

    ## finst ##d  d12, d4, d8;                                                \

    fstmiad       r0!, {d12, d13, d14, d15};                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop2;                                    \

    fmxr          fpscr, lr;                /* restore original fpscr */      \

  vfp_ ## fname ## _end:                                                      \

    ldmia         sp!, {fp, pc};        /* recovering from stack and return */   

.align 2

UNROLL_F32_TEMPLATE(add_f32,fadd);

UNROLL_F64_TEMPLATE(add_f64,fadd);

UNROLL_F32_TEMPLATE(divide_f32,fdiv);

UNROLL_F64_TEMPLATE(divide_f64,fdiv);

UNROLL_F32_TEMPLATE(multiply_f32,fmul);

UNROLL_F64_TEMPLATE(multiply_f64,fmul);

UNROLL_F32_TEMPLATE(subtract_f32,fsub);

UNROLL_F64_TEMPLATE(subtract_f64,fsub);

#undef UNROLL_F32_TEMPLATE

#undef UNROLL_F64_TEMPLATE

/* 

**

** void vfp_scalaradd_f32_ns (float *d, const float *s1, const float *s2_1, int n);

** void vfp_scalaradd_f64_ns (double *d, const double *s1, const double *s2_1, int n);

** void vfp_scalarmultiply_f32_ns (float *d, const float *s1, const float *s2_1, int n);

** void vfp_scalarmultiply_f64_ns (double *d, const double *s1, const double *s2_1, int n);

**

** d:   $r0     |   s1: $r1     | s2_1:  $r2   |   n:  $r3     |

**

*/

#define UNROLL_F32_TEMPLATE(fname,finst) \

  .global vfp_ ## fname ## ;                                                  \

  vfp_ ## fname ## :                                                          \

    stmdb         sp!, {fp, lr};            /* save registers to stack */     \

    fldmias       r2, {s1};                 /* load scalar value */           \

    ands          ip, r3, #7;               /* ip = n % 8 */                  \

    beq           vfp_ ## fname ## _unroll; /* if ip == 0 goto prep_loop2 */  \

  vfp_ ## fname ## _loop1:                                                    \

    fldmias       r1!, {s0};                                                  \

    ## finst ##s  s2, s0, s1;                                                 \

    fstmias       r0!, {s2};                                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop1;                                    \

  vfp_ ## fname ## _unroll:                 /* unroll by 8 */                 \

    movs          ip, r3, lsr #3;           /* ip = n / 8 */                  \

    beq           vfp_ ## fname ## _end;    /* if ip == 0 goto finish */      \

    fmrx          lr, fpscr;                /* read fpscr register into arm */\

    mov           fp, #7;                                                     \

    orr           fp, lr, fp, lsl #16;      /* set vector lenght to 8 */      \

    fmxr          fpscr, fp;                                                  \

  vfp_ ## fname ## _loop2:                                                    \

    fldmias       r1!, {s8, s9, s10, s11, s12, s13, s14, s15};                \

    ## finst ##s  s24, s8, s1;                                                \

    fstmias       r0!, {s24, s25, s26, s27, s28, s29, s30, s31};              \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop2;                                    \

    fmxr          fpscr, lr;                /* restore original fpscr */      \

  vfp_ ## fname ## _end:                                                      \

    ldmia         sp!, {fp, pc};        /* recovering from stack and return */   

#define UNROLL_F64_TEMPLATE(fname,finst) \

  .global vfp_ ## fname ## ;                                                  \

  vfp_ ## fname ## :                                                          \

    stmdb         sp!, {fp, lr};            /* save registers to stack */     \

    fldmiad       r2, {d1};                 /* load scalar value */           \

    ands          ip, r3, #3;               /* ip = n % 3 */                  \

    beq           vfp_ ## fname ## _unroll; /* if ip == 0 goto prep_loop2 */  \

  vfp_ ## fname ## _loop1:                                                    \

    fldmiad       r1!, {d0};                                                  \

    ## finst ##d  d2, d0, d1;                                                 \

    fstmiad       r0!, {d2};                                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop1;                                    \

  vfp_ ## fname ## _unroll:                 /* unroll by 4 */                 \

    movs          ip, r3, lsr #2;           /* ip = n / 4 */                  \

    beq           vfp_ ## fname ## _end;    /* if ip == 0 goto finish */      \

    fmrx          lr, fpscr;                /* read fpscr register into arm */\

    mov           fp, #3;                                                     \

    orr           fp, lr, fp, lsl #16;      /* set vector lenght to 4 */      \

    fmxr          fpscr, fp;                                                  \

  vfp_ ## fname ## _loop2:                                                    \

    fldmiad       r1!, {d4, d5, d6, d7};                                      \

    ## finst ##d  d12, d4, d1;                                                \

    fstmiad       r0!, {d12, d13, d14, d15};                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop2;                                    \

    fmxr          fpscr, lr;                /* restore original fpscr */      \

  vfp_ ## fname ## _end:                                                      \

    ldmia         sp!, {fp, pc};        /* recovering from stack and return */   

UNROLL_F32_TEMPLATE(scalaradd_f32_ns,fadd);

UNROLL_F64_TEMPLATE(scalaradd_f64_ns,fadd);

UNROLL_F32_TEMPLATE(scalarmultiply_f32_ns,fmul);

UNROLL_F64_TEMPLATE(scalarmultiply_f64_ns,fmul);

#undef UNROLL_F32_TEMPLATE

#undef UNROLL_F64_TEMPLATE

/* 

**

** void vfp_abs_f32_f32_ns(float *d, const float *s, int n);

** void vfp_abs_f64_f64_ns(double *d, const double *s, int n);

** void vfp_negative_f32(float *d, const float *s, int n);

** void vfp_negative_f64(double *d, const double *s, int n);

**

** d:   $r0     |   s: $r1      |   n:  $r2     |

**

*/

#define UNROLL_F32_TEMPLATE(fname,finst) \

  .global vfp_ ## fname ## ;                                                  \

  vfp_ ## fname ## :                                                          \

    stmdb         sp!, {fp, lr};            /* save registers to stack */     \

    ands          ip, r2, #7;               /* ip = n % 8 */                  \

    beq           vfp_ ## fname ## _unroll; /* if ip == 0 goto prep_loop2 */  \

  vfp_ ## fname ## _loop1:                                                    \

    fldmias       r1!, {s0};                                                  \

    ## finst ##s  s2, s0;                                                     \

    fstmias       r0!, {s2};                                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop1;                                    \

  vfp_ ## fname ## _unroll:                 /* unroll by 8 */                 \

    movs          ip, r2, lsr #3;           /* ip = n / 8 */                  \

    beq           vfp_ ## fname ## _end;    /* if ip == 0 goto finish */      \

    fmrx          lr, fpscr;                /* read fpscr register into arm */\

    mov           fp, #7;                                                     \

    orr           fp, lr, fp, lsl #16;      /* set vector lenght to 8 */      \

    fmxr          fpscr, fp;                                                  \

  vfp_ ## fname ## _loop2:                                                    \

    fldmias       r1!, {s8, s9, s10, s11, s12, s13, s14, s15};                \

    ## finst ##s  s24, s8;                                                    \

    fstmias       r0!, {s24, s25, s26, s27, s28, s29, s30, s31};              \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop2;                                    \

    fmxr          fpscr, lr;                /* restore original fpscr */      \

  vfp_ ## fname ## _end:                                                      \

    ldmia         sp!, {fp, pc};        /* recovering from stack and return */   

#define UNROLL_F64_TEMPLATE(fname,finst) \

  .global vfp_ ## fname ## ;                                                  \

  vfp_ ## fname ## :                                                          \

    stmdb         sp!, {fp, lr};            /* save registers to stack */     \

    ands          ip, r2, #3;               /* ip = n % 3 */                  \

    beq           vfp_ ## fname ## _unroll; /* if ip == 0 goto prep_loop2 */  \

  vfp_ ## fname ## _loop1:                                                    \

    fldmiad       r1!, {d0};                                                  \

    ## finst ##d  d2, d0;                                                     \

    fstmiad       r0!, {d2};                                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop1;                                    \

  vfp_ ## fname ## _unroll:                 /* unroll by 4 */                 \

    movs          ip, r2, lsr #2;           /* ip = n / 4 */                  \

    beq           vfp_ ## fname ## _end;    /* if ip == 0 goto finish */      \

    fmrx          lr, fpscr;                /* read fpscr register into arm */\

    mov           fp, #3;                                                     \

    orr           fp, lr, fp, lsl #16;      /* set vector lenght to 4 */      \

    fmxr          fpscr, fp;                                                  \

  vfp_ ## fname ## _loop2:                                                    \

    fldmiad       r1!, {d4, d5, d6, d7};                                      \

    ## finst ##d  d12, d4;                                                    \

    fstmiad       r0!, {d12, d13, d14, d15};                                  \

    subs          ip, ip, #1;                                                 \

    bne           vfp_ ## fname ## _loop2;                                    \

    fmxr          fpscr, lr;                /* restore original fpscr */      \

  vfp_ ## fname ## _end:                                                      \

    ldmia         sp!, {fp, pc};        /* recovering from stack and return */   

UNROLL_F32_TEMPLATE(abs_f32_f32_ns,fabs);

UNROLL_F64_TEMPLATE(abs_f64_f64_ns,fabs);

UNROLL_F32_TEMPLATE(negative_f32,fneg);

UNROLL_F64_TEMPLATE(negative_f64,fneg);

#undef UNROLL_F32_TEMPLATE

#undef UNROLL_F64_TEMPLATE

#endif