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

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/mman.h>	/* for MCL_CURRENT and MCL_FUTURE */


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

pthread_t task1;
pthread_t task2;

void CreateRunTask( pthread_t * threadPtr, int prio, void * fnz )
{
	pthread_attr_t thread_attr;
	struct sched_param sched;
	
	pthread_attr_init(&thread_attr);
	
	pthread_attr_setschedpolicy(&thread_attr, SCHED_RR);
	sched.sched_priority = prio;
	pthread_attr_setschedparam(&thread_attr, &sched);

	pthread_create(threadPtr, &thread_attr, fnz, NULL );
	pthread_attr_destroy(&thread_attr);
}

	//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 )
{
	struct timespec ts;
    ts.tv_sec = 0;
    ts.tv_nsec = 2000000;
	
	for(;;)
	{
		GPIO6_ON;
		nanosleep( &ts, NULL );
		++ cnt2ms;
		GPIO6_OFF;
	}
}

void funct10ms( void * params )
{
	struct timespec ts;
    ts.tv_sec = 0;
    ts.tv_nsec = 10000000;

	for( ;;)
	{
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		for( x=0; x < 1000000; x++ );
		
		nanosleep( &ts, NULL );
	}
}

int main(int argc, char *argv[])
{
	int count = 0;
	int traceOn = 0;
	int fd, ret;
	unsigned long reg, reg1;
	int device;
	unsigned long memShared=0;

	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 );
	GPIO6_ON;
	GPIO6_OFF;

	CreateRunTask( &task1, 99, funct2ms );
	CreateRunTask( &task2, 1, funct10ms );

	while( 1 )
		sleep( 1 );

	return 0;
}