// SPDX-License-Identifier: GPL-2.0-only
/*
 * Generic MMIO clocksource support
 */
#include <linux/clocksource.h>
#include <linux/errno.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/spinlock.h>
#include <linux/uaccess.h>
#include <linux/miscdevice.h>
#include <linux/list.h>
#include <linux/slab.h>
#include <linux/fs.h>
#include <linux/mm.h>
#include <linux/mman.h>
#include <linux/device.h>

struct clocksource_user_mapping {
	struct mm_struct *mm;
	struct clocksource_user_mmio *ucs;
	void *regs;
	struct hlist_node link;
	atomic_t refs;
};

static struct class *user_mmio_class;
static dev_t user_mmio_devt;

static DEFINE_SPINLOCK(user_clksrcs_lock);
static unsigned int user_clksrcs_count;
static LIST_HEAD(user_clksrcs);

static inline struct clocksource_mmio *to_mmio_clksrc(struct clocksource *c)
{
	return container_of(c, struct clocksource_mmio, clksrc);
}

u64 clocksource_mmio_readl_up(struct clocksource *c)
{
	return (u64)readl_relaxed(to_mmio_clksrc(c)->reg);
}
EXPORT_SYMBOL_GPL(clocksource_mmio_readl_up);

u64 clocksource_mmio_readl_down(struct clocksource *c)
{
	return ~(u64)readl_relaxed(to_mmio_clksrc(c)->reg) & c->mask;
}

u64 clocksource_mmio_readw_up(struct clocksource *c)
{
	return (u64)readw_relaxed(to_mmio_clksrc(c)->reg);
}

u64 clocksource_mmio_readw_down(struct clocksource *c)
{
	return ~(u64)readw_relaxed(to_mmio_clksrc(c)->reg) & c->mask;
}

static inline struct clocksource_user_mmio *
to_mmio_ucs(struct clocksource *c)
{
	return container_of(c, struct clocksource_user_mmio, mmio.clksrc);
}

u64 clocksource_dual_mmio_readl_up(struct clocksource *c)
{
	struct clocksource_user_mmio *ucs = to_mmio_ucs(c);
	u32 upper, old_upper, lower;

	upper = readl_relaxed(ucs->reg_upper);
	do {
		old_upper = upper;
		lower = readl_relaxed(ucs->mmio.reg);
		upper = readl_relaxed(ucs->reg_upper);
	} while (upper != old_upper);

	return (((u64)upper) << ucs->bits_lower) | lower;
}

u64 clocksource_dual_mmio_readw_up(struct clocksource *c)
{
	struct clocksource_user_mmio *ucs = to_mmio_ucs(c);
	u16 upper, old_upper, lower;

	upper = readw_relaxed(ucs->reg_upper);
	do {
		old_upper = upper;
		lower = readw_relaxed(ucs->mmio.reg);
		upper = readw_relaxed(ucs->reg_upper);
	} while (upper != old_upper);

	return (((u64)upper) << ucs->bits_lower) | lower;
}

static void mmio_base_init(const char *name,int rating, unsigned int bits,
			   u64 (*read)(struct clocksource *),
			   struct clocksource *cs)
{
	cs->name = name;
	cs->rating = rating;
	cs->read = read;
	cs->mask = CLOCKSOURCE_MASK(bits);
	cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
}

/**
 * clocksource_mmio_init - Initialize a simple mmio based clocksource
 * @base:	Virtual address of the clock readout register
 * @name:	Name of the clocksource
 * @hz:		Frequency of the clocksource in Hz
 * @rating:	Rating of the clocksource
 * @bits:	Number of valid bits
 * @read:	One of clocksource_mmio_read*() above
 */
int clocksource_mmio_init(void __iomem *base, const char *name,
	unsigned long hz, int rating, unsigned bits,
	u64 (*read)(struct clocksource *))
{
	struct clocksource_mmio *cs;
	int err;

	if (bits > 64 || bits < 16)
		return -EINVAL;

	cs = kzalloc(sizeof(struct clocksource_mmio), GFP_KERNEL);
	if (!cs)
		return -ENOMEM;

	cs->reg = base;
	mmio_base_init(name, rating, bits, read, &cs->clksrc);

	err = clocksource_register_hz(&cs->clksrc, hz);
	if (err < 0) {
		kfree(cs);
		return err;
	}

	return err;
}

