path: root/src/soc/nvidia/tegra124/displayhack.c

/* this is too ugly to be allowed to live. But it's what works for now. */
/*
 * This file is part of the coreboot project.
 *
 * Copyright 2013 Google Inc.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2 of the License.
 *
 * 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <console/console.h>
#include <arch/io.h>
#include <stdint.h>
#include <lib.h>
#include <stdlib.h>
#include <delay.h>
#include <soc/addressmap.h>
#include <device/device.h>
#include <stdlib.h>
#include <string.h>
#include <cpu/cpu.h>
#include <boot/tables.h>
#include <cbmem.h>
#include <soc/nvidia/tegra/dc.h>
#include "clk_rst.h"
#include <soc/clock.h>
#include "chip.h"
#include "sor.h"
#include <soc/display.h>
#include <soc/ardisplay.h>
#include <soc/arsor.h>
#include <soc/ardpaux.h>
//#include <soc/nvidia/tegra/displayport.h>
extern int dump;
unsigned long READL(void *p);
void WRITEL(unsigned long value, void *p);
void debug_dpaux_print(u32 addr, u32 size);
int dpaux_write(u32 addr, u32 size, u32 data);
int dpaux_read(u32 addr, u32 size, u8 * data);

void init_dca_regs(void)
{
	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)TEGRA_CLK_RST_BASE;
//  u32 val;

	printk(BIOS_SPEW, "JZ: %s: entry\n", __func__);

#define SWR_DISP1_RST				(1 << 27)
#define SWR_HOST1X_RST				(1 << 28)
#define CLK_ENB_DISP1				SWR_DISP1_RST
#define CLK_ENB_HOST1X				SWR_HOST1X_RST
//    REG(CLK_RST_CONTROLLER_RST_DEVICES_L_0,    SWR_DISP1_RST, 1);
//    REG(CLK_RST_CONTROLLER_RST_DEVICES_L_0,    SWR_DISP1_RST, 0);
//    REG(CLK_RST_CONTROLLER_CLK_OUT_ENB_L_0,    CLK_ENB_DISP1, 1);
	/* enable disp1 */
	setbits_le32(&clkrst->rst_dev_l, SWR_DISP1_RST);	// Set Reset
	clrbits_le32(&clkrst->rst_dev_l, SWR_DISP1_RST);	// Clear Reset
	setbits_le32(&clkrst->clk_out_enb_l, CLK_ENB_DISP1);	// Set Enable
	WRITEL(0x00000000, (void *)(0x60006000 + 0x138));	// CLK_SOURCE_DISP1 = PLLP
//  WRITEL(0x40000000, (void *)(0x60006000 + 0x138));   // CLK_SOURCE_DISP1 = PLLD
	/* enable host1x */
	clrbits_le32(&clkrst->rst_dev_l, SWR_HOST1X_RST);	// Clear Reset
	setbits_le32(&clkrst->clk_out_enb_l, CLK_ENB_HOST1X);	// Set Enable
	WRITEL(0x80000000, (void *)(0x60006000 + 0x180));	// CLK_SOURCE_HOST1X = PLLP
//  WRITEL(0x40000000, (0x60006000 + 0x180));   // CLK_SOURCE_HOST1X = PLLC

#if 1
#define DCA_WRITE(reg, val) \
	{ \
		WRITEL(val, (void *)(TEGRA_ARM_DISPLAYA + (reg<<2)));	\
	}
#define DCA_READ_M_WRITE(reg, mask, val) \
	{ \
	u32 _reg_val; \
	_reg_val = READL( (void *)(TEGRA_ARM_DISPLAYA + (reg<<2))); \
	_reg_val &= ~mask; \
	_reg_val |= val; \
	WRITEL(_reg_val,  (void *)(TEGRA_ARM_DISPLAYA + (reg<<2))); \
	}
#else // read back
#define DCA_WRITE(reg, val) \
	{ \
	printk(BIOS_SPEW,"DCA_WRITE: addr %#x = %#x\n",
	(unsigned int) (TEGRA_ARM_DISPLAYA + (reg<<2)), val);  \
	WRITEL(val,  (void *)(TEGRA_ARM_DISPLAYA + (reg<<2))); \
	printk(BIOS_SPEW,"reads as %#x\n",  \
		(unsigned int)READL( (void *)(TEGRA_ARM_DISPLAYA + (reg<<2)))); \
	}
#endif

	DCA_WRITE(DC_DISP_DISP_CLOCK_CONTROL_0, 0x00000006);	//0x542410b8
	DCA_WRITE(DC_CMD_INT_STATUS_0, 0xffffffff);	//0x542400dc
	DCA_WRITE(DC_CMD_INT_MASK_0, 0x00000000);	//0x542400e0
	DCA_WRITE(DC_CMD_INT_ENABLE_0, 0x00800701);	//0x542400e4
	DCA_WRITE(DC_CMD_INT_POLARITY_0, 0x00c00706);	//0x542400ec
	DCA_WRITE(DC_DISP_DISP_SIGNAL_OPTIONS0_0, 0x00000000);	//0x54241000
	DCA_WRITE(DC_DISP_DISP_SIGNAL_OPTIONS1_0, 0x00000000);	//0x54241004
	DCA_WRITE(DC_DISP_DISP_WIN_OPTIONS_0, 0x00000000);	//0x54241008
	DCA_WRITE(DC_DISP_MEM_HIGH_PRIORITY_0, 0x00000000);	//0x5424100c
	DCA_WRITE(DC_DISP_MEM_HIGH_PRIORITY_TIMER_0, 0x00000000);	//0x54241010
	DCA_WRITE(DC_DISP_DISP_TIMING_OPTIONS_0, 0x00000000);	//0x54241014
	DCA_WRITE(DC_DISP_REF_TO_SYNC_0, 0x00000000);	//0x54241018
	DCA_WRITE(DC_DISP_SYNC_WIDTH_0, 0x00000000);	//0x5424101c
	DCA_WRITE(DC_DISP_BACK_PORCH_0, 0x00000000);	//0x54241020
	DCA_WRITE(DC_DISP_DISP_ACTIVE_0, 0x00030003);	//0x54241024
	DCA_WRITE(DC_DISP_FRONT_PORCH_0, 0x00000000);	//0x54241028
	DCA_WRITE(DC_DISP_H_PULSE0_CONTROL_0, 0x00000000);	//0x5424102c
	DCA_WRITE(DC_DISP_H_PULSE0_POSITION_A_0, 0x00000000);	//0x54241030
	DCA_WRITE(DC_DISP_H_PULSE0_POSITION_B_0, 0x00000000);	//0x54241034
	DCA_WRITE(DC_DISP_H_PULSE0_POSITION_C_0, 0x00000000);	//0x54241038
	DCA_WRITE(DC_DISP_H_PULSE0_POSITION_D_0, 0x00000000);	//0x5424103c
	DCA_WRITE(DC_DISP_H_PULSE1_CONTROL_0, 0x00000000);	//0x54241040
	DCA_WRITE(DC_DISP_H_PULSE1_POSITION_A_0, 0x00000000);	//0x54241044
	DCA_WRITE(DC_DISP_H_PULSE1_POSITION_B_0, 0x00000000);	//0x54241048
	DCA_WRITE(DC_DISP_H_PULSE1_POSITION_C_0, 0x00000000);	//0x5424104c
	DCA_WRITE(DC_DISP_H_PULSE1_POSITION_D_0, 0x00000000);	//0x54241050
	DCA_WRITE(DC_DISP_H_PULSE2_CONTROL_0, 0x00000000);	//0x54241054
	DCA_WRITE(DC_DISP_H_PULSE2_POSITION_A_0, 0x00000000);	//0x54241058
	DCA_WRITE(DC_DISP_H_PULSE2_POSITION_B_0, 0x00000000);	//0x5424105c
	DCA_WRITE(DC_DISP_H_PULSE2_POSITION_C_0, 0x00000000);	//0x54241060
	DCA_WRITE(DC_DISP_H_PULSE2_POSITION_D_0, 0x00000000);	//0x54241064
	DCA_WRITE(DC_DISP_V_PULSE0_CONTROL_0, 0x00000000);	//0x54241068
	DCA_WRITE(DC_DISP_V_PULSE0_POSITION_A_0, 0x00000000);	//0x5424106c
	DCA_WRITE(DC_DISP_V_PULSE0_POSITION_B_0, 0x00000000);	//0x54241070
	DCA_WRITE(DC_DISP_V_PULSE0_POSITION_C_0, 0x00000000);	//0x54241074
	DCA_WRITE(DC_DISP_V_PULSE1_CONTROL_0, 0x00000000);	//0x54241078
	DCA_WRITE(DC_DISP_V_PULSE1_POSITION_A_0, 0x00000000);	//0x5424107c
	DCA_WRITE(DC_DISP_V_PULSE1_POSITION_B_0, 0x00000000);	//0x54241080
	DCA_WRITE(DC_DISP_V_PULSE1_POSITION_C_0, 0x00000000);	//0x54241084
	DCA_WRITE(DC_DISP_V_PULSE2_CONTROL_0, 0x00000000);	//0x54241088
	DCA_WRITE(DC_DISP_V_PULSE2_POSITION_A_0, 0x00000000);	//0x5424108c
	DCA_WRITE(DC_DISP_V_PULSE3_CONTROL_0, 0x00000000);	//0x54241090
	DCA_WRITE(DC_DISP_V_PULSE3_POSITION_A_0, 0x00000000);	//0x54241094
	DCA_WRITE(DC_DISP_M0_CONTROL_0, 0x00000000);	//0x54241098
	DCA_WRITE(DC_DISP_M1_CONTROL_0, 0x00000000);	//0x5424109c
	DCA_WRITE(DC_DISP_DI_CONTROL_0, 0x00000000);	//0x542410a0
	DCA_WRITE(DC_DISP_PP_CONTROL_0, 0x00000000);	//0x542410a4
	DCA_WRITE(DC_DISP_PP_SELECT_A_0, 0x00000000);	//0x542410a8
	DCA_WRITE(DC_DISP_PP_SELECT_B_0, 0x00000000);	//0x542410ac
	DCA_WRITE(DC_DISP_PP_SELECT_C_0, 0x00000000);	//0x542410b0
	DCA_WRITE(DC_DISP_PP_SELECT_D_0, 0x00000000);	//0x542410b4
	DCA_WRITE(DC_DISP_DISP_INTERFACE_CONTROL_0, 0x00000000);	//0x542410bc
	DCA_WRITE(DC_DISP_DISP_COLOR_CONTROL_0, 0x00000000);	//0x542410c0
	DCA_WRITE(DC_DISP_SHIFT_CLOCK_OPTIONS_0, 0x00000000);	//0x542410c4
	DCA_WRITE(DC_DISP_DATA_ENABLE_OPTIONS_0, 0x00000000);	//0x542410c8
	DCA_WRITE(DC_DISP_SERIAL_INTERFACE_OPTIONS_0, 0x00000000);	//0x542410cc
	DCA_WRITE(DC_DISP_LCD_SPI_OPTIONS_0, 0x00000000);	//0x542410d0
	DCA_WRITE(DC_DISP_COLOR_KEY0_LOWER_0, 0x00000000);	//0x542410d8
	DCA_WRITE(DC_DISP_COLOR_KEY0_UPPER_0, 0x00000000);	//0x542410dc
	DCA_WRITE(DC_DISP_COLOR_KEY1_LOWER_0, 0x00000000);	//0x542410e0
	DCA_WRITE(DC_DISP_COLOR_KEY1_UPPER_0, 0x00000000);	//0x542410e4
	DCA_WRITE(DC_DISP_CURSOR_FOREGROUND_0, 0x00000000);	//0x542410f0
	DCA_WRITE(DC_DISP_CURSOR_BACKGROUND_0, 0x00000000);	//0x542410f4
	DCA_WRITE(DC_DISP_CURSOR_START_ADDR_0, 0x00200000);	//0x542410f8
	DCA_WRITE(DC_DISP_CURSOR_START_ADDR_NS_0, 0x00000000);	//0x542410fc
	DCA_WRITE(DC_DISP_CURSOR_POSITION_0, 0x00000000);	//0x54241100
	DCA_WRITE(DC_DISP_CURSOR_POSITION_NS_0, 0x00000000);	//0x54241104
	DCA_WRITE(DC_DISP_INIT_SEQ_CONTROL_0, 0x00000000);	//0x54241108
	DCA_WRITE(DC_DISP_SPI_INIT_SEQ_DATA_A_0, 0x00000000);	//0x5424110c
	DCA_WRITE(DC_DISP_SPI_INIT_SEQ_DATA_B_0, 0x00000000);	//0x54241110
	DCA_WRITE(DC_DISP_SPI_INIT_SEQ_DATA_C_0, 0x00000000);	//0x54241114
	DCA_WRITE(DC_DISP_SPI_INIT_SEQ_DATA_D_0, 0x00000000);	//0x54241118
	DCA_WRITE(DC_DISP_DC_MCCIF_FIFOCTRL_0, 0x00000000);	//0x54241200
	DCA_WRITE(DC_DISP_MCCIF_DISPLAY0A_HYST_0, 0xcf401f1f);	//0x54241204
	DCA_WRITE(DC_DISP_MCCIF_DISPLAY0B_HYST_0, 0xcf401f1f);	//0x54241208
	DCA_WRITE(DC_DISP_MCCIF_DISPLAY0C_HYST_0, 0xcf401f1f);	//0x5424120c
	DCA_WRITE(DC_DISP_DISP_MISC_CONTROL_0, 0x00000002);	//0x54241304
	DCA_WRITE(DC_DISP_SD_CONTROL_0, 0x00004000);	//0x54241308
	DCA_WRITE(DC_DISP_SD_CSC_COEFF_0, 0x00000000);	//0x5424130c

	DCA_WRITE(DC_DISP_SD_LUT_0, 0x00000000);	//0x54241310
