path: root/drivers/nvpps/nvpps_main.c

/*
 * Copyright (c) 2018-2020, NVIDIA CORPORATION. All rights reserved.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * 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.
 *
 */

#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/interrupt.h>
#include <linux/platform_device.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/cdev.h>
#include <linux/poll.h>
#include <linux/gpio.h>
#include <linux/list.h>
#include <linux/of_device.h>
#include <linux/of_gpio.h>
#include <asm/arch_timer.h>
#include <linux/platform/tegra/ptp-notifier.h>
#include <uapi/linux/nvpps_ioctl.h>


//#define NVPPS_MAP_EQOS_REGS
//#define NVPPS_ARM_COUNTER_PROFILING
//#define NVPPS_EQOS_REG_PROFILING



#define MAX_NVPPS_SOURCES	1
#define NVPPS_DEF_MODE	NVPPS_MODE_GPIO

/* statics */
static struct class	*s_nvpps_class;
static dev_t		s_nvpps_devt;
static DEFINE_MUTEX(s_nvpps_lock);
static DEFINE_IDR(s_nvpps_idr);



/* platform device instance data */
struct nvpps_device_data {
	struct platform_device	*pdev;
	struct cdev		cdev;
	struct device		*dev;
	unsigned int		id;
	unsigned int		gpio_pin;
	int			irq;
	bool			irq_registered;

	bool			pps_event_id_valid;
	unsigned int		pps_event_id;
	u64			tsc;
	u64			phc;
	u64			irq_latency;
	u64			tsc_res_ns;
	raw_spinlock_t		lock;

	u32			evt_mode;
	u32			tsc_mode;

	struct timer_list	timer;
	volatile bool		timer_inited;

	wait_queue_head_t	pps_event_queue;
	struct fasync_struct	*pps_event_async_queue;

#ifdef NVPPS_MAP_EQOS_REGS
	u64			eqos_base_addr;
#endif /*NVPPS_MAP_EQOS_REGS*/
};


/* file instance data */
struct nvpps_file_data {
	struct nvpps_device_data	*pdev_data;
	unsigned int			pps_event_id_rd;
};



#ifdef NVPPS_MAP_EQOS_REGS

#define EQOS_BASE_ADDR	0x2490000
#define BASE_ADDRESS pdev_data->eqos_base_addr
#define MAC_STNSR_TSSS_LPOS 0
#define MAC_STNSR_TSSS_HPOS 30

#define GET_VALUE(data, lbit, hbit) ((data >> lbit) & (~(~0<<(hbit-lbit+1))))
#define MAC_STNSR_OFFSET ((u32 *)(BASE_ADDRESS + 0xb0c))
#define MAC_STNSR_RD(data) \
	do { \
		data = ioread32((void *)MAC_STNSR_OFFSET); \
	} while(0)

#define MAC_STSR_OFFSET ((u32 *)(BASE_ADDRESS + 0xb08))
#define MAC_STSR_RD(data) \
	do { \
		data = ioread32((void *)MAC_STSR_OFFSET); \
	} while(0)

#endif /*NVPPS_MAP_EQOS_REGS*/



static inline u64 __arch_counter_get_cntvct(void)
{
	u64 cval;

	asm volatile("mrs %0, cntvct_el0" : "=r" (cval));

	return cval;
}


#ifdef NVPPS_MAP_EQOS_REGS
static inline u64 get_systime(struct nvpps_device_data *pdev_data, u64 *tsc)
{
	u64 ns1, ns2, ns;
	u32 varmac_stnsr1, varmac_stnsr2;
	u32 varmac_stsr;

	/* read the PHC */
	MAC_STNSR_RD(varmac_stnsr1);
	MAC_STSR_RD(varmac_stsr);
	/* read the TSC */
	*tsc = __arch_counter_get_cntvct();

	/* read the nsec part of the PHC one more time */
	MAC_STNSR_RD(varmac_stnsr2);

	ns1 = GET_VALUE(varmac_stnsr1, MAC_STNSR_TSSS_LPOS,
			MAC_STNSR_TSSS_HPOS);
	ns2 = GET_VALUE(varmac_stnsr2, MAC_STNSR_TSSS_LPOS,
			MAC_STNSR_TSSS_HPOS);

	/* if ns1 is greater than ns2, it means nsec counter rollover
	 * happened. In that case read the updated sec counter again
	 */
	if (ns1 > ns2) {
		/* let's read the TSC again */
		*tsc = __arch_counter_get_cntvct();
		/* read the second portion of the PHC */
		MAC_STSR_RD(varmac_stsr);
		/* convert sec/high time value to nanosecond */
		ns = ns2 + (varmac_stsr * 1000000000ull);
	} else {
		/* convert sec/high time value to nanosecond */
		ns = ns1 + (varmac_stsr * 1000000000ull);
	}

	return ns;
}
#endif /*NVPPS_MAP_EQOS_REGS*/