static void mmio_ucs_vmopen(struct vm_area_struct *vma)
{
	struct clocksource_user_mapping *mapping, *clone;
	struct clocksource_user_mmio *ucs;
	unsigned long h_key;

	mapping = vma->vm_private_data;

	if (mapping->mm == vma->vm_mm) {
		atomic_inc(&mapping->refs);
	} else if (mapping->mm) {
		/*
		 * We must be duplicating the original mm upon fork(),
		 * clone the parent ucs mapping struct then rehash it
		 * on the child mm key. If we cannot get memory for
		 * this, mitigate the issue for users by preventing a
		 * stale parent mm from being matched later on by a
		 * process which reused its mm_struct (h_key is based
		 * on this struct address).
		 */
		clone = kmalloc(sizeof(*mapping), GFP_KERNEL);
		if (clone == NULL) {
			pr_alert("out-of-memory for UCS mapping!\n");
			atomic_inc(&mapping->refs);
			mapping->mm = NULL;
			return;
		}
		ucs = mapping->ucs;
		clone->mm = vma->vm_mm;
		clone->ucs = ucs;
		clone->regs = mapping->regs;
		atomic_set(&clone->refs, 1);
		vma->vm_private_data = clone;
		h_key = (unsigned long)vma->vm_mm / sizeof(*vma->vm_mm);
		spin_lock(&ucs->lock);
		hash_add(ucs->mappings, &clone->link, h_key);
		spin_unlock(&ucs->lock);
	}
}

static void mmio_ucs_vmclose(struct vm_area_struct *vma)
{
	struct clocksource_user_mapping *mapping;

	mapping = vma->vm_private_data;

	if (atomic_dec_and_test(&mapping->refs)) {
		spin_lock(&mapping->ucs->lock);
		hash_del(&mapping->link);
		spin_unlock(&mapping->ucs->lock);
		kfree(mapping);
	}
}

static const struct vm_operations_struct mmio_ucs_vmops = {
	.open = mmio_ucs_vmopen,
	.close = mmio_ucs_vmclose,
};

static int mmio_ucs_mmap(struct file *file, struct vm_area_struct *vma)
{
	unsigned long addr, upper_pfn, lower_pfn;
	struct clocksource_user_mapping *mapping, *tmp;
	struct clocksource_user_mmio *ucs;
	unsigned int bits_upper;
	unsigned long h_key;
	pgprot_t prot;
	size_t pages;
	int err;

	pages = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
	if (pages > 2)
		return -EINVAL;

	vma->vm_private_data = NULL;

	ucs = file->private_data;
	upper_pfn = ucs->phys_upper >> PAGE_SHIFT;
	lower_pfn = ucs->phys_lower >> PAGE_SHIFT;
	bits_upper = fls(ucs->mmio.clksrc.mask) - ucs->bits_lower;
	if (pages == 2 && (!bits_upper || upper_pfn == lower_pfn))
		return -EINVAL;

	mapping = kmalloc(sizeof(*mapping), GFP_KERNEL);
	if (!mapping)
		return -ENOSPC;

	mapping->mm = vma->vm_mm;
	mapping->ucs = ucs;
	mapping->regs = (void *)vma->vm_start;
	atomic_set(&mapping->refs, 1);

	vma->vm_private_data = mapping;
	vma->vm_ops = &mmio_ucs_vmops;
	prot = pgprot_noncached(vma->vm_page_prot);
	addr = vma->vm_start;

	err = remap_pfn_range(vma, addr, lower_pfn, PAGE_SIZE, prot);
	if (err < 0)
		goto fail;

	if (pages > 1) {
		addr += PAGE_SIZE;
		err = remap_pfn_range(vma, addr, upper_pfn, PAGE_SIZE, prot);
		if (err < 0)
			goto fail;
	}

	h_key = (unsigned long)vma->vm_mm / sizeof(*vma->vm_mm);

	spin_lock(&ucs->lock);
	hash_for_each_possible(ucs->mappings, tmp, link, h_key) {
		if (tmp->mm == vma->vm_mm) {
			spin_unlock(&ucs->lock);
			err = -EBUSY;
			goto fail;
		}
	}
	hash_add(ucs->mappings, &mapping->link, h_key);
	spin_unlock(&ucs->lock);

	return 0;
fail:
	kfree(mapping);

	return err;
}