#define DC_DISP_SD_LUT_1_0	0x4c5
#define DC_DISP_SD_LUT_2_0	0x4c6
#define DC_DISP_SD_LUT_3_0	0x4c7
#define DC_DISP_SD_LUT_4_0	0x4c8
#define DC_DISP_SD_LUT_5_0	0x4c9
#define DC_DISP_SD_LUT_6_0	0x4ca
#define DC_DISP_SD_LUT_7_0	0x4cb
#define DC_DISP_SD_LUT_8_0	0x4cc
	DCA_WRITE(DC_DISP_SD_LUT_1_0, 0x00000000);	//0x54241314
	DCA_WRITE(DC_DISP_SD_LUT_2_0, 0x00000000);	//0x54241318
	DCA_WRITE(DC_DISP_SD_LUT_3_0, 0x00000000);	//0x5424131c
	DCA_WRITE(DC_DISP_SD_LUT_4_0, 0x00000000);	//0x54241320
	DCA_WRITE(DC_DISP_SD_LUT_5_0, 0x00000000);	//0x54241324
	DCA_WRITE(DC_DISP_SD_LUT_6_0, 0x00000000);	//0x54241328
	DCA_WRITE(DC_DISP_SD_LUT_7_0, 0x00000000);	//0x5424132c
	DCA_WRITE(DC_DISP_SD_LUT_8_0, 0x00000000);	//0x54241330
	DCA_WRITE(DC_DISP_SD_FLICKER_CONTROL_0, 0x00000000);	//0x54241334
	DCA_WRITE(DC_DISP_SD_PIXEL_COUNT_0, 0x00000000);	//0x54241338

	DCA_WRITE(DC_DISP_SD_HISTOGRAM_0, 0x00000000);	//0x5424133c
#define DC_DISP_SD_HISTOGRAM_1_0	0x4d0
#define DC_DISP_SD_HISTOGRAM_2_0	0x4d1
#define DC_DISP_SD_HISTOGRAM_3_0	0x4d2
#define DC_DISP_SD_HISTOGRAM_4_0	0x4d3
#define DC_DISP_SD_HISTOGRAM_5_0	0x4d4
#define DC_DISP_SD_HISTOGRAM_6_0	0x4d5
#define DC_DISP_SD_HISTOGRAM_7_0	0x4d6
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_1_0, 0x00000000);	//0x54241340
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_2_0, 0x00000000);	//0x54241344
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_3_0, 0x00000000);	//0x54241348
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_4_0, 0x00000000);	//0x5424134c
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_5_0, 0x00000000);	//0x54241350
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_6_0, 0x00000000);	//0x54241354
	DCA_WRITE(DC_DISP_SD_HISTOGRAM_7_0, 0x00000000);	//0x54241358
	DCA_WRITE(DC_DISP_SD_BL_PARAMETERS_0, 0x00000400);	//0x5424135c
	DCA_WRITE(DC_DISP_SD_BL_TF_0, 0x00000000);	//0x54241360
#define DC_DISP_SD_BL_TF_1_0	0x4d9
#define DC_DISP_SD_BL_TF_2_0	0x4da
#define DC_DISP_SD_BL_TF_3_0	0x4db
	DCA_WRITE(DC_DISP_SD_BL_TF_1_0, 0x00000000);	//0x54241364
	DCA_WRITE(DC_DISP_SD_BL_TF_2_0, 0x00000000);	//0x54241368
	DCA_WRITE(DC_DISP_SD_BL_TF_3_0, 0x00000000);	//0x5424136c
	DCA_WRITE(DC_DISP_SD_BL_CONTROL_0, 0x00000000);	//0x54241370
	DCA_WRITE(DC_DISP_SD_HW_K_VALUES_0, 0x00000000);	//0x54241374
	DCA_WRITE(DC_DISP_SD_MAN_K_VALUES_0, 0x00000000);	//0x54241378
	DCA_WRITE(DC_DISP_SD_K_LIMIT_0, 0x00000000);	//0x5424137c
	DCA_WRITE(DC_DISP_SD_WINDOW_POSITION_0, 0x00000000);	//0x54241380
	DCA_WRITE(DC_DISP_SD_WINDOW_SIZE_0, 0x00000000);	//0x54241384
	DCA_WRITE(DC_DISP_SD_SOFT_CLIPPING_0, 0x02000080);	//0x54241388
	DCA_WRITE(DC_DISP_SD_SMOOTH_K_0, 0x00000000);	//0x5424138c
	DCA_WRITE(DC_DISP_BLEND_BACKGROUND_COLOR_0, 0x00000000);	//0x54241390
	DCA_WRITE(DC_DISP_INTERLACE_CONTROL_0, 0x00000000);	//0x54241394
	DCA_WRITE(DC_DISP_INTERLACE_FIELD2_REF_TO_SYNC_0, 0x00000000);	//0x54241398
	DCA_WRITE(DC_DISP_INTERLACE_FIELD2_SYNC_WIDTH_0, 0x00000000);	//0x5424139c
	DCA_WRITE(DC_DISP_INTERLACE_FIELD2_BACK_PORCH_0, 0x00000000);	//0x542413a0
	DCA_WRITE(DC_DISP_INTERLACE_FIELD2_FRONT_PORCH_0, 0x00000000);	//0x542413a4
	DCA_WRITE(DC_DISP_INTERLACE_FIELD2_DISP_ACTIVE_0, 0x00000000);	//0x542413a8
	DCA_WRITE(DC_DISP_CURSOR_UNDERFLOW_CTRL_0, 0x00000000);	//0x542413ac
	DCA_WRITE(DC_DISP_CURSOR_START_ADDR_HI_0, 0x00000000);	//0x542413b0
	DCA_WRITE(DC_DISP_CURSOR_START_ADDR_HI_NS_0, 0x00000000);	//0x542413b4
	DCA_WRITE(DC_DISP_CURSOR_INTERLACE_CONTROL_0, 0x00000000);	//0x542413b8
	DCA_WRITE(DC_DISP_CSC2_CONTROL_0, 0x00000000);	//0x542413bc
	DCA_WRITE(DC_DISP_BLEND_CURSOR_CONTROL_0, 0x00000000);	//0x542413c4
	DCA_WRITE(DC_DISP_DVFS_CURSOR_CONTROL_0, 0x00000003);	//0x542413c8
	DCA_WRITE(DC_DISP_CURSOR_UFLOW_DBG_PIXEL_0, 0x00000000);	//0x542413cc
	DCA_WRITE(DC_DISP_CURSOR_SPOOLUP_CONTROL_0, 0x00000001);	//0x542413d0
	DCA_WRITE(DC_DISP_DISPLAY_CLK_GATE_OVERRIDE_0, 0x00000000);	//0x542413d4
	DCA_WRITE(DC_DISP_DISPLAY_DBG_TIMING_0, 0x00000000);	//0x542413d8
	DCA_WRITE(DC_DISP_DISPLAY_SPARE0_0, 0x00000000);	//0x542413dc
	DCA_WRITE(DC_DISP_DISPLAY_SPARE1_0, 0x00000000);	//0x542413e0

