/****************************************************************************
 *
 *   Copyright (c) 2015 Andrew Tridgell. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file pwm_input.cpp 
 *
 * PWM input driver based on earlier driver from Evan Slatyer,
 * which in turn was based on drv_hrt.c
 */

#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>

#include <sys/types.h>
#include <stdbool.h>

#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <board_config.h>
#include <drivers/drv_pwm_input.h>
#include <drivers/drv_hrt.h>

#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"

#include "stm32.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
#include <systemlib/err.h>

#include <uORB/uORB.h>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/subsystem_info.h>

#include <drivers/drv_device.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#if HRT_TIMER == PWMIN_TIMER
#error cannot share timer between HRT and PWMIN
#endif

#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL)
#error PWMIN defines are needed in board_config.h for this board
#endif

/* PWMIN configuration */
#if   PWMIN_TIMER == 1
# define PWMIN_TIMER_BASE	STM32_TIM1_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB2ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1CC
# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
#elif PWMIN_TIMER == 2
# define PWMIN_TIMER_BASE	STM32_TIM2_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM2
# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
#elif PWMIN_TIMER == 3
# define PWMIN_TIMER_BASE	STM32_TIM3_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB1ENR_TIM3EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM3
# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
#elif PWMIN_TIMER == 4
# define PWMIN_TIMER_BASE	STM32_TIM4_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB1ENR_TIM4EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM4
# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
#elif PWMIN_TIMER == 5
# define PWMIN_TIMER_BASE	STM32_TIM5_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM5
# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
#elif PWMIN_TIMER == 8
# define PWMIN_TIMER_BASE	STM32_TIM8_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB2ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM8CC
# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
#elif PWMIN_TIMER == 9
# define PWMIN_TIMER_BASE	STM32_TIM9_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1BRK
# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
#elif PWMIN_TIMER == 10
# define PWMIN_TIMER_BASE	STM32_TIM10_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1UP
# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM10_CLKIN
#elif PWMIN_TIMER == 11
# define PWMIN_TIMER_BASE	STM32_TIM11_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM11_CLKIN
#elif PWMIN_TIMER == 12
# define PWMIN_TIMER_BASE	STM32_TIM12_BASE
# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM12EN
# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM12_CLKIN
#else
# error PWMIN_TIMER must be a value between 1 and 12
#endif

/*
 * HRT clock must be at least 1MHz
 */
#if PWMIN_TIMER_CLOCK <= 1000000
# error PWMIN_TIMER_CLOCK must be greater than 1MHz
#endif

/*
 * Timer register accessors
 */
#define REG(_reg)	(*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg))

#define rCR1		REG(STM32_GTIM_CR1_OFFSET)
#define rCR2		REG(STM32_GTIM_CR2_OFFSET)
#define rSMCR		REG(STM32_GTIM_SMCR_OFFSET)
#define rDIER		REG(STM32_GTIM_DIER_OFFSET)
#define rSR		REG(STM32_GTIM_SR_OFFSET)
#define rEGR		REG(STM32_GTIM_EGR_OFFSET)
#define rCCMR1		REG(STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2		REG(STM32_GTIM_CCMR2_OFFSET)
#define rCCER		REG(STM32_GTIM_CCER_OFFSET)
#define rCNT		REG(STM32_GTIM_CNT_OFFSET)
#define rPSC		REG(STM32_GTIM_PSC_OFFSET)
#define rARR		REG(STM32_GTIM_ARR_OFFSET)
#define rCCR1		REG(STM32_GTIM_CCR1_OFFSET)
#define rCCR2		REG(STM32_GTIM_CCR2_OFFSET)
#define rCCR3		REG(STM32_GTIM_CCR3_OFFSET)
#define rCCR4		REG(STM32_GTIM_CCR4_OFFSET)
#define rDCR		REG(STM32_GTIM_DCR_OFFSET)
#define rDMAR		REG(STM32_GTIM_DMAR_OFFSET)

/*
 * Specific registers and bits used by HRT sub-functions
 */
