sdk-hwV1.3/lichee/linux-4.9/drivers/char/sunxi_dspo/dspo_device.c

/*
 * dspo_device/dspo_device.c
 *
 * Copyright (c) 2007-2021 Allwinnertech Co., Ltd.
 * Author: zhengxiaobin <zhengxiaobin@allwinnertech.com>
 *
 * 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 "dspo_device.h"

#define DSPO_ALIGN(value, align) ((align == 0) ? \
				value : \
				(((value) + ((align) - 1)) & ~((align) - 1)))
#define MAX_DMA_BUF_CACHE_CNT 3

struct dspo_dmabuf_t {
	struct list_head list;
	int fd;
	struct dma_buf *buf;
	struct dma_buf_attachment *attachment;
	struct sg_table *sgt;
	dma_addr_t dma_addr;
};

static void dspo_sync_checkin(struct dspo_device_t *p_dev)
{
	u32 index = p_dev->health.sync_time_index;

	p_dev->health.sync_time[index] = jiffies;
	index++;
	index = (index >= 100) ? 0 : index;
	p_dev->health.sync_time_index = index;
}

irqreturn_t dspo_handle_irq(int irq, void *parg)
{
	struct dspo_device_t *p_dev = (struct dspo_device_t *)parg;
	unsigned int int_status = 0;

	int_status = dspo_irq_process(0);
	switch (int_status & 0x007f) {
	case DSPO_V_INT:
		break;
	case DSPO_DMA_DONE_INT:
		p_dev->dma_finish_flag = true;
		wake_up(&p_dev->queue);
		break;
	default:
		pr_warn("Wrong irq status:%u\n", int_status);
		break;
	}

	if (int_status > DSPO_UV_LINE_DONE_INT)
		++p_dev->health.err_cnt;

	++p_dev->health.irq_cnt;
	dspo_clr_irq(0, int_status & 0x007f);
	dspo_sync_checkin(p_dev);

	return IRQ_HANDLED;
}

static int dspo_clk_enable(struct dspo_device_t *p_dev, bool enable)
{
	int ret = -1;
	unsigned long rate = 0, round_rate = 0;
	long rate_diff = 0;
	unsigned long parent_rate = 0, parent_round_rate = 0;
	long parent_rate_diff = 0;
	unsigned int div = 1;

	if (!p_dev) {
		pr_warn("NULL dspo device!\n");
		goto OUT;
	}

	if (enable == true) {
		if (p_dev->clk_parent && p_dev->clk) {
			rate = p_dev->cfg.timing_info.pixel_clk * 2;
			clk_set_parent(p_dev->clk, p_dev->clk_parent);
			clk_set_rate(p_dev->clk_parent, 594000000);
			round_rate = clk_round_rate(p_dev->clk, rate);
			rate_diff = (long)(round_rate - rate);
			//-5Mhz to 5Mhz
			if ((rate_diff > 5000000) || (rate_diff < -5000000)) {
				for (div = 1; (rate * div) <= 600000000;
				     div++) {
					parent_rate = rate * div;
					parent_round_rate = clk_round_rate(
					    p_dev->clk_parent, parent_rate);
					parent_rate_diff =
					    (long)(parent_round_rate -
						   parent_rate);
					if ((parent_rate_diff < 5000000) &&
					    (parent_rate_diff > -5000000)) {
						clk_set_rate(p_dev->clk_parent,
							     parent_rate);
						clk_set_rate(p_dev->clk, rate);
						break;
					}
				}
				if ((rate * div) > 600000000)
					clk_set_rate(p_dev->clk, rate);
			} else {
				clk_set_rate(p_dev->clk, rate);
			}
			clk_prepare_enable(p_dev->clk);
			ret = 0;
		} else
			pr_warn("NULL clk\n");
	} else {
		if (p_dev->clk_parent && p_dev->clk) {
			clk_disable(p_dev->clk);
			ret = 0;
		} else
			pr_warn("NULL clk\n");
	}

OUT:
	return ret;
}

static int dspo_pin_cfg(struct dspo_device_t *p_dev, bool enable)
{
	int ret = -1;
	struct pinctrl *pctl;
	struct pinctrl_state *state;
	char name[80] = {0};
	char temp[80] = {0};

	if (!p_dev) {
		pr_warn("NULL dspo device!\n");
		goto OUT;
	}

	pctl = pinctrl_get(p_dev->dev);
	if (IS_ERR_OR_NULL(pctl)) {
		ret = PTR_ERR(pctl);
		pr_warn("Get dspo pinctrl fail!:%d\n", ret);
		goto OUT;
	}

	switch (p_dev->cfg.bit_width) {
	case DSPO_8_BIT:
		snprintf(name, 80, "bt656");
		break;
	case DSPO_16_BIT:
		snprintf(name, 80, "bt1120");
		break;
	default:
		pr_warn("Undefine bitwidth:%d\n", p_dev->cfg.bit_width);
		goto OUT;
		break;
	}

	if (p_dev->cfg.separate_sync == true)
		snprintf(temp, 80, "%s-seperate", name);
	else
		snprintf(temp, 80, "%s-embed", name);

	snprintf(name, 80, "%s", temp);

	if (enable == false)
		snprintf(temp, 80, "%s-sleep", name);

	snprintf(name, 80, "%s", temp);


	state = pinctrl_lookup_state(pctl, name);
	if (IS_ERR_OR_NULL(state)) {
		ret = PTR_ERR(state);
		pr_warn("pinctrl_lookup_state for %s fail:%d\n", name, ret);
		goto OUT;
	}

	ret = pinctrl_select_state(pctl, state);
	if (ret < 0) {
		pr_warn("pinctrl_select_state %s fail:%d\n", name, ret);
		goto OUT;
	}
OUT:
	return ret;

}

int dspo_enable(struct dspo_device_t *p_dev, struct dspo_config_t *p_cfg)
{
	int ret = -1;
	bool is_mode_support = true;
	struct dspo_video_timings *t = NULL;

	if (!p_cfg || !p_dev) {
		pr_warn("NULL pointer!\n");
		goto OUT;
	}

	mutex_lock(&p_dev->dev_en_mutex);
	memcpy(&p_dev->cfg, p_cfg, sizeof(struct dspo_config_t));
	if (p_dev->cfg.output_mode  != DSPO_CUSTOM_MODE) {
		is_mode_support = p_dev->mode_support(p_dev, p_dev->cfg.output_mode);
		if (is_mode_support == false) {
			pr_warn("Do not support %d mode\n", p_dev->cfg.output_mode);
			goto OUT;
		} else {
			ret = dspo_get_timing_info(p_dev->cfg.output_mode, &p_dev->cfg.timing_info);
			if (ret) {
				pr_warn("Get video timing info fail!\n");
				goto OUT;
			}
		}
	}

	ret = dspo_clk_enable(p_dev, true);
	if (ret)
		goto OUT;

	ret = request_irq(p_dev->irq, dspo_handle_irq, 0,
			  dev_name(p_dev->dev), (void *)p_dev);
	if (ret) {
		pr_warn("failed to install irq resource\n");
		goto CLK_DISABLE;
	}

	ret = dspo_pin_cfg(p_dev, true);
	if (ret)
		goto FREE_IRQ;

	t = &p_dev->cfg.timing_info;
	dspo_timing_set(0, t->x_res, t->hor_back_porch, t->hor_front_porch,
			t->hor_sync_time, t->y_res, t->ver_back_porch,
			t->ver_total_time, t->ver_front_porch, t->ver_sync_time,
			t->b_interlace, p_dev->cfg.protocol);
	dspo_fmt_set(0, p_dev->cfg.data_seq_sel, p_dev->cfg.protocol,
		     p_dev->cfg.bit_width, t->b_interlace);
	dspo_chroma_spl_set(0, 4, 4);
	if (t->b_interlace == false)
		dspo_clamp_set(0, 16, 235, 16, 240, 16, 240);
	dspo_sync_pol_set(0, t->hor_sync_polarity, t->ver_sync_polarity, 0);
	dspo_dclk_adjust(0, p_dev->cfg.dclk_set.dclk_invt,
			 p_dev->cfg.dclk_set.dclk_en,
			 p_dev->cfg.dclk_set.dclk_dly_num);
	dspo_irq_en(0, DSPO_DMA_DONE_INT, 0);

	dspo_module_en(0, 1, p_dev->cfg.separate_sync);
	/*
	dspo_dma_ctrl_set(0, REG_SET_MODE, BYTES_512, DSPO_FORMAT_YUV420_SP_VUVU);
	dspo_dma_start_check_set(0, 30, 1);
	dspo_dma_size_set(0, t->x_res, t->y_res);
	dspo_dma_mode_line_buf_range_set(0, t->x_res);
	dspo_dma_buf_line_stride_set(0, DSPO_ALIGN(t->x_res, 4) * 4, 0);
	dspo_dma_start(0);
	*/

	p_dev->enabled = true;

	mutex_unlock(&p_dev->dev_en_mutex);
	return 0;
