#include <stdio.h>
#include <stdint.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <string.h>

#include <stdlib.h>
#include <stdio.h>
#include <sys/mman.h>	/* for MCL_CURRENT and MCL_FUTURE */
#include <rtdm/rtdm.h>
#include <native/task.h>
#include <nucleus/trace.h>

#define GPIO6_ON 		*Gpio2ValAddr |= 1 << 6
#define GPIO6_OFF		*Gpio2ValAddr &= ~(1 << 6);

static RT_TASK rt_task_desc;
static RT_TASK tsk2ms;

	//dati per la gestione memoria mappata
static unsigned long * Gpio2ValAddr = NULL;

volatile int cnt2ms = 0;
volatile int cntmain = 0;
volatile int x;

void funct2ms( void * params )
{
	for(;;)
	{
		GPIO6_ON;
		rt_task_sleep( 2000000 );
		++ cnt2ms;
		GPIO6_OFF;
	}
}

int main(int argc, char *argv[])
{
	int count = 0;
	int traceOn = 0;
	int fd, ret;

	// no memory-swapping for this programm
	ret = mlockall(MCL_CURRENT | MCL_FUTURE);
	if( ret )
	{
		perror("ERROR : mlockall has failled");
		exit(1);
	}


	fd = open( "/dev/mem", O_RDWR | O_SYNC );
	printf( "FD=%d", fd );
	fflush( stdout );

	Gpio2ValAddr = (unsigned long  *)mmap(  NULL, 0x1000, PROT_READ | PROT_WRITE,  MAP_SHARED, fd, 0x53FD0000 );

	ret = rt_task_shadow(&rt_task_desc, NULL, 1, 0);
	if( ret != 0 )
	{
		fprintf(stderr, "ERROR : rt_task_shadow: %s\n", strerror(-ret));
		exit( 1 );
	}

	rt_task_create( &tsk2ms, "task2ms",0, 99, 0 );
	rt_task_start( &tsk2ms, funct2ms, NULL );


	for(;;)
	{
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		xntrace_user_freeze(0, 0);
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		rt_task_sleep( 10000000 );
	}

	return 0;
}