#include "FreeRTOS.h"
#include "board.h"
#include "chip.h"

#ifndef abs
#define abs(a, b)	((a > b) ? (a - b) : (b - a))
#endif

static SemaphoreHandle_t pxp_mutex = NULL;
static QueueHandle_t pxp_done;

struct ark_pxp_data
{
	pxp_param param;
	uint32_t block_size;
};

struct ark_pxp_data *g_pxp = NULL;

static int pxp_set_bits(uint32_t reg, uint32_t mask, uint32_t offset, uint32_t val)
{
	uint32_t tmp = PXP_READL(reg);
	tmp &= (~(mask << offset));
	tmp |= ((val&mask) << offset);
	PXP_WRITEL(tmp, reg);
	return 0;
}

/*
 *	PXP Overlay Functions
 */
int pxp_overlay_set_addr(PXP_OL_LAYER layer, uint32_t addr)
{
	uint32_t reg = PXP_OL0BUF + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}

	PXP_WRITEL(addr, reg);
	return 0;
}

int pxp_get_block_size(void)
{
#if 0
	if(PXP_READL(PXP_CTRL) & (1<<23))
		return PXP_BLOCK_SIZE_16X16;

	return PXP_BLOCK_SIZE_8X8;
#else
	if(g_pxp)
		return g_pxp->block_size;
	return (PXP_READL(PXP_CTRL)>>23)&0x1;
#endif
}

int pxp_overlay_set_size(PXP_OL_LAYER layer, uint32_t xbase, uint32_t ybase, uint32_t width, uint32_t height)
{
	uint32_t reg = PXP_OL0SIZE + layer*0x40;
	uint32_t block_size;
	uint32_t div;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}

	block_size = pxp_get_block_size();
	if(block_size == PXP_BLOCK_SIZE_8X8) {
		div = 8;
	} else if(block_size == PXP_BLOCK_SIZE_16X16) {
		div = 16;
	}
	xbase /= div;	//x_blocks = x_pixel /block_size;
	ybase /= div;	//
	width /= div;	//w_blocks = w_pixel /block_size;
	height /= div;	//
	if((xbase > 0xFF) || (ybase > 0xFF) || (width > 0xFF) || (height > 0xFF)) {
		printf("ERR: Invalid overlay:%d xbase:%d, ybase:%d, width:%d or height:%d\n",
			xbase, ybase, layer, width, height);
		return -1;
	}

	PXP_WRITEL((xbase<<24)|(ybase<<16)|(width<<8)|height, reg);
	return 0;
}

int pxp_overlay_set_rop(PXP_OL_LAYER layer, uint32_t rop)
{
	uint32_t reg = PXP_OL0PARAM + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}
	if(rop > 0xF) {
		printf("ERR: Invalid overlay%d rop:%d\n", layer, rop);
		return -1;
	}

	return pxp_set_bits(reg, 0xF, 16, rop);
}

int pxp_overlay_set_format(PXP_OL_LAYER layer, PXP_OL_FORMAT format)
{
	uint32_t reg = PXP_OL0PARAM + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}
	if(format >= PXP_OL_FORMAT_END) {
		printf("ERR: Invalid format:%d\n", format);
		return -1;
	}

	return pxp_set_bits(reg, 0xF, 4, format);
}

int pxp_overlay_set_alpha_value(PXP_OL_LAYER layer, uint32_t value)
{
	uint32_t reg = PXP_OL0PARAM + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}
	if(value > 0xFF) {
		printf("ERR: Invalid alpha value:%d\n", value);
		return -1;
	}

	return pxp_set_bits(reg, 0xFF, 8, value);
}

int pxp_overlay_set_alpha_cntl_type(PXP_OL_LAYER layer, PXP_OL_ALPHA_CNTL_TYPE type)
{
	uint32_t reg = PXP_OL0PARAM + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}
	if(type >= PXP_OL_ALPHA_CNTL_TYPE_END) {
		printf("ERR: Invalid alpha cntl type:%d\n", type);
		return -1;
	}

	return pxp_set_bits(reg, 0x3, 1, type);
}