FREE_IRQ:
	free_irq(p_dev->irq, (void *)p_dev);
CLK_DISABLE:
	dspo_clk_enable(p_dev, false);
OUT:

	mutex_unlock(&p_dev->dev_en_mutex);
	return ret;
}

struct dspo_config_t *dspo_get_config(struct dspo_device_t *p_dev)
{
	if (!p_dev) {
		pr_warn("NULL pointer!\n");
		return NULL;
	}

	return &p_dev->cfg;
}

bool dspo_mode_support(struct dspo_device_t *p_dev, enum dspo_output_mode mode)
{
	if (!p_dev) {
		pr_warn("NULL pointer!\n");
		return false;
	}

	return dspo_is_mode_support(mode);
}


bool dspo_is_enabled(struct dspo_device_t *p_dev)
{
	if (!p_dev)
		return false;

	return p_dev->enabled;
}

int dspo_disable(struct dspo_device_t *p_dev)
{
	int ret = -1;

	if (!p_dev) {
		pr_warn("NULL pointer!\n");
		goto OUT;
	}
	mutex_lock(&p_dev->dev_en_mutex);

	dspo_irq_disable(0, DSPO_V_INT);
	dspo_irq_disable(0, DSPO_DMA_DONE_INT);
	free_irq(p_dev->irq, (void *)p_dev);
	dspo_module_en(0, 0, p_dev->cfg.separate_sync);
	ret = dspo_pin_cfg(p_dev, false);
	ret += dspo_clk_enable(p_dev, false);
	p_dev->enabled = false;
OUT:
	mutex_unlock(&p_dev->dev_en_mutex);
	return ret;
}

