// gcc -O3 -mcpu=cortex-a15 -Wall bbai-test.c -o bbai-test
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <time.h>
#include <sys/mman.h>

#define LOOPCNT 100 

#define GPIO_START_ADDR 0x48059000 // gpio4
#define GPIO_SIZE 408
#define GPIO_OE 0x134
#define GPIO_SETDATAOUT 0x194
#define GPIO_CLEARDATAOUT 0x190

#define P8_14 (1<<13) // gpio4_13

struct timespec sleep_time_1ns = {0, 1};

// https://github.com/openbsd/src/blob/master/sys/sys/time.h
#define timespecsub(tsp, usp, vsp)                                      \
        do {                                                            \
                (vsp)->tv_sec = (tsp)->tv_sec - (usp)->tv_sec;          \
                (vsp)->tv_nsec = (tsp)->tv_nsec - (usp)->tv_nsec;       \
                if ((vsp)->tv_nsec < 0) {                               \
                        (vsp)->tv_sec--;                                \
                        (vsp)->tv_nsec += 1000000000L;                  \
                }                                                       \
        } while (0)


int main(int argc, char *argv[])
{
	unsigned int n;
	int testno = 0;
	struct timespec time_start, time_stop, time_elapsed;
	long long timediffs[LOOPCNT];
	volatile void *gpio_addr = NULL;
	volatile unsigned int *gpio_oe_addr = NULL;
	volatile unsigned int *gpio_setdataout_addr = NULL;
	volatile unsigned int *gpio_cleardataout_addr = NULL;
	unsigned int reg;

	if (argc != 3 || strcmp(argv[1], "-t") != 0) {
		printf("Usage: sudo %s -t Test# \n", argv[0]);
		exit(EXIT_FAILURE);
	}
	testno = atoi(argv[2]);

	// https://github.com/chiragnagpal/beaglebone_mmap
	int fdgpio = open("/dev/mem", O_RDWR);
	gpio_addr = mmap(0, GPIO_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fdgpio, GPIO_START_ADDR);
	gpio_oe_addr = gpio_addr + GPIO_OE;
	gpio_setdataout_addr = gpio_addr + GPIO_SETDATAOUT;
	gpio_cleardataout_addr = gpio_addr + GPIO_CLEARDATAOUT;

	if(gpio_addr == MAP_FAILED) {
		perror("Unable to map GPIO\n");
		exit(EXIT_FAILURE);
	}

	reg = *gpio_oe_addr;
	reg = reg & (0xFFFFFFFF - P8_14);
	*gpio_oe_addr = reg;

	if (testno == 1) {
	   printf("Test %i: Time difference measurement\n", testno);
	   for (n=0; n<LOOPCNT; n++) {
		clock_gettime(CLOCK_MONOTONIC, &time_start);
		// do soemthing ...
		*gpio_setdataout_addr= P8_14;
		*gpio_cleardataout_addr = P8_14;
		clock_gettime(CLOCK_MONOTONIC, &time_stop);
		// calculate the time difference
		timespecsub(&time_stop, &time_start, &time_elapsed);
		timediffs[n] = (long long)(time_elapsed.tv_sec*1e9+time_elapsed.tv_nsec);
	   }

	   for (n=0; n<LOOPCNT; n++)
		printf("%3i elapsed time: %llu ns\n",  n, timediffs[n]);
	};

	if (testno == 2) {
	   printf("Test %i: clock_gettime() duration @ P8_14\n", testno);
           for (n=0; n<LOOPCNT; n++) {
		*gpio_setdataout_addr= P8_14;
		clock_gettime(CLOCK_MONOTONIC, &time_start);
		*gpio_cleardataout_addr = P8_14;
		clock_gettime(CLOCK_MONOTONIC, &time_start);
	   }
	};

	if (testno == 3) {
	   printf("Test %i: clock_nanosleep(1ns) duration @ P8_14\n", testno);
           for (n=0; n<LOOPCNT; n++) {
		*gpio_setdataout_addr= P8_14;
		clock_nanosleep(CLOCK_MONOTONIC, 0, &sleep_time_1ns, NULL);
		*gpio_cleardataout_addr = P8_14;
		clock_nanosleep(CLOCK_MONOTONIC, 0, &sleep_time_1ns, NULL);
	   }
	};

	if (testno == 4) {
	   printf("Test %i: Pin toggle frequency @ P8_14\n", testno);
           for (n=0; n<LOOPCNT; n++) {
		*gpio_setdataout_addr= P8_14;
		*gpio_cleardataout_addr = P8_14;
	   }
	};

	munmap((void *)gpio_addr, GPIO_SIZE * sizeof(uint32_t));
	close(fdgpio);
	return (EXIT_SUCCESS);
}