int pxp_overlay_colorkey_enable(PXP_OL_LAYER layer, uint32_t enable)
{
	uint32_t reg = PXP_OL0PARAM + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}

	return pxp_set_bits(reg, 0x1, 3, (enable?1:0));
}

int pxp_set_overlay_color_key(uint32_t low, uint32_t high)
{
	if((low > 0xFFFFFF) || (high > 0xFFFFFF)) {
		printf("ERR: Invalid low(%d) or high(%d)\n", low, high);
		return -1;
	}
	PXP_WRITEL(low, PXP_OLCKEYL);
	PXP_WRITEL(high, PXP_OLCKEYH);
	return 0;
}

int pxp_overlay_enable(PXP_OL_LAYER layer, uint32_t enable)
{
	uint32_t reg = PXP_OL0PARAM + layer*0x40;

	if(layer >= PXP_OL_COUNT) {
		printf("ERR: Invalid layer:%d\n", layer);
		return -1;
	}

	return pxp_set_bits(reg, 0x1, 0, (enable?1:0));
}


/*
 * PXP Common Functions
 */
static int pxp_set_out_buf(uint32_t addr)
{
	PXP_WRITEL(addr, PXP_OUTBUF);
	return 0;
}

static int pxp_set_out_buf2(uint32_t addr)
{
	PXP_WRITEL(addr, PXP_OUTBUF2);
	return 0;
}

int pxp_set_output_size(uint32_t width, uint32_t height)
{
	if((width > 0xFFF) || ((height > 0xFFF)))  {
		printf("ERR: Invalid width:%d or height:%d\n", width, height);
		return -1;
	}

	return pxp_set_bits(PXP_OUTSIZE, 0xFFFFFF, 0, (width<<12)|height);
}

static int pxp_set_s0_input_yaddr(uint32_t addr)
{
	PXP_WRITEL(addr, PXP_S0BUF);
	return 0;
}

static int pxp_set_s0_input_uaddr(uint32_t addr)
{
	PXP_WRITEL(addr, PXP_S0UBUF);
	return 0;
}

static int pxp_set_s0_input_vaddr(uint32_t addr)
{
	PXP_WRITEL(addr, PXP_S0VBUF);
	return 0;
}

int pxp_set_s0_input_size(uint32_t xoffset, uint32_t yoffset, uint32_t width, uint32_t height)
{
	uint32_t block_size;
	uint32_t in_xbase = xoffset;
	uint32_t in_ybase = yoffset;
	uint32_t in_width = width;
	uint32_t in_height = height;
	uint32_t div;

	block_size = pxp_get_block_size();
	if(block_size == PXP_BLOCK_SIZE_8X8) {
		div = 8;
	} else if(block_size == PXP_BLOCK_SIZE_16X16) {
		div = 16;
	}
	xoffset /= div;	//x_block = x_pixel /block_size;
	yoffset /= div;	//
	width /= div;	//w_block = w_pixel /block_size;
	height /= div;	//
	if((xoffset > 0xFF) || (yoffset > 0xFF) || (width > 0xFF) || (height > 0xFF))  {
		printf("ERR: Invalid xoffset:%d or yoffset:%d, or width:%d or height:%d\n",
			in_xbase, in_ybase, in_width, in_height);
		return -1;
	}

	PXP_WRITEL((xoffset<<24)|(yoffset<<16)|(width<<8)|height, PXP_S0PARAM);
	return 0;
}

int pxp_set_s0_cropping_size(uint32_t xbase, uint32_t ybase, uint32_t width, uint32_t height)
{
	uint32_t block_size;
	uint32_t div;

	block_size = pxp_get_block_size();
	if(block_size == PXP_BLOCK_SIZE_8X8) {
		div = 8;
	} else if(block_size == PXP_BLOCK_SIZE_16X16) {
		div = 16;
	}
	xbase /= div;	//x_blocks = x_pixel /block_size;
	ybase /= div;	//
	width /= div;	//w_blocks = w_pixel /block_size;
	height /= div;	//
	if((xbase > 0xFF) || (ybase > 0xFF) || (width > 0xFF) || (height > 0xFF))  {
		printf("ERR:  Invalid xbase:%d or ybase:%d, or width:%d or height:%d\n",
			xbase*div, ybase*div, width*div, height*div);
		return -1;
	}
	PXP_WRITEL((xbase<<24)|(ybase<<16)|(width<<8)|height, PXP_S0CROP);
	return 0;
}