#if PWMIN_TIMER_CHANNEL == 1
 #define rCCR_PWMIN_A		rCCR1			/* compare register for PWMIN */
 #define DIER_PWMIN_A		(GTIM_DIER_CC1IE) 	/* interrupt enable for PWMIN */
 #define SR_INT_PWMIN_A		GTIM_SR_CC1IF		/* interrupt status for PWMIN */
 #define rCCR_PWMIN_B		rCCR2 			/* compare register for PWMIN */
 #define SR_INT_PWMIN_B		GTIM_SR_CC2IF		/* interrupt status for PWMIN */
 #define CCMR1_PWMIN		((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
 #define CCMR2_PWMIN		0
 #define CCER_PWMIN		(GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
 #define SR_OVF_PWMIN		(GTIM_SR_CC1OF | GTIM_SR_CC2OF)
 #define SMCR_PWMIN_1		(0x05 << GTIM_SMCR_TS_SHIFT)
 #define SMCR_PWMIN_2		((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#elif PWMIN_TIMER_CHANNEL == 2
 #define rCCR_PWMIN_A		rCCR2			/* compare register for PWMIN */
 #define DIER_PWMIN_A		(GTIM_DIER_CC2IE)	/* interrupt enable for PWMIN */
 #define SR_INT_PWMIN_A		GTIM_SR_CC2IF		/* interrupt status for PWMIN */
 #define rCCR_PWMIN_B		rCCR1			/* compare register for PWMIN */
 #define DIER_PWMIN_B		GTIM_DIER_CC1IE		/* interrupt enable for PWMIN */
 #define SR_INT_PWMIN_B		GTIM_SR_CC1IF		/* interrupt status for PWMIN */
 #define CCMR1_PWMIN		((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
 #define CCMR2_PWMIN		0
 #define CCER_PWMIN		(GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
 #define SR_OVF_PWMIN		(GTIM_SR_CC1OF | GTIM_SR_CC2OF)
 #define SMCR_PWMIN_1		(0x06 << GTIM_SMCR_TS_SHIFT)
 #define SMCR_PWMIN_2		((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#else
 #error PWMIN_TIMER_CHANNEL must be either 1 and 2.
#endif


class PWMIN : device::CDev
{
public:
	PWMIN();
	virtual ~PWMIN();

	virtual int init();
	virtual int open(struct file *filp);
	virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
	virtual int ioctl(struct file *filp, int cmd, unsigned long arg);

	void _publish(uint16_t status, uint32_t period, uint32_t pulse_width);
	void _print_info(void);

private:
	uint32_t error_count;
	uint32_t pulses_captured;
	uint32_t last_period;
	uint32_t last_width;
	RingBuffer *reports;
	bool timer_started;

	void timer_init(void);
};

static int pwmin_tim_isr(int irq, void *context);
static void pwmin_start(void);
static void pwmin_info(void);
static void pwmin_test(void);
static void pwmin_reset(void);

static PWMIN *g_dev;

PWMIN::PWMIN() :
	CDev("pwmin", PWMIN0_DEVICE_PATH),
	error_count(0),
	pulses_captured(0),
	last_period(0),
	last_width(0),
	reports(nullptr),
	timer_started(false)
{
}

PWMIN::~PWMIN()
{
	if (reports != nullptr)
		delete reports;
}

/*
  initialise the driver. This doesn't actually start the timer (that
  is done on open). We don't start the timer to allow for this driver
  to be started in init scripts when the user may be using the input
  pin as PWM output
 */
int
PWMIN::init()
{
	// we just register the device in /dev, and only actually
	// activate the timer when requested to when the device is opened
	CDev::init();

	reports = new RingBuffer(2, sizeof(struct pwm_input_s));
	if (reports == nullptr) {
		return -ENOMEM;
	}

	return OK;
}

/*
 * Initialise the timer we are going to use.
 */
void PWMIN::timer_init(void) 
{
	// run with interrupts disabled in case the timer is already
	// setup. We don't want it firing while we are doing the setup
	irqstate_t flags = irqsave();
	stm32_configgpio(GPIO_PWM_IN);

	/* claim our interrupt vector */
	irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr);

	/* Clear no bits, set timer enable bit.*/
	modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); 

	/* disable and configure the timer */
	rCR1 = 0;
	rCR2 = 0;
	rSMCR = 0;
	rDIER = DIER_PWMIN_A;
	rCCER = 0;		/* unlock CCMR* registers */
	rCCMR1 = CCMR1_PWMIN;
	rCCMR2 = CCMR2_PWMIN;
	rSMCR = SMCR_PWMIN_1;	/* Set up mode */
	rSMCR = SMCR_PWMIN_2;	/* Enable slave mode controller */
	rCCER = CCER_PWMIN;
	rDCR = 0;

	// for simplicity scale by the clock in MHz. This gives us
	// readings in microseconds which is typically what is needed
	// for a PWM input driver
	uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL;

	/* 
	 * define the clock speed. We want the highest possible clock 
	 * speed that avoids overflows.
	 */
	rPSC = prescaler - 1;

	/* run the full span of the counter. All timers can handle
	 * uint16 */
	rARR = UINT16_MAX;

	/* generate an update event; reloads the counter, all registers */
	rEGR = GTIM_EGR_UG;

	/* enable the timer */
	rCR1 = GTIM_CR1_CEN;

	/* enable interrupts */
	up_enable_irq(PWMIN_TIMER_VECTOR);

	irqrestore(flags);

	timer_started = true;
}

/*
  hook for open of the driver. We start the timer at this point, then
  leave it running
 */