#define wr32(reg, val) \
	WRITEL(val,  (void *)reg)

	wr32((TEGRA_ARM_DISPLAYA + 0x0200), 0x00000000);
	wr32((TEGRA_ARM_DISPLAYA + 0x0400), 0x00000000);

	DCA_WRITE(DC_CMD_DISPLAY_WINDOW_HEADER_0, 0x000000F0);
	DCA_WRITE(DC_WIN_A_WIN_OPTIONS_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_BYTE_SWAP_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_BUFFER_CONTROL_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_COLOR_DEPTH_0, 0x0000000C);
	DCA_WRITE(DC_WIN_A_POSITION_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_SIZE_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_PRESCALED_SIZE_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_H_INITIAL_DDA_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_V_INITIAL_DDA_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_DDA_INCREMENT_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_LINE_STRIDE_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_DV_CONTROL_0, 0x00000000);

	DCA_WRITE(DC_WIN_A_BLEND_LAYER_CONTROL_0, 0x01000000);
	DCA_WRITE(DC_WIN_A_BLEND_MATCH_SELECT_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_BLEND_NOMATCH_SELECT_0, 0x00000000);
	DCA_WRITE(DC_WIN_A_BLEND_ALPHA_1BIT_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_PALETTE_COLOR_EXT_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_YOF_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KYRGB_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KUR_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KVR_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KUG_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KVG_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KUB_0, 0x00000000);
	DCA_WRITE(DC_WINC_A_CSC_KVB_0, 0x00000000);
	DCA_WRITE(DC_WINBUF_A_START_ADDR_HI_0, 0x00000000);
	DCA_WRITE(DC_WINBUF_A_ADDR_H_OFFSET_0, 0x00000000);
	DCA_WRITE(DC_WINBUF_A_ADDR_V_OFFSET_0, 0x00000000);
	DCA_WRITE(DC_CMD_DISPLAY_WINDOW_HEADER_0, 0x00000000);

	DCA_WRITE(DC_COM_CRC_CONTROL_0, 0x00000000);	//0x54240c00
	DCA_WRITE(DC_COM_CRC_CHECKSUM_0, 0x00000000);	//0x54240c04
	DCA_WRITE(DC_COM_PIN_OUTPUT_ENABLE0_0, 0x00000000);	//0x54240c08
	DCA_WRITE(DC_COM_PIN_OUTPUT_ENABLE1_0, 0x00000000);	//0x54240c0c
	DCA_WRITE(DC_COM_PIN_OUTPUT_ENABLE2_0, 0x00510104);	//0x54240c10
	DCA_WRITE(DC_COM_PIN_OUTPUT_ENABLE3_0, 0x00000555);	//0x54240c14
	DCA_WRITE(DC_COM_PIN_OUTPUT_POLARITY0_0, 0x00000000);	//0x54240c18
	DCA_WRITE(DC_COM_PIN_OUTPUT_POLARITY1_0, 0x00000000);	//0x54240c1c
	DCA_WRITE(DC_COM_PIN_OUTPUT_POLARITY2_0, 0x00000000);	//0x54240c20
	DCA_WRITE(DC_COM_PIN_OUTPUT_POLARITY3_0, 0x00000000);	//0x54240c24
	DCA_WRITE(DC_COM_PIN_OUTPUT_DATA0_0, 0x00000000);	//0x54240c28
	DCA_WRITE(DC_COM_PIN_OUTPUT_DATA1_0, 0x00000000);	//0x54240c2c
	DCA_WRITE(DC_COM_PIN_OUTPUT_DATA2_0, 0x00000000);	//0x54240c30
	DCA_WRITE(DC_COM_PIN_OUTPUT_DATA3_0, 0x00000000);	//0x54240c34
	DCA_WRITE(DC_COM_PIN_INPUT_DATA0_0, 0x00000000);	//0x54240c48
	DCA_WRITE(DC_COM_PIN_INPUT_DATA1_0, 0x00000000);	//0x54240c4c
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT0_0, 0x00000000);	//0x54240c50
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT1_0, 0x00000000);	//0x54240c54
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT2_0, 0x00000000);	//0x54240c58
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT3_0, 0x00000000);	//0x54240c5c
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT4_0, 0x00000000);	//0x54240c60
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT5_0, 0x00000000);	//0x54240c64
	DCA_WRITE(DC_COM_PIN_OUTPUT_SELECT6_0, 0x00000000);	//0x54240c68
	DCA_WRITE(DC_COM_PIN_MISC_CONTROL_0, 0x00000000);	//0x54240c6c
	DCA_WRITE(DC_COM_PM0_CONTROL_0, 0x00000000);	//0x54240c70
	DCA_WRITE(DC_COM_PM0_DUTY_CYCLE_0, 0x00000000);	//0x54240c74
	DCA_WRITE(DC_COM_PM1_CONTROL_0, 0x00000000);	//0x54240c78
	DCA_WRITE(DC_COM_PM1_DUTY_CYCLE_0, 0x00000000);	//0x54240c7c
	DCA_WRITE(DC_COM_SPI_CONTROL_0, 0x00000000);	//0x54240c80
	DCA_WRITE(DC_COM_SPI_START_BYTE_0, 0x00000000);	//0x54240c84
	DCA_WRITE(DC_COM_HSPI_WRITE_DATA_AB_0, 0x00000000);	//0x54240c88
	DCA_WRITE(DC_COM_HSPI_WRITE_DATA_CD_0, 0x00000000);	//0x54240c8c
	DCA_WRITE(DC_COM_HSPI_CS_DC_0, 0x00000000);	//0x54240c90
	DCA_WRITE(DC_COM_SCRATCH_REGISTER_A_0, 0x00000000);	//0x54240c94
	DCA_WRITE(DC_COM_SCRATCH_REGISTER_B_0, 0x00000000);	//0x54240c98
	DCA_WRITE(DC_COM_CRC_CHECKSUM_LATCHED_0, 0x00000000);	//0x54240ca4
	DCA_WRITE(DC_COM_CMU_CSC_KRR_0, 0x00000000);	//0x54240ca8
	DCA_WRITE(DC_COM_CMU_CSC_KGR_0, 0x00000000);	//0x54240cac
	DCA_WRITE(DC_COM_CMU_CSC_KBR_0, 0x00000000);	//0x54240cb0
	DCA_WRITE(DC_COM_CMU_CSC_KRG_0, 0x00000000);	//0x54240cb4
	DCA_WRITE(DC_COM_CMU_CSC_KGG_0, 0x00000000);	//0x54240cb8
	DCA_WRITE(DC_COM_CMU_CSC_KBG_0, 0x00000000);	//0x54240cbc
	DCA_WRITE(DC_COM_CMU_CSC_KRB_0, 0x00000000);	//0x54240cc0
	DCA_WRITE(DC_COM_CMU_CSC_KGB_0, 0x00000000);	//0x54240cc4
	DCA_WRITE(DC_COM_CMU_CSC_KBB_0, 0x00000000);	//0x54240cc8
	DCA_WRITE(DC_COM_CMU_LUT_MASK_0, 0x00000000);	//0x54240ccc
	DCA_WRITE(DC_COM_CMU_LUT1_0, 0x00000000);	//0x54240cd8
	DCA_WRITE(DC_COM_CMU_LUT2_0, 0x00000000);	//0x54240cdc
	DCA_WRITE(DC_COM_CMU_LUT1_READ_0, 0x00000000);	//0x54240ce0
	DCA_WRITE(DC_COM_CMU_LUT2_READ_0, 0x00000000);	//0x54240ce4
	DCA_WRITE(DC_CMD_GENERAL_INCR_SYNCPT_CNTRL_0, 0x00000000);	//0x54240004
	DCA_WRITE(DC_CMD_GENERAL_INCR_SYNCPT_ERROR_0, 0x00000000);	//0x54240008
	DCA_WRITE(DC_CMD_WIN_A_INCR_SYNCPT_CNTRL_0, 0x00000000);	//0x54240024
	DCA_WRITE(DC_CMD_WIN_A_INCR_SYNCPT_ERROR_0, 0x00000000);	//0x54240028
	DCA_WRITE(DC_CMD_WIN_B_INCR_SYNCPT_CNTRL_0, 0x00000000);	//0x54240044
	DCA_WRITE(DC_CMD_WIN_B_INCR_SYNCPT_ERROR_0, 0x00000000);	//0x54240048
	DCA_WRITE(DC_CMD_WIN_C_INCR_SYNCPT_CNTRL_0, 0x00000000);	//0x54240064
	DCA_WRITE(DC_CMD_WIN_C_INCR_SYNCPT_ERROR_0, 0x00000000);	//0x54240068
	DCA_WRITE(DC_CMD_CONT_SYNCPT_VSYNC_0, 0x00000000);	//0x542400a0
	DCA_WRITE(DC_CMD_DISPLAY_COMMAND_OPTION0_0, 0x00000000);	//0x542400c4
	DCA_WRITE(DC_CMD_DISPLAY_COMMAND_0, 0x00000000);	//0x542400c8
	DCA_WRITE(DC_CMD_SIGNAL_RAISE_0, 0x00000000);	//0x542400cc
	DCA_WRITE(DC_CMD_DISPLAY_POWER_CONTROL_0, 0x00000000);	//0x542400d8
	DCA_WRITE(DC_CMD_SIGNAL_RAISE1_0, 0x00000000);	//0x542400f0
	DCA_WRITE(DC_CMD_SIGNAL_RAISE2_0, 0x00000000);	//0x542400f4
	DCA_WRITE(DC_CMD_SIGNAL_RAISE3_0, 0x00000000);	//0x542400f8
	DCA_WRITE(DC_CMD_STATE_CONTROL_0, 0x00000000);	//0x54240104
	DCA_WRITE(DC_CMD_DISPLAY_WINDOW_HEADER_0, 0x00000000);	//0x54240108
	DCA_WRITE(DC_CMD_REG_ACT_CONTROL_0, 0x00000000);	//0x5424010c
	DCA_WRITE(DC_CMD_WIN_T_STATE_CONTROL_0, 0x00000000);	//0x54240110
	DCA_WRITE(DC_CMD_SECURE_CONTROL_0, 0x00000000);	//0x54240114
	DCA_WRITE(DC_CMD_WIN_D_INCR_SYNCPT_CNTRL_0, 0x00000000);	//0x54240134
	DCA_WRITE(DC_CMD_WIN_D_INCR_SYNCPT_ERROR_0, 0x00000000);	//0x54240138
	printk(BIOS_SPEW, "initial DCA done\n");
}

