/*!
 * @file mx6_neon.c
 *
 * @brief Simple NEON test program 
 *
 *  Created on: Jan 25, 2019
 *      Author: Ilko Iliev
 *
 * @note Copyright (c) Ronetix GmbH. All rights reserved.
 */

static int init_fpu(void)
{			
	__asm__(
            "mrc p15, 0, r1, c1, c1, 2;"

/*NSACR*/	"orr r1, r1, #(0x3<<10);"
          //"mov r0, #0xCC00;"
            "mcr p15, 0, r1, c1, c1, 2;"

            "MRC  p15,0,r0,c1,c0,2;" // Read CPACR into r0
            "ORR  r0,r0,#(3<<20);"   // OR in User and Privileged access for CP10
            "ORR  r0,r0,#(3<<22);"   // OR in User and Privileged access for CP11
			"BIC  r0, r0, #(3<<30);" // Clear ASEDIS/D32DIS if set
			"MCR  p15,0,r0,c1,c0,2;" // Store new access permissions into CPACR
            "ISB;"                   // Ensure side-effect of CPACR is visible
            "MOV  r0,#(1<<30);"	 	 // Create value with FPEXC (bit 30) set in r0
            "VMSR FPEXC,r0;"         // Enable VFP and SIMD extensions
#if 0
/*CPACR*/	"ldr r1, =(0xF << 20);"
                "mcr p15, 0, r1, c1, c0, 2;"

/*FPEXC*/       "mov r1, #0x40000000;"
//              "vmrs r2, FPEXC;"
                "vmsr FPEXC, r1;"
//              "vmrs r0, FPEXC;"

///*MVFR0*/     "vmrs r1, MVFR0;"
///*FPSCR*/     "vmrs r5, FPSCR;"
#endif
	);
  return 0;
}

typedef float vec4f __attribute__ ((vector_size (16)));

/* Accessor macros for each component */
#define VEC4F_C0(v) (((float *)&(v))[0])
#define VEC4F_C1(v) (((float *)&(v))[1])
#define VEC4F_C2(v) (((float *)&(v))[2])
#define VEC4F_C3(v) (((float *)&(v))[3])

/*!
 * @brief Entry point
 */
int main()
{
	init_fpu();

	vec4f a = { 0, 1, 2, 3 };
	vec4f b = { 1, 2, 3, 1 };
	vec4f t1, t2;

	t1 = a + b;
	t2 = (t1 - (b + b));
	t1 = (t1 * t2);
	t2 = (t1 / a);

	while (1)
	{
		t1 += t2;
	}

	return 0;
}