/*!
 * @file mx6_fpu.c
 *
 * @brief Simple FPU 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;
}

/*!
 * @brief Entry point
 */
int main(void)
{
	int ii;
	float x = 0.1, y = 2.9, z = 0.0;

	init_fpu();

	z = (x + y);

	if (z == 3.0)		// Set compare flag in FPSCR
		x = 0.9, y = 1.1;
	else
		z = 0;

	if (z == 1)
		x = 0;
	else
		y = 0;

	x = 0.5;
	for (ii = 0; ii < 100; ii++)
	{
		x += ii;
		y += x * 3;
	}

	return 0;
}