#if __ARMCC__

#define __CPU_ARM 

#define __CPU_HAS_VFP

#include <arm_vfp.h>

#include <e32std.h>

extern "C" {

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

    {

    asm(" stmdb sp!, {fp, lr}"); 

    asm("ands ip, r3, #7"); 

    asm("beq vfp_add_f32_unroll");

   //asm("fldmias r1!, {s0}"); 

   VFP_FLDMIAS(CC_AL,1,0,1);

   asm("vfp_add_f32_loop1: ");

   //asm("fldmias r2!, {s1}");    

     VFP_FLDMIAS(CC_AL,2,1,1);

    //asm("fadds s2, s0, s1");

    VFP_FADDS(CC_AL,2,0,1);

    //asm("fstmias r0!, {s2}");

    VFP_FSTMIAS(CC_AL,0,2,1);   

    asm("subs ip, ip, #1"); 

    asm("bne vfp_add_f32_loop1 ");

	asm("vfp_add_f32_unroll: movs ip, r3, lsr #3"); 

    asm("beq vfp_add_f32_end");

    //asm("fmrx lr, fpscr");  

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov fp, #7"); 

    asm("orr fp, lr, fp, lsl #16"); 

    //asm("fmxr fpscr, fp"); 

    VFP_FMXR(,VFP_XREG_FPSCR,11);

    asm("vfp_add_f32_loop2:");

    //asm("fldmias r1!, {s8, s9, s10, s11, s12, s13, s14, s15}"); 

        VFP_FLDMIAS(CC_AL,1,8,8);   

    //asm("fldmias r2!, {s16, s17, s18, s19, s20, s21, s22, s23}");

    VFP_FLDMIAS(CC_AL,2,16,8);

    //asm("fadds s24, s8, s16"); 

        VFP_FADDS(CC_AL,24,8,16);      

    asm("subs ip, ip, #1"); 

    asm("bne vfp_add_f32_loop2"); 

    //asm("fmxr fpscr, lr"); 

    VFP_FMXR(,VFP_XREG_FPSCR,14);

   asm("vfp_add_f32_end:");

   asm ("ldmia sp!, {fp, pc}");

    }

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

    {

    asm(" stmdb sp!, {fp, lr}"); 

    asm("ands ip, r3, #7"); 

    asm("beq vfp_divide_f32_unroll");

   //asm("fldmias r1!, {s0}"); 

   VFP_FLDMIAS(CC_AL,1,0,1);

   asm("vfp_divide_f32_loop1:");

   //asm("fldmias r2!, {s1}");    

     VFP_FLDMIAS(CC_AL,2,1,1);

    //asm("fadds s2, s0, s1");

    VFP_FDIVS(CC_AL,2,0,1);

    //asm("fstmias r0!, {s2}");

    VFP_FSTMIAS(CC_AL,0,2,1);   

    asm("subs ip, ip, #1"); 

    asm("bne vfp_divide_f32_loop1");

    asm("vfp_divide_f32_unroll: movs ip, r3, lsr #3"); 

    asm("beq vfp_divide_f32_end");

    //asm("fmrx lr, fpscr");  

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov fp, #7"); 

    asm("orr fp, lr, fp, lsl #16"); 

    //asm("fmxr fpscr, fp"); 

    VFP_FMXR(,VFP_XREG_FPSCR,11);

    asm("vfp_divide_f32_loop2:");

    //asm("fldmias r1!, {s8, s9, s10, s11, s12, s13, s14, s15}"); 

        VFP_FLDMIAS(CC_AL,1,8,8);   

    //asm("fldmias r2!, {s16, s17, s18, s19, s20, s21, s22, s23}");

    VFP_FLDMIAS(CC_AL,2,16,8);

    //asm("fadds s24, s8, s16"); 

        VFP_FDIVS(CC_AL,24,8,16);      

    asm("subs ip, ip, #1"); 

    asm("bne vfp_divide_f32_loop2"); 

    //asm("fmxr fpscr, lr"); 

    VFP_FMXR(,VFP_XREG_FPSCR,14);

   asm("vfp_divide_f32_end:");

   asm ("ldmia sp!, {fp, pc}");

    }

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

    {

    asm(" stmdb sp!, {fp, lr}"); 

    asm("ands ip, r3, #7"); 

    asm("beq vfp_multiply_f32_unroll");

   //asm("fldmias r1!, {s0}"); 

   VFP_FLDMIAS(CC_AL,1,0,1);

   asm("vfp_multiply_f32_loop1:");

   //asm("fldmias r2!, {s1}");    

     VFP_FLDMIAS(CC_AL,2,1,1);

    //asm("fadds s2, s0, s1");

    VFP_FMULS(CC_AL,2,0,1);

    //asm("fstmias r0!, {s2}");

    VFP_FSTMIAS(CC_AL,0,2,1);   

    asm("subs ip, ip, #1"); 

    asm("bne vfp_multiply_f32_loop1");

    asm("vfp_multiply_f32_unroll: movs ip, r3, lsr #3"); 

    asm("beq vfp_multiply_f32_end");

    //asm("fmrx lr, fpscr");  

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov fp, #7"); 

    asm("orr fp, lr, fp, lsl #16"); 

    //asm("fmxr fpscr, fp"); 

    VFP_FMXR(,VFP_XREG_FPSCR,11);

    asm("vfp_multiply_f32_loop2:");

    //asm("fldmias r1!, {s8, s9, s10, s11, s12, s13, s14, s15}"); 

        VFP_FLDMIAS(CC_AL,1,8,8);   

    //asm("fldmias r2!, {s16, s17, s18, s19, s20, s21, s22, s23}");

    VFP_FLDMIAS(CC_AL,2,16,8);

    //asm("fadds s24, s8, s16"); 

        VFP_FMULS(CC_AL,24,8,16);      

    asm("subs ip, ip, #1"); 

    asm("bne vfp_multiply_f32_loop2"); 

    //asm("fmxr fpscr, lr"); 

    VFP_FMXR(,VFP_XREG_FPSCR,14);

   asm("vfp_multiply_f32_end:");

   asm ("ldmia sp!, {fp, pc}");

    }

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

    {

    asm(" stmdb sp!, {fp, lr}"); 

    asm("ands ip, r3, #7"); 

    asm("beq vfp_subtract_f32_unroll");

   //asm("fldmias r1!, {s0}"); 

   VFP_FLDMIAS(CC_AL,1,0,1);

   asm("vfp_subtract_f32_loop1:");

   //asm("fldmias r2!, {s1}");    

     VFP_FLDMIAS(CC_AL,2,1,1);

    //asm("fadds s2, s0, s1");

    VFP_FSUBS(CC_AL,2,0,1);

    //asm("fstmias r0!, {s2}");

    VFP_FSTMIAS(CC_AL,0,2,1);   

    asm("subs ip, ip, #1"); 

    asm("bne vfp_subtract_f32_loop1");

    asm("vfp_subtract_f32_unroll: movs ip, r3, lsr #3"); 

    asm("beq vfp_subtract_f32_end");

    //asm("fmrx lr, fpscr");  

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov fp, #7"); 

    asm("orr fp, lr, fp, lsl #16"); 

    //asm("fmxr fpscr, fp"); 

    VFP_FMXR(,VFP_XREG_FPSCR,11);

    asm("vfp_subtract_f32_loop2:");

    //asm("fldmias r1!, {s8, s9, s10, s11, s12, s13, s14, s15}"); 

        VFP_FLDMIAS(CC_AL,1,8,8);   

    //asm("fldmias r2!, {s16, s17, s18, s19, s20, s21, s22, s23}");

    VFP_FLDMIAS(CC_AL,2,16,8);

    //asm("fadds s24, s8, s16"); 

        VFP_FSUBS(CC_AL,24,8,16);      

    asm("subs ip, ip, #1"); 

    asm("bne vfp_subtract_f32_loop2"); 

    //asm("fmxr fpscr, lr"); 

    VFP_FMXR(,VFP_XREG_FPSCR,14);

   asm("vfp_subtract_f32_end:");

   asm ("ldmia sp!, {fp, pc}");

    }

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

{

    asm("stmdb         sp!, {fp, lr}");            /* save registers to stack */     

    asm("ands          ip, r3, #3");               /* ip = n % 3 */                 

    asm("beq           vfp_add_f64_unroll"); /* if ip == 0 goto prep_loop2 */ 

    asm("vfp_add_f64_loop1:");                                                   

    //asm("fldmiad       r1!, {d0}");   

    VFP_FLDMIAD(CC_AL,1,0,1);

    //asm("fldmiad       r2!, {d1}");       

    VFP_FLDMIAD(CC_AL,2,1,1);         

    //asm("faddd  d2, d0, d1");       

    VFP_FADDD(,2,0,1);

    //asm("fstmiad       r0!, {d2}");     

    VFP_FSTMIAD(CC_AL,0,2,1);                                                     

    asm("subs          ip, ip, #1");                                                

    asm("bne           vfp_add_f64_loop1");                                   

    asm("vfp_add_f64_unroll:");                  /* unroll by 4 */                

    asm("movs          ip, r3, lsr #2");           /* ip = n / 4 */                 

    asm("  beq           vfp_add_f64_end");    /* if ip == 0 goto finish */     

    //asm("  fmrx          lr, fpscr");                /* read fpscr register into arm */

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov           fp, #3");                                                    

    asm("orr           fp, lr, fp, lsl #16");      /* set vector lenght to 8 */     

    //asm("fmxr          fpscr, fp");      

    VFP_FMXR(,VFP_XREG_FPSCR,11);    

    asm("vfp_add_f64_loop2:");                                                    

    //asm("fldmiad       r1!, {d4, d5, d6, d7}");    

    VFP_FLDMIAS(CC_AL,1,4,4);                                 

    //asm("fldmiad       r2!, {d8, d9, d10, d11}");                                    

    VFP_FLDMIAS(CC_AL,2,8,4);                                 

    //asm("faddd  d12, d4, d8");                                                

    VFP_FADDD(,12,4,8);

    //asm("fstmiad       r0!, {d12, d13, d14, d15}");                                  

    VFP_FSTMIAS(CC_AL,0,12,4);                                 

    asm("subs          ip, ip, #1");                                                

    asm("bne           vfp_add_f64_loop2");                                   

    //asm("fmxr          fpscr, lr");                /* restore original fpscr */      

    VFP_FMXR(,VFP_XREG_FPSCR,14);

    asm("vfp_add_f64_end:");                                                      

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

}     

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

    {                                                         

    asm("stmdb         sp!, {fp, lr}");            /* save registers to stack */    

    asm("ands          ip, r2, #7");               /* ip = n % 8 */                 

    asm("beq           vfp_abs_f32_f32_ns_unroll"); /* if ip == 0 goto prep_loop2 */ 

  	asm("vfp_abs_f32_f32_ns_loop1:");                                                   

    //asm("fldmias       r1!, {s0}");  

    VFP_FLDMIAS(CC_AL,1,0,1);

    //asm("fabss  s2, s0");  

    VFP_FABSS(CC_AL,2,0);

    //asm("fstmias       r0!, {s2}");                                                 

    VFP_FSTMIAS(CC_AL,0,2,1);   

    asm("subs          ip, ip, #1");                                                

    asm("bne           vfp_abs_f32_f32_ns_loop1");                                   

  	asm("vfp_abs_f32_f32_ns_unroll:");                 /* unroll by 8 */                

    asm("movs          ip, r2, lsr #3");           /* ip = n / 8 */                 

    asm("beq           vfp_abs_f32_f32_ns_end");    /* if ip == 0 goto finish */     

    //asm("fmrx          lr, fpscr");                /* read fpscr register into arm */

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov           fp, #7");                                                    

    asm("orr           fp, lr, fp, lsl #16");      /* set vector lenght to 8 */     

    //asm("fmxr          fpscr, fp");                                                 

  	VFP_FMXR(,VFP_XREG_FPSCR,11); 

  	asm("vfp_abs_f32_f32_ns_loop2:");                                                   

    //asm("fldmias       r1!, {s8, s9, s10, s11, s12, s13, s14, s15}"); 

    VFP_FLDMIAS(CC_AL,1,8,8);

    //asm("fabss  s24, s8");                                                   

    VFP_FABSS(CC_AL,2,0);

    //asm("fstmias       r0!, {s24, s25, s26, s27, s28, s29, s30, s31}");             

    VFP_FSTMIAS(CC_AL,0,24,8);

    asm("subs          ip, ip, #1");                                                

    asm("bne           vfp_abs_f32_f32_ns_loop2");                                   

    //asm("fmxr          fpscr, lr");                /* restore original fpscr */      

  	VFP_FMXR(,VFP_XREG_FPSCR,14);

  	asm("vfp_abs_f32_f32_ns_end:");                                                      

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

	} 

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

    {                                                         

    asm("stmdb         sp!, {fp, lr}");            /* save registers to stack */    

    asm("ands          ip, r2, #7");               /* ip = n % 8 */                 

    asm("beq           vfp_negative_f32_unroll"); /* if ip == 0 goto prep_loop2 */ 

  	asm("vfp_negative_f32_loop1:");                                                   

    //asm("fldmias       r1!, {s0}"); 

    VFP_FLDMIAS(CC_AL,1,0,1);

    //asm("fnegs  s2, s0");                                                    

    VFP_FNEGS(CC_AL,2,0);

    //asm("fstmias       r0!, {s2}");                                                 

    VFP_FSTMIAS(CC_AL,0,2,1); 

    asm("subs          ip, ip, #1");                                                

    asm("bne           vfp_negative_f32_loop1");                                   

  	asm("vfp_negative_f32_unroll:");                 /* unroll by 8 */                

    asm("movs          ip, r2, lsr #3");           /* ip = n / 8 */                 

    asm("beq           vfp_negative_f32_end");    /* if ip == 0 goto finish */     

    //asm("fmrx          lr, fpscr");                /* read fpscr register into arm */

    VFP_FMRX(,14,VFP_XREG_FPSCR);

    asm("mov           fp, #7");                                                    

    asm("orr           fp, lr, fp, lsl #16");      /* set vector lenght to 8 */     

   // asm("fmxr          fpscr, fp");                                                 

  	VFP_FMXR(,VFP_XREG_FPSCR,11); 

  	asm("vfp_negative_f32_loop2:");                                                   

    //asm("fldmias       r1!, {s8, s9, s10, s11, s12, s13, s14, s15}"); 

    VFP_FLDMIAS(CC_AL,1,8,8);

    //asm("fnegs  s24, s8");                                                   

    VFP_FNEGS(CC_AL,2,0);

    //asm("fstmias       r0!, {s24, s25, s26, s27, s28, s29, s30, s31}");   

    VFP_FSTMIAS(CC_AL,0,24,8);

    asm("subs          ip, ip, #1");                                                

    asm("bne           vfp_negative_f32_loop2");           

    //asm("fmxr          fpscr, lr");                /* restore original fpscr */      

  	VFP_FMXR(,VFP_XREG_FPSCR,14);

  	asm("vfp_negative_f32_end:");                                                      

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

	} 

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

	{                                                       

   asm("stmdb         sp!, {fp, lr}");            /* save registers to stack */    

   asm("ands          ip, r2, #3");               /* ip = n % 3 */                 

   asm("beq           vfp_abs_f64_f64_ns_unroll"); /* if ip == 0 goto prep_loop2 */ 

   asm("vfp_abs_f64_f64_ns_loop1:");                                                   

   //asm("fldmiad       r1!, {d0}"); 

   VFP_FLDMIAD(CC_AL,1,0,1);

   //asm("fabsd  d2, d0"); 

   VFP_FABSD(,2,0);

   //asm("fstmiad       r0!, {d2}");                                                 

   VFP_FSTMIAD(CC_AL,0,2,1);  

   asm("subs          ip, ip, #1");                                                

   asm("bne           vfp_abs_f64_f64_ns_loop1");                                   

   asm("vfp_abs_f64_f64_ns_unroll:");                 /* unroll by 4 */                

   asm("movs          ip, r2, lsr #2");           /* ip = n / 4 */                 

   asm("beq           vfp_abs_f64_f64_ns_end");    /* if ip == 0 goto finish */     

   //asm("fmrx          lr, fpscr");                /* read fpscr register into arm */

 	 VFP_FMRX(,14,VFP_XREG_FPSCR);

   asm("mov           fp, #3");                                                    

   asm("orr           fp, lr, fp, lsl #16");      /* set vector lenght to 4 */     

   //asm("fmxr          fpscr, fp");                                                 

   VFP_FMXR(,VFP_XREG_FPSCR,11); 

   asm("vfp_abs_f64_f64_ns_loop2:");                                                   

   //asm("fldmiad       r1!, {d4, d5, d6, d7}");                                     

   VFP_FLDMIAD(CC_AL,1,4,4);

   //asm("fabsd  d12, d4");   

   VFP_FABSD(,12,4);

   //asm("fstmiad       r0!, {d12, d13, d14, d15}");                                 

   VFP_FSTMIAD(CC_AL,0,12,4);

   asm("subs          ip, ip, #1");                                                

   asm("bne           vfp_abs_f64_f64_ns_loop2");                                   

  // asm("fmxr          fpscr, lr");                /* restore original fpscr */     

   	VFP_FMXR(,VFP_XREG_FPSCR,14);

   asm("vfp_abs_f64_f64_ns_end:");                                                     

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

	}

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

	{                                                       

   asm("stmdb         sp!, {fp, lr}");            /* save registers to stack */    

   asm("ands          ip, r2, #3");               /* ip = n % 3 */                 

   asm("beq           vfp_negative_f64_unroll"); /* if ip == 0 goto prep_loop2 */ 

   asm("vfp_negative_f64_loop1:");                                                   

   //asm("fldmiad       r1!, {d0}");                                                 

   VFP_FLDMIAD(CC_AL,1,0,1);

   //asm("fnegd  d2, d0");                                                    

   VFP_FNEGD(,2,0);

   //asm("fstmiad       r0!, {d2}");                                                 

   VFP_FSTMIAD(CC_AL,0,2,1);

   asm("subs          ip, ip, #1");                                                

   asm("bne           vfp_negative_f64_loop1");                                   

   asm("vfp_negative_f64_unroll:");                 /* unroll by 4 */                

   asm("movs          ip, r2, lsr #2");           /* ip = n / 4 */                 

   asm("beq           vfp_negative_f64_end");    /* if ip == 0 goto finish */     

   //asm("fmrx          lr, fpscr");                /* read fpscr register into arm */

   VFP_FMRX(,14,VFP_XREG_FPSCR);

   asm("mov           fp, #3");                                                    

   asm("orr           fp, lr, fp, lsl #16");      /* set vector lenght to 4 */     

   //asm("fmxr          fpscr, fp");                                                 

   VFP_FMXR(,VFP_XREG_FPSCR,11); 

   asm("vfp_negative_f64_loop2:");           

   //asm("fldmiad       r1!, {d4, d5, d6, d7}");   

   VFP_FLDMIAD(CC_AL,1,4,4);

   //asm("fnegd  d12, d4"); 

   VFP_FNEGD(,12,4);

   //asm("fstmiad       r0!, {d12, d13, d14, d15}");                                 

   VFP_FSTMIAD(CC_AL,0,12,4);

   asm("subs          ip, ip, #1");                                                

   asm("bne           vfp_negative_f64_loop2");                                   

   //asm("fmxr          fpscr, lr");                /* restore original fpscr */     

   VFP_FMXR(,VFP_XREG_FPSCR,14);

   asm("vfp_negative_f64_end:");                                                     

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

	}

//Rakhi changes

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

{

    asm("stmdb         sp!, {fp, lr}");            /* save registers to stack */     

    asm("ands          ip, r3, #3");               /* ip = n % 3 */                 

    asm("beq           vfp_divide_f64_unroll"); /* if ip == 0 goto prep_loop2 */ 

    asm("vfp_divide_f64_loop1:");                                                   

    //asm("fldmiad       r1!, {d0}");