void init_sor_regs(void)
{
	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)TEGRA_CLK_RST_BASE;

	printk(BIOS_SPEW, "JZ: %s: entry\n", __func__);

#define SWR_SOR0_RST			(1 << 22)
#define CLK_ENB_SOR0			SWR_SOR0_RST
//    REG(CLK_RST_CONTROLLER_RST_DEVICES_X_0,   SWR_SOR0_RST,  1)
//    REG(CLK_RST_CONTROLLER_RST_DEVICES_X_0,   SWR_SOR0_RST,  0)
//    REG(CLK_RST_CONTROLLER_CLK_OUT_ENB_X_0,   CLK_ENB_SOR0,  1)
//    REG(CLK_RST_CONTROLLER_CLK_SOURCE_SOR0_0, 0) //0x60006414
	setbits_le32(&clkrst->rst_devices_x, SWR_SOR0_RST);	// Set Reset
	clrbits_le32(&clkrst->rst_devices_x, SWR_SOR0_RST);	// Clear Reset
	setbits_le32(&clkrst->clk_out_enb_x, CLK_ENB_SOR0);	// Set Enable
	WRITEL(0x0, (void *)(0x60006000 + 0x414));	// CLK_SOURCE_SOR0 = PLLP

	//WRITEL(0xc000c000, (0x60006000 + 0x414)); // CLK_SOURCE_SOR0 = CLK_M

#if 1
#define SOR_WRITE(reg, val) \
	{ \
	WRITEL(val,  (void *)(TEGRA_ARM_SOR + (reg<<2))); \
	}

#define SOR_READ(reg) READL( (void *)(TEGRA_ARM_SOR + (reg<<2)))

#else // read back
#define SOR_WRITE(reg, val) \
	{ \
	printk(BIOS_SPEW,"SOR_WRITE: addr %#x = %#x\n",
	(unsigned int) (TEGRA_ARM_SOR + (reg<<2)), val);  \
	WRITEL(val,  (void *)(TEGRA_ARM_SOR + (reg<<2))); \
	printk(BIOS_SPEW,"= %#x\n",  \
		(unsigned int)READL( (void *)(TEGRA_ARM_SOR + (reg<<2)))); \
	}
#endif
#define SOR_READ_M_WRITE(reg, mask, val) \
	{	\
	u32 _reg_val; \
	_reg_val = READL( (void *)(TEGRA_ARM_SOR + (reg<<2))); \
	_reg_val &= ~mask;	\
	_reg_val |= val;		\
	WRITEL(_reg_val,  (void *)(TEGRA_ARM_SOR + (reg<<2))); \
	}

	SOR_WRITE(SOR_NV_PDISP_SOR_SUPER_STATE0_0, 0x00000000);	//0x54540004
	SOR_WRITE(SOR_NV_PDISP_SOR_SUPER_STATE1_0, 0x00000000);	//0x54540008
	SOR_WRITE(SOR_NV_PDISP_SOR_STATE0_0, 0x00000000);	//0x5454000c
	SOR_WRITE(SOR_NV_PDISP_SOR_STATE1_0, 0x00000040);	//0x54540010
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE0, 0x00000000);	//0x54540014
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE0_1, 0x00000000);	//0x54540018
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE1, 0x01011000);	//0x5454001c
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE1_1, 0x01011000);	//0x54540020
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE2, 0x00000001);	//0x54540024
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE2_1, 0x00000001);	//0x54540028
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE3, 0x00010011);	//0x5454002c
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE3_1, 0x00010011);	//0x54540030
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE4, 0x00110100);	//0x54540034
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE4_1, 0x00110100);	//0x54540038
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE5, 0x00000001);	//0x5454003c
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE5_1, 0x00000001);	//0x54540040
	SOR_WRITE(SOR_NV_PDISP_SOR_CRC_CNTRL_0, 0x00000000);	//0x54540044
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_DEBUG_MVID_0, 0x00000000);	//0x54540048
	SOR_WRITE(SOR_NV_PDISP_SOR_CLK_CNTRL_0, 0x00000018);	//0x5454004c
	SOR_WRITE(SOR_NV_PDISP_SOR_CAP_0, 0x00000000);	//0x54540050
	SOR_WRITE(SOR_NV_PDISP_SOR_PWR_0, 0x00000000);	//0x54540054
	SOR_WRITE(SOR_NV_PDISP_SOR_TEST_0, 0x00800000);	//0x54540058
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL0_0, 0x0f0003d5);	//0x5454005c
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL1_0, 0x00001000);	//0x54540060
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL2_0, 0x01c00000);	//0x54540064
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL3_0, 0x38002220);	//0x54540068
	SOR_WRITE(SOR_NV_PDISP_SOR_CSTM_0, 0x0001c800);	//0x5454006c
	SOR_WRITE(SOR_NV_PDISP_SOR_LVDS_0, 0x0001c800);	//0x54540070
	SOR_WRITE(SOR_NV_PDISP_SOR_CRCA_0, 0x00000000);	//0x54540074
	SOR_WRITE(SOR_NV_PDISP_SOR_CRCB_0, 0x00000000);	//0x54540078
	SOR_WRITE(SOR_NV_PDISP_SOR_BLANK_0, 0x00000000);	//0x5454007c
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_CTL_0, 0x00008800);	//0x54540080
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE_SEQ_CTL_0, 0x00011000);	//0x54540084
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST0_0, 0x01008000);	//0x54540088
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST1_0, 0x01008000);	//0x5454008c
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST2_0, 0x01008000);	//0x54540090
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST3_0, 0x01008000);	//0x54540094
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST4_0, 0x01008000);	//0x54540098
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST5_0, 0x01008000);	//0x5454009c
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST6_0, 0x01008000);	//0x545400a0
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST7_0, 0x01008000);	//0x545400a4
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST8_0, 0x01008000);	//0x545400a8
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INST9_0, 0x01008000);	//0x545400ac
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INSTA_0, 0x01008000);	//0x545400b0
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INSTB_0, 0x01008000);	//0x545400b4
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INSTC_0, 0x01008000);	//0x545400b8
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INSTD_0, 0x01008000);	//0x545400bc
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INSTE_0, 0x01008000);	//0x545400c0
	SOR_WRITE(SOR_NV_PDISP_SOR_SEQ_INSTF_0, 0x01008000);	//0x545400c4
	SOR_WRITE(SOR_NV_PDISP_SOR_PWM_DIV_0, 0x00000000);	//0x545400c8
	SOR_WRITE(SOR_NV_PDISP_SOR_PWM_CTL_0, 0x00000000);	//0x545400cc
	SOR_WRITE(SOR_NV_PDISP_SOR_VCRCA0_0, 0x00000000);	//0x545400d0
	SOR_WRITE(SOR_NV_PDISP_SOR_VCRCA1_0, 0x00000000);	//0x545400d4
	SOR_WRITE(SOR_NV_PDISP_SOR_VCRCB0_0, 0x00000000);	//0x545400d8
	SOR_WRITE(SOR_NV_PDISP_SOR_VCRCB1_0, 0x00000000);	//0x545400dc
	SOR_WRITE(SOR_NV_PDISP_SOR_CCRCA0_0, 0x00000000);	//0x545400e0
	SOR_WRITE(SOR_NV_PDISP_SOR_CCRCA1_0, 0x00000000);	//0x545400e4
	SOR_WRITE(SOR_NV_PDISP_SOR_CCRCB0_0, 0x00000000);	//0x545400e8
	SOR_WRITE(SOR_NV_PDISP_SOR_CCRCB1_0, 0x00000000);	//0x545400ec
	SOR_WRITE(SOR_NV_PDISP_SOR_EDATAA0_0, 0x00000000);	//0x545400f0
	SOR_WRITE(SOR_NV_PDISP_SOR_EDATAA1_0, 0x00000000);	//0x545400f4
	SOR_WRITE(SOR_NV_PDISP_SOR_EDATAB0_0, 0x00000000);	//0x545400f8
	SOR_WRITE(SOR_NV_PDISP_SOR_EDATAB1_0, 0x00000000);	//0x545400fc
	SOR_WRITE(SOR_NV_PDISP_SOR_COUNTA0_0, 0x00000000);	//0x54540100
	SOR_WRITE(SOR_NV_PDISP_SOR_COUNTA1_0, 0x00000000);	//0x54540104
	SOR_WRITE(SOR_NV_PDISP_SOR_COUNTB0_0, 0x00000000);	//0x54540108
	SOR_WRITE(SOR_NV_PDISP_SOR_COUNTB1_0, 0x00000000);	//0x5454010c
	SOR_WRITE(SOR_NV_PDISP_SOR_DEBUGA0_0, 0x00000000);	//0x54540110
	SOR_WRITE(SOR_NV_PDISP_SOR_DEBUGA1_0, 0x00000000);	//0x54540114
	SOR_WRITE(SOR_NV_PDISP_SOR_DEBUGB0_0, 0x00000000);	//0x54540118
	SOR_WRITE(SOR_NV_PDISP_SOR_DEBUGB1_0, 0x00000000);	//0x5454011c
	SOR_WRITE(SOR_NV_PDISP_SOR_TRIG_0, 0x00000000);	//0x54540120
	SOR_WRITE(SOR_NV_PDISP_SOR_MSCHECK_0, 0x80000000);	//0x54540124
	SOR_WRITE(SOR_NV_PDISP_SOR_XBAR_CTRL_0, 0x8d111a23);	//0x54540128
	SOR_WRITE(SOR_NV_PDISP_SOR_XBAR_POL_0, 0x00000000);	//0x5454012c
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LINKCTL0_0, 0x00000100);	//0x54540130
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LINKCTL1_0, 0x00000100);	//0x54540134
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT0_0, 0x40404040);	//0x54540138
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT1_0, 0x80808080);	//0x5454013c
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE4_DRIVE_CURRENT0_0, 0x00000040);	//0x54540140
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE4_DRIVE_CURRENT1_0, 0x00000080);	//0x54540144
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE_PREEMPHASIS0_0, 0x00000000);	//0x54540148
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE_PREEMPHASIS1_0, 0x00000000);	//0x5454014c
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE4_PREEMPHASIS0_0, 0x00000000);	//0x54540150
	SOR_WRITE(SOR_NV_PDISP_SOR_LANE4_PREEMPHASIS1_0, 0x00000000);	//0x54540154
	SOR_WRITE(SOR_NV_PDISP_SOR_POSTCURSOR0_0, 0x00000000);	//0x54540158
	SOR_WRITE(SOR_NV_PDISP_SOR_POSTCURSOR1_0, 0x00000000);	//0x5454015c
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG0_0, 0x94000000);	//0x54540160
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG1_0, 0x94000000);	//0x54540164
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_MN0_0, 0x00008000);	//0x54540168
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_MN1_0, 0x00008000);	//0x5454016c
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0, 0x00800000);	//0x54540170
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL1_0, 0x00000000);	//0x54540174
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_DEBUG0_0, 0x00000000);	//0x54540178
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_DEBUG1_0, 0x00000000);	//0x5454017c
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_SPARE0_0, 0x00000002);	//0x54540180
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_SPARE1_0, 0x00000000);	//0x54540184
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_AUDIO_CTRL_0, 0x001f0001);	//0x54540188
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_AUDIO_HBLANK_SYMBOLS_0, 0x00000000);	//0x5454018c
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_AUDIO_VBLANK_SYMBOLS_0, 0x00000000);	//0x54540190
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_HEADER_0, 0x00000000);	//0x54540194
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK0_0,0x00000000);	//0x54540198
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK1_0,0x00000000);	//0x5454019c
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK2_0,0x00000000);	//0x545401a0
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK3_0,0x00000000);	//0x545401a4
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK4_0,0x00000000);	//0x545401a8
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK5_0,0x00000000);	//0x545401ac
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_GENERIC_INFOFRAME_SUBPACK6_0,0x00000000);	//0x545401b0
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_TPG_0, 0x50505050);	//0x545401b4
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_TPG_CONFIG_0, 0x00000000);	//0x545401b8
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LQ_CSTM0_0, 0x00000000);	//0x545401bc
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LQ_CSTM1_0, 0x00000000);	//0x545401c0
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LQ_CSTM2_0, 0x00000000);	//0x545401c4
	printk(BIOS_SPEW, "initial SOR done\n");
}