void dspo_dma_unmap(struct dspo_device_t *p_dev, struct dspo_dmabuf_t *item)
{

	dma_unmap_sg_attrs(p_dev->dev, item->sgt->sgl,
			      item->sgt->nents, DMA_FROM_DEVICE,
			      DMA_ATTR_SKIP_CPU_SYNC);
	dma_buf_unmap_attachment(item->attachment, item->sgt, DMA_FROM_DEVICE);
	sg_free_table(item->sgt);
	kfree(item->sgt);
	dma_buf_detach(item->buf, item->attachment);
	dma_buf_put(item->buf);
	kfree(item);
}

static int dspo_dma_map(struct dspo_device_t *p_dev, int fd, struct dspo_dmabuf_t *p_item)
{
	struct dma_buf *dmabuf;
	struct dma_buf_attachment *attachment;
	struct sg_table *sgt, *sgt_bak;
	struct scatterlist *sgl, *sgl_bak;
	s32 sg_count = 0;
	int ret = -1;
	int i;

	if (fd < 0) {
		pr_warn("dma_buf_id(%d) is invalid\n", fd);
		goto exit;
	}
	dmabuf = dma_buf_get(fd);
	if (IS_ERR(dmabuf)) {
		pr_warn("dma_buf_get failed, fd=%d\n", fd);
		goto exit;
	}

	attachment = dma_buf_attach(dmabuf, p_dev->dev);
	if (IS_ERR(attachment)) {
		pr_warn("dma_buf_attach failed\n");
		goto err_buf_put;
	}
	sgt = dma_buf_map_attachment(attachment, DMA_FROM_DEVICE);
	if (IS_ERR_OR_NULL(sgt)) {
		pr_warn("dma_buf_map_attachment failed\n");
		goto err_buf_detach;
	}

	/* create a private sgtable base on the given dmabuf */
	sgt_bak = kmalloc(sizeof(struct sg_table), GFP_KERNEL | __GFP_ZERO);
	if (sgt_bak == NULL) {
		pr_warn("alloc sgt fail\n");
		goto err_buf_unmap;
	}
	ret = sg_alloc_table(sgt_bak, sgt->nents, GFP_KERNEL);
	if (ret != 0) {
		pr_warn("alloc sgt fail\n");
		goto err_kfree;
	}
	sgl_bak = sgt_bak->sgl;
	for_each_sg(sgt->sgl, sgl, sgt->nents, i)  {
		sg_set_page(sgl_bak, sg_page(sgl), sgl->length, sgl->offset);
		sgl_bak = sg_next(sgl_bak);
	}

	sg_count = dma_map_sg_attrs(p_dev->dev, sgt_bak->sgl,
			      sgt_bak->nents, DMA_FROM_DEVICE,
			      DMA_ATTR_SKIP_CPU_SYNC);

	if (sg_count != 1) {
		pr_warn("dma_map_sg failed:%d\n", sg_count);
		goto err_sgt_free;
	}

	p_item->fd = fd;
	p_item->buf = dmabuf;
	p_item->sgt = sgt_bak;
	p_item->attachment = attachment;
	p_item->dma_addr = sg_dma_address(sgt_bak->sgl);
	ret = 0;
	list_add_tail(&p_item->list, &p_dev->dmabuf_list);
	++p_dev->dmabuf_cnt;


	goto exit;

err_sgt_free:
	sg_free_table(sgt_bak);
err_kfree:
	kfree(sgt_bak);
err_buf_unmap:
	/* unmap attachment sgt, not sgt_bak, because it's not alloc yet! */
	dma_buf_unmap_attachment(attachment, sgt, DMA_FROM_DEVICE);
err_buf_detach:
	dma_buf_detach(dmabuf, attachment);
err_buf_put:
	dma_buf_put(dmabuf);
exit:
	return ret;
}