/*
 * Report the PPS event
 */
static void nvpps_get_ts(struct nvpps_device_data *pdev_data, bool in_isr)
{
	u64		tsc;
	u64 		irq_tsc = 0;
	u64 		phc = 0;
	u64		irq_latency = 0;
	unsigned long 	flags;

	/* get the gpio interrupt timestamp */
	if (in_isr) {
		irq_tsc = __arch_counter_get_cntvct();
	} else {
		irq_tsc = __arch_counter_get_cntvct();//0;
	}

#ifdef NVPPS_MAP_EQOS_REGS
	/* get the PTP timestamp */
	if (pdev_data->eqos_base_addr) {
		/* get both the phc and tsc */
		phc = get_systime(pdev_data, &tsc);
	} else {
#endif /*NVPPS_MAP_EQOS_REGS*/
		/* get the phc from eqos driver */
		get_ptp_hwtime(&phc);
		/* get the current TSC time */
		tsc = __arch_counter_get_cntvct();
#ifdef NVPPS_MAP_EQOS_REGS
	}
#endif /*NVPPS_MAP_EQOS_REGS*/

#ifdef NVPPS_ARM_COUNTER_PROFILING
	{
	u64 	tmp;
	int	i;
	irq_tsc = __arch_counter_get_cntvct();
	for (i = 0; i < 98; i++) {
		tmp = __arch_counter_get_cntvct();
	}
		tsc = __arch_counter_get_cntvct();
	}
#endif /*NVPPS_ARM_COUNTER_PROFILING*/

#ifdef NVPPS_EQOS_REG_PROFILING
	{
	u32 	varmac_stnsr;
	u32 	varmac_stsr;
	int	i;
	irq_tsc = __arch_counter_get_cntvct();
	for (i = 0; i < 100; i++) {
		MAC_STNSR_RD(varmac_stnsr);
		MAC_STSR_RD(varmac_stsr)
	}
	tsc = __arch_counter_get_cntvct();
	}
#endif /*NVPPS_EQOS_REG_PROFILING*/

	/* get the interrupt latency */
	if (irq_tsc) {
		irq_latency = (tsc - irq_tsc) * pdev_data->tsc_res_ns;
	}

	raw_spin_lock_irqsave(&pdev_data->lock, flags);
	pdev_data->pps_event_id_valid = true;
	pdev_data->pps_event_id++;
	pdev_data->tsc = irq_tsc ? irq_tsc : tsc;
	/* adjust the ptp time for the interrupt latency */
#if defined (NVPPS_ARM_COUNTER_PROFILING) || defined (NVPPS_EQOS_REG_PROFILING)
	pdev_data->phc = phc;
#else /* !NVPPS_ARM_COUNTER_PROFILING && !NVPPS_EQOS_REG_PROFILING */
	pdev_data->phc = phc ? phc - irq_latency : phc;
#endif /* NVPPS_ARM_COUNTER_PROFILING || NVPPS_EQOS_REG_PROFILING */
	pdev_data->irq_latency = irq_latency;
	raw_spin_unlock_irqrestore(&pdev_data->lock, flags);

	/*dev_info(pdev_data->dev, "evt(%d) tsc(%llu) phc(%llu)\n", pdev_data->pps_event_id, pdev_data->tsc, pdev_data->phc);*/

	/* event notification */
	wake_up_interruptible(&pdev_data->pps_event_queue);
	kill_fasync(&pdev_data->pps_event_async_queue, SIGIO, POLL_IN);
}


static irqreturn_t nvpps_gpio_isr(int irq, void *data)
{
	struct nvpps_device_data	*pdev_data = (struct nvpps_device_data *)data;

	/* get timestamps for this event */
	nvpps_get_ts(pdev_data, true);

	return IRQ_HANDLED;
}


static void nvpps_timer_callback(unsigned long data)
{
	struct nvpps_device_data	*pdev_data = (struct nvpps_device_data *)data;

	/* get timestamps for this event */
	nvpps_get_ts(pdev_data, false);

	/* set the next expire time */
	if (pdev_data->timer_inited) {
		mod_timer(&pdev_data->timer, jiffies + msecs_to_jiffies(1000));
	}
}



