/* Copyright 2018 Raptor Engineering, LLC
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

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

#define SYSCTL_BASE	0x1e6e2000
#define SYSCTL_SCU88	0x88
#define SYSCTL_PWM7	7

#define PWM_BASE	0x1e786000
#define PTCR40		0x40
#define PTCR44		0x44
#define PTCR4C		0x4c

static void *sysctl_reg = NULL;
static void *pwm_reg = NULL;
static int mem_fd = 0;

// Adapted from ASpeed documentation
uint32_t get_ideal_divisor(double clk_in, double desired_clk) {
	uint32_t div_low = 0;
	uint32_t div_high = 0;
	uint32_t retval = 0;
	uint32_t tmpval2;
	double tmpval;
	double tmpval1;

	if (desired_clk < 1) {
		return 0;
	}

	double divisor = clk_in / desired_clk;
	if (divisor < (0x1 << 8)) {
		return 0;
	}

	tmpval = divisor;
	tmpval2 = ((uint32_t)divisor) >> 8;
	while ((div_high < (0x1 << 4)) && (tmpval2)) {
		div_high++;
		tmpval2 >>= 1;
	}
	if (!(div_high ^ (0x1 << 4))) {
		div_high--;
	}
	tmpval1 = divisor / ((double)(1 << div_high));
	tmpval = tmpval1;
	while (tmpval >= (0x1 << 8));

	retval |= div_low << 0;
	retval |= div_high << 4;
	retval |= ((uint32_t)(tmpval - 1)) << 8;

	return retval;
}

inline void timespec_diff(struct timespec *start, struct timespec *stop, struct timespec *result) {
	if ((stop->tv_nsec - start->tv_nsec) < 0) {
		result->tv_sec = stop->tv_sec - start->tv_sec - 1;
		result->tv_nsec = stop->tv_nsec - start->tv_nsec + 1000000000;
	}
	else {
		result->tv_sec = stop->tv_sec - start->tv_sec;
		result->tv_nsec = stop->tv_nsec - start->tv_nsec;
	}

	return;
}

void delay_for_interval(uint32_t interval) {
	uint32_t abort_count = 0;
	uint64_t delay_ns = interval * 1000;

	struct timespec start_time = {.tv_sec=0,.tv_nsec=0};
	struct timespec current_time = {.tv_sec=0,.tv_nsec=0};
	struct timespec difference_time = {.tv_sec=0,.tv_nsec=0};

	abort_count = 0;
	while (clock_gettime(CLOCK_MONOTONIC_RAW, &start_time) < 0) {
		abort_count++;
		if (abort_count > 10) {
			return;
		}
	}
	while (1) {
		current_time.tv_sec = 0;
		current_time.tv_nsec = 0;
		if (clock_gettime(CLOCK_MONOTONIC_RAW, &current_time) < 0) {
			continue;
		}
		timespec_diff(&start_time, &current_time, &difference_time);
		if (difference_time.tv_nsec > delay_ns) {
			break;
		}
	}

	if (difference_time.tv_nsec > (delay_ns * 2)) {
		printf("OVERRAN timer (wanted %lld, got %lld, start: %lld.%lld end: %lld.%lld)\n", delay_ns, difference_time.tv_nsec, start_time.tv_sec, start_time.tv_nsec, current_time.tv_sec, current_time.tv_nsec);
	}
}

int main(int argc, char* argv[]) {
	uint32_t *offset = 0;
	long unsigned int frequency_hz = 1000;
	long unsigned int duration_ms = 100;
	long unsigned int ast_clock_hz = 24000000;

	if (argc > 1) {
		frequency_hz = atoi(argv[1]);
	}

	if (argc > 2) {
		duration_ms = atoi(argv[2]);
	}

	// Open physical memory device
	if (!mem_fd) {
		mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
		if (mem_fd < 0) {
			perror("Unable to open /dev/mem");
			exit(1);
		}
	}

	// Map sytem control registers into virtual memory
	sysctl_reg = mmap(NULL, getpagesize(), PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, SYSCTL_BASE);
	if (sysctl_reg == MAP_FAILED) {
		perror("Unable to map system control register memory");
		exit(-1);
	}

	// Map PWM control registers into virtual memory
	pwm_reg = mmap(NULL, getpagesize(), PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PWM_BASE);
	if (pwm_reg == MAP_FAILED) {
		perror("Unable to map PWM register memory");
		exit(-1);
	}

	// Elevate priority
	if (setpriority(PRIO_PROCESS, 0, -20) < 0) {
		perror("Unable to set priority");
		exit(-1);
	}

	// Set GPIO N7 to PWM7
	offset = sysctl_reg + SYSCTL_SCU88;
	*offset |= (0x1 << SYSCTL_PWM7);

	// Set H port to type "O"
	offset = pwm_reg + PTCR40;
	*offset &= ~(0x1 << 15);
	*offset |= 0x1 << 7;

	// Set PWM H rising / falling points
	offset = pwm_reg + PTCR4C;
	*offset &= ~0xffff0000;
	*offset |= 0x8000 << 16;

	// Set PWM H divisor and period
	uint32_t divisor_register = get_ideal_divisor(ast_clock_hz, frequency_hz);
	offset = pwm_reg + PTCR44;
	*offset = divisor_register;

	// Enable H port PWM output
	offset = pwm_reg + PTCR40;
	*offset |= 0x1 << 11;

	// Ideally usleep() would be used here, but the platform timers are buggy...
	delay_for_interval(duration_ms * 1000);

	// Disable H port PWM output
	offset = pwm_reg + PTCR40;
	*offset &= ~(0x1 << 11);

	return 0;
}