static long
mmio_ucs_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	struct clocksource_user_mapping *mapping;
	struct clksrc_user_mmio_info __user *u;
	unsigned long upper_pfn, lower_pfn;
	struct clksrc_user_mmio_info info;
	struct clocksource_user_mmio *ucs;
	unsigned int bits_upper;
	void __user *map_base;
	unsigned long h_key;
	size_t size;

	u = (struct clksrc_user_mmio_info __user *)arg;

	switch (cmd) {
	case CLKSRC_USER_MMIO_MAP:
		break;
	default:
		return -ENOTTY;
	}

	h_key = (unsigned long)current->mm / sizeof(*current->mm);

	ucs = file->private_data;
	upper_pfn = ucs->phys_upper >> PAGE_SHIFT;
	lower_pfn = ucs->phys_lower >> PAGE_SHIFT;
	bits_upper = fls(ucs->mmio.clksrc.mask) - ucs->bits_lower;
	size = PAGE_SIZE;
	if (bits_upper && upper_pfn != lower_pfn)
		size += PAGE_SIZE;

	do {
		spin_lock(&ucs->lock);
		hash_for_each_possible(ucs->mappings, mapping, link, h_key) {
			if (mapping->mm == current->mm) {
				spin_unlock(&ucs->lock);
				map_base = mapping->regs;
				goto found;
			}
		}
		spin_unlock(&ucs->lock);

		map_base = (void *)
			vm_mmap(file, 0, size, PROT_READ, MAP_SHARED, 0);
	} while (IS_ERR(map_base) && PTR_ERR(map_base) == -EBUSY);

	if (IS_ERR(map_base))
		return PTR_ERR(map_base);

found:
	info.type = ucs->type;
	info.reg_lower = map_base + offset_in_page(ucs->phys_lower);
	info.mask_lower = ucs->mmio.clksrc.mask;
	info.bits_lower = ucs->bits_lower;
	info.reg_upper = NULL;
	if (ucs->phys_upper)
		info.reg_upper = map_base + (size - PAGE_SIZE)
			+ offset_in_page(ucs->phys_upper);
	info.mask_upper = ucs->mask_upper;

	return copy_to_user(u, &info, sizeof(*u));
}

static int mmio_ucs_open(struct inode *inode, struct file *file)
{
	struct clocksource_user_mmio *ucs;

	if (file->f_mode & FMODE_WRITE)
		return -EINVAL;

	ucs = container_of(inode->i_cdev, typeof(*ucs), cdev);
	file->private_data = ucs;

	return 0;
}

static const struct file_operations mmio_ucs_fops = {
	.owner		= THIS_MODULE,
	.unlocked_ioctl = mmio_ucs_ioctl,
	.open		= mmio_ucs_open,
	.mmap		= mmio_ucs_mmap,
};

static int __init
ucs_create_cdev(struct class *class, struct clocksource_user_mmio *ucs)
{
	int err;

	ucs->dev = device_create(class, NULL,
				MKDEV(MAJOR(user_mmio_devt), ucs->id),
				ucs, "ucs/%d", ucs->id);
	if (IS_ERR(ucs->dev))
		return PTR_ERR(ucs->dev);

	spin_lock_init(&ucs->lock);
	hash_init(ucs->mappings);

	cdev_init(&ucs->cdev, &mmio_ucs_fops);
	ucs->cdev.kobj.parent = &ucs->dev->kobj;

	err = cdev_add(&ucs->cdev, ucs->dev->devt, 1);
	if (err < 0)
		goto err_device_destroy;

	return 0;

err_device_destroy:
	device_destroy(class, MKDEV(MAJOR(user_mmio_devt), ucs->id));
	return err;
}

static unsigned long default_revmap(void *virt)
{
	struct vm_struct *vm;

	vm = find_vm_area(virt);
	if (!vm)
		return 0;

	return vm->phys_addr + (virt - vm->addr);
}

