drivers/amlogic/media/vout/cvbs/cvbs_out_reg.c

// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
/*
 * Copyright (c) 2019 Amlogic, Inc. All rights reserved.
 */

#include <linux/init.h>
#include <linux/version.h>
#include <linux/types.h>
#include <linux/slab.h>
#include <linux/kernel.h>
#include <linux/io.h>

#include "cvbs_out_reg.h"
#include "cvbs_log.h"

enum cvbs_map_e {
	CVBS_MAP_HIU = 0,
	CVBS_MAP_VCBUS,
	CVBS_MAP_MAX,
};

struct cvbs_reg_map_s {
	unsigned int base_addr;
	unsigned int size;
	void __iomem *p;
	char flag;
};

static struct cvbs_reg_map_s *cvbs_reg_map;

int cvbs_ioremap(struct platform_device *pdev)
{
	int i = 0;
	struct resource *res;

	cvbs_reg_map = kcalloc(CVBS_MAP_MAX, sizeof(struct cvbs_reg_map_s),
			       GFP_KERNEL);
	if (!cvbs_reg_map)
		return -1;
	while (i < CVBS_MAP_MAX) {
		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
		if (!res) {
			cvbs_log_err("%s: resource get error\n", __func__);
			kfree(cvbs_reg_map);
			cvbs_reg_map = NULL;
			return -1;
		}
		cvbs_reg_map[i].base_addr = res->start;
		cvbs_reg_map[i].size = resource_size(res);
		cvbs_reg_map[i].p = devm_ioremap_nocache(&pdev->dev,
							 res->start,
							 cvbs_reg_map[i].size);
		if (!cvbs_reg_map[i].p) {
			cvbs_reg_map[i].flag = 0;
			cvbs_log_err("%s: reg map failed: 0x%x\n",
				     __func__, cvbs_reg_map[i].base_addr);
			kfree(cvbs_reg_map);
			cvbs_reg_map = NULL;
			return -1;
		}
		cvbs_reg_map[i].flag = 1;
		cvbs_log_info("%s: reg mapped: 0x%x\n",
			      __func__, cvbs_reg_map[i].base_addr);

		i++;
	}

	return 0;
}

static int check_cvbs_ioremap(int n)
{
	if (!cvbs_reg_map)
		return -1;
	if (n >= CVBS_MAP_MAX)
		return -1;
	if (cvbs_reg_map[n].flag == 0) {
		cvbs_log_err("reg 0x%x mapped error\n",
			     cvbs_reg_map[n].base_addr);
		return -1;
	}
	return 0;
}

static inline void __iomem *check_cvbs_hiu_reg(unsigned int _reg)
{
	void __iomem *p;
	int reg_bus;
	unsigned int reg_offset;

	reg_bus = CVBS_MAP_HIU;
	if (check_cvbs_ioremap(reg_bus))
		return NULL;

	reg_offset = (_reg << 2);
	if (reg_offset >= cvbs_reg_map[reg_bus].size) {
		cvbs_log_err("invalid hiu reg offset: 0x%04x\n", _reg);
		return NULL;
	}
	p = cvbs_reg_map[reg_bus].p + reg_offset;
	return p;
}

static inline void __iomem *check_cvbs_vcbus_reg(unsigned int _reg)
{
	void __iomem *p;
	int reg_bus;
	unsigned int reg_offset;

	reg_bus = CVBS_MAP_VCBUS;
	if (check_cvbs_ioremap(reg_bus))
		return NULL;

	reg_offset = (_reg << 2);
	if (reg_offset >= cvbs_reg_map[reg_bus].size) {
		cvbs_log_err("invalid vcbus reg offset: 0x%04x\n", _reg);
		return NULL;
	}
	p = cvbs_reg_map[reg_bus].p + reg_offset;
	return p;
}

/* ********************************
 * register access api
 * **********************************/
unsigned int cvbs_hiu_read(unsigned int _reg)
{
	void __iomem *p;

	p = check_cvbs_hiu_reg(_reg);
	if (p)
		return readl(p);
	else
		return -1;
};

void cvbs_hiu_write(unsigned int _reg, unsigned int _value)
{
	void __iomem *p;

	p = check_cvbs_hiu_reg(_reg);
	if (p)
		writel(_value, p);
};

void cvbs_hiu_setb(unsigned int _reg, unsigned int _value,
		   unsigned int _start, unsigned int _len)
{
	cvbs_hiu_write(_reg, ((cvbs_hiu_read(_reg) &
			~(((1L << (_len)) - 1) << (_start))) |
			(((_value) & ((1L << (_len)) - 1)) << (_start))));
}

unsigned int cvbs_hiu_getb(unsigned int _reg, unsigned int _start,
			   unsigned int _len)
{
	return (cvbs_hiu_read(_reg) >> (_start)) & ((1L << (_len)) - 1);
}

unsigned int cvbs_vcbus_read(unsigned int _reg)
{
	void __iomem *p;

	p = check_cvbs_vcbus_reg(_reg);
	if (p)
		return readl(p);
	else
		return -1;
};

void cvbs_vcbus_write(unsigned int _reg, unsigned int _value)
{
	void __iomem *p;

	p = check_cvbs_vcbus_reg(_reg);
	if (p)
		writel(_value, p);
};

void cvbs_vcbus_setb(unsigned int _reg, unsigned int _value,
		     unsigned int _start, unsigned int _len)
{
	cvbs_vcbus_write(_reg, ((cvbs_vcbus_read(_reg) &
			 ~(((1L << (_len)) - 1) << (_start))) |
			 (((_value) & ((1L << (_len)) - 1)) << (_start))));
}

unsigned int cvbs_vcbus_getb(unsigned int _reg, unsigned int _start,
			     unsigned int _len)
{
	return (cvbs_vcbus_read(_reg) >> (_start)) & ((1L << (_len)) - 1);
}