void init_dpaux_regs(void)
{
	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)TEGRA_CLK_RST_BASE;
//  u32 val;

	printk(BIOS_SPEW, "JZ: %s: entry\n", __func__);

#define SWR_DPAUX_RST			(1 << 21)
#define CLK_ENB_DPAUX			SWR_DPAUX_RST
//    REG(CLK_RST_CONTROLLER_RST_DEVICES_X_0, SWR_DPAUX_RST, 1)
//    REG(CLK_RST_CONTROLLER_RST_DEVICES_X_0, SWR_DPAUX_RST, 0)
//    REG(CLK_RST_CONTROLLER_CLK_OUT_ENB_X_0, CLK_ENB_DPAUX, 1)
	setbits_le32(&clkrst->rst_devices_x, SWR_DPAUX_RST);	// Set Reset
	clrbits_le32(&clkrst->rst_devices_x, SWR_DPAUX_RST);	// Clear Reset
	setbits_le32(&clkrst->clk_out_enb_x, CLK_ENB_DPAUX);	// Set Enable

#if 1
#define DPAUX_WRITE(reg, val) \
	{ \
	WRITEL(val,  (void *)(TEGRA_ARM_DPAUX + (reg<<2))); \
	}
#define DPAUX_READ(reg) READL( (void *)(TEGRA_ARM_DPAUX + (reg<<2)))

#else // read back
#define DPAUX_WRITE(reg, val) \
	{ \
	printk(BIOS_SPEW,"DPAUX_WRITE: addr %#x = %#x\n", (unsigned int)
	(TEGRA_ARM_DPAUX + (reg<<2)), val);		    \
	WRITEL(val,  (void *)(TEGRA_ARM_DPAUX + (reg<<2))); \
	printk(BIOS_SPEW,"= %#x\n",  \
		(unsigned int)READL( (void *)(TEGRA_ARM_DPAUX + (reg<<2)))); \
	}
#endif

	DPAUX_WRITE(DPAUX_INTR_EN_AUX, 0x00000000);	//0x545c0004
	DPAUX_WRITE(DPAUX_INTR_AUX, 0x00000000);	//0x545c0014
	DPAUX_WRITE(DPAUX_DP_AUXDATA_WRITE_W0, 0x00000000);	//0x545c0024
	DPAUX_WRITE(DPAUX_DP_AUXDATA_WRITE_W1, 0x00000000);	//0x545c0034
	DPAUX_WRITE(DPAUX_DP_AUXDATA_WRITE_W2, 0x00000000);	//0x545c0044
	DPAUX_WRITE(DPAUX_DP_AUXDATA_WRITE_W3, 0x00000000);	//0x545c0054
	DPAUX_WRITE(DPAUX_DP_AUXDATA_READ_W0, 0x00000000);	//0x545c0064
	DPAUX_WRITE(DPAUX_DP_AUXDATA_READ_W1, 0x00000000);	//0x545c0074
	DPAUX_WRITE(DPAUX_DP_AUXDATA_READ_W2, 0x00000000);	//0x545c0084
	DPAUX_WRITE(DPAUX_DP_AUXDATA_READ_W3, 0x00000000);	//0x545c0094
	DPAUX_WRITE(DPAUX_DP_AUXADDR, 0x00000000);	//0x545c00a4
	DPAUX_WRITE(DPAUX_DP_AUXCTL, 0x00000000);	//0x545c00b4
	DPAUX_WRITE(DPAUX_DP_AUXSTAT, 0x10000000);	//0x545c00c4
	DPAUX_WRITE(DPAUX_DP_AUX_SINKSTATLO, 0x00000000);	//0x545c00d4
	DPAUX_WRITE(DPAUX_DP_AUX_SINKSTATHI, 0x00000000);	//0x545c00e4
	DPAUX_WRITE(DPAUX_HPD_CONFIG, 0x07d000fa);	//0x545c00f4
	DPAUX_WRITE(DPAUX_HPD_IRQ_CONFIG, 0x000000fa);	//0x545c0104
	DPAUX_WRITE(DPAUX_DP_AUX_CONFIG, 0x00000190);	//0x545c0114
	DPAUX_WRITE(DPAUX_HYBRID_PADCTL, 0x00002462);	//0x545c0124
	DPAUX_WRITE(DPAUX_HYBRID_SPARE, 0x00000000);	//0x545c0134
	DPAUX_WRITE(DPAUX_SCRATCH_REG0_0, 0x00000000);	//0x545c0144
	DPAUX_WRITE(DPAUX_SCRATCH_REG1_0, 0x00000000);	//0x545c0154
	DPAUX_WRITE(DPAUX_SCRATCH_REG2_0, 0x00000000);	//0x545c0164
	printk(BIOS_SPEW, "initial DPAUX done\n");
}

static int dp_poll_register(void *addr, u32 exp_val, u32 mask, u32 timeout_ms)
{

	u32 reg_val = 0;

	printk(BIOS_SPEW, "JZ: %s: enter, addr %#x: exp_val: %#x, mask: %#x\n",
		   __func__, (unsigned int)addr, exp_val, mask);
	do {
		udelay(1);
		reg_val = READL(addr);
	} while (((reg_val & mask) != exp_val) && (--timeout_ms > 0));

	if ((reg_val & mask) == exp_val)
		return 0;	/* success */
	printk(BIOS_SPEW, "poll_register %p: timeout\n", addr);
	return timeout_ms;
}

static void dp_io_set_dpd(u32 power_down)
{
	/*
	 * power_down:
	 *  0: out of Deep power down
	 *  1: into deep power down
	 */
	u32 val_reg;
#define DP_LVDS_SHIFT	25
#define DP_LVDS		(1 << DP_LVDS_SHIFT)

	val_reg = READL((void *)(0x7000e400 + 0x1c4));	/* APBDEV_PMC_IO_DPD2_STATUS_0 */
	printk(BIOS_SPEW, "JZ: %s: enter, into dpd %d, cur_status: %#x\n",
		   __func__, power_down, val_reg);

	if ((((val_reg & DP_LVDS) >> DP_LVDS_SHIFT) & 1) == power_down) {
		printk(BIOS_SPEW, "PAD already POWER=%d\n", 1 - power_down);
		return;
	}

	/* APBDEV_PMC_IO_DPD2_REQ_0: E_DPD = power on */
	WRITEL((DP_LVDS | ((1 + power_down) << 30)), (void *)(0x7000e400 + 0x1c0));

	dp_poll_register((void *)(0x7000e400 + 0x1C4), 0, DP_LVDS, 1000);
	//APBDEV_PMC_IO_DPD2_STATUS_0
}

