#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <inttypes.h>
#include <sys/mman.h>
#include <time.h>

#ifndef PAGE_SIZE
#define PAGE_SIZE 		(4*1024)
#endif
#ifndef BLOCK_SIZE
#define BLOCK_SIZE 		(4*1024)
#endif

#define BCM2708_PERI_BASE       0xFE000000
#define SYST_BASE 		(BCM2708_PERI_BASE + 0x3000)
#define SYST_CS  0
#define SYST_CLO 1
#define SYST_CHI 2

struct bcm2835_peripheral {
	unsigned long addr_p;
	int mem_fd;
	void *map;
	volatile unsigned int *addr;
};

struct bcm2835_peripheral syst = {SYST_BASE};

unsigned short map_peripheral(struct bcm2835_peripheral *p)
{
	if (geteuid() != 0)
	{
		printf("Error, you must be root!\n");

		return 0;
	}

	// Open /dev/mem
	if ((p->mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0)
	{
		printf("Failed to open /dev/mem, try checking permissions.\n");
		return 0;
	}

	p->map = mmap(
		 NULL,
		 BLOCK_SIZE,
		 PROT_READ|PROT_WRITE,
		 MAP_SHARED,
		 p->mem_fd,  // File descriptor to physical memory virtual file '/dev/mem'
		 p->addr_p      // Address in physical map that we want this memory block to expose
	);

	if (p->map == MAP_FAILED) {
		perror("mmap");
		return 0;
	}

	p->addr = (volatile unsigned int *)p->map;

	return 1;
}

void unmap_peripheral(struct bcm2835_peripheral *p)
{
	munmap(p->map, BLOCK_SIZE);
	p->map = MAP_FAILED;
	close(p->mem_fd);
}

uint64_t nanos(void)
{
	struct timespec ts;
	timespec_get(&ts, TIME_UTC);
	return (uint64_t)ts.tv_nsec;
}

uint32_t gpioTick(void)
{
	return syst.addr[SYST_CLO];
}

int main(void)
{
	uint8_t i = 0;
	uint32_t ticks = 0;
	uint32_t rr = 0;

	if(!map_peripheral(&syst))
	{
		printf("Failed to map the physical SYST registers into the virtual memory space.\n");
		return -1;
	}

	ticks = gpioTick();

	for (i=0; i<100; ++i)
	{
		rr = gpioTick() - ticks;
	}

	printf("%d\n", rr);

	unmap_peripheral(&syst);

	return 0;
}