int pxp_set_s0_scale_factor(uint32_t xscale, uint32_t yscale)
{
	if((xscale >= 0x7FFF) || (yscale >= 0x7FFF)) {
		printf("ERR: Invalid scale_factor xscale:%d or yscale:%d\n", xscale, yscale);
		return -1;
	}
	PXP_WRITEL((yscale<<16)|xscale, PXP_S0SCALE);
	return 0;
}

/*
 *	PXP Global Functions
 */
int pxp_set_s0_scale_offset(uint32_t x, uint32_t y)
{
	if((x >= 0xFFF) || (y >= 0xFFF)) {
		printf("ERR: Invalid x:%d or y:%d\n", x, y);
		return -1;
	}
	PXP_WRITEL((y<<16)|x, PXP_S0OFFSET);
	return 0;
}

int pxp_set_color_space_conversion_coefficient(PXP_CSCOEFF mode)
{
	if(mode >= PCP_CSCCOEFF_END) {
		printf("ERR: Invalid mode:%d\n", mode);
		return -1;
	}
	if(mode == PXP_CSCCOEFF_YUV2RGB) {
		PXP_WRITEL((0x0<<31)|(0x100<<18)|(0x0<<9)|(0x0<<0), PXP_CSCCOEFF0);
		PXP_WRITEL((0x123<<16)|(0x208<<0), PXP_CSCCOEFF1);
		PXP_WRITEL((0x76B<<16)|(0x76C<<0), PXP_CSCCOEFF2);
	} else if(mode == PXP_CSCCOEFF_YCBCR2RGB) {
		PXP_WRITEL((0x1<<31)|(0x12A<<18)|(0x180<<9)|(0x1F0<<0), PXP_CSCCOEFF0);
		PXP_WRITEL((0x198<<16)|(0x204<<0), PXP_CSCCOEFF1);
		PXP_WRITEL((0x730<<16)|(0x79C<<0), PXP_CSCCOEFF2);
	}
	return 0;
}

int pxp_set_block_size(PXP_BLOCK_SIZE_TYPE type)
{
	int ret;

	if(!g_pxp) {
		printf("ERR: Invalid g_pxp(NULL)\n");
		return -1;
	}

	if(type >= PXP_BLOCK_SIZE_END) {
		type = PXP_BLOCK_SIZE_8X8;
		printf("ERR: Invalid type:%d, using default block size(8x8)\n", type);
	}
	g_pxp->block_size = type;
	ret = pxp_set_bits(PXP_CTRL, 0x1, 23, type);

	return ret;
}

int pxp_set_output_alpha(uint32_t alpha)
{
	if(alpha > 0xFF)  {
		printf("ERR: Invalid alpha:%d\n", alpha);
		return -1;
	}

	return pxp_set_bits(PXP_OUTSIZE, 0xFF, 24, alpha);
}

int pxp_set_s0_background_color(uint32_t color)
{
	PXP_WRITEL(color, PXP_S0BACKGROUND);
	return 0;
}

int pxp_set_s0_color_key(uint32_t low, uint32_t high)
{
	if((low > 0xFFFFFF) || (high > 0xFFFFFF)) {
		printf("ERR: Invalid low(%d) or high(%d)\n", low, high);
		return -1;
	}
	PXP_WRITEL(low, PXP_S0CKEYL);
	PXP_WRITEL(high, PXP_S0CKEYH);
	return 0;
}

int pxp_set_input_addr(uint32_t yaddr, uint32_t uaddr, uint32_t vaddr)
{
	struct ark_pxp_data *pxp = g_pxp;

	if(!pxp) {
		printf("ERR: Invalid g_pxp(NULL)\n");
		return -1;
	}
	if(!yaddr) {
		printf("ERR: Invalid yaddr(0)\n");
		return -1;
	}

	pxp_set_s0_input_yaddr(yaddr);
	pxp_set_s0_input_uaddr(uaddr);
	pxp_set_s0_input_vaddr(vaddr);

	return 0;
}