void dp_io_powerup(void)
{

//E_DPD  = PMC.dpd2_status[25]
//PDBG   = SOR_NV_PDISP_SOR_PLL2_0.AUX6(1) | SEQ.POWERDOWN_MACRO(1) &
//SOR_NV_PDISP_SOR_PLL2_0.AUX2(0)
//PDPLL  = SOR_NV_PDISP_SOR_PLL0_0.PWR(1)  | SEQ.PDPLL(0)  & ~ SOR_NV_PDISP_SOR_PLL2_0.AUX1(0)
//VCOPD  = SOR_NV_PDISP_SOR_PLL0_0.VCOPD(1)
//CAPPD  = SOR_NV_PDISP_SOR_PLL2_0.AUX8(1) | SEQ.ASSERT_PLL_RESET(0) &
//~ SOR_NV_PDISP_SOR_PLL2_0.AUX1(0)
//PDPORT = SOR_NV_PDISP_SOR_PLL2_0.AUX7(1) | SEQ.PDPORT(1) &
//~ SOR_NV_PDISP_SOR_DP_LINKCTL0_0.ENABLE(0)
//PDCAL  = SOR_NV_PDISP_SOR_DP_PADCTL0_0.PAD_CAL_PD(1)

//  struct clk_rst_ctlr *clkrst =
//          (struct clk_rst_ctlr *)TEGRA_CLK_RST_BASE;
	u32 reg_val;

	printk(BIOS_SPEW, "%s: entry\n", __func__);

#if 0
	printk(BIOS_SPEW, "JZ: %s: %d: do nothing, ret\n", __func__, __LINE__);
	return;
#endif

#define SOR0_CLK_SEL0			(1 << 14)
#define SOR0_CLK_SEL1			(1 << 15)
//    REG(CLK_RST_CONTROLLER_CLK_SOURCE_SOR0_0, SOR0_CLK_SEL1, 0);
//    REG(CLK_RST_CONTROLLER_CLK_SOURCE_SOR0_0, SOR0_CLK_SEL0, 0);//sor safe clock
	reg_val = READL((void *)(0x60006000 + 0x414));
	reg_val &= ~(SOR0_CLK_SEL0 | SOR0_CLK_SEL1);
	WRITEL(reg_val, (void *)(0x60006000 + 0x414));

//    clock(PLLDP, 270)
	WRITEL(0, (void *)(0x60006000 + 0x594));	// plldp_misc
	WRITEL(0x11400000, (void *)(0x60006000 + 0x598));	// plldp_ss_cfg
	WRITEL(0x80305a01, (void *)(0x60006000 + 0x590));// plldp_base, 12 * 90 / 4 = 270
	WRITEL(0x11400000, (void *)(0x60006000 + 0x598));	// plldp_ss_cfg
	WRITEL(0x40000000, (void *)(0x60006000 + 0x594));	// misc: enable lock
	WRITEL(0xc0305a01, (void *)(0x60006000 + 0x590));	// base: enable
	WRITEL(0xd8305a01, (void *)(0x60006000 + 0x590));	// base: check lock
	WRITEL(0x58305a01, (void *)(0x60006000 + 0x590));	// base: disable bypass
	WRITEL(0x11000000, (void *)(0x60006000 + 0x598));	// release clamp
	udelay(10);	// wait for plldp ready

	SOR_WRITE(SOR_NV_PDISP_SOR_CLK_CNTRL_0, (6 << 2) | 2);//select PLLDP,  lowest speed(6x)
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0, 0x00800000);	//set PDCAL
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL0_0, 0x050003D5);	//set PWR,VCOPD
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL1_0, 0x00001100);	//default
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL2_0, 0x01C20000);	//set AUX1,6,7,8; clr AUX2
	SOR_WRITE(SOR_NV_PDISP_SOR_PLL3_0, 0x38002220);

	//REG(SOR_NV_PDISP_SOR_PLL3_0,PLLVDD_MODE, V1_8)
	dp_io_set_dpd(0);
	udelay(1);	//Deassert E_DPD to enable core logic circuits, and wait for > 5us

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_PLL2_0,
					 SOR_NV_PDISP_SOR_PLL2_0_AUX6_FIELD,
					 (0 << SOR_NV_PDISP_SOR_PLL2_0_AUX6_SHIFT));
	udelay(20);	//Deassert PDBG to enable bandgap, and wait for > 20us.

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_PLL0_0,
					 SOR_NV_PDISP_SOR_PLL0_0_PWR_FIELD,
					 (0 << SOR_NV_PDISP_SOR_PLL0_0_PWR_SHIFT));
	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_PLL0_0,
					 SOR_NV_PDISP_SOR_PLL0_0_VCOPD_FIELD,
					 (0 << SOR_NV_PDISP_SOR_PLL0_0_VCOPD_SHIFT));
	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_PLL2_0,
					 SOR_NV_PDISP_SOR_PLL2_0_AUX8_FIELD,
					 (0 << SOR_NV_PDISP_SOR_PLL2_0_AUX8_SHIFT));
	udelay(200);
	//Enable the PLL/charge-pump/VCO, and wait for >200us for the PLL to
	//lock. Input Clock must be running and stable before PDPLL
	//de-assertion.
	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_PLL2_0,
					 SOR_NV_PDISP_SOR_PLL2_0_AUX7_FIELD,
					 (0 << SOR_NV_PDISP_SOR_PLL2_0_AUX7_SHIFT));
	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_PLL2_0,
					 SOR_NV_PDISP_SOR_PLL2_0_AUX9_FIELD,
					 (1 << SOR_NV_PDISP_SOR_PLL2_0_AUX9_SHIFT));
	udelay(100);

	printk(BIOS_SPEW, "%s: exit\n", __func__);
}

static int dpaux_check(u32 bytes, u32 data, u32 mask)
{
	u32 status = 0;
	u8 buf[16];
	u32 temp;

	DPAUX_WRITE(DPAUX_DP_AUXDATA_READ_W0, 0);
	status = dpaux_read(0x202, bytes, buf);
	if (status != 0)
		printk(BIOS_SPEW, "******AuxRead Error:%04x: status %08x\n", 0x202,
			   status);
	else {
		//temp = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0] ;
		//memcpy(&temp, buf, 4);
		temp = DPAUX_READ(DPAUX_DP_AUXDATA_READ_W0);
		if ((temp & mask) != (data & mask)) {
			printk(BIOS_SPEW, "AuxCheck ERROR:(r_data) %08x & (mask) %08x != "
				   "(data) %08x & (mask) %08x\n", temp, mask, data, mask);
			return -1;
		} else {
			printk(BIOS_SPEW,
				   "AuxCheck PASS:(bytes=%d,data=%08x,mask=%08x):0x%08x\n",
				   bytes, data, mask, temp);
			return 0;
		}
	}
	return -1;
}

/* Modify the drive parameters for DP.  There are up to four DP
 * lanes. In principle, each lane can have different current,
 * pre-emphasis, and postcur values. Nobody individualizes them; every
 * single driver I've seen drives all the lanes to the same value
 * (across x86 and ARM code). Actualy adjusting them individually and
 * getting it all to work is probably a PhD thesis anyway. So, rather
 * than the very complex code we see many places, the people who wrote
 * this code realize: we can represent the 'volume' as a number in the
 * range 0..3, with '0' as the base and '3' as being 'not to exceed'.
 *
 * So they abstract the values away, take care of the proper values,
 * and set it all in one blow. Very nice. By far the easiest one of
 * these functions we've seen. Sure, they could have constants, but
 * nobody knows what PRE_EMPHASIS_3_5 and the other values actually
 * *mean* anyway. Well, the hardware guys might.
 */
static void pattern_level(u32 current, u32 preemph, u32 postcur)
{
	printk(BIOS_SPEW, "set level:%d %d %d\n", current, preemph, postcur);

	//calibrating required
	if (current == 0)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT0_0, 0x20202020);
	if (current == 1)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT0_0, 0x24242424);
	if (current == 2)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT0_0, 0x30303030);
	if (current == 3)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT0_0, 0x40404040);
	if (preemph == 0)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_PREEMPHASIS0_0, 0x00000000);
	if (preemph == 1)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_PREEMPHASIS0_0, 0x08080808);
	if (preemph == 2)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_PREEMPHASIS0_0, 0x10101010);
	if (preemph == 3)
		SOR_WRITE(SOR_NV_PDISP_SOR_LANE_PREEMPHASIS0_0, 0x18181818);
	if (postcur == 0)
		SOR_WRITE(SOR_NV_PDISP_SOR_POSTCURSOR0_0, 0x00000000);
	if (postcur == 1)
		SOR_WRITE(SOR_NV_PDISP_SOR_POSTCURSOR0_0, 0x04040404);
	if (postcur == 2)
		SOR_WRITE(SOR_NV_PDISP_SOR_POSTCURSOR0_0, 0x08080808);
	if (postcur == 3)
		SOR_WRITE(SOR_NV_PDISP_SOR_POSTCURSOR0_0, 0x10101010);
}

static int dp_training(u32 level, u32 check, u32 speed)
{
	/* The levels are one of four choices. This code
	 * packs them into the three lowest nibl's. We may change this.
	 */
	u32 dc_lv = level & 0x0f;
	u32 pe_lv = (level >> 4) & 0x0f;
	u32 pc_lv = (level >> 8) & 0x0f;
	u32 cnt = 0;
	u32 cfg, cfg_d = 0;
	u32 wcfg;
//    u32 status = 0;
	u8 buf[16];

	while (cnt <= 5) {
		pattern_level(dc_lv, pe_lv, pc_lv);
		wcfg = (pe_lv << 3) | dc_lv;
		if (dc_lv == 3)
			wcfg = wcfg | 0x04;
		if (pe_lv == 3)
			wcfg = wcfg | 0x20;
		wcfg = wcfg | (wcfg << 8) | (wcfg << 16) | (wcfg << 24);
		dpaux_write(0x103, 4, wcfg);
		udelay(50);	//100us
		DPAUX_WRITE(DPAUX_DP_AUXDATA_READ_W0, 0);
		if (!dpaux_check(2, check, check))
			cnt = 100;
		else {
			dpaux_read(0x206, 1, buf);
			cfg = DPAUX_READ(DPAUX_DP_AUXDATA_READ_W0);
			cfg &= 0x00ff;
			if (cfg == cfg_d) {
				++cnt;
				if (cnt > 5)
					printk(BIOS_SPEW, "link training FAILED\n");
			} else {
				cnt = 0;
				cfg_d = cfg;
				dc_lv = cfg & 0x3;
				pe_lv = (cfg >> 2) & 0x3;
				if (speed == 20) {
					dpaux_read(0x20C, 1, buf);
					cfg = DPAUX_READ(DPAUX_DP_AUXDATA_READ_W0);
					pc_lv = cfg & 0x3;
				} else {
					pc_lv = 0;
				}
			}

		}
		debug_dpaux_print(0x200, 16);
	}

	return ((pc_lv << 8) | (pe_lv << 4) | (dc_lv));

}

void dp_link_training(u32 lanes, u32 speed);
void dp_link_training(u32 lanes, u32 speed)
{
	u32 lane_on;
	u32 mask, level;
	u32 reg_val;

	printk(BIOS_SPEW, "%s: entry, lanes: %d, speed: %d\n", __func__, lanes,
		   speed);

	printk(BIOS_SPEW, "\nLink training start\n");

	switch (lanes) {
		case 1:
			lane_on = 0x04;
			break;
		case 2:
			lane_on = 0x06;
			break;
		case 4:
			lane_on = 0x0f;
			break;
		default:
			printk(BIOS_SPEW, "dp: invalid lane count: %d\n", lanes);
			return;
	}

	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0, (0x008000000 | lane_on));
	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0,
			 SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_VALUE_FIELD,
			 (6 << SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_VALUE_SHIFT));
	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0,
			 SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_FIELD,
			 (1 << SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_SHIFT));
	SOR_WRITE(SOR_NV_PDISP_SOR_LVDS_0, 0);

	SOR_WRITE(SOR_NV_PDISP_SOR_CLK_CNTRL_0, ((speed << 2) | 2));
	udelay(100);

	//REG(CLK_RST_CONTROLLER_CLK_SOURCE_SOR0_0,SOR0_CLK_SEL0, 1) //sor clk=pad macro output
	reg_val = readl((void *)(0x60006000 + 0x414));
	reg_val |= SOR0_CLK_SEL0;

	writel(reg_val, (void *)(0x60006000 + 0x414));

	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LINKCTL0_0,
			  (((0xF >> (4 - lanes)) << 16) | 1));

	SOR_WRITE(SOR_NV_PDISP_SOR_LANE_SEQ_CTL_0, 0x80100000);
	printk(BIOS_SPEW, "Polling SOR_NV_PDISP_SOR_LANE_SEQ_CTL_0.DONE\n");

	dp_poll_register((void *)0x54540084, 0x00000000, 0x80000000, 1000);

	debug_dpaux_print(0x202, 4);

	printk(BIOS_SPEW, "set link rate and lane number: %dMHz, %d lanes\n",
		   (speed * 27), lanes);