int dspo_commit(struct dspo_device_t *p_dev, struct dspo_dma_info_t *p_info)
{
	int ret = -1;
	u32 timeout = 0, y_size = 0, y_pitch = 0;
	struct dspo_dmabuf_t *p_item = NULL, *tmp;

	if (!p_dev || !p_info) {
		pr_warn("NULL pointer!\n");
		return ret;
	}


	if (p_info->width > p_dev->cfg.timing_info.x_res ||
	    p_info->height > p_dev->cfg.timing_info.y_res) {
	    pr_warn("Image resolution[%ux%u] is too large\n", p_info->width,
		    p_info->height);
	    goto OUT;
	}

	if (p_info->use_phy_addr == false) {
		p_item = kmalloc(sizeof(struct dspo_dmabuf_t), __GFP_ZERO | GFP_KERNEL);
		if (!p_item) {
			pr_warn("Malloc dma buf item fail!\n");
			goto OUT;
		}
	}

	dspo_dma_ctrl_set(0, REG_SET_MODE, BYTES_512, p_info->fmt);
	if (p_info->fmt == DSPO_FORMAT_ARGB888)
		dspo_rgb2yuv(0);
	else
		dspo_ceu_enable(0, 0);
	dspo_dma_start_check_set(0, 30, 1);

	dspo_dma_size_set(0, p_info->width, p_info->height);
	dspo_dma_mode_line_buf_range_set(0, p_info->width);
	y_pitch = DSPO_ALIGN(p_info->width, 4);
	y_size = p_info->height * y_pitch;
	if (p_info->use_phy_addr == true) {
		dspo_dma_y_addr_set(0, p_info->addr[0]);
		if (p_info->fmt >= DSPO_FORMAT_YUV422_SP_UVUV)
			dspo_dma_uv_addr_set(0, p_info->addr[1]);
	} else {
		ret = dspo_dma_map(p_dev, p_info->fd, p_item);
		if (ret) {
			pr_warn("dspo_dma_map fail:ret:%d!\n", ret);
			goto FREE_ITEM;
		}
		dspo_dma_y_addr_set(0, (u32)p_item->dma_addr);
		if (p_info->fmt >= DSPO_FORMAT_YUV422_SP_UVUV) {
		    dspo_dma_uv_addr_set(0, (u32)(p_item->dma_addr + y_size));
		}
	}

	if (p_info->fmt < DSPO_FORMAT_YUV422_I_VYUY) {
		dspo_yuv_data_src_sel(0, YUV444_TO_PG);
	} else
		dspo_yuv_data_src_sel(0, YUV422_TO_PG);

	if (p_info->fmt < DSPO_FORMAT_YUV422_I_VYUY) {
	    dspo_dma_buf_line_stride_set(0, y_pitch * 4, 0);
	} else if (p_info->fmt < DSPO_FORMAT_YUV422_SP_UVUV) {
	    dspo_dma_buf_line_stride_set(0, y_pitch * 2,
					 p_info->height * 2);
	} else {
	    dspo_dma_buf_line_stride_set(0, y_pitch, y_pitch);
	}
	dspo_dma_start(0);

	p_dev->dma_finish_flag = false;
	timeout = wait_event_timeout(p_dev->queue,
				     p_dev->dma_finish_flag == true, 100);
	if (timeout == 0) {
	    p_dev->dma_finish_flag = true;
	    wake_up(&p_dev->queue);
	    ret = 1;
	    pr_warn("DSPO dma timeout!\n");
	} else {
		p_dev->dma_finish_flag = false;
		ret = 0;
	}

	if (p_dev->dmabuf_cnt == MAX_DMA_BUF_CACHE_CNT) {
		list_for_each_entry_safe(p_item, tmp, &p_dev->dmabuf_list, list) {
			if (p_dev->dmabuf_cnt > 1) {
				list_del(&p_item->list);
				dspo_dma_unmap(p_dev, p_item);
				--p_dev->dmabuf_cnt;
			}
		}
	}

OUT:
	return ret;
FREE_ITEM:
	if (p_item)
		kfree(p_item);
	return ret;
}