int
PWMIN::open(struct file *filp)
{
	if (g_dev == nullptr) {
		return -EIO;
	}
	int ret = CDev::open(filp);	
	if (ret == OK && !timer_started) {
		g_dev->timer_init();
	}
	return ret;
}


/*
  handle ioctl requests
 */
int
PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {
	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 500))
				return -EINVAL;

			irqstate_t flags = irqsave();
			if (!reports->resize(arg)) {
				irqrestore(flags);
				return -ENOMEM;
			}
			irqrestore(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return reports->size();

	case SENSORIOCRESET:
		/* user has asked for the timer to be reset. This may
		   be needed if the pin was used for a different
		   purpose (such as PWM output)
		*/
		timer_init();
		return OK;

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}


/*
  read some samples from the device
 */
ssize_t
PWMIN::read(struct file *filp, char *buffer, size_t buflen)
{
	unsigned count = buflen / sizeof(struct pwm_input_s);
	struct pwm_input_s *buf = reinterpret_cast<struct pwm_input_s *>(buffer);
	int ret = 0;

	/* buffer must be large enough */
	if (count < 1)
		return -ENOSPC;

	while (count--) {
		if (reports->get(buf)) {
			ret += sizeof(struct pwm_input_s);
			buf++;
		}
	}

	/* if there was no data, warn the caller */
	return ret ? ret : -EAGAIN;
}

/*
  publish some data from the ISR in the ring buffer
 */
void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width)
{
	/* if we missed an edge, we have to give up */
	if (status & SR_OVF_PWMIN) {
		error_count++;
		return;
	}

	struct pwm_input_s pwmin_report;
	pwmin_report.timestamp = hrt_absolute_time();
	pwmin_report.error_count = error_count;
	pwmin_report.period = period;
	pwmin_report.pulse_width = pulse_width;

	reports->force(&pwmin_report);

	last_period = period;
	last_width = pulse_width;
	pulses_captured++;
}

/*
  print information on the last captured
 */
void PWMIN::_print_info(void)
{
	if (!timer_started) {
		printf("timer not started - try the 'test' command\n");
	} else {
		printf("count=%u period=%u width=%u\n",
		       (unsigned)pulses_captured,
		       (unsigned)last_period,
		       (unsigned)last_width);
	}
}


/*
  Handle the interupt, gathering pulse data
 */
static int pwmin_tim_isr(int irq, void *context)
{
	uint16_t status = rSR;
	uint32_t period = rCCR_PWMIN_A;
	uint32_t pulse_width = rCCR_PWMIN_B;

	/* ack the interrupts we just read */
	rSR = 0;

	if (g_dev != nullptr) {
		g_dev->_publish(status, period, pulse_width);
	}

	return OK;
}

/*
  start the driver
 */
static void pwmin_start(void)
{
	if (g_dev != nullptr) {
		printf("driver already started\n");
		exit(1);
	}
	g_dev = new PWMIN();
	if (g_dev == nullptr) {
		errx(1, "driver allocation failed");
	}
	if (g_dev->init() != OK) {
		errx(1, "driver init failed");
	}
	exit(0);
}

/*
  test the driver
 */
static void pwmin_test(void)
{
	int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
	if (fd == -1) {
		errx(1, "Failed to open device");
	}
	uint64_t start_time = hrt_absolute_time();

	printf("Showing samples for 5 seconds\n");

	while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) {
		struct pwm_input_s buf;
		if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) {
			printf("period=%u width=%u error_count=%u\n",
			       (unsigned)buf.period,
			       (unsigned)buf.pulse_width,
			       (unsigned)buf.error_count);
		}
	}
	close(fd);
	exit(0);
}

/*
  reset the timer
 */
static void pwmin_reset(void)
{
	int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
	if (fd == -1) {
		errx(1, "Failed to open device");
	}
	if (ioctl(fd, SENSORIOCRESET, 0) != OK) {
		errx(1, "reset failed");
	}
	close(fd);
	exit(0);
}

/*
  show some information on the driver
 */
static void pwmin_info(void)
{
	if (g_dev == nullptr) {
		printf("driver not started\n");
		exit(1);
	}
	g_dev->_print_info();
	exit(0);
}


/*
  driver entry point
 */
int pwm_input_main(int argc, char * argv[]) 
{
	const char *verb = argv[1];

	/*
	 * Start/load the driver.
	 */
	if (!strcmp(verb, "start")) {
		pwmin_start();
	}

	/*
	 * Print driver information.
	 */
	if (!strcmp(verb, "info")) {
		pwmin_info();
	}

	/*
	 * print test results
	 */
	if (!strcmp(verb, "test")) {
		pwmin_test();
	}

	/*
	 * reset the timer
	 */
	if (!strcmp(verb, "reset")) {
		pwmin_reset();
	}

	errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'");
	return 0;
}