//    printk(BIOS_SPEW,"JZ: dbg ret\n");
//    return;

//    %d = (%lanes<<8) | %speed
	dpaux_write(0x100, 2, ((lanes << 8) | speed));
	printk(BIOS_SPEW, "precharge lane 10us\n");
	reg_val = SOR_READ(SOR_NV_PDISP_SOR_DP_PADCTL0_0);
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0, (0x000000f0 | reg_val));
	udelay(100);
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0, reg_val);

	printk(BIOS_SPEW, "link training cr start\n");
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_TPG_0, 0x41414141);
	dpaux_write(0x102, 1, 0x21);

	mask = 0x0000ffff >> ((4 - lanes) * 4);
	level = 0;
	level = dp_training(level, 0x1111 & mask, speed);
	printk(BIOS_SPEW, "level:%x\n", level);

	debug_dpaux_print(0x210, 16);

	printk(BIOS_SPEW, "link training eq start\n");
	if (speed == 20) {
		SOR_WRITE(SOR_NV_PDISP_SOR_DP_TPG_0, 0x43434343);
		dpaux_write(0x102, 1, 0x23);
	} else {
		SOR_WRITE(SOR_NV_PDISP_SOR_DP_TPG_0, 0x42424242);
		dpaux_write(0x102, 1, 0x22);
	}

	level = dp_training(level, (0x7777 & mask) | 0x10000, speed);
	printk(BIOS_SPEW, "level:%x\n", level);

	debug_dpaux_print(0x210, 16);

	SOR_WRITE(SOR_NV_PDISP_SOR_DP_TPG_0, 0x50505050);
	dpaux_write(0x102, 1, 0);
	dpaux_write(0x600, 1, 1);

	debug_dpaux_print(0x200, 16);
	debug_dpaux_print(0x210, 16);

	printk(BIOS_SPEW, "Link training done\n\n");
	printk(BIOS_SPEW, "%s: exit\n", __func__);
}

static u32 div_f(u32 a, u32 b, u32 one)
{
	u32 d = (((a - (a / b * b)) * one) + (b / 2)) / b;
	return (d);
}

u32 dp_setup_timing(u32 panel_id, u32 width, u32 height);
u32 dp_setup_timing(u32 panel_id, u32 width, u32 height)
{
	u32 reg_val;
	u32 pclk_freq = 0;

	///////////////////////////////////////////
	// set up clocks, using PLLD2
	// format: pclk  , PLL  ,  panel
	//2560x1440:  241.5  , 483/2,  dp   VESA.red.
	//1920x1080:  148.5  , 594/4,  dp   CEA
	//1600x1200:  161.0  , 483/3,  dp   VESA
	//1280x 720:   74.25 , 594/8,  dp   CEA
	// 1024x768:   63.5  , 508/8,  dp   VESA
	//  800x600:   38.25 , 459/12, dp   VESA
	//  720x480:   27.00 , 594/22, dp   CEA
	//  640x480:   23.75 , 475/20, dp   VESA

	u32 PLL_FREQ = 570;
	u32 PLL_DIV = 2;
	u32 SYNC_WIDTH = (10 << 16) | 32;
	u32 BACK_PORCH = (36 << 16) | 80;
	u32 FRONT_PORCH = (3 << 16) | 48;
	u32 HSYNC_NEG = 1;
	u32 VSYNC_NEG = 1;

	u32 SHIFT_CLK_DIVIDER = PLL_DIV * 2 - 2;
	u32 DISP_ACTIVE = (height << 16) | width;
	u32 DISP_TOTAL = DISP_ACTIVE + SYNC_WIDTH + BACK_PORCH + FRONT_PORCH;
	u32 SYNC_END = SYNC_WIDTH - 0x10001;
	u32 BLANK_END = SYNC_END + BACK_PORCH;
	u32 BLANK_START = BLANK_END + DISP_ACTIVE;
	u32 TOTAL_PIXELS = (DISP_TOTAL & 0xffff) * (DISP_TOTAL >> 16);

	u32 PLL_FREQ_I, PLL_FREQ_F;
	u32 PCLK_FREQ_I, PCLK_FREQ_F;
	u32 FRATE_I, FRATE_F;

	printk(BIOS_SPEW, "%s: entry\n", __func__);

	if (panel_id != 5) {
		printk(BIOS_SPEW, "%s: Unsupported panel_id: %d, format=%dx%d\n",
			   __func__, panel_id, width, height);
		return pclk_freq;
	}

//    clock(plld2, %PLL_FREQ)   // PLL_FREQ = 570
	writel(0, (void *)(0x60006000 + 0x4bc));	// plld2_misc
	writel(0x13400000, (void *)(0x60006000 + 0x570));	// plld2_ss_cfg
	writel(0x8008010c, (void *)(0x60006000 + 0x4b8));	// plld2_base
	writel(0x80105F01, (void *)(0x60006000 + 0x4b8));// plld2_base: 12 * 95 / 2 = 570
	writel(0x40000000, (void *)(0x60006000 + 0x4bc));	// misc: enable lock
	writel(0x80105f01, (void *)(0x60006000 + 0x4b8));	// base: enable
	writel(0xc0105f01, (void *)(0x60006000 + 0x4b8));	// base: check lock
	writel(0x58105f01, (void *)(0x60006000 + 0x4b8));	// base: disable bypass
	writel(0x13800000, (void *)(0x60006000 + 0x570));	// plld2_ss_cfg
	udelay(10);	// wait for plld2 ready

// REG(CLK_RST_CONTROLLER_CLK_SOURCE_DISP1_0, DISP1_CLK_SRC, PLLD2_OUT0)
#define DISP1_CLK_SRC			(0x7 << 29)
#define PLLD2_OUT0			(0x5 << 29)
	reg_val = readl((void *)(0x60006000 + 0x138));
	reg_val &= ~DISP1_CLK_SRC;
	reg_val |= PLLD2_OUT0;
	writel(reg_val, (void *)(0x60006000 + 0x138));
	udelay(10);

	PLL_FREQ = PLL_FREQ * 1000000;
	pclk_freq = PLL_FREQ / PLL_DIV;
	PLL_FREQ_I = PLL_FREQ / 1000000;
	PLL_FREQ_F = div_f(PLL_FREQ, 1000000, 100);
	PCLK_FREQ_I = PLL_FREQ / (PLL_DIV * 1000000);
	PCLK_FREQ_F = div_f(PLL_FREQ, PLL_DIV * 1000000, 100);
	FRATE_I = PLL_FREQ / (PLL_DIV * TOTAL_PIXELS);
	FRATE_F = div_f(PLL_FREQ, (PLL_DIV * TOTAL_PIXELS), 100);
	//bug 1021453
	BACK_PORCH = BACK_PORCH - 0x10000;
	FRONT_PORCH = FRONT_PORCH + 0x10000;

	printk(BIOS_SPEW, "ACTIVE:            %dx%d\n", (DISP_ACTIVE & 0xFFFF),
		   (DISP_ACTIVE >> 16));
	printk(BIOS_SPEW, "TOTAL:             %dx%d\n", (DISP_TOTAL & 0xffff),
		   (DISP_TOTAL >> 16));
	printk(BIOS_SPEW, "PLL Freq:          %d.%d MHz\n", PLL_FREQ_I, PLL_FREQ_F);
	printk(BIOS_SPEW, "Pclk Freq:         %d.%d MHz\n", PCLK_FREQ_I,
		   PCLK_FREQ_F);
	printk(BIOS_SPEW, "Frame Rate:        %d.%d Hz\n", FRATE_I, FRATE_F);
	printk(BIOS_SPEW, "\n");

	DCA_WRITE(DC_CMD_STATE_ACCESS_0, 0x00000004);
	DCA_WRITE(DC_DISP_DISP_CLOCK_CONTROL_0, SHIFT_CLK_DIVIDER);
	//Raster Timing
	DCA_WRITE(DC_DISP_DISP_TIMING_OPTIONS_0, 0x00000001);
	DCA_WRITE(DC_DISP_REF_TO_SYNC_0, 0x00010001);
	DCA_WRITE(DC_DISP_SYNC_WIDTH_0, SYNC_WIDTH);
	DCA_WRITE(DC_DISP_BACK_PORCH_0, BACK_PORCH);
	DCA_WRITE(DC_DISP_DISP_ACTIVE_0, DISP_ACTIVE);
	DCA_WRITE(DC_DISP_FRONT_PORCH_0, FRONT_PORCH);

	printk(BIOS_SPEW,
		   "JZ: sync_width: %d, back_porch: %d, disp_active: %d, front_porch: %d\n",
		   SYNC_WIDTH, BACK_PORCH, DISP_ACTIVE, FRONT_PORCH);
	//REG(DC_DISP_DISP_WIN_OPTIONS_0, SOR_ENABLE , 1)
	DCA_READ_M_WRITE(DC_DISP_DISP_WIN_OPTIONS_0,
					 DC_DISP_DISP_WIN_OPTIONS_0_SOR_ENABLE_FIELD,
					 (1 << DC_DISP_DISP_WIN_OPTIONS_0_SOR_ENABLE_SHIFT));

	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE1_0, DISP_TOTAL);
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE2_0, SYNC_END);
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE3_0, BLANK_END);
	SOR_WRITE(SOR_NV_PDISP_HEAD_STATE4_0, BLANK_START);

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
					 SOR_NV_PDISP_SOR_STATE1_0_ASY_HSYNCPOL_FIELD,
					 (HSYNC_NEG <<
					  SOR_NV_PDISP_SOR_STATE1_0_ASY_HSYNCPOL_SHIFT));

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
					 SOR_NV_PDISP_SOR_STATE1_0_ASY_VSYNCPOL_FIELD,
					 (VSYNC_NEG <<
					  SOR_NV_PDISP_SOR_STATE1_0_ASY_VSYNCPOL_SHIFT));

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
					 SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_FIELD,
					 (SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_DP_A <<
					  SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_SHIFT));

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
					 SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_FIELD,
					 (SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_COMPLETE_RASTER <<
					  SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_SHIFT));

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
					 SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_FIELD,
					 (SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_NONE <<
					  SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_SHIFT));

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
					 SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_FIELD,
					 (SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_HEAD0 <<
					  SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_SHIFT));
	printk(BIOS_SPEW, "%s: exit\n", __func__);
	return pclk_freq;
}