int __init clocksource_user_mmio_init(struct clocksource_user_mmio *ucs,
				      const struct clocksource_mmio_regs *regs,
				      unsigned long hz)
{
	static u64 (*user_types[CLKSRC_MMIO_TYPE_NR])(struct clocksource *) = {
		[CLKSRC_MMIO_L_UP] = clocksource_mmio_readl_up,
		[CLKSRC_MMIO_L_DOWN] = clocksource_mmio_readl_down,
		[CLKSRC_DMMIO_L_UP] = clocksource_dual_mmio_readl_up,
		[CLKSRC_MMIO_W_UP] = clocksource_mmio_readw_up,
		[CLKSRC_MMIO_W_DOWN] = clocksource_mmio_readw_down,
		[CLKSRC_DMMIO_W_UP] = clocksource_dual_mmio_readw_up,
	};
	const char *name = ucs->mmio.clksrc.name;
	unsigned long phys_upper = 0, phys_lower;
	enum clksrc_user_mmio_type type;
	unsigned long (*revmap)(void *);
	int err;

	if (regs->bits_lower > 32 || regs->bits_lower < 16 ||
	    regs->bits_upper > 32)
		return -EINVAL;

	for (type = 0; type < ARRAY_SIZE(user_types); type++)
		if (ucs->mmio.clksrc.read == user_types[type])
			break;

	if (type == ARRAY_SIZE(user_types))
		return -EINVAL;

	if (!(ucs->mmio.clksrc.flags & CLOCK_SOURCE_IS_CONTINUOUS))
		return -EINVAL;

	revmap = regs->revmap;
	if (!revmap)
		revmap = default_revmap;

	phys_lower = revmap(regs->reg_lower);
	if (!phys_lower)
		return -EINVAL;

	if (regs->bits_upper) {
		phys_upper = revmap(regs->reg_upper);
		if (!phys_upper)
			return -EINVAL;
	}

	ucs->mmio.reg = regs->reg_lower;
	ucs->type = type;
	ucs->bits_lower = regs->bits_lower;
	ucs->reg_upper = regs->reg_upper;
	ucs->mask_lower = CLOCKSOURCE_MASK(regs->bits_lower);
	ucs->mask_upper = CLOCKSOURCE_MASK(regs->bits_upper);
	ucs->phys_lower = phys_lower;
	ucs->phys_upper = phys_upper;
	spin_lock_init(&ucs->lock);

	err = clocksource_register_hz(&ucs->mmio.clksrc, hz);
	if (err < 0)
		return err;

	spin_lock(&user_clksrcs_lock);

	ucs->id = user_clksrcs_count++;
	if (ucs->id < CLKSRC_USER_MMIO_MAX)
		list_add_tail(&ucs->link, &user_clksrcs);

	spin_unlock(&user_clksrcs_lock);

	if (ucs->id >= CLKSRC_USER_MMIO_MAX) {
		pr_warn("%s: Too many clocksources\n", name);
		err = -EAGAIN;
		goto fail;
	}

	ucs->mmio.clksrc.vdso_type = CLOCKSOURCE_VDSO_MMIO + ucs->id;

	if (user_mmio_class) {
		err = ucs_create_cdev(user_mmio_class, ucs);
		if (err < 0) {
			pr_warn("%s: Failed to add character device\n", name);
			goto fail;
		}
	}

	return 0;

fail:
	clocksource_unregister(&ucs->mmio.clksrc);

	return err;
}

int __init clocksource_user_single_mmio_init(
	void __iomem *base, const char *name,
	unsigned long hz, int rating, unsigned int bits,
	u64 (*read)(struct clocksource *))
{
	struct clocksource_user_mmio *ucs;
	struct clocksource_mmio_regs regs;
	int ret;

	ucs = kzalloc(sizeof(*ucs), GFP_KERNEL);
	if (!ucs)
		return -ENOMEM;

	mmio_base_init(name, rating, bits, read, &ucs->mmio.clksrc);
	regs.reg_lower = base;
	regs.reg_upper = NULL;
	regs.bits_lower = bits;
	regs.bits_upper = 0;
	regs.revmap = NULL;

	ret = clocksource_user_mmio_init(ucs, &regs, hz);
	if (ret)
		kfree(ucs);

	return ret;
}

static int __init mmio_clksrc_chr_dev_init(void)
{
	struct clocksource_user_mmio *ucs;
	struct class *class;
	int err;

	class = class_create(THIS_MODULE, "mmio_ucs");
	if (IS_ERR(class)) {
		pr_err("couldn't create user mmio clocksources class\n");
		return PTR_ERR(class);
	}

	err = alloc_chrdev_region(&user_mmio_devt, 0, CLKSRC_USER_MMIO_MAX,
				  "mmio_ucs");
	if (err < 0) {
		pr_err("failed to allocate user mmio clocksources character devivces region\n");
		goto err_class_destroy;
	}

	/*
	 * Calling list_for_each_entry is safe here: clocksources are always
	 * added to the list tail, never removed.
	 */
	spin_lock(&user_clksrcs_lock);
	list_for_each_entry(ucs, &user_clksrcs, link) {
		spin_unlock(&user_clksrcs_lock);

		err = ucs_create_cdev(class, ucs);
		if (err < 0)
			pr_err("%s: Failed to add character device\n",
			       ucs->mmio.clksrc.name);

		spin_lock(&user_clksrcs_lock);
	}
	user_mmio_class = class;
	spin_unlock(&user_clksrcs_lock);

	return 0;

err_class_destroy:
	class_destroy(class);
	return err;
}
device_initcall(mmio_clksrc_chr_dev_init);