int pxp_set_output_addr(uint32_t addr, uint32_t addr2)
{
	struct ark_pxp_data *pxp = g_pxp;
	if(!pxp) {
		printf("ERR: Invalid g_pxp(NULL)\n");
		return -1;
	}
	if(!addr) {
		printf("ERR: Invalid yaddr(0)\n");
		return -1;
	}
	pxp_set_out_buf(addr);
	pxp_set_out_buf2(addr2);
	return 0;
}

int pxp_set_control_param(pxp_param *param)
{
	struct ark_pxp_data *pxp = g_pxp;
	if(!pxp || !param) {
		printf("ERR: Invalid pxp(%p) or param(%p)\n", pxp, param);
		return -1;
	}

	memcpy(&pxp->param, param, sizeof(pxp_param));
	return 0;
}

int pxp_reset(void)
{
	PXP_WRITEL(1 << 31, PXP_CTRL);
    udelay(10);
	PXP_WRITEL(0, PXP_CTRL);
	
	return 0;
}

int pxp_start(void)
{
	struct ark_pxp_data *pxp = g_pxp;
	pxp_param *param = NULL;
	PXP_BLOCK_SIZE_TYPE block_size = PXP_BLOCK_SIZE_8X8;

	if(!pxp) {
		printf("ERR: Invalid pxp(%p)\n", pxp);
		return -1;
	}
	param = &pxp->param;
	block_size = pxp->block_size;
	PXP_WRITEL(
		(0<<SFTRST) |
		(0<<CLKGATE) |
		((param->out_interlace_enable)<<INTERLACED_OUTPUT) |
		((param->in_interlace_enable)<<INTERLACED_INPUT) |
		((block_size)<<BLOCK_SIZE) |
		((param->alpha_out_enable)<<ALPHA_OUTPUT) |
		((param->in_place)<<IN_PLACE) |
		((param->crop_enable)<<CROP) |
		((param->scale_enable)<<SCALE) |
		((param->in_format&0xF)<<S0_FORMAT) |
		((param->vflip_enable)<<VFLIP) |
		((param->hflip_enable)<<HFLIP) |
		((param->rotate&0x3)<<ROTATE) |
		((param->out_format&0xF)<<OUTBUF_FORMAT) |
		(0<<LCD_HANDSHAKE_EN) |
		(0<<NEXT_IRQ_EN) |
		(1<<IRQ_EN) |
		(1<<PXP_EN)
		, PXP_CTRL);

	return 0;
}

/* this function can not used in isr */
int pxp_scaler_rotate(uint32_t s0buf, uint32_t s0ubuf, uint32_t s0vbuf,
					int s0format, uint32_t s0width, uint32_t s0height,
					uint32_t outbuf, uint32_t outbuf2, int outformat,
					uint32_t outwidth, uint32_t outheight, int outangle)
{
	uint32_t ctrl;
	int ret = 0;
	int cutx = 0;
	int cuty = 0;

	configASSERT(outangle >= PXP_ROTATE_0 && outangle <= PXP_ROTATE_270 && g_pxp);

	if(abs(s0width, outwidth) < 16)
		cutx = 0;
	if(abs(s0height, outheight) < 16)
		cutx = 0;

	if (outangle == PXP_ROTATE_90 || outangle == PXP_ROTATE_270) {
		uint32_t tmp = outheight;
		outheight = outwidth;
		outwidth = tmp;
	}

	xSemaphoreTake(pxp_mutex, portMAX_DELAY);

	pxp_reset();
	pxp_set_block_size(PXP_BLOCK_SIZE_8X8);
	pxp_set_output_addr(outbuf, outbuf2);
	pxp_set_output_size(outwidth, outheight);
	pxp_set_output_alpha(0xff);
	pxp_set_input_addr(s0buf, s0ubuf, s0vbuf);
	pxp_set_s0_input_size(0, 0, s0width, s0height);
	pxp_set_s0_background_color(0);
	pxp_set_s0_cropping_size(0, 0, outwidth, outheight);
	pxp_set_s0_scale_factor(s0width * 0x1000 / (outwidth + cutx), s0height * 0x1000 / (outheight + cuty));
	pxp_set_color_space_conversion_coefficient(PXP_CSCCOEFF_YCBCR2RGB);

	pxp_param param;
	memset(&param, 0, sizeof(pxp_param));
	param.in_format				= s0format;
	param.out_format			= outformat;
	param.in_interlace_enable	= 0;
	param.out_interlace_enable	= 0;
	param.rotate				= outangle;
	param.vflip_enable			= 0;
#ifdef PXP_OUTPUT_MIRROR
	param.hflip_enable			= 1;
#endif
	param.crop_enable			= 1;
	param.scale_enable			= 1;
	if(outformat == PXP_OUT_FORMAT_ARGB888)
		param.alpha_out_enable = 1;
	pxp_set_control_param(&param);

	xQueueReset(pxp_done);
	pxp_start();
	if (xQueueReceive(pxp_done, NULL, pdMS_TO_TICKS(1000)) != pdTRUE) {
		printf("pxp timeout.\n");
		ret = -ETIMEDOUT;
	}

	xSemaphoreGive(pxp_mutex);

	return ret;
}