static u32 calc_config(u32 ts, u32 a, u32 b, u32 bpp)
{
	u32 act_cnt = (ts * a) / b;
	u32 diff = (ts * a) - (act_cnt * b);
	u32 act_pol;
	u32 act_frac;
	u32 err;
	u32 water_mark;
	printk(BIOS_SPEW, "calc_config ts %d a %d b %d bpp %d\n", ts, a, b, bpp);
	if (diff != 0) {
		if (diff > (b / 2)) {
			diff = b - diff;
			act_pol = 1;
			act_frac = (b + diff - 1) / diff;
			err = diff * act_frac - b;
		} else {
			act_pol = 0;
			act_frac = b / diff;
			err = b - (diff * act_frac);
		}
		if (act_frac > 15) {
			act_pol = 1 - act_pol;
			act_frac = 1;
			err = diff;
		}
	} else {
		act_pol = 1;
		act_frac = 1;
		err = 0;
	}

	if (bpp) {
		water_mark = (a * (b - a) * ts / (b * b)) + (2 * bpp / 8);
		if (water_mark > 30)
			water_mark = 30;

		SOR_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG0_0, 0x84000000);
		SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG0_0,
				 SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_POLARITY_FIELD,
				 (act_pol <<
				  SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_POLARITY_SHIFT));
		SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG0_0,
				 SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_FRAC_FIELD,
				 (act_frac <<
				  SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_FRAC_SHIFT));
		SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG0_0,
				 SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_COUNT_FIELD,
				 (act_cnt <<
				  SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_COUNT_SHIFT));
		SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_CONFIG0_0,
				 SOR_NV_PDISP_SOR_DP_CONFIG0_0_WATERMARK_FIELD,
				 (water_mark <<
				  SOR_NV_PDISP_SOR_DP_CONFIG0_0_WATERMARK_SHIFT));

		printk(BIOS_SPEW,
		       "SOR_DP_CONFIG0:TU,CNT,POL,FRAC,WMK,ERR=%d,%d,%d,%d,%d,%d/%d\n",
		       ts, act_cnt, act_pol, act_frac, water_mark, err, b);
	}
	return (err);
}

static u32 dp_buf_config(u32 pclkfreq, u32 linkfreq, u32 lanes, u32 bpp)
{
	//to avoid 32bit overflow
	u32 tusize = 0;
	u32 pf = pclkfreq;
	u32 lf = linkfreq;
	u32 i;
	u32 a, b;
	u32 min_err = 1000000000;
	u32 ts = 64;
	u32 c_err;
	printk(BIOS_SPEW, "dp buf config pclkfreq %d linkfreq %d lanes %d bpp %d\n",
		   pclkfreq, linkfreq, lanes, bpp);
	for (i = 2; i <= 7; ++i) {
		while (((pf / i * i) == pf) && ((lf / i * i) == lf)) {
			pf = pf / i;
			lf = lf / i;
		}
	}

	a = pf * bpp / 8;
	b = lf * lanes;
	printk(BIOS_SPEW, "ratio:%d/%d\n", a, b);
	if (a > (b * 98 / 100))
		printk(BIOS_SPEW, "Error:link speed not enough\n");

	//search best tusize
	//min_err = 1000000000;
	//ts = 64;
	while (ts >= 32) {
		c_err = calc_config(ts, a, b, 0);
		if (c_err < min_err) {
			if (c_err == 0) {
				tusize = ts;
				ts = 1;
			} else {
				min_err = c_err;
				tusize = ts;
			}
		}
		--ts;
	}

	SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_DP_LINKCTL0_0,
			 SOR_NV_PDISP_SOR_DP_LINKCTL0_0_TUSIZE_FIELD,
			 (tusize << SOR_NV_PDISP_SOR_DP_LINKCTL0_0_TUSIZE_SHIFT));
	calc_config(tusize, a, b, bpp);

	return (tusize);
}

void dp_misc_setting(u32 panel_bpp, u32 width, u32 height, u32 winb_addr,
		     u32 lane_count, u32 enhanced_framing, u32 panel_edp,
		     u32 pclkfreq, u32 linkfreq);

void dp_misc_setting(u32 panel_bpp, u32 width, u32 height, u32 winb_addr,
		     u32 lane_count, u32 enhanced_framing, u32 panel_edp,
		     u32 pclkfreq, u32 linkfreq)
{
	u32 tusize;
	u32 linkctl;

	printk(BIOS_SPEW, "%s: entry, winb: 0x%08x ", __func__, winb_addr);
	printk(BIOS_SPEW, " panel_bpp %d\n", panel_bpp);

	if (panel_bpp == 18) {
		//0x54540010
		SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
				 SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_FIELD,
				 (SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_18_444 <<
				  SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_SHIFT));
	}
	if (panel_bpp == 24) {
		SOR_READ_M_WRITE(SOR_NV_PDISP_SOR_STATE1_0,
				 SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_FIELD,
				 (SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_24_444 <<
				  SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_SHIFT));
	}

#define SRC_BPP		16
#define COLORDEPTH	0x6

#define BLUE	0xFF0000
#define GREEN	0x00FF00
#define RED	0x0000FF
#define YELLOW	0x00FFFF
#define BLACK	0x000000
#define WHITE	0xFFFFFF
#define GREY	0x55aa00

	DCA_WRITE(DC_B_WIN_BD_SIZE_0, ((height << 16) | width));
	DCA_WRITE(DC_B_WIN_BD_PRESCALED_SIZE_0,
			  ((height << 16) | (width * SRC_BPP / 8)));
	DCA_WRITE(DC_B_WIN_BD_LINE_STRIDE_0,
			  ((width * SRC_BPP / 8 + 31) / 32 * 32));
	DCA_WRITE(DC_B_WIN_BD_COLOR_DEPTH_0, COLORDEPTH);
	DCA_WRITE(DC_B_WINBUF_BD_START_ADDR_0, winb_addr);
	DCA_WRITE(DC_B_WIN_BD_DDA_INCREMENT_0, 0x10001000);

	SOR_WRITE(SOR_NV_PDISP_SOR_CRC_CNTRL_0, 0x00000001);
	DCA_WRITE(DC_COM_CRC_CONTROL_0, 0x00000009);	//CRC_ALWAYS+CRC_ENABLE
	DCA_WRITE(DC_COM_PIN_OUTPUT_ENABLE2_0, 0x00000000);
	DCA_WRITE(DC_COM_PIN_OUTPUT_ENABLE3_0, 0x00000000);
	DCA_WRITE(DC_DISP_DISP_SIGNAL_OPTIONS0_0, 0x00000000);
	//  DCA_WRITE (DC_DISP_BLEND_BACKGROUND_COLOR_0                ,WHITE     );
	DCA_WRITE(DC_DISP_BLEND_BACKGROUND_COLOR_0, YELLOW);
	DCA_WRITE(DC_CMD_DISPLAY_COMMAND_0, 0x00000020);
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_AUDIO_VBLANK_SYMBOLS_0, 0x00000e48);


	dpaux_write(0x101, 1, (enhanced_framing << 7) | lane_count);
	if (panel_edp)
		dpaux_write(0x10A, 1, 1);

	tusize =
		dp_buf_config(pclkfreq, (linkfreq * 1000000), lane_count, panel_bpp);

	printk(BIOS_SPEW, "JZ, after dp_buf_config, tusize: 0x%08x\n", tusize);

	linkctl =
		((0xF >> (4 - lane_count)) << 16) | (enhanced_framing << 14) | (tusize
										<< 2) |
		1;

	SOR_WRITE(SOR_NV_PDISP_SOR_DP_LINKCTL0_0, linkctl);
	SOR_WRITE(SOR_NV_PDISP_SOR_DP_SPARE0_0, ((panel_edp << 1) | 0x05));

	SOR_WRITE(SOR_NV_PDISP_SOR_PWR_0, 0x80000001);
	printk(BIOS_SPEW, "Polling SOR_NV_PDISP_SOR_PWR_0.DONE\n");
	dp_poll_register((void *)0x54540054, 0x00000000, 0x80000000, 1000);
	//SOR_NV_PDISP_SOR_PWR_0
	//sor_update
	SOR_WRITE(SOR_NV_PDISP_SOR_STATE0_0, 0x00000000);
	SOR_WRITE(SOR_NV_PDISP_SOR_SUPER_STATE1_0, 0x00000006);
	//sor_super_update
	SOR_WRITE(SOR_NV_PDISP_SOR_SUPER_STATE0_0, 0x00000000);
	SOR_WRITE(SOR_NV_PDISP_SOR_SUPER_STATE1_0, 0x0000000e);
	//sor_super_update
	SOR_WRITE(SOR_NV_PDISP_SOR_SUPER_STATE0_0, 0x00000000);
	printk(BIOS_SPEW, "Polling SOR_NV_PDISP_SOR_TEST_0.ATTACHED\n");
	dp_poll_register((void *)0x54540058, 0x00000400, 0x00000400, 1000);
	//SOR_NV_PDISP_SOR_TEST_0

	DCA_WRITE(DC_CMD_STATE_CONTROL_0, 0x00009f00);
	DCA_WRITE(DC_CMD_STATE_CONTROL_0, 0x0000009f);
	DCA_WRITE(DC_CMD_DISPLAY_POWER_CONTROL_0, 0x00050155);

	printk(BIOS_SPEW, "Polling SOR_NV_PDISP_SOR_TEST_0.AWAKE\n");
	dp_poll_register((void *)0x54540058, 0x00000200, 0x00000300, 1000);
	//SOR_NV_PDISP_SOR_TEST_0

	//  DCA_WRITE (DC_CMD_STATE_ACCESS_0   ,0);
	DCA_WRITE(DC_CMD_STATE_ACCESS_0, 4);
	DCA_WRITE(DC_CMD_STATE_CONTROL_0, 0x0000ffff);
	/* enable win_b */

	DCA_READ_M_WRITE(DC_B_WIN_BD_WIN_OPTIONS_0,
			 DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_FIELD,
			 (DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_ENABLE <<
			  DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_SHIFT));


	printk(BIOS_SPEW, "JZ: %s: f_ret @ line %d\n", __func__, __LINE__);
}