static int set_mode(struct nvpps_device_data *pdev_data, u32 mode)
{
	int	err = 0;
	if (mode != pdev_data->evt_mode) {
		switch (mode) {
			case NVPPS_MODE_GPIO:
				if (pdev_data->timer_inited) {
					pdev_data->timer_inited = false;
					del_timer_sync(&pdev_data->timer);
				}
				if (!pdev_data->irq_registered) {
					/* register IRQ handler */
					err = devm_request_irq(&pdev_data->pdev->dev,
					pdev_data->irq, nvpps_gpio_isr,
					IRQF_TRIGGER_RISING | IRQF_NO_THREAD,
					"nvpps_isr", pdev_data);

					if (err) {
						dev_err(pdev_data->dev, "failed to acquire IRQ %d\n", pdev_data->irq);
					} else {
						pdev_data->irq_registered = true;
						dev_info(pdev_data->dev, "Registered IRQ %d for nvpps\n", pdev_data->irq);
					}
				}
				break;

			case NVPPS_MODE_TIMER:
				if (pdev_data->irq_registered) {
					/* unregister IRQ handler */
					devm_free_irq(&pdev_data->pdev->dev, pdev_data->irq, pdev_data);
					pdev_data->irq_registered = false;
				}
				if (!pdev_data->timer_inited) {
					setup_timer(&pdev_data->timer, nvpps_timer_callback, (unsigned long)pdev_data);
					pdev_data->timer_inited = true;
					/* setup timer interval to 1000 msecs */
					mod_timer(&pdev_data->timer, jiffies + msecs_to_jiffies(1000));
				}
				break;

			default:
				return -EINVAL;
		}
	}
	return err;
}



/* Character device stuff */
static unsigned int nvpps_poll(struct file *file, poll_table *wait)
{
	struct nvpps_file_data		*pfile_data = (struct nvpps_file_data *)file->private_data;
	struct nvpps_device_data	*pdev_data = pfile_data->pdev_data;

	poll_wait(file, &pdev_data->pps_event_queue, wait);
	if (pdev_data->pps_event_id_valid &&
		(pfile_data->pps_event_id_rd != pdev_data->pps_event_id)) {
		return POLLIN | POLLRDNORM;
	} else {
		return 0;
	}
}


static int nvpps_fasync(int fd, struct file *file, int on)
{
	struct nvpps_file_data		*pfile_data = (struct nvpps_file_data *)file->private_data;
	struct nvpps_device_data	*pdev_data = pfile_data->pdev_data;

	return fasync_helper(fd, file, on, &pdev_data->pps_event_async_queue);
}


static long nvpps_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	struct nvpps_file_data		*pfile_data = (struct nvpps_file_data *)file->private_data;
	struct nvpps_device_data	*pdev_data = pfile_data->pdev_data;
	struct nvpps_params 		params;
	void __user 			*uarg = (void __user *)arg;
	int 				err;

	switch (cmd) {
		case NVPPS_GETVERSION: {
			struct nvpps_version	version;

			dev_dbg(pdev_data->dev, "NVPPS_GETVERSION\n");

			/* Get the current parameters */
			version.version.major = NVPPS_VERSION_MAJOR;
			version.version.minor = NVPPS_VERSION_MINOR;
			version.api.major = NVPPS_API_MAJOR;
			version.api.minor = NVPPS_API_MINOR;

			err = copy_to_user(uarg, &version, sizeof(struct nvpps_version));
			if (err) {
				return -EFAULT;
			}
			break;
		}

		case NVPPS_GETPARAMS:
			dev_dbg(pdev_data->dev, "NVPPS_GETPARAMS\n");

			/* Get the current parameters */
			params.evt_mode = pdev_data->evt_mode;
			params.tsc_mode = pdev_data->tsc_mode;

			err = copy_to_user(uarg, &params, sizeof(struct nvpps_params));
			if (err) {
				return -EFAULT;
			}
			break;

		case NVPPS_SETPARAMS:
			dev_dbg(pdev_data->dev, "NVPPS_SETPARAMS\n");

			err = copy_from_user(&params, uarg, sizeof(struct nvpps_params));
			if (err) {
				return -EFAULT;
			}
			err = set_mode(pdev_data, params.evt_mode);
			if (err) {
				dev_dbg(pdev_data->dev, "switch_mode to %d failed err(%d)\n", params.evt_mode, err);
				return err;
			}
			pdev_data->evt_mode = params.evt_mode;
			pdev_data->tsc_mode = params