int pxp_rotate(uint32_t s0buf, uint32_t s0ubuf, uint32_t s0vbuf,
					int s0format, uint32_t s0width, uint32_t s0height,
					uint32_t outbuf, uint32_t outbuf2, int outformat,
					int outangle)
{
	uint32_t ctrl;
	int ret = 0;

	configASSERT(outangle >= PXP_ROTATE_0 && outangle <= PXP_ROTATE_270);

	xSemaphoreTake(pxp_mutex, portMAX_DELAY);

	pxp_reset();
	pxp_set_block_size(PXP_BLOCK_SIZE_8X8);
	pxp_set_output_addr(outbuf, outbuf2);
	pxp_set_output_size(s0width, s0height);
	pxp_set_output_alpha(0xff);
	pxp_set_input_addr(s0buf, s0ubuf, s0vbuf);
	pxp_set_s0_input_size(0, 0, s0width, s0height);
	pxp_set_s0_background_color(0);
	pxp_set_color_space_conversion_coefficient(PXP_CSCCOEFF_YCBCR2RGB);

	pxp_param param;
	memset(&param, 0, sizeof(pxp_param));
	param.in_format				= s0format;
	param.out_format			= outformat;
	param.in_interlace_enable	= 0;
	param.out_interlace_enable	= 0;
	param.rotate				= outangle;
	param.vflip_enable			= 0;
#ifdef PXP_OUTPUT_MIRROR
	param.hflip_enable			= 1;
#endif
	param.crop_enable			= 1;
	param.scale_enable			= 1;
	if(outformat == PXP_OUT_FORMAT_ARGB888)
		param.alpha_out_enable = 1;
	pxp_set_control_param(&param);

	xQueueReset(pxp_done);
	pxp_start();

	if (xQueueReceive(pxp_done, NULL, pdMS_TO_TICKS(1000)) != pdTRUE) {
		printf("pxp timeout.\n");
		ret = -ETIMEDOUT;
	}

	xSemaphoreGive(pxp_mutex);

	return ret;
}

static void pxp_interupt_handler(void *param)
{
	uint32_t status = PXP_READL(PXP_STAT);

	PXP_WRITEL(status, PXP_STAT_CLR);
	if (status & 1) {
		xQueueSendFromISR(pxp_done, NULL, 0);
	}
}

int pxp_init(void)
{
	pxp_mutex = xSemaphoreCreateMutex();
	pxp_done = xQueueCreate(1, 0);

	g_pxp = (struct ark_pxp_data *)malloc(sizeof(struct ark_pxp_data));
	if(!g_pxp) {
		printf("ERR: rt_malloc failed\n");
		return -1;
	}
	memset(g_pxp, 0, sizeof(struct ark_pxp_data));
	g_pxp->block_size = PXP_BLOCK_SIZE_8X8;

	pxp_reset();
	pxp_set_color_space_conversion_coefficient(PXP_CSCCOEFF_YCBCR2RGB);

	request_irq(PXP_IRQn, 0, pxp_interupt_handler, NULL);

	return 0;
}