static u32 dspo_get_fps(struct dspo_device_t *p_dev)
{
	u32 pre_time_index, cur_time_index;
	u32 pre_time, cur_time;
	u32 fps = 0xff;

	pre_time_index = p_dev->health.sync_time_index;
	cur_time_index =
	    (pre_time_index == 0) ?
	    (100 - 1) : (pre_time_index - 1);

	pre_time = p_dev->health.sync_time[pre_time_index];
	cur_time = p_dev->health.sync_time[cur_time_index];


	 /* HZ:timer interrupt times in 1 sec */
	 /* jiffies:increase 1 when timer interrupt occur. */
	 /* jiffies/HZ:current kernel boot time(second). */
	 /* 1000MS / ((cur_time_jiffies/HZ - pre_time_jiffes/HZ)*1000) */

	if (pre_time != cur_time)
		fps = MSEC_PER_SEC * HZ / (cur_time - pre_time);

	return fps;
}

static int dspo_get_health_info(struct dspo_device_t *p_dev,
				struct dspo_health_info_t *p_info)
{
    if (!p_dev || !p_info) {
		pr_warn("NULL pointer!\n");
		return -1;
    }

    p_dev->health.realtime_fps = dspo_get_fps(p_dev);
    memcpy(p_info, &p_dev->health, sizeof(struct dspo_health_info_t));
    return 0;
}

struct dspo_device_t *create_dspo_device(struct device *p_dev)
{
	struct dspo_device_t *p_dspo = NULL;

	p_dspo = kmalloc(sizeof(struct dspo_device_t), __GFP_ZERO | GFP_KERNEL);
	if (!p_dspo) {
		pr_warn("Malloc dspo_device_t fail!\n");
		goto OUT;
	}

	p_dspo->io = of_iomap(p_dev->of_node, 0);
	if (!p_dspo->io) {
		pr_warn("iormap() of register failed\n");
		goto FREE_DEV;
	}
	p_dspo->irq = irq_of_parse_and_map(p_dev->of_node, 0);
	if (!p_dspo->irq) {
		pr_warn("irq_of_parse_and_map irq fail for transform\n");
		goto IOUNMAP;
	}

	/* clk init */
	p_dspo->clk = of_clk_get(p_dev->of_node, 0);
	if (IS_ERR_OR_NULL(p_dspo->clk)) {
		pr_warn("Fail to get clk\n");
		goto IOUNMAP;
	}

	p_dspo->clk_parent = clk_get_parent(p_dspo->clk);

	p_dspo->dev = p_dev;
	p_dspo->enable = dspo_enable;
	p_dspo->disable = dspo_disable;
	p_dspo->get_config = dspo_get_config;
	p_dspo->mode_support = dspo_mode_support;
	p_dspo->is_enable = dspo_is_enabled;
	p_dspo->commit = dspo_commit;
	p_dspo->get_health_info = dspo_get_health_info;

	mutex_init(&p_dspo->dev_en_mutex);
	init_waitqueue_head(&p_dspo->queue);
	INIT_LIST_HEAD(&p_dspo->dmabuf_list);
	p_dspo->dma_finish_flag = false;

	dspo_set_reg_base(0, (unsigned long)p_dspo->io);




	return p_dspo;

IOUNMAP:
	if (p_dspo->io)
		iounmap(p_dspo->io);
FREE_DEV:
	kfree(p_dspo);
OUT:
	return NULL;
}

int destroy_dspo_device(struct dspo_device_t *p_dev)
{
	struct dspo_dmabuf_t *p_item = NULL, *tmp;

	if (!p_dev)
		goto OUT;

	if (p_dev->is_enable(p_dev))
		p_dev->disable(p_dev);

	list_for_each_entry_safe(p_item, tmp, &p_dev->dmabuf_list, list) {
		list_del(&p_item->list);
		dspo_dma_unmap(p_dev, p_item);
		--p_dev->dmabuf_cnt;
	}
	mutex_destroy(&p_dev->dev_en_mutex);

	if (p_dev->io)
		iounmap(p_dev->io);
	kfree(p_dev);
OUT:
	return 0;
}