t124: Clean up display init functions

The existing display init functions were translated from a script. The new
code will play the same functions but are cleaner and readable and easier to
be ported to new panel.

BUG=none
TEST=build nyan and boot up kernel.

Signed-off-by: Jimmy Zhang <jimmzhang@nvidia.com>

Original-Change-Id: Ic9983e57684a03e206efe3731968ec62905f4ee8
Original-Reviewed-on: https://chromium-review.googlesource.com/189518
Original-Commit-Queue: Jimmy Zhang <jimmzhang@nvidia.com>
Original-Tested-by: Jimmy Zhang <jimmzhang@nvidia.com>
Original-Reviewed-by: Julius Werner <jwerner@chromium.org>
(cherry picked from commit 5998f991ea3069d603443b93c2ebdcdcd04af961)
Signed-off-by: Marc Jones <marc.jones@se-eng.com>

Squashed to pass abuild

nyan: Fix the build for big and blaze.

The display code for the tegra124 was cleaned up recently, but only the nyan
device tree was updated to match the new code, not big's or blaze's. This
change copies nyan's device tree over to those other two boards which will get
them building again. The settings may not be correct, but they'll be no less
correct than they were before. I also updated the copyright date for nyan.

BUG=none
TEST=Built for nyan, nyan_big, nyan_blaze. Booted on nyan_big and verified the
panel wasn't damaged by the new display code or settings.
BRANCH=None

Original-Change-Id: I75055a01f9402b3a9de9a787a9d3e737d25bb515
Original-Signed-off-by: Gabe Black <gabeblack@google.com>
Original-Reviewed-on: https://chromium-review.googlesource.com/191364
Original-Reviewed-by: Hung-Te Lin <hungte@chromium.org>
Original-Commit-Queue: Gabe Black <gabeblack@chromium.org>
Original-Tested-by: Gabe Black <gabeblack@chromium.org>
(cherry picked from commit ea235f23df31b4ca8006dcdf3628eed096e062b9)
Signed-off-by: Marc Jones <marc.jones@se-eng.com>

Change-Id: Icdad74bf2d013c3677e1a3373b8f89fad99f616e
Reviewed-on: http://review.coreboot.org/7454
Tested-by: build bot (Jenkins)
Reviewed-by: David Hendricks <dhendrix@chromium.org>

15 files changed, 1477 insertions, 1687 deletions

diff --git a/src/mainboard/google/nyan/devicetree.cb b/src/mainboard/google/nyan/devicetree.cb
index 5ae86913d1..bf6c7f3bb3 100644
--- a/src/mainboard/google/nyan/devicetree.cb
+++ b/src/mainboard/google/nyan/devicetree.cb
@@ -1,7 +1,7 @@
 ##
 ## This file is part of the coreboot project.
 ##
-## Copyright 2013 Google Inc.
+## Copyright 2014 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
@@ -28,9 +28,14 @@ chip soc/nvidia/tegra124
 	register "display_controller" = "TEGRA_ARM_DISPLAYA"
 	register "xres" = "1366"
 	register "yres" = "768"
-	# this setting is what nvidia does; it makes no sense
-	# and does not agree with hardware. Why'd they do it?
-	register "framebuffer_bits_per_pixel" = "18"
+
+	# bits per pixel and color depth
+	register "framebuffer_bits_per_pixel" = "16"
+	register "color_depth" = "6"
+	# "6" is defined as COLOR_DEPTH_B5G6R5 in dc_reg.h
+
+	register "panel_bits_per_pixel" = "18"
+
 	register "cache_policy" = "DCACHE_WRITETHROUGH"
 
 	# With some help from the mainbaord designer
@@ -40,12 +45,12 @@ chip soc/nvidia/tegra124
 	register "panel_vdd_gpio" = "0"
 	register "pwm" = "1"
 
-# taken from u-boot; these look wrong however.
-	register "vdd_delay" = "400"
-	register "vdd_data_delay" = "4"
-	register "data_backlight_delay" = "203"
-	register "backlight_pwm_delay" = "17"
-	register "pwm_backlight_en_delay" = "15"
+	# various panel delay time
+	register "vdd_delay_ms" = "200"
+	register "vdd_to_hpd_delay_ms" = "200"
+	register "hpd_unplug_min_us" = "2000"
+	register "hpd_plug_min_us" = "250"
+	register "hpd_irq_min_us" = "250"
 
 # How to compute these: xrandr --verbose will give you this:
 #Detailed mode: Clock 285.250 MHz, 272 mm x 181 mm
@@ -64,17 +69,33 @@ chip soc/nvidia/tegra124
 #  1366x768 (0x45)   76.4MHz -HSync -VSync *current +preferred
 #        h: width  1366 start 1502 end 1532 total 1592
 #        v: height  768 start  776 end  788 total  800
-# These numbers were provided by Nvidia.
-	register "href_to_sync" = "1"
-	register "hfront_porch" = "44"
-	register "hsync_width" = "46"
-	register "hback_porch" = "44"
+	register "href_to_sync" = "68"
+	register "hfront_porch" = "136"
+	register "hsync_width" = "30"
+	register "hback_porch" = "60"
 
-	register "vref_to_sync" = "1"
-	register "vfront_porch" = "6"
-	register "vsync_width" = "8"
-	register "vback_porch" = "6"
+	register "vref_to_sync" = "4"
+	register "vfront_porch" = "8"
+	register "vsync_width" = "12"
+	register "vback_porch" = "12"
 
 	# we *know* the pixel clock for this system.
-	register "pixel_clock" = "71"
+	# 1592 x 800 x 60Hz = 76416000
+	register "pixel_clock" = "76416000"
+	register "pll_div" = "2"
+
+	# use plld_out0 (ie, plld/2) as clock source
+	#  plld -> plld_out0 -> pclk
+	#  plld = plld_out0 * 2 = (pclk * pll_div) * 2
+	#       = 305664000Hz
+
+	# link configurations
+	register "lane_count" = "1"
+	register "enhanced_framing" = "1"
+	register "link_bw" = "10"
+	# "10" is defined as SOR_LINK_SPEED_G2_7 in sor.h
+
+	register "drive_current" = "0x13131313"
+	register "preemphasis" = "0x00000000"
+	register "postcursor" = "0"
 end
diff --git a/src/mainboard/google/nyan_big/devicetree.cb b/src/mainboard/google/nyan_big/devicetree.cb
index 9541ce9c9d..bf6c7f3bb3 100644
--- a/src/mainboard/google/nyan_big/devicetree.cb
+++ b/src/mainboard/google/nyan_big/devicetree.cb
@@ -28,9 +28,14 @@ chip soc/nvidia/tegra124
 	register "display_controller" = "TEGRA_ARM_DISPLAYA"
 	register "xres" = "1366"
 	register "yres" = "768"
-	# this setting is what nvidia does; it makes no sense
-	# and does not agree with hardware. Why'd they do it?
-	register "framebuffer_bits_per_pixel" = "18"
+
+	# bits per pixel and color depth
+	register "framebuffer_bits_per_pixel" = "16"
+	register "color_depth" = "6"
+	# "6" is defined as COLOR_DEPTH_B5G6R5 in dc_reg.h
+
+	register "panel_bits_per_pixel" = "18"
+
 	register "cache_policy" = "DCACHE_WRITETHROUGH"
 
 	# With some help from the mainbaord designer
@@ -40,12 +45,12 @@ chip soc/nvidia/tegra124
 	register "panel_vdd_gpio" = "0"
 	register "pwm" = "1"
 
-# taken from u-boot; these look wrong however.
-	register "vdd_delay" = "400"
-	register "vdd_data_delay" = "4"
-	register "data_backlight_delay" = "203"
-	register "backlight_pwm_delay" = "17"
-	register "pwm_backlight_en_delay" = "15"
+	# various panel delay time
+	register "vdd_delay_ms" = "200"
+	register "vdd_to_hpd_delay_ms" = "200"
+	register "hpd_unplug_min_us" = "2000"
+	register "hpd_plug_min_us" = "250"
+	register "hpd_irq_min_us" = "250"
 
 # How to compute these: xrandr --verbose will give you this:
 #Detailed mode: Clock 285.250 MHz, 272 mm x 181 mm
@@ -64,17 +69,33 @@ chip soc/nvidia/tegra124
 #  1366x768 (0x45)   76.4MHz -HSync -VSync *current +preferred
 #        h: width  1366 start 1502 end 1532 total 1592
 #        v: height  768 start  776 end  788 total  800
-# These numbers were provided by Nvidia.
-	register "href_to_sync" = "1"
-	register "hfront_porch" = "44"
-	register "hsync_width" = "46"
-	register "hback_porch" = "44"
+	register "href_to_sync" = "68"
+	register "hfront_porch" = "136"
+	register "hsync_width" = "30"
+	register "hback_porch" = "60"
 
-	register "vref_to_sync" = "1"
-	register "vfront_porch" = "6"
-	register "vsync_width" = "8"
-	register "vback_porch" = "6"
+	register "vref_to_sync" = "4"
+	register "vfront_porch" = "8"
+	register "vsync_width" = "12"
+	register "vback_porch" = "12"
 
 	# we *know* the pixel clock for this system.
-	register "pixel_clock" = "71"
+	# 1592 x 800 x 60Hz = 76416000
+	register "pixel_clock" = "76416000"
+	register "pll_div" = "2"
+
+	# use plld_out0 (ie, plld/2) as clock source
+	#  plld -> plld_out0 -> pclk
+	#  plld = plld_out0 * 2 = (pclk * pll_div) * 2
+	#       = 305664000Hz
+
+	# link configurations
+	register "lane_count" = "1"
+	register "enhanced_framing" = "1"
+	register "link_bw" = "10"
+	# "10" is defined as SOR_LINK_SPEED_G2_7 in sor.h
+
+	register "drive_current" = "0x13131313"
+	register "preemphasis" = "0x00000000"
+	register "postcursor" = "0"
 end
diff --git a/src/mainboard/google/nyan_blaze/devicetree.cb b/src/mainboard/google/nyan_blaze/devicetree.cb
index 9541ce9c9d..bf6c7f3bb3 100644
--- a/src/mainboard/google/nyan_blaze/devicetree.cb
+++ b/src/mainboard/google/nyan_blaze/devicetree.cb
@@ -28,9 +28,14 @@ chip soc/nvidia/tegra124
 	register "display_controller" = "TEGRA_ARM_DISPLAYA"
 	register "xres" = "1366"
 	register "yres" = "768"
-	# this setting is what nvidia does; it makes no sense
-	# and does not agree with hardware. Why'd they do it?
-	register "framebuffer_bits_per_pixel" = "18"
+
+	# bits per pixel and color depth
+	register "framebuffer_bits_per_pixel" = "16"
+	register "color_depth" = "6"
+	# "6" is defined as COLOR_DEPTH_B5G6R5 in dc_reg.h
+
+	register "panel_bits_per_pixel" = "18"
+
 	register "cache_policy" = "DCACHE_WRITETHROUGH"
 
 	# With some help from the mainbaord designer
@@ -40,12 +45,12 @@ chip soc/nvidia/tegra124
 	register "panel_vdd_gpio" = "0"
 	register "pwm" = "1"
 
-# taken from u-boot; these look wrong however.
-	register "vdd_delay" = "400"
-	register "vdd_data_delay" = "4"
-	register "data_backlight_delay" = "203"
-	register "backlight_pwm_delay" = "17"
-	register "pwm_backlight_en_delay" = "15"
+	# various panel delay time
+	register "vdd_delay_ms" = "200"
+	register "vdd_to_hpd_delay_ms" = "200"
+	register "hpd_unplug_min_us" = "2000"
+	register "hpd_plug_min_us" = "250"
+	register "hpd_irq_min_us" = "250"
 
 # How to compute these: xrandr --verbose will give you this:
 #Detailed mode: Clock 285.250 MHz, 272 mm x 181 mm
@@ -64,17 +69,33 @@ chip soc/nvidia/tegra124
 #  1366x768 (0x45)   76.4MHz -HSync -VSync *current +preferred
 #        h: width  1366 start 1502 end 1532 total 1592
 #        v: height  768 start  776 end  788 total  800
-# These numbers were provided by Nvidia.
-	register "href_to_sync" = "1"
-	register "hfront_porch" = "44"
-	register "hsync_width" = "46"
-	register "hback_porch" = "44"
+	register "href_to_sync" = "68"
+	register "hfront_porch" = "136"
+	register "hsync_width" = "30"
+	register "hback_porch" = "60"
 
-	register "vref_to_sync" = "1"
-	register "vfront_porch" = "6"
-	register "vsync_width" = "8"
-	register "vback_porch" = "6"
+	register "vref_to_sync" = "4"
+	register "vfront_porch" = "8"
+	register "vsync_width" = "12"
+	register "vback_porch" = "12"
 
 	# we *know* the pixel clock for this system.
-	register "pixel_clock" = "71"
+	# 1592 x 800 x 60Hz = 76416000
+	register "pixel_clock" = "76416000"
+	register "pll_div" = "2"
+
+	# use plld_out0 (ie, plld/2) as clock source
+	#  plld -> plld_out0 -> pclk
+	#  plld = plld_out0 * 2 = (pclk * pll_div) * 2
+	#       = 305664000Hz
+
+	# link configurations
+	register "lane_count" = "1"
+	register "enhanced_framing" = "1"
+	register "link_bw" = "10"
+	# "10" is defined as SOR_LINK_SPEED_G2_7 in sor.h
+
+	register "drive_current" = "0x13131313"
+	register "preemphasis" = "0x00000000"
+	register "postcursor" = "0"
 end
diff --git a/src/soc/nvidia/tegra/dc.h b/src/soc/nvidia/tegra/dc.h
index 48ffbda356..c0b1986c4f 100644
--- a/src/soc/nvidia/tegra/dc.h
+++ b/src/soc/nvidia/tegra/dc.h
@@ -85,7 +85,7 @@ struct dc_cmd_reg {
 
 	/* Address 0x040 ~ 0x043 */
 	u32 state_access;		/* _CMD_STATE_ACCESS_0 */
-	u32 state_ctrl;		/* _CMD_STATE_CONTROL_0 */
+	u32 state_ctrl;			/* _CMD_STATE_CONTROL_0 */
 	u32 disp_win_header;		/* _CMD_DISPLAY_WINDOW_HEADER_0 */
 	u32 reg_act_ctrl;		/* _CMD_REG_ACT_CONTROL_0 */
 };
@@ -195,14 +195,13 @@ enum dc_disp_pp_select {
 struct dc_disp_reg {
 	/* Address 0x400 ~ 0x40a */
 	u32 disp_signal_opt0;		/* _DISP_DISP_SIGNAL_OPTIONS0_0 */
-	u32 disp_signal_opt1;		/* _DISP_DISP_SIGNAL_OPTIONS1_0 */
+	u32 rsvd_401;
 	u32 disp_win_opt;		/* _DISP_DISP_WIN_OPTIONS_0 */
-	u32 mem_high_pri;		/* _DISP_MEM_HIGH_PRIORITY_0 */
-	u32 mem_high_pri_timer;	/* _DISP_MEM_HIGH_PRIORITY_TIMER_0 */
+	u32 rsvd_403[2];		/* 403 - 404 */
 	u32 disp_timing_opt;		/* _DISP_DISP_TIMING_OPTIONS_0 */
 	u32 ref_to_sync;		/* _DISP_REF_TO_SYNC_0 */
-	u32 sync_width;		/* _DISP_SYNC_WIDTH_0 */
-	u32 back_porch;		/* _DISP_BACK_PORCH_0 */
+	u32 sync_width;			/* _DISP_SYNC_WIDTH_0 */
+	u32 back_porch;			/* _DISP_BACK_PORCH_0 */
 	u32 disp_active;		/* _DISP_DISP_ACTIVE_0 */
 	u32 front_porch;		/* _DISP_FRONT_PORCH_0 */
 
@@ -217,64 +216,49 @@ struct dc_disp_reg {
 	struct _disp_v_pulse2 v_pulse3;	/* _DISP_V_PULSE2_ */
 	struct _disp_v_pulse2 v_pulse4;	/* _DISP_V_PULSE3_ */
 
-	/* Address 0x426 ~ 0x429 */
-	u32 m0_ctrl;			/* _DISP_M0_CONTROL_0 */
-	u32 m1_ctrl;			/* _DISP_M1_CONTROL_0 */
-	u32 di_ctrl;			/* _DISP_DI_CONTROL_0 */
-	u32 pp_ctrl;			/* _DISP_PP_CONTROL_0 */
+	u32 rsvd_426[8];		/* 426 - 42d */
 
-	/* Address 0x42a ~ 0x42d: _DISP_PP_SELECT_A/B/C/D_0 */
-	u32 pp_select[PP_SELECT_COUNT];
-
-	/* Address 0x42e ~ 0x435 */
+	/* Address 0x42e ~ 0x430 */
 	u32 disp_clk_ctrl;		/* _DISP_DISP_CLOCK_CONTROL_0 */
 	u32 disp_interface_ctrl;	/* _DISP_DISP_INTERFACE_CONTROL_0 */
 	u32 disp_color_ctrl;		/* _DISP_DISP_COLOR_CONTROL_0 */
-	u32 shift_clk_opt;		/* _DISP_SHIFT_CLOCK_OPTIONS_0 */
-	u32 data_enable_opt;		/* _DISP_DATA_ENABLE_OPTIONS_0 */
-	u32 serial_interface_opt;	/* _DISP_SERIAL_INTERFACE_OPTIONS_0 */
-	u32 lcd_spi_opt;		/* _DISP_LCD_SPI_OPTIONS_0 */
-	u32 border_color;		/* _DISP_BORDER_COLOR_0 */
-
-	/* Address 0x436 ~ 0x439 */
-	u32 color_key0_lower;		/* _DISP_COLOR_KEY0_LOWER_0 */
+
+	u32 rsvd_431[6];		/* 431 - 436 */
+
+	/* Address 0x437 ~ 0x439 */
 	u32 color_key0_upper;		/* _DISP_COLOR_KEY0_UPPER_0 */
 	u32 color_key1_lower;		/* _DISP_COLOR_KEY1_LOWER_0 */
 	u32 color_key1_upper;		/* _DISP_COLOR_KEY1_UPPER_0 */
 
-	u32 reserved0[2];		/* reserved_0[2] */
+	u32 reserved0[2];		/* 43a - 43b */
 
-	/* Address 0x43c ~ 0x442 */
+	/* Address 0x43c ~ 0x441 */
 	u32 cursor_foreground;		/* _DISP_CURSOR_FOREGROUND_0 */
 	u32 cursor_background;		/* _DISP_CURSOR_BACKGROUND_0 */
 	u32 cursor_start_addr;		/* _DISP_CURSOR_START_ADDR_0 */
 	u32 cursor_start_addr_ns;	/* _DISP_CURSOR_START_ADDR_NS_0 */
 	u32 cursor_pos;		/* _DISP_CURSOR_POSITION_0 */
 	u32 cursor_pos_ns;		/* _DISP_CURSOR_POSITION_NS_0 */
-	u32 seq_ctrl;			/* _DISP_INIT_SEQ_CONTROL_0 */
-
-	/* Address 0x442 ~ 0x446 */
-	u32 spi_init_seq_data_a;	/* _DISP_SPI_INIT_SEQ_DATA_A_0 */
-	u32 spi_init_seq_data_b;	/* _DISP_SPI_INIT_SEQ_DATA_B_0 */
-	u32 spi_init_seq_data_c;	/* _DISP_SPI_INIT_SEQ_DATA_C_0 */
-	u32 spi_init_seq_data_d;	/* _DISP_SPI_INIT_SEQ_DATA_D_0 */
 
-	u32 reserved1[0x39];		/* reserved1[0x39], */
+	u32 rsvd_442[62];		/* 442 - 47f */
 
-	/* Address 0x480 ~ 0x484 */
+	/* Address 0x480 ~ 0x483 */
 	u32 dc_mccif_fifoctrl;		/* _DISP_DC_MCCIF_FIFOCTRL_0 */
 	u32 mccif_disp0a_hyst;		/* _DISP_MCCIF_DISPLAY0A_HYST_0 */
 	u32 mccif_disp0b_hyst;		/* _DISP_MCCIF_DISPLAY0B_HYST_0 */
 	u32 mccif_disp0c_hyst;		/* _DISP_MCCIF_DISPLAY0C_HYST_0 */
-	u32 mccif_disp1b_hyst;		/* _DISP_MCCIF_DISPLAY1B_HYST_0 */
 
-	u32 reserved2[0x3b];		/* reserved2[0x3b] */
+	u32 rsvd_484[61];		/* 484 - 4c0 */
 
-	/* Address 0x4c0 ~ 0x4c1 */
-	u32 dac_crt_ctrl;		/* _DISP_DAC_CRT_CTRL_0 */
+	/* Address 0x4c1 */
 	u32 disp_misc_ctrl;		/* _DISP_DISP_MISC_CONTROL_0 */
+
+	u32 rsvd_4c2[34];		/* 4c2 - 4e3 */
+
+	/* Address 0x4e4 */
+	u32 blend_background_color;	/* _DISP_BLEND_BACKGROUND_COLOR_0 */
 };
-check_member(dc_disp_reg, disp_misc_ctrl, (0x4c1 - 0x400) * 4);
+check_member(dc_disp_reg, blend_background_color, (0x4e4 - 0x400) * 4);
 
 enum dc_winc_filter_p {
 	WINC_FILTER_COUNT	= 0x10,
@@ -310,9 +294,9 @@ struct dc_winc_reg {
 };
 check_member(dc_winc_reg, v_filter_p, (0x619 - 0x500) * 4);
 
-/* WIN A/B/C Register 0x700 ~ 0x714*/
+/* WIN A/B/C Register 0x700 ~ 0x719*/
 struct dc_win_reg {
-	/* Address 0x700 ~ 0x714 */
+	/* Address 0x700 ~ 0x719 */
 	u32 win_opt;			/* _WIN_WIN_OPTIONS_0 */
 	u32 byte_swap;			/* _WIN_BYTE_SWAP_0 */
 	u32 buffer_ctrl;		/* _WIN_BUFFER_CONTROL_0 */
@@ -324,22 +308,27 @@ struct dc_win_reg {
 	u32 v_initial_dda;		/* _WIN_V_INITIAL_DDA_0 */
 	u32 dda_increment;		/* _WIN_DDA_INCREMENT_0 */
 	u32 line_stride;		/* _WIN_LINE_STRIDE_0 */
-	u32 buf_stride;		/* _WIN_BUF_STRIDE_0 */
+	u32 buf_stride;			/* _WIN_BUF_STRIDE_0 */
 	u32 uv_buf_stride;		/* _WIN_UV_BUF_STRIDE_0 */
 	u32 buffer_addr_mode;		/* _WIN_BUFFER_ADDR_MODE_0 */
 	u32 dv_ctrl;			/* _WIN_DV_CONTROL_0 */
 	u32 blend_nokey;		/* _WIN_BLEND_NOKEY_0 */
-	u32 blend_1win;		/* _WIN_BLEND_1WIN_0 */
+	u32 blend_1win;			/* _WIN_BLEND_1WIN_0 */
 	u32 blend_2win_x;		/* _WIN_BLEND_2WIN_X_0 */
 	u32 blend_2win_y;		/* _WIN_BLEND_2WIN_Y_0 */
 	u32 blend_3win_xy;		/* _WIN_BLEND_3WIN_XY_0 */
 	u32 hp_fetch_ctrl;		/* _WIN_HP_FETCH_CONTROL_0 */
+	u32 global_alpha;		/* _WIN_GLOBAL_ALPHA */
+	u32 blend_layer_ctrl;		/* _WINBUF_BLEND_LAYER_CONTROL_0 */
+	u32 blend_match_select;		/* _WINBUF_BLEND_MATCH_SELECT_0 */
+	u32 blend_nomatch_select;	/* _WINBUF_BLEND_NOMATCH_SELECT_0 */
+	u32 blend_alpha_1bit;		/* _WINBUF_BLEND_ALPHA_1BIT_0 */
 };
-check_member(dc_win_reg, hp_fetch_ctrl, (0x714 - 0x700) * 4);
+check_member(dc_win_reg, blend_alpha_1bit, (0x719 - 0x700) * 4);
 
-/* WINBUF A/B/C Register 0x800 ~ 0x80a */
+/* WINBUF A/B/C Register 0x800 ~ 0x80d */
 struct dc_winbuf_reg {
-	/* Address 0x800 ~ 0x80a */
+	/* Address 0x800 ~ 0x80d */
 	u32 start_addr;		/* _WINBUF_START_ADDR_0 */
 	u32 start_addr_ns;		/* _WINBUF_START_ADDR_NS_0 */
 	u32 start_addr_u;		/* _WINBUF_START_ADDR_U_0 */
@@ -351,8 +340,11 @@ struct dc_winbuf_reg {
 	u32 addr_v_offset;		/* _WINBUF_ADDR_V_OFFSET_0 */
 	u32 addr_v_offset_ns;		/* _WINBUF_ADDR_V_OFFSET_NS_0 */
 	u32 uflow_status;		/* _WINBUF_UFLOW_STATUS_0 */
+	u32 buffer_surface_kind;	/* DC_WIN_BUFFER_SURFACE_KIND */
+	u32 rsvd_80c;
+	u32 start_addr_hi;		/* DC_WINBUF_START_ADDR_HI_0 */
 };
-check_member(dc_winbuf_reg, uflow_status, (0x80a - 0x800) * 4);
+check_member(dc_winbuf_reg, start_addr_hi, (0x80d - 0x800) * 4);
 
 /* Display Controller (DC_) regs */
 struct display_controller {
@@ -362,55 +354,28 @@ struct display_controller {
 	struct dc_com_reg com;		/* COM register 0x300 ~ 0x329 */
 	u32 reserved1[0xd6];
 
-	struct dc_disp_reg disp;	/* DISP register 0x400 ~ 0x4c1 */
-	u32 reserved2[0x3e];
+	struct dc_disp_reg disp;	/* DISP register 0x400 ~ 0x4e4 */
+	u32 reserved2[0x1b];
 
 	struct dc_winc_reg winc;	/* Window A/B/C 0x500 ~ 0x628 */
 	u32 reserved3[0xd7];
 
-	struct dc_win_reg win;		/* WIN A/B/C 0x700 ~ 0x714*/
-	u32 reserved4[0xeb];
+	struct dc_win_reg win;		/* WIN A/B/C 0x700 ~ 0x719*/
+	u32 reserved4[0xe6];
 
-	struct dc_winbuf_reg winbuf;	/* WINBUF A/B/C 0x800 ~ 0x80a */
+	struct dc_winbuf_reg winbuf;	/* WINBUF A/B/C 0x800 ~ 0x80d */
 };
 check_member(display_controller, winbuf, 0x800 * 4);
 
-#define BIT(pos)	(1U << pos)
+#define	BIT(pos)	(1U << pos)
 
 /* DC_CMD_DISPLAY_COMMAND 0x032 */
-#define CTRL_MODE_SHIFT		5
-#define CTRL_MODE_MASK		(0x3 << CTRL_MODE_SHIFT)
-enum {
-	CTRL_MODE_STOP,
-	CTRL_MODE_C_DISPLAY,
-	CTRL_MODE_NC_DISPLAY,
-};
-
-/* _WIN_COLOR_DEPTH_0 */
-enum win_color_depth_id {
-	COLOR_DEPTH_P1,
-	COLOR_DEPTH_P2,
-	COLOR_DEPTH_P4,
-	COLOR_DEPTH_P8,
-	COLOR_DEPTH_B4G4R4A4,
-	COLOR_DEPTH_B5G5R5A,
-	COLOR_DEPTH_B5G6R5,
-	COLOR_DEPTH_AB5G5R5,
-	COLOR_DEPTH_B8G8R8A8 = 12,
-	COLOR_DEPTH_R8G8B8A8,
-	COLOR_DEPTH_B6x2G6x2R6x2A8,
-	COLOR_DEPTH_R6x2G6x2B6x2A8,
-	COLOR_DEPTH_YCbCr422,
-	COLOR_DEPTH_YUV422,
-	COLOR_DEPTH_YCbCr420P,
-	COLOR_DEPTH_YUV420P,
-	COLOR_DEPTH_YCbCr422P,
-	COLOR_DEPTH_YUV422P,
-	COLOR_DEPTH_YCbCr422R,
-	COLOR_DEPTH_YUV422R,
-	COLOR_DEPTH_YCbCr422RA,
-	COLOR_DEPTH_YUV422RA,
-};
+#define  DISP_COMMAND_RAISE			(1 << 0)
+#define  DISP_CTRL_MODE_STOP			(0 << 5)
+#define  DISP_CTRL_MODE_C_DISPLAY		(1 << 5)
+#define  DISP_CTRL_MODE_NC_DISPLAY		(2 << 5)
+#define  DISP_COMMAND_RAISE_VECTOR(x)		(((x) & 0x1f) << 22)
+#define  DISP_COMMAND_RAISE_CHANNEL_ID(x)	(((x) & 0xf) << 27)
 
 /* DC_CMD_DISPLAY_POWER_CONTROL 0x036 */
 #define PW0_ENABLE		BIT(0)
@@ -423,20 +388,45 @@ enum win_color_depth_id {
 #define SPI_ENABLE		BIT(24)
 #define HSPI_ENABLE		BIT(25)
 
+/* DC_CMD_STATE_ACCESS 0x040 */
+#define  READ_MUX_ASSEMBLY	(0 << 0)
+#define  READ_MUX_ACTIVE	(1 << 0)
+#define  WRITE_MUX_ASSEMBLY	(0 << 2)
+#define  WRITE_MUX_ACTIVE	(1 << 2)
+
 /* DC_CMD_STATE_CONTROL 0x041 */
 #define GENERAL_ACT_REQ		BIT(0)
 #define WIN_A_ACT_REQ		BIT(1)
 #define WIN_B_ACT_REQ		BIT(2)
 #define WIN_C_ACT_REQ		BIT(3)
+#define WIN_D_ACT_REQ		BIT(4)
+#define WIN_H_ACT_REQ		BIT(5)
+#define CURSOR_ACT_REQ		BIT(7)
 #define GENERAL_UPDATE		BIT(8)
 #define WIN_A_UPDATE		BIT(9)
 #define WIN_B_UPDATE		BIT(10)
 #define WIN_C_UPDATE		BIT(11)
+#define WIN_D_UPDATE		BIT(12)
+#define WIN_H_UPDATE		BIT(13)
+#define CURSOR_UPDATE		BIT(15)
+#define NC_HOST_TRIG		BIT(24)
 
 /* DC_CMD_DISPLAY_WINDOW_HEADER 0x042 */
 #define WINDOW_A_SELECT		BIT(4)
 #define WINDOW_B_SELECT		BIT(5)
 #define WINDOW_C_SELECT		BIT(6)
+#define	WINDOW_D_SELECT		BIT(7)
+#define	WINDOW_H_SELECT		BIT(8)
+
+/* DC_DISP_DISP_WIN_OPTIONS 0x402 */
+#define	CURSOR_ENABLE		BIT(16)
+#define	SOR_ENABLE		BIT(25)
+#define	TVO_ENABLE		BIT(28)
+#define	DSI_ENABLE		BIT(29)
+#define	HDMI_ENABLE		BIT(30)
+
+/* DC_DISP_DISP_TIMING_OPTIONS 0x405 */
+#define	VSYNC_H_POSITION(x)	((x) & 0xfff)
 
 /* DC_DISP_DISP_CLOCK_CONTROL 0x42e */
 #define SHIFT_CLK_DIVIDER_SHIFT	0
@@ -458,118 +448,61 @@ enum {
 	PIXEL_CLK_DIVIDER_PCD24,
 	PIXEL_CLK_DIVIDER_PCD13,
 };
-
-/* DC_DISP_DISP_INTERFACE_CONTROL 0x42f */
-#define DATA_FORMAT_SHIFT	0
-#define DATA_FORMAT_MASK	(0xf << DATA_FORMAT_SHIFT)
-enum {
-	DATA_FORMAT_DF1P1C,
-	DATA_FORMAT_DF1P2C24B,
-	DATA_FORMAT_DF1P2C18B,
-	DATA_FORMAT_DF1P2C16B,
-	DATA_FORMAT_DF2S,
-	DATA_FORMAT_DF3S,
-	DATA_FORMAT_DFSPI,
-	DATA_FORMAT_DF1P3C24B,
-	DATA_FORMAT_DF1P3C18B,
-};
-#define DATA_ALIGNMENT_SHIFT	8
-enum {
-	DATA_ALIGNMENT_MSB,
-	DATA_ALIGNMENT_LSB,
-};
-#define DATA_ORDER_SHIFT	9
-enum {
-	DATA_ORDER_RED_BLUE,
-	DATA_ORDER_BLUE_RED,
-};
-
-/* DC_DISP_DATA_ENABLE_OPTIONS 0x432 */
-#define DE_SELECT_SHIFT		0
-#define DE_SELECT_MASK		(0x3 << DE_SELECT_SHIFT)
-#define DE_SELECT_ACTIVE_BLANK	0x0
-#define DE_SELECT_ACTIVE	0x1
-#define DE_SELECT_ACTIVE_IS	0x2
-#define DE_CONTROL_SHIFT	2
-#define DE_CONTROL_MASK		(0x7 << DE_CONTROL_SHIFT)
-enum {
-	DE_CONTROL_ONECLK,
-	DE_CONTROL_NORMAL,
-	DE_CONTROL_EARLY_EXT,
-	DE_CONTROL_EARLY,
-	DE_CONTROL_ACTIVE_BLANK,
-};
+#define  SHIFT_CLK_DIVIDER(x)		((x) & 0xff)
 
 /* DC_WIN_WIN_OPTIONS 0x700 */
-#define H_DIRECTION		BIT(0)
-enum {
-	H_DIRECTION_INCREMENT,
-	H_DIRECTION_DECREMENT,
-};
-#define V_DIRECTION		BIT(2)
-enum {
-	V_DIRECTION_INCREMENT,
-	V_DIRECTION_DECREMENT,
-};
-#define COLOR_EXPAND		BIT(6)
-#define CP_ENABLE		BIT(16)
-#define DV_ENABLE		BIT(20)
-#define WIN_ENABLE		BIT(30)
-
-/* DC_WIN_BYTE_SWAP 0x701 */
-#define BYTE_SWAP_SHIFT		0
-#define BYTE_SWAP_MASK		(3 << BYTE_SWAP_SHIFT)
+#define  H_DIRECTION_DECREMENT(x)	((x) << 0)
+#define  V_DIRECTION_DECREMENT(x)	((x) << 2)
+#define  WIN_SCAN_COLUMN		BIT(4)
+#define  COLOR_EXPAND			BIT(6)
+#define  H_FILTER_ENABLE(x)		((x) << 8)
+#define  V_FILTER_ENABLE(x)		((x) << 10)
+#define  CP_ENABLE			BIT(16)
+#define  CSC_ENABLE			BIT(18)
+#define  DV_ENABLE			BIT(20)
+#define  INTERLACE_ENABLE		BIT(23)
+#define  INTERLACE_DISABLE		(0 << 23)
+#define  WIN_ENABLE			BIT(30)
+
+/* _WIN_COLOR_DEPTH_0 0x703 */
 enum {
-	BYTE_SWAP_NOSWAP,
-	BYTE_SWAP_SWAP2,
-	BYTE_SWAP_SWAP4,
-	BYTE_SWAP_SWAP4HW
+	COLOR_DEPTH_P8 = 3,
+	COLOR_DEPTH_B4G4R4A4,
+	COLOR_DEPTH_B5G5R5A,
+	COLOR_DEPTH_B5G6R5,
+	COLOR_DEPTH_AB5G5R5,
+	COLOR_DEPTH_B8G8R8A8 = 12,
+	COLOR_DEPTH_R8G8B8A8,
+	COLOR_DEPTH_YCbCr422 = 16,
+	COLOR_DEPTH_YUV422,
+	COLOR_DEPTH_YCbCr420P,
+	COLOR_DEPTH_YUV420P,
+	COLOR_DEPTH_YCbCr422P,
+	COLOR_DEPTH_YUV422P,
+	COLOR_DEPTH_N422R,
+	COLOR_DEPTH_YCbCr422R = COLOR_DEPTH_N422R,
+	COLOR_DEPTH_N422R_TRUE,
+	COLOR_DEPTH_YUV422R = COLOR_DEPTH_N422R_TRUE,
+	COLOR_DEPTH_CrYCbY422,
+	COLOR_DEPTH_VYUY422,
 };
 
-/* DC_WIN_POSITION 0x704 */
-#define H_POSITION_SHIFT	0
-#define H_POSITION_MASK		(0x1FFF << H_POSITION_SHIFT)
-#define V_POSITION_SHIFT	16
-#define V_POSITION_MASK		(0x1FFF << V_POSITION_SHIFT)
-
-/* DC_WIN_SIZE 0x705 */
-#define H_SIZE_SHIFT		0
-#define H_SIZE_MASK		(0x1FFF << H_SIZE_SHIFT)
-#define V_SIZE_SHIFT		16
-#define V_SIZE_MASK		(0x1FFF << V_SIZE_SHIFT)
-
-/* DC_WIN_PRESCALED_SIZE 0x706 */
-#define H_PRESCALED_SIZE_SHIFT	0
-#define H_PRESCALED_SIZE_MASK	(0x7FFF << H_PRESCALED_SIZE)
-#define V_PRESCALED_SIZE_SHIFT	16
-#define V_PRESCALED_SIZE_MASK	(0x1FFF << V_PRESCALED_SIZE)
-
 /* DC_WIN_DDA_INCREMENT 0x709 */
-#define H_DDA_INC_SHIFT		0
-#define H_DDA_INC_MASK		(0xFFFF << H_DDA_INC_SHIFT)
-#define V_DDA_INC_SHIFT		16
-#define V_DDA_INC_MASK		(0xFFFF << V_DDA_INC_SHIFT)
-
-/* This holds information about a window which can be displayed */
-/* TODO: do we really need this for basic setup? Not sure yet. */
-struct disp_ctl_win {
-	enum win_color_depth_id fmt;	/* Color depth/format */
-	u32	bpp;		/* Bits per pixel */
-	u32	phys_addr;	/* Physical address in memory */
-	u32	x;		/* Horizontal address offset (bytes) */
-	u32	y;		/* Veritical address offset (bytes) */
-	u32	w;		/* Width of source window */
-	u32	h;		/* Height of source window */
-	u32	stride;		/* Number of bytes per line */
-	u32	out_x;		/* Left edge of output window (col) */
-	u32	out_y;		/* Top edge of output window (row) */
-	u32	out_w;		/* Width of output window in pixels */
-	u32	out_h;		/* Height of output window in pixels */
+#define	H_DDA_INC(x)		(((x) & 0xffff) << 0)
+#define	V_DDA_INC(x)		(((x) & 0xffff) << 16)
+
+struct tegra_dc {
+	void				*config;
+	void				*out;
+	void				*base;
 };
 
-void display_startup(device_t dev);
-void dp_bringup(u32 winb_addr);
+unsigned long READL(void * p);
+void WRITEL(unsigned long value, void * p);
 
+void display_startup(device_t dev);
+void dp_init(void * _config);
+void dp_enable(void * _dp);
 unsigned int fb_base_mb(void);
 
 #endif /* __SOC_NVIDIA_TEGRA_DC_H */
diff --git a/src/soc/nvidia/tegra/displayport.h b/src/soc/nvidia/tegra/displayport.h
index 0a86c41d9c..338ab775c1 100644
--- a/src/soc/nvidia/tegra/displayport.h
+++ b/src/soc/nvidia/tegra/displayport.h
@@ -1,7 +1,7 @@
 /*
  * drivers/video/tegra/dc/dpaux_regs.h
  *
- * Copyright (c) 2011, NVIDIA Corporation.
+ * Copyright (c) 2014, NVIDIA Corporation.
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -121,6 +121,8 @@
 #define DPAUX_HYBRID_SPARE_PAD_PWR_POWERUP		(0)
 #define DPAUX_HYBRID_SPARE_PAD_PWR_POWERDOWN		(1)
 
+#define	DPAUX_HPD_CONFIG_UNPLUG_MIN_TIME_SHIFT		(16)
+
 /* TODO: figure out which of the NV_ constants are the same as all the other
  * display port standard constants.
  */
@@ -133,7 +135,6 @@
 
 #define DP_AUX_MAX_BYTES		16
 
-#define DP_LCDVCC_TO_HPD_DELAY_MS	200
 #define DP_AUX_TIMEOUT_MS		40
 #define DP_DPCP_RETRY_SLEEP_NS		400
 
@@ -171,9 +172,12 @@ enum {
 #define EDP_PWR_OFF_TO_ON_TIME_MS	    (500+10)
 
 struct tegra_dc_dp_data {
+	struct tegra_dc			*dc;
 	struct tegra_dc_sor_data	sor;
 	void				*aux_base;
-	struct tegra_dc_dp_link_config	 link_cfg;
+	struct tegra_dc_dp_link_config	link_cfg;
+	u8				revision;
+	int				enabled;
 };
 
 
@@ -304,11 +308,4 @@ struct tegra_dc_dp_data {
 #define NV_DPCD_HDCP_BINFO_OFFSET			(0x0006802A)
 #define NV_DPCD_HDCP_KSV_FIFO_OFFSET			(0x0006802C)
 #define NV_DPCD_HDCP_AINFO_OFFSET			(0x0006803B)
-
-int tegra_dc_dpaux_read(struct tegra_dc_dp_data *dp, u32 cmd, u32 addr,
-			u8 *data, u32 *size, u32 *aux_stat);
-int dpaux_write(u32 addr, u32 size, u32 data);
-int dpaux_read(u32 addr, u32 size, u8 *data);
-void debug_dpaux_print(u32 addr, u32 size);
-void dp_link_training(u32 lanes, u32 speed);
 #endif /* __SOC_NVIDIA_TEGRA_DISPLAYPORT_H__ */
diff --git a/src/soc/nvidia/tegra124/Makefile.inc b/src/soc/nvidia/tegra124/Makefile.inc
index 07184f88b0..8d5fb26eb2 100644
--- a/src/soc/nvidia/tegra124/Makefile.inc
+++ b/src/soc/nvidia/tegra124/Makefile.inc
@@ -38,7 +38,7 @@ romstage-$(CONFIG_CONSOLE_SERIAL) += uart.c
 ramstage-y += cbfs.c
 ramstage-y += cbmem.c
 ramstage-y += clock.c
-ramstage-y += display.c displayhack.c
+ramstage-y += display.c
 ramstage-y += dma.c
 ramstage-y += i2c.c
 ramstage-y += maincpu.S
diff --git a/src/soc/nvidia/tegra124/chip.h b/src/soc/nvidia/tegra124/chip.h
index b05bcc7cba..87d043a8ca 100644
--- a/src/soc/nvidia/tegra124/chip.h
+++ b/src/soc/nvidia/tegra124/chip.h
@@ -23,14 +23,19 @@
 #include <soc/addressmap.h>
 #include "gpio.h"
 
+#define EFAULT  1
+#define EINVAL  2
+
 /* this is a misuse of the device tree. We're going to let it go for now but
  * we should at minimum have a struct for the display controller, since
  * the chip supports two.
  */
 struct soc_nvidia_tegra124_config {
-	int xres;
-	int yres;
-	int framebuffer_bits_per_pixel;
+	u32 xres;
+	u32 yres;
+	u32 framebuffer_bits_per_pixel;
+	u32 color_depth;
+	u32 panel_bits_per_pixel;
 	int cache_policy;
 	/* there are two. It's not unimaginable that we might someday
 	 * have two of these structs in a single mainboard.
@@ -60,32 +65,41 @@ struct soc_nvidia_tegra124_config {
 	 * This is stated to be four timings in the
 	 * u-boot docs. In any event, in coreboot, we generally
 	 * only delay long enough to let the panel wake up and then
-	 * do the control operations -- meaming, for *coreboot*
+	 * do the control operations -- meaning, for *coreboot*
 	 * we probably only need the vdd_delay, but payloads may
 	 * need the other info.
 	 */
 	/* Delay before from power on asserting vdd */
-	int vdd_delay;
-        /* delay between panel_vdd-rise and data-rise*/
-	int vdd_data_delay;
-	/* delay between data-rise and backlight_vdd-rise */
-	int data_backlight_delay;
-	/* delay between backlight_vdd and pwm-rise */
-	int backlight_pwm_delay;
-	/* delay between pwm-rise and backlight_en-rise */
-	int pwm_backlight_en_delay;
-	/* display timing.
-	 * we have not found a dts in which these are set */
-	int href_to_sync; /* u-boot code says 'set to 1' */
+	int vdd_delay_ms;
+
+	/* Delay before HPD high */
+	int vdd_to_hpd_delay_ms;
+
+	int hpd_unplug_min_us;
+	int hpd_plug_min_us;
+        int hpd_irq_min_us;
+
+	int href_to_sync;
 	int hsync_width;
 	int hback_porch;
 	int hfront_porch;
-	int vref_to_sync; /* u-boot code says 'set to 1' */
+	int vref_to_sync;
 	int vsync_width;
 	int vback_porch;
 	int vfront_porch;
 
 	int pixel_clock;
+	int pll_div;;
+
+	/* The minimum link configuraton settings */
+	u32 lane_count;
+	u32 enhanced_framing;
+	u32 link_bw;
+	u32 drive_current;
+	u32 preemphasis;
+	u32 postcursor;
+
+	void *dc_data;
 };
 
 #endif /* __SOC_NVIDIA_TEGRA124_CHIP_H__ */
diff --git a/src/soc/nvidia/tegra124/clock.c b/src/soc/nvidia/tegra124/clock.c
index c292355f33..cef4262f99 100644
--- a/src/soc/nvidia/tegra124/clock.c
+++ b/src/soc/nvidia/tegra124/clock.c
@@ -88,10 +88,9 @@ struct {
 	int khz;
 	struct pllcx_dividers	pllx;	/* target:  CONFIG_PLLX_KHZ */
 	struct pllcx_dividers	pllc;	/* target:  600 MHz */
-	struct pllpad_dividers	plld;	/* target:  925 MHz */
+	struct pllpad_dividers	plld;	/* target:  306 MHz */
 	struct pllu_dividers	pllu;	/* target;  960 MHz */
 	struct pllcx_dividers	plldp;	/* target;  270 MHz */
-	struct pllcx_dividers	plld2;	/* target;  570 MHz */
 	/* Based on T124 TRM (to be updatd), PLLP is set to 408MHz in HW.
 	 * Unless configuring PLLP to a frequency other than 408MHz,
 	 * software configuration on PLLP is unneeded. */
@@ -100,46 +99,41 @@ struct {
 		.khz = 12000,
 		.pllx = {.n = TEGRA_PLLX_KHZ / 12000, .m =  1, .p = 0},
 		.pllc = {.n =  50, .m =  1, .p = 0},
-		.plld = {.n = 283, .m = 12, .p = 0, .cpcon = 8}, /* 283 MHz */
+		.plld = {.n = 306, .m = 12, .p = 0, .cpcon = 8},
 		.pllu = {.n = 960, .m = 12, .p = 0, .cpcon = 12, .lfcon = 2},
 		.plldp = {.n = 90, .m =  1, .p = 3},
-		.plld2 = {.n = 95, .m =  1, .p = 1},
 	},
 	[OSC_FREQ_OSC13]{
 		.khz = 13000,
 		.pllx = {.n = TEGRA_PLLX_KHZ / 13000, .m =  1, .p = 0},
 		.pllc = {.n = 231, .m =  5, .p = 0},		/* 600.6 MHz */
-		.plld = {.n = 283, .m = 13, .p = 0, .cpcon = 8}, /* 283 MHz*/
+		.plld = {.n = 306, .m = 13, .p = 0, .cpcon = 8},
 		.pllu = {.n = 960, .m = 13, .p = 0, .cpcon = 12, .lfcon = 2},
 		.plldp = {.n = 83, .m =  1, .p = 3},		/* 269.75 MHz */
-		.plld2 = {.n = 88, .m =  1, .p = 1},		/* 572 MHz */
 	},
 	[OSC_FREQ_OSC16P8]{
 		.khz = 16800,
 		.pllx = {.n = TEGRA_PLLX_KHZ / 16800, .m =  1, .p = 0},
 		.pllc = {.n = 250, .m =  7, .p = 0},
-		.plld = {.n = 286, .m = 17, .p = 0, .cpcon = 8}, /* 282.6 MHz*/
+		.plld = {.n = 309, .m = 17, .p = 0, .cpcon = 8}, /* 305.4 MHz*/
 		.pllu = {.n = 400, .m =  7, .p = 0, .cpcon = 5, .lfcon = 2},
 		.plldp = {.n = 64, .m =  1, .p = 3},		/* 268.8 MHz */
-		.plld2 = {.n = 68, .m =  1, .p = 1},		/* 571.2  MHz */
 	},
 	[OSC_FREQ_OSC19P2]{
 		.khz = 19200,
 		.pllx = {.n = TEGRA_PLLX_KHZ / 19200, .m =  1, .p = 0},
 		.pllc = {.n = 125, .m =  4, .p = 0},
-		.plld = {.n = 251, .m = 17, .p = 0, .cpcon = 8}, /* 283.5 MHz */
+		.plld = {.n = 271, .m = 17, .p = 0, .cpcon = 8}, /* 306.1 MHz */
 		.pllu = {.n = 200, .m =  4, .p = 0, .cpcon = 3, .lfcon = 2},
 		.plldp = {.n = 56, .m =  1, .p = 3},		/* 270.75 MHz */
-		.plld2 = {.n = 59, .m =  1, .p = 1},		/* 566.4 MHz */
 	},
 	[OSC_FREQ_OSC26]{
 		.khz = 26000,
 		.pllx = {.n = TEGRA_PLLX_KHZ / 26000, .m =  1, .p = 0},
 		.pllc = {.n =  23, .m =  1, .p = 0},		   /* 598 MHz */
-		.plld = {.n = 283, .m = 26, .p = 0, .cpcon = 8}, /* 283 MHz */
+		.plld = {.n = 306, .m = 26, .p = 0, .cpcon = 8},
 		.pllu = {.n = 960, .m = 26, .p = 0, .cpcon = 12, .lfcon = 2},
 		.plldp = {.n = 83, .m =  2, .p = 3},		/* 266.50 MHz */
-		.plld2 = {.n = 88, .m =  2, .p = 1},		/* 572 MHz */
 	},
 	[OSC_FREQ_OSC38P4]{
 		.khz = 38400,
@@ -149,10 +143,9 @@ struct {
 		 */
 		.pllx = {.n = TEGRA_PLLX_KHZ / 19200, .m =  1, .p = 0},
 		.pllc = {.n = 125, .m =  4, .p = 0},
-		.plld = {.n = 125, .m = 17, .p = 0, .cpcon = 8}, /* 282.4 MHz */
+		.plld = {.n = 271, .m = 17, .p = 0, .cpcon = 8}, /* 306.1 MHz */
 		.pllu = {.n = 200, .m =  4, .p = 0, .cpcon = 3, .lfcon = 2},
 		.plldp = {.n = 56, .m =  2, .p = 3},		/* 268 MHz */
-		.plld2 = {.n = 59, .m =  2, .p = 1},		/* 566 MHz */
 	},
 	[OSC_FREQ_OSC48]{
 		.khz = 48000,
@@ -162,10 +155,9 @@ struct {
 		 */
 		.pllx = {.n = TEGRA_PLLX_KHZ / 12000, .m =  1, .p = 0},
 		.pllc = {.n =  50, .m =  1, .p = 0},
-		.plld = {.n = 71, .m = 12, .p = 0, .cpcon = 8}, /* 284 MHz */
+		.plld = {.n = 306, .m = 12, .p = 0, .cpcon = 8},
 		.pllu = {.n = 960, .m = 12, .p = 0, .cpcon = 12, .lfcon = 2},
 		.plldp = {.n = 90, .m =  4, .p = 3},		/* 264 MHz */
-		.plld2 = {.n = 95, .m =  4, .p = 1},
 	},
 };
 
diff --git a/src/soc/nvidia/tegra124/display.c b/src/soc/nvidia/tegra124/display.c
index 8c4e8232dd..4a0f4501a2 100644
--- a/src/soc/nvidia/tegra124/display.c
+++ b/src/soc/nvidia/tegra124/display.c
@@ -38,18 +38,26 @@
 #include "chip.h"
 #include <soc/display.h>
 
+struct tegra_dc dc_data;
+
 int dump = 0;
-unsigned long READL(void * p);
-void WRITEL(unsigned long value, void * p);
 unsigned long READL(void * p)
 {
-        unsigned long value = readl(p);
+        unsigned long value;
+
+	/*
+	 * In case of hard hung on readl(p), we can set dump > 1 to print out
+	 * the address accessed.
+	 */
+        if (dump > 1)
+		printk(BIOS_SPEW, "readl %p\n", p);
+
+        value = readl(p);
         if (dump)
 		printk(BIOS_SPEW, "readl %p %08lx\n", p, value);
         return value;
 }
 
-
 void WRITEL(unsigned long value, void * p)
 {
         if (dump)
@@ -57,174 +65,133 @@ void WRITEL(unsigned long value, void * p)
         writel(value, p);
 }
 
-static const u32 rgb_enb_tab[PIN_REG_COUNT] = {
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-};
-
-static const u32 rgb_polarity_tab[PIN_REG_COUNT] = {
-	0x00000000,
-	0x01000000,
-	0x00000000,
-	0x00000000,
-};
-
-static const u32 rgb_data_tab[PIN_REG_COUNT] = {
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-};
-
-static const u32 rgb_sel_tab[PIN_OUTPUT_SEL_COUNT] = {
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00210222,
-	0x00002200,
-	0x00020000,
-};
-
-static int update_display_mode(struct dc_disp_reg *disp,
-			       struct soc_nvidia_tegra124_config *config)
+/* return in 1000ths of a Hertz */
+static int tegra_dc_calc_refresh(const struct soc_nvidia_tegra124_config *config)
 {
-	u32 val;
-	u32 rate;
-	u32 div;
-
-	WRITEL(0x0, &disp->disp_timing_opt);
+	int h_total, v_total, refresh;
+	int pclk = config->pixel_clock;
+
+	h_total = config->xres + config->hfront_porch + config->hback_porch +
+		config->hsync_width;
+	v_total = config->yres + config->vfront_porch + config->vback_porch +
+		config->vsync_width;
+	if (!pclk || !h_total || !v_total)
+		return 0;
+	refresh = pclk / h_total;
+	refresh *= 1000;
+	refresh /= v_total;
+	return refresh;
+}
 
-	WRITEL(config->vref_to_sync << 16 | config->href_to_sync,
-		   &disp->ref_to_sync);
-	WRITEL(config->vsync_width << 16 | config->hsync_width, &disp->sync_width);
-	WRITEL(config->vback_porch << 16 | config->hback_porch, &disp->back_porch);
-	WRITEL(config->vfront_porch << 16 | config->hfront_porch,
-		   &disp->front_porch);
+static void print_mode(const struct soc_nvidia_tegra124_config *config)
+{
+	if (config) {
+		int refresh = tegra_dc_calc_refresh(config);
+		printk(BIOS_ERR,
+			"MODE:%dx%d@%d.%03uHz pclk=%d\n",
+			config->xres, config->yres,
+			refresh / 1000, refresh % 1000,
+			config->pixel_clock);
+	}
+}
 
-	WRITEL(config->xres | (config->yres << 16), &disp->disp_active);
+static int update_display_mode(struct display_controller *disp_ctrl,
+			       struct soc_nvidia_tegra124_config *config)
+{
+	unsigned long div = config->pll_div;
 
-	val = DE_SELECT_ACTIVE << DE_SELECT_SHIFT;
-	val |= DE_CONTROL_NORMAL << DE_CONTROL_SHIFT;
-	WRITEL(val, &disp->data_enable_opt);
+	print_mode(config);
 
-	val = DATA_FORMAT_DF1P1C << DATA_FORMAT_SHIFT;
-	val |= DATA_ALIGNMENT_MSB << DATA_ALIGNMENT_SHIFT;
-	val |= DATA_ORDER_RED_BLUE << DATA_ORDER_SHIFT;
-	WRITEL(val, &disp->disp_interface_ctrl);
+	WRITEL(0x1, &disp_ctrl->disp.disp_timing_opt);
 
-	/*
-	 * The pixel clock divider is in 7.1 format (where the bottom bit
-	 * represents 0.5). Here we calculate the divider needed to get from
-	 * the display clock (typically 600MHz) to the pixel clock. We round
-	 * up or down as requried.
-	 * We use pllp for now.
-	 */
-	rate = 600 * 1000000;
-	div = ((rate * 2 + config->pixel_clock / 2) / config->pixel_clock) - 2;
-	printk(BIOS_SPEW, "Display clock %d, divider %d\n", rate, div);
+	WRITEL(config->vref_to_sync << 16 | config->href_to_sync,
+		&disp_ctrl->disp.ref_to_sync);
 
-	WRITEL(0x00010001, &disp->shift_clk_opt);
+	WRITEL(config->vsync_width << 16 | config->hsync_width,
+		&disp_ctrl->disp.sync_width);
 
-	val = PIXEL_CLK_DIVIDER_PCD1 << PIXEL_CLK_DIVIDER_SHIFT;
-	val |= div << SHIFT_CLK_DIVIDER_SHIFT;
-	WRITEL(val, &disp->disp_clk_ctrl);
+	WRITEL(((config->vback_porch - config->vref_to_sync) << 16) | config->hback_porch,
+		&disp_ctrl->disp.back_porch);
 
-	return 0;
-}
+	WRITEL(((config->vfront_porch + config->vref_to_sync) << 16) | config->hfront_porch,
+		&disp_ctrl->disp.front_porch);
 
-static int setup_window(struct disp_ctl_win *win,
-						struct soc_nvidia_tegra124_config *config)
-{
-	int log2_bpp = log2(config->framebuffer_bits_per_pixel);
-	win->x = 0;
-	win->y = 0;
-	win->w = config->xres;
-	win->h = config->yres;
-	win->out_x = 0;
-	win->out_y = 0;
-	win->out_w = config->xres;
-	win->out_h = config->yres;
-	win->phys_addr = config->framebuffer_base;
-	win->stride = config->xres * (1 << log2_bpp) / 8;
-	printk(BIOS_SPEW, "%s: depth = %d\n", __func__, log2_bpp);
-	switch (log2_bpp) {
-		case 5:
-		case 24:
-			win->fmt = COLOR_DEPTH_R8G8B8A8;
-			win->bpp = 32;
-			break;
-		case 4:
-			win->fmt = COLOR_DEPTH_B5G6R5;
-			win->bpp = 16;
-			break;
-
-		default:
-			printk(BIOS_SPEW, "Unsupported LCD bit depth");
-			return -1;
-	}
+	WRITEL(config->xres | (config->yres << 16),
+		&disp_ctrl->disp.disp_active);
 
+	WRITEL((PIXEL_CLK_DIVIDER_PCD1 << PIXEL_CLK_DIVIDER_SHIFT) |
+		SHIFT_CLK_DIVIDER(div),
+		&disp_ctrl->disp.disp_clk_ctrl);
 	return 0;
 }
 
-static void update_window(struct display_controller *dc,
-						  struct disp_ctl_win *win,
-						  struct soc_nvidia_tegra124_config *config)
+static void update_window(struct display_controller *disp_ctrl,
+			  struct soc_nvidia_tegra124_config *config)
 {
-	u32 h_dda, v_dda;
 	u32 val;
 
-	val = READL(&dc->cmd.disp_win_header);
-	val |= WINDOW_A_SELECT;
-	WRITEL(val, &dc->cmd.disp_win_header);
-
-	WRITEL(win->fmt, &dc->win.color_depth);
-
-	clrsetbits_le32(&dc->win.byte_swap, BYTE_SWAP_MASK,
-					BYTE_SWAP_NOSWAP << BYTE_SWAP_SHIFT);
+	WRITEL(WINDOW_A_SELECT, &disp_ctrl->cmd.disp_win_header);
 
-	val = win->out_x << H_POSITION_SHIFT;
-	val |= win->out_y << V_POSITION_SHIFT;
-	WRITEL(val, &dc->win.pos);
+	WRITEL(((config->yres << 16) | config->xres), &disp_ctrl->win.size);
+	WRITEL(((config->yres << 16) |
+		(config->xres * config->framebuffer_bits_per_pixel / 8)),
+		&disp_ctrl->win.prescaled_size);
+	WRITEL(((config->xres * config->framebuffer_bits_per_pixel / 8 + 31) /
+		32 * 32), &disp_ctrl->win.line_stride);
 
-	val = win->out_w << H_SIZE_SHIFT;
-	val |= win->out_h << V_SIZE_SHIFT;
-	WRITEL(val, &dc->win.size);
+	WRITEL(config->color_depth, &disp_ctrl->win.color_depth);
 
-	val = (win->w * win->bpp / 8) << H_PRESCALED_SIZE_SHIFT;
-	val |= win->h << V_PRESCALED_SIZE_SHIFT;
-	WRITEL(val, &dc->win.prescaled_size);
+	WRITEL(config->framebuffer_base, &disp_ctrl->winbuf.start_addr);
+	WRITEL((V_DDA_INC(0x1000) | H_DDA_INC(0x1000)), &disp_ctrl->win.dda_increment);
 
-	WRITEL(0, &dc->win.h_initial_dda);
-	WRITEL(0, &dc->win.v_initial_dda);
+	WRITEL(COLOR_WHITE, &disp_ctrl->disp.blend_background_color);
+	WRITEL(DISP_CTRL_MODE_C_DISPLAY, &disp_ctrl->cmd.disp_cmd);
 
-	h_dda = (win->w * 0x1000) / MAX(win->out_w - 1, 1);
-	v_dda = (win->h * 0x1000) / MAX(win->out_h - 1, 1);
+	WRITEL(WRITE_MUX_ACTIVE, &disp_ctrl->cmd.state_access);
 
-	val = h_dda << H_DDA_INC_SHIFT;
-	val |= v_dda << V_DDA_INC_SHIFT;
-	WRITEL(val, &dc->win.dda_increment);
-
-	WRITEL(win->stride, &dc->win.line_stride);
-	WRITEL(0, &dc->win.buf_stride);
-
-	val = WIN_ENABLE;
-	if (win->bpp < 24)
-		val |= COLOR_EXPAND;
-	WRITEL(val, &dc->win.win_opt);
+	val = GENERAL_ACT_REQ | WIN_A_ACT_REQ;
+	val |= GENERAL_UPDATE | WIN_A_UPDATE;
+	WRITEL(val, &disp_ctrl->cmd.state_ctrl);
 
-	WRITEL((u32) win->phys_addr, &dc->winbuf.start_addr);
-	WRITEL(win->x, &dc->winbuf.addr_h_offset);
-	WRITEL(win->y, &dc->winbuf.addr_v_offset);
+	// Enable win_a
+	val = READL(&disp_ctrl->win.win_opt);
+	WRITEL(val | WIN_ENABLE, &disp_ctrl->win.win_opt);
+}
 
-	WRITEL(0xff00, &dc->win.blend_nokey);
-	WRITEL(0xff00, &dc->win.blend_1win);
+static int tegra_dc_init(struct display_controller *disp_ctrl)
+{
+	/* do not accept interrupts during initialization */
+	WRITEL(0x00000000, &disp_ctrl->cmd.int_mask);
+	WRITEL(WRITE_MUX_ASSEMBLY | READ_MUX_ASSEMBLY,
+		&disp_ctrl->cmd.state_access);
+	WRITEL(WINDOW_A_SELECT, &disp_ctrl->cmd.disp_win_header);
+	WRITEL(0x00000000, &disp_ctrl->win.win_opt);
+	WRITEL(0x00000000, &disp_ctrl->win.byte_swap);
+	WRITEL(0x00000000, &disp_ctrl->win.buffer_ctrl);
+
+	WRITEL(0x00000000, &disp_ctrl->win.pos);
+	WRITEL(0x00000000, &disp_ctrl->win.h_initial_dda);
+	WRITEL(0x00000000, &disp_ctrl->win.v_initial_dda);
+	WRITEL(0x00000000, &disp_ctrl->win.dda_increment);
+	WRITEL(0x00000000, &disp_ctrl->win.dv_ctrl);
+
+	WRITEL(0x01000000, &disp_ctrl->win.blend_layer_ctrl);
+	WRITEL(0x00000000, &disp_ctrl->win.blend_match_select);
+	WRITEL(0x00000000, &disp_ctrl->win.blend_nomatch_select);
+	WRITEL(0x00000000, &disp_ctrl->win.blend_alpha_1bit);
+
+	WRITEL(0x00000000, &disp_ctrl->winbuf.start_addr_hi);
+	WRITEL(0x00000000, &disp_ctrl->winbuf.addr_h_offset);
+	WRITEL(0x00000000, &disp_ctrl->winbuf.addr_v_offset);
+
+	WRITEL(0x00000000, &disp_ctrl->com.crc_checksum);
+	WRITEL(0x00000000, &disp_ctrl->com.pin_output_enb[0]);
+	WRITEL(0x00000000, &disp_ctrl->com.pin_output_enb[1]);
+	WRITEL(0x00000000, &disp_ctrl->com.pin_output_enb[2]);
+	WRITEL(0x00000000, &disp_ctrl->com.pin_output_enb[3]);
+	WRITEL(0x00000000, &disp_ctrl->disp.disp_signal_opt0);
 
-	val = GENERAL_ACT_REQ | WIN_A_ACT_REQ;
-	val |= GENERAL_UPDATE | WIN_A_UPDATE;
-	WRITEL(val, &dc->cmd.state_ctrl);
+	return 0;
 }
 
 uint32_t fb_base_mb(void)
@@ -237,16 +204,22 @@ uint32_t fb_base_mb(void)
  */
 void display_startup(device_t dev)
 {
-	u32 val;
-	int i;
 	struct soc_nvidia_tegra124_config *config = dev->chip_info;
-	struct display_controller *dc = (void *)config->display_controller;
-	struct pwm_controller *pwm = (void *)TEGRA_PWM_BASE;
-	struct disp_ctl_win window;
+	struct display_controller *disp_ctrl = (void *)config->display_controller;
+	struct pwm_controller 	*pwm = (void *)TEGRA_PWM_BASE;
+	struct tegra_dc		*dc = &dc_data;
 
 	/* should probably just make it all MiB ... in future */
 	u32 framebuffer_size_mb = config->framebuffer_size / MiB;
 	u32 framebuffer_base_mb= config->framebuffer_base / MiB;
+
+	/* init dc */
+	dc->base = (void *)TEGRA_ARM_DISPLAYA;
+	dc->config = config;
+	config->dc_data = dc;
+
+	dp_init(config);
+
 	/* light it all up */
 	/* This one may have been done in romstage but that's ok for now. */
 	if (config->panel_vdd_gpio){
@@ -254,13 +227,12 @@ void display_startup(device_t dev)
 		printk(BIOS_SPEW,"%s: panel_vdd setting gpio %08x to %d\n",
 			__func__, config->panel_vdd_gpio, 1);
 	}
-	delay(1);
+	udelay(config->vdd_delay_ms * 1000);
 	if (config->backlight_vdd_gpio){
 		gpio_output(config->backlight_vdd_gpio, 1);
 		printk(BIOS_SPEW,"%s: backlight vdd setting gpio %08x to %d\n",
 			__func__, config->backlight_vdd_gpio, 1);
 	}
-	delay(1);
 	if (config->lvds_shutdown_gpio){
 		gpio_output(config->lvds_shutdown_gpio, 0);
 		printk(BIOS_SPEW,"%s: lvds shutdown setting gpio %08x to %d\n",
@@ -281,10 +253,6 @@ void display_startup(device_t dev)
 		0x02e), /* frequency divider */
 	       &pwm->pwm[config->pwm].csr);
 
-	printk(BIOS_SPEW,
-		"%s: xres %d yres %d framebuffer_bits_per_pixel %d\n",
-		__func__,
-	       config->xres, config->yres, config->framebuffer_bits_per_pixel);
 	if (framebuffer_size_mb == 0){
 		framebuffer_size_mb = ALIGN_UP(config->xres * config->yres *
 			(config->framebuffer_bits_per_pixel / 8), MiB)/MiB;
@@ -293,13 +261,12 @@ void display_startup(device_t dev)
 	if (! framebuffer_base_mb)
 		framebuffer_base_mb = fb_base_mb();
 
+	config->framebuffer_size = framebuffer_size_mb * MiB;
+	config->framebuffer_base = framebuffer_base_mb * MiB;
+
 	mmu_config_range(framebuffer_base_mb, framebuffer_size_mb,
 		config->cache_policy);
 
-	/* Enable flushing after LCD writes if requested */
-	/* I don't understand this part yet.
-	   lcd_set_flush_dcache(config.cache_type & FDT_LCD_CACHE_FLUSH);
-	 */
 	printk(BIOS_SPEW, "LCD frame buffer at %dMiB to %dMiB\n", framebuffer_base_mb,
 		   framebuffer_base_mb + framebuffer_size_mb);
 
@@ -311,63 +278,34 @@ void display_startup(device_t dev)
 	 * light things up here once we're sure it's all working.
 	 */
 
-	/* init dc_a */
-	init_dca_regs();
+	/* Init dc */
+	if (tegra_dc_init(disp_ctrl)) {
+		printk(BIOS_ERR, "dc: init failed\n");
+		return;
+	}
 
-	/* power up perip */
-	dp_io_powerup();
+	/* Configure dc mode */
+	update_display_mode(disp_ctrl, config);
 
-	/* bringup dp */
-	dp_bringup(framebuffer_base_mb*MiB);
+	/* Enable dp */
+	dp_enable(dc->out);
 
-	/* init frame buffer */
+	/* Init frame buffer */
 	memset((void *)(framebuffer_base_mb*MiB), 0x00,
 			framebuffer_size_mb*MiB);
 
+	update_window(disp_ctrl, config);
+
+	printk(BIOS_INFO, "%s: display init done.\n", __func__);
+
 	/* tell depthcharge ...
 	 */
 	struct edid edid;
-	edid.x_resolution = 1376;
-	edid.y_resolution = 768;
-	edid.bytes_per_line = 1376 * 2;
-	edid.framebuffer_bits_per_pixel = 16;
+	edid.bytes_per_line = ((config->xres * config->framebuffer_bits_per_pixel / 8 + 31) /
+				32 * 32);
+	edid.x_resolution = edid.bytes_per_line / (config->framebuffer_bits_per_pixel / 8);
+	edid.y_resolution = config->yres;
+	edid.framebuffer_bits_per_pixel = config->framebuffer_bits_per_pixel;
 	set_vbe_mode_info_valid(&edid, (uintptr_t)(framebuffer_base_mb*MiB));
-
-	if (0){
-/* do we still need these? */
-	WRITEL(0x00000100, &dc->cmd.gen_incr_syncpt_ctrl);
-	WRITEL(0x0000011a, &dc->cmd.cont_syncpt_vsync);
-	WRITEL(0x00000000, &dc->cmd.int_type);
-	WRITEL(0x00000000, &dc->cmd.int_polarity);
-	WRITEL(0x00000000, &dc->cmd.int_mask);
-	WRITEL(0x00000000, &dc->cmd.int_enb);
-
-	val = PW0_ENABLE | PW1_ENABLE | PW2_ENABLE;
-	val |= PW3_ENABLE | PW4_ENABLE | PM0_ENABLE;
-	val |= PM1_ENABLE;
-	WRITEL(val, &dc->cmd.disp_pow_ctrl);
-
-	val = READL(&dc->cmd.disp_cmd);
-	val |= CTRL_MODE_C_DISPLAY << CTRL_MODE_SHIFT;
-	WRITEL(val, &dc->cmd.disp_cmd);
-
-	WRITEL(0x00000020, &dc->disp.mem_high_pri);
-	WRITEL(0x00000001, &dc->disp.mem_high_pri_timer);
-
-	for (i = 0; i < PIN_REG_COUNT; i++) {
-		WRITEL(rgb_enb_tab[i], &dc->com.pin_output_enb[i]);
-		WRITEL(rgb_polarity_tab[i], &dc->com.pin_output_polarity[i]);
-		WRITEL(rgb_data_tab[i], &dc->com.pin_output_data[i]);
-	}
-
-	for (i = 0; i < PIN_OUTPUT_SEL_COUNT; i++)
-		WRITEL(rgb_sel_tab[i], &dc->com.pin_output_sel[i]);
-
-	if (config->pixel_clock)
-		update_display_mode(&dc->disp, config);
-
-	if (!setup_window(&window, config))
-		update_window(dc, &window, config);
-	}
 }
 
diff --git a/src/soc/nvidia/tegra124/displayhack.c b/src/soc/nvidia/tegra124/displayhack.c
deleted file mode 100644
index af0ac16673..0000000000
--- a/src/soc/nvidia/tegra124/displayhack.c
+++ /dev/null
@@ -1,738 +0,0 @@
-/* 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/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);
-
-#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))); \
-	}
-
-#define SOR_WRITE(reg, val) \
-	{ \
-	WRITEL(val,  (void *)(TEGRA_ARM_SOR + (reg<<2))); \
-	}
-
-#define SOR_READ(reg) READL((void *)(TEGRA_ARM_SOR + (reg<<2)))
-
-#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))); \
-	}
-
-#define DPAUX_WRITE(reg, val) \
-	{ \
-	WRITEL(val,  (void *)(TEGRA_ARM_DPAUX + (reg<<2))); \
-	}
-#define DPAUX_READ(reg) READL((void *)(TEGRA_ARM_DPAUX + (reg<<2)))
-
-void init_dca_regs(void)
-{
-	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_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);
-	DCA_WRITE (DC_COM_CRC_CHECKSUM_0, 0x00000000);
-	DCA_WRITE (DC_COM_PIN_OUTPUT_ENABLE0_0, 0x00000000);
-	DCA_WRITE (DC_COM_PIN_OUTPUT_ENABLE1_0, 0x00000000);
-	DCA_WRITE (DC_COM_PIN_OUTPUT_ENABLE2_0, 0x00510104);
-	DCA_WRITE (DC_COM_PIN_OUTPUT_ENABLE3_0, 0x00000555);
-}
-
-static int dp_poll_register(void *addr, u32 exp_val, u32 mask, u32 timeout_ms)
-{
-	u32 reg_val = 0;
-
-	do {
-		udelay(1000);
-		reg_val = READL(addr);
-	} while (((reg_val & mask) != exp_val) && (--timeout_ms > 0));
-
-	if ((reg_val & mask) == exp_val)
-		return 0;	/* success */
-	printk(BIOS_WARNING, "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;
-
-	val_reg = READL((void *)(0x7000e400 + 0x1c4));	/* APBDEV_PMC_IO_DPD2_STATUS_0 */
-	if ((((val_reg & DP_LVDS) >> DP_LVDS_SHIFT) & 1) == power_down) {
-		printk(BIOS_DEBUG, "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)
-{
-	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);
-
-	/* Deassert E_DPD to enable core logic circuits, and wait for > 5us */
-	dp_io_set_dpd(0);
-	udelay(10);
-
-	/* Deassert PDBG to enable bandgap, and wait for > 20us. */
-	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(25);
-
-	/*
-	 * 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_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(210);
-
-	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);
-
-}
-
-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_ERR, "******AuxRead Error:%04x: status %08x\n",
-				 0x202, status);
-	else {
-		temp = DPAUX_READ(DPAUX_DP_AUXDATA_READ_W0);
-		if ((temp & mask) != (data & mask)) {
-			printk(BIOS_ERR, "AuxCheck ERROR:(r_data) %08x &"
-				" (mask) %08x != (data) %08x & (mask) %08x\n",
-				temp, mask, data, mask);
-			return -1;
-		} else {
-			printk(BIOS_DEBUG, "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)
-{
-	//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;
-	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(100);
-		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_ERR, "Error: 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_DEBUG, "\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_DEBUG, "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 * 1000);
-
-	sor_clock_start();
-
-	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_DEBUG, "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_DEBUG, "set link rate and lane number: %dMHz, %d lanes\n",
-		   (speed * 27), lanes);
-
-	dpaux_write(0x100, 2, ((lanes << 8) | speed));
-	printk(BIOS_DEBUG, "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 * 1000);
-	SOR_WRITE(SOR_NV_PDISP_SOR_DP_PADCTL0_0, reg_val);
-
-	printk(BIOS_DEBUG, "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_DEBUG, "level:%x\n", level);
-
-	debug_dpaux_print(0x210, 16);
-
-	printk(BIOS_DEBUG, "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_DEBUG, "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_DEBUG, "Link training done\n\n");
-}
-
-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 width, u32 height)
-{
-	u32 pclk_freq = 0;
-
-	u32 PLL_FREQ = (12 / 12 * 283) / 1 / 2;	/* 141.5 */
-	u32 PLL_DIV = 2;
-	u32 SYNC_WIDTH = (8 << 16) | 46;
-	u32 BACK_PORCH = (6 << 16) | 44;
-	u32 FRONT_PORCH = (6 << 16) | 44;
-	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;
-
-	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);
-	/* nv_bug 1021453 */
-	BACK_PORCH = BACK_PORCH - 0x10000;
-	FRONT_PORCH = FRONT_PORCH + 0x10000;
-
-	printk(BIOS_DEBUG, "ACTIVE:      %dx%d\n", (DISP_ACTIVE & 0xFFFF),
-		   (DISP_ACTIVE >> 16));
-	printk(BIOS_DEBUG, "TOTAL:       %dx%d\n", (DISP_TOTAL & 0xffff),
-		   (DISP_TOTAL >> 16));
-	printk(BIOS_DEBUG, "PLL Freq:    %d.%d MHz\n", PLL_FREQ_I, PLL_FREQ_F);
-	printk(BIOS_DEBUG, "Pclk Freq:   %d.%d MHz\n", PCLK_FREQ_I,
-		   PCLK_FREQ_F);
-	printk(BIOS_DEBUG, "Frame Rate:  %d.%d Hz\n", FRATE_I, FRATE_F);
-	printk(BIOS_DEBUG, "\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);
-
-	//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));
-	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_DEBUG, "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_DEBUG,
-		       "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_DEBUG, "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_DEBUG, "ratio:%d/%d\n", a, b);
-	if (a > (b * 98 / 100))
-		printk(BIOS_ERR, "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_DEBUG, "%s: winb: 0x%08x, panel_bpp %d ",
-			__func__, winb_addr, 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));
-	}
-
-	DCA_WRITE(DC_CMD_DISPLAY_WINDOW_HEADER_0, 0x00000010);
-	DCA_WRITE(DC_WIN_A_SIZE_0, ((height << 16) | width));
-	DCA_WRITE(DC_WIN_A_PRESCALED_SIZE_0,
-			  ((height << 16) | (width * SRC_BPP / 8)));
-	DCA_WRITE(DC_WIN_A_LINE_STRIDE_0,
-			  ((width * SRC_BPP / 8 + 31) / 32 * 32));
-	DCA_WRITE(DC_WIN_A_COLOR_DEPTH_0, COLORDEPTH);
-	DCA_WRITE(DC_WINBUF_A_START_ADDR_LO_0, winb_addr);
-	DCA_WRITE(DC_WIN_A_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, COLOR_WHITE);
-	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);
-
-	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_DEBUG, "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_DEBUG, "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_DEBUG, "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);
-
-	DCA_READ_M_WRITE(DC_WIN_A_WIN_OPTIONS_0,
-			 DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_FIELD,
-			 (DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_ENABLE <<
-			  DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_SHIFT));
-}
diff --git a/src/soc/nvidia/tegra124/dp.c b/src/soc/nvidia/tegra124/dp.c
index 6b0f4c8698..a9da269da9 100644
--- a/src/soc/nvidia/tegra124/dp.c
+++ b/src/soc/nvidia/tegra124/dp.c
@@ -24,12 +24,11 @@
 #include <soc/addressmap.h>
 #include <soc/nvidia/tegra/i2c.h>
 #include <soc/nvidia/tegra/dc.h>
+#include "chip.h"
 #include "sor.h"
 #include <soc/nvidia/tegra/displayport.h>
 
-extern int dump;
-unsigned long READL(void *p);
-void WRITEL(unsigned long value, void *p);
+struct tegra_dc_dp_data dp_data;
 
 static inline u32 tegra_dpaux_readl(struct tegra_dc_dp_data *dp, u32 reg)
 {
@@ -182,35 +181,6 @@ static int tegra_dc_dpaux_write_chunk(struct tegra_dc_dp_data *dp, u32 cmd,
 	return -1;
 }
 
-static int tegra_dc_dpaux_write(struct tegra_dc_dp_data *dp, u32 cmd, u32 addr,
-					u8 *data, u32 *size, u32 *aux_stat)
-{
-	u32 cur_size = 0;
-	u32 finished = 0;
-	u32 cur_left;
-	int ret = 0;
-
-	do {
-		cur_size = *size - finished;
-		if (cur_size > DP_AUX_MAX_BYTES)
-			cur_size = DP_AUX_MAX_BYTES;
-		cur_left = cur_size;
-		ret = tegra_dc_dpaux_write_chunk(dp, cmd, addr,
-						 data, &cur_left, aux_stat);
-
-		cur_size -= cur_left;
-		finished += cur_size;
-		addr += cur_size;
-		data += cur_size;
-
-		if (ret)
-			break;
-	} while (*size > finished);
-
-	*size = finished;
-	return ret;
-}
-
 static int tegra_dc_dpaux_read_chunk(struct tegra_dc_dp_data *dp, u32 cmd,
 					 u32 addr, u8 *data, u32 *size,
 					 u32 *aux_stat)
@@ -235,12 +205,10 @@ static int tegra_dc_dpaux_read_chunk(struct tegra_dc_dp_data *dp, u32 cmd,
 			return -1;
 	}
 
-	if (0) {
-		*aux_stat = tegra_dpaux_readl(dp, DPAUX_DP_AUXSTAT);
-		if (!(*aux_stat & DPAUX_DP_AUXSTAT_HPD_STATUS_PLUGGED)) {
-			printk(BIOS_SPEW, "dp: HPD is not detected\n");
-			//return EFAULT;
-		}
+	*aux_stat = tegra_dpaux_readl(dp, DPAUX_DP_AUXSTAT);
+	if (!(*aux_stat & DPAUX_DP_AUXSTAT_HPD_STATUS_PLUGGED)) {
+		printk(BIOS_SPEW, "dp: HPD is not detected\n");
+		return -1;
 	}
 
 	tegra_dpaux_writel(dp, DPAUX_DP_AUXADDR, addr);
@@ -262,8 +230,6 @@ static int tegra_dc_dpaux_read_chunk(struct tegra_dc_dp_data *dp, u32 cmd,
 			printk(BIOS_INFO, "dp: aux read transaction timeout\n");
 
 		*aux_stat = tegra_dpaux_readl(dp, DPAUX_DP_AUXSTAT);
-		printk(BIOS_DEBUG, "dp: %s: aux stat: 0x%08x\n", __func__,
-				*aux_stat);
 
 		if ((*aux_stat & DPAUX_DP_AUXSTAT_TIMEOUT_ERROR_PENDING) ||
 			(*aux_stat & DPAUX_DP_AUXSTAT_RX_ERROR_PENDING) ||
@@ -309,8 +275,6 @@ static int tegra_dc_dpaux_read_chunk(struct tegra_dc_dp_data *dp, u32 cmd,
 						DPAUX_DP_AUXDATA_READ_W(i));
 
 			*size = ((*aux_stat) & DPAUX_DP_AUXSTAT_REPLY_M_MASK);
-			printk(BIOS_INFO, "dp: aux read data %d bytes\n",
-					*size);
 			memcpy(data, temp_data, *size);
 
 			return 0;
@@ -325,230 +289,407 @@ static int tegra_dc_dpaux_read_chunk(struct tegra_dc_dp_data *dp, u32 cmd,
 	return -1;
 }
 
-int tegra_dc_dpaux_read(struct tegra_dc_dp_data *dp, u32 cmd, u32 addr,
-						u8 * data, u32 * size, u32 * aux_stat)
+static int tegra_dc_dp_dpcd_read(struct tegra_dc_dp_data *dp, u32 cmd,
+				 u8 * data_ptr)
 {
-	u32 finished = 0;
-	u32 cur_size;
-	int ret = 0;
-
-	do {
-		cur_size = *size - finished;
-		if (cur_size > DP_AUX_MAX_BYTES)
-			cur_size = DP_AUX_MAX_BYTES;
-
-		ret = tegra_dc_dpaux_read_chunk(dp, cmd, addr,
-						data, &cur_size, aux_stat);
-
-		/* cur_size should be the real size returned */
-		addr += cur_size;
-		data += cur_size;
-		finished += cur_size;
-
-		if (ret)
-			break;
+	u32 size = 1;
+	u32 status = 0;
+	int ret;
 
-	} while (*size > finished);
+	ret = tegra_dc_dpaux_read_chunk(dp, DPAUX_DP_AUXCTL_CMD_AUXRD,
+					cmd, data_ptr, &size, &status);
+	if (ret)
+		printk(BIOS_ERR,
+			"dp: Failed to read DPCD data. CMD 0x%x, Status 0x%x\n",
+			cmd, status);
 
-	*size = finished;
 	return ret;
 }
 
-static int tegra_dc_dp_dpcd_read(struct tegra_dc_dp_data *dp, u32 cmd,
-								 u8 * data_ptr)
+static int tegra_dc_dp_dpcd_write(struct tegra_dc_dp_data *dp, u32 cmd,
+				u8 data)
 {
 	u32 size = 1;
 	u32 status = 0;
 	int ret;
 
-	ret = tegra_dc_dpaux_read_chunk(dp, DPAUX_DP_AUXCTL_CMD_AUXRD,
-									cmd, data_ptr, &size, &status);
+	ret = tegra_dc_dpaux_write_chunk(dp, DPAUX_DP_AUXCTL_CMD_AUXWR,
+					cmd, &data, &size, &status);
 	if (ret)
 		printk(BIOS_ERR,
-			   "dp: Failed to read DPCD data. CMD 0x%x, Status 0x%x\n", cmd,
-			   status);
-
+		       "dp: Failed to write DPCD data. CMD 0x%x, Status 0x%x\n",
+		       cmd, status);
 	return ret;
 }
 
-static int tegra_dc_dp_init_max_link_cfg(struct tegra_dc_dp_data *dp,
-					 struct tegra_dc_dp_link_config *cfg)
+static void tegra_dc_dpaux_enable(struct tegra_dc_dp_data *dp)
 {
-	u8 dpcd_data;
-	int ret;
+	/* clear interrupt */
+	tegra_dpaux_writel(dp, DPAUX_INTR_AUX, 0xffffffff);
+	/* do not enable interrupt for now. Enable them when Isr in place */
+	tegra_dpaux_writel(dp, DPAUX_INTR_EN_AUX, 0x0);
+
+	tegra_dpaux_writel(dp, DPAUX_HYBRID_PADCTL,
+		DPAUX_HYBRID_PADCTL_AUX_DRVZ_OHM_50 |
+		DPAUX_HYBRID_PADCTL_AUX_CMH_V0_70 |
+		0x18 << DPAUX_HYBRID_PADCTL_AUX_DRVI_SHIFT |
+		DPAUX_HYBRID_PADCTL_AUX_INPUT_RCV_ENABLE);
+
+	tegra_dpaux_writel(dp, DPAUX_HYBRID_SPARE,
+			DPAUX_HYBRID_SPARE_PAD_PWR_POWERUP);
+}
 
-	ret = tegra_dc_dp_dpcd_read(dp, NV_DPCD_MAX_LANE_COUNT, &dpcd_data);
-	if (ret)
-		return ret;
+static void tegra_dc_dp_dump_link_cfg(struct tegra_dc_dp_data *dp,
+	const struct tegra_dc_dp_link_config *link_cfg)
+{
+	printk(BIOS_INFO, "DP config: cfg_name               "
+		"cfg_value\n");
+	printk(BIOS_INFO, "           Lane Count             %d\n",
+		link_cfg->max_lane_count);
+	printk(BIOS_INFO, "           SupportEnhancedFraming %s\n",
+		link_cfg->support_enhanced_framing ? "Y" : "N");
+	printk(BIOS_INFO, "           Bandwidth              %d\n",
+		link_cfg->max_link_bw);
+	printk(BIOS_INFO, "           bpp                    %d\n",
+		link_cfg->bits_per_pixel);
+	printk(BIOS_INFO, "           EnhancedFraming        %s\n",
+		link_cfg->enhanced_framing ? "Y" : "N");
+	printk(BIOS_INFO, "           Scramble_enabled       %s\n",
+		link_cfg->scramble_ena ? "Y" : "N");
+	printk(BIOS_INFO, "           LinkBW                 %d\n",
+		link_cfg->link_bw);
+	printk(BIOS_INFO, "           lane_count             %d\n",
+		link_cfg->lane_count);
+	printk(BIOS_INFO, "           activespolarity        %d\n",
+		link_cfg->activepolarity);
+	printk(BIOS_INFO, "           active_count           %d\n",
+		link_cfg->active_count);
+	printk(BIOS_INFO, "           tu_size                %d\n",
+		link_cfg->tu_size);
+	printk(BIOS_INFO, "           active_frac            %d\n",
+		link_cfg->active_frac);
+	printk(BIOS_INFO, "           watermark              %d\n",
+		link_cfg->watermark);
+	printk(BIOS_INFO, "           hblank_sym             %d\n",
+		link_cfg->hblank_sym);
+	printk(BIOS_INFO, "           vblank_sym             %d\n",
+		link_cfg->vblank_sym);
+};
+
+/* Calcuate if given cfg can meet the mode request. */
+/* Return true if mode is possible, false otherwise. */
+static int tegra_dc_dp_calc_config(struct tegra_dc_dp_data *dp,
+	const struct soc_nvidia_tegra124_config *config,
+	struct tegra_dc_dp_link_config *link_cfg)
+{
+	const u32	link_rate = 27 * link_cfg->link_bw * 1000 * 1000;
+	const u64	f	  = 100000;	/* precision factor */
+
+	u32	num_linkclk_line; /* Number of link clocks per line */
+	u64	ratio_f; /* Ratio of incoming to outgoing data rate */
+
+	u64	frac_f;
+	u64	activesym_f;	/* Activesym per TU */
+	u64	activecount_f;
+	u32	activecount;
+	u32	activepolarity;
+	u64	approx_value_f;
+	u32	activefrac		  = 0;
+	u64	accumulated_error_f	  = 0;
+	u32	lowest_neg_activecount	  = 0;
+	u32	lowest_neg_activepolarity = 0;
+	u32	lowest_neg_tusize	  = 64;
+	u32	num_symbols_per_line;
+	u64	lowest_neg_activefrac	  = 0;
+	u64	lowest_neg_error_f	  = 64 * f;
+	u64	watermark_f;
+
+	int	i;
+	int	neg;
+
+	if (!link_rate || !link_cfg->lane_count || !config->pixel_clock ||
+		!link_cfg->bits_per_pixel)
+		return -1;
 
-	cfg->max_lane_count = dpcd_data & NV_DPCD_MAX_LANE_COUNT_MASK;
-	printk(BIOS_INFO, "%s: max_lane_count: %d\n", __func__,
-		   cfg->max_lane_count);
+	if ((u64)config->pixel_clock * link_cfg->bits_per_pixel >=
+		(u64)link_rate * 8 * link_cfg->lane_count)
+		return -1;
 
-	cfg->support_enhanced_framing =
-		(dpcd_data & NV_DPCD_MAX_LANE_COUNT_ENHANCED_FRAMING_YES) ? 1 : 0;
-	printk(BIOS_INFO, "%s: enh-framing: %d\n", __func__,
-		   cfg->support_enhanced_framing);
+	num_linkclk_line = (u32)((u64)link_rate * (u64)config->xres / config->pixel_clock);
 
-	ret = tegra_dc_dp_dpcd_read(dp, NV_DPCD_MAX_DOWNSPREAD, &dpcd_data);
-	if (ret)
-		return ret;
-	cfg->downspread = (dpcd_data & NV_DPCD_MAX_DOWNSPREAD_VAL_0_5_PCT) ? 1 : 0;
-	printk(BIOS_INFO, "%s: downspread: %d\n", __func__, cfg->downspread);
+	ratio_f = (u64)config->pixel_clock * link_cfg->bits_per_pixel * f;
+	ratio_f /= 8;
+	ratio_f = (u64)(ratio_f / (link_rate * link_cfg->lane_count));
 
-	ret = tegra_dc_dp_dpcd_read(dp, NV_DPCD_MAX_LINK_BANDWIDTH,
-								&cfg->max_link_bw);
-	if (ret)
-		return ret;
-	printk(BIOS_INFO, "%s: max_link_bw: %d\n", __func__, cfg->max_link_bw);
+	for (i = 64; i >= 32; --i) {
+		activesym_f	= ratio_f * i;
+		activecount_f	= (u64)(activesym_f / (u32)f) * f;
+		frac_f		= activesym_f - activecount_f;
+		activecount	= (u32)((u64)(activecount_f / (u32)f));
 
-	// cfg->bits_per_pixel = dp->dc->pdata->default_out->depth;
-	cfg->bits_per_pixel = 18;
+		if (frac_f < (f / 2)) /* fraction < 0.5 */
+			activepolarity = 0;
+		else {
+			activepolarity = 1;
+			frac_f = f - frac_f;
+		}
 
-	/* TODO: need to come from the board file */
-	/* Venice2 settings */
-	cfg->drive_current = 0x20202020;
-	cfg->preemphasis = 0;
-	cfg->postcursor = 0;
+		if (frac_f != 0) {
+			frac_f = (u64)((f * f) / frac_f); /* 1/fraction */
+			if (frac_f > (15 * f))
+				activefrac = activepolarity ? 1 : 15;
+			else
+				activefrac = activepolarity ?
+					(u32)((u64)(frac_f / (u32)f)) + 1 :
+					(u32)((u64)(frac_f / (u32)f));
+		}
+
+		if (activefrac == 1)
+			activepolarity = 0;
+
+		if (activepolarity == 1)
+			approx_value_f = activefrac ? (u64)(
+				(activecount_f + (activefrac * f - f) * f) /
+				(activefrac * f)) :
+				activecount_f + f;
+		else
+			approx_value_f = activefrac ?
+				activecount_f + (u64)(f / activefrac) :
+				activecount_f;
+
+		if (activesym_f < approx_value_f) {
+			accumulated_error_f = num_linkclk_line *
+				(u64)((approx_value_f - activesym_f) / i);
+			neg = 1;
+		} else {
+			accumulated_error_f = num_linkclk_line *
+				(u64)((activesym_f - approx_value_f) / i);
+			neg = 0;
+		}
+
+		if ((neg && (lowest_neg_error_f > accumulated_error_f)) ||
+			(accumulated_error_f == 0)) {
+			lowest_neg_error_f = accumulated_error_f;
+			lowest_neg_tusize = i;
+			lowest_neg_activecount = activecount;
+			lowest_neg_activepolarity = activepolarity;
+			lowest_neg_activefrac = activefrac;
+
+			if (accumulated_error_f == 0)
+				break;
+		}
+	}
+
+	if (lowest_neg_activefrac == 0) {
+		link_cfg->activepolarity = 0;
+		link_cfg->active_count   = lowest_neg_activepolarity ?
+			lowest_neg_activecount : lowest_neg_activecount - 1;
+		link_cfg->tu_size	      = lowest_neg_tusize;
+		link_cfg->active_frac    = 1;
+	} else {
+		link_cfg->activepolarity = lowest_neg_activepolarity;
+		link_cfg->active_count   = (u32)lowest_neg_activecount;
+		link_cfg->tu_size	      = lowest_neg_tusize;
+		link_cfg->active_frac    = (u32)lowest_neg_activefrac;
+	}
+
+	watermark_f = (u64)((ratio_f * link_cfg->tu_size * (f - ratio_f)) / f);
+	link_cfg->watermark = (u32)((u64)((watermark_f + lowest_neg_error_f) /
+		f)) + link_cfg->bits_per_pixel / 4 - 1;
+	num_symbols_per_line = (config->xres * link_cfg->bits_per_pixel) /
+		(8 * link_cfg->lane_count);
+
+	if (link_cfg->watermark > 30) {
+		printk(BIOS_INFO,
+			"dp: sor setting: unable to get a good tusize, "
+			"force watermark to 30.\n");
+		link_cfg->watermark = 30;
+		return -1;
+	} else if (link_cfg->watermark > num_symbols_per_line) {
+		printk(BIOS_INFO,
+			"dp: sor setting: force watermark to the number "
+			"of symbols in the line.\n");
+		link_cfg->watermark = num_symbols_per_line;
+		return -1;
+	}
+
+	/* Refer to dev_disp.ref for more information. */
+	/* # symbols/hblank = ((SetRasterBlankEnd.X + SetRasterSize.Width - */
+	/*                      SetRasterBlankStart.X - 7) * link_clk / pclk) */
+	/*                      - 3 * enhanced_framing - Y */
+	/* where Y = (# lanes == 4) 3 : (# lanes == 2) ? 6 : 12 */
+	link_cfg->hblank_sym = (int)((u64)(((u64)(config->hback_porch +
+			config->hfront_porch + config->hsync_width - 7) *
+			link_rate) / config->pixel_clock)) -
+			3 * link_cfg->enhanced_framing -
+			(12 / link_cfg->lane_count);
+
+	if (link_cfg->hblank_sym < 0)
+		link_cfg->hblank_sym = 0;
+
+
+	/* Refer to dev_disp.ref for more information. */
+	/* # symbols/vblank = ((SetRasterBlankStart.X - */
+	/*                      SetRasterBlankEen.X - 25) * link_clk / pclk) */
+	/*                      - Y - 1; */
+	/* where Y = (# lanes == 4) 12 : (# lanes == 2) ? 21 : 39 */
+	link_cfg->vblank_sym = (int)((u64)((u64)(config->xres - 25)
+			* link_rate / config->pixel_clock)) - (36 /
+			link_cfg->lane_count) - 4;
+
+	if (link_cfg->vblank_sym < 0)
+		link_cfg->vblank_sym = 0;
+
+	link_cfg->is_valid = 1;
+	tegra_dc_dp_dump_link_cfg(dp, link_cfg);
 
-	ret = tegra_dc_dp_dpcd_read(dp, NV_DPCD_EDP_CONFIG_CAP, &dpcd_data);
-	if (ret)
-		return ret;
-	cfg->alt_scramber_reset_cap =
-		(dpcd_data & NV_DPCD_EDP_CONFIG_CAP_ASC_RESET_YES) ? 1 : 0;
-	cfg->only_enhanced_framing =
-		(dpcd_data & NV_DPCD_EDP_CONFIG_CAP_FRAMING_CHANGE_YES) ? 1 : 0;
-	printk(BIOS_DEBUG, "%s: alt_reset_cap: %d, only_enh_framing: %d\n",
-		   __func__, cfg->alt_scramber_reset_cap, cfg->only_enhanced_framing);
-
-	cfg->lane_count = cfg->max_lane_count;
-	cfg->link_bw = NV_SOR_LINK_SPEED_G1_62;
-	cfg->enhanced_framing = cfg->support_enhanced_framing;
 	return 0;
 }
 
-struct tegra_dc_dp_data dp_data;
-
-static int tegra_dc_dpcd_read_rev(struct tegra_dc_dp_data *dp, u8 * rev)
+static int tegra_dc_dp_init_link_cfg(
+			struct soc_nvidia_tegra124_config *config,
+			struct tegra_dc_dp_data *dp,
+			struct tegra_dc_dp_link_config *link_cfg)
 {
-	u32 size;
+	u8 dpcd_data;
 	int ret;
-	u32 status = 0;
 
-	size = 3;
-	ret = tegra_dc_dpaux_read(dp, DPAUX_DP_AUXCTL_CMD_AUXRD,
-							  NV_DPCD_REV, rev, &size, &status);
-	if (ret) {
-		printk(BIOS_WARNING, "dp: Failed to read NV_DPCD_REV\n");
-		return ret;
-	}
+	link_cfg->max_lane_count = config->lane_count;
+	link_cfg->support_enhanced_framing = config->enhanced_framing;
+	link_cfg->max_link_bw = config->link_bw;
+	link_cfg->drive_current = config->drive_current;
+	link_cfg->preemphasis = config->preemphasis;
+	link_cfg->postcursor = config->postcursor;
+	link_cfg->bits_per_pixel = config->panel_bits_per_pixel;
+
+	CHECK_RET(tegra_dc_dp_dpcd_read(dp, NV_DPCD_EDP_CONFIG_CAP,
+			&dpcd_data));
+	link_cfg->alt_scramber_reset_cap =
+		(dpcd_data & NV_DPCD_EDP_CONFIG_CAP_ASC_RESET_YES) ?
+		1 : 0;
+	link_cfg->only_enhanced_framing =
+		(dpcd_data & NV_DPCD_EDP_CONFIG_CAP_FRAMING_CHANGE_YES) ?
+		1 : 0;
+
+	link_cfg->lane_count = link_cfg->max_lane_count;
+	link_cfg->link_bw = link_cfg->max_link_bw;
+	link_cfg->enhanced_framing = link_cfg->support_enhanced_framing;
+
+	tegra_dc_dp_calc_config(dp, config, link_cfg);
 	return 0;
 }
 
-u32 dp_setup_timing(u32 width, u32 height);
-void dp_bringup(u32 winb_addr)
+void dp_init(void * _config)
 {
+	struct soc_nvidia_tegra124_config *config = (void *)_config;
+	struct tegra_dc *dc = config->dc_data;
 	struct tegra_dc_dp_data *dp = &dp_data;
 
-	u32 dpcd_rev;
-	u32 pclk_freq;
-
-	u32 xres = 1366;	/* norrin display */
-	u32 yres = 768;
+        // set up links among config, dc, dp and sor
+        dp->dc = dc;
+        dc->out = dp;
+        dp->sor.dc = dc;
 
+	dp->sor.power_is_up = 0;
 	dp->sor.base = (void *)TEGRA_ARM_SOR;
+	dp->sor.pmc_base = (void *)TEGRA_PMC_BASE;
 	dp->sor.portnum = 0;
-
+	dp->sor.link_cfg = &dp->link_cfg;
 	dp->aux_base = (void *)TEGRA_ARM_DPAUX;
+	dp->link_cfg.is_valid = 0;
+	dp->enabled = 0;
+}
 
-	/* read panel info */
-	if (!tegra_dc_dpcd_read_rev(dp, (u8 *)&dpcd_rev)) {
-		printk(BIOS_INFO, "PANEL info:\n");
-		printk(BIOS_INFO, "--DPCP version(%#x): %d.%d\n",
-				dpcd_rev, (dpcd_rev >> 4) & 0x0f,
-				(dpcd_rev & 0x0f));
-	}
-
-	if (tegra_dc_dp_init_max_link_cfg(dp, &dp->link_cfg))
-		printk(BIOS_ERR, "dp: failed to init link configuration\n");
-
-	dp_link_training((u32) (dp->link_cfg.lane_count),
-					 (u32) (dp->link_cfg.link_bw));
-
-	pclk_freq = dp_setup_timing(xres, yres);
-	printk(BIOS_DEBUG, "%s: pclk_freq: %d\n", __func__, pclk_freq);
+static void tegra_dp_hpd_config(struct tegra_dc_dp_data *dp,
+				struct soc_nvidia_tegra124_config *config)
+{
+	u32 val;
 
+	val = config->hpd_plug_min_us |
+		(config->hpd_unplug_min_us <<
+		DPAUX_HPD_CONFIG_UNPLUG_MIN_TIME_SHIFT);
+	tegra_dpaux_writel(dp, DPAUX_HPD_CONFIG, val);
 
-	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);
+	tegra_dpaux_writel(dp, DPAUX_HPD_IRQ_CONFIG, config->hpd_irq_min_us);
+}
 
-	dp_misc_setting(dp->link_cfg.bits_per_pixel,
-				xres, yres, winb_addr,
-				(u32) dp->link_cfg.lane_count,
-				(u32) dp->link_cfg.enhanced_framing,
-				(u32) dp->link_cfg.alt_scramber_reset_cap,
-				pclk_freq, dp->link_cfg.link_bw * 27);
+static int tegra_dp_hpd_plug(struct tegra_dc_dp_data *dp, int timeout_ms)
+{
+	u32 val;
+	u32 timeout = timeout_ms * 1000;
+	do {
+		val = tegra_dpaux_readl(dp, DPAUX_DP_AUXSTAT);
+		if (val & DPAUX_DP_AUXSTAT_HPD_STATUS_PLUGGED)
+			return 0;
+		udelay(100);
+		timeout -= 100;
+	} while (timeout > 0);
+	return -1;
 }
 
-void debug_dpaux_print(u32 addr, u32 size)
+void dp_enable(void * _dp)
 {
-	struct tegra_dc_dp_data *dp = &dp_data;
-	u32 status = 0;
-	u8 buf[16];
-	int i;
+	struct tegra_dc_dp_data *dp = _dp;
+	struct tegra_dc *dc = dp->dc;
+	struct soc_nvidia_tegra124_config *config = dc->config;
 
-	if ((size == 0) || (size > 16)) {
-		printk(BIOS_ERR, "dp: %s: invalid size %d\n", __func__, size);
-		return;
-	}
+	u8      data;
+	u32     retry;
+	int     ret;
+
+	tegra_dc_dpaux_enable(dp);
 
-	if (tegra_dc_dpaux_read(dp, DPAUX_DP_AUXCTL_CMD_AUXRD,
-				addr, buf, &size, &status)) {
-		printk(BIOS_ERR, "******AuxRead Error: 0x%04x: status 0x%08x\n",
-				addr, status);
-		return;
+	tegra_dp_hpd_config(dp, config);
+	if (tegra_dp_hpd_plug(dp, config->vdd_to_hpd_delay_ms) < 0) {
+		printk(BIOS_ERR, "dp: hpd plug failed\n");
+		goto error_enable;
 	}
-	printk(BIOS_DEBUG, "%s: addr: 0x%04x, size: %d\n", __func__,
-			addr, size);
-	for (i = 0; i < size; ++i)
-		printk(BIOS_DEBUG, " %02x", buf[i]);
 
-	printk(BIOS_DEBUG, "\n");
-}
+	if (tegra_dc_dp_init_link_cfg(config, dp, &dp->link_cfg)) {
+		printk(BIOS_ERR, "dp: failed to init link configuration\n");
+		goto error_enable;
+        }
 
-int dpaux_read(u32 addr, u32 size, u8 * data)
-{
+	tegra_dc_sor_enable_dp(&dp->sor);
 
-	struct tegra_dc_dp_data *dp = &dp_data;
-	u32 status = 0;
+	tegra_dc_sor_set_panel_power(&dp->sor, 1);
 
-	if ((size == 0) || (size > 16)) {
-		printk(BIOS_ERR, "dp: %s: invalid size %d\n", __func__, size);
-		return -1;
+	/* Write power on to DPCD */
+	data = NV_DPCD_SET_POWER_VAL_D0_NORMAL;
+	retry = 0;
+	do {
+		ret = tegra_dc_dp_dpcd_write(dp,
+			NV_DPCD_SET_POWER, data);
+	} while ((retry++ < DP_POWER_ON_MAX_TRIES) && ret);
+
+	if (ret || retry >= DP_POWER_ON_MAX_TRIES) {
+		printk(BIOS_ERR,
+			"dp: failed to power on panel (0x%x)\n", ret);
+		goto error_enable;
 	}
 
-	if (tegra_dc_dpaux_read(dp, DPAUX_DP_AUXCTL_CMD_AUXRD,
-					addr, data, &size, &status)) {
-		printk(BIOS_ERR, "dp: Failed to read reg %#x, status: %#x\n",
-				addr, status);
-		return -1;
+	/* Confirm DP is plugging status */
+	if (!(tegra_dpaux_readl(dp, DPAUX_DP_AUXSTAT) &
+			DPAUX_DP_AUXSTAT_HPD_STATUS_PLUGGED)) {
+		printk(BIOS_ERR, "dp: could not detect HPD\n");
+		goto error_enable;
 	}
 
-	return 0;
-}
+	/* Check DP version */
+	if (tegra_dc_dp_dpcd_read(dp, NV_DPCD_REV, &dp->revision))
+		printk(BIOS_ERR,
+			"dp: failed to read the revision number from sink\n");
 
-int dpaux_write(u32 addr, u32 size, u32 data)
-{
-	struct tegra_dc_dp_data *dp = &dp_data;
-	u32 status = 0;
-	int ret;
+	tegra_dc_sor_set_power_state(&dp->sor, 1);
+	tegra_dc_sor_attach(&dp->sor);
 
-	ret = tegra_dc_dpaux_write(dp, DPAUX_DP_AUXCTL_CMD_AUXWR,
-							   addr, (u8 *) & data, &size, &status);
-	if (ret)
-		printk(BIOS_ERR, "dp: Failed to write to reg %#x, status: 0x%x\n",
-			   addr, status);
-	return ret;
+	/*
+	 * Power down the unused lanes to save power
+	 * (about hundreds milli-watts, varies from boards).
+	 */
+	tegra_dc_sor_power_down_unused_lanes(&dp->sor);
+
+	dp->enabled = 1;
+error_enable:
+	return;
 }
diff --git a/src/soc/nvidia/tegra124/include/soc/display.h b/src/soc/nvidia/tegra124/include/soc/display.h
index 36348ff0c3..980d2b38db 100644
--- a/src/soc/nvidia/tegra124/include/soc/display.h
+++ b/src/soc/nvidia/tegra124/include/soc/display.h
@@ -17,185 +17,10 @@
 #ifndef __SOC_NVIDIA_TEGRA124_INCLUDE_SOC_DISPLAY_H__
 #define __SOC_NVIDIA_TEGRA124_INCLUDE_SOC_DISPLAY_H__
 
-/* ardisplay.h */
-#define DC_CMD_DISPLAY_WINDOW_HEADER_0		0x42
-#define DC_COM_CRC_CONTROL_0			0x300
-#define DC_COM_CRC_CHECKSUM_0			0x301
-#define DC_COM_PIN_OUTPUT_ENABLE0_0		0x302
-#define DC_COM_PIN_OUTPUT_ENABLE1_0		0x303
-#define DC_COM_PIN_OUTPUT_ENABLE2_0		0x304
-#define DC_COM_PIN_OUTPUT_ENABLE3_0		0x305
-#define DC_CMD_STATE_ACCESS_0			0x40
-#define DC_DISP_DISP_CLOCK_CONTROL_0		0x42e
-#define DC_DISP_DISP_TIMING_OPTIONS_0		0x405
-#define DC_DISP_REF_TO_SYNC_0			0x406
-#define DC_DISP_SYNC_WIDTH_0			0x407
-#define DC_DISP_BACK_PORCH_0			0x408
-#define DC_DISP_DISP_ACTIVE_0			0x409
-#define DC_DISP_FRONT_PORCH_0			0x40a
-#define DC_DISP_DISP_WIN_OPTIONS_0		0x402
-#define DC_DISP_DISP_WIN_OPTIONS_0_SOR_ENABLE_SHIFT	25
-#define DC_DISP_DISP_WIN_OPTIONS_0_SOR_ENABLE_FIELD	(0x1 << DC_DISP_DISP_WIN_OPTIONS_0_SOR_ENABLE_SHIFT)
-#define DC_DISP_DISP_SIGNAL_OPTIONS0_0		0x400
-#define DC_DISP_BLEND_BACKGROUND_COLOR_0	0x4e4
-#define DC_CMD_DISPLAY_COMMAND_0		0x32
-#define DC_CMD_STATE_CONTROL_0			0x41
-#define DC_CMD_DISPLAY_POWER_CONTROL_0		0x36
-
-/* ardisplay_a.h */
-#define DC_WIN_A_WIN_OPTIONS_0			0x700
-#define DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_SHIFT	30
-#define DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_FIELD	(0x1 << DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_SHIFT)
-#define DC_WIN_A_WIN_OPTIONS_0_A_WIN_ENABLE_ENABLE	(1)
-#define DC_WIN_A_BYTE_SWAP_0			0x701
-#define DC_WIN_A_BUFFER_CONTROL_0		0x702
-#define DC_WIN_A_COLOR_DEPTH_0			0x703
-#define DC_WIN_A_POSITION_0			0x704
-#define DC_WIN_A_SIZE_0				0x705
-#define DC_WIN_A_PRESCALED_SIZE_0		0x706
-#define DC_WIN_A_H_INITIAL_DDA_0		0x707
-#define DC_WIN_A_V_INITIAL_DDA_0		0x708
-#define DC_WIN_A_DDA_INCREMENT_0		0x709
-#define DC_WIN_A_LINE_STRIDE_0			0x70a
-#define DC_WIN_A_DV_CONTROL_0			0x70e
-#define DC_WIN_A_BLEND_LAYER_CONTROL_0		0x716
-#define DC_WIN_A_BLEND_MATCH_SELECT_0		0x717
-#define DC_WIN_A_BLEND_NOMATCH_SELECT_0		0x718
-#define DC_WIN_A_BLEND_ALPHA_1BIT_0		0x719
-#define DC_WINBUF_A_START_ADDR_LO_0		0x800
-#define DC_WINBUF_A_START_ADDR_HI_0		0x80d
-#define DC_WINBUF_A_ADDR_H_OFFSET_0		0x806
-#define DC_WINBUF_A_ADDR_V_OFFSET_0		0x808
-
-/* ardisplay_bd.h */
-#define DC_B_WIN_BD_SIZE_0			0xd85
-#define DC_B_WIN_BD_PRESCALED_SIZE_0		0xd86
-#define DC_B_WIN_BD_LINE_STRIDE_0		0xd8a
-#define DC_B_WIN_BD_COLOR_DEPTH_0		0xd83
-#define DC_B_WINBUF_BD_START_ADDR_0		0xdc0
-#define DC_B_WIN_BD_DDA_INCREMENT_0		0xd89
-#define DC_B_WIN_BD_WIN_OPTIONS_0		0xd80
-#define DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_SHIFT	30
-#define DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_FIELD	(0x1 << DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_SHIFT)
-#define DC_B_WIN_BD_WIN_OPTIONS_0_BD_WIN_ENABLE_ENABLE	(1)
-
-/* arsor.h */
-#define SOR_NV_PDISP_SOR_CLK_CNTRL_0		0x13
-#define SOR_NV_PDISP_SOR_DP_PADCTL0_0		0x5c
-#define SOR_NV_PDISP_SOR_PLL0_0			0x17
-#define SOR_NV_PDISP_SOR_PLL1_0			0x18
-#define SOR_NV_PDISP_SOR_PLL2_0			0x19
-#define SOR_NV_PDISP_SOR_PLL3_0			0x1a
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX6_SHIFT	22
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX6_FIELD	(0x1 << SOR_NV_PDISP_SOR_PLL2_0_AUX6_SHIFT)
-#define SOR_NV_PDISP_SOR_PLL0_0_PWR_SHIFT	0
-#define SOR_NV_PDISP_SOR_PLL0_0_PWR_FIELD	(0x1 << SOR_NV_PDISP_SOR_PLL0_0_PWR_SHIFT)
-#define SOR_NV_PDISP_SOR_PLL0_0_VCOPD_SHIFT	2
-#define SOR_NV_PDISP_SOR_PLL0_0_VCOPD_FIELD	(0x1 << SOR_NV_PDISP_SOR_PLL0_0_VCOPD_SHIFT)
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX8_SHIFT	24
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX8_FIELD	(0x1 << SOR_NV_PDISP_SOR_PLL2_0_AUX8_SHIFT)
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX7_SHIFT	23
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX7_FIELD	(0x1 << SOR_NV_PDISP_SOR_PLL2_0_AUX7_SHIFT)
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX9_SHIFT	25
-#define SOR_NV_PDISP_SOR_PLL2_0_AUX9_FIELD	(0x1 << SOR_NV_PDISP_SOR_PLL2_0_AUX9_SHIFT)
-#define SOR_NV_PDISP_SOR_LANE_DRIVE_CURRENT0_0	0x4e
-#define SOR_NV_PDISP_SOR_LANE_PREEMPHASIS0_0	0x52
-#define SOR_NV_PDISP_SOR_POSTCURSOR0_0		0x56
-#define SOR_NV_PDISP_SOR_DP_PADCTL0_0		0x5c
-#define SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_VALUE_SHIFT	8
-#define SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_VALUE_FIELD	(0xff << SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_VALUE_SHIFT)
-#define SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_SHIFT	22
-#define SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_FIELD	(0x1 << SOR_NV_PDISP_SOR_DP_PADCTL0_0_TX_PU_SHIFT)
-#define SOR_NV_PDISP_SOR_LVDS_0			0x1c
-#define SOR_NV_PDISP_SOR_CLK_CNTRL_0		0x13
-#define SOR_NV_PDISP_SOR_DP_LINKCTL0_0		0x4c
-#define SOR_NV_PDISP_SOR_LANE_SEQ_CTL_0		0x21
-#define SOR_NV_PDISP_SOR_DP_TPG_0		0x6d
-#define SOR_NV_PDISP_HEAD_STATE1_0		0x7
-#define SOR_NV_PDISP_HEAD_STATE2_0		0x9
-#define SOR_NV_PDISP_HEAD_STATE3_0		0xb
-#define SOR_NV_PDISP_HEAD_STATE4_0		0xd
-#define SOR_NV_PDISP_SOR_STATE1_0		0x4
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_HSYNCPOL_SHIFT	12
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_HSYNCPOL_FIELD	(0x1 << SOR_NV_PDISP_SOR_STATE1_0_ASY_HSYNCPOL_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_VSYNCPOL_SHIFT	13
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_VSYNCPOL_FIELD	(0x1 << SOR_NV_PDISP_SOR_STATE1_0_ASY_VSYNCPOL_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_SHIFT	8
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_FIELD	(0xf << SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_LVDS_CUSTOM	(0)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_DP_A		(8)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_DP_B		(9)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PROTOCOL_CUSTOM		(15)
-
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_ACTIVE_RASTER	(0)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_COMPLETE_RASTER	(1)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_NON_ACTIVE_RASTER	(2)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_SHIFT		6
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_FIELD		(0x3 << SOR_NV_PDISP_SOR_STATE1_0_ASY_CRCMODE_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_SHIFT		4
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_FIELD		(0x3 << SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_NONE		(0)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_SUBHEAD0		(1)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_SUBHEAD1		(2)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_SUBOWNER_BOTH		(3)
-
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_SHIFT		0
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_FIELD		(0xf << SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_NONE		(0)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_HEAD0		(1)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_OWNER_HEAD1		(2)
-
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0				0x58
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_POLARITY_SHIFT	24
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_POLARITY_FIELD	(0x1 << SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_POLARITY_SHIFT)
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_FRAC_SHIFT	16
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_FRAC_FIELD	(0xf << SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_FRAC_SHIFT)
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_COUNT_SHIFT	8
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_COUNT_FIELD	(0x7f << SOR_NV_PDISP_SOR_DP_CONFIG0_0_ACTIVESYM_COUNT_SHIFT)
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_WATERMARK_SHIFT		0
-#define SOR_NV_PDISP_SOR_DP_CONFIG0_0_WATERMARK_FIELD		(0x3f << SOR_NV_PDISP_SOR_DP_CONFIG0_0_WATERMARK_SHIFT)
-#define SOR_NV_PDISP_SOR_DP_LINKCTL0_0_TUSIZE_SHIFT		2
-#define SOR_NV_PDISP_SOR_DP_LINKCTL0_0_TUSIZE_FIELD		(0x7f << SOR_NV_PDISP_SOR_DP_LINKCTL0_0_TUSIZE_SHIFT)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_SHIFT		17
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_FIELD		(0xf << SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_SHIFT)
-
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_DEFAULTVAL	(0)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_16_422	(1)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_18_444	(2)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_20_422	(3)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_24_422	(4)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_24_444	(5)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_30_444	(6)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_32_422	(7)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_36_444	(8)
-#define SOR_NV_PDISP_SOR_STATE1_0_ASY_PIXELDEPTH_BPP_48_444	(9)
-
-#define SOR_NV_PDISP_SOR_CRC_CNTRL_0				0x11
-#define SOR_NV_PDISP_SOR_DP_AUDIO_VBLANK_SYMBOLS_0		0x64
-#define SOR_NV_PDISP_SOR_DP_SPARE0_0				0x60
-#define SOR_NV_PDISP_SOR_PWR_0					0x15
-#define SOR_NV_PDISP_SOR_STATE0_0				0x3
-#define SOR_NV_PDISP_SOR_SUPER_STATE1_0				0x2
-#define SOR_NV_PDISP_SOR_SUPER_STATE0_0				0x1
-
-/* ardpaux.h */
-#define DPAUX_DP_AUXDATA_READ_W0				0x19
-
-#define DP_LVDS_SHIFT	25
-#define DP_LVDS		(1 << DP_LVDS_SHIFT)
-
-#define SRC_BPP		16
-#define COLORDEPTH	0x6
 #define COLOR_WHITE	0xFFFFFF
 
 struct soc_nvidia_tegra124_config;	/* forward declaration */
 void setup_display(struct soc_nvidia_tegra124_config *config);
-void init_dca_regs(void);
-void dp_io_powerup(void);
-u32 dp_setup_timing(u32 width, u32 height);
-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);
 
 #define FB_SIZE_MB (32)
 
diff --git a/src/soc/nvidia/tegra124/soc.c b/src/soc/nvidia/tegra124/soc.c
index bc47954790..08a4cf8e13 100644
--- a/src/soc/nvidia/tegra124/soc.c
+++ b/src/soc/nvidia/tegra124/soc.c
@@ -21,8 +21,10 @@
 
 #include <console/console.h>
 #include <device/device.h>
+#include <arch/io.h>
 #include <soc/nvidia/tegra/dc.h>
 #include <soc/nvidia/tegra124/sdram.h>
+#include "chip.h"
 #include <soc/display.h>
 
 /* this sucks, but for now, fb size/location are hardcoded.
diff --git a/src/soc/nvidia/tegra124/sor.c b/src/soc/nvidia/tegra124/sor.c
index 6a47833bef..3ff39ed0f2 100644
--- a/src/soc/nvidia/tegra124/sor.c
+++ b/src/soc/nvidia/tegra124/sor.c
@@ -54,17 +54,18 @@
 #define APBDEV_PMC_IO_DPD2_STATUS_LVDS_OFF		(0 << 25)
 #define APBDEV_PMC_IO_DPD2_STATUS_LVDS_ON		(1 << 25)
 
-
 static inline u32 tegra_sor_readl(struct tegra_dc_sor_data *sor, u32 reg)
 {
-	u32 reg_val = readl((sor->base + reg * 4));
+	void *addr = sor->base + (u32) (reg << 2);
+	u32 reg_val = READL(addr);
 	return reg_val;
 }
 
 static inline void tegra_sor_writel(struct tegra_dc_sor_data *sor,
 	u32 reg, u32 val)
 {
-	writel(val, (sor->base + reg * 4));
+	void *addr = sor->base + (u32) (reg << 2);
+	WRITEL(val, addr);
 }
 
 static inline void tegra_sor_write_field(struct tegra_dc_sor_data *sor,
@@ -76,8 +77,61 @@ static inline void tegra_sor_write_field(struct tegra_dc_sor_data *sor,
 	tegra_sor_writel(sor, reg, reg_val);
 }
 
+static u32 tegra_dc_sor_poll_register(struct tegra_dc_sor_data *sor,
+	u32 reg, u32 mask, u32 exp_val, u32 poll_interval_us, u32 timeout_us)
+{
+	u32 temp = timeout_us;
+	u32 reg_val = 0;
+
+	do {
+		udelay(poll_interval_us);
+		reg_val = tegra_sor_readl(sor, reg);
+		if (timeout_us > poll_interval_us)
+			timeout_us -= poll_interval_us;
+		else
+			break;
+	} while ((reg_val & mask) != exp_val);
+
+	if ((reg_val & mask) == exp_val)
+		return 0;	/* success */
+	printk(BIOS_ERR,
+		"sor_poll_register 0x%x: timeout, "
+		"(reg_val)0x%08x & (mask)0x%08x != (exp_val)0x%08x\n",
+		reg, reg_val, mask, exp_val);
+
+	return temp;
+}
+
+int tegra_dc_sor_set_power_state(struct tegra_dc_sor_data *sor, int pu_pd)
+{
+	u32 reg_val;
+	u32 orig_val;
+
+	orig_val = tegra_sor_readl(sor, NV_SOR_PWR);
+
+	reg_val = pu_pd ? NV_SOR_PWR_NORMAL_STATE_PU :
+		NV_SOR_PWR_NORMAL_STATE_PD; /* normal state only */
+
+	if (reg_val == orig_val)
+		return 0;	/* No update needed */
+
+	reg_val |= NV_SOR_PWR_SETTING_NEW_TRIGGER;
+	tegra_sor_writel(sor, NV_SOR_PWR, reg_val);
+
+	/* Poll to confirm it is done */
+	if (tegra_dc_sor_poll_register(sor, NV_SOR_PWR,
+			NV_SOR_PWR_SETTING_NEW_DEFAULT_MASK,
+			NV_SOR_PWR_SETTING_NEW_DONE,
+			100, TEGRA_SOR_TIMEOUT_MS * 1000)) {
+		printk(BIOS_ERR,
+			"dc timeout waiting for SOR_PWR = NEW_DONE\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
 void tegra_dc_sor_set_dp_linkctl(struct tegra_dc_sor_data *sor, int ena,
-	u8 training_pattern, const struct tegra_dc_dp_link_config *cfg)
+	u8 training_pattern, const struct tegra_dc_dp_link_config *link_cfg)
 {
 	u32 reg_val;
 
@@ -89,20 +143,20 @@ void tegra_dc_sor_set_dp_linkctl(struct tegra_dc_sor_data *sor, int ena,
 		reg_val &= NV_SOR_DP_LINKCTL_ENABLE_NO;
 
 	reg_val &= ~NV_SOR_DP_LINKCTL_TUSIZE_MASK;
-	reg_val |= (cfg->tu_size << NV_SOR_DP_LINKCTL_TUSIZE_SHIFT);
+	reg_val |= (link_cfg->tu_size << NV_SOR_DP_LINKCTL_TUSIZE_SHIFT);
 
-	if (cfg->enhanced_framing)
+	if (link_cfg->enhanced_framing)
 		reg_val |= NV_SOR_DP_LINKCTL_ENHANCEDFRAME_ENABLE;
 
 	tegra_sor_writel(sor, NV_SOR_DP_LINKCTL(sor->portnum), reg_val);
 
 	switch (training_pattern) {
-	case trainingPattern_1:
+	case training_pattern_1:
 		tegra_sor_writel(sor, NV_SOR_DP_TPG, 0x41414141);
 		break;
-	case trainingPattern_2:
-	case trainingPattern_3:
-		reg_val = (cfg->link_bw == NV_SOR_LINK_SPEED_G5_4) ?
+	case training_pattern_2:
+	case training_pattern_3:
+		reg_val = (link_cfg->link_bw == SOR_LINK_SPEED_G5_4) ?
 			0x43434343 : 0x42424242;
 		tegra_sor_writel(sor, NV_SOR_DP_TPG, reg_val);
 		break;
@@ -112,58 +166,612 @@ void tegra_dc_sor_set_dp_linkctl(struct tegra_dc_sor_data *sor, int ena,
 	}
 }
 
-void tegra_dc_sor_set_dp_lanedata(struct tegra_dc_sor_data *sor,
-	u32 lane, u32 pre_emphasis, u32 drive_current, u32 tx_pu)
+static int tegra_dc_sor_enable_lane_sequencer(struct tegra_dc_sor_data *sor,
+	int pu, int is_lvds)
+{
+	u32 reg_val;
+
+	/* SOR lane sequencer */
+	if (pu)
+		reg_val = NV_SOR_LANE_SEQ_CTL_SETTING_NEW_TRIGGER |
+			NV_SOR_LANE_SEQ_CTL_SEQUENCE_DOWN |
+			NV_SOR_LANE_SEQ_CTL_NEW_POWER_STATE_PU;
+	else
+		reg_val = NV_SOR_LANE_SEQ_CTL_SETTING_NEW_TRIGGER |
+			NV_SOR_LANE_SEQ_CTL_SEQUENCE_UP |
+			NV_SOR_LANE_SEQ_CTL_NEW_POWER_STATE_PD;
+
+	if (is_lvds)
+		reg_val |= 15 << NV_SOR_LANE_SEQ_CTL_DELAY_SHIFT;
+	else
+		reg_val |= 1 << NV_SOR_LANE_SEQ_CTL_DELAY_SHIFT;
+
+	tegra_sor_writel(sor, NV_SOR_LANE_SEQ_CTL, reg_val);
+
+	if (tegra_dc_sor_poll_register(sor, NV_SOR_LANE_SEQ_CTL,
+			NV_SOR_LANE_SEQ_CTL_SETTING_MASK,
+			NV_SOR_LANE_SEQ_CTL_SETTING_NEW_DONE,
+			100, TEGRA_SOR_TIMEOUT_MS*1000)) {
+		printk(BIOS_ERR,
+			"dp: timeout while waiting for SOR lane sequencer "
+			"to power down langes\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int tegra_dc_sor_power_dplanes(struct tegra_dc_sor_data *sor,
+	u32 lane_count, int pu)
+{
+	u32 reg_val;
+
+	reg_val = tegra_sor_readl(sor, NV_SOR_DP_PADCTL(sor->portnum));
+
+	if (pu) {
+		switch (lane_count) {
+		case 4:
+			reg_val |= (NV_SOR_DP_PADCTL_PD_TXD_3_NO |
+				NV_SOR_DP_PADCTL_PD_TXD_2_NO);
+			/* fall through */
+		case 2:
+			reg_val |= NV_SOR_DP_PADCTL_PD_TXD_1_NO;
+		case 1:
+			reg_val |= NV_SOR_DP_PADCTL_PD_TXD_0_NO;
+			break;
+		default:
+			printk(BIOS_ERR,
+				"dp: invalid lane number %d\n", lane_count);
+			return -1;
+		}
+
+		tegra_sor_writel(sor, NV_SOR_DP_PADCTL(sor->portnum), reg_val);
+		tegra_dc_sor_set_lane_count(sor, lane_count);
+	}
+	return tegra_dc_sor_enable_lane_sequencer(sor, pu, 0);
+}
+
+void tegra_dc_sor_set_panel_power(struct tegra_dc_sor_data *sor,
+	int power_up)
+{
+	u32 reg_val;
+
+	/* !!TODO: need to enable panel power through GPIO operations */
+	/* Check bug 790854 for HW progress */
+
+	reg_val = tegra_sor_readl(sor, NV_SOR_DP_PADCTL(sor->portnum));
+
+	if (power_up)
+		reg_val |= NV_SOR_DP_PADCTL_PAD_CAL_PD_POWERUP;
+	else
+		reg_val &= ~NV_SOR_DP_PADCTL_PAD_CAL_PD_POWERUP;
+
+	tegra_sor_writel(sor, NV_SOR_DP_PADCTL(sor->portnum), reg_val);
+}
+
+static void tegra_dc_sor_set_dp_mode(struct tegra_dc_sor_data *sor,
+	const struct tegra_dc_dp_link_config *link_cfg)
+{
+	u32 reg_val;
+
+	tegra_dc_sor_set_link_bandwidth(sor, link_cfg->link_bw);
+
+	tegra_dc_sor_set_dp_linkctl(sor, 1, training_pattern_none, link_cfg);
+	reg_val = tegra_sor_readl(sor, NV_SOR_DP_CONFIG(sor->portnum));
+	reg_val &= ~NV_SOR_DP_CONFIG_WATERMARK_MASK;
+	reg_val |= link_cfg->watermark;
+	reg_val &= ~NV_SOR_DP_CONFIG_ACTIVESYM_COUNT_MASK;
+	reg_val |= (link_cfg->active_count <<
+		NV_SOR_DP_CONFIG_ACTIVESYM_COUNT_SHIFT);
+	reg_val &= ~NV_SOR_DP_CONFIG_ACTIVESYM_FRAC_MASK;
+	reg_val |= (link_cfg->active_frac <<
+		NV_SOR_DP_CONFIG_ACTIVESYM_FRAC_SHIFT);
+	if (link_cfg->activepolarity)
+		reg_val |= NV_SOR_DP_CONFIG_ACTIVESYM_POLARITY_POSITIVE;
+	else
+		reg_val &= ~NV_SOR_DP_CONFIG_ACTIVESYM_POLARITY_POSITIVE;
+	reg_val |= (NV_SOR_DP_CONFIG_ACTIVESYM_CNTL_ENABLE |
+		NV_SOR_DP_CONFIG_RD_RESET_VAL_NEGATIVE);
+
+	tegra_sor_writel(sor, NV_SOR_DP_CONFIG(sor->portnum), reg_val);
+
+	/* enable CRC */
+	reg_val = NV_SOR_CRC_CNTRL_ARM_CRC_ENABLE_EN <<
+		NV_SOR_CRC_CNTRL_ARM_CRC_ENABLE_SHIFT;
+	tegra_sor_writel(sor, NV_SOR_CRC_CNTRL, reg_val);
+
+	/* program h/vblank sym */
+	tegra_sor_write_field(sor, NV_SOR_DP_AUDIO_HBLANK_SYMBOLS,
+		NV_SOR_DP_AUDIO_HBLANK_SYMBOLS_MASK, link_cfg->hblank_sym);
+
+	tegra_sor_write_field(sor, NV_SOR_DP_AUDIO_VBLANK_SYMBOLS,
+		NV_SOR_DP_AUDIO_VBLANK_SYMBOLS_MASK, link_cfg->vblank_sym);
+}
+
+static inline void tegra_dc_sor_super_update(struct tegra_dc_sor_data *sor)
+{
+	tegra_sor_writel(sor, NV_SOR_SUPER_STATE0, 0);
+	tegra_sor_writel(sor, NV_SOR_SUPER_STATE0, 1);
+	tegra_sor_writel(sor, NV_SOR_SUPER_STATE0, 0);
+}
+
+static inline void tegra_dc_sor_update(struct tegra_dc_sor_data *sor)
+{
+	tegra_sor_writel(sor, NV_SOR_STATE0, 0);
+	tegra_sor_writel(sor, NV_SOR_STATE0, 1);
+	tegra_sor_writel(sor, NV_SOR_STATE0, 0);
+}
+
+static void tegra_dc_sor_io_set_dpd(struct tegra_dc_sor_data *sor, int up)
+{
+	u32 reg_val;
+	void *pmc_base = sor->pmc_base;
+
+	if (up) {
+		WRITEL(APBDEV_PMC_DPD_SAMPLE_ON_ENABLE,
+			pmc_base + APBDEV_PMC_DPD_SAMPLE);
+		WRITEL(10, pmc_base + APBDEV_PMC_SEL_DPD_TIM);
+	}
+
+	reg_val = READL(pmc_base + APBDEV_PMC_IO_DPD2_REQ);
+	reg_val &= ~(APBDEV_PMC_IO_DPD2_REQ_LVDS_ON ||
+		APBDEV_PMC_IO_DPD2_REQ_CODE_DEFAULT_MASK);
+
+	reg_val = up ? APBDEV_PMC_IO_DPD2_REQ_LVDS_ON |
+		APBDEV_PMC_IO_DPD2_REQ_CODE_DPD_OFF :
+		APBDEV_PMC_IO_DPD2_REQ_LVDS_OFF |
+		APBDEV_PMC_IO_DPD2_REQ_CODE_DPD_ON;
+
+	WRITEL(reg_val, pmc_base + APBDEV_PMC_IO_DPD2_REQ);
+
+	/* Polling */
+	u32 temp = 10*1000;
+	do {
+		udelay(20);
+		reg_val = READL(pmc_base + APBDEV_PMC_IO_DPD2_STATUS);
+		if (temp > 20)
+			temp -= 20;
+		else
+			break;
+	} while ((reg_val & APBDEV_PMC_IO_DPD2_STATUS_LVDS_ON) != 0);
+
+	if ((reg_val & APBDEV_PMC_IO_DPD2_STATUS_LVDS_ON) != 0)
+		printk(BIOS_ERR,
+			"PMC_IO_DPD2 polling failed (0x%x)\n", reg_val);
+
+	if (up)
+		WRITEL(APBDEV_PMC_DPD_SAMPLE_ON_DISABLE,
+			pmc_base + APBDEV_PMC_DPD_SAMPLE);
+}
+
+void tegra_dc_sor_set_internal_panel(struct tegra_dc_sor_data *sor, int is_int)
 {
-	u32 d_cur;
-	u32 p_emp;
+	u32 reg_val;
 
+	reg_val = tegra_sor_readl(sor, NV_SOR_DP_SPARE(sor->portnum));
+	if (is_int)
+		reg_val |= NV_SOR_DP_SPARE_PANEL_INTERNAL;
+	else
+		reg_val &= ~NV_SOR_DP_SPARE_PANEL_INTERNAL;
+
+	reg_val |= NV_SOR_DP_SPARE_SOR_CLK_SEL_MACRO_SORCLK |
+		NV_SOR_DP_SPARE_SEQ_ENABLE_YES;
+	tegra_sor_writel(sor, NV_SOR_DP_SPARE(sor->portnum), reg_val);
+}
+
+void tegra_dc_sor_read_link_config(struct tegra_dc_sor_data *sor, u8 *link_bw,
+				   u8 *lane_count)
+{
+	u32 reg_val;
+
+	reg_val = tegra_sor_readl(sor, NV_SOR_CLK_CNTRL);
+	*link_bw = (reg_val & NV_SOR_CLK_CNTRL_DP_LINK_SPEED_MASK)
+		>> NV_SOR_CLK_CNTRL_DP_LINK_SPEED_SHIFT;
+	reg_val = tegra_sor_readl(sor,
+		NV_SOR_DP_LINKCTL(sor->portnum));
+
+	switch (reg_val & NV_SOR_DP_LINKCTL_LANECOUNT_MASK) {
+	case NV_SOR_DP_LINKCTL_LANECOUNT_ZERO:
+		*lane_count = 0;
+		break;
+	case NV_SOR_DP_LINKCTL_LANECOUNT_ONE:
+		*lane_count = 1;
+		break;
+	case NV_SOR_DP_LINKCTL_LANECOUNT_TWO:
+		*lane_count = 2;
+		break;
+	case NV_SOR_DP_LINKCTL_LANECOUNT_FOUR:
+		*lane_count = 4;
+		break;
+	default:
+		printk(BIOS_ERR, "Unknown lane count\n");
+	}
+}
+
+void tegra_dc_sor_set_link_bandwidth(struct tegra_dc_sor_data *sor, u8 link_bw)
+{
+	tegra_sor_write_field(sor, NV_SOR_CLK_CNTRL,
+		NV_SOR_CLK_CNTRL_DP_LINK_SPEED_MASK,
+		link_bw << NV_SOR_CLK_CNTRL_DP_LINK_SPEED_SHIFT);
+}
 
-	d_cur = tegra_sor_readl(sor, NV_SOR_DC(sor->portnum));
-	p_emp = tegra_sor_readl(sor, NV_SOR_PR(sor->portnum));
+void tegra_dc_sor_set_lane_count(struct tegra_dc_sor_data *sor, u8 lane_count)
+{
+	u32 reg_val;
 
-	switch (lane) {
+	reg_val = tegra_sor_readl(sor, NV_SOR_DP_LINKCTL(sor->portnum));
+	reg_val &= ~NV_SOR_DP_LINKCTL_LANECOUNT_MASK;
+	switch (lane_count) {
 	case 0:
-		p_emp &= ~NV_SOR_PR_LANE2_DP_LANE0_MASK;
-		p_emp |= (pre_emphasis <<
-			NV_SOR_PR_LANE2_DP_LANE0_SHIFT);
-		d_cur &= ~NV_SOR_DC_LANE2_DP_LANE0_MASK;
-		d_cur |= (drive_current <<
-			NV_SOR_DC_LANE2_DP_LANE0_SHIFT);
 		break;
 	case 1:
-		p_emp &= ~NV_SOR_PR_LANE1_DP_LANE1_MASK;
-		p_emp |= (pre_emphasis <<
-			NV_SOR_PR_LANE1_DP_LANE1_SHIFT);
-		d_cur &= ~NV_SOR_DC_LANE1_DP_LANE1_MASK;
-		d_cur |= (drive_current <<
-			NV_SOR_DC_LANE1_DP_LANE1_SHIFT);
+		reg_val |= NV_SOR_DP_LINKCTL_LANECOUNT_ONE;
 		break;
 	case 2:
-		p_emp &= ~NV_SOR_PR_LANE0_DP_LANE2_MASK;
-		p_emp |= (pre_emphasis <<
-			NV_SOR_PR_LANE0_DP_LANE2_SHIFT);
-		d_cur &= ~NV_SOR_DC_LANE0_DP_LANE2_MASK;
-		d_cur |= (drive_current <<
-			NV_SOR_DC_LANE0_DP_LANE2_SHIFT);
+		reg_val |= NV_SOR_DP_LINKCTL_LANECOUNT_TWO;
 		break;
-	case 3:
-		p_emp &= ~NV_SOR_PR_LANE3_DP_LANE3_MASK;
-		p_emp |= (pre_emphasis <<
-			NV_SOR_PR_LANE3_DP_LANE3_SHIFT);
-		d_cur &= ~NV_SOR_DC_LANE3_DP_LANE3_MASK;
-		d_cur |= (drive_current <<
-			NV_SOR_DC_LANE3_DP_LANE3_SHIFT);
+	case 4:
+		reg_val |= NV_SOR_DP_LINKCTL_LANECOUNT_FOUR;
 		break;
 	default:
-		printk(BIOS_SPEW, "dp: sor lane count %d is invalid\n", lane);
+		/* 0 should be handled earlier. */
+		printk(BIOS_ERR, "dp: Invalid lane count %d\n",
+			lane_count);
+		return;
+	}
+	tegra_sor_writel(sor, NV_SOR_DP_LINKCTL(sor->portnum), reg_val);
+}
+
+static void tegra_sor_enable_edp_clock(struct tegra_dc_sor_data *sor)
+{
+	sor_clock_start();
+}
+
+/* The SOR power sequencer does not work for t124 so SW has to
+   go through the power sequence manually */
+/* Power up steps from spec: */
+/* STEP	PDPORT	PDPLL	PDBG	PLLVCOD	PLLCAPD	E_DPD	PDCAL */
+/* 1	1	1	1	1	1	1	1 */
+/* 2	1	1	1	1	1	0	1 */
+/* 3	1	1	0	1	1	0	1 */
+/* 4	1	0	0	0	0	0	1 */
+/* 5	0	0	0	0	0	0	1 */
+static void tegra_dc_sor_power_up(struct tegra_dc_sor_data *sor,
+				int is_lvds)
+{
+	if (sor->power_is_up)
+		return;
+
+	/* Set link bw */
+	tegra_dc_sor_set_link_bandwidth(sor,
+		is_lvds ? NV_SOR_CLK_CNTRL_DP_LINK_SPEED_LVDS :
+		NV_SOR_CLK_CNTRL_DP_LINK_SPEED_G1_62);
+
+	/* step 1 */
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX7_PORT_POWERDOWN_MASK | /* PDPORT */
+		NV_SOR_PLL2_AUX6_BANDGAP_POWERDOWN_MASK | /* PDBG */
+		NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_MASK, /* PLLCAPD */
+		NV_SOR_PLL2_AUX7_PORT_POWERDOWN_ENABLE |
+		NV_SOR_PLL2_AUX6_BANDGAP_POWERDOWN_ENABLE |
+		NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_ENABLE);
+	tegra_sor_write_field(sor, NV_SOR_PLL0,
+		NV_SOR_PLL0_PWR_MASK | /* PDPLL */
+		NV_SOR_PLL0_VCOPD_MASK, /* PLLVCOPD */
+		NV_SOR_PLL0_PWR_OFF |
+		NV_SOR_PLL0_VCOPD_ASSERT);
+	tegra_sor_write_field(sor, NV_SOR_DP_PADCTL(sor->portnum),
+		NV_SOR_DP_PADCTL_PAD_CAL_PD_POWERDOWN, /* PDCAL */
+		NV_SOR_DP_PADCTL_PAD_CAL_PD_POWERDOWN);
+
+	/* step 2 */
+	tegra_dc_sor_io_set_dpd(sor, 1);
+	udelay(15);
+
+	/* step 3 */
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX6_BANDGAP_POWERDOWN_MASK,
+		NV_SOR_PLL2_AUX6_BANDGAP_POWERDOWN_DISABLE);
+	udelay(25);
+
+	/* step 4 */
+	tegra_sor_write_field(sor, NV_SOR_PLL0,
+		NV_SOR_PLL0_PWR_MASK | /* PDPLL */
+		NV_SOR_PLL0_VCOPD_MASK, /* PLLVCOPD */
+		NV_SOR_PLL0_PWR_ON | NV_SOR_PLL0_VCOPD_RESCIND);
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_MASK, /* PLLCAPD */
+		NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_DISABLE);
+	udelay(225);
+
+	/* step 5 */
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX7_PORT_POWERDOWN_MASK, /* PDPORT */
+		NV_SOR_PLL2_AUX7_PORT_POWERDOWN_DISABLE);
+
+	sor->power_is_up = 1;
+}
+
+static void tegra_dc_sor_config_panel(struct tegra_dc_sor_data *sor,
+	int is_lvds)
+{
+	const struct tegra_dc 			*dc = sor->dc;
+	const struct tegra_dc_dp_data 		*dp = dc->out;
+	const struct tegra_dc_dp_link_config *link_cfg = &dp->link_cfg;
+	const struct soc_nvidia_tegra124_config *config = dc->config;
+
+	const int	head_num = 0; // based on kernel dc driver
+	u32		reg_val	 = NV_SOR_STATE1_ASY_OWNER_HEAD0 << head_num;
+	u32		vtotal, htotal;
+	u32		vsync_end, hsync_end;
+	u32		vblank_end, hblank_end;
+	u32		vblank_start, hblank_start;
+
+	reg_val |= is_lvds ? NV_SOR_STATE1_ASY_PROTOCOL_LVDS_CUSTOM :
+		NV_SOR_STATE1_ASY_PROTOCOL_DP_A;
+	reg_val |= NV_SOR_STATE1_ASY_SUBOWNER_NONE |
+		NV_SOR_STATE1_ASY_CRCMODE_COMPLETE_RASTER;
+
+	reg_val |= NV_SOR_STATE1_ASY_HSYNCPOL_NEGATIVE_TRUE;
+	reg_val |= NV_SOR_STATE1_ASY_VSYNCPOL_NEGATIVE_TRUE;
+	reg_val |= (link_cfg->bits_per_pixel > 18) ?
+		NV_SOR_STATE1_ASY_PIXELDEPTH_BPP_24_444 :
+		NV_SOR_STATE1_ASY_PIXELDEPTH_BPP_18_444;
+
+	tegra_sor_writel(sor, NV_SOR_STATE1, reg_val);
+
+	/* Skipping programming NV_HEAD_STATE0, assuming:
+	   interlacing: PROGRESSIVE, dynamic range: VESA, colorspace: RGB */
+
+	vtotal = config->vsync_width + config->vback_porch +
+		config->yres + config->vfront_porch;
+	htotal = config->hsync_width + config->hback_porch +
+		config->xres + config->hfront_porch;
+
+	tegra_sor_writel(sor, NV_HEAD_STATE1(head_num),
+		vtotal << NV_HEAD_STATE1_VTOTAL_SHIFT |
+		htotal << NV_HEAD_STATE1_HTOTAL_SHIFT);
+
+	vsync_end = config->vsync_width - 1;
+	hsync_end = config->hsync_width - 1;
+	tegra_sor_writel(sor, NV_HEAD_STATE2(head_num),
+		vsync_end << NV_HEAD_STATE2_VSYNC_END_SHIFT |
+		hsync_end << NV_HEAD_STATE2_HSYNC_END_SHIFT);
+
+	vblank_end = vsync_end + config->vback_porch;
+	hblank_end = hsync_end + config->hback_porch;
+	tegra_sor_writel(sor, NV_HEAD_STATE3(head_num),
+		vblank_end << NV_HEAD_STATE3_VBLANK_END_SHIFT |
+		hblank_end << NV_HEAD_STATE3_HBLANK_END_SHIFT);
+
+	vblank_start = vblank_end + config->yres;
+	hblank_start = hblank_end + config->xres;
+	tegra_sor_writel(sor, NV_HEAD_STATE4(head_num),
+		vblank_start << NV_HEAD_STATE4_VBLANK_START_SHIFT |
+		hblank_start << NV_HEAD_STATE4_HBLANK_START_SHIFT);
+
+	/* TODO: adding interlace mode support */
+	tegra_sor_writel(sor, NV_HEAD_STATE5(head_num), 0x1);
+
+	tegra_sor_write_field(sor, NV_SOR_CSTM,
+		NV_SOR_CSTM_ROTCLK_DEFAULT_MASK |
+		NV_SOR_CSTM_LVDS_EN_ENABLE,
+		2 << NV_SOR_CSTM_ROTCLK_SHIFT |
+		is_lvds ? NV_SOR_CSTM_LVDS_EN_ENABLE :
+		NV_SOR_CSTM_LVDS_EN_DISABLE);
+}
+
+static void tegra_dc_sor_enable_dc(struct tegra_dc_sor_data *sor)
+{
+	struct tegra_dc		*dc   = sor->dc;
+	struct display_controller *disp_ctrl = (void *)dc->base;
+
+	u32	reg_val = READL(&disp_ctrl->cmd.state_access);
+
+	WRITEL(reg_val | WRITE_MUX_ACTIVE, &disp_ctrl->cmd.state_access);
+	WRITEL(VSYNC_H_POSITION(1), &disp_ctrl->disp.disp_timing_opt);
+
+	/* Enable DC */
+	WRITEL(DISP_CTRL_MODE_C_DISPLAY, &disp_ctrl->cmd.disp_cmd);
+	WRITEL(reg_val, &disp_ctrl->cmd.state_access);
+}
+
+void tegra_dc_sor_enable_dp(struct tegra_dc_sor_data *sor)
+{
+	const struct tegra_dc_dp_link_config *link_cfg = sor->link_cfg;
+
+	tegra_sor_write_field(sor, NV_SOR_CLK_CNTRL,
+		NV_SOR_CLK_CNTRL_DP_CLK_SEL_MASK,
+		NV_SOR_CLK_CNTRL_DP_CLK_SEL_SINGLE_DPCLK);
+
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX6_BANDGAP_POWERDOWN_MASK,
+		NV_SOR_PLL2_AUX6_BANDGAP_POWERDOWN_DISABLE);
+	udelay(25);
+
+	tegra_sor_write_field(sor, NV_SOR_PLL3,
+		NV_SOR_PLL3_PLLVDD_MODE_MASK,
+		NV_SOR_PLL3_PLLVDD_MODE_V3_3);
+	tegra_sor_writel(sor, NV_SOR_PLL0,
+		0xf << NV_SOR_PLL0_ICHPMP_SHFIT |
+		0x3 << NV_SOR_PLL0_VCOCAP_SHIFT |
+		NV_SOR_PLL0_PLLREG_LEVEL_V45 |
+		NV_SOR_PLL0_RESISTORSEL_EXT |
+		NV_SOR_PLL0_PWR_ON | NV_SOR_PLL0_VCOPD_RESCIND);
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX1_SEQ_MASK | NV_SOR_PLL2_AUX9_LVDSEN_OVERRIDE |
+		NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_MASK,
+		NV_SOR_PLL2_AUX1_SEQ_PLLCAPPD_OVERRIDE |
+		NV_SOR_PLL2_AUX9_LVDSEN_OVERRIDE |
+		NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_DISABLE);
+	tegra_sor_writel(sor, NV_SOR_PLL1,
+		NV_SOR_PLL1_TERM_COMPOUT_HIGH | NV_SOR_PLL1_TMDS_TERM_ENABLE);
+
+	if (tegra_dc_sor_poll_register(sor, NV_SOR_PLL2,
+			NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_MASK,
+			NV_SOR_PLL2_AUX8_SEQ_PLLCAPPD_ENFORCE_DISABLE,
+			100, TEGRA_SOR_TIMEOUT_MS * 1000)) {
+		printk(BIOS_ERR, "DP failed to lock PLL\n");
+		return;
+	}
+
+	tegra_sor_write_field(sor, NV_SOR_PLL2,
+		NV_SOR_PLL2_AUX2_MASK | NV_SOR_PLL2_AUX7_PORT_POWERDOWN_MASK,
+		NV_SOR_PLL2_AUX2_OVERRIDE_POWERDOWN |
+		NV_SOR_PLL2_AUX7_PORT_POWERDOWN_DISABLE);
+
+	tegra_dc_sor_power_up(sor, 0);
+
+	/* re-enable SOR clock */
+	tegra_sor_enable_edp_clock(sor);	// select pll_dp as clock source
+
+	/* Power up lanes */
+	tegra_dc_sor_power_dplanes(sor, link_cfg->lane_count, 1);
+
+	tegra_dc_sor_set_dp_mode(sor, link_cfg);
+
+}
+
+void tegra_dc_sor_attach(struct tegra_dc_sor_data *sor)
+{
+
+	u32 reg_val;
+	struct display_controller *disp_ctrl = (void *)sor->dc->base;
+
+	tegra_dc_sor_enable_dc(sor);
+	tegra_dc_sor_config_panel(sor, 0);
+
+	WRITEL(SOR_ENABLE, &disp_ctrl->disp.disp_win_opt);
+
+	reg_val = tegra_sor_readl(sor, NV_SOR_TEST);
+	if (reg_val & NV_SOR_TEST_ATTACHED_TRUE) {
+		printk(BIOS_INFO, "sor: Attached\n");
+		return;
+	}
+
+	/* Attach head */
+	tegra_dc_sor_update(sor);
+	reg_val = NV_SOR_SUPER_STATE1_ASY_HEAD_OP_AWAKE |
+		NV_SOR_SUPER_STATE1_ASY_ORMODE_NORMAL |
+		NV_SOR_SUPER_STATE1_ATTACHED_NO;
+	tegra_sor_writel(sor, NV_SOR_SUPER_STATE1, reg_val);
+	tegra_dc_sor_super_update(sor);
+
+	reg_val |= NV_SOR_SUPER_STATE1_ATTACHED_YES;
+	tegra_sor_writel(sor, NV_SOR_SUPER_STATE1, reg_val);
+	tegra_dc_sor_super_update(sor);
+
+	if (tegra_dc_sor_poll_register(sor, NV_SOR_TEST,
+			NV_SOR_TEST_ATTACHED_DEFAULT_MASK,
+			NV_SOR_TEST_ATTACHED_TRUE,
+			100, TEGRA_SOR_ATTACH_TIMEOUT_MS * 1000)) {
+		printk(BIOS_ERR,
+			"dc timeout waiting for ATTACHED = TRUE\n");
+	}
+
+	/* Enable dc after attaching head */
+	WRITEL(0x9f00, &disp_ctrl->cmd.state_ctrl);
+	WRITEL(0x9f, &disp_ctrl->cmd.state_ctrl);
+	WRITEL(PW0_ENABLE | PW1_ENABLE | PW2_ENABLE |
+		PW3_ENABLE | PW4_ENABLE | PM0_ENABLE | PM1_ENABLE,
+		&disp_ctrl->cmd.disp_pow_ctrl);
+
+	if (tegra_dc_sor_poll_register(sor, NV_SOR_TEST,
+			NV_SOR_TEST_ACT_HEAD_OPMODE_DEFAULT_MASK,
+			NV_SOR_TEST_ACT_HEAD_OPMODE_AWAKE,
+			100, TEGRA_SOR_ATTACH_TIMEOUT_MS * 1000)) {
+		printk(BIOS_ERR,
+			"dc timeout waiting for OPMOD = AWAKE\n");
 	}
+}
+
+void tegra_dc_sor_set_lane_parm(struct tegra_dc_sor_data *sor,
+	const struct tegra_dc_dp_link_config *link_cfg)
+{
+	tegra_sor_writel(sor, NV_SOR_LANE_DRIVE_CURRENT(sor->portnum),
+		link_cfg->drive_current);
+	tegra_sor_writel(sor, NV_SOR_PR(sor->portnum),
+		link_cfg->preemphasis);
+	tegra_sor_writel(sor, NV_SOR_POSTCURSOR(sor->portnum),
+		link_cfg->postcursor);
+	tegra_sor_writel(sor, NV_SOR_LVDS, 0);
 
-	tegra_sor_write_field(sor, NV_SOR_DP_LINKCTL(sor->portnum),
+	tegra_dc_sor_set_link_bandwidth(sor, link_cfg->link_bw);
+	tegra_dc_sor_set_lane_count(sor, link_cfg->lane_count);
+
+	tegra_sor_write_field(sor, NV_SOR_DP_PADCTL(sor->portnum),
+		NV_SOR_DP_PADCTL_TX_PU_ENABLE |
 		NV_SOR_DP_PADCTL_TX_PU_VALUE_DEFAULT_MASK,
-		tx_pu << NV_SOR_DP_PADCTL_TX_PU_VALUE_SHIFT);
+		NV_SOR_DP_PADCTL_TX_PU_ENABLE |
+		2 << NV_SOR_DP_PADCTL_TX_PU_VALUE_SHIFT);
+
+	/* Precharge */
+	tegra_sor_write_field(sor, NV_SOR_DP_PADCTL(sor->portnum),
+		0xf0, 0xf0);
+	udelay(20);
 
-	tegra_sor_writel(sor, NV_SOR_DC(sor->portnum), d_cur);
-	tegra_sor_writel(sor, NV_SOR_PR(sor->portnum), p_emp);
+	tegra_sor_write_field(sor, NV_SOR_DP_PADCTL(sor->portnum),
+		0xf0, 0x0);
 }
 
+void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor)
+{
+	u32 pad_ctrl = 0;
+	u32 drive_current = 0;
+	u32 pre_emphasis = 0;
+	int err = 0;
+
+	switch (sor->link_cfg->lane_count) {
+	case 4:
+		pad_ctrl = (NV_SOR_DP_PADCTL_PD_TXD_0_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_1_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_2_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_3_NO);
+		break;
+	case 2:
+		pad_ctrl = (NV_SOR_DP_PADCTL_PD_TXD_0_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_1_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_2_YES |
+			NV_SOR_DP_PADCTL_PD_TXD_3_YES);
+		break;
+	case 1:
+		pad_ctrl = (NV_SOR_DP_PADCTL_PD_TXD_0_NO |
+			NV_SOR_DP_PADCTL_PD_TXD_1_YES |
+			NV_SOR_DP_PADCTL_PD_TXD_2_YES |
+			NV_SOR_DP_PADCTL_PD_TXD_3_YES);
+		break;
+	default:
+		printk(BIOS_ERR, "Invalid sor lane count: %u\n",
+			sor->link_cfg->lane_count);
+		return;
+	}
+
+	pad_ctrl |= NV_SOR_DP_PADCTL_PAD_CAL_PD_POWERDOWN;
+	tegra_sor_writel(sor, NV_SOR_DP_PADCTL(sor->portnum), pad_ctrl);
+
+	err = tegra_dc_sor_enable_lane_sequencer(sor, 0, 0);
+	if (err) {
+		printk(BIOS_ERR,
+			"Wait for lane power down failed: %d\n", err);
+		return;
+	}
+
+	/* Set to a known-good pre-calibrated setting */
+	switch (sor->link_cfg->link_bw) {
+	case SOR_LINK_SPEED_G1_62:
+	case SOR_LINK_SPEED_G2_7:
+		drive_current = 0x13131313;
+		pre_emphasis = 0;
+		break;
+	case SOR_LINK_SPEED_G5_4:
+		drive_current = 0x19191919;
+		pre_emphasis = 0x09090909;
+	default:
+		printk(BIOS_ERR, "Invalid sor link bandwidth: %d\n",
+			sor->link_cfg->link_bw);
+		return;
+	}
+
+	tegra_sor_writel(sor, NV_SOR_LANE_DRIVE_CURRENT(sor->portnum),
+				drive_current);
+	tegra_sor_writel(sor, NV_SOR_PR(sor->portnum), pre_emphasis);
+}
diff --git a/src/soc/nvidia/tegra124/sor.h b/src/soc/nvidia/tegra124/sor.h
index 1f885eaa17..bf6286871e 100644
--- a/src/soc/nvidia/tegra124/sor.h
+++ b/src/soc/nvidia/tegra124/sor.h
@@ -17,7 +17,6 @@
 #ifndef __TEGRA124_SOR_H__
 #define __TEGRA124_SOR_H__
 
-
 #define NV_SOR_SUPER_STATE0					(0x1)
 #define NV_SOR_SUPER_STATE0_UPDATE_SHIFT			(0)
 #define NV_SOR_SUPER_STATE0_UPDATE_DEFAULT_MASK			(0x1)
@@ -830,11 +829,11 @@
 #define NV_SOR_DP_TPG_LANE0_PATTERN_HBR2_COMPLIANCE		(8)
 
 enum {
-	trainingPattern_Disabled	= 0,
-	trainingPattern_1		= 1,
-	trainingPattern_2		= 2,
-	trainingPattern_3		= 3,
-	trainingPattern_None		= 0xff
+	training_pattern_disabled	= 0,
+	training_pattern_1		= 1,
+	training_pattern_2		= 2,
+	training_pattern_3		= 3,
+	training_pattern_none		= 0xff
 };
 
 enum tegra_dc_sor_protocol {
@@ -842,10 +841,10 @@ enum tegra_dc_sor_protocol {
 	SOR_LVDS,
 };
 
-#define NV_SOR_LINK_SPEED_G1_62	6
-#define NV_SOR_LINK_SPEED_G2_7	10
-#define NV_SOR_LINK_SPEED_G5_4	20
-#define NV_SOR_LINK_SPEED_LVDS	7
+#define SOR_LINK_SPEED_G1_62	6
+#define SOR_LINK_SPEED_G2_7	10
+#define SOR_LINK_SPEED_G5_4	20
+#define SOR_LINK_SPEED_LVDS	7
 
 /* todo: combine this and the intel_dp struct into one struct. */
 struct tegra_dc_dp_link_config {
@@ -885,21 +884,37 @@ struct tegra_dc_dp_link_config {
  * having two channels.
  */
 struct tegra_dc_sor_data {
-	void	*base;
-	u8					 portnum;	/* 0 or 1 */
+	struct tegra_dc			*dc;
+	void				*base;
+	void				*pmc_base;
+	u8				 portnum;	/* 0 or 1 */
+	struct tegra_dc_dp_link_config *link_cfg;
 	int   power_is_up;
 };
 
 #define TEGRA_SOR_TIMEOUT_MS		1000
 #define TEGRA_SOR_ATTACH_TIMEOUT_MS	100000
 
-void tegra_dc_sor_set_dp_linkctl(struct tegra_dc_sor_data *sor,
-				 int ena,
-				 u8 training_pattern,
-				 const struct tegra_dc_dp_link_config *cfg);
-void tegra_dc_sor_set_dp_lanedata(struct tegra_dc_sor_data *sor,
-				  u32 lane, u32 pre_emphasis,
-				  u32 drive_current, u32 tx_pu);
-
+#define CHECK_RET(x)			\
+	do {				\
+		ret = (x);		\
+		if (ret != 0)		\
+			return ret;	\
+	} while (0)
 
+void tegra_dc_sor_enable_dp(struct tegra_dc_sor_data *sor);
+int tegra_dc_sor_set_power_state(struct tegra_dc_sor_data *sor, int pu_pd);
+void tegra_dc_sor_set_dp_linkctl(struct tegra_dc_sor_data *sor, int ena,
+	u8 training_pattern, const struct tegra_dc_dp_link_config *link_cfg);
+void tegra_dc_sor_set_link_bandwidth(struct tegra_dc_sor_data *sor, u8 link_bw);
+void tegra_dc_sor_set_lane_count(struct tegra_dc_sor_data *sor, u8 lane_count);
+void tegra_dc_sor_set_panel_power(struct tegra_dc_sor_data *sor,
+				  int power_up);
+void tegra_dc_sor_set_internal_panel(struct tegra_dc_sor_data *sor, int is_int);
+void tegra_dc_sor_read_link_config(struct tegra_dc_sor_data *sor, u8 *link_bw,
+				   u8 *lane_count);
+void tegra_dc_sor_attach(struct tegra_dc_sor_data *sor);
+void tegra_dc_sor_set_lane_parm(struct tegra_dc_sor_data *sor,
+			const struct tegra_dc_dp_link_config *link_cfg);
+void tegra_dc_sor_power_down_unused_lanes(struct tegra_dc_sor_data *sor);
 #endif /*__TEGRA